AP_Motors: spool_mode moved out of flags struction

No functional change
This commit is contained in:
Randy Mackay 2016-06-04 14:54:00 +09:00
parent 8fff32bde3
commit ef106e4b0f
6 changed files with 17 additions and 21 deletions

View File

@ -114,7 +114,7 @@ void AP_MotorsCoax::enable()
void AP_MotorsCoax::output_to_motors() void AP_MotorsCoax::output_to_motors()
{ {
switch (_multicopter_flags.spool_mode) { switch (_spool_mode) {
case SHUT_DOWN: case SHUT_DOWN:
// sends minimum values out to the motors // sends minimum values out to the motors
hal.rcout->cork(); hal.rcout->cork();

View File

@ -88,7 +88,7 @@ void AP_MotorsMatrix::output_to_motors()
int8_t i; int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
switch (_multicopter_flags.spool_mode) { switch (_spool_mode) {
case SHUT_DOWN: case SHUT_DOWN:
// sends minimum values out to the motors // sends minimum values out to the motors
// set motor output based on thrust requests // set motor output based on thrust requests

View File

@ -193,7 +193,7 @@ void AP_MotorsMulticopter::output()
void AP_MotorsMulticopter::output_min() void AP_MotorsMulticopter::output_min()
{ {
set_desired_spool_state(DESIRED_SHUT_DOWN); set_desired_spool_state(DESIRED_SHUT_DOWN);
_multicopter_flags.spool_mode = SHUT_DOWN; _spool_mode = SHUT_DOWN;
output(); output();
} }
@ -385,10 +385,10 @@ void AP_MotorsMulticopter::output_logic()
// force desired and current spool mode if disarmed or not interlocked // force desired and current spool mode if disarmed or not interlocked
if (!_flags.armed || !_flags.interlock) { if (!_flags.armed || !_flags.interlock) {
_spool_desired = DESIRED_SHUT_DOWN; _spool_desired = DESIRED_SHUT_DOWN;
_multicopter_flags.spool_mode = SHUT_DOWN; _spool_mode = SHUT_DOWN;
} }
switch (_multicopter_flags.spool_mode) { switch (_spool_mode) {
case SHUT_DOWN: case SHUT_DOWN:
// Motors should be stationary. // Motors should be stationary.
// Servos set to their trim values or in a test condition. // Servos set to their trim values or in a test condition.
@ -401,7 +401,7 @@ void AP_MotorsMulticopter::output_logic()
// make sure the motors are spooling in the correct direction // make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) { if (_spool_desired != DESIRED_SHUT_DOWN) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED; _spool_mode = SPIN_WHEN_ARMED;
break; break;
} }
@ -427,14 +427,14 @@ void AP_MotorsMulticopter::output_logic()
// constrain ramp value and update mode // constrain ramp value and update mode
if (_spin_up_ratio <= 0.0f) { if (_spin_up_ratio <= 0.0f) {
_spin_up_ratio = 0.0f; _spin_up_ratio = 0.0f;
_multicopter_flags.spool_mode = SHUT_DOWN; _spool_mode = SHUT_DOWN;
} }
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) { } else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_spin_up_ratio += spool_step; _spin_up_ratio += spool_step;
// constrain ramp value and update mode // constrain ramp value and update mode
if (_spin_up_ratio >= 1.0f) { if (_spin_up_ratio >= 1.0f) {
_spin_up_ratio = 1.0f; _spin_up_ratio = 1.0f;
_multicopter_flags.spool_mode = SPOOL_UP; _spool_mode = SPOOL_UP;
} }
} else { // _spool_desired == SPIN_WHEN_ARMED } else { // _spool_desired == SPIN_WHEN_ARMED
float spin_up_armed_ratio = 0.0f; float spin_up_armed_ratio = 0.0f;
@ -458,7 +458,7 @@ void AP_MotorsMulticopter::output_logic()
// make sure the motors are spooling in the correct direction // make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){ if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){
_multicopter_flags.spool_mode = SPOOL_DOWN; _spool_mode = SPOOL_DOWN;
break; break;
} }
@ -469,7 +469,7 @@ void AP_MotorsMulticopter::output_logic()
// constrain ramp value and update mode // constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) { if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
_throttle_thrust_max = get_current_limit_max_throttle(); _throttle_thrust_max = get_current_limit_max_throttle();
_multicopter_flags.spool_mode = THROTTLE_UNLIMITED; _spool_mode = THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f) { } else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f; _throttle_thrust_max = 0.0f;
} }
@ -487,7 +487,7 @@ void AP_MotorsMulticopter::output_logic()
// make sure the motors are spooling in the correct direction // make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) { if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) {
_multicopter_flags.spool_mode = SPOOL_DOWN; _spool_mode = SPOOL_DOWN;
break; break;
} }
@ -508,7 +508,7 @@ void AP_MotorsMulticopter::output_logic()
// make sure the motors are spooling in the correct direction // make sure the motors are spooling in the correct direction
if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) { if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_multicopter_flags.spool_mode = SPOOL_UP; _spool_mode = SPOOL_UP;
break; break;
} }
@ -523,7 +523,7 @@ void AP_MotorsMulticopter::output_logic()
if (_throttle_thrust_max >= get_current_limit_max_throttle()) { if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle(); _throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max)) { } else if (is_zero(_throttle_thrust_max)) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED; _spool_mode = SPIN_WHEN_ARMED;
} }
break; break;
} }

View File

@ -82,7 +82,7 @@ public:
float get_throttle_thrust_max() const { return _throttle_thrust_max; } float get_throttle_thrust_max() const { return _throttle_thrust_max; }
// return true if spool up is complete // return true if spool up is complete
bool spool_up_complete() const { return _multicopter_flags.spool_mode == THROTTLE_UNLIMITED; } bool spool_up_complete() const { return _spool_mode == THROTTLE_UNLIMITED; }
// output a thrust to all motors that match a given motor // output a thrust to all motors that match a given motor
// mask. This is used to control tiltrotor motors in forward // mask. This is used to control tiltrotor motors in forward
@ -140,11 +140,6 @@ protected:
// save parameters as part of disarming // save parameters as part of disarming
void save_params_on_disarm(); void save_params_on_disarm();
// flag bitmask
struct {
spool_up_down_mode spool_mode : 3; // motor's current spool mode
} _multicopter_flags;
// parameters // parameters
AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range
AP_Float _thrust_curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation AP_Float _thrust_curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation
@ -166,6 +161,7 @@ protected:
int16_t _throttle_radio_max; // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX) int16_t _throttle_radio_max; // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX)
// spool variables // spool variables
spool_up_down_mode _spool_mode; // motor's current spool mode
float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min
// battery voltage, current and air pressure compensation variables // battery voltage, current and air pressure compensation variables

View File

@ -117,7 +117,7 @@ void AP_MotorsSingle::enable()
void AP_MotorsSingle::output_to_motors() void AP_MotorsSingle::output_to_motors()
{ {
switch (_multicopter_flags.spool_mode) { switch (_spool_mode) {
case SHUT_DOWN: case SHUT_DOWN:
// sends minimum values out to the motors // sends minimum values out to the motors
hal.rcout->cork(); hal.rcout->cork();

View File

@ -124,7 +124,7 @@ void AP_MotorsTri::enable()
void AP_MotorsTri::output_to_motors() void AP_MotorsTri::output_to_motors()
{ {
switch (_multicopter_flags.spool_mode) { switch (_spool_mode) {
case SHUT_DOWN: case SHUT_DOWN:
// sends minimum values out to the motors // sends minimum values out to the motors
hal.rcout->cork(); hal.rcout->cork();