From 46a6f45e4a9d50d7068a0f68011e963e7a29a088 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 9 Apr 2019 22:16:58 +1000 Subject: [PATCH] Copter: adjust for desired spool state and spool state renames --- ArduCopter/afs_copter.cpp | 2 +- ArduCopter/heli.cpp | 2 +- ArduCopter/land_detector.cpp | 2 +- ArduCopter/mode.cpp | 36 ++++++++++++++++-------------- ArduCopter/mode_acro.cpp | 20 ++++++++++++----- ArduCopter/mode_acro_heli.cpp | 18 ++++++++++----- ArduCopter/mode_althold.cpp | 2 +- ArduCopter/mode_auto.cpp | 14 ++++++------ ArduCopter/mode_autotune.cpp | 4 ++-- ArduCopter/mode_brake.cpp | 4 ++-- ArduCopter/mode_circle.cpp | 4 ++-- ArduCopter/mode_drift.cpp | 20 ++++++++++++----- ArduCopter/mode_flip.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 8 +++---- ArduCopter/mode_follow.cpp | 2 +- ArduCopter/mode_guided.cpp | 16 ++++++------- ArduCopter/mode_land.cpp | 8 +++---- ArduCopter/mode_loiter.cpp | 2 +- ArduCopter/mode_poshold.cpp | 2 +- ArduCopter/mode_rtl.cpp | 10 ++++----- ArduCopter/mode_smart_rtl.cpp | 6 ++--- ArduCopter/mode_sport.cpp | 2 +- ArduCopter/mode_stabilize.cpp | 20 ++++++++++++----- ArduCopter/mode_stabilize_heli.cpp | 17 +++++++++----- ArduCopter/mode_throw.cpp | 14 ++++++------ ArduCopter/mode_zigzag.cpp | 4 ++-- ArduCopter/motors.cpp | 2 +- ArduCopter/system.cpp | 4 ++-- ArduCopter/takeoff.cpp | 2 +- 29 files changed, 145 insertions(+), 104 deletions(-) diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index 8653ca18d6..c36ece99f0 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -21,7 +21,7 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void) copter.set_mode(LAND, MODE_REASON_TERMINATE); } else { // stop motors - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); copter.motors->output(); // disarm as well diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 7bc927dd95..6c5eb7ac59 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -23,7 +23,7 @@ void Copter::heli_init() // should be called at 50hz void Copter::check_dynamic_flight(void) { - if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED || + if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED || control_mode == LAND || (control_mode==RTL && mode_rtl.state() == RTL_Land) || (control_mode == AUTO && mode_auto.mode() == Auto_Land)) { heli_dynamic_flight_counter = 0; heli_flags.dynamic_flight = false; diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index f6272912b7..52ca11dec1 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -47,7 +47,7 @@ void Copter::update_land_detector() } else if (ap.land_complete) { #if FRAME_CONFIG == HELI_FRAME // if rotor speed and collective pitch are high then clear landing flag - if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { #else // if throttle output is high then clear landing flag if (motors->get_throttle() > get_non_takeoff_throttle()) { diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 049948d9ac..8f3c07216f 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -193,7 +193,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) #if FRAME_CONFIG == HELI_FRAME // do not allow helis to enter a non-manual throttle mode if the // rotor runup is not complete - if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_mode() == AP_Motors::SPOOL_UP || motors->get_spool_mode() == AP_Motors::SPOOL_DOWN)) { + if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) { gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed"); AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; @@ -382,7 +382,7 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const return false; } - if (copter.motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED) { + if (copter.motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) { // hold aircraft on the ground until rotor speed runup has finished return false; } @@ -393,9 +393,9 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up) { if (spool_up) { - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); @@ -411,17 +411,19 @@ void Copter::Mode::zero_throttle_and_hold_attitude() void Copter::Mode::make_safe_spool_down() { // command aircraft to initiate the shutdown process - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); - switch (motors->get_spool_mode()) { + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + switch (motors->get_spool_state()) { - case AP_Motors::SHUT_DOWN: - case AP_Motors::GROUND_IDLE: + case AP_Motors::SpoolState::SHUT_DOWN: + case AP_Motors::SpoolState::GROUND_IDLE: // relax controllers during idle states attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); break; - default: + case AP_Motors::SpoolState::SPOOLING_UP: + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: + case AP_Motors::SpoolState::SPOOLING_DOWN: // while transitioning though active states continue to operate normally break; } @@ -627,15 +629,15 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli // Alt Hold State Machine Determination if (!motors->armed()) { // the aircraft should moved to a shut down state - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); // transition through states as aircraft spools down - switch (motors->get_spool_mode()) { + switch (motors->get_spool_state()) { - case AP_Motors::SHUT_DOWN: + case AP_Motors::SpoolState::SHUT_DOWN: return AltHold_MotorStopped; - case AP_Motors::DESIRED_GROUND_IDLE: + case AP_Motors::SpoolState::GROUND_IDLE: return AltHold_Landed_Ground_Idle; default: @@ -651,14 +653,14 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli // the aircraft is armed and landed if (target_climb_rate_cms < 0.0f && !ap.using_interlock) { // the aircraft should move to a ground idle state - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { // the aircraft should prepare for imminent take off - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } - if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + if (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { // the aircraft is waiting in ground idle return AltHold_Landed_Ground_Idle; @@ -669,7 +671,7 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli } else { // the aircraft is in a flying state - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); return AltHold_Flying; } } diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 6e5ba9ceab..4241f9528b 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -16,27 +16,35 @@ void Copter::ModeAcro::run() if (!motors->armed()) { // Motors should be Stopped - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } else if (ap.throttle_zero) { // Attempting to Land - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } - if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + switch (motors->get_spool_state()) { + case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped 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) { + break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed 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) { + 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 + break; } // run attitude controller diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 5d16f68997..b941c581b9 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -36,27 +36,35 @@ void Copter::ModeAcro_Heli::run() if (!motors->armed()) { // Motors should be Stopped - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } else { // heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } - if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + switch (motors->get_spool_state()) { + case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped 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) { + break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed if (motors->init_targets_on_arming()) { 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) { + 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 + break; } if (!motors->has_flybar()){ diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 8d08a11e89..34b5b9b9a0 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -88,7 +88,7 @@ void Copter::ModeAltHold::run() break; case AltHold_Flying: - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); #if AC_AVOID_ENABLED == ENABLED // apply avoidance diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index de9427177b..5b63bacc53 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -747,7 +747,7 @@ void Copter::ModeAuto::wp_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); @@ -787,7 +787,7 @@ void Copter::ModeAuto::spline_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller wp_nav->update_spline(); @@ -818,7 +818,7 @@ void Copter::ModeAuto::land_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); land_run_horizontal_control(); land_run_vertical_control(); @@ -879,7 +879,7 @@ void Copter::ModeAuto::loiter_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint and z-axis position controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); @@ -972,7 +972,7 @@ void Copter::ModeAuto::payload_place_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); switch (nav_payload_place.state) { case PayloadPlaceStateType_FlyToLocation: @@ -1509,7 +1509,7 @@ bool Copter::ModeAuto::verify_land() case LandStateType_Descending: // rely on THROTTLE_LAND mode to correctly update landing status - retval = ap.land_complete && (motors->get_spool_mode() == AP_Motors::GROUND_IDLE); + retval = ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE); break; default: @@ -1729,7 +1729,7 @@ bool Copter::ModeAuto::verify_RTL() { return (copter.mode_rtl.state_complete() && (copter.mode_rtl.state() == RTL_FinalDescent || copter.mode_rtl.state() == RTL_Land) && - (motors->get_spool_mode() == AP_Motors::GROUND_IDLE)); + (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE)); } /********************************************************************************/ diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 5b40da47dd..1a0382d317 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -54,9 +54,9 @@ void Copter::AutoTune::run() // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } copter.attitude_control->reset_rate_controller_I_terms(); copter.attitude_control->set_yaw_target_to_current_heading(); diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 309d441511..0f972cda01 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -39,7 +39,7 @@ void Copter::ModeBrake::run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // relax stop target if we might be landed if (ap.land_complete_maybe) { @@ -54,7 +54,7 @@ void Copter::ModeBrake::run() // update altitude target and call position controller // protects heli's from inflight motor interlock disable - if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) { + if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !ap.land_complete) { pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); } else { pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index ba15742cd1..928fa92513 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -54,7 +54,7 @@ void Copter::ModeCircle::run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run circle controller copter.circle_nav->update(); @@ -72,7 +72,7 @@ void Copter::ModeCircle::run() // update altitude target and call position controller // protects heli's from inflight motor interlock disable - if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) { + if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !ap.land_complete) { pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); } else { pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index b8ff409c2f..bf61a171f3 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -80,27 +80,35 @@ void Copter::ModeDrift::run() if (!motors->armed()) { // Motors should be Stopped - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } else if (ap.throttle_zero) { // Attempting to Land - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } - if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + switch (motors->get_spool_state()) { + case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); - } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); - } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + 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 + break; } // call attitude controller diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index 34bb79c149..df1186a9a6 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -103,7 +103,7 @@ void Copter::ModeFlip::run() float throttle_out = get_pilot_desired_throttle(); // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // get corrected angle based on direction and axis of rotation // we flip the sign of flip_angle to minimize the code repetition diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 4a8bd896df..79bde883ea 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -255,7 +255,7 @@ void Copter::ModeFlowHold::run() case AltHold_MotorStopped: - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + 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(); copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero @@ -264,7 +264,7 @@ void Copter::ModeFlowHold::run() case AltHold_Takeoff: // set motors to full range - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // initiate take-off if (!takeoff.running()) { @@ -297,7 +297,7 @@ void Copter::ModeFlowHold::run() break; case AltHold_Flying: - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // adjust climb rate using rangefinder target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt); @@ -353,7 +353,7 @@ void Copter::ModeFlowHold::update_height_estimate(void) #if 1 // assume on ground when disarmed, or if we have only just started spooling the motors up if (!hal.util->get_soft_armed() || - copter.motors->get_desired_spool_state() != AP_Motors::DESIRED_THROTTLE_UNLIMITED || + copter.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED || AP_HAL::millis() - copter.arm_time_ms < 1500) { height_offset = -ins_height; last_ins_height = ins_height; diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index b433ab0289..8ed425eab0 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -33,7 +33,7 @@ void Copter::ModeFollow::run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // re-use guided mode's velocity controller // Note: this is safe from interference from GCSs and companion computer's whose guided mode diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index b70217e60a..c9eaa78705 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -392,14 +392,14 @@ void Copter::Mode::auto_takeoff_run() } // aircraft stays in landed state until rotor speed runup has finished - if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { set_land_complete(false); } else { wp_nav->shift_wp_origin_to_current_pos(); } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); @@ -432,7 +432,7 @@ void Copter::ModeGuided::pos_control_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); @@ -474,7 +474,7 @@ void Copter::ModeGuided::vel_control_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); @@ -527,7 +527,7 @@ void Copter::ModeGuided::posvel_control_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); @@ -614,8 +614,8 @@ void Copter::ModeGuided::angle_control_run() // landed with positive desired climb rate, takeoff if (ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) { zero_throttle_and_relax_ac(); - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { set_land_complete(false); set_throttle_takeoff(); } @@ -623,7 +623,7 @@ void Copter::ModeGuided::angle_control_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // call attitude controller if (guided_angle_state.use_yaw_rate) { diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index c23328f5b9..2148c24df1 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -54,7 +54,7 @@ void Copter::ModeLand::run() void Copter::ModeLand::gps_run() { // disarm when the landing detector says we've landed - if (ap.land_complete && motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { copter.init_disarm_motors(); } @@ -64,7 +64,7 @@ void Copter::ModeLand::gps_run() loiter_nav->init_target(); } else { // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // pause before beginning land descent if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { @@ -105,7 +105,7 @@ void Copter::ModeLand::nogps_run() } // disarm when the landing detector says we've landed - if (ap.land_complete && motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { copter.init_disarm_motors(); } @@ -114,7 +114,7 @@ void Copter::ModeLand::nogps_run() make_safe_spool_down(); } else { // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // pause before beginning land descent if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 7a679a6622..7739f929f5 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -172,7 +172,7 @@ void Copter::ModeLoiter::run() case AltHold_Flying: // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); #if PRECISION_LANDING == ENABLED if (do_precision_loiter()) { diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index d0982c7b7f..9a68e9ffed 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -224,7 +224,7 @@ void Copter::ModePosHold::run() case AltHold_Flying: - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); #if AC_AVOID_ENABLED == ENABLED // apply avoidance diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 08cfc8d820..9433e56fc4 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -144,7 +144,7 @@ void Copter::ModeRTL::climb_return_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); @@ -201,7 +201,7 @@ void Copter::ModeRTL::loiterathome_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); @@ -293,7 +293,7 @@ void Copter::ModeRTL::descent_run() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // process roll, pitch inputs loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); @@ -357,7 +357,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) _state_complete = ap.land_complete; // disarm when the landing detector says we've landed - if (disarm_on_land && ap.land_complete && motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + if (disarm_on_land && ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { copter.init_disarm_motors(); } @@ -369,7 +369,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); land_run_horizontal_control(); land_run_vertical_control(); diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index c49e3f9a0b..7ebf231caf 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -62,7 +62,7 @@ void Copter::ModeSmartRTL::run() void Copter::ModeSmartRTL::wait_cleanup_run() { // hover at current target position - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true); @@ -105,7 +105,7 @@ void Copter::ModeSmartRTL::path_follow_run() } // update controllers - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); @@ -135,7 +135,7 @@ void Copter::ModeSmartRTL::pre_land_position_run() } // update controllers - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 6277ba72ff..0f53aaffe6 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -118,7 +118,7 @@ void Copter::ModeSport::run() break; case AltHold_Flying: - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // adjust climb rate using rangefinder target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index aac81c0cb2..4fde8d5d91 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -20,27 +20,35 @@ void Copter::ModeStabilize::run() if (!motors->armed()) { // Motors should be Stopped - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } else if (ap.throttle_zero) { // Attempting to Land - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } - if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + switch (motors->get_spool_state()) { + case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); - } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); - } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + 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 + break; } // call attitude controller diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index b0ade84275..be0b9dde56 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -41,27 +41,34 @@ void Copter::ModeStabilize_Heli::run() if (!motors->armed()) { // Motors should be Stopped - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } else { // heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } - if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + switch (motors->get_spool_state()) { + case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); - } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + break; + case AP_Motors::SpoolState::GROUND_IDLE: // Landed if (motors->init_targets_on_arming()) { attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); } - } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + break; + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: // clear landing flag above zero throttle if (!motors->limit.throttle_lower) { set_land_complete(false); } + case AP_Motors::SpoolState::SPOOLING_UP: + case AP_Motors::SpoolState::SPOOLING_DOWN: + // do nothing + break; } // call attitude controller diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index f96f9d9b9e..44899f0c11 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -109,9 +109,9 @@ void Copter::ModeThrow::run() // prevent motors from rotating before the throw is detected unless enabled by the user if (g.throw_motor_start == 1) { - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller @@ -124,9 +124,9 @@ void Copter::ModeThrow::run() // prevent motors from rotating before the throw is detected unless enabled by the user if (g.throw_motor_start == 1) { - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } // Hold throttle at zero during the throw and continually reset the attitude controller @@ -142,7 +142,7 @@ void Copter::ModeThrow::run() case Throw_Uprighting: // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // demand a level roll/pitch attitude with zero yaw rate attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); @@ -155,7 +155,7 @@ void Copter::ModeThrow::run() case Throw_HgtStabilise: // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); @@ -169,7 +169,7 @@ void Copter::ModeThrow::run() case Throw_PosHold: // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run loiter controller loiter_nav->update(); diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index c9c7d9c7b9..d53a40c3c4 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -139,7 +139,7 @@ void Copter::ModeZigZag::auto_control() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); @@ -183,7 +183,7 @@ void Copter::ModeZigZag::manual_control() } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run loiter controller loiter_nav->update(); diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index e9b15a0ec5..c41dd0423c 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -94,7 +94,7 @@ void Copter::auto_disarm_check() } // if the rotor is still spinning, don't initiate auto disarm - if (motors->get_spool_mode() != AP_Motors::GROUND_IDLE) { + if (motors->get_spool_state() != AP_Motors::SpoolState::GROUND_IDLE) { auto_disarm_begin = tnow_ms; return; } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 9853bfc454..4480a58765 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -370,7 +370,7 @@ void Copter::update_auto_armed() } // if helicopters are on the ground, and the motor is switched off, auto-armed should be false // so that rotor runup is checked again before attempting to take-off - if(ap.land_complete && motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) { + if(ap.land_complete && motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) { set_auto_armed(false); } }else{ @@ -378,7 +378,7 @@ void Copter::update_auto_armed() // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true if(motors->armed() && ap.using_interlock) { - if(!ap.throttle_zero && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + if(!ap.throttle_zero && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { set_auto_armed(true); } // if motors are armed and throttle is above zero auto_armed should be true diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index cad132b906..48356a0f09 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -33,7 +33,7 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) } // Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning - if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) { + if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) { return false; }