Servers by jDrones

Throttle drop when switching to Loiter from RTL


(rmackay9) #18

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.


(Sergey) #19

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


(rmackay9) #20

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.


(Sergey) #21

Thanks Randy!

That’s sound very reasonable.
Checked my WP_SPEED_DN and LAND_SPEED, they are = 200
But PILOT_SPEED_DN = 500

I will increase WP_SPEED_DN and test again.

Also as I now understand it is a bad configuration to have an equal WP_SPEED_DN and LAND_SPEED.


(rmackay9) #22

Sergey,

Great. It’s generally OK to have the WPNAV_SPEED_DN and LAND_SPEED be the same… at least it won’t cause an error anywhere I think. The LAND_SPEED is there to allow configuring the vehicle landings to be much slower… so normally LAND_SPEED would be a lower value (i.e like 50cm/s) than the WPNAV_SPEED_DN. Anyway, you probably understand all this already.


(Sergey) #23

Hi Randy,
I’m really appreciate if you could see my recent log.
I’ve set WP_SPEED_DN to 350 and keep LAND_SPEED 200.

First bad thing that opposite to RTL documentation my land speed was 350 now instead of 200.

And the second one is that I still have this throttle zeroing when switch from RTL Land to Loiter. It was even worse this time because land speed was higher.


(Sergey) #24

Forgot to clarify the RTL to loiter case is on the 14 minute
@rmackay9


(Sergey) #25

I’m looking at recent log and got confused more and more. There is still the Alt-DAlt divergence.
I’m think I don’t understand what a DAlt means at LAND mode at all. DCrt indeed makes sense but not DAlt.
Does the land controller actually controls the DAlt value not the DCrt?


(rmackay9) #26

@sergbokh

I think in LAND mode the desired altitude is still updated. Copter integrates the desired climb rate into the desired altitude.


(Sergey) #27

Hi Randy,
thanks, I’m stuck with that. Have no idea what should I try further to get to the bottom of that issue.

BTW, any idea why LAND uses WP_SPEED_DN instead of LAND_SPEED?


(rmackay9) #28

@sergbokh,

In LAND mode, I think the vehicle will descend at either the LAND_SPEED or the LAND_SPEED_HIGH parameter value.

From looking at the 40.BIN log posted above, it doesn’t look like the vehicle is ever in LAND mode so perhaps you mean the last stage of RTL mode? In RTL mode it should descend quickly until it reaches about 10m and then it slows down to the LAND_SPEED.


(Sergey) #29

That’s correct. I didn’t know the final stage consists of descending and landing with its own rates.

So my test was not correct. I will retest in LAND mode and see if the issue with throttle occurs.


(Sergey) #30

Did another 3 tests this weekend.
WP_SPEED_DN is 350 and LAND_SPEED is 200.

  • Descending in LAND then switch to Loiter: everything is OK.
  • Descending as RTL step then switch to Loiter (TEST1 on screenshot): drop in climb rate. Still nothing changed here.
  • Hovering as RTL step after descend then switch to Loiter (TEST2): drop in climb rate again

The drop is in TEST2 is something new I didn’t aware of. May be it could be a hint for someone who knows how attitude control works in arducopter.


(Sergey) #31

From logs; DCrt is negative right after Loiter engaged.
2018-11-11 14:37:16.514 | DCrt = -1
2018-11-11 14:37:16.583 | MODE Loiter
2018-11-11 14:37:16.617 | Dcrt = -289


(Sergey) #32

Just an assumption, not digging into the code very deep.
I’m using a RTL_ALT_FINAL = 1000 so as per source codes there shall be no LAND at the end of RTL at all (by design).
OK, so after the copter reaches the 1000cm it just stuck in Copter::ModeRTL::descent_run() forever.

Note this method uses following lines:
// call z-axis position controller
pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);
pos_control->update_z_controller();

Then, if you switch to a Loiter, there is the Init():
// initialise position and desired velocity
if (!pos_control->is_active_z()) {
pos_control->set_alt_target_to_current_alt();
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
}
I think is_active_z() == true so this actions are not executed.


(Sergey) #33

@rmackay9
good news, this is reproducible with SITL autotest


(rmackay9) #34

@sergbokh,

Oh really? I haven’t been able to reproduce it in SITL in the past but I guess it’s the RTL_ALT parameter that is key? So once that is set it is reproduce-able? I’ll give it a try…


(Sergey) #35

I’ve added test details to the ticket https://github.com/ArduPilot/ardupilot/issues/7435
Yes, seems RTL_ALT_FINAL is the key to repro.


(Sergey) #36

@rmackay9
I see this your commit added the condition.

Previously we always reset the altitude target to the current altitude but this causes a jump if the vehicle is already in an alt-hold flight mode but has an altitude error

Could you please clarify “an altitude error” means the altitude estimation error or means that current Alt != Desired Alt?

And if it’s all about the altitude then doesn’t the code look like:

if (!pos_control.is_active_z()) {
    pos_control.set_alt_target_to_current_alt();
    }

pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());

(rmackay9) #37

@sergbokh,

yes, when referring to “altitude error” I meant that there was a difference between the desired and actual altitude.

It’s super clear what the issue is now I think. The initialisation of the altitude target in the descent_start() method is not quite right. This line

pos_control->set_target_to_stopping_point_z();

should be changed to this:

if (!pos_control->is_active_z()) {
pos_control->set_alt_target_to_current_alt();
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
}

This is great. Thanks so much for this Sergey. I’ll do a PR later today probably (or you can if you’d like) and we can very likely get this out into Copter 3.6.3.