AP_MotorsHeli: Change rotor control state into Enum.

This commit is contained in:
Robert Lefebvre 2015-10-13 17:56:06 -04:00 committed by Randy Mackay
parent 6922f7a2b1
commit bf8001cb88
6 changed files with 18 additions and 16 deletions

View File

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

View File

@ -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();

View File

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

View File

@ -5,12 +5,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
// rotor controller states
#define ROTOR_CONTROL_STOP 0
#define ROTOR_CONTROL_IDLE 1
#define ROTOR_CONTROL_ACTIVE 2
#include <RC_Channel/RC_Channel.h> // 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:

View File

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

View File

@ -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();