diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp index 6f93be68b5..068b777777 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.cpp +++ b/libraries/AP_Motors/AP_Motors6DOF.cpp @@ -229,8 +229,8 @@ void AP_Motors6DOF::output_to_motors() int8_t i; int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor - switch (_spool_mode) { - case SHUT_DOWN: + switch (_spool_state) { + case SpoolState::SHUT_DOWN: // sends minimum values out to the motors // set motor output based on thrust requests for (i=0; i= 1.0f) { _spin_up_ratio = 1.0f; - _spool_mode = SPOOL_UP; + _spool_state = SpoolState::SPOOLING_UP; } - } else { // _spool_desired == GROUND_IDLE + break; + case DesiredSpoolState::GROUND_IDLE: float spin_up_armed_ratio = 0.0f; if (_spin_min > 0.0f) { spin_up_armed_ratio = _spin_arm / _spin_min; } _spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step); + break; } _throttle_thrust_max = 0.0f; @@ -579,7 +583,7 @@ void AP_MotorsMulticopter::output_logic() _thrust_boost_ratio = 0.0f; break; } - case SPOOL_UP: + case SpoolState::SPOOLING_UP: // Maximum throttle should move from minimum to maximum. // Servos should exhibit normal flight behavior. @@ -590,8 +594,8 @@ void AP_MotorsMulticopter::output_logic() limit.throttle_upper = false; // make sure the motors are spooling in the correct direction - if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){ - _spool_mode = SPOOL_DOWN; + if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED ){ + _spool_state = SpoolState::SPOOLING_DOWN; break; } @@ -602,7 +606,7 @@ void AP_MotorsMulticopter::output_logic() // constrain ramp value and update mode if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) { _throttle_thrust_max = get_current_limit_max_throttle(); - _spool_mode = THROTTLE_UNLIMITED; + _spool_state = SpoolState::THROTTLE_UNLIMITED; } else if (_throttle_thrust_max < 0.0f) { _throttle_thrust_max = 0.0f; } @@ -612,7 +616,7 @@ void AP_MotorsMulticopter::output_logic() _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0 / (_spool_up_time * _loop_rate)); break; - case THROTTLE_UNLIMITED: + case SpoolState::THROTTLE_UNLIMITED: // Throttle should exhibit normal flight behavior. // Servos should exhibit normal flight behavior. @@ -623,8 +627,8 @@ void AP_MotorsMulticopter::output_logic() limit.throttle_upper = false; // make sure the motors are spooling in the correct direction - if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) { - _spool_mode = SPOOL_DOWN; + if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) { + _spool_state = SpoolState::SPOOLING_DOWN; break; } @@ -639,7 +643,7 @@ void AP_MotorsMulticopter::output_logic() } break; - case SPOOL_DOWN: + case SpoolState::SPOOLING_DOWN: // Maximum throttle should move from maximum to minimum. // Servos should exhibit normal flight behavior. @@ -650,8 +654,8 @@ void AP_MotorsMulticopter::output_logic() limit.throttle_upper = false; // make sure the motors are spooling in the correct direction - if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) { - _spool_mode = SPOOL_UP; + if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) { + _spool_state = SpoolState::SPOOLING_UP; break; } @@ -666,7 +670,7 @@ void AP_MotorsMulticopter::output_logic() if (_throttle_thrust_max >= get_current_limit_max_throttle()) { _throttle_thrust_max = get_current_limit_max_throttle(); } else if (is_zero(_throttle_thrust_max)) { - _spool_mode = GROUND_IDLE; + _spool_state = SpoolState::GROUND_IDLE; } _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio-1.0f/(_spool_up_time*_loop_rate)); diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 5934b966cd..c65cdfe2bd 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -68,7 +68,7 @@ public: float get_throttle_thrust_max() const { return _throttle_thrust_max; } // return true if spool up is complete - bool spool_up_complete() const { return _spool_mode == THROTTLE_UNLIMITED; } + bool spool_up_complete() const { return _spool_state == SpoolState::THROTTLE_UNLIMITED; } // output a thrust to all motors that match a given motor // mask. This is used to control tiltrotor motors in forward diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 54dd8e1714..84480daa50 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -71,8 +71,8 @@ void AP_MotorsSingle::output_to_motors() if (!_flags.initialised_ok) { return; } - switch (_spool_mode) { - case SHUT_DOWN: + switch (_spool_state) { + case SpoolState::SHUT_DOWN: // sends minimum values out to the motors rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); @@ -81,7 +81,7 @@ void AP_MotorsSingle::output_to_motors() rc_write(AP_MOTORS_MOT_5, output_to_pwm(0)); rc_write(AP_MOTORS_MOT_6, output_to_pwm(0)); break; - case GROUND_IDLE: + case SpoolState::GROUND_IDLE: // sends output to motors when armed but not flying for (uint8_t i=0; iget_trim()); break; - case GROUND_IDLE: + case SpoolState::GROUND_IDLE: // sends output to motors when armed but not flying set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle()); set_actuator_with_slew(_actuator[2], actuator_spin_up_to_ground_idle()); @@ -95,9 +95,9 @@ void AP_MotorsTri::output_to_motors() rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4])); rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim()); break; - case SPOOL_UP: - case THROTTLE_UNLIMITED: - case SPOOL_DOWN: + case SpoolState::SPOOLING_UP: + case SpoolState::THROTTLE_UNLIMITED: + case SpoolState::SPOOLING_DOWN: // set motor output based on thrust requests set_actuator_with_slew(_actuator[1], thrust_to_actuator(_thrust_right)); set_actuator_with_slew(_actuator[2], thrust_to_actuator(_thrust_left)); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index cf85dafb2a..8e14dadd03 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -34,8 +34,8 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) : _loop_rate(loop_rate), _speed_hz(speed_hz), _throttle_filter(), - _spool_desired(DESIRED_SHUT_DOWN), - _spool_mode(SHUT_DOWN), + _spool_desired(DesiredSpoolState::SHUT_DOWN), + _spool_state(SpoolState::SHUT_DOWN), _air_density_ratio(1.0f) { _singleton = this; @@ -64,9 +64,9 @@ void AP_Motors::armed(bool arm) } }; -void AP_Motors::set_desired_spool_state(enum spool_up_down_desired spool) +void AP_Motors::set_desired_spool_state(DesiredSpoolState spool) { - if (_flags.armed || (spool == DESIRED_SHUT_DOWN)) { + if (_flags.armed || (spool == DesiredSpoolState::SHUT_DOWN)) { _spool_desired = spool; } }; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 5668037961..9bee95560c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -105,27 +105,27 @@ public: virtual uint8_t get_lost_motor() const { return 0; } // desired spool states - enum spool_up_down_desired { - DESIRED_SHUT_DOWN = 0, // all motors stop - DESIRED_GROUND_IDLE = 1, // all motors at ground idle - DESIRED_THROTTLE_UNLIMITED = 2, // motors are no longer constrained by start up procedure + enum class DesiredSpoolState : uint8_t { + SHUT_DOWN = 0, // all motors should move to stop + GROUND_IDLE = 1, // all motors should move to ground idle + THROTTLE_UNLIMITED = 2, // motors should move to being a state where throttle is unconstrained (e.g. by start up procedure) }; - void set_desired_spool_state(enum spool_up_down_desired spool); + void set_desired_spool_state(enum DesiredSpoolState spool); - enum spool_up_down_desired get_desired_spool_state(void) const { return _spool_desired; } + enum DesiredSpoolState get_desired_spool_state(void) const { return _spool_desired; } // spool states - enum spool_up_down_mode { + enum class SpoolState : uint8_t { SHUT_DOWN = 0, // all motors stop GROUND_IDLE = 1, // all motors at ground idle - SPOOL_UP = 2, // increasing maximum throttle while stabilizing + SPOOLING_UP = 2, // increasing maximum throttle while stabilizing THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure - SPOOL_DOWN = 4, // decreasing maximum throttle while stabilizing + SPOOLING_DOWN = 4, // decreasing maximum throttle while stabilizing }; - // get_spool_mode - get current spool mode - enum spool_up_down_mode get_spool_mode(void) const { return _spool_mode; } + // get_spool_state - get current spool state + enum SpoolState get_spool_state(void) const { return _spool_state; } // set_density_ratio - sets air density as a proportion of sea level density void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; } @@ -221,8 +221,8 @@ protected: float _lateral_in; // last lateral input from set_lateral caller float _throttle_avg_max; // last throttle input from set_throttle_avg_max LowPassFilterFloat _throttle_filter; // throttle input filter - spool_up_down_desired _spool_desired; // desired spool state - spool_up_down_mode _spool_mode; // current spool mode + DesiredSpoolState _spool_desired; // desired spool state + SpoolState _spool_state; // current spool mode // air pressure compensation variables float _air_density_ratio; // air density / sea level density - decreases in altitude diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index 5dd9e2ca45..9048d6a6b1 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -163,7 +163,7 @@ void stability_test() motors.set_pitch(pitch_in/4500.0f); motors.set_yaw(yaw_in/4500.0f); motors.set_throttle(throttle_in); - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); update_motors(); avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4); // display input and output