Browse Source

Plane: replace set_throttle_out_unstabilized

master
Leonard Hall 6 years ago committed by Randy Mackay
parent
commit
2daa5ffb9e
  1. 16
      ArduPlane/quadplane.cpp

16
ArduPlane/quadplane.cpp

@ -773,11 +773,10 @@ void QuadPlane::hold_stabilize(float throttle_in)
if (throttle_in <= 0) { if (throttle_in <= 0) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
if (is_tailsitter()) { attitude_control->set_throttle_out(0, true, 0);
if (!is_tailsitter()) {
// always stabilize with tailsitters so we can do belly takeoffs // always stabilize with tailsitters so we can do belly takeoffs
attitude_control->set_throttle_out(0, true, 0); attitude_control->relax_attitude_controllers();
} else {
attitude_control->set_throttle_out_unstabilized(0, true, 0);
} }
} else { } else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -912,7 +911,8 @@ void QuadPlane::control_qacro(void)
{ {
if (throttle_wait) { if (throttle_wait) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
attitude_control->set_throttle_out_unstabilized(0, true, 0); attitude_control->set_throttle_out(0, true, 0);
attitude_control->relax_attitude_controllers();
} else { } else {
check_attitude_relax(); check_attitude_relax();
@ -976,7 +976,8 @@ void QuadPlane::control_hover(void)
{ {
if (throttle_wait) { if (throttle_wait) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
attitude_control->set_throttle_out_unstabilized(0, true, 0); attitude_control->set_throttle_out(0, true, 0);
attitude_control->relax_attitude_controllers();
pos_control->relax_alt_hold_controllers(0); pos_control->relax_alt_hold_controllers(0);
} else { } else {
hold_hover(get_pilot_desired_climb_rate_cms()); hold_hover(get_pilot_desired_climb_rate_cms());
@ -1104,7 +1105,8 @@ void QuadPlane::control_loiter()
{ {
if (throttle_wait) { if (throttle_wait) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
attitude_control->set_throttle_out_unstabilized(0, true, 0); attitude_control->set_throttle_out(0, true, 0);
attitude_control->relax_attitude_controllers();
pos_control->relax_alt_hold_controllers(0); pos_control->relax_alt_hold_controllers(0);
loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target(); loiter_nav->init_target();

Loading…
Cancel
Save