diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 1619282988..b4aacbdc97 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -30,17 +30,20 @@ void ModeAcro::run() attitude_control->set_attitude_target_to_current_attitude(); attitude_control->reset_rate_controller_I_terms(); break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_attitude_target_to_current_attitude(); attitude_control->reset_rate_controller_I_terms(); break; + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: // clear landing flag above zero throttle if (!motors->limit.throttle_lower) { set_land_complete(false); } break; + case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_DOWN: // do nothing diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index b8e4d875a3..548a918c41 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -48,25 +48,21 @@ void ModeAltHold::run() switch (althold_state) { case AltHold_MotorStopped: - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; case AltHold_Landed_Ground_Idle: - - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - + attitude_control->reset_rate_controller_I_terms(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; case AltHold_Takeoff: - // initiate take-off if (!takeoff.running()) { takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 56d7ea2545..d7f542b889 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -94,17 +94,20 @@ void ModeDrift::run() attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); break; + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: // clear landing flag above zero throttle if (!motors->limit.throttle_lower) { set_land_complete(false); } break; + case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_DOWN: // do nothing diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index 445c027a71..af57529384 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -200,6 +200,7 @@ void ModeFlip::run() Log_Write_Event(DATA_FLIP_END); } break; + } case FlipState::Abandon: // restore original flight mode diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 11ffa56cdb..ab92a8242b 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -254,7 +254,6 @@ void ModeFlowHold::run() switch (flowhold_state) { case AltHold_MotorStopped: - copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); copter.attitude_control->reset_rate_controller_I_terms(); copter.attitude_control->set_yaw_target_to_current_heading(); @@ -283,12 +282,11 @@ void ModeFlowHold::run() break; case AltHold_Landed_Ground_Idle: - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - + attitude_control->reset_rate_controller_I_terms(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 30a6e2acb9..8597624c14 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -117,7 +117,6 @@ void ModeLoiter::run() switch (loiter_state) { case AltHold_MotorStopped: - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero @@ -127,7 +126,6 @@ void ModeLoiter::run() break; case AltHold_Takeoff: - // initiate take-off if (!takeoff.running()) { takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); @@ -152,13 +150,11 @@ void ModeLoiter::run() break; case AltHold_Landed_Ground_Idle: - - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - + attitude_control->reset_rate_controller_I_terms(); loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero @@ -166,7 +162,6 @@ void ModeLoiter::run() break; case AltHold_Flying: - // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index e7a5b0f480..cc4e435e91 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -107,7 +107,6 @@ void ModePosHold::run() switch (poshold_state) { case AltHold_MotorStopped: - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero @@ -121,7 +120,6 @@ void ModePosHold::run() break; case AltHold_Takeoff: - // initiate take-off if (!takeoff.running()) { takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); @@ -148,16 +146,14 @@ void ModePosHold::run() break; case AltHold_Landed_Ground_Idle: - loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); loiter_nav->update(); - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - + attitude_control->reset_rate_controller_I_terms(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero // set poshold state to pilot override @@ -166,7 +162,6 @@ void ModePosHold::run() break; case AltHold_Flying: - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); #if AC_AVOID_ENABLED == ENABLED diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 588f8c50ad..0b0fa1f51d 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -79,14 +79,12 @@ void ModeSport::run() switch (sport_state) { case AltHold_MotorStopped: - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; case AltHold_Takeoff: - // initiate take-off if (!takeoff.running()) { takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); @@ -104,12 +102,11 @@ void ModeSport::run() break; case AltHold_Landed_Ground_Idle: - attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - + attitude_control->reset_rate_controller_I_terms(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 49714f3d33..93edffc2c3 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -34,17 +34,20 @@ void ModeStabilize::run() attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); break; + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: // clear landing flag above zero throttle if (!motors->limit.throttle_lower) { set_land_complete(false); } break; + case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_DOWN: // do nothing diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 47daff5136..0242d710da 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -128,6 +128,7 @@ void ModeSystemId::run() attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed // Tradheli initializes targets when going from disarmed to armed state. @@ -137,12 +138,14 @@ void ModeSystemId::run() attitude_control->reset_rate_controller_I_terms(); } break; + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: // clear landing flag above zero throttle if (!motors->limit.throttle_lower) { set_land_complete(false); } break; + case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_DOWN: // do nothing