Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions src/main/fc/fc_msp.c
Original file line number Diff line number Diff line change
Expand Up @@ -642,6 +642,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());
Expand Down
10 changes: 6 additions & 4 deletions src/main/msp/msp_protocol_v2_inav.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Loading