From 976b3db95b570ce8dcdb127b59c3134789fc63e2 Mon Sep 17 00:00:00 2001 From: yakorch Date: Fri, 19 Sep 2025 20:38:57 +0300 Subject: [PATCH 1/4] add 'MSP2_INAV_FULL_LOCAL_POSE' message that exposes 3D attitude, position, and velocity. --- src/main/fc/fc_msp.c | 20 +++++++++++++++++--- src/main/msp/msp_protocol_v2_inav.h | 10 ++++++---- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6bcea76f847..d9de115c666 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -349,6 +349,13 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, uint16_t } #endif +void mspWriteAttitudePayload(sbuf_t *dst) +{ + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); +} + /* * Returns true if the command was processd, false otherwise. * May set mspPostProcessFunc to a function to be called once the command has been processed @@ -627,9 +634,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_ATTITUDE: - sbufWriteU16(dst, attitude.values.roll); - sbufWriteU16(dst, attitude.values.pitch); - sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); + mspWriteAttitudePayload(dst); break; case MSP_ALTITUDE: @@ -642,6 +647,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif break; + case MSP2_INAV_FULL_LOCAL_POSE: + mspWriteAttitudePayload(dst); + 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 From a158717c1d96bcd8b77483d35fb2a0e9f4f95696 Mon Sep 17 00:00:00 2001 From: yakorch Date: Fri, 19 Sep 2025 21:34:55 +0300 Subject: [PATCH 2/4] revert 'MSP2_INAV_FULL_LOCAL_POSE' attitude compatibility with 'MSP_ATTITUDE' in favor of more accuracy. --- src/main/fc/fc_msp.c | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d9de115c666..d0b7bc4a99c 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -349,13 +349,6 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, uint16_t } #endif -void mspWriteAttitudePayload(sbuf_t *dst) -{ - sbufWriteU16(dst, attitude.values.roll); - sbufWriteU16(dst, attitude.values.pitch); - sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); -} - /* * Returns true if the command was processd, false otherwise. * May set mspPostProcessFunc to a function to be called once the command has been processed @@ -634,7 +627,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_ATTITUDE: - mspWriteAttitudePayload(dst); + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); break; case MSP_ALTITUDE: @@ -648,7 +643,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_FULL_LOCAL_POSE: - mspWriteAttitudePayload(dst); + 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])); From d99447917a5d5ea0fb4d97880889e350e707baa2 Mon Sep 17 00:00:00 2001 From: yakorch Date: Sun, 9 Nov 2025 16:09:02 +0200 Subject: [PATCH 3/4] fix docs error in yaw units of 'MSP_ATTITUDE'; add 'MSP2_INAV_FULL_LOCAL_POSE' docs. --- docs/development/msp/docs_v2_header.md | 2 +- docs/development/msp/inav_enums_ref.md | 136 +++++++++++++++++-------- docs/development/msp/msp_messages.json | 73 ++++++++++++- docs/development/msp/msp_ref.md | 30 +++++- docs/development/msp/rev | 2 +- 5 files changed, 189 insertions(+), 54 deletions(-) diff --git a/docs/development/msp/docs_v2_header.md b/docs/development/msp/docs_v2_header.md index ab9d4703932..ab2618fbcf5 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 2a8d309b27c..765a2c0177e 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -162,6 +162,7 @@ - [ltm_modes_e](#enum-ltm_modes_e) - [ltmUpdateRate_e](#enum-ltmupdaterate_e) - [magSensor_e](#enum-magsensor_e) +- [mavlinkAutopilotType_e](#enum-mavlinkautopilottype_e) - [mavlinkRadio_e](#enum-mavlinkradio_e) - [measurementSteps_e](#enum-measurementsteps_e) - [mixerProfileATRequest_e](#enum-mixerprofileatrequest_e) @@ -205,6 +206,7 @@ - [navWaypointP3Flags_e](#enum-navwaypointp3flags_e) - [opflowQuality_e](#enum-opflowquality_e) - [opticalFlowSensor_e](#enum-opticalflowsensor_e) +- [osd_adsb_warning_style_e](#enum-osd_adsb_warning_style_e) - [osd_ahi_style_e](#enum-osd_ahi_style_e) - [osd_alignment_e](#enum-osd_alignment_e) - [osd_crosshairs_style_e](#enum-osd_crosshairs_style_e) @@ -212,6 +214,7 @@ - [osd_items_e](#enum-osd_items_e) - [osd_sidebar_arrow_e](#enum-osd_sidebar_arrow_e) - [osd_sidebar_scroll_e](#enum-osd_sidebar_scroll_e) +- [osd_SpeedTypes_e](#enum-osd_speedtypes_e) - [osd_stats_energy_unit_e](#enum-osd_stats_energy_unit_e) - [osd_unit_e](#enum-osd_unit_e) - [osdCustomElementType_e](#enum-osdcustomelementtype_e) @@ -1320,7 +1323,8 @@ | `CURRENT_SENSOR_VIRTUAL` | 2 | | | `CURRENT_SENSOR_FAKE` | 3 | | | `CURRENT_SENSOR_ESC` | 4 | | -| `CURRENT_SENSOR_MAX` | CURRENT_SENSOR_FAKE | | +| `CURRENT_SENSOR_SMARTPORT` | 5 | | +| `CURRENT_SENSOR_MAX` | CURRENT_SENSOR_SMARTPORT | | --- ## `devHardwareType_e` @@ -1356,35 +1360,36 @@ | `DEVHW_IST8310_1` | 24 | | | `DEVHW_IST8308` | 25 | | | `DEVHW_QMC5883` | 26 | | -| `DEVHW_MAG3110` | 27 | | -| `DEVHW_LIS3MDL` | 28 | | -| `DEVHW_RM3100` | 29 | | -| `DEVHW_VCM5883` | 30 | | -| `DEVHW_MLX90393` | 31 | | -| `DEVHW_LM75_0` | 32 | | -| `DEVHW_LM75_1` | 33 | | -| `DEVHW_LM75_2` | 34 | | -| `DEVHW_LM75_3` | 35 | | -| `DEVHW_LM75_4` | 36 | | -| `DEVHW_LM75_5` | 37 | | -| `DEVHW_LM75_6` | 38 | | -| `DEVHW_LM75_7` | 39 | | -| `DEVHW_DS2482` | 40 | | -| `DEVHW_MAX7456` | 41 | | -| `DEVHW_SRF10` | 42 | | -| `DEVHW_VL53L0X` | 43 | | -| `DEVHW_VL53L1X` | 44 | | -| `DEVHW_US42` | 45 | | -| `DEVHW_TOF10120_I2C` | 46 | | -| `DEVHW_TERARANGER_EVO_I2C` | 47 | | -| `DEVHW_MS4525` | 48 | | -| `DEVHW_DLVR` | 49 | | -| `DEVHW_M25P16` | 50 | | -| `DEVHW_W25N01G` | 51 | | -| `DEVHW_UG2864` | 52 | | -| `DEVHW_SDCARD` | 53 | | -| `DEVHW_IRLOCK` | 54 | | -| `DEVHW_PCF8574` | 55 | | +| `DEVHW_QMC5883P` | 27 | | +| `DEVHW_MAG3110` | 28 | | +| `DEVHW_LIS3MDL` | 29 | | +| `DEVHW_RM3100` | 30 | | +| `DEVHW_VCM5883` | 31 | | +| `DEVHW_MLX90393` | 32 | | +| `DEVHW_LM75_0` | 33 | | +| `DEVHW_LM75_1` | 34 | | +| `DEVHW_LM75_2` | 35 | | +| `DEVHW_LM75_3` | 36 | | +| `DEVHW_LM75_4` | 37 | | +| `DEVHW_LM75_5` | 38 | | +| `DEVHW_LM75_6` | 39 | | +| `DEVHW_LM75_7` | 40 | | +| `DEVHW_DS2482` | 41 | | +| `DEVHW_MAX7456` | 42 | | +| `DEVHW_SRF10` | 43 | | +| `DEVHW_VL53L0X` | 44 | | +| `DEVHW_VL53L1X` | 45 | | +| `DEVHW_US42` | 46 | | +| `DEVHW_TOF10120_I2C` | 47 | | +| `DEVHW_TERARANGER_EVO_I2C` | 48 | | +| `DEVHW_MS4525` | 49 | | +| `DEVHW_DLVR` | 50 | | +| `DEVHW_M25P16` | 51 | | +| `DEVHW_W25N01G` | 52 | | +| `DEVHW_UG2864` | 53 | | +| `DEVHW_SDCARD` | 54 | | +| `DEVHW_IRLOCK` | 55 | | +| `DEVHW_PCF8574` | 56 | | --- ## `deviceFlags_e` @@ -2623,7 +2628,8 @@ | `INPUT_RC_CH32` | 57 | | | `INPUT_RC_CH33` | 58 | | | `INPUT_RC_CH34` | 59 | | -| `INPUT_SOURCE_COUNT` | 60 | | +| `INPUT_MIXER_SWITCH_HELPER` | 60 | | +| `INPUT_SOURCE_COUNT` | 61 | | --- ## `itermRelax_e` @@ -2754,6 +2760,7 @@ | `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS` | (1 << 9) | | | `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS` | (1 << 10) | | | `LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX` | (1 << 11) | USE_GPS_FIX_ESTIMATION | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED` | (1 << 12) | | --- ## `logicFlightModeOperands_e` @@ -2833,6 +2840,9 @@ | `LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS` | 43 | | | `LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK` | 44 | | | `LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM` | 45 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MIN_GROUND_SPEED` | 46 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED` | 47 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION` | 48 | | --- ## `logicOperation_e` @@ -2897,7 +2907,8 @@ | `LOGIC_CONDITION_DISABLE_GPS_FIX` | 53 | | | `LOGIC_CONDITION_RESET_MAG_CALIBRATION` | 54 | | | `LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY` | 55 | | -| `LOGIC_CONDITION_LAST` | 56 | | +| `LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED` | 56 | | +| `LOGIC_CONDITION_LAST` | 57 | | --- ## `logicWaypointOperands_e` @@ -3105,16 +3116,27 @@ | `MAG_AK8963` | 5 | | | `MAG_IST8310` | 6 | | | `MAG_QMC5883` | 7 | | -| `MAG_MPU9250` | 8 | | -| `MAG_IST8308` | 9 | | -| `MAG_LIS3MDL` | 10 | | -| `MAG_MSP` | 11 | | -| `MAG_RM3100` | 12 | | -| `MAG_VCM5883` | 13 | | -| `MAG_MLX90393` | 14 | | -| `MAG_FAKE` | 15 | | +| `MAG_QMC5883P` | 8 | | +| `MAG_MPU9250` | 9 | | +| `MAG_IST8308` | 10 | | +| `MAG_LIS3MDL` | 11 | | +| `MAG_MSP` | 12 | | +| `MAG_RM3100` | 13 | | +| `MAG_VCM5883` | 14 | | +| `MAG_MLX90393` | 15 | | +| `MAG_FAKE` | 16 | | | `MAG_MAX` | MAG_FAKE | | +--- +## `mavlinkAutopilotType_e` + +> Source: ../../../src/main/telemetry/telemetry.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAVLINK_AUTOPILOT_GENERIC` | 0 | | +| `MAVLINK_AUTOPILOT_ARDUPILOT` | 1 | | + --- ## `mavlinkRadio_e` @@ -3777,6 +3799,16 @@ | `OPFLOW_MSP` | 2 | | | `OPFLOW_FAKE` | 3 | | +--- +## `osd_adsb_warning_style_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_ADSB_WARNING_STYLE_COMPACT` | 0 | | +| `OSD_ADSB_WARNING_STYLE_EXTENDED` | 1 | | + --- ## `osd_ahi_style_e` @@ -3856,7 +3888,7 @@ | `OSD_HOME_DIST` | 23 | | | `OSD_HEADING` | 24 | | | `OSD_VARIO` | 25 | | -| `OSD_VARIO_NUM` | 26 | | +| `OSD_VERTICAL_SPEED_INDICATOR` | 26 | | | `OSD_AIR_SPEED` | 27 | | | `OSD_ONTIME_FLYTIME` | 28 | | | `OSD_RTC_TIME` | 29 | | @@ -3997,7 +4029,9 @@ | `OSD_H_DIST_TO_FENCE` | 164 | | | `OSD_V_DIST_TO_FENCE` | 165 | | | `OSD_NAV_FW_ALT_CONTROL_RESPONSE` | 166 | | -| `OSD_ITEM_COUNT` | 167 | | +| `OSD_NAV_MIN_GROUND_SPEED` | 167 | | +| `OSD_THROTTLE_GAUGE` | 168 | | +| `OSD_ITEM_COUNT` | 169 | | --- ## `osd_sidebar_arrow_e` @@ -4023,6 +4057,18 @@ | `OSD_SIDEBAR_SCROLL_HOME_DISTANCE` | 3 | | | `OSD_SIDEBAR_SCROLL_MAX` | OSD_SIDEBAR_SCROLL_HOME_DISTANCE | | +--- +## `osd_SpeedTypes_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_SPEED_TYPE_GROUND` | 0 | | +| `OSD_SPEED_TYPE_AIR` | 1 | | +| `OSD_SPEED_TYPE_3D` | 2 | | +| `OSD_SPEED_TYPE_MIN_GROUND` | 3 | | + --- ## `osd_stats_energy_unit_e` @@ -4694,7 +4740,7 @@ --- ## `sdcardReceiveBlockStatus_e` -> Source: ../../../src/main/drivers/sdcard/sdcard_sdio.c +> Source: ../../../src/main/drivers/sdcard/sdcard_spi.c | Enumerator | Value | Condition | |---|---:|---| @@ -4705,7 +4751,7 @@ --- ## `sdcardReceiveBlockStatus_e` -> Source: ../../../src/main/drivers/sdcard/sdcard_spi.c +> Source: ../../../src/main/drivers/sdcard/sdcard_sdio.c | Enumerator | Value | Condition | |---|---:|---| @@ -5539,6 +5585,8 @@ | `VOLTAGE_SENSOR_ADC` | 1 | | | `VOLTAGE_SENSOR_ESC` | 2 | | | `VOLTAGE_SENSOR_FAKE` | 3 | | +| `VOLTAGE_SENSOR_SMARTPORT` | 4 | | +| `VOLTAGE_SENSOR_MAX` | VOLTAGE_SENSOR_SMARTPORT | | --- ## `vs600Band_e` diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 46719fc4a41..6e254e69c8a 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -3882,13 +3882,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": { @@ -10850,6 +10850,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 ee237e03333..59a6e39f7b2 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) @@ -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: 1 - 17026ea2745c17ce7ca73eafc26eb91e** +**JSON file rev: 2 + - 17026ea2745c17ce7ca73eafc26eb91e** **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** @@ -402,6 +403,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)` @@ -1887,9 +1889,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. @@ -4482,6 +4484,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/docs/development/msp/rev b/docs/development/msp/rev index 56a6051ca2b..0cfbf08886f 100644 --- a/docs/development/msp/rev +++ b/docs/development/msp/rev @@ -1 +1 @@ -1 \ No newline at end of file +2 From 54f792aa68af91658b7c03dd1d8fd5519952bf8f Mon Sep 17 00:00:00 2001 From: yakorch Date: Sun, 9 Nov 2025 16:13:26 +0200 Subject: [PATCH 4/4] revision file formatting update. --- docs/development/msp/msp_ref.md | 3 +-- docs/development/msp/rev | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/docs/development/msp/msp_ref.md b/docs/development/msp/msp_ref.md index 59a6e39f7b2..d3dad135058 100644 --- a/docs/development/msp/msp_ref.md +++ b/docs/development/msp/msp_ref.md @@ -10,8 +10,7 @@ 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 - - 17026ea2745c17ce7ca73eafc26eb91e** +**JSON file rev: 2 - 17026ea2745c17ce7ca73eafc26eb91e** **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** diff --git a/docs/development/msp/rev b/docs/development/msp/rev index 0cfbf08886f..d8263ee9860 100644 --- a/docs/development/msp/rev +++ b/docs/development/msp/rev @@ -1 +1 @@ -2 +2 \ No newline at end of file