diff --git a/docs/development/msp/docs_v2_header.md b/docs/development/msp/docs_v2_header.md
index 41656f2178e..88c057f898e 100644
--- a/docs/development/msp/docs_v2_header.md
+++ b/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)
diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md
index 258c44d5713..765a2c0177e 100644
--- a/docs/development/msp/inav_enums_ref.md
+++ b/docs/development/msp/inav_enums_ref.md
@@ -4740,7 +4740,7 @@
---
## `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 @@
---
## `sdcardReceiveBlockStatus_e`
-> Source: ../../../src/main/drivers/sdcard/sdcard_spi.c
+> Source: ../../../src/main/drivers/sdcard/sdcard_sdio.c
| Enumerator | Value | Condition |
|---|---:|---|
diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index 8bd766dd324..dde6d3ecef5 100644
--- a/docs/development/msp/msp_messages.json
+++ b/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,
diff --git a/docs/development/msp/msp_ref.md b/docs/development/msp/msp_ref.md
index da7b64c8134..97851bcec1f 100644
--- a/docs/development/msp/msp_ref.md
+++ b/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)
## `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.
## `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).
+## `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.
+
## `MSP2_BETAFLIGHT_BIND (12288 / 0x3000)`
**Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2).
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index d2f429d0e5c..1d2db838d9b 100644
--- a/src/main/fc/fc_msp.c
+++ b/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());
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index dcbdeb5e71f..64e869b49d5 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/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
\ No newline at end of file
+#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
\ No newline at end of file