Copter: Initialise desired acceleration before loiter init

This commit is contained in:
Leonard Hall 2018-09-18 17:36:24 +09:30 committed by Andrew Tridgell
parent 724d498dba
commit 870db7d714
4 changed files with 6 additions and 0 deletions

View File

@ -57,6 +57,7 @@ void Copter::ModeLand::gps_run()
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// disarm when the landing detector says we've landed

View File

@ -100,6 +100,7 @@ bool Copter::ModePosHold::init(bool ignore_checks)
poshold.pitch_mode = POSHOLD_LOITER;
// set target to current position
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
}else{
// if not landed start in pilot override to avoid hard twitch
@ -133,6 +134,7 @@ void Copter::ModePosHold::run()
// initialize vertical speeds and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
loiter_nav->clear_pilot_desired_acceleration();
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {

View File

@ -265,6 +265,7 @@ void Copter::ModeRTL::descent_run()
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// set target to current position
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
return;
}
@ -356,6 +357,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// set target to current position
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// disarm when the landing detector says we've landed

View File

@ -79,6 +79,7 @@ void Copter::ModeThrow::run()
stage = Throw_PosHold;
// initialise the loiter target to the curent position and velocity
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum