MSP2 message exposing 3D attitude, position, and velocity

docs/development/msp/docs_v2_header.md

@@ -1,7 +1,7 @@
 
 # INAV MSP Messages reference
 
-**This page is auto-generated from the [Master INAV-MSP definition file](https://github.com/xznhj8129/msp_documentation/blob/master/msp_messages.json) (temporary link until merge)**  
+**This page is auto-generated from the [master INAV MSP definitions file](https://github.com/iNavFlight/inav/blob/master/docs/development/msp/msp_messages.json)**  
 
 For details on the structure of MSP, see [The wiki page](https://github.com/iNavFlight/inav/wiki/MSP-V2)
 

docs/development/msp/inav_enums_ref.md

@@ -4740,7 +4740,7 @@
 ---
 ## <a id="enum-sdcardreceiveblockstatus_e"></a>`sdcardReceiveBlockStatus_e`
 
-> Source: ../../../src/main/drivers/sdcard/sdcard_sdio.c
+> Source: ../../../src/main/drivers/sdcard/sdcard_spi.c
 
 | Enumerator | Value | Condition |
 |---|---:|---|
@@ -4751,7 +4751,7 @@
 ---
 ## <a id="enum-sdcardreceiveblockstatus_e"></a>`sdcardReceiveBlockStatus_e`
 
-> Source: ../../../src/main/drivers/sdcard/sdcard_spi.c
+> Source: ../../../src/main/drivers/sdcard/sdcard_sdio.c
 
 | Enumerator | Value | Condition |
 |---|---:|---|

docs/development/msp/msp_messages.json

@@ -3873,13 +3873,13 @@
                 },
                 {
                     "name": "yaw",
-                    "ctype": "uint16_t",
-                    "desc": "Yaw/Heading angle (`attitude.values.yaw`)",
-                    "units": "deci-degrees"
+                    "ctype": "int16_t",
+                    "desc": "Yaw/Heading angle (`DECIDEGREES_TO_DEGREES(attitude.values.yaw)`)",
+                    "units": "degrees"
                 }
             ]
         },
-        "notes": "Yaw is converted from deci-degrees to degrees.",
+        "notes": "Yaw is in degrees.",
         "description": "Provides the current attitude estimate (roll, pitch, yaw)."
     },
     "MSP_ALTITUDE": {
@@ -10829,6 +10829,71 @@
         "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_FULL_LOCAL_POSE": {
+        "code": 8736,
+        "mspv": 2,
+        "request": null,
+        "reply": {
+            "payload": [
+                {
+                    "name": "roll",
+                    "ctype": "int16_t",
+                    "desc": "Roll angle (`attitude.values.roll`)",
+                    "units": "deci-degrees"
+                },
+                {
+                    "name": "pitch",
+                    "ctype": "int16_t",
+                    "desc": "Pitch angle (`attitude.values.pitch`)",
+                    "units": "deci-degrees"
+                },
+                {
+                    "name": "yaw",
+                    "ctype": "int16_t",
+                    "desc": "Yaw/Heading angle (`attitude.values.yaw`)",
+                    "units": "deci-degrees"
+                },
+                {
+                    "name": "localPositionNorth",
+                    "ctype": "int32_t",
+                    "desc": "Estimated North coordinate in local NEU frame (`posControl.actualState.abs.pos.x`)",
+                    "units": "cm"
+                },
+                {
+                    "name": "localVelocityNorth",
+                    "ctype": "int16_t",
+                    "desc": "Estimated North component of velocity in local NEU frame (`posControl.actualState.abs.vel.x`)",
+                    "units": "cm/s"
+                },
+                {
+                    "name": "localPositionEast",
+                    "ctype": "int32_t",
+                    "desc": "Estimated East coordinate in local NEU frame (`posControl.actualState.abs.pos.y`)",
+                    "units": "cm"
+                },
+                {
+                    "name": "localVelocityEast",
+                    "ctype": "int16_t",
+                    "desc": "Estimated East component of velocity in local NEU frame (`posControl.actualState.abs.vel.y`)",
+                    "units": "cm/s"
+                },
+                {
+                    "name": "localPositionUp",
+                    "ctype": "int32_t",
+                    "desc": "Estimated Up coordinate in local NEU frame (`posControl.actualState.abs.pos.z`)",
+                    "units": "cm"
+                },
+                {
+                    "name": "localVelocityUp",
+                    "ctype": "int16_t",
+                    "desc": "Estimated Up component of velocity in local NEU frame (`posControl.actualState.abs.vel.z`)",
+                    "units": "cm/s"
+                }
+            ]
+        },
+        "notes": "All attitude angles are in deci-degrees.",
+        "description": "Provides estimates of current attitude, local NEU position, and velocity."
+    },
     "MSP2_BETAFLIGHT_BIND": {
         "code": 12288,
         "mspv": 2,

docs/development/msp/msp_ref.md

@@ -1,7 +1,7 @@
 
 # INAV MSP Messages reference
 
-**This page is auto-generated from the [Master INAV-MSP definition file](https://github.com/xznhj8129/msp_documentation/blob/master/msp_messages.json) (temporary link until merge)**  
+**This page is auto-generated from the [master INAV MSP definitions file](https://github.com/iNavFlight/inav/blob/master/docs/development/msp/msp_messages.json)**  
 
 For details on the structure of MSP, see [The wiki page](https://github.com/iNavFlight/inav/wiki/MSP-V2)
 
@@ -411,6 +411,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)  
+[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)  
 [12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)  
 
 ## <a id="msp_api_version"></a>`MSP_API_VERSION (1 / 0x1)`
@@ -1896,9 +1897,9 @@ For current generation code, see [documentation project](https://github.com/xznh
 |---|---|---|---|---|
 | `roll` | `int16_t` | 2 | deci-degrees | Roll angle (`attitude.values.roll`) |
 | `pitch` | `int16_t` | 2 | deci-degrees | Pitch angle (`attitude.values.pitch`) |
-| `yaw` | `uint16_t` | 2 | deci-degrees | Yaw/Heading angle (`attitude.values.yaw`) |
+| `yaw` | `int16_t` | 2 | degrees | Yaw/Heading angle (`DECIDEGREES_TO_DEGREES(attitude.values.yaw)`) |
 
-**Notes:** Yaw is converted from deci-degrees to degrees.
+**Notes:** Yaw is in degrees.
 
 ## <a id="msp_altitude"></a>`MSP_ALTITUDE (109 / 0x6d)`
 **Description:** Provides estimated altitude, vertical speed (variometer), and raw barometric altitude.  
@@ -4491,6 +4492,26 @@ 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).
 
+## <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.  
+
+**Request Payload:** **None**  
+
+**Reply Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `roll` | `int16_t` | 2 | deci-degrees | Roll angle (`attitude.values.roll`) |
+| `pitch` | `int16_t` | 2 | deci-degrees | Pitch angle (`attitude.values.pitch`) |
+| `yaw` | `int16_t` | 2 | deci-degrees | Yaw/Heading angle (`attitude.values.yaw`) |
+| `localPositionNorth` | `int32_t` | 4 | cm | Estimated North coordinate in local NEU frame (`posControl.actualState.abs.pos.x`) |
+| `localVelocityNorth` | `int16_t` | 2 | cm/s | Estimated North component of velocity in local NEU frame (`posControl.actualState.abs.vel.x`) |
+| `localPositionEast` | `int32_t` | 4 | cm | Estimated East coordinate in local NEU frame (`posControl.actualState.abs.pos.y`) |
+| `localVelocityEast` | `int16_t` | 2 | cm/s | Estimated East component of velocity in local NEU frame (`posControl.actualState.abs.vel.y`) |
+| `localPositionUp` | `int32_t` | 4 | cm | Estimated Up coordinate in local NEU frame (`posControl.actualState.abs.pos.z`) |
+| `localVelocityUp` | `int16_t` | 2 | cm/s | Estimated Up component of velocity in local NEU frame (`posControl.actualState.abs.vel.z`) |
+
+**Notes:** All attitude angles are in deci-degrees.
+
 ## <a id="msp2_betaflight_bind"></a>`MSP2_BETAFLIGHT_BIND (12288 / 0x3000)`
 **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2).  
 

src/main/fc/fc_msp.c

@@ -632,6 +632,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
         break;
 
+    case MSP2_INAV_FULL_LOCAL_POSE:
+        sbufWriteU16(dst, attitude.values.roll);
+        sbufWriteU16(dst, attitude.values.pitch);
+        sbufWriteU16(dst, attitude.values.yaw);
+        const navEstimatedPosVel_t *absoluteActualState = &posControl.actualState.abs;
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            sbufWriteU32(dst, (int32_t)lrintf(absoluteActualState->pos.v[axis]));
+            sbufWriteU16(dst, (int16_t)lrintf(absoluteActualState->vel.v[axis]));
+        }
+        break;
+
     case MSP_SONAR_ALTITUDE:
 #ifdef USE_RANGEFINDER
         sbufWriteU32(dst, rangefinderGetLatestAltitude());

src/main/msp/msp_protocol_v2_inav.h

@@ -116,7 +116,9 @@
 #define MSP2_INAV_SERVO_CONFIG                  0x2200
 #define MSP2_INAV_SET_SERVO_CONFIG              0x2201
 
-#define MSP2_INAV_GEOZONE                      0x2210
-#define MSP2_INAV_SET_GEOZONE                  0x2211
-#define MSP2_INAV_GEOZONE_VERTEX               0x2212
-#define MSP2_INAV_SET_GEOZONE_VERTEX           0x2213
+#define MSP2_INAV_GEOZONE                       0x2210
+#define MSP2_INAV_SET_GEOZONE                   0x2211
+#define MSP2_INAV_GEOZONE_VERTEX                0x2212
+#define MSP2_INAV_SET_GEOZONE_VERTEX            0x2213
+
+#define MSP2_INAV_FULL_LOCAL_POSE               0x2220