diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index a6b546c54cf..bcd5c9fa8bd 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -119,9 +119,12 @@ for complete documentation on using JavaScript to program your flight controller | 54 | Mag calibration | Trigger a magnetometer calibration. | | 55 | Set Gimbal Sensitivity | Scales `Operand A` from [`-16` : `15`] | 56 | Override Minimum Ground Speed | When active, sets the minimum ground speed to the value specified in `Operand A` [m/s]. Minimum allowed value is set in `nav_min_ground_speed`. Maximum value is `150` | +| 57 | Set Altitude Target | Sets the navigation altitude target. `Operand A` selects the altitude datum and `Operand B` supplies the target altitude in centimetres. | | 58 | Trigonometry: ACos | Computes ACOS of (`Operand A` / `Operand B`) using the fast approximation. If `Operand B` is `0`, `1000` is used. Input is clamped to [-1, 1] and the result is returned in degrees. | | 59 | Trigonometry: ASin | Computes ASIN of (`Operand A` / `Operand B`) using the fast approximation. If `Operand B` is `0`, `1000` is used. Input is clamped to [-1, 1] and the result is returned in degrees. | | 60 | Trigonometry: ATan2 | Computes ATAN2 using `Operand A` as Y and `Operand B` as X with the fast approximation. Returns a signed angle in degrees in `(-180, 180]`. | +| 61 | Activate RTH | While armed, activates normal return-to-home mode through the same mode selector path as RC RTH. Returns true when RTH mode is accepted. | +| 62 | Activate Landing | While armed with usable navigation estimates, commands a normal landing at the current position through the waypoint LAND path. This is not emergency landing. | ### Operands diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index fafcf5d4762..9a5df73b174 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -452,6 +452,10 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: [8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) [8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index) [8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading) +[8740 - MSP2_INAV_ACTIVATE_LANDING](#msp2_inav_activate_landing) +[8741 - MSP2_INAV_ACTIVATE_RTH](#msp2_inav_activate_rth) +[8743 - MSP2_INAV_ARM_DISARM](#msp2_inav_arm_disarm) +[8744 - MSP2_INAV_TIMESYNC](#msp2_inav_timesync) [8752 - MSP2_INAV_SET_AUX_RC](#msp2_inav_set_aux_rc) [12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) [12289 - MSP2_RX_BIND](#msp2_rx_bind) @@ -4603,7 +4607,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Request Payload:** |Field|C Type|Size (Bytes)|Units|Description| |---|---|---|---|---| -| `altitudeDatum` | `uint8_t` | 1 | [geoAltitudeDatumFlag_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-geoaltitudedatumflag_e) | Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM` (default), `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` (not implemented yet) | +| `altitudeDatum` | `uint8_t` | 1 | [geoAltitudeDatumFlag_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-geoaltitudedatumflag_e) | Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM` (default), `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` and `NAV_WP_RELATIVE_DATUM` (not implemented yet) | | `altitudeTarget` | `int32_t` | 4 | cm | Desired altitude target according to reference datum | **Reply Payload:** **None** @@ -4683,10 +4687,11 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude coordinate | | `altitudeTarget` | `int32_t` | 4 | cm | Desired altitude target according to reference datum (0 keeps current altitude) | | `altitudeDatum` | `uint8_t` | 1 | [geoAltitudeDatumFlag_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-geoaltitudedatumflag_e) | Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM`, `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` (not implemented yet) | +| `loiterRadius` | `int32_t` | 4 | cm | Optional temporary fixed-wing PosHold loiter radius override. Appended field; omit to leave unchanged. `0` clears the override and uses `navConfig()->fw.loiter_radius`. | **Reply Payload:** **None** -**Notes:** Uses the GCSNAV/offboard path; rejected when GCSNAV is not active. Rejects `NAV_WP_TERRAIN_DATUM`; other datums are converted to local NEU and applied through `setDesiredPosition()`. Altitude of 0 leaves current Z unchanged. +**Notes:** Uses the GCSNAV/offboard path; rejected when GCSNAV is not active. Rejects `NAV_WP_TERRAIN_DATUM`; other datums are converted to local NEU and applied through `setDesiredPosition()`. Altitude of 0 leaves current Z unchanged. Existing 13-byte payloads are still accepted; 17-byte payloads append `loiterRadius`, where `0` clears the temporary override and non-zero values are centimeters. ## `MSP2_INAV_NAV_TARGET (8731 / 0x221b)` **Description:** Returns the current navigation desired global target (lat/lon/alt, heading, climb rate). @@ -4701,8 +4706,9 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `altitudeTarget` | `int32_t` | 4 | cm | Desired altitude target (takeoff datum, cm) as used by altitude/position hold | | `headingTarget` | `uint16_t` | 2 | degrees | Current heading-hold target (`getHeadingHoldTarget()`), wrapped to 0–359.99 | | `climbRate` | `int16_t` | 2 | cm/s | Desired climb rate demand (`posControl.desiredState.climbRateDemand`) | +| `loiterRadius` | `uint32_t` | 4 | cm | Temporary fixed-wing PosHold loiter radius override. `0` means no override; the configured `navConfig()->fw.loiter_radius` is used. | -**Notes:** Altitude target is reported in the takeoff datum frame (local Z). Heading is sourced from the heading-hold target. Intended for monitoring the active navigation desired target (Goto/Followme/RTH/Safehome). +**Notes:** Altitude target is reported in the takeoff datum frame (local Z). Heading is sourced from the heading-hold target. Intended for monitoring the active navigation desired target (Goto/Followme/RTH/Safehome). The appended `loiterRadius` reports the temporary override only; `0` means the configured default is active. ## `MSP2_INAV_FULL_LOCAL_POSE (8736 / 0x2220)` **Description:** Provides estimates of current attitude, local NEU position, and velocity. @@ -4748,6 +4754,48 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Notes:** Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle. +## `MSP2_INAV_ACTIVATE_LANDING (8740 / 0x2224)` +**Description:** Commands an immediate normal landing at the current position. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires the aircraft to be armed with usable position, altitude, and heading estimates. Creates a transient LAND waypoint at the current position without changing the uploaded mission, then enters the normal `NAV_STATE_WAYPOINT_RTH_LAND` path. This is not emergency landing. + +## `MSP2_INAV_ACTIVATE_RTH (8741 / 0x2225)` +**Description:** Commands the aircraft to execute its configured return-to-home sequence. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires the aircraft to be armed. Enters normal return-to-home mode through the same mode selector path as RC RTH, without setting the failsafe/geozone forced-RTH latch. + +## `MSP2_INAV_ARM_DISARM (8743 / 0x2227)` +**Description:** Arms or disarms the flight controller using the normal FC arming path. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `arm` | `uint8_t` | 1 | Boolean | Requested armed state: 0 disarms, 1 arms through the normal arming checks. | + +**Reply Payload:** **None** + +**Notes:** Returns an error for values other than 0 or 1, or when the requested armed state is not reached. + +## `MSP2_INAV_TIMESYNC (8744 / 0x2228)` +**Description:** Returns the local monotonic boot time in nanoseconds. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `timeNs` | `uint64_t` | 8 | ns | Monotonic flight-controller boot time, calculated as `(uint64_t)micros() * 1000`. | + +**Notes:** The value is little-endian like other MSP integer fields and uses the same boot-time clock returned by MAVLink `TIMESYNC`. + ## `MSP2_INAV_SET_AUX_RC (8752 / 0x2230)` **Description:** Bandwidth-efficient auxiliary RC channel update. Sets CH13-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough. diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 52241112fd3..b8149da5e34 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -1383,7 +1383,8 @@ "_source": "inav/src/main/navigation/navigation.h", "NAV_WP_TAKEOFF_DATUM": "0", "NAV_WP_MSL_DATUM": "1", - "NAV_WP_TERRAIN_DATUM": "2" + "NAV_WP_TERRAIN_DATUM": "2", + "NAV_WP_RELATIVE_DATUM": "3" }, "geoOriginResetMode_e": { "_source": "inav/src/main/navigation/navigation.h", @@ -2094,7 +2095,9 @@ "LOGIC_CONDITION_ACOS": "58", "LOGIC_CONDITION_ASIN": "59", "LOGIC_CONDITION_ATAN2": "60", - "LOGIC_CONDITION_LAST": "61" + "LOGIC_CONDITION_ACTIVATE_RTH": "61", + "LOGIC_CONDITION_ACTIVATE_LANDING": "62", + "LOGIC_CONDITION_LAST": "63" }, "logicWaypointOperands_e": { "_source": "inav/src/main/programming/logic_condition.h", @@ -2385,12 +2388,43 @@ "MF_SUSPEND_TRACKBACK": "(1 << 1)", "MF_TURTLE_MODE": "(1 << 2)" }, + "nav_control_type_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_GPS_ATTI": "0", + "NAV_GPS_CRUISE": "1" + }, + "nav_heading_control_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_HEADING_CONTROL_NONE": "0", + "NAV_HEADING_CONTROL_AUTO": "1", + "NAV_HEADING_CONTROL_MANUAL": "2" + }, + "nav_loiter_type_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_LOITER_RIGHT": "0", + "NAV_LOITER_LEFT": "1", + "NAV_LOITER_YAW": "2" + }, "nav_reset_type_e": { "_source": "inav/src/main/navigation/navigation.h", "NAV_RESET_NEVER": "0", "NAV_RESET_ON_FIRST_ARM": "1", "NAV_RESET_ON_EACH_ARM": "2" }, + "nav_rth_alt_profile_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_RTH_NO_ALT": "0", + "NAV_RTH_EXTRA_ALT": "1", + "NAV_RTH_CONST_ALT": "2", + "NAV_RTH_MAX_ALT": "3", + "NAV_RTH_AT_LEAST_ALT": "4", + "NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT": "5" + }, + "nav_rth_climb_profile_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_RTH_CLIMB_STAGE_AT_LEAST": "0", + "NAV_RTH_CLIMB_STAGE_EXTRA": "1" + }, "navAGLEstimateQuality_e": { "_source": "inav/src/main/navigation/navigation_pos_estimator_private.h", "SURFACE_QUAL_LOW": "0", @@ -2448,11 +2482,12 @@ "NAV_FSM_EVENT_SWITCH_TO_MIXERAT": "14", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING": "15", "NAV_FSM_EVENT_SWITCH_TO_SEND_TO": "16", - "NAV_FSM_EVENT_STATE_SPECIFIC_1": "17", - "NAV_FSM_EVENT_STATE_SPECIFIC_2": "18", - "NAV_FSM_EVENT_STATE_SPECIFIC_3": "19", - "NAV_FSM_EVENT_STATE_SPECIFIC_4": "20", - "NAV_FSM_EVENT_STATE_SPECIFIC_5": "21", + "NAV_FSM_EVENT_SWITCH_TO_LANDING": "17", + "NAV_FSM_EVENT_STATE_SPECIFIC_1": "18", + "NAV_FSM_EVENT_STATE_SPECIFIC_2": "19", + "NAV_FSM_EVENT_STATE_SPECIFIC_3": "20", + "NAV_FSM_EVENT_STATE_SPECIFIC_4": "21", + "NAV_FSM_EVENT_STATE_SPECIFIC_5": "22", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT": "NAV_FSM_EVENT_STATE_SPECIFIC_1", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED": "NAV_FSM_EVENT_STATE_SPECIFIC_2", "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME": "NAV_FSM_EVENT_STATE_SPECIFIC_1", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 275145df9f6..974be9bc22b 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -191,7 +191,12 @@ - [mspSDCardState_e](#enum-mspsdcardstate_e) - [multi_function_e](#enum-multi_function_e) - [multiFunctionFlags_e](#enum-multifunctionflags_e) +- [nav_control_type_e](#enum-nav_control_type_e) +- [nav_heading_control_e](#enum-nav_heading_control_e) +- [nav_loiter_type_e](#enum-nav_loiter_type_e) - [nav_reset_type_e](#enum-nav_reset_type_e) +- [nav_rth_alt_profile_e](#enum-nav_rth_alt_profile_e) +- [nav_rth_climb_profile_e](#enum-nav_rth_climb_profile_e) - [navAGLEstimateQuality_e](#enum-navaglestimatequality_e) - [navArmingBlocker_e](#enum-navarmingblocker_e) - [navDefaultAltitudeSensor_e](#enum-navdefaultaltitudesensor_e) @@ -2246,6 +2251,7 @@ | `NAV_WP_TAKEOFF_DATUM` | 0 | | | `NAV_WP_MSL_DATUM` | 1 | | | `NAV_WP_TERRAIN_DATUM` | 2 | | +| `NAV_WP_RELATIVE_DATUM` | 3 | | --- ## `geoOriginResetMode_e` @@ -3200,7 +3206,9 @@ | `LOGIC_CONDITION_ACOS` | 58 | | | `LOGIC_CONDITION_ASIN` | 59 | | | `LOGIC_CONDITION_ATAN2` | 60 | | -| `LOGIC_CONDITION_LAST` | 61 | | +| `LOGIC_CONDITION_ACTIVATE_RTH` | 61 | | +| `LOGIC_CONDITION_ACTIVATE_LANDING` | 62 | | +| `LOGIC_CONDITION_LAST` | 63 | | --- ## `logicWaypointOperands_e` @@ -3607,6 +3615,38 @@ | `MF_SUSPEND_TRACKBACK` | (1 << 1) | | | `MF_TURTLE_MODE` | (1 << 2) | | +--- +## `nav_control_type_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_GPS_ATTI` | 0 | | +| `NAV_GPS_CRUISE` | 1 | | + +--- +## `nav_heading_control_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_HEADING_CONTROL_NONE` | 0 | | +| `NAV_HEADING_CONTROL_AUTO` | 1 | | +| `NAV_HEADING_CONTROL_MANUAL` | 2 | | + +--- +## `nav_loiter_type_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_LOITER_RIGHT` | 0 | | +| `NAV_LOITER_LEFT` | 1 | | +| `NAV_LOITER_YAW` | 2 | | + --- ## `nav_reset_type_e` @@ -3618,6 +3658,30 @@ | `NAV_RESET_ON_FIRST_ARM` | 1 | | | `NAV_RESET_ON_EACH_ARM` | 2 | | +--- +## `nav_rth_alt_profile_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_RTH_NO_ALT` | 0 | | +| `NAV_RTH_EXTRA_ALT` | 1 | | +| `NAV_RTH_CONST_ALT` | 2 | | +| `NAV_RTH_MAX_ALT` | 3 | | +| `NAV_RTH_AT_LEAST_ALT` | 4 | | +| `NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT` | 5 | | + +--- +## `nav_rth_climb_profile_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_RTH_CLIMB_STAGE_AT_LEAST` | 0 | | +| `NAV_RTH_CLIMB_STAGE_EXTRA` | 1 | | + --- ## `navAGLEstimateQuality_e` @@ -3710,11 +3774,12 @@ | `NAV_FSM_EVENT_SWITCH_TO_MIXERAT` | 14 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING` | 15 | | | `NAV_FSM_EVENT_SWITCH_TO_SEND_TO` | 16 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_1` | 17 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_2` | 18 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_3` | 19 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_4` | 20 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_5` | 21 | | +| `NAV_FSM_EVENT_SWITCH_TO_LANDING` | 17 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_1` | 18 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_2` | 19 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_3` | 20 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_4` | 21 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_5` | 22 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | | `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index e8f64d0cb93..06f97d7210e 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -1,7 +1,7 @@ { "version": { "major": 2, - "minor": 0, + "minor": 1, "patch": 0 }, "messages": { @@ -11015,7 +11015,7 @@ { "name": "altitudeDatum", "ctype": "uint8_t", - "desc": "Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM` (default), `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` (not implemented yet)", + "desc": "Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM` (default), `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` and `NAV_WP_RELATIVE_DATUM` (not implemented yet)", "units": "", "enum": "geoAltitudeDatumFlag_e" }, @@ -11224,11 +11224,17 @@ "desc": "Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM`, `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` (not implemented yet)", "units": "", "enum": "geoAltitudeDatumFlag_e" + }, + { + "name": "loiterRadius", + "ctype": "int32_t", + "desc": "Optional temporary fixed-wing PosHold loiter radius override. Appended field; omit to leave unchanged. `0` clears the override and uses `navConfig()->fw.loiter_radius`.", + "units": "cm" } ] }, "reply": null, - "notes": "Uses the GCSNAV/offboard path; rejected when GCSNAV is not active. Rejects `NAV_WP_TERRAIN_DATUM`; other datums are converted to local NEU and applied through `setDesiredPosition()`. Altitude of 0 leaves current Z unchanged.", + "notes": "Uses the GCSNAV/offboard path; rejected when GCSNAV is not active. Rejects `NAV_WP_TERRAIN_DATUM`; other datums are converted to local NEU and applied through `setDesiredPosition()`. Altitude of 0 leaves current Z unchanged. Existing 13-byte payloads are still accepted; 17-byte payloads append `loiterRadius`, where `0` clears the temporary override and non-zero values are centimeters.", "description": "Sets desired GCS Nav position with global coordinates (WP 254/GOTO)." }, "MSP2_INAV_NAV_TARGET": { @@ -11266,10 +11272,16 @@ "ctype": "int16_t", "desc": "Desired climb rate demand (`posControl.desiredState.climbRateDemand`)", "units": "cm/s" + }, + { + "name": "loiterRadius", + "ctype": "uint32_t", + "desc": "Temporary fixed-wing PosHold loiter radius override. `0` means no override; the configured `navConfig()->fw.loiter_radius` is used.", + "units": "cm" } ] }, - "notes": "Altitude target is reported in the takeoff datum frame (local Z). Heading is sourced from the heading-hold target. Intended for monitoring the active navigation desired target (Goto/Followme/RTH/Safehome).", + "notes": "Altitude target is reported in the takeoff datum frame (local Z). Heading is sourced from the heading-hold target. Intended for monitoring the active navigation desired target (Goto/Followme/RTH/Safehome). The appended `loiterRadius` reports the temporary override only; `0` means the configured default is active.", "description": "Returns the current navigation desired global target (lat/lon/alt, heading, climb rate)." }, "MSP2_INAV_FULL_LOCAL_POSE": { @@ -11371,6 +11383,56 @@ "notes": "Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.", "description": "Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading." }, + "MSP2_INAV_ACTIVATE_LANDING": { + "code": 8740, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Requires the aircraft to be armed with usable position, altitude, and heading estimates. Creates a transient LAND waypoint at the current position without changing the uploaded mission, then enters the normal `NAV_STATE_WAYPOINT_RTH_LAND` path. This is not emergency landing.", + "description": "Commands an immediate normal landing at the current position." + }, + "MSP2_INAV_ACTIVATE_RTH": { + "code": 8741, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Requires the aircraft to be armed. Enters normal return-to-home mode through the same mode selector path as RC RTH, without setting the failsafe/geozone forced-RTH latch.", + "description": "Commands the aircraft to execute its configured return-to-home sequence." + }, + "MSP2_INAV_ARM_DISARM": { + "code": 8743, + "mspv": 2, + "request": { + "payload": [ + { + "name": "arm", + "ctype": "uint8_t", + "desc": "Requested armed state: 0 disarms, 1 arms through the normal arming checks.", + "units": "Boolean" + } + ] + }, + "reply": null, + "notes": "Returns an error for values other than 0 or 1, or when the requested armed state is not reached.", + "description": "Arms or disarms the flight controller using the normal FC arming path." + }, + "MSP2_INAV_TIMESYNC": { + "code": 8744, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "timeNs", + "ctype": "uint64_t", + "desc": "Monotonic flight-controller boot time, calculated as `(uint64_t)micros() * 1000`.", + "units": "ns" + } + ] + }, + "notes": "The value is little-endian like other MSP integer fields and uses the same boot-time clock returned by MAVLink `TIMESYNC`.", + "description": "Returns the local monotonic boot time in nanoseconds." + }, "MSP2_INAV_SET_AUX_RC": { "code": 8752, "mspv": 2, @@ -11448,4 +11510,4 @@ "description": "Initiates binding for MSP receivers (mLRS)." } } -} \ No newline at end of file +} diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..0fc7ecf1070 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -619,6 +619,17 @@ void tryArm(void) } } +bool fcSetArmState(bool arm) +{ + if (arm) { + tryArm(); + } else { + disarm(DISARM_SWITCH); + } + + return ARMING_FLAG(ARMED) == arm; +} + #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) void releaseSharedTelemetryPorts(void) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 02a5c889a65..3e5ba12714b 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -39,6 +39,7 @@ void handleInflightCalibrationStickPosition(void); void disarm(disarmReason_t disarmReason); timeUs_t getLastDisarmTimeUs(void); void tryArm(void); +bool fcSetArmState(bool arm); disarmReason_t getDisarmReason(void); bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm); @@ -48,4 +49,4 @@ float getFlightTime(void); void resetFlightTime(void); float getArmTime(void); void fcReboot(bool bootLoader); -bool isMspConfigActive(bool isActive); \ No newline at end of file +bool isMspConfigActive(bool isActive); diff --git a/src/main/fc/fc_mavlink.c b/src/main/fc/fc_mavlink.c index de527b67614..b3109c6b931 100644 --- a/src/main/fc/fc_mavlink.c +++ b/src/main/fc/fc_mavlink.c @@ -260,21 +260,25 @@ mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIn return mavlinkHandleIncomingMissionItemInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: return mavlinkHandleIncomingMissionRequestList() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_COMMAND_LONG: + return mavlinkHandleIncomingCommandLong() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_COMMAND_INT: + return mavlinkHandleIncomingCommandInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST: return mavlinkHandleIncomingMissionRequest() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST_INT: return mavlinkHandleIncomingMissionRequestInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_ACK: return mavlinkHandleIncomingMissionAck() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; - case MAVLINK_MSG_ID_COMMAND_LONG: - return mavlinkHandleIncomingCommandLong() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; - case MAVLINK_MSG_ID_COMMAND_INT: - return mavlinkHandleIncomingCommandInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: return mavlinkHandleIncomingRequestDataStream() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: handleIncoming_RC_CHANNELS_OVERRIDE(); return MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + return mavlinkHandleIncomingSetPositionTargetLocalNed() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: + return mavlinkHandleIncomingSetPositionTargetGlobalInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; #ifdef USE_ADSB case MAVLINK_MSG_ID_ADSB_VEHICLE: return handleIncoming_ADSB_VEHICLE() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 50ddc2d9f09..72767ae122c 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -742,6 +742,14 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; + case MSP2_INAV_TIMESYNC: + { + const uint64_t timeNs = (uint64_t)micros() * 1000ULL; + sbufWriteU32(dst, (uint32_t)timeNs); + sbufWriteU32(dst, (uint32_t)(timeNs >> 32)); + } + break; + case MSP_SONAR_ALTITUDE: #ifdef USE_RANGEFINDER sbufWriteU32(dst, rangefinderGetLatestAltitude()); @@ -1772,7 +1780,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif sbufWriteU32(dst, timerHardware[i].usageFlags); - + #if defined(SITL_BUILD) || defined(WASM_BUILD) sbufWriteU8(dst, 0); #else @@ -3966,6 +3974,27 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } return MSP_RESULT_ERROR; + case MSP2_INAV_ACTIVATE_LANDING: + if (dataSize == 0 && activateForcedLanding()) { + break; + } + return MSP_RESULT_ERROR; + + case MSP2_INAV_ACTIVATE_RTH: + if (dataSize == 0 && activateRTHMode()) { + break; + } + return MSP_RESULT_ERROR; + + case MSP2_INAV_ARM_DISARM: + if (dataSize == 1) { + uint8_t arm; + if (sbufReadU8Safe(&arm, src) && arm <= 1 && fcSetArmState(arm)) { + break; + } + } + return MSP_RESULT_ERROR; + default: return MSP_RESULT_ERROR; } @@ -4418,7 +4447,7 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_ } // Feed data to navigation gpsProcessNewDriverData(); - gpsProcessNewSolutionData(false); + gpsProcessNewSolutionData(false); } else { sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } @@ -4471,26 +4500,26 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_ sbufReadU16(src); } - if (simMspVersion == SIMULATOR_MSP_VERSION_3) { - + if (simMspVersion == SIMULATOR_MSP_VERSION_3) { + if (SIMULATOR_HAS_OPTION(HITL_RANGEFINDER)) { simulatorData.rangefinder = sbufReadU16(src); if (simulatorData.rangefinder == 0xFFFF) { fakeRangefindersSetData(-1); } else { - fakeRangefindersSetData(simulatorData.rangefinder); + fakeRangefindersSetData(simulatorData.rangefinder); } - + } else { sbufReadU16(src); } - + if (SIMULATOR_HAS_OPTION(HITL_CURRENT_SENSOR)) { simulatorData.current = sbufReadU16(src); } else { sbufReadU16(src); } - + if (SIMULATOR_HAS_OPTION(HITL_SIM_RC_INPUT)) { for (int i = 0; i < HITL_SIM_MAX_RC_INPUTS; i++) { simulatorData.rcInput[i] = sbufReadU16(src); @@ -4792,7 +4821,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; case MSP2_INAV_SET_GLOBAL_TARGET: - if (dataSize != (3 * sizeof(int32_t) + sizeof(uint8_t)) || !isGCSValid()) { + if ((dataSize != (3 * sizeof(int32_t) + sizeof(uint8_t)) && dataSize != (4 * sizeof(int32_t) + sizeof(uint8_t))) || !isGCSValid()) { *ret = MSP_RESULT_ERROR; break; } @@ -4804,8 +4833,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu targetLlh.alt = (int32_t)sbufReadU32(src); const geoAltitudeDatumFlag_e datumFlag = (geoAltitudeDatumFlag_e)sbufReadU8(src); + const bool hasLoiterRadius = dataSize == (4 * sizeof(int32_t) + sizeof(uint8_t)); + const int32_t loiterRadius = hasLoiterRadius ? (int32_t)sbufReadU32(src) : 0; - if (datumFlag == NAV_WP_TERRAIN_DATUM) { + if (datumFlag == NAV_WP_TERRAIN_DATUM || loiterRadius < 0) { *ret = MSP_RESULT_ERROR; break; } @@ -4823,6 +4854,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } setDesiredPosition(&targetPos, posControl.desiredState.yaw, updateMask); + if (hasLoiterRadius) { + navigationSetLoiterRadiusOverride((uint32_t)loiterRadius); + } *ret = MSP_RESULT_ACK; } break; @@ -4844,6 +4878,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu const uint16_t headingTarget = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(getHeadingHoldTarget()))); sbufWriteU16(dst, headingTarget); sbufWriteU16(dst, posControl.desiredState.climbRateDemand); + sbufWriteU32(dst, navigationGetLoiterRadiusOverride()); *ret = MSP_RESULT_ACK; } break; diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index b593bddcc6a..c69613b0bb1 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -42,6 +42,10 @@ static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT]; static bool isUsingNAVModes = false; boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e +static boxBitmask_t rcModeRawActivationMask; +static boxBitmask_t rcModeActivationOverrideMask; +static boxBitmask_t rcModeActivationOverrideSnapshot; +static bool rcModeActivationOverrideActive; // TODO(alberto): It looks like we can now safely remove this assert, since everything // but BB is able to handle more than 32 boxes and all the definitions use @@ -141,9 +145,84 @@ bool IS_RC_MODE_ACTIVE(boxId_e boxId) return bitArrayGet(rcModeActivationMask.bits, boxId); } +static boxBitmask_t rcModeFlightModeMask(const boxBitmask_t *source) +{ + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + + const boxId_e modeBoxes[] = { + BOXANGLE, + BOXHORIZON, + BOXNAVALTHOLD, + BOXHEADINGHOLD, + BOXNAVRTH, + BOXNAVPOSHOLD, + BOXMANUAL, + BOXNAVLAUNCH, + BOXNAVWP, + BOXTURNASSIST, + BOXSURFACE, + BOXNAVCOURSEHOLD, + BOXBRAKING, + BOXNAVCRUISE, + BOXSOARING, + BOXANGLEHOLD, + }; + + for (unsigned i = 0; i < ARRAYLEN(modeBoxes); i++) { + if (bitArrayGet(source->bits, modeBoxes[i])) { + bitArraySet(mask.bits, modeBoxes[i]); + } + } + + return mask; +} + +static void rcModeUpdateEffectiveActivationMask(void) +{ + rcModeActivationMask = rcModeRawActivationMask; + + if (rcModeActivationOverrideActive && !ARMING_FLAG(ARMED)) { + BITARRAY_CLR_ALL(rcModeActivationOverrideMask.bits); + rcModeActivationOverrideActive = false; + } + + if (!rcModeActivationOverrideActive) { + return; + } + + const boxBitmask_t currentFlightModeMask = rcModeFlightModeMask(&rcModeRawActivationMask); + if (memcmp(¤tFlightModeMask, &rcModeActivationOverrideSnapshot, sizeof(currentFlightModeMask)) != 0) { + BITARRAY_CLR_ALL(rcModeActivationOverrideMask.bits); + rcModeActivationOverrideActive = false; + return; + } + + for (unsigned i = 0; i < ARRAYLEN(rcModeActivationMask.bits); i++) { + rcModeActivationMask.bits[i] |= rcModeActivationOverrideMask.bits[i]; + } +} + void rcModeUpdate(boxBitmask_t *newState) { - rcModeActivationMask = *newState; + rcModeRawActivationMask = *newState; + rcModeUpdateEffectiveActivationMask(); +} + +void rcModeSetActivationOverride(boxId_e boxId) +{ + rcModeActivationOverrideSnapshot = rcModeFlightModeMask(&rcModeRawActivationMask); + BITARRAY_CLR_ALL(rcModeActivationOverrideMask.bits); + bitArraySet(rcModeActivationOverrideMask.bits, boxId); + rcModeActivationOverrideActive = true; + rcModeUpdateEffectiveActivationMask(); +} + +void rcModeClearActivationOverride(boxId_e boxId) +{ + bitArrayClr(rcModeActivationOverrideMask.bits, boxId); + rcModeActivationOverrideActive = BITARRAY_FIND_FIRST_SET(rcModeActivationOverrideMask.bits, 0) >= 0; + rcModeUpdateEffectiveActivationMask(); } bool isModeActivationConditionPresent(boxId_e modeId) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 2e972d1b304..63714b6c3b9 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -135,6 +135,8 @@ PG_DECLARE(modeActivationOperatorConfig_t, modeActivationOperatorConfig); bool IS_RC_MODE_ACTIVE(boxId_e boxId); void rcModeUpdate(boxBitmask_t *newState); +void rcModeSetActivationOverride(boxId_e boxId); +void rcModeClearActivationOverride(boxId_e boxId); bool isModeActivationConditionPresent(boxId_e modeId); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index dc592dca874..b6e293d3e56 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2606,6 +2606,8 @@ static bool osdDrawSingleElement(uint8_t item) p = "TURT"; else if (FLIGHT_MODE(NAV_RTH_MODE)) p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; + else if (isGCSValid()) + p = "GCSN"; else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) p = "LOTR"; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) diff --git a/src/main/mavlink/mavlink_command.c b/src/main/mavlink/mavlink_command.c index c4b4755ec5c..9a8f4ab8a60 100644 --- a/src/main/mavlink/mavlink_command.c +++ b/src/main/mavlink/mavlink_command.c @@ -58,13 +58,109 @@ static bool handleIncoming_COMMAND( float longitudeDeg, float altitudeMeters) { - UNUSED(param3); - if (!mavlinkIsLocalTarget(targetSystem, targetComponent)) { return false; } switch (command) { + case MAV_CMD_COMPONENT_ARM_DISARM: + if (param1 != 0.0f && param1 != 1.0f) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + mavlinkSendCommandAck(command, fcSetArmState(param1 == 1.0f) ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (!ARMING_FLAG(ARMED)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + mavlinkSendCommandAck(command, activateRTHMode() ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + case MAV_CMD_DO_SET_MODE: + { + uint32_t modeFlags; + uint32_t customMode; + if (!mavlinkCommandParamToUint32(param1, UINT8_MAX, &modeFlags) || + !mavlinkCommandParamToUint32(param2, UINT8_MAX, &customMode) || + (modeFlags & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) == 0) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + const bool fixedWing = mavlinkIsFixedWingVehicle(); + const uint8_t rthMode = fixedWing ? PLANE_MODE_RTL : COPTER_MODE_RTL; + const bool posHoldMode = fixedWing ? + (customMode == PLANE_MODE_LOITER) : + (customMode == COPTER_MODE_LOITER || customMode == COPTER_MODE_POSHOLD || customMode == COPTER_MODE_BRAKE); + + if (customMode != rthMode && !posHoldMode) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + if (!ARMING_FLAG(ARMED)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + if (customMode == rthMode) { + mavlinkSendCommandAck(command, activateRTHMode() ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavlinkSendCommandAck(command, activatePositionHoldMode() ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_NAV_LAND: + mavlinkSendCommandAck(command, activateForcedLanding() ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + case MAV_CMD_NAV_TAKEOFF: + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + case MAV_CMD_DO_SET_HOME: + { + if ((param1 != 0.0f && param1 != 1.0f) || + !mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + if (!navCanSetHome()) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + navWaypoint_t wp = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + if (param1 == 1.0f) { + wp.lat = gpsSol.llh.lat; + wp.lon = gpsSol.llh.lon; + wp.alt = gpsSol.llh.alt - posControl.gpsOrigin.alt; + } else { + if (!isfinite(latitudeDeg) || latitudeDeg < -90.0f || latitudeDeg > 90.0f || + !isfinite(longitudeDeg) || longitudeDeg < -180.0f || longitudeDeg > 180.0f || + !isfinite(altitudeMeters) || + altitudeMeters < (float)INT32_MIN / 100.0f || + altitudeMeters > (float)INT32_MAX / 100.0f) { + mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); + return true; + } + wp.lat = (int32_t)lrintf(latitudeDeg * 1e7f); + wp.lon = (int32_t)lrintf(longitudeDeg * 1e7f); + wp.alt = (int32_t)lrintf(altitudeMeters * 100.0f); + if (mavlinkFrameUsesAbsoluteAltitude(frame)) { + wp.alt -= posControl.gpsOrigin.alt; + } + } + setWaypoint(0, &wp); + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } case MAV_CMD_DO_REPOSITION: if (!mavlinkFrameIsSupported(frame, MAV_FRAME_SUPPORTED_GLOBAL | @@ -85,6 +181,16 @@ static bool handleIncoming_COMMAND( } if (isGCSValid()) { + if (isfinite(param3)) { + const float maxLoiterRadiusMeters = (float)(UINT32_MAX / 100U); + + if (param3 < 0.0f || param3 > maxLoiterRadiusMeters) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + navigationSetLoiterRadiusOverride((uint32_t)lrintf(METERS_TO_CENTIMETERS(param3))); + } + navWaypoint_t wp = {0}; wp.action = NAV_WP_ACTION_WAYPOINT; wp.lat = (int32_t)(latitudeDeg * 1e7f); @@ -117,6 +223,38 @@ static bool handleIncoming_COMMAND( mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); return true; } + case MAV_CMD_CONDITION_YAW: + { + if (!isfinite(param1) || fabsf(param1) > 360.0f || !isfinite(param3) || !isfinite(param4)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + if (!(navGetCurrentStateFlags() & NAV_CTL_YAW)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + int32_t targetHeadingCd = wrap_36000((int32_t)lrintf(param1 * 100.0f)); + + if (param4 != 0.0f) { + const int32_t currentHeadingCd = STATE(AIRPLANE) ? posControl.actualState.cog : posControl.actualState.yaw; + const int32_t headingChangeCd = (int32_t)lrintf(fabsf(param1) * 100.0f); + + if (param3 < 0.0f) { + targetHeadingCd = wrap_36000(currentHeadingCd - headingChangeCd); + } else { + targetHeadingCd = wrap_36000(currentHeadingCd + headingChangeCd); + } + } + + updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(targetHeadingCd)); + posControl.desiredState.yaw = targetHeadingCd; + posControl.cruise.course = targetHeadingCd; + posControl.cruise.previousCourse = targetHeadingCd; + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } case MAV_CMD_SET_MESSAGE_INTERVAL: { mavlinkPeriodicMessage_e periodicMessage; diff --git a/src/main/mavlink/mavlink_guided.c b/src/main/mavlink/mavlink_guided.c index 0658b70694f..1281ab7fee8 100644 --- a/src/main/mavlink/mavlink_guided.c +++ b/src/main/mavlink/mavlink_guided.c @@ -59,4 +59,101 @@ MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters #endif } +static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) +{ + if (targetSystem != 0 && targetSystem != mavSystemId) { + return false; + } + + if (targetComponent != 0 && targetComponent != mavComponentId) { + return false; + } + + return true; +} + +bool mavlinkHandleIncomingSetPositionTargetGlobalInt(void) +{ + mavlink_set_position_target_global_int_t msg; + mavlink_msg_set_position_target_global_int_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + uint8_t frame = msg.coordinate_frame; + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + return true; + } + + const uint16_t typeMask = msg.type_mask; + const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0; + const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0; + const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0; + + // Altitude-only SET_POSITION_TARGET_GLOBAL_INT mirrors MAV_CMD_DO_CHANGE_ALTITUDE semantics. + if (xIgnored && yIgnored && !zIgnored) { + if (isGCSValid()) { + mavlinkSetAltitudeTargetFromFrame(frame, msg.alt); + } + return true; + } + + if (xIgnored || yIgnored) { + return true; + } + + if (isGCSValid()) { + navWaypoint_t wp = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = msg.lat_int; + wp.lon = msg.lon_int; + wp.alt = zIgnored ? 0 : (int32_t)(msg.alt * 100.0f); + wp.p1 = 0; + wp.p2 = 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + wp.flag = 0; + + setWaypoint(255, &wp); + } + + return true; +} + +bool mavlinkHandleIncomingSetPositionTargetLocalNed(void) +{ + mavlink_set_position_target_local_ned_t msg; + mavlink_msg_set_position_target_local_ned_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + if (msg.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) { + return true; + } + + const uint16_t typeMask = msg.type_mask; + const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0; + const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0; + const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0; + + if (!isGCSValid() || zIgnored) { + return true; + } + + if ((!xIgnored && fabsf(msg.x) > 0.01f) || (!yIgnored && fabsf(msg.y) > 0.01f)) { + return true; + } + + const int32_t targetAltitudeCm = (int32_t)lrintf((float)getEstimatedActualPosition(Z) - (msg.z * 100.0f)); + navigationSetAltitudeTargetWithDatum(NAV_WP_TAKEOFF_DATUM, targetAltitudeCm); + + return true; +} + #endif diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 180597c4527..0ea2cbaa939 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -146,5 +146,9 @@ #define MSP2_INAV_SET_WP_INDEX 0x2221 //in message jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start) #define MSP2_INAV_SET_CRUISE_HEADING 0x2223 //in message set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999) +#define MSP2_INAV_ACTIVATE_LANDING 0x2224 //in message land at the current position through the normal waypoint LAND path; payload: none +#define MSP2_INAV_ACTIVATE_RTH 0x2225 //in message trigger normal return-to-home mode; payload: none +#define MSP2_INAV_ARM_DISARM 0x2227 //in message set armed state; payload: U8 arm +#define MSP2_INAV_TIMESYNC 0x2228 //out message local monotonic boot time in nanoseconds; payload: U64 time_ns -#define MSP2_INAV_SET_AUX_RC 0x2230 \ No newline at end of file +#define MSP2_INAV_SET_AUX_RC 0x2230 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ca0f74face7..bb93fcb4934 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2170,10 +2170,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState); if (landEvent == NAV_FSM_EVENT_SUCCESS) { + const bool commandedLanding = posControl.flags.forcedLandingActivated; + posControl.flags.forcedLandingActivated = false; // Landing controller returned success - invoke RTH finish states and finish the waypoint. // Success maps straight to NAV_STATE_WAYPOINT_FINISHED, bypassing NAV_STATE_WAYPOINT_NEXT, - // so the LAND item must be marked reached here or it never reports completion. - navMarkWaypointReached(posControl.activeWaypointIndex); + // so the LAND item must be marked reached here or it never reports completion. A + // command-triggered landing borrows this state with a transient waypoint and must + // not credit an unrelated mission item. + if (!commandedLanding) { + navMarkWaypointReached(posControl.activeWaypointIndex); + } navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState); navOnEnteringState_NAV_STATE_RTH_FINISHED(previousState); } @@ -2575,11 +2581,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(nav { UNUSED(previousState); + const bool commandedLanding = posControl.flags.forcedLandingActivated; + posControl.flags.forcedLandingActivated = false; + // A mission LAND item handed off to the autoland FSM finishes here, not // through NAV_STATE_WAYPOINT_RTH_LAND's success branch - credit the item // or the mission never reports completion. landState guards re-entry - // (this state self-loops on timeout). - if (posControl.fwLandState.landWp && posControl.fwLandState.landState != FW_AUTOLAND_STATE_IDLE) { + // (this state self-loops on timeout). A command-triggered landing must + // not credit an unrelated mission item. + if (!commandedLanding && posControl.fwLandState.landWp && posControl.fwLandState.landState != FW_AUTOLAND_STATE_IDLE) { navMarkWaypointReached(posControl.activeWaypointIndex); } @@ -2594,6 +2604,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga posControl.fwLandState.landAborted = true; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; + if (posControl.flags.forcedLandingActivated) { + posControl.flags.forcedLandingActivated = false; + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + return posControl.fwLandState.landWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH; } #endif @@ -2676,7 +2691,9 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) /* Process new injected event if event defined, * otherwise process timeout event if defined */ - if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) { + if (injectedEvent == NAV_FSM_EVENT_SWITCH_TO_LANDING) { + previousState = navSetNewFSMState(NAV_STATE_WAYPOINT_RTH_LAND); + } else if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) { /* Update state */ previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]); } else if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) && @@ -2731,7 +2748,9 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) NAV_Status.activeWpNumber = NAV_Status.activeWpIndex + 1; NAV_Status.activeWpAction = 0; - if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) { + if (posControl.flags.forcedLandingActivated) { + NAV_Status.activeWpAction = NAV_WP_ACTION_LAND; + } else if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) { NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action; } } @@ -2783,15 +2802,22 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) } break; - case RTH_HOME_FINAL_LAND: + case RTH_HOME_FINAL_LAND: { // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation) - if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) { - posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100; // 100 -> m to cm - if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) { + const navWaypoint_t *landingWaypoint = NULL; + if (posControl.flags.forcedLandingActivated) { + landingWaypoint = &posControl.commandLandingWaypoint; + } else if (FLIGHT_MODE(NAV_WP_MODE)) { + landingWaypoint = &posControl.waypointList[posControl.activeWaypointIndex]; + } + if (landingWaypoint && landingWaypoint->action == NAV_WP_ACTION_LAND && landingWaypoint->p2 != 0) { + posControl.rthState.homeTmpWaypoint.z = landingWaypoint->p2 * 100; // 100 -> m to cm + if (waypointMissionAltConvMode(landingWaypoint->p3) == GEO_ALT_ABSOLUTE) { posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt; // correct to relative if absolute SL altitude datum used } } break; + } } return &posControl.rthState.homeTmpWaypoint; @@ -3848,6 +3874,26 @@ void resetGCSFlags(void) posControl.flags.isGCSAssistedNavigationEnabled = false; } +void navigationSetLoiterRadiusOverride(uint32_t loiterRadiusCm) +{ + posControl.gcsLoiterRadiusOverride = loiterRadiusCm; +} + +uint32_t navigationGetLoiterRadiusOverride(void) +{ + return posControl.gcsLoiterRadiusOverride; +} + +uint32_t navigationGetLoiterRadius(void) +{ + if (posControl.gcsLoiterRadiusOverride != 0 && + (posControl.navState == NAV_STATE_POSHOLD_3D_INITIALIZE || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { + return posControl.gcsLoiterRadiusOverride; + } + + return navConfig()->fw.loiter_radius; +} + void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) { /* Default waypoint to send */ @@ -3912,6 +3958,14 @@ int isGCSValid(void) (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)); } +bool navCanSetHome(void) +{ + return ARMING_FLAG(ARMED) && + posControl.flags.estPosStatus >= EST_USABLE && + posControl.gpsOrigin.valid && + posControl.flags.isGCSAssistedNavigationEnabled; +} + /* * navSetActiveWaypointIndex - MSP2_INAV_SET_WP_INDEX handler * @@ -3986,7 +4040,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) wpLLH.alt = wpData->alt; // WP #0 - special waypoint - HOME - if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) { + if ((wpNumber == 0) && navCanSetHome()) { // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL); @@ -3995,7 +4049,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) // Only valid when armed and in poshold mode else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && isGCSValid()) { // Convert to local coordinates - geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, waypointMissionAltConvMode(wpData->p3)); navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY; @@ -4113,6 +4167,7 @@ void loadSelectedMultiMission(uint8_t missionIndex) posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0; posControl.activeWaypointIndex = posControl.startWpIndex; + posControl.wpReachedNotificationPending = false; } bool updateWpMissionChange(void) @@ -4210,7 +4265,16 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded) bool saveNonVolatileWaypointList(void) { - if (ARMING_FLAG(ARMED) || !posControl.waypointListValid) + if (ARMING_FLAG(ARMED)) + return false; + + if (posControl.waypointCount == 0) { + memset(nonVolatileWaypointListMutable(0), 0, sizeof(navWaypoint_t) * NAV_MAX_WAYPOINTS); + saveConfigAndNotify(); + return true; + } + + if (!posControl.waypointListValid) return false; for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { @@ -4412,8 +4476,10 @@ void applyWaypointNavigationAndAltitudeHold(void) if (!ARMING_FLAG(ARMED)) { // If we are disarmed, abort forced RTH or Emergency Landing posControl.flags.forcedRTHActivated = false; + posControl.flags.forcedLandingActivated = false; posControl.flags.forcedEmergLandingActivated = false; posControl.flags.manualEmergLandActive = false; + posControl.gcsLoiterRadiusOverride = 0; // ensure WP missions always restart from first waypoint after disarm posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback @@ -4623,6 +4689,17 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } + if (posControl.flags.forcedLandingActivated) { + if (posControl.navState == NAV_STATE_WAYPOINT_RTH_LAND +#ifdef USE_FW_AUTOLAND + || FLIGHT_MODE(NAV_FW_AUTOLAND) +#endif + ) { + return NAV_FSM_EVENT_NONE; + } + return NAV_FSM_EVENT_SWITCH_TO_LANDING; + } + #ifdef USE_GEOZONE if (posControl.flags.sendToActive) { return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; @@ -5107,9 +5184,12 @@ void navigationInit(void) posControl.flags.forcedRTHActivated = false; posControl.flags.forcedEmergLandingActivated = false; + posControl.gcsLoiterRadiusOverride = 0; posControl.waypointCount = 0; posControl.activeWaypointIndex = 0; posControl.waypointListValid = false; + posControl.wpReachedSeq = 0; + posControl.wpReachedNotificationPending = false; posControl.wpPlannerActiveWPIndex = 0; posControl.flags.wpMissionPlannerActive = false; posControl.startWpIndex = 0; @@ -5170,9 +5250,57 @@ float getEstimatedActualPosition(int axis) /*----------------------------------------------------------- * Ability to execute RTH on external event *-----------------------------------------------------------*/ +bool activateRTHMode(void) +{ + if (!ARMING_FLAG(ARMED)) { + return false; + } + + abortFixedWingLaunch(); + posControl.flags.forcedLandingActivated = false; + rcModeSetActivationOverride(BOXNAVRTH); +#ifdef USE_SAFE_HOME + checkSafeHomeState(true); +#endif + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + + if (navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) { + return true; + } + + rcModeClearActivationOverride(BOXNAVRTH); + return false; +} + +bool activatePositionHoldMode(void) +{ + if (!ARMING_FLAG(ARMED) || + posControl.flags.estPosStatus < EST_USABLE || + posControl.flags.estVelStatus < EST_TRUSTED || + posControl.flags.estAltStatus < EST_USABLE || + posControl.flags.estHeadingStatus < EST_USABLE) { + return false; + } + + abortFixedWingLaunch(); + posControl.flags.forcedRTHActivated = false; + posControl.flags.forcedLandingActivated = false; + rcModeSetActivationOverride(BOXNAVPOSHOLD); + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + + if (navGetStateFlags(posControl.navState) & NAV_CTL_HOLD) { + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return true; + } + + rcModeClearActivationOverride(BOXNAVPOSHOLD); + return false; +} + void activateForcedRTH(void) { abortFixedWingLaunch(); + posControl.flags.forcedLandingActivated = false; posControl.flags.forcedRTHActivated = true; #ifdef USE_SAFE_HOME checkSafeHomeState(true); @@ -5180,6 +5308,41 @@ void activateForcedRTH(void) navProcessFSMEvents(selectNavEventFromBoxModeInput()); } +bool activateForcedLanding(void) +{ + if (!ARMING_FLAG(ARMED) || + posControl.flags.estPosStatus < EST_USABLE || + posControl.flags.estAltStatus < EST_USABLE || + posControl.flags.estHeadingStatus < EST_USABLE) { + return false; + } + + if (posControl.flags.forcedLandingActivated) { + return true; + } + + abortFixedWingLaunch(); + posControl.flags.forcedRTHActivated = false; +#ifdef USE_SAFE_HOME + checkSafeHomeState(false); +#endif + posControl.flags.forcedLandingActivated = true; + posControl.commandLandingWaypoint = (navWaypoint_t) { + .action = NAV_WP_ACTION_LAND, + .alt = (int32_t)lrintf(posControl.actualState.abs.pos.z), + .flag = NAV_WP_FLAG_LAST, + }; + + resetPositionController(); + resetAltitudeController(false); + setupAltitudeController(); + calculateAndSetActiveWaypointToLocalPosition(&posControl.actualState.abs.pos); + resetLandingDetector(); + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_LANDING); + + return (navGetStateFlags(posControl.navState) & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND); +} + void abortForcedRTH(void) { // Disable failsafe RTH and make sure we back out of navigation mode to IDLE diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 65b66d778ce..37ce6480f01 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,18 +239,18 @@ void abortForcedPosHold(void); #define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target -enum { +typedef enum { NAV_GPS_ATTI = 0, // Pitch/roll stick controls attitude (pitch/roll lean angles) NAV_GPS_CRUISE = 1 // Pitch/roll stick controls velocity (forward/right speed) -}; +} nav_control_type_e; -enum { +typedef enum { NAV_LOITER_RIGHT = 0, // Loitering direction right NAV_LOITER_LEFT = 1, // Loitering direction left NAV_LOITER_YAW = 2 -}; +} nav_loiter_type_e; -enum { +typedef enum { NAV_RTH_NO_ALT = 0, // Maintain current altitude NAV_RTH_EXTRA_ALT = 1, // Maintain current altitude + predefined safety margin NAV_RTH_CONST_ALT = 2, // Climb/descend to predefined altitude @@ -258,18 +258,18 @@ enum { NAV_RTH_AT_LEAST_ALT = 4, // Climb to predefined altitude if below it NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT = 5, // Climb to predefined altitude if below it, // descend linearly to reach home at predefined altitude if above it -}; +} nav_rth_alt_profile_e; -enum { +typedef enum { NAV_RTH_CLIMB_STAGE_AT_LEAST = 0, // Will climb to the lesser of rth_climb_first_stage_altitude or rth_altitude, before turning NAV_RTH_CLIMB_STAGE_EXTRA = 1, // Will climb the lesser of rth_climb_first_stage_altitude above the current altitude or to nav_rth_altitude, before turning -}; +} nav_rth_climb_profile_e; -enum { +typedef enum { NAV_HEADING_CONTROL_NONE = 0, NAV_HEADING_CONTROL_AUTO, NAV_HEADING_CONTROL_MANUAL -}; +} nav_heading_control_e; typedef enum { NAV_RESET_NEVER = 0, @@ -707,6 +707,9 @@ bool isWaypointListValid(void); int isGCSValid(void); void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData); void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData); +void navigationSetLoiterRadiusOverride(uint32_t loiterRadiusCm); +uint32_t navigationGetLoiterRadiusOverride(void); +uint32_t navigationGetLoiterRadius(void); void resetWaypointList(void); bool navSetActiveWaypointIndex(uint8_t index); // MSP2_INAV_SET_WP_INDEX: jump to WP during active mission bool navSetCruiseHeading(int32_t headingCd); // MSP2_INAV_SET_CRUISE_HEADING: set cruise/course-hold heading (centidegrees) @@ -732,7 +735,8 @@ typedef enum { typedef enum { NAV_WP_TAKEOFF_DATUM, NAV_WP_MSL_DATUM, - NAV_WP_TERRAIN_DATUM + NAV_WP_TERRAIN_DATUM, + NAV_WP_RELATIVE_DATUM } geoAltitudeDatumFlag_e; // geoSetOrigin stores the location provided in llh as a GPS origin in the @@ -765,11 +769,20 @@ bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypoint bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); // NOT USED uint32_t distanceToFirstWP(void); +/* Commanded normal RTH mode */ +bool activateRTHMode(void); + +/* Commanded normal position hold mode */ +bool activatePositionHoldMode(void); + /* Failsafe-forced RTH mode */ void activateForcedRTH(void); void abortForcedRTH(void); rthState_e getStateOfForcedRTH(void); +/* Commanded normal landing mode */ +bool activateForcedLanding(void); + /* Failsafe-forced Emergency Landing mode */ void activateForcedEmergLanding(void); void abortForcedEmergLanding(void); @@ -783,6 +796,8 @@ bool navigationIsFlyingAutonomousMode(void); bool navigationIsExecutingAnEmergencyLanding(void); bool navigationIsControllingAltitude(void); bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm); +bool navigationConsumeWaypointReached(uint16_t *seq); +bool navCanSetHome(void); /* Returns true if navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. */ diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f7b43bf2bd5..0b954f276b9 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -332,7 +332,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Limit minimum forward velocity to 1 m/s float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); - uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius); + uint32_t navLoiterRadius = getLoiterRadius(navigationGetLoiterRadius()); fpVector3_t loiterCenterPos = posControl.desiredState.pos; int8_t loiterTurnDirection = loiterDirection(); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 9f4d546f98c..c59c30a5ae1 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -104,6 +104,7 @@ typedef struct navigationFlags_s { // Failsafe actions bool forcedRTHActivated; + bool forcedLandingActivated; bool forcedEmergLandingActivated; /* Landing detector */ @@ -167,6 +168,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_MIXERAT, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING, NAV_FSM_EVENT_SWITCH_TO_SEND_TO, + NAV_FSM_EVENT_SWITCH_TO_LANDING, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event @@ -455,6 +457,7 @@ typedef struct { /* Local system state, both actual (estimated) and desired (target setpoint)*/ navigationEstimatedState_t actualState; navigationDesiredState_t desiredState; // waypoint coordinates + velocity + uint32_t gcsLoiterRadiusOverride; // Temporary GCS PosHold loiter radius override, cm; 0 = configured default uint32_t lastValidPositionTimeMs; uint32_t lastValidAltitudeTimeMs; @@ -477,6 +480,7 @@ typedef struct { /* Waypoint list */ navWaypoint_t waypointList[NAV_MAX_WAYPOINTS]; + navWaypoint_t commandLandingWaypoint; bool waypointListValid; int8_t waypointCount; // number of WPs in loaded mission int8_t startWpIndex; // index of first waypoint in mission @@ -498,9 +502,9 @@ typedef struct { float wpInitialDistance; // Distance when starting flight to WP float wpDistance; // Distance to active WP timeMs_t wpReachedTime; // Time the waypoint was reached + bool wpAltitudeReached; // WP altitude achieved uint16_t wpReachedSeq; // Last reached mission item sequence relative to startWpIndex bool wpReachedNotificationPending; - bool wpAltitudeReached; // WP altitude achieved #ifdef USE_FW_AUTOLAND /* Fixedwing autoland */ diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 8c988d78384..ec2deb2ba67 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -490,6 +490,14 @@ static int logicConditionCompute( return navigationSetAltitudeTargetWithDatum((geoAltitudeDatumFlag_e)operandA, operandB); break; + case LOGIC_CONDITION_ACTIVATE_RTH: + return activateRTHMode(); + break; + + case LOGIC_CONDITION_ACTIVATE_LANDING: + return activateForcedLanding(); + break; + case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE: if (operandA >= 0 && operandA <= 2) { diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 183c0cfa054..2e63a6cd1d7 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -90,6 +90,8 @@ typedef enum { LOGIC_CONDITION_ACOS = 58, LOGIC_CONDITION_ASIN = 59, LOGIC_CONDITION_ATAN2 = 60, + LOGIC_CONDITION_ACTIVATE_RTH = 61, + LOGIC_CONDITION_ACTIVATE_LANDING = 62, LOGIC_CONDITION_LAST } logicOperation_e; diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index df3fd1b9923..83b760dad00 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -643,17 +643,323 @@ TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams) EXPECT_EQ(lastWaypoint.p1, 123); } +TEST(MavlinkTelemetryTest, ComponentArmDisarmUsesFcArmingPath) +{ + initMavlinkTestState(); + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_COMPONENT_ARM_DISARM, + 0, + 1.0f, 0, 0, 0, 0, 0, 0); + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + EXPECT_EQ(ack.command, MAV_CMD_COMPONENT_ARM_DISARM); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(setArmStateCalls, 1); + EXPECT_TRUE(requestedArmState); +} +TEST(MavlinkTelemetryTest, ReturnToLaunchRequiresArmedState) +{ + initMavlinkTestState(); + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_NAV_RETURN_TO_LAUNCH, + 0, + 0, 0, 0, 0, 0, 0, 0); + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_DENIED); + EXPECT_EQ(activateRthCalls, 0); +} + +TEST(MavlinkTelemetryTest, ReturnToLaunchUsesNormalRthModePath) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + forcedRthState = RTH_IN_PROGRESS; + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_NAV_RETURN_TO_LAUNCH, + 0, + 0, 0, 0, 0, 0, 0, 0); + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateRthCalls, 1); +} +TEST(MavlinkTelemetryTest, DoSetModeRtlRequiresArmedState) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PLANE_MODE_RTL, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_DENIED); + EXPECT_EQ(activateRthCalls, 0); +} + +TEST(MavlinkTelemetryTest, DoSetModeRtlUsesNormalRthModePath) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + forcedRthState = RTH_IN_PROGRESS; + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PLANE_MODE_RTL, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateRthCalls, 1); +} + +TEST(MavlinkTelemetryTest, DoSetModeCopterRtlUsesNormalRthModePath) +{ + initMavlinkTestState(); + mixerProfilesMutable(0)->mixer_config.platformType = PLATFORM_MULTIROTOR; + ENABLE_ARMING_FLAG(ARMED); + forcedRthState = RTH_IN_PROGRESS; + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, COPTER_MODE_RTL, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateRthCalls, 1); +} + +TEST(MavlinkTelemetryTest, DoSetModeLoiterUsesNormalPositionHoldModePath) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PLANE_MODE_LOITER, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateRthCalls, 0); + EXPECT_EQ(activatePositionHoldCalls, 1); +} + +TEST(MavlinkTelemetryTest, DoSetModeUnsupportedModeStaysUnsupported) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PLANE_MODE_CRUISE, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_UNSUPPORTED); + EXPECT_EQ(activateRthCalls, 0); + EXPECT_EQ(activatePositionHoldCalls, 0); +} + +TEST(MavlinkTelemetryTest, LandUsesNormalForcedLandingPath) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_NAV_LAND, + 0, + 0, 0, 0, 0, 37.5f, -122.25f, 12.3f); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateLandingCalls, 1); +} + +TEST(MavlinkTelemetryTest, TakeoffFoundationReturnsUnsupported) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_NAV_TAKEOFF, + 0, + 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_UNSUPPORTED); +} + +TEST(MavlinkTelemetryTest, SetHomeUsesWaypointZeroAndConvertsAbsoluteAltitude) +{ + initMavlinkTestState(); + posControl.gpsOrigin.alt = 1000; + + mavlink_message_t cmd; + mavlink_msg_command_int_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_FRAME_GLOBAL_INT, + MAV_CMD_DO_SET_HOME, + 0, 0, + 0, 0, 0, 0, + 375000000, -1222500000, 34.56f); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(lastWaypointNumber, 0); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_NEAR((double)lastWaypoint.lon, -1222500000.0, 100.0); + EXPECT_EQ(lastWaypoint.alt, 2456); +} + +TEST(MavlinkTelemetryTest, SetHomeCurrentPositionUsesGpsLocation) +{ + initMavlinkTestState(); + posControl.gpsOrigin.alt = 1000; + gpsSol.llh.lat = 375000000; + gpsSol.llh.lon = -1222500000; + gpsSol.llh.alt = 3456; + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_HOME, + 0, + 1.0f, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(lastWaypointNumber, 0); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); + EXPECT_EQ(lastWaypoint.alt, 2456); +} TEST(MavlinkTelemetryTest, BroadcastCommandLongRepositionExecutesLocally) { @@ -772,6 +1078,34 @@ TEST(MavlinkTelemetryTest, CommandIntRepositionScalesCoordinates) EXPECT_EQ(lastWaypoint.p1, 45); } +TEST(MavlinkTelemetryTest, CommandIntRepositionSetsLoiterRadiusOverride) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_int_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_DO_REPOSITION, + 0, 0, + 0, 0, 42.5f, 45.6f, + 375000000, -1222500000, 12.3f); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_COMMAND_ACK); + + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_REPOSITION); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(lastLoiterRadiusOverride, 4250U); +} TEST(MavlinkTelemetryTest, MissionClearAllAcksAndResets) { @@ -1080,7 +1414,61 @@ TEST(MavlinkTelemetryTest, MissionItemIntNonFinalAutocontinueZeroIsRejected) EXPECT_EQ(ack.type, MAV_MISSION_UNSUPPORTED); } +TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedUpdatesWaypoint) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t msg; + mavlink_msg_mission_item_int_pack( + 42, 200, &msg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 2, 1, + 0, 0, 0, 0, + 375000000, -1222500000, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); + EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f)); + EXPECT_EQ(lastWaypoint.p3, 0); +} + +TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedCurrentThreeChangesAltitude) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t msg; + mavlink_msg_mission_item_int_pack( + 42, 200, &msg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 3, 1, + 0, 0, 0, 0, + 375000000, -1222500000, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK); + + mavlink_mission_ack_t ack; + mavlink_msg_mission_ack_decode(&ackMsg, &ack); + EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED); + EXPECT_EQ(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetDatum, NAV_WP_TAKEOFF_DATUM); + EXPECT_EQ(lastAltitudeTargetCm, 1230); +} TEST(MavlinkTelemetryTest, MissionRequestListSendsCount) { @@ -1170,6 +1558,27 @@ TEST(MavlinkTelemetryTest, MissionItemReachedIsBroadcastOnceWhenPending) EXPECT_FALSE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &reachedMsg)); } +TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesAbsoluteAltitude) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t msg; + mavlink_msg_mission_item_pack( + 42, 200, &msg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL, + MAV_CMD_NAV_WAYPOINT, 2, 1, + 0, 0, 0, 0, + 37.5f, -122.25f, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE); +} TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam) { @@ -1216,12 +1625,183 @@ TEST(MavlinkTelemetryTest, BroadcastParamRequestListRespondsWithEmptyParam) EXPECT_EQ(param.param_index, 0); } +TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntSetsWaypoint) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, + 375000000, -1222500000, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); + EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f)); + EXPECT_EQ(lastWaypoint.p3, 0); +} + +TEST(MavlinkTelemetryTest, BroadcastSetPositionTargetGlobalIntSetsWaypoint) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 0, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, + 375000000, -1222500000, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); +} + +TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_GLOBAL_INT, 0, + 375000000, -1222500000, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE); +} + +TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntAltitudeOnlyRequiresValidGcs) +{ + initMavlinkTestState(); + gcsValid = false; + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0, 0, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 0); +} + +TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedAltitudeOnlySetsAltitudeTarget) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetDatum, NAV_WP_TAKEOFF_DATUM); + EXPECT_EQ(lastAltitudeTargetCm, 1250); +} + +TEST(MavlinkTelemetryTest, BroadcastSetPositionTargetLocalNedAltitudeOnlySetsAltitudeTarget) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 0, 0, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + EXPECT_EQ(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetCm, 1250); +} +TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedIgnoresXyMotionRequests) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 1.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + EXPECT_EQ(altitudeTargetSetCalls, 0); +} TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) { diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc index 7968233c9f5..623c93fafb9 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc @@ -34,6 +34,7 @@ extern "C" { #include "fc/rc_controls.h" #include "fc/rc_modes.h" + #include "fc/runtime_config.h" @@ -124,6 +125,15 @@ static void resetRCDeviceStatus() clearResponseBuff(); } +static void resetRcModeActivationForTest() +{ + DISABLE_ARMING_FLAG(ARMED); + + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + rcModeUpdate(&mask); +} + static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlushSerial) { UNUSED(withDataForFlushSerial); @@ -132,6 +142,75 @@ static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlus testData.responseBufCount++; } +TEST(RCModeTest, ActivationOverrideSurvivesUnrelatedModeChange) +{ + resetRcModeActivationForTest(); + + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + bitArraySet(mask.bits, BOXANGLE); + rcModeUpdate(&mask); + + ENABLE_ARMING_FLAG(ARMED); + rcModeSetActivationOverride(BOXNAVRTH); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXANGLE)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + + bitArraySet(mask.bits, BOXCAMERA1); + rcModeUpdate(&mask); + + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXANGLE)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXCAMERA1)); + + resetRcModeActivationForTest(); +} + +TEST(RCModeTest, ActivationOverrideClearsOnFlightModeChange) +{ + resetRcModeActivationForTest(); + + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + bitArraySet(mask.bits, BOXANGLE); + rcModeUpdate(&mask); + + ENABLE_ARMING_FLAG(ARMED); + rcModeSetActivationOverride(BOXNAVRTH); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + + bitArrayClr(mask.bits, BOXANGLE); + bitArraySet(mask.bits, BOXHORIZON); + rcModeUpdate(&mask); + + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXHORIZON)); + + resetRcModeActivationForTest(); +} + +TEST(RCModeTest, ActivationOverrideClearsWhenDisarmed) +{ + resetRcModeActivationForTest(); + + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + bitArraySet(mask.bits, BOXANGLE); + rcModeUpdate(&mask); + + ENABLE_ARMING_FLAG(ARMED); + rcModeSetActivationOverride(BOXNAVRTH); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + + DISABLE_ARMING_FLAG(ARMED); + rcModeUpdate(&mask); + + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXANGLE)); + + resetRcModeActivationForTest(); +} + TEST(RCDeviceTest, TestRCSplitInitWithoutPortConfigurated) { runcamDevice_t device; @@ -949,7 +1028,7 @@ extern "C" { uint32_t millis(void) { return testData.millis++; } uint32_t micros(void) { return millis() * 1000; } void beeper(beeperMode_e mode) { UNUSED(mode); } - uint8_t armingFlags = 0; + uint32_t armingFlags = 0; bool cmsInMenu; uint32_t resumeRefreshAt = 0; int getArmingDisableFlags(void) {return 0;}