Browse Source

Copter: use zero_throttle_and_relax_ac in poshold, drift and autotune

mission-4.1.18
Peter Barker 7 years ago committed by Randy Mackay
parent
commit
ab7a9c9073
  1. 3
      ArduCopter/mode_autotune.cpp
  2. 3
      ArduCopter/mode_drift.cpp
  3. 3
      ArduCopter/mode_poshold.cpp

3
ArduCopter/mode_autotune.cpp

@ -328,8 +328,7 @@ void Copter::ModeAutoTune::run() @@ -328,8 +328,7 @@ void Copter::ModeAutoTune::run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
// this should not actually be possible because of the init() checks
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
zero_throttle_and_relax_ac();
pos_control->relax_alt_hold_controllers(0.0f);
return;
}

3
ArduCopter/mode_drift.cpp

@ -48,8 +48,7 @@ void Copter::ModeDrift::run() @@ -48,8 +48,7 @@ void Copter::ModeDrift::run()
// if landed and throttle at zero, set throttle to zero and exit immediately
if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
zero_throttle_and_relax_ac();
return;
}

3
ArduCopter/mode_poshold.cpp

@ -140,9 +140,8 @@ void Copter::ModePosHold::run() @@ -140,9 +140,8 @@ void Copter::ModePosHold::run()
// 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()) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
wp_nav->init_loiter_target();
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
zero_throttle_and_relax_ac();
pos_control->relax_alt_hold_controllers(0.0f);
return;
}

Loading…
Cancel
Save