From 976b3db95b570ce8dcdb127b59c3134789fc63e2 Mon Sep 17 00:00:00 2001 From: yakorch Date: Fri, 19 Sep 2025 20:38:57 +0300 Subject: [PATCH 1/2] 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/2] 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]));