RTL after Quadchute: drone flying away instead of returning home

[ThomasRigi]

Describe the bug After a Quadchute soon after transition the drone flew away from home in RTL mode for a good 100m and about 20s before our pilot took over manually. The problem is also SITL-reproducible.

@bresch @MaEtUgR I need some help from MC experts, and maybe @dagar's overall knowledge. And I reckon it's of interest to @sfuhrer too. I suspect that the fault lies with the MC trajectory initialisation when done at high speeds.

General Overview To give some context for non-VTOL people, if in Mission mode the drone quadchutes (immediate switch to MC mode) because of a problem in FW mode, an RTL is automatically triggered. This part works fine, from the logged info messages you can see that the RTL logic works as expected.

But the drone doesn't fully brake and return as expected. Instead, it just kind of drifts on its path until at some point it eventually remembers to fly back. Took more than 20s on the real flight, and 25s on the third log attached below. During all this time the drone basically doesn't control its position while in an automatic mode, which can easily lead to crashes or geofence breaches.

Also, the drone flies back to the place where the RTL was triggered before it starts climbing and effectively execute the RTL. Consider the case where the drone was flying towards home, stalled for some reason, was almost at home after the quadchute stabilized, then flies away again to where it quadchuted before finally coming home. Doesn't really make sense, it should go to RTL altitude directly and fly towards the destination without going back to where it was initiated.

Log Files and Screenshots Real quadplane drone: https://logs.px4.io/plot_app?log=101ed7b8-2a88-4bd0-8faf-8a197772bd72 (v1.12.1 with minor unrelated modifs) SITL, master, no airmode: https://logs.px4.io/plot_app?log=f726849a-bf6e-4871-acd6-318bc0b80236 Some oscillations due to lack of control authority on low thrust, but general problem remains the same. SITL, master, airmode: https://logs.px4.io/plot_app?log=35c28707-3dc2-4afb-8e51-2c6c94fa046c

Details of what I suspect goes wrong (log files at the end) I think the main problem is that the MC trajectory generation algorithm gets initialized with values that are way out of its normal constraints. And things get worse if you have a significant downspeed upon initialization, but actually gain a lot of altitude just after the quadchute.

Speeds at initialisation : (screenshots from real flight log)

• vz = 3.4m/s (down)
• vx = 28m/s (way out of MPC cruise speed)

Quadchute effect 2s later:

• vz = 16m/s (up!)
• vz setpoint asking for max descend speed
• vx = 12m/s (already well slowed down)

About 10s, speeds have converged, but still drifting away.

• vz = vz setpoint = 2.2m/s (down) even though at RTL return altitude!
• vx and vx setpoint are within constraints, but still moving away from destination without trying to slow down and come back!
• local position setpoint z is way too low for what is asked for by the RTL. It should be at 40m, not 20m.

I'm not familiar with all the details on how the MC trajectory is generated, but it's that the algorithm fails in this case. The drone knows that its final altitude goal is at 40m, yet it asks to go down to 20m even when it is at 40m... Also, it knows that it has to turn around but I guess because of the faulty altitude mismatch between local_position_setpoint.z and local_position.z it does not consider that it's going in the wrong direction? Why is horizontal position not controlled when altitude is off?

To Reproduce Steps to reproduce the behavior:

1. Get your drone to quadchute in mission mode
2. Observe undesired behaviour

Easiest way to reproduce in SITL is to set the transition airspeed to an unattainable value (e.g. VT_ARSP_TRANS = 25) and have the transition time out.

Expected behavior The drone should brake and fly back to the designated RTL spot immediately.

First ideas for a solution (assuming my above analysis is mostly correct)

• reinitialise the trajectory generation when the estimated position is off by more than a threshold
• (at least partly) decouple horizontal and vertical control. It doesn't make sense to keep moving in the wrong direction just because you're above your (imaginary) target)
• take more into account where your final position setpoint is and reinitialise the trajectory generation accordingly. Don't go down from 55m to 20m when your final target altitude is 40m.

[MaEtUgR]

@ThomasRigi Sorry for not reacting instantly. I think you already narrowed it down quite well with the trajectory initialisation. The trajectory setpoint initializes with the much too high horizontal velocity that is way above the limit. Also the vehicle seems to gain a lot of altitude just after the transition happens and hence the controller brings it down again to the same altitude transition was triggered. So we can see the trajectory_setpoint horizontal velocity stays above the limit following the acceleration limits but the controller enforces the hard limit on velocity so it's not smooth even though that's the goal of the acceleration limit.

We had a similar case with RTL getting triggered in free fall on a normal multicopter and you seem to see the same issue in the horizontal direction. The idea for the solution is to initialize with the actual velocity estimate, only allow the setpoint to get smaller until it's below the limit, not strictly enforce the limit in that time and have a much higher acceleration limit for this "emergency slow down". Alternatively we could jump directly to the limit and leave the error above to the controller, not sure if that's acceptable.

[bresch]

It's really similar to the improvement I made recently for the Z axis: https://github.com/PX4/PX4-Autopilot/pull/18277

We could add a horizontal speed trigger for the "emergency braking mode" (e.g.: more than 2x max horizontal speed) and increase maximum horizontal accel/jerk when in that mode. Then we also need to change the limits in the velocity controller to avoid cutting the setpoint.

[ThomasRigi]

Thanks for your replies!

@MaEtUgR just one small correction: the vertical altitude setpoint actually goes below the altitude where the quadchute was triggered (smooth trajectory...). Also it's very interesting to see that the trajectory_setpoint/vz actually already gets inside the constraints relatively fast as is asking for climbing from ca t=1087s on, but the vehicle_local_position_setpoint/vz remains at max down speed. Something's fishy...

@bresch Great! Yes I think the equivalent should also be done for horizontal velocities. And I would be more aggressive on the emergency limits. If you're in shit you want the drone to use all the power it has to get out of it. We achieved a z-acceleration of 7.5g during the quadchute and an x-acceleration of 3g. I know, quadchutes are extreme cases that normal MCs probably shouldn't achieve, but I still feel that 1g is a bit limited for an emergency mode. What do you think of exposing the emergency limits as parameters?

[ThomasRigi]

I did some more simulations and I don't think anymore that the approach in #18277 is sufficient. Just after the transition the drone gains a lot of altitude for sure, so the "emergency braking mode" is deactivated straight away. So we'd also have to relax the max accel and jerk of the trajectory generation when the drone is climbing too fast. And it doesn't properly solve the mismatch between current position and trajectory setpoint that you inevitably have after a quadchute. So you'd probably have to also relax the constraints when the mismatch is too big...

So I experimented with how the trajectory is initialised. Please have a look at my branch https://github.com/ThomasRigi/Firmware/tree/mpc-constrained-auto-smooth-trajectory-init. I can make a PR out of it if you agree with my solution.

Log with a much shorter phase after quadchute Note that I had to increase MPC_THR_MIN to have enough control authority, otherwise the drone wouldn't manage to pitch up as it was trying to reduce altitude.

Basically I limit the initial velocity with which the trajectory generation is initialized upon activation of the flight task. I think this is in general a sensible thing to do, in any case for as long as the controller hard-limits the final velocity setpoint anyway. You know that the trajectory can't evolve with more than MPC_*_VEL_MAX* m/s, so you shouldn't initialise the trajectory with something faster.

[ThomasRigi]

I encountered an issue with my approach that I really don't understand. It works fine if the quadchute is a transition timeout, i.e. the drone was never in FW mode, but it doesn't work as expected when I trigger the quadchute via minimum altitude from FW mode: https://logs.px4.io/plot_app?log=7abb4c8b-308d-4288-a2f6-6f32994c928c The trajectory_setpoint/vy is not correctly constrained anymore...

Is there anything that is called differently upon initialisation? I tried to debug with VSC. For me it should still work but somehow it doesn't. My constrained velocities still seem to be set correctly but they don't have the desired effect anymore.

[ThomasRigi]

I found a force-reset of the velocity in the trajectory generation that I had previously missed. I added the constrained velocity also there (commit 60dda62) and now it works mostly satisfactorily.

https://logs.px4.io/plot_app?log=9fbca181-a2c8-4aef-b5b6-4598b18e3e7e

It's not smooth at all (quadchutes never are smooth almost by definition...) but at least it's no longer drifting way too far, so I'm quite happy with the result. One thing is still not properly handled though: The RTL return altitude was higher than current altitude, and current altitude was higher than local_position_setpoint/z. So the drone still moved down before moving up again.

And I think I can still do some clean-up because as of my current understanding half of my constrained velocities were useless as it just got reset elsewhere anyway in the normal update() process.

[bresch]

It's not smooth at all (quadchutes never are smooth almost by definition...) but at least it's no longer drifting way too far

I understand that you mostly care of not having the drone flying too far, but the reason why we created the emergency braking mode for altitude is exactly to do this while still having smooth setpoints. Simply clipping the setpoints like you did will result in huge control efforts and some larger airframes will suffer from this.

I don't think anymore that the approach in #18277 is sufficient. Just after the transition the drone gains a lot of altitude for sure, so the "emergency braking mode" is deactivated straight away

Of course, the "horizontal emergency braking mode" needs to have its own set of condition and not just use the ones for the "vertical emergency braking mode"

[ThomasRigi]

Thanks for your reply @bresch. I think I had a flaw in my head's logic... Anyway I now experimented with the horizontal emergency mode (branch here) and I have to say it works decently well. :) As exit condition of the horizontal emergency braking mode I have simply put that the xy velocity and acceleration have to be below the normal constraints.

Here's a log with 1g horizontal acceleration: Log 1 I also experimented with 2g as I was trying to find out how fast I could have it brake, but there's not much of a difference: Log 2 and Log 3

There's one thing I don't understand: What is going on with the Z trajectory generation? In the first and second log it absolutely wants to stay at around 20m until the drone has descended again, but on the third one it actually already started to go up. If it always behaved like in the third log I'd be super happy but I'm unable to reproduce it. I don't know what's different between logs 2 and 3... For me they are the same, but don't behave the same. Initially I thought it might be linked to the _max_allowed_horizontal_error of the PositionSmoothing (see Log 4 where _param_mpc_xy_err_max is small), but this can't really be the reason as in Log 2 I put it to a high value again.

Log 1, Z trajectory not good (somehow stuck):

Log2, Z trajectory working correctly:

[ThomasRigi]

I was thinking some more about the conditions when the emergency braking should be activated and when deactivated. Wouldn't it make sense to activate it earlier than (2.0 max velocity), maybe around 1.2 or 1.3 max velocity? And then deactivate the emergency mode once everything is back inside the constraints.

My main two points of reasoning are: 1) if you fly faster than the constraints something is not good and you should get back into nominal conditions as fast as possible in any case. (I see that it is debatable if you should allow to violate your acceleration constraints to get back inside the velocity constraints faster - I'm probably biased by our drone that we want to fly really smooth in normal conditions but we know it's strong enough to be able to deliver hard responses with large jerk and acceleration if needed) 2) with the current solution if we don't activate the emergency braking mode the position controller will anyway hard-cut the setpoints and we end up with a non-smooth behaviour. -> the real jerk is probably larger than what my proposition above would output.

I would argue that this reasoning also applies to the vertical component. I'd be curious to hear your thoughts on this @bresch and @MaEtUgR

[ryanjAA]

Still relevant?

From reading all this I first thought the aircraft "flew away" but really it just sort of keeps coasting or going in the wrong direction due to vastly exceeding the MC trajectory init levels but it does eventually turn around and get home, it just essentially flies down (or up), then back to where the RTL happened and then home. Is that a correct understanding?

[ThomasRigi]

Should have gotten closed with https://github.com/PX4/PX4-Autopilot/pull/18614.