diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md
index 9a4c64e6e39..b0941d90897 100644
--- a/docs/development/msp/inav_enums_ref.md
+++ b/docs/development/msp/inav_enums_ref.md
@@ -1385,7 +1385,7 @@
| `DEVHW_MS4525` | 49 | |
| `DEVHW_DLVR` | 50 | |
| `DEVHW_M25P16` | 51 | |
-| `DEVHW_W25N01G` | 52 | |
+| `DEVHW_W25N` | 52 | |
| `DEVHW_UG2864` | 53 | |
| `DEVHW_SDCARD` | 54 | |
| `DEVHW_IRLOCK` | 55 | |
@@ -1994,6 +1994,7 @@
|---|---:|---|
| `NAV_WP_TAKEOFF_DATUM` | 0 | |
| `NAV_WP_MSL_DATUM` | 1 | |
+| `NAV_WP_TERRAIN_DATUM` | 2 | |
---
## `geoOriginResetMode_e`
@@ -2843,6 +2844,7 @@
| `LOGIC_CONDITION_OPERAND_FLIGHT_MIN_GROUND_SPEED` | 46 | |
| `LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED` | 47 | |
| `LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION` | 48 | |
+| `LOGIC_CONDITION_OPERAND_FLIGHT_RELATIVE_WIND_OFFSET` | 49 | |
---
## `logicOperation_e`
@@ -2908,7 +2910,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`
@@ -4740,7 +4743,7 @@
---
## `sdcardReceiveBlockStatus_e`
-> Source: ../../../src/main/drivers/sdcard/sdcard_spi.c
+> Source: ../../../src/main/drivers/sdcard/sdcard_sdio.c
| Enumerator | Value | Condition |
|---|---:|---|
@@ -4751,7 +4754,7 @@
---
## `sdcardReceiveBlockStatus_e`
-> Source: ../../../src/main/drivers/sdcard/sdcard_sdio.c
+> Source: ../../../src/main/drivers/sdcard/sdcard_spi.c
| Enumerator | Value | Condition |
|---|---:|---|
diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum
index 3c235d8aaf9..f90eaa7b1bf 100644
--- a/docs/development/msp/msp_messages.checksum
+++ b/docs/development/msp/msp_messages.checksum
@@ -1 +1 @@
-ca27e198f4405b721ad8a15719e15e5d
+7601652331e47d41941de8efdcb0c1e7
diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index dde6d3ecef5..3710e411153 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -10829,6 +10829,61 @@
"notes": "Requires `USE_GEOZONE`. Expects 10 bytes (Polygon) or 14 bytes (Circular). Returns error if indexes invalid or if trying to set vertex beyond `vertexCount` defined in `MSP2_INAV_SET_GEOZONE`. Calls `geozoneSetVertex()`. For circular zones, sets center (vertex 0) and radius (vertex 1's latitude).",
"description": "Sets a specific vertex (or center+radius for circular zones) for a Geozone."
},
+ "MSP2_INAV_ALT_TARGET": {
+ "code": 8725,
+ "mspv": 2,
+ "variable_len": true,
+ "variants": {
+ "get": {
+ "description": "Get current altitude target",
+ "request": {
+ "payload": null
+ },
+ "reply": {
+ "payload": [
+ {
+ "name": "altitudeDatum",
+ "ctype": "uint8_t",
+ "desc": "Default internal reference altitude datum `NAV_WP_TAKEOFF_DATUM`",
+ "units": "",
+ "enum": "geoAltitudeDatumFlag_e"
+ },
+ {
+ "name": "altitudeTarget",
+ "ctype": "int32_t",
+ "desc": "Current altitude target (`posControl.desiredState.pos.z`)",
+ "units": "cm"
+ }
+ ]
+ }
+ },
+ "set": {
+ "description": "Set new altitude target",
+ "request": {
+ "payload": [
+ {
+ "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"
+ },
+ {
+ "name": "altitudeTarget",
+ "ctype": "int32_t",
+ "desc": "Desired altitude target according to reference datum",
+ "units": "cm"
+ }
+ ]
+ },
+ "reply": {
+ "payload": null
+ }
+ }
+ },
+ "notes": "Empty request payload returns the current altitude target with datum. Sending a 5-byte payload sets a new target: 1 byte datum, 4 bytes altitude. 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": "Get or set the active altitude hold target using updateClimbRateToAltitudeController."
+ },
"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 97851bcec1f..ba1e32ac9a2 100644
--- a/docs/development/msp/msp_ref.md
+++ b/docs/development/msp/msp_ref.md
@@ -10,7 +10,8 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
For current generation code, see [documentation project](https://github.com/xznhj8129/msp_documentation) (temporary until official implementation)
-**JSON file rev: 2**
+**JSON file rev: 3
+**
**Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty**
@@ -411,6 +412,7 @@ For current generation code, see [documentation project](https://github.com/xznh
[8721 - MSP2_INAV_SET_GEOZONE](#msp2_inav_set_geozone)
[8722 - MSP2_INAV_GEOZONE_VERTEX](#msp2_inav_geozone_vertex)
[8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex)
+[8725 - MSP2_INAV_ALT_TARGET](#msp2_inav_alt_target)
[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)
[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)
@@ -4492,6 +4494,35 @@ For current generation code, see [documentation project](https://github.com/xznh
**Notes:** Requires `USE_GEOZONE`. Expects 10 bytes (Polygon) or 14 bytes (Circular). Returns error if indexes invalid or if trying to set vertex beyond `vertexCount` defined in `MSP2_INAV_SET_GEOZONE`. Calls `geozoneSetVertex()`. For circular zones, sets center (vertex 0) and radius (vertex 1's latitude).
+## `MSP2_INAV_ALT_TARGET (8725 / 0x2215)`
+**Description:** Get or set the active altitude hold target using updateClimbRateToAltitudeController.
+#### Variant: `get`
+
+**Description:** Get current altitude target
+
+**Request Payload:** **None**
+
+**Reply 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) | Default internal reference altitude datum `NAV_WP_TAKEOFF_DATUM` |
+| `altitudeTarget` | `int32_t` | 4 | cm | Current altitude target (`posControl.desiredState.pos.z`) |
+
+#### Variant: `set`
+
+**Description:** Set new altitude target
+
+**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`, `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:** Empty request payload returns the current altitude target with datum. Sending a 5-byte payload sets a new target: 1 byte datum, 4 bytes altitude. 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_FULL_LOCAL_POSE (8736 / 0x2220)`
**Description:** Provides estimates of current attitude, local NEU position, and velocity.
diff --git a/docs/development/msp/original_msp_ref.md b/docs/development/msp/original_msp_ref.md
index d635bbc6843..209ca629481 100644
--- a/docs/development/msp/original_msp_ref.md
+++ b/docs/development/msp/original_msp_ref.md
@@ -2,7 +2,6 @@
# WARNING: DEPRECATED, OBSOLETE, FULL OF ERRORS, DO NOT USE AS REFERENCE!!!
# (OBSOLETE) INAV MSP Messages reference
-**Warning: Work in progress**\
**Generated with Gemini 2.5 Pro Preview O3-25 on source code files**\
**Verification needed, exercise caution until completely verified for accuracy and cleared**\
**Refer to source for absolute certainty**
diff --git a/docs/development/msp/rev b/docs/development/msp/rev
index d8263ee9860..00750edc07d 100644
--- a/docs/development/msp/rev
+++ b/docs/development/msp/rev
@@ -1 +1 @@
-2
\ No newline at end of file
+3
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 54c225c78bc..5ed2f29d067 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -4111,6 +4111,35 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = mspFcGeozoneVerteciesOutCommand(dst, src);
break;
#endif
+
+#ifdef USE_BARO
+ case MSP2_INAV_ALT_TARGET:
+ {
+ if (dataSize == 0) {
+ sbufWriteU8(dst, NAV_WP_TAKEOFF_DATUM);
+ sbufWriteU32(dst, (uint32_t)lrintf(posControl.desiredState.pos.z));
+ *ret = MSP_RESULT_ACK;
+ break;
+ }
+
+ if (dataSize != (sizeof(int32_t) + sizeof(uint8_t))) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ const uint8_t datumFlag = sbufReadU8(src);
+ const int32_t targetAltitudeCm = (int32_t)sbufReadU32(src);
+
+ if (!navigationSetAltitudeTargetWithDatum((geoAltitudeDatumFlag_e)datumFlag, targetAltitudeCm)) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ *ret = MSP_RESULT_ACK;
+ break;
+ }
+#endif
+
#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 b44aa539887..217409f8422 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -123,4 +123,6 @@
#define MSP2_INAV_GEOZONE_VERTEX 0x2212
#define MSP2_INAV_SET_GEOZONE_VERTEX 0x2213
-#define MSP2_INAV_FULL_LOCAL_POSE 0x2220
\ No newline at end of file
+#define MSP2_INAV_ALT_TARGET 0x2215
+
+#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..fa1712e1f7f 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -471,6 +471,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) {
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/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index d5084b32a62..13e6c0344b7 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;
@@ -1226,6 +1225,45 @@ static bool handleIncoming_COMMAND_INT(void)
mavlinkSendMessage();
}
return true;
+#ifdef USE_BARO
+ } else 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
}
return false;
}
@@ -1355,7 +1393,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: