AP_Motors: Heli: add helper to convert from AP_Motors::SpoolState to …

…AP_MotorsHeli_RSC::RotorControlState
ArduPilot · Dec 18, 2023 · 1bcf69e · 1bcf69e
1 parent 287e972
commit 1bcf69e
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 57 deletions.
diff --git 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
@@ -191,6 +191,9 @@ class AP_MotorsHeli : public AP_Motors {
     // 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
@@ -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
@@ -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
@@ -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()) {