diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json
index ebd1fba018a..e0fce4ef268 100644
--- a/docs/development/msp/inav_enums.json
+++ b/docs/development/msp/inav_enums.json
@@ -1349,7 +1349,8 @@
"geoAltitudeDatumFlag_e": {
"_source": "inav/src/main/navigation/navigation.h",
"NAV_WP_TAKEOFF_DATUM": "0",
- "NAV_WP_MSL_DATUM": "1"
+ "NAV_WP_MSL_DATUM": "1",
+ "NAV_WP_TERRAIN_DATUM": "2"
},
"geoOriginResetMode_e": {
"_source": "inav/src/main/navigation/navigation.h",
@@ -2053,7 +2054,8 @@
"LOGIC_CONDITION_RESET_MAG_CALIBRATION": "54",
"LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY": "55",
"LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED": "56",
- "LOGIC_CONDITION_LAST": "57"
+ "LOGIC_CONDITION_SET_ALTITUDE_TARGET": "57",
+ "LOGIC_CONDITION_LAST": "58"
},
"logicWaypointOperands_e": {
"_source": "inav/src/main/programming/logic_condition.h",
diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md
index 864efcd8a7e..d258c66d4a7 100644
--- a/docs/development/msp/inav_enums_ref.md
+++ b/docs/development/msp/inav_enums_ref.md
@@ -2206,6 +2206,7 @@
|---|---:|---|
| `NAV_WP_TAKEOFF_DATUM` | 0 | |
| `NAV_WP_MSL_DATUM` | 1 | |
+| `NAV_WP_TERRAIN_DATUM` | 2 | |
---
## `geoOriginResetMode_e`
@@ -3153,7 +3154,8 @@
| `LOGIC_CONDITION_RESET_MAG_CALIBRATION` | 54 | |
| `LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY` | 55 | |
| `LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED` | 56 | |
-| `LOGIC_CONDITION_LAST` | 57 | |
+| `LOGIC_CONDITION_SET_ALTITUDE_TARGET` | 57 | |
+| `LOGIC_CONDITION_LAST` | 58 | |
---
## `logicWaypointOperands_e`
diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum
index ff3e21a1d69..ae689c4b8eb 100644
--- a/docs/development/msp/msp_messages.checksum
+++ b/docs/development/msp/msp_messages.checksum
@@ -1 +1 @@
-82a3d2eee9d0d1fe609363a08405ed21
+6e8861395a4275489a12012e6985d733
diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index e4c2a39993f..0824a199416 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -5335,15 +5335,15 @@
"request": {
"payload": [
{
- "name": "heading",
- "ctype": "uint16_t",
- "desc": "Target heading (0-359)",
- "units": "degrees"
+ "name": "heading",
+ "ctype": "uint16_t",
+ "desc": "Target heading (0-359)",
+ "units": "degrees"
}
]
},
"reply": null,
- "notes": "Expects 2 bytes. Calls `updateHeadingHoldTarget()`.",
+ "notes": "Expects 2 bytes. Calls `updateHeadingHoldTarget()`. Also synchronizes navigation yaw targets (including cruise/course) when NAV is controlling yaw.",
"description": "Sets the target heading for the heading hold controller (e.g., during MAG mode)."
},
"MSP_SET_SERVO_CONFIGURATION": {
@@ -10853,6 +10853,271 @@
"notes": "Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 5 bytes. Returns error if index is outside `MAX_GLOBAL_VARIABLES`.",
"description": "Sets the specified Global Variable (GVAR) to the provided value."
},
+ "MSP2_INAV_SET_ALT_TARGET": {
+ "code": 8725,
+ "mspv": 2,
+ "request": {
+ "payload": [
+ {
+ "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)",
+ "units": "",
+ "enum": "geoAltitudeDatumFlag_e"
+ },
+ {
+ "name": "altitudeTarget",
+ "ctype": "int32_t",
+ "desc": "Desired altitude target according to reference datum",
+ "units": "cm"
+ }
+ ]
+ },
+ "reply": {
+ "payload": null
+ },
+ "notes": "Set new altitude target. Requires 5-byte payload (datum + target) and is set-only. Valid only in NAV or ALTHOLD modes. Command is rejected unless altitude control is active, not landing/emergency landing, altitude estimation is valid, and datum is supported (MSL requires valid GPS origin; TERRAIN is reserved and rejected).",
+ "description": "Set the active altitude hold target using updateClimbRateToAltitudeController."
+ },
+ "MSP2_INAV_FLIGHT_AXIS_ANGLE_OVERRIDE": {
+ "code": 8726,
+ "mspv": 2,
+ "request": {
+ "payload": [
+ {
+ "name": "overrideMask",
+ "ctype": "uint8_t",
+ "desc": "Bitmask of desired-state fields that follow (Roll, Pitch, Yaw). Non-zero enables the override; zero disables it for that axis.",
+ "units": "Bitmask",
+ "bitmask": true
+ },
+ {
+ "name": "angleTargetRoll",
+ "ctype": "int16_t",
+ "desc": "Angle target in deci-degrees. Roll/Pitch clamped to configured angle limits",
+ "units": "deci-degrees"
+ },
+ {
+ "name": "angleTargetPitch",
+ "ctype": "int16_t",
+ "desc": "Angle target in deci-degrees. Roll/Pitch clamped to configured angle limits",
+ "units": "deci-degrees"
+ },
+ {
+ "name": "angleTargetYaw",
+ "ctype": "int16_t",
+ "desc": "Angle target in deci-degrees. Yaw clamped to 0–3600.",
+ "units": "deci-degrees"
+ }
+ ]
+ },
+ "reply": null,
+ "notes": "Uses the same override path as logic conditions and bypasses stick-derived angle targets.",
+ "description": "Enables or disables a flight-axis angle override for the selected axis."
+ },
+ "MSP2_INAV_FLIGHT_AXIS_RATE_OVERRIDE": {
+ "code": 8727,
+ "mspv": 2,
+ "request": {
+ "payload": [
+ {
+ "name": "overrideMask",
+ "ctype": "uint8_t",
+ "desc": "Bitmask of desired-state fields that follow (Roll, Pitch, Yaw). Non-zero enables the override; zero disables it for that axis.",
+ "units": "Bitmask",
+ "bitmask": true
+ },
+ {
+ "name": "rateTargetRoll",
+ "ctype": "int16_t",
+ "desc": "Rate target, clamped to ±2000",
+ "units": "deg/s"
+ },
+ {
+ "name": "rateTargetPitch",
+ "ctype": "int16_t",
+ "desc": "Rate target, clamped to ±2000",
+ "units": "deg/s"
+ },
+ {
+ "name": "rateTargetYaw",
+ "ctype": "int16_t",
+ "desc": "Rate target, clamped to ±2000",
+ "units": "deg/s"
+ }
+ ]
+ },
+ "reply": null,
+ "notes": "Expects 7 bytes. Overrides rate targets just before control is applied, bypassing stick-derived setpoints.",
+ "description": "Enables or disables a flight-axis rate override for the selected axis."
+ },
+ "MSP2_INAV_SET_LOCAL_TARGET": {
+ "code": 8728,
+ "mspv": 2,
+ "request": {
+ "payload": [
+ {
+ "name": "posX",
+ "ctype": "int32_t",
+ "desc": "Desired X in local NEU frame",
+ "units": "cm"
+ },
+ {
+ "name": "posY",
+ "ctype": "int32_t",
+ "desc": "Desired Y in local NEU frame",
+ "units": "cm"
+ },
+ {
+ "name": "posZ",
+ "ctype": "int32_t",
+ "desc": "Desired Z in local NEU frame (up-positive). Omit this field to leave Z unchanged.",
+ "units": "cm",
+ "optional": true
+ }
+ ]
+ },
+ "reply": null,
+ "variable_len": true,
+ "notes": "Offsets are in the vehicle body frame (forward/right/up, cm) and are rotated into the NEU frame using the current yaw, applied relative to current position. Z offset is always provided; Z=0 keeps current altitude, non-zero offsets are relative to current altitude. Requires GCSNAV/offboard to be active and a valid guided poshold; updates the navigation desired position via `setDesiredPosition()`.",
+ "description": "Sets a body-frame offset target relative to the current vehicle position."
+ },
+ "MSP2_INAV_LOCAL_TARGET": {
+ "code": 8729,
+ "mspv": 2,
+ "request": null,
+ "reply": {
+ "payload": [
+ {
+ "name": "posX",
+ "ctype": "int32_t",
+ "desc": "Desired X in local NEU frame (`posControl.desiredState.pos.x`)",
+ "units": "cm"
+ },
+ {
+ "name": "posY",
+ "ctype": "int32_t",
+ "desc": "Desired Y in local NEU frame (`posControl.desiredState.pos.y`)",
+ "units": "cm"
+ },
+ {
+ "name": "posZ",
+ "ctype": "int32_t",
+ "desc": "Desired Z in local NEU frame (`posControl.desiredState.pos.z`, up-positive)",
+ "units": "cm"
+ },
+ {
+ "name": "velX",
+ "ctype": "int16_t",
+ "desc": "Desired X velocity (`posControl.desiredState.vel.x`)",
+ "units": "cm/s"
+ },
+ {
+ "name": "velY",
+ "ctype": "int16_t",
+ "desc": "Desired Y velocity (`posControl.desiredState.vel.y`)",
+ "units": "cm/s"
+ },
+ {
+ "name": "velZ",
+ "ctype": "int16_t",
+ "desc": "Desired Z velocity (`posControl.desiredState.vel.z`)",
+ "units": "cm/s"
+ },
+ {
+ "name": "yaw",
+ "ctype": "int32_t",
+ "desc": "Desired heading (`posControl.desiredState.yaw`)",
+ "units": "centi-degrees"
+ },
+ {
+ "name": "climbRate",
+ "ctype": "int16_t",
+ "desc": "Desired climb rate demand (`posControl.desiredState.climbRateDemand`)",
+ "units": "cm/s"
+ }
+ ]
+ },
+ "notes": "Local frame is NEU. Mirrors `posControl.desiredState` (position, velocity, yaw, climb rate) used by the position controller.",
+ "description": "Returns the current navigation desired state (position, velocity, yaw, and climb rate)."
+ },
+ "MSP2_INAV_SET_GLOBAL_TARGET": {
+ "code": 8730,
+ "mspv": 2,
+ "request": {
+ "payload": [
+ {
+ "name": "latitude",
+ "ctype": "int32_t",
+ "desc": "Latitude coordinate",
+ "units": "deg * 1e7"
+ },
+ {
+ "name": "longitude",
+ "ctype": "int32_t",
+ "desc": "Longitude coordinate",
+ "units": "deg * 1e7"
+ },
+ {
+ "name": "altitudeTarget",
+ "ctype": "int32_t",
+ "desc": "Desired altitude target according to reference datum (0 keeps current altitude)",
+ "units": "cm"
+ },
+ {
+ "name": "altitudeDatum",
+ "ctype": "uint8_t",
+ "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"
+ }
+ ]
+ },
+ "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.",
+ "description": "Sets desired GCS Nav position with global coordinates (WP 254/GOTO)."
+ },
+ "MSP2_INAV_NAV_TARGET": {
+ "code": 8731,
+ "mspv": 2,
+ "request": null,
+ "reply": {
+ "payload": [
+ {
+ "name": "latTarget",
+ "ctype": "int32_t",
+ "desc": "Latitude in degrees * 1e7",
+ "units": "1e-7 deg"
+ },
+ {
+ "name": "lonTarget",
+ "ctype": "int32_t",
+ "desc": "Longitude in degrees * 1e7",
+ "units": "1e-7 deg"
+ },
+ {
+ "name": "altitudeTarget",
+ "ctype": "int32_t",
+ "desc": "Desired altitude target (takeoff datum, cm) as used by altitude/position hold",
+ "units": "cm"
+ },
+ {
+ "name": "headingTarget",
+ "ctype": "uint16_t",
+ "desc": "Current heading-hold target (`getHeadingHoldTarget()`), wrapped to 0–359.99",
+ "units": "degrees"
+ },
+ {
+ "name": "climbRate",
+ "ctype": "int16_t",
+ "desc": "Desired climb rate demand (`posControl.desiredState.climbRateDemand`)",
+ "units": "cm/s"
+ }
+ ]
+ },
+ "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).",
+ "description": "Returns the current navigation desired global target (lat/lon/alt, heading, climb rate)."
+ },
"MSP2_INAV_FULL_LOCAL_POSE": {
"code": 8736,
"mspv": 2,
diff --git a/docs/development/msp/msp_ref.md b/docs/development/msp/msp_ref.md
index d01b978d2eb..2438223efc7 100644
--- a/docs/development/msp/msp_ref.md
+++ b/docs/development/msp/msp_ref.md
@@ -412,6 +412,13 @@ For current generation code, see [documentation project](https://github.com/xznh
[8722 - MSP2_INAV_GEOZONE_VERTEX](#msp2_inav_geozone_vertex)
[8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex)
[8724 - MSP2_INAV_SET_GVAR](#msp2_inav_set_gvar)
+[8725 - MSP2_INAV_SET_ALT_TARGET](#msp2_inav_set_alt_target)
+[8726 - MSP2_INAV_FLIGHT_AXIS_ANGLE_OVERRIDE](#msp2_inav_flight_axis_angle_override)
+[8727 - MSP2_INAV_FLIGHT_AXIS_RATE_OVERRIDE](#msp2_inav_flight_axis_rate_override)
+[8728 - MSP2_INAV_SET_LOCAL_TARGET](#msp2_inav_set_local_target)
+[8729 - MSP2_INAV_LOCAL_TARGET](#msp2_inav_local_target)
+[8730 - MSP2_INAV_SET_GLOBAL_TARGET](#msp2_inav_set_global_target)
+[8731 - MSP2_INAV_NAV_TARGET](#msp2_inav_nav_target)
[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)
[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)
@@ -2474,7 +2481,7 @@ For current generation code, see [documentation project](https://github.com/xznh
**Reply Payload:** **None**
-**Notes:** Expects 2 bytes. Calls `updateHeadingHoldTarget()`.
+**Notes:** Expects 2 bytes. Calls `updateHeadingHoldTarget()`. Also synchronizes navigation yaw targets (including cruise/course) when NAV is controlling yaw.
## `MSP_SET_SERVO_CONFIGURATION (212 / 0xd4)`
**Description:** Sets the configuration for a single servo (legacy format).
@@ -4506,6 +4513,113 @@ For current generation code, see [documentation project](https://github.com/xznh
**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 5 bytes. Returns error if index is outside `MAX_GLOBAL_VARIABLES`.
+## `MSP2_INAV_SET_ALT_TARGET (8725 / 0x2215)`
+**Description:** Set the active altitude hold target using updateClimbRateToAltitudeController.
+
+**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) |
+| `altitudeTarget` | `int32_t` | 4 | cm | Desired altitude target according to reference datum |
+
+**Reply Payload:** **None**
+
+**Notes:** Set new altitude target. Requires 5-byte payload (datum + target) and is set-only. Valid only in NAV or ALTHOLD modes. Command is rejected unless altitude control is active, not landing/emergency landing, altitude estimation is valid, and datum is supported (MSL requires valid GPS origin; TERRAIN is reserved and rejected).
+
+## `MSP2_INAV_FLIGHT_AXIS_ANGLE_OVERRIDE (8726 / 0x2216)`
+**Description:** Enables or disables a flight-axis angle override for the selected axis.
+
+**Request Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `overrideMask` | `uint8_t` | 1 | Bitmask | Bitmask of desired-state fields that follow (Roll, Pitch, Yaw). Non-zero enables the override; zero disables it for that axis. |
+| `angleTargetRoll` | `int16_t` | 2 | deci-degrees | Angle target in deci-degrees. Roll/Pitch clamped to configured angle limits |
+| `angleTargetPitch` | `int16_t` | 2 | deci-degrees | Angle target in deci-degrees. Roll/Pitch clamped to configured angle limits |
+| `angleTargetYaw` | `int16_t` | 2 | deci-degrees | Angle target in deci-degrees. Yaw clamped to 0–3600. |
+
+**Reply Payload:** **None**
+
+**Notes:** Uses the same override path as logic conditions and bypasses stick-derived angle targets.
+
+## `MSP2_INAV_FLIGHT_AXIS_RATE_OVERRIDE (8727 / 0x2217)`
+**Description:** Enables or disables a flight-axis rate override for the selected axis.
+
+**Request Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `overrideMask` | `uint8_t` | 1 | Bitmask | Bitmask of desired-state fields that follow (Roll, Pitch, Yaw). Non-zero enables the override; zero disables it for that axis. |
+| `rateTargetRoll` | `int16_t` | 2 | deg/s | Rate target, clamped to ±2000 |
+| `rateTargetPitch` | `int16_t` | 2 | deg/s | Rate target, clamped to ±2000 |
+| `rateTargetYaw` | `int16_t` | 2 | deg/s | Rate target, clamped to ±2000 |
+
+**Reply Payload:** **None**
+
+**Notes:** Expects 7 bytes. Overrides rate targets just before control is applied, bypassing stick-derived setpoints.
+
+## `MSP2_INAV_SET_LOCAL_TARGET (8728 / 0x2218)`
+**Description:** Sets a body-frame offset target relative to the current vehicle position.
+
+**Request Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `posX` | `int32_t` | 4 | cm | Desired X in local NEU frame |
+| `posY` | `int32_t` | 4 | cm | Desired Y in local NEU frame |
+| `posZ` | `int32_t` | 4 | cm | Desired Z in local NEU frame (up-positive). Omit this field to leave Z unchanged. |
+
+**Reply Payload:** **None**
+
+**Notes:** Offsets are in the vehicle body frame (forward/right/up, cm) and are rotated into the NEU frame using the current yaw, applied relative to current position. Z offset is always provided; Z=0 keeps current altitude, non-zero offsets are relative to current altitude. Requires GCSNAV/offboard to be active and a valid guided poshold; updates the navigation desired position via `setDesiredPosition()`.
+
+## `MSP2_INAV_LOCAL_TARGET (8729 / 0x2219)`
+**Description:** Returns the current navigation desired state (position, velocity, yaw, and climb rate).
+
+**Request Payload:** **None**
+
+**Reply Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `posX` | `int32_t` | 4 | cm | Desired X in local NEU frame (`posControl.desiredState.pos.x`) |
+| `posY` | `int32_t` | 4 | cm | Desired Y in local NEU frame (`posControl.desiredState.pos.y`) |
+| `posZ` | `int32_t` | 4 | cm | Desired Z in local NEU frame (`posControl.desiredState.pos.z`, up-positive) |
+| `velX` | `int16_t` | 2 | cm/s | Desired X velocity (`posControl.desiredState.vel.x`) |
+| `velY` | `int16_t` | 2 | cm/s | Desired Y velocity (`posControl.desiredState.vel.y`) |
+| `velZ` | `int16_t` | 2 | cm/s | Desired Z velocity (`posControl.desiredState.vel.z`) |
+| `yaw` | `int32_t` | 4 | centi-degrees | Desired heading (`posControl.desiredState.yaw`) |
+| `climbRate` | `int16_t` | 2 | cm/s | Desired climb rate demand (`posControl.desiredState.climbRateDemand`) |
+
+**Notes:** Local frame is NEU. Mirrors `posControl.desiredState` (position, velocity, yaw, climb rate) used by the position controller.
+
+## `MSP2_INAV_SET_GLOBAL_TARGET (8730 / 0x221a)`
+**Description:** Sets desired GCS Nav position with global coordinates (WP 254/GOTO).
+
+**Request Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude coordinate |
+| `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) |
+
+**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.
+
+## `MSP2_INAV_NAV_TARGET (8731 / 0x221b)`
+**Description:** Returns the current navigation desired global target (lat/lon/alt, heading, climb rate).
+
+**Request Payload:** **None**
+
+**Reply Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `latTarget` | `int32_t` | 4 | 1e-7 deg | Latitude in degrees * 1e7 |
+| `lonTarget` | `int32_t` | 4 | 1e-7 deg | Longitude in degrees * 1e7 |
+| `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`) |
+
+**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).
+
## `MSP2_INAV_FULL_LOCAL_POSE (8736 / 0x2220)`
**Description:** Provides estimates of current attitude, local NEU position, and velocity.
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 455d5f20897..7eddbfb2dbd 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -113,6 +113,7 @@
#include "rx/rx.h"
#include "rx/msp.h"
+#include "rx/msp_override.h"
#include "rx/srxl2.h"
#include "rx/crsf.h"
@@ -1960,9 +1961,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_HEAD:
- if (sbufReadU16Safe(&tmp_u16, src))
- updateHeadingHoldTarget(tmp_u16);
- else
+ if (sbufReadU16Safe(&tmp_u16, src)) {
+ const int32_t headingCentidegrees = wrap_36000(DEGREES_TO_CENTIDEGREES(tmp_u16));
+ updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(headingCentidegrees));
+
+ if (navGetCurrentStateFlags() & NAV_CTL_YAW) {
+ posControl.desiredState.yaw = headingCentidegrees;
+ posControl.cruise.course = headingCentidegrees;
+ posControl.cruise.previousCourse = headingCentidegrees;
+ }
+ } else
return MSP_RESULT_ERROR;
break;
@@ -2404,6 +2412,33 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
gvSet(gvarIndex, gvarValue);
}
break;
+#endif
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ case MSP2_INAV_FLIGHT_AXIS_ANGLE_OVERRIDE:
+ if (dataSize != 7) {
+ return MSP_RESULT_ERROR;
+ }
+ {
+ const uint8_t overrideMask = sbufReadU8(src);
+ const int16_t rollTarget = (int16_t)sbufReadU16(src);
+ const int16_t pitchTarget = (int16_t)sbufReadU16(src);
+ const int16_t yawTarget = (int16_t)sbufReadU16(src);
+ mspOverrideSetFlightAxisAngleOverride(overrideMask, rollTarget, pitchTarget, yawTarget);
+ }
+ break;
+
+ case MSP2_INAV_FLIGHT_AXIS_RATE_OVERRIDE:
+ if (dataSize != 7) {
+ return MSP_RESULT_ERROR;
+ }
+ {
+ const uint8_t overrideMask = sbufReadU8(src);
+ const int16_t rollTarget = (int16_t)sbufReadU16(src);
+ const int16_t pitchTarget = (int16_t)sbufReadU16(src);
+ const int16_t yawTarget = (int16_t)sbufReadU16(src);
+ mspOverrideSetFlightAxisRateOverride(overrideMask, rollTarget, pitchTarget, yawTarget);
+ }
+ break;
#endif
case MSP2_COMMON_SET_MOTOR_MIXER:
sbufReadU8Safe(&tmp_u8, src);
@@ -4175,6 +4210,127 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = mspFcGeozoneVerteciesOutCommand(dst, src);
break;
#endif
+
+#ifdef USE_BARO
+ case MSP2_INAV_SET_ALT_TARGET:
+ if (dataSize != (sizeof(int32_t) + sizeof(uint8_t))) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ if (navigationSetAltitudeTargetWithDatum((geoAltitudeDatumFlag_e)sbufReadU8(src), (int32_t)sbufReadU32(src))) {
+ *ret = MSP_RESULT_ACK;
+ break;
+ }
+
+ *ret = MSP_RESULT_ERROR;
+ break;
+#endif
+
+ case MSP2_INAV_SET_LOCAL_TARGET:
+ if (dataSize != 3 * sizeof(int32_t) || !isGCSValid()) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ {
+ fpVector3_t targetPos = posControl.desiredState.pos;
+
+ const navEstimatedPosVel_t *actual = navGetCurrentActualPositionAndVelocity();
+ const float offsetBodyX = (float)(int32_t)sbufReadU32(src);
+ const float offsetBodyY = (float)(int32_t)sbufReadU32(src);
+ const float offsetBodyZ = (float)(int32_t)sbufReadU32(src);
+
+ const float cosYaw = posControl.actualState.cosYaw;
+ const float sinYaw = posControl.actualState.sinYaw;
+
+ // Rotate body-frame offsets into NEU and apply relative to current position
+ const float offsetN = offsetBodyX * cosYaw - offsetBodyY * sinYaw;
+ const float offsetE = offsetBodyX * sinYaw + offsetBodyY * cosYaw;
+
+ targetPos.x = actual->pos.x + offsetN;
+ targetPos.y = actual->pos.y + offsetE;
+
+ navSetWaypointFlags_t updateMask = NAV_POS_UPDATE_XY;
+ if (offsetBodyZ != 0.0f) {
+ targetPos.z = actual->pos.z + offsetBodyZ;
+ }
+ updateMask |= NAV_POS_UPDATE_Z;
+
+ setDesiredPosition(&targetPos, posControl.desiredState.yaw, updateMask);
+ *ret = MSP_RESULT_ACK;
+ }
+ break;
+
+ case MSP2_INAV_LOCAL_TARGET:
+ sbufWriteU32(dst, lrintf(posControl.desiredState.pos.x));
+ sbufWriteU32(dst, lrintf(posControl.desiredState.pos.y));
+ sbufWriteU32(dst, lrintf(posControl.desiredState.pos.z));
+ sbufWriteU16(dst, (int16_t)lrintf(posControl.desiredState.vel.x));
+ sbufWriteU16(dst, (int16_t)lrintf(posControl.desiredState.vel.y));
+ sbufWriteU16(dst, (int16_t)lrintf(posControl.desiredState.vel.z));
+ sbufWriteU32(dst, posControl.desiredState.yaw);
+ sbufWriteU16(dst, posControl.desiredState.climbRateDemand);
+ *ret = MSP_RESULT_ACK;
+ break;
+
+ case MSP2_INAV_SET_GLOBAL_TARGET:
+ if (dataSize != (3 * sizeof(int32_t) + sizeof(uint8_t)) || !isGCSValid()) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ {
+ gpsLocation_t targetLlh;
+ targetLlh.lat = (int32_t)sbufReadU32(src);
+ targetLlh.lon = (int32_t)sbufReadU32(src);
+ targetLlh.alt = (int32_t)sbufReadU32(src);
+
+ const geoAltitudeDatumFlag_e datumFlag = (geoAltitudeDatumFlag_e)sbufReadU8(src);
+
+ if (datumFlag == NAV_WP_TERRAIN_DATUM) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ fpVector3_t targetPos;
+ if (!geoConvertGeodeticToLocal(&targetPos, &posControl.gpsOrigin, &targetLlh, waypointMissionAltConvMode(datumFlag))) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ navSetWaypointFlags_t updateMask = NAV_POS_UPDATE_XY;
+
+ if (targetLlh.alt != 0) {
+ updateMask |= NAV_POS_UPDATE_Z;
+ }
+
+ setDesiredPosition(&targetPos, posControl.desiredState.yaw, updateMask);
+ *ret = MSP_RESULT_ACK;
+ }
+ break;
+
+ case MSP2_INAV_NAV_TARGET:
+ if (!posControl.gpsOrigin.valid) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ {
+ gpsLocation_t targetLlh;
+ geoConvertLocalToGeodetic(&targetLlh, &posControl.gpsOrigin, &posControl.desiredState.pos);
+
+ sbufWriteU32(dst, targetLlh.lat);
+ sbufWriteU32(dst, targetLlh.lon);
+ sbufWriteU32(dst, lrintf(posControl.desiredState.pos.z));
+
+ const uint16_t headingTarget = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(getHeadingHoldTarget())));
+ sbufWriteU16(dst, headingTarget);
+ sbufWriteU16(dst, posControl.desiredState.climbRateDemand);
+ *ret = MSP_RESULT_ACK;
+ }
+ break;
+
#ifdef USE_SIMULATOR
case MSP_SIMULATOR:
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 0b893916895..02cd758cf02 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -125,4 +125,12 @@
#define MSP2_INAV_SET_GEOZONE_VERTEX 0x2213
#define MSP2_INAV_SET_GVAR 0x2214
+#define MSP2_INAV_SET_ALT_TARGET 0x2215
+#define MSP2_INAV_FLIGHT_AXIS_ANGLE_OVERRIDE 0x2216
+#define MSP2_INAV_FLIGHT_AXIS_RATE_OVERRIDE 0x2217
+#define MSP2_INAV_SET_LOCAL_TARGET 0x2218
+#define MSP2_INAV_LOCAL_TARGET 0x2219
+#define MSP2_INAV_SET_GLOBAL_TARGET 0x221A
+#define MSP2_INAV_NAV_TARGET 0x221B
+
#define MSP2_INAV_FULL_LOCAL_POSE 0x2220
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index e7f0774925e..a3752233149 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -5201,6 +5201,33 @@ bool navigationIsControllingAltitude(void) {
return (stateFlags & NAV_CTL_ALT);
}
+bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm)
+{
+ const navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
+ if (!(stateFlags & NAV_CTL_ALT) || (stateFlags & NAV_CTL_LAND) || navigationIsExecutingAnEmergencyLanding() || posControl.flags.estAltStatus == EST_NONE) {
+ return false;
+ }
+
+ float targetAltitudeLocalCm;
+ switch (datumFlag) {
+ case NAV_WP_TAKEOFF_DATUM:
+ targetAltitudeLocalCm = (float)targetAltitudeCm;
+ break;
+ case NAV_WP_MSL_DATUM:
+ if (!posControl.gpsOrigin.valid) {
+ return false;
+ }
+ targetAltitudeLocalCm = (float)(targetAltitudeCm - posControl.gpsOrigin.alt);
+ break;
+ case NAV_WP_TERRAIN_DATUM:
+ default:
+ return false;
+ }
+
+ updateClimbRateToAltitudeController(0.0f, targetAltitudeLocalCm, ROC_TO_ALT_TARGET);
+ return true;
+}
+
bool navigationIsFlyingAutonomousMode(void)
{
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index ddc7e23e76d..810fdc26ef8 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -729,7 +729,8 @@ typedef enum {
typedef enum {
NAV_WP_TAKEOFF_DATUM,
- NAV_WP_MSL_DATUM
+ NAV_WP_MSL_DATUM,
+ NAV_WP_TERRAIN_DATUM
} geoAltitudeDatumFlag_e;
// geoSetOrigin stores the location provided in llh as a GPS origin in the
@@ -779,6 +780,7 @@ bool isFixedWingAutoThrottleManuallyIncreased(void);
bool navigationIsFlyingAutonomousMode(void);
bool navigationIsExecutingAnEmergencyLanding(void);
bool navigationIsControllingAltitude(void);
+bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm);
/* 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/programming/logic_condition.c b/src/main/programming/logic_condition.c
index edba68b8e94..ee5ebd1202c 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -50,6 +50,7 @@
#include "flight/wind_estimator.h"
#include "drivers/io_port_expander.h"
#include "drivers/gimbal_common.h"
+#include "rx/msp_override.h"
#include "io/osd_common.h"
#include "sensors/diagnostics.h"
@@ -471,6 +472,10 @@ static int logicConditionCompute(
return true;
break;
+ case LOGIC_CONDITION_SET_ALTITUDE_TARGET:
+ return navigationSetAltitudeTargetWithDatum((geoAltitudeDatumFlag_e)operandA, operandB);
+ break;
+
case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE:
if (operandA >= 0 && operandA <= 2) {
@@ -1200,6 +1205,12 @@ uint32_t getMinGroundSpeed(uint32_t minGroundSpeed) {
}
float getFlightAxisAngleOverride(uint8_t axis, float angle) {
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ int mspAngleTarget;
+ if (mspOverrideFlightAxisAngleActive(axis, &mspAngleTarget)) {
+ return mspAngleTarget;
+ }
+#endif
if (flightAxisOverride[axis].angleTargetActive) {
return flightAxisOverride[axis].angleTarget;
} else {
@@ -1208,6 +1219,12 @@ float getFlightAxisAngleOverride(uint8_t axis, float angle) {
}
float getFlightAxisRateOverride(uint8_t axis, float rate) {
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ int mspRateTarget;
+ if (mspOverrideFlightAxisRateActive(axis, &mspRateTarget)) {
+ return mspRateTarget;
+ }
+#endif
if (flightAxisOverride[axis].rateTargetActive) {
return flightAxisOverride[axis].rateTarget;
} else {
@@ -1216,6 +1233,12 @@ float getFlightAxisRateOverride(uint8_t axis, float rate) {
}
bool isFlightAxisAngleOverrideActive(uint8_t axis) {
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ int mspAngleTarget;
+ if (mspOverrideFlightAxisAngleActive(axis, &mspAngleTarget)) {
+ return true;
+ }
+#endif
if (flightAxisOverride[axis].angleTargetActive) {
return true;
} else {
@@ -1224,6 +1247,12 @@ bool isFlightAxisAngleOverrideActive(uint8_t axis) {
}
bool isFlightAxisRateOverrideActive(uint8_t axis) {
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+ int mspRateTarget;
+ if (mspOverrideFlightAxisRateActive(axis, &mspRateTarget)) {
+ return true;
+ }
+#endif
if (flightAxisOverride[axis].rateTargetActive) {
return true;
} else {
diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
index 74ea96c98ee..e9d8eced5eb 100644
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@@ -86,6 +86,7 @@ typedef enum {
LOGIC_CONDITION_RESET_MAG_CALIBRATION = 54,
LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY = 55,
LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED = 56,
+ LOGIC_CONDITION_SET_ALTITUDE_TARGET = 57,
LOGIC_CONDITION_LAST
} logicOperation_e;
diff --git a/src/main/rx/msp_override.c b/src/main/rx/msp_override.c
index 2941cf82cdc..1ff254d0b1d 100755
--- a/src/main/rx/msp_override.c
+++ b/src/main/rx/msp_override.c
@@ -23,6 +23,7 @@
#include "build/build_config.h"
#include "build/debug.h"
+#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
@@ -37,6 +38,7 @@
#include "fc/rc_modes.h"
#include "flight/failsafe.h"
+#include "flight/pid.h"
#include "rx/rx.h"
#include "rx/msp.h"
@@ -53,6 +55,7 @@ static bool rxFailsafe = true;
static timeMs_t rxDataFailurePeriod;
static timeMs_t rxDataRecoveryPeriod;
+static timeMs_t lastAxisOverrideAt = 0;
static timeMs_t validRxDataReceivedAt = 0;
static timeMs_t validRxDataFailedAt = 0;
@@ -64,6 +67,14 @@ static rcChannel_t mspRcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
static rxRuntimeConfig_t rxRuntimeConfigMSP;
+typedef struct mspFlightAxisOverride_s {
+ int angleTarget;
+ int rateTarget;
+ uint8_t angleTargetActive;
+ uint8_t rateTargetActive;
+} mspFlightAxisOverride_t;
+
+static mspFlightAxisOverride_t mspFlightAxisOverride[XYZ_AXIS_COUNT];
void mspOverrideInit(void)
{
@@ -104,6 +115,13 @@ void mspOverrideInit(void)
rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
rxMspInit(rxConfig(), &rxRuntimeConfigMSP);
+
+ for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ mspFlightAxisOverride[axis].angleTargetActive = 0;
+ mspFlightAxisOverride[axis].rateTargetActive = 0;
+ mspFlightAxisOverride[axis].angleTarget = 0;
+ mspFlightAxisOverride[axis].rateTarget = 0;
+ }
}
bool mspOverrideIsReceivingSignal(void)
@@ -121,6 +139,33 @@ bool mspOverrideIsInFailsafe(void)
return rxFailsafe;
}
+static bool mspFlightAxisOverridesEnabled(void)
+{
+ bool enabled;
+ if (rxConfig()->receiverType == RX_TYPE_MSP) {
+ enabled = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && rxIsReceivingSignal() && rxAreFlightChannelsValid();
+ } else {
+ enabled = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe();
+ }
+
+ const timeMs_t nowMs = millis();
+ const timeMs_t overrideTimeoutMs = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
+ const bool freshOverride = lastAxisOverrideAt && (nowMs - lastAxisOverrideAt) <= overrideTimeoutMs;
+ enabled = enabled && freshOverride;
+
+ if (!enabled) {
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ mspFlightAxisOverride[axis].angleTargetActive = 0;
+ mspFlightAxisOverride[axis].rateTargetActive = 0;
+ mspFlightAxisOverride[axis].angleTarget = 0;
+ mspFlightAxisOverride[axis].rateTarget = 0;
+ }
+ lastAxisOverrideAt = 0;
+ }
+
+ return enabled;
+}
+
bool mspOverrideUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
{
UNUSED(currentDeltaTime);
@@ -238,4 +283,74 @@ int16_t mspOverrideGetRawChannelValue(unsigned channelNumber)
return mspRcChannels[channelNumber].raw;
}
+void mspOverrideSetFlightAxisAngleOverride(uint8_t overrideMask, int16_t roll, int16_t pitch, int16_t yaw)
+{
+ lastAxisOverrideAt = millis();
+
+ const int16_t rollTarget = constrain(roll, -pidProfile()->max_angle_inclination[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
+ const int16_t pitchTarget = constrain(pitch, -pidProfile()->max_angle_inclination[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
+ const int16_t yawTarget = constrain(yaw, 0, 3600);
+
+ mspFlightAxisOverride[FD_ROLL].angleTarget = rollTarget;
+ mspFlightAxisOverride[FD_PITCH].angleTarget = pitchTarget;
+ mspFlightAxisOverride[FD_YAW].angleTarget = yawTarget;
+
+ mspFlightAxisOverride[FD_ROLL].angleTargetActive = (overrideMask & 0x01) ? 1 : 0;
+ mspFlightAxisOverride[FD_PITCH].angleTargetActive = (overrideMask & 0x02) ? 1 : 0;
+ mspFlightAxisOverride[FD_YAW].angleTargetActive = (overrideMask & 0x04) ? 1 : 0;
+}
+
+void mspOverrideSetFlightAxisRateOverride(uint8_t overrideMask, int16_t roll, int16_t pitch, int16_t yaw)
+{
+ lastAxisOverrideAt = millis();
+
+ const int16_t rollTarget = constrain(roll, -2000, 2000);
+ const int16_t pitchTarget = constrain(pitch, -2000, 2000);
+ const int16_t yawTarget = constrain(yaw, -2000, 2000);
+
+ mspFlightAxisOverride[FD_ROLL].rateTarget = rollTarget;
+ mspFlightAxisOverride[FD_PITCH].rateTarget = pitchTarget;
+ mspFlightAxisOverride[FD_YAW].rateTarget = yawTarget;
+
+ mspFlightAxisOverride[FD_ROLL].rateTargetActive = (overrideMask & 0x01) ? 1 : 0;
+ mspFlightAxisOverride[FD_PITCH].rateTargetActive = (overrideMask & 0x02) ? 1 : 0;
+ mspFlightAxisOverride[FD_YAW].rateTargetActive = (overrideMask & 0x04) ? 1 : 0;
+}
+
+bool mspOverrideFlightAxisAngleActive(uint8_t axis, int *target)
+{
+ if (!mspFlightAxisOverridesEnabled()) {
+ return false;
+ }
+
+ if (axis >= XYZ_AXIS_COUNT) {
+ return false;
+ }
+
+ if (!mspFlightAxisOverride[axis].angleTargetActive) {
+ return false;
+ }
+
+ *target = mspFlightAxisOverride[axis].angleTarget;
+ return true;
+}
+
+bool mspOverrideFlightAxisRateActive(uint8_t axis, int *target)
+{
+ if (!mspFlightAxisOverridesEnabled()) {
+ return false;
+ }
+
+ if (axis >= XYZ_AXIS_COUNT) {
+ return false;
+ }
+
+ if (!mspFlightAxisOverride[axis].rateTargetActive) {
+ return false;
+ }
+
+ *target = mspFlightAxisOverride[axis].rateTarget;
+ return true;
+}
+
#endif // defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
diff --git a/src/main/rx/msp_override.h b/src/main/rx/msp_override.h
index 441d9c71ea1..31290d30242 100644
--- a/src/main/rx/msp_override.h
+++ b/src/main/rx/msp_override.h
@@ -26,3 +26,7 @@ bool mspOverrideIsInFailsafe(void);
bool mspOverrideAreFlightChannelsValid(void);
int16_t mspOverrideGetChannelValue(unsigned channelNumber);
int16_t mspOverrideGetRawChannelValue(unsigned channelNumber);
+void mspOverrideSetFlightAxisAngleOverride(uint8_t overrideMask, int16_t roll, int16_t pitch, int16_t yaw);
+void mspOverrideSetFlightAxisRateOverride(uint8_t overrideMask, int16_t roll, int16_t pitch, int16_t yaw);
+bool mspOverrideFlightAxisAngleActive(uint8_t axis, int *target);
+bool mspOverrideFlightAxisRateActive(uint8_t axis, int *target);
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index d5084b32a62..60f6aed67c3 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1157,7 +1157,6 @@ static bool handleIncoming_MISSION_REQUEST(void)
return false;
}
-
static bool handleIncoming_COMMAND_INT(void)
{
mavlink_command_int_t msg;
@@ -1216,6 +1215,46 @@ static bool handleIncoming_COMMAND_INT(void)
mavlinkSendMessage();
}
} else {
+#ifdef USE_BARO
+ if (msg.command == MAV_CMD_DO_CHANGE_ALTITUDE) {
+ const float altitudeMeters = msg.param1;
+ const uint8_t frame = (uint8_t)msg.frame;
+
+ geoAltitudeDatumFlag_e datum;
+ switch (frame) {
+ case MAV_FRAME_GLOBAL:
+ case MAV_FRAME_GLOBAL_INT:
+ datum = NAV_WP_MSL_DATUM;
+ break;
+ case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+ case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
+ datum = NAV_WP_TAKEOFF_DATUM;
+ break;
+ default:
+ mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ msg.command,
+ MAV_RESULT_UNSUPPORTED,
+ 0,
+ 0,
+ mavRecvMsg.sysid,
+ mavRecvMsg.compid);
+ mavlinkSendMessage();
+ return true;
+ }
+
+ const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f);
+ const bool accepted = navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm);
+ mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ msg.command,
+ accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED,
+ 0,
+ 0,
+ mavRecvMsg.sysid,
+ mavRecvMsg.compid);
+ mavlinkSendMessage();
+ return true;
+ }
+#endif
mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
msg.command,
MAV_RESULT_UNSUPPORTED,
@@ -1355,7 +1394,7 @@ static bool processMAVLinkIncomingTelemetry(void)
//TODO:
//case MAVLINK_MSG_ID_COMMAND_LONG; //up to 7 float parameters
//return handleIncoming_COMMAND_LONG();
-
+
case MAVLINK_MSG_ID_COMMAND_INT: //7 parameters: parameters 1-4, 7 are floats, and parameters 5,6 are scaled integers
return handleIncoming_COMMAND_INT();
case MAVLINK_MSG_ID_MISSION_REQUEST: