search

PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.48k stars 13.5k forks source link

MC pos control failsafe velocity setpoint discontinuity #11717

Open bresch opened 5 years ago

bresch commented 5 years ago

Describe the bug In mission, if local position is lost, the FlightTask stops updating the setpoints. This causes the position controller to go into failsafe and forces the Z velocity setpoint to the land speed: https://github.com/PX4/Firmware/blob/3d271245a1052ea612b88a4ddf871286356cba94/src/modules/mc_pos_control/mc_pos_control_main.cpp#L1112-L1140

The problem is that when the local position is regained, the setpoint is again generated by the FlightTask and doesn't know the previous setpoint -> velocity setpoint discontinuity.

To Reproduce Steps to reproduce the behavior in SITL:

  1. Takeoff
  2. Before the drone reaches the desired altitude, stop the GPS (gpssim stop)
  3. Wait until the position controller goes into failsafe: 
    [mc_pos_control] Position-Control Setpoint-Update failed
WARN  [mc_pos_control] Failsafe: Descend with land-speed.
WARN  [commander] Failsafe enabled: no global position
  4. Restart the GPS (gpssim start)

Expected behavior A continuous velocity setpoint

Log Files and Screenshots In the following plot, local_position is lost at t = 207s and is regained at t = 222s 2019-03-25_09-40-23_01_plot

On master 3d271245a1052ea612b88a4ddf871286356cba94

Possible solutions The quick 'n dirty fix: Reset the FlightTask when in failsafe

The nicer one: Since the Z velocity estimate is valid, we can still use the velocity-based jerk-limited trajectory algorithm to create the Z setpoints. We could create an AutoFailsafe flighttask that handle local_position loss.

FYI @RomanBapst @MaEtUgR

MaEtUgR commented 5 years ago

If I understand it correctly your problem description is about switching out of that no valid position failsafe and continuing with the flight task which assumes the state of before the failsafe happened. Right?

In general, completely feed-forward based states can be dangerous because if the controller fails to track them they can reach arbitrarily far from the real state.

In this case, I think the update of the FlightTaskAuto should fail because a required input the valid position is not available. After it failed and you reactivate the AutoTask with smoothing again it should smoothly take over from the current state.

About your "nicer" suggestion. There is already a failsafe task: https://github.com/PX4/Firmware/tree/0eb4942f66cd1032dc5989a25e51f45bb9813406/src/lib/FlightTasks/tasks/Failsafe The remaining problem with it is how to activate it because currently the commander sets the desired task on every loop iteration and you don't want to switch back and forth so either you have the failsafe task in parallel in memory and check if the desired task is able to run or even better you have a static way to check if a task is able to run without it having to be instantiated. The failsafe mechanism you ran into is to be considered last resort because all the other measures failed and you still set a position setpoint even though it's not possible to control the position.

bresch commented 5 years ago

If I understand it correctly your problem description is about switching out of that no valid position failsafe and continuing with the flight task which assumes the state of before the failsafe happened. Right?

Exactly

After it failed and you reactivate the AutoTask with smoothing again it should smoothly take over from the current state.

That's not the case right now, no one reactivates the task, right?

commander sets the desired task on every loop iteration

Where is it doing that?

stale[bot] commented 5 years ago

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

bresch commented 5 years ago

I guess this is still not solved. @MaEtUgR What could we do to go towards a fix for that?

stale[bot] commented 5 years ago

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

bresch commented 5 years ago

I think this is still not solved

MaEtUgR commented 5 years ago

After https://github.com/PX4/Firmware/pull/13130 it doesn't directly descend anymore but the original issue is still present. We need to discuss a better way to handle the failsafe using the flight task interface and motion primitives to step up from just setting safe setpoints and get a seamless experience.

stale[bot] commented 4 years ago

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.