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.
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.
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?
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.
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.
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…
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());
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.
Thanks Randy,
Sorry, I cannot say the reason and the fix is very clear for me
PosControl interface is not very intuitive. There are a number of different set_… methods, all with its different side effects. There are also loiter_nav and wp_naw does add confusion, because hierarchy and responsibility of each is not obvious.
Why changing Alt initialization in descend_start() does fix if the alt target then changes continuously in descend_run().