From bf8001cb88f6ab4d6d6f0e8673608922caae86e1 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 13 Oct 2015 17:56:06 -0400 Subject: [PATCH] AP_MotorsHeli: Change rotor control state into Enum. --- libraries/AP_Motors/AP_MotorsHeli.cpp | 10 +++++----- libraries/AP_Motors/AP_MotorsHeli.h | 2 +- libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 2 +- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 16 +++++++++------- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 2 +- libraries/AP_Motors/AP_MotorsHeli_Single.h | 2 +- 6 files changed, 18 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index ab2581e77f..60de3bbd79 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -176,7 +176,7 @@ void AP_MotorsHeli::output_min() // move swash to mid move_actuators(0,0,500,0); - update_motor_control(ROTOR_CONTROL_STOP); + update_motor_control(AP_MotorsHeli_RSC::ROTOR_CONTROL_STOP); // override limits flags limit.roll_pitch = true; @@ -214,7 +214,7 @@ void AP_MotorsHeli::output_armed_stabilizing() move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - update_motor_control(ROTOR_CONTROL_ACTIVE); + update_motor_control(AP_MotorsHeli_RSC::ROTOR_CONTROL_ACTIVE); } void AP_MotorsHeli::output_armed_not_stabilizing() @@ -227,7 +227,7 @@ void AP_MotorsHeli::output_armed_not_stabilizing() // helicopters always run stabilizing flight controls move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - update_motor_control(ROTOR_CONTROL_ACTIVE); + update_motor_control(AP_MotorsHeli_RSC::ROTOR_CONTROL_ACTIVE); } // output_armed_zero_throttle - sends commands to the motors @@ -240,7 +240,7 @@ void AP_MotorsHeli::output_armed_zero_throttle() move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - update_motor_control(ROTOR_CONTROL_IDLE); + update_motor_control(AP_MotorsHeli_RSC::ROTOR_CONTROL_IDLE); } // output_disarmed - sends commands to the motors @@ -270,7 +270,7 @@ void AP_MotorsHeli::output_disarmed() // helicopters always run stabilizing flight controls move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - update_motor_control(ROTOR_CONTROL_STOP); + update_motor_control(AP_MotorsHeli_RSC::ROTOR_CONTROL_STOP); } // parameter_check - check if helicopter specific parameters are sensible diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index ca6851507b..5f6b16ecfd 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -182,7 +182,7 @@ protected: void output_disarmed(); // update_motor_controls - sends commands to motor controllers - virtual void update_motor_control(uint8_t state) = 0; + virtual void update_motor_control(AP_MotorsHeli_RSC::MotorControlState state) = 0; // reset_flight_controls - resets all controls and scalars to flight status void reset_flight_controls(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index f6668281b4..a1b540b7a4 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -59,7 +59,7 @@ void AP_MotorsHeli_RSC::set_power_output_range(uint16_t power_low, uint16_t powe } // output - update value to send to ESC/Servo -void AP_MotorsHeli_RSC::output(uint8_t state) +void AP_MotorsHeli_RSC::output(MotorControlState state) { switch (state){ case ROTOR_CONTROL_STOP: diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 087e229f68..ea54b65554 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -5,12 +5,7 @@ #include #include // ArduPilot Mega Vector/Matrix math Library -#include // RC Channel Library - -// rotor controller states -#define ROTOR_CONTROL_STOP 0 -#define ROTOR_CONTROL_IDLE 1 -#define ROTOR_CONTROL_ACTIVE 2 +#include // RC Channel Library // rotor control modes #define ROTOR_CONTROL_MODE_DISABLED 0 @@ -74,8 +69,15 @@ public: // recalc_scalers void recalc_scalers(); + // motor control states + enum MotorControlState { + ROTOR_CONTROL_STOP, + ROTOR_CONTROL_IDLE, + ROTOR_CONTROL_ACTIVE, + }; + // output - update value to send to ESC/Servo - void output(uint8_t state); + void output(MotorControlState state); private: diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 94fe52fff3..b8192c4b59 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -314,7 +314,7 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() } // update_motor_controls - sends commands to motor controllers -void AP_MotorsHeli_Single::update_motor_control(uint8_t state) +void AP_MotorsHeli_Single::update_motor_control(AP_MotorsHeli_RSC::MotorControlState state) { // Send state update to motors _tail_rotor.output(state); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 5caf5d341a..c9eba1ea7a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -131,7 +131,7 @@ protected: void init_outputs(); // update_motor_controls - sends commands to motor controllers - void update_motor_control(uint8_t state); + void update_motor_control(AP_MotorsHeli_RSC::MotorControlState state); // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position void calculate_roll_pitch_collective_factors();