Browse Source

Copter: spool fixes for acro stabilize and throw

master
Leonard Hall 6 years ago committed by Randy Mackay
parent
commit
df3e73f161
  1. 4
      ArduCopter/mode_acro.cpp
  2. 5
      ArduCopter/mode_throw.cpp

4
ArduCopter/mode_acro.cpp

@ -26,11 +26,11 @@ void Copter::ModeAcro::run() @@ -26,11 +26,11 @@ void Copter::ModeAcro::run()
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
// Landed
attitude_control->set_yaw_target_to_current_heading();
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
// clear landing flag above zero throttle

5
ArduCopter/mode_throw.cpp

@ -34,7 +34,6 @@ void Copter::ModeThrow::run() @@ -34,7 +34,6 @@ void Copter::ModeThrow::run()
Throw_PosHold - the copter is kept at a constant position and height
*/
// Don't enter THROW mode if interlock will prevent motors running
if (!motors->armed()) {
// state machine entry is always from a disarmed state
stage = Throw_Disarmed;
@ -116,6 +115,8 @@ void Copter::ModeThrow::run() @@ -116,6 +115,8 @@ void Copter::ModeThrow::run()
}
// demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
break;
@ -129,6 +130,8 @@ void Copter::ModeThrow::run() @@ -129,6 +130,8 @@ void Copter::ModeThrow::run()
}
// Hold throttle at zero during the throw and continually reset the attitude controller
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
// Play the waiting for throw tone sequence to alert the user

Loading…
Cancel
Save