Throttle drop when switching to Loiter from RTL

This is a discussion surrounding this issue raised by @sergbokh. There is an earlier discussion for Copter-3.5 here.

This is a graph of what happens with Copter-3.5. We can see the desired climbrate is starting at -6m/s when the vehicle is switched to loiter when it should probably be the vehicle’s current climb rate (at the moment it was switched into Loiter).

I have not been able to reproduce it in SITL but it’s possible I don’t know exactly how to setup the circumstances to make this happen.

@sergbokh,

Leonard and I spent some time today looking into this and we’ve noticed that you may not be using the official version of AP. It looks like this is not quite official ardupilot but is instead using some custom code.

Can you try and reproduce with the official or beta versions? By the way, it’s quite important to tell us this kind of thing when reporting issues.

Randy, thanks for looking into that.
I understand your concern about custom FW but it is not very “custom” in fact.

Here is my recent log where you can see the same issue again (minor difference is that RTL was initiated by mission, not by RC).
Again, this is a slightly customized FW based on 3.6rc-12. And you can easily see in github it only contains a few changes that are far away from copter attitude control.

I will flash official rc12 to get repro this weekends. But I’m pretty sure it will show the same behavior.
Thanks!

1 Like

This looks very much like a problem I had yesterday on the new loiter-to-alt stuff I’m working on.

The test I was running uses the AC_PosControl::run_z_controller. It climbs to 30m then immediately descends to 2m. It uses one of the sqrtf controllers to determine a target climb rate and sets the alt-target based on that.

The problem is that moving between the 30m and 2m targets yields a huge delta here: https://github.com/peterbarker/ardupilot/blob/loiter-to-alt/libraries/AC_AttitudeControl/AC_PosControl.cpp#L556 - _accel_desired.z goes to about 580m/s/s - an unhealthy thing for a Copter to attempt.

I added this patch https://github.com/peterbarker/ardupilot/commit/9fb1886e0a0600cd7607d8faa597e7ab7f24e3cd to attempt to fix the problem.

Before the patch (note discontinuity in - and zeroing of -ThO at the point the vehicle reaches its peak and resets its desired altitude to 2m)):

Post-patch:

What I’m really curious about is that nowhere in the z-controller do we limit _accel_target.z. Adding in a constrain_float seemed to fix the problem for me, but I’ve no doubt it will cause other issues (e.g. DAlt gets out of whack with Alt). Adding this patch https://github.com/peterbarker/ardupilot/commit/4934da79e069d65fcedca5c1d1e3c0db6f0019ec seems to restrict the z-accel to the specified target.

Note what CTUN.CRt does in the following two graphs in the descend-from-30m phase; one honours the 1m/s/s z-accel-limit, the other does not:

Pre-accel-limitting:

Post-accel-limiting:

This might be the wrong way to fix things - the limit might want to be on the desired side of things rather than the output side of things.

tldr; I think it’s possible that pushing the target climbrate up and down significantly can cause the z-controller to act up.

Small follow-up. DCRt and CRt diverging does seem to cause integrator windup (as expected).

@peterbarker thanks, this is very interesting.
As I understood you, by restricting max acceleration we’re trying to fight with the effect, not the cause.

I almost have no knowledge of how attitude thing works in AP, but from the common sense standpoint here is the another thought.

Here is the moment RTL doing a descend and switch to loiter. At the moment of switch the DCRt goes very low for a moment. But in fact we cannot say this is a bug: controller just corrected the divergence in DAlt and Alt. And it did it very fast but precisely.


So what I’m trying to say: may be the bug is not in “why Loiter does the corrections so fast” but “why it is so large divergence between DAlt and Alt on the descending step”?

Hope this makes sense :slight_smile:

The large divergence could be explained by large throttle input when moving between the modes (i.e. pilot input). Check RCIN.C3, perhaps.

I should also mention this code in QuadPlane, which is almost certainly related: https://github.com/ardupilot/ardupilot/blob/master/ArduPlane/quadplane.cpp#L2494

The commit message is:

Plane: adjust target altitude slowly in QRTL mode

this fixes a sudden throttle drop when starting the descent in QRTL mode
thanks to Marco for finding this issue

Now that was obviously a quick patch for a scary bug report. But I think it’s possible we need a patch further up as we can clearly see the same symptoms in Copter. It’s definitely attempting to fix things on the demand side of things - but that method seems a little inelegant, perhaps not letting the z-controller plan appropriately.

But in that case we’re talking about the divergence that exists in RLT-descend step. So RC input doesn’t affect.

I believe the another option could be to adjust the DAlt to the current Alt at the moment of switching to Loiter.
That way we will avoid the necessity to do rapid altitude correction. The thick line below:
image

can someone describe how to reproduce this issue?
I did the switch to loiter during the RTL descent phase many times with 3.5 and now with 3.6 without any noticeable incidents.

@mtbsteve may be you could share any of your log just to compare?

No log at hand right now. But if you could describe what you did I can try to reproduce tomorrow

Basically there is nothing specific to do. Just switch to RTL being on 40-50m and wait for copter to start descending. At 10-15 m set Loiter flight mode to abort RTL.
For me it is 100% reproducible.

If you could write an autotest test for it, that would be ideal. That
gives Randy and/or Leonard something to play with.

This test is what I included the pictures from:

1 Like

OK - I tried RTL->Loiter today several times from different altitudes and at different stages of the descent but I could not reproduce anything like you described.
RTL gets aborted and the copter switches into Loiter as it should.
Here is one logfile for reference:
https://1drv.ms/u/s!AnKeW8KMoCcymATEXrBlN2Q6WQrv
Maybe some different parameter settings?

1 Like

No logs, 'cause I had “bad logging” and “missing i/o hartbeat” today, but the RTL descent stopped with some increased engine noise, not decreased as your bug implies. Going to futz around with it more tomorrow.

1 Like

Thank’s @mtbsteve
For me it’s now looks like I have some settings (or some another reason) that causes my Alt follow DAlt kind of lazy in autonomous modes. My altitude lag is 2 meters on RTL descend.
Your log shows no divergence between Alt and DAlt, so you have no that scary altitude correction switching to Loiter.

Yes, this is the key thing - there needs to be a large-ish altitude error for the problem to occur. I can’t reproduce it in SITL with the default config at least. We probably need to make the altitude hold tuning worse to see the problem.

What (setting?) can affect alt error rate in RTL/Auto to be different than in Loiter?

Sergey,

For the most part the altitude controller is the same in Loiter mode vs RTL/Auto. It’s really just the PILOT_* vs WPNAV_* parameters that set the maximum velocity and acceleration differently.

As noticed before, what’s odd is that the Desired vs Actual climb rates match (CTUN.DCRt vs CRt) but the Desired vs Actual altitude does not (i.e. CTUN.DAlt vs Alt). the WPNAV_SPEED_DN is set to 200 so it appears that this is what is limiting the descent.

I’ve reviewed the logs again and I don’t see anything that immedialyte stands out as odd. The hover throttle learning (MOT_HOVER_LEARN) param has been set to “1” (default is “2”) so the hover has been disabled and I suspect that the MOT_THST_HOVER value isn’t being saved at the end of the flight. I suspect the current value (0.4) is a little high for the vehicle and I wonder if this might be part of the cause.