Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: make desired spool state and spool state enum-classes 

This will help avoid getting the enumerations mixed up
This commit is contained in:
Peter Barker 2019-04-09 22:15:45 +10:00 committed by Randy Mackay
parent 79fa39e13b
commit 1e606cdc5b
15 changed files with 126 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Motors/AP_Motors6DOF.cpp
View File

@ -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<AP_MOTORS_MAX_NUM_MOTORS; i++) {
@ -239,7 +239,7 @@ void AP_Motors6DOF::output_to_motors()
}
}
break;
case GROUND_IDLE:
case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
@ -247,9 +247,9 @@ void AP_Motors6DOF::output_to_motors()
}
}
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
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {

12
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -68,8 +68,8 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz )
void AP_MotorsCoax::output_to_motors()
{
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_COAX_SERVO_INPUT_RANGE);
rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
@ -78,7 +78,7 @@ void AP_MotorsCoax::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; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
@ -88,9 +88,9 @@ void AP_MotorsCoax::output_to_motors()
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
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
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);

46
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -365,81 +365,81 @@ void AP_MotorsHeli::output_logic()
// force desired and current spool mode if disarmed and armed with interlock enabled
if (_flags.armed) {
if (!_flags.interlock) {
_spool_desired = DESIRED_GROUND_IDLE;
_spool_desired = DesiredSpoolState::GROUND_IDLE;
} else {
_heliflags.init_targets_on_arming = false;
}
} else {
_heliflags.init_targets_on_arming = true;
_spool_desired = DESIRED_SHUT_DOWN;
_spool_mode = SHUT_DOWN;
_spool_desired = DesiredSpoolState::SHUT_DOWN;
_spool_state = SpoolState::SHUT_DOWN;
}
switch (_spool_mode) {
case SHUT_DOWN:
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) {
_spool_mode = GROUND_IDLE;
if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
_spool_state = SpoolState::GROUND_IDLE;
break;
}
break;
case GROUND_IDLE: {
case SpoolState::GROUND_IDLE: {
// Motors should be stationary or at ground idle.
// Servos should be moving to correct the current attitude.
if (_spool_desired == DESIRED_SHUT_DOWN){
_spool_mode = SHUT_DOWN;
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_spool_mode = SPOOL_UP;
if (_spool_desired == DesiredSpoolState::SHUT_DOWN){
_spool_state = SpoolState::SHUT_DOWN;
} else if(_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) {
_spool_state = SpoolState::SPOOLING_UP;
} else { // _spool_desired == GROUND_IDLE
}
break;
}
case SPOOL_UP:
case SpoolState::SPOOLING_UP:
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// 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;
}
if (_heliflags.rotor_runup_complete){
_spool_mode = THROTTLE_UNLIMITED;
_spool_state = SpoolState::THROTTLE_UNLIMITED;
}
break;
case THROTTLE_UNLIMITED:
case SpoolState::THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
// 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;
}
break;
case SPOOL_DOWN:
case SpoolState::SPOOLING_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
// 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;
}
if (!rotor_speed_above_critical()){
_spool_mode = GROUND_IDLE;
_spool_state = SpoolState::GROUND_IDLE;
}
break;
}

13
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -492,25 +492,24 @@ void AP_MotorsHeli_Dual::output_to_motors()
rc_write_swash(AP_MOTORS_MOT_8, _servo_out[CH_8]);
}
switch (_spool_mode) {
case SHUT_DOWN:
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// sends minimum values out to the motors
update_motor_control(ROTOR_CONTROL_STOP);
break;
case GROUND_IDLE:
case SpoolState::GROUND_IDLE:
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
update_motor_control(ROTOR_CONTROL_IDLE);
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
// set motor output based on thrust requests
update_motor_control(ROTOR_CONTROL_ACTIVE);
break;
case SPOOL_DOWN:
case SpoolState::SPOOLING_DOWN:
// sends idle output to motors and wait for rotor to stop
update_motor_control(ROTOR_CONTROL_IDLE);
break;
}
}

12
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -279,21 +279,21 @@ void AP_MotorsHeli_Quad::output_to_motors()
rc_write_angle(AP_MOTORS_MOT_1+i, _out[i] * QUAD_SERVO_MAX_ANGLE);
}
switch (_spool_mode) {
case SHUT_DOWN:
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// sends minimum values out to the motors
update_motor_control(ROTOR_CONTROL_STOP);
break;
case GROUND_IDLE:
case SpoolState::GROUND_IDLE:
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
update_motor_control(ROTOR_CONTROL_IDLE);
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
// set motor output based on thrust requests
update_motor_control(ROTOR_CONTROL_ACTIVE);
break;
case SPOOL_DOWN:
case SpoolState::SPOOLING_DOWN:
// sends idle output to motors and wait for rotor to stop
update_motor_control(ROTOR_CONTROL_IDLE);
break;

12
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -433,23 +433,23 @@ void AP_MotorsHeli_Single::output_to_motors()
}
}
switch (_spool_mode) {
case SHUT_DOWN:
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// sends minimum values out to the motors
update_motor_control(ROTOR_CONTROL_STOP);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
}
break;
case GROUND_IDLE:
case SpoolState::GROUND_IDLE:
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
update_motor_control(ROTOR_CONTROL_IDLE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
}
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
// set motor output based on thrust requests
update_motor_control(ROTOR_CONTROL_ACTIVE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
@ -459,7 +459,7 @@ void AP_MotorsHeli_Single::output_to_motors()
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
}
break;
case SPOOL_DOWN:
case SpoolState::SPOOLING_DOWN:
// sends idle output to motors and wait for rotor to stop
update_motor_control(ROTOR_CONTROL_IDLE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){

12
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -73,8 +73,8 @@ void AP_MotorsMatrix::output_to_motors()
{
int8_t i;
switch (_spool_mode) {
case SHUT_DOWN: {
switch (_spool_state) {
case SpoolState::SHUT_DOWN: {
// no output
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
@ -83,7 +83,7 @@ void AP_MotorsMatrix::output_to_motors()
}
break;
}
case GROUND_IDLE:
case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
@ -91,9 +91,9 @@ void AP_MotorsMatrix::output_to_motors()
}
}
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
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {

56
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -261,8 +261,8 @@ void AP_MotorsMulticopter::output_boost_throttle(void)
// sends minimum values out to the motors
void AP_MotorsMulticopter::output_min()
{
set_desired_spool_state(DESIRED_SHUT_DOWN);
_spool_mode = SHUT_DOWN;
set_desired_spool_state(DesiredSpoolState::SHUT_DOWN);
_spool_state = SpoolState::SHUT_DOWN;
output();
}
@ -385,7 +385,7 @@ float AP_MotorsMulticopter::get_compensation_gain() const
int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
{
float pwm_output;
if (_spool_mode == SHUT_DOWN) {
if (_spool_state == SpoolState::SHUT_DOWN) {
// in shutdown mode, use PWM 0 or minimum PWM
if (_disarm_disable_pwm && _disarm_safety_timer == 0 && !armed()) {
pwm_output = 0;
@ -504,8 +504,8 @@ void AP_MotorsMulticopter::output_logic()
// force desired and current spool mode if disarmed or not interlocked
if (!_flags.armed || !_flags.interlock) {
_spool_desired = DESIRED_SHUT_DOWN;
_spool_mode = SHUT_DOWN;
_spool_desired = DesiredSpoolState::SHUT_DOWN;
_spool_state = SpoolState::SHUT_DOWN;
}
if (_spool_up_time < 0.05) {
@ -513,8 +513,8 @@ void AP_MotorsMulticopter::output_logic()
_spool_up_time.set(0.05);
}
switch (_spool_mode) {
case SHUT_DOWN:
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
@ -525,8 +525,8 @@ void AP_MotorsMulticopter::output_logic()
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) {
_spool_mode = GROUND_IDLE;
if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
_spool_state = SpoolState::GROUND_IDLE;
break;
}
@ -539,7 +539,7 @@ void AP_MotorsMulticopter::output_logic()
_thrust_boost_ratio = 0.0f;
break;
case GROUND_IDLE: {
case SpoolState::GROUND_IDLE: {
// Motors should be stationary or at ground idle.
// Servos should be moving to correct the current attitude.
@ -551,26 +551,30 @@ void AP_MotorsMulticopter::output_logic()
// set and increment ramp variables
float spool_step = 1.0f/(_spool_up_time*_loop_rate);
if (_spool_desired == DESIRED_SHUT_DOWN){
switch (_spool_desired) {
case DesiredSpoolState::SHUT_DOWN:
_spin_up_ratio -= spool_step;
// constrain ramp value and update mode
if (_spin_up_ratio <= 0.0f) {
_spin_up_ratio = 0.0f;
_spool_mode = SHUT_DOWN;
_spool_state = SpoolState::SHUT_DOWN;
}
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
break;
case DesiredSpoolState::THROTTLE_UNLIMITED:
_spin_up_ratio += spool_step;
// constrain ramp value and update mode
if (_spin_up_ratio >= 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));

2
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -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

12
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -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; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
@ -91,9 +91,9 @@ void AP_MotorsSingle::output_to_motors()
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
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
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);

12
libraries/AP_Motors/AP_MotorsTailsitter.cpp
View File

@ -82,19 +82,19 @@ void AP_MotorsTailsitter::output_to_motors()
return;
}
switch (_spool_mode) {
case SHUT_DOWN:
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, get_pwm_output_min());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, get_pwm_output_min());
break;
case GROUND_IDLE:
case SpoolState::GROUND_IDLE:
set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, output_to_pwm(actuator_spin_up_to_ground_idle()));
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, output_to_pwm(actuator_spin_up_to_ground_idle()));
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN:
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, output_to_pwm(thrust_to_actuator(_thrust_left)));
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, output_to_pwm(thrust_to_actuator(_thrust_right)));
break;

12
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -77,15 +77,15 @@ void AP_MotorsTri::set_update_rate( uint16_t speed_hz )
void AP_MotorsTri::output_to_motors()
{
switch (_spool_mode) {
case SHUT_DOWN:
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, output_to_pwm(0));
rc_write(AP_MOTORS_MOT_2, output_to_pwm(0));
rc_write(AP_MOTORS_MOT_4, output_to_pwm(0));
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_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));

8
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -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;
}
};

26
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -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

2
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
View File

@ -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