Set altitude target functionality

docs/development/msp/README.md

@@ -9,7 +9,8 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 
 
 
-**JSON file rev: 4**
+**JSON file rev: 4
+**
 
 **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 list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 [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)  
 [8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)  
 [12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)  
 
@@ -4505,6 +4507,19 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 
 **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 5 bytes. Returns error if index is outside `MAX_GLOBAL_VARIABLES`.
 
+## <a id="msp2_inav_set_alt_target"></a>`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`, `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).
+
 ## <a id="msp2_inav_full_local_pose"></a>`MSP2_INAV_FULL_LOCAL_POSE (8736 / 0x2220)`
 **Description:** Provides estimates of current attitude, local NEU position, and velocity.  
 

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 |  |
 
 ---
 ## <a id="enum-geooriginresetmode_e"></a>`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 |  |
 
 ---
 ## <a id="enum-logicwaypointoperands_e"></a>`logicWaypointOperands_e`

docs/development/msp/msp_messages.json

@@ -10853,6 +10853,32 @@
         "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`, `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_FULL_LOCAL_POSE": {
         "code": 8736,
         "mspv": 2,

docs/development/msp/rev

@@ -1 +1 @@
-4
+4

src/main/fc/fc_msp.c

@@ -4175,6 +4175,23 @@ 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
+
 #ifdef USE_SIMULATOR
     case MSP_SIMULATOR:
         tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version

src/main/msp/msp_protocol_v2_inav.h

@@ -125,4 +125,6 @@
 #define MSP2_INAV_SET_GEOZONE_VERTEX            0x2213
 #define MSP2_INAV_SET_GVAR                      0x2214
 
+#define MSP2_INAV_SET_ALT_TARGET                0x2215
+
 #define MSP2_INAV_FULL_LOCAL_POSE               0x2220

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();

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.
  */

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) {
 

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;
 

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: