From 1bcf69e0c7f55139ee0d9a5bb16577428016336a Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 13 Dec 2023 03:06:29 +0000 Subject: [PATCH] AP_Motors: Heli: add helper to convert from AP_Motors::SpoolState to AP_MotorsHeli_RSC::RotorControlState --- libraries/AP_Motors/AP_MotorsHeli.cpp | 23 ++++++++++++++++++++ libraries/AP_Motors/AP_MotorsHeli.h | 3 +++ libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 21 ++---------------- libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 21 ++---------------- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 20 +---------------- 5 files changed, 31 insertions(+), 57 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 0e246ded6b..fc93264ed5 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -590,3 +590,26 @@ void AP_MotorsHeli::set_desired_rotor_speed(float desired_speed) { _main_rotor.set_desired_speed(desired_speed); } + +// Converts AP_Motors::SpoolState from _spool_state variable to AP_MotorsHeli_RSC::RotorControlState +AP_MotorsHeli_RSC::RotorControlState AP_MotorsHeli::get_rotor_control_state() const +{ + switch (_spool_state) { + case SpoolState::SHUT_DOWN: + // sends minimum values out to the motors + return AP_MotorsHeli_RSC::RotorControlState::STOP; + case SpoolState::GROUND_IDLE: + // sends idle output to motors when armed. rotor could be static or turning (autorotation) + return AP_MotorsHeli_RSC::RotorControlState::IDLE; + case SpoolState::SPOOLING_UP: + case SpoolState::THROTTLE_UNLIMITED: + // set motor output based on thrust requests + return AP_MotorsHeli_RSC::RotorControlState::ACTIVE; + case SpoolState::SPOOLING_DOWN: + // sends idle output to motors and wait for rotor to stop + return AP_MotorsHeli_RSC::RotorControlState::IDLE; + } + + // Should be unreachable, but needed to keep the compiler happy + return AP_MotorsHeli_RSC::RotorControlState::STOP; +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 82287f9b1e..f2a8ce6100 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -191,6 +191,9 @@ protected: // update_motor_controls - sends commands to motor controllers virtual void update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) = 0; + // Converts AP_Motors::SpoolState from _spool_state variable to AP_MotorsHeli_RSC::RotorControlState + AP_MotorsHeli_RSC::RotorControlState get_rotor_control_state() const; + // run spool logic void output_logic(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 18a916e4ae..ddbcedc896 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -525,25 +525,8 @@ void AP_MotorsHeli_Dual::output_to_motors() _swashplate1.output(); _swashplate2.output(); - switch (_spool_state) { - case SpoolState::SHUT_DOWN: - // sends minimum values out to the motors - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::STOP); - break; - case SpoolState::GROUND_IDLE: - // sends idle output to motors when armed. rotor could be static or turning (autorotation) - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE); - break; - case SpoolState::SPOOLING_UP: - case SpoolState::THROTTLE_UNLIMITED: - // set motor output based on thrust requests - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::ACTIVE); - break; - case SpoolState::SPOOLING_DOWN: - // sends idle output to motors and wait for rotor to stop - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE); - break; - } + update_motor_control(get_rotor_control_state()); + } // servo_test - move servos through full range of movement diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 27ea78b251..515ec063e0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -273,25 +273,8 @@ void AP_MotorsHeli_Quad::output_to_motors() rc_write_angle(AP_MOTORS_MOT_1+i, _out[i] * QUAD_SERVO_MAX_ANGLE); } - switch (_spool_state) { - case SpoolState::SHUT_DOWN: - // sends minimum values out to the motors - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::STOP); - break; - case SpoolState::GROUND_IDLE: - // sends idle output to motors when armed. rotor could be static or turning (autorotation) - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE); - break; - case SpoolState::SPOOLING_UP: - case SpoolState::THROTTLE_UNLIMITED: - // set motor output based on thrust requests - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::ACTIVE); - break; - case SpoolState::SPOOLING_DOWN: - // sends idle output to motors and wait for rotor to stop - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE); - break; - } + update_motor_control(get_rotor_control_state()); + } // servo_test - move servos through full range of movement diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index f9ba47407a..6fcefd6004 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -501,25 +501,7 @@ void AP_MotorsHeli_Single::output_to_motors() _swashplate.output(); // Output main rotor - switch (_spool_state) { - case SpoolState::SHUT_DOWN: - // sends minimum values out to the motors - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::STOP); - break; - case SpoolState::GROUND_IDLE: - // sends idle output to motors when armed. rotor could be static or turning (autorotation) - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE); - break; - case SpoolState::SPOOLING_UP: - case SpoolState::THROTTLE_UNLIMITED: - // set motor output based on thrust requests - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::ACTIVE); - break; - case SpoolState::SPOOLING_DOWN: - // sends idle output to motors and wait for rotor to stop - update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE); - break; - } + update_motor_control(get_rotor_control_state()); // Output tail rotor switch (get_tail_type()) {