Gracefully handling vision position system failures

Dear all,

I’m currently working on a project that uses a visual SLAM system for position estimates, feeds them to PX4 via the VISION_POSITION_ESTIMATE Mavlink message, and then sends SET_POSITION_TARGET_LOCAL_NED messages in offboard mode to fly to some specific waypoint.
This is mostly working (in simulation), but I wonder what I should do when the Slam system (inevitably) fails at some point (i.e. looses tracking). Since I cannot continue sending VISION_POSITION_ESTIMATE messages in that situation, PX4 will trigger a failsafe because offboard mode requires valid position estimates.
The behavior I would prefer would be to just stabilize roll and pitch to zero and keep the current altitude (using the barometer) so I can start some sort of recovery/initialization procedure.
However, I’m not sure which flight mode to use here. Offboard isn’t possible (since it requires valid position estimates) and switching to Altitude-Control mode is only allowed if there is a remote control. Of course I could just send MANUAL_CONTROL Mavlink messages from the companion computer to fake a remote, with all axes zero but this does not feel like the right way to do this.

Does anyone have suggestions on how I could handle this situation? Am I missing some “secret” flight mode or Mavlink messages that could be helpful here? :slight_smile:

Thanks a lot!