iNavFlight
Expose full pose estimate via MSP
Current Behavior
3D attitude, estimated altitude, etc., are exposed via the MSP protocol. However, a full current pose (3D attitude and local/global position) estimate, along with 3D velocity, is not exposed similarly. This can be verified in the source code at inav/src/main/fc/fc_msp.c.
Desired Behavior
I believe the community working with autonomous navigation would benefit if such a full pose estimate were exposed via MSP. One may argue that MSP_RAW_GPS message can be used to get a good position estimate. However, it requires a working/available GPS module, which is not always the case.
Suggested Solution
Implementation should be feasible because MSP_ALTITUDE message already uses the getEstimatedActualPosition and getEstimatedActualVelocity functions, which call navGetCurrentActualPositionAndVelocity under the hood. Now, one needs to efficiently retrieve these estimates for every axis.
A proposed MSP2_FULL_POSE message could have the following behavior:
- Direction: Out
- Description: Retrieves a full 3D agent pose, consisting of attitude, position, and velocity estimates. Dead-reckoned position is returned if available.
- Payload: 3D orientation, 3D position, and 3D velocity.
Who does this impact? Who is this for?
I assume this is for advanced users working on autonomous systems and for all the platforms that can communicate via MSP.
Additional context
The benefit of exposing the full pose at once rather than, for example, implementing a message MSP2_POSITION specifically for the position estimate is in avoiding duplicate MSP calls: I suspect usually when one needs a position, an attitude, and velocity are also needed. Also, this way, the position, attitude, and velocity are synchronized in one message.
I don’t remember if I read it here or in the Betaflight discussions/instructions, but I recall there was a decision not to introduce new MSP codes for specialized use cases. That said, I do see the value in having this MSP message.
For my own needs, I added a few specialized codes - it wasn’t difficult. You can take a look at Betalink project for Betaflight; the iNav implementation is essentially the same. It’s just a matter of pulling the code together from other codes and adding to buffer the new fields and then assigning to it an available MSP code number.
If you end up implementing this, I’d be happy if you submit a PR, even if it might be declined 😄 Also if you find the aforementioned instruction for new MSP messages development, please link it here so it has reference for future development.
