Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: add calc_spin_up_to_pwm to reduce repeated code

This commit is contained in:
Leonard Hall 2016-06-08 18:10:54 +09:00 committed by Randy Mackay
parent 25778a24e2
commit 488f90b39d
6 changed files with 37 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -129,12 +129,12 @@ void AP_MotorsCoax::output_to_motors()
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
hal.rcout->push();
break;
case SPOOL_UP:

2
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -102,7 +102,7 @@ void AP_MotorsMatrix::output_to_motors()
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle);
motor_out[i] = calc_spin_up_to_pwm();
}
}
break;

31
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -310,7 +310,10 @@ int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
thrust_in = constrain_float(thrust_in, 0, 1);
return constrain_int16((get_pwm_output_min() + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) *
(get_pwm_output_max() - (get_pwm_output_min() + _min_throttle))), get_pwm_output_min() + _min_throttle, get_pwm_output_max());
}
int16_t AP_MotorsMulticopter::calc_spin_up_to_pwm() const
{
return get_pwm_output_min() + constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min * (get_pwm_output_max()-get_pwm_output_min());}
// get minimum or maximum pwm value that can be output to motors
int16_t AP_MotorsMulticopter::get_pwm_output_min() const
@ -379,7 +382,7 @@ void AP_MotorsMulticopter::output_logic()
}
// set and increment ramp variables
_throttle_low_end_pct = 0.0f;
_spin_up_ratio = 0.0f;
_throttle_thrust_max = 0.0f;
break;
@ -396,25 +399,25 @@ void AP_MotorsMulticopter::output_logic()
// set and increment ramp variables
float spool_step = 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
if (_spool_desired == DESIRED_SHUT_DOWN){
_throttle_low_end_pct -= spool_step;
_spin_up_ratio -= spool_step;
// constrain ramp value and update mode
if (_throttle_low_end_pct <= 0.0f) {
_throttle_low_end_pct = 0.0f;
if (_spin_up_ratio <= 0.0f) {
_spin_up_ratio = 0.0f;
_multicopter_flags.spool_mode = SHUT_DOWN;
}
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_throttle_low_end_pct += spool_step;
_spin_up_ratio += spool_step;
// constrain ramp value and update mode
if (_throttle_low_end_pct >= 1.0f) {
_throttle_low_end_pct = 1.0f;
if (_spin_up_ratio >= 1.0f) {
_spin_up_ratio = 1.0f;
_multicopter_flags.spool_mode = SPOOL_UP;
}
} else { // _spool_desired == SPIN_WHEN_ARMED
float spin_when_armed_low_end_pct = 0.0f;
float spin_up_armed_ratio = 0.0f;
if (_min_throttle > 0) {
spin_when_armed_low_end_pct = (float)_spin_when_armed / _min_throttle;
spin_up_armed_ratio = (float)_spin_when_armed / _min_throttle;
}
_throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct-_throttle_low_end_pct, -spool_step, spool_step);
_spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step);
}
_throttle_thrust_max = 0.0f;
break;
@ -436,7 +439,7 @@ void AP_MotorsMulticopter::output_logic()
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_spin_up_ratio = 1.0f;
_throttle_thrust_max += 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
// constrain ramp value and update mode
@ -465,7 +468,7 @@ void AP_MotorsMulticopter::output_logic()
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_spin_up_ratio = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
break;
@ -486,7 +489,7 @@ void AP_MotorsMulticopter::output_logic()
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_spin_up_ratio = 1.0f;
_throttle_thrust_max -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
// constrain ramp value and update mode

5
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -128,6 +128,9 @@ protected:
// convert thrust (0~1) range back to pwm range
int16_t calc_thrust_to_pwm(float thrust_in) const;
// calculate spin up to pwm range
int16_t calc_spin_up_to_pwm() const;
// apply any thrust compensation for the frame
virtual void thrust_compensation(void) {}
@ -161,7 +164,7 @@ protected:
float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
// spool variables
float _throttle_low_end_pct; // 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
float _batt_voltage_resting; // battery voltage reading at minimum throttle

12
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -132,12 +132,12 @@ void AP_MotorsSingle::output_to_motors()
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
hal.rcout->push();
break;
case SPOOL_UP:

6
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -137,9 +137,9 @@ void AP_MotorsTri::output_to_motors()
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_2, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_4, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_1, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_2, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_4, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
hal.rcout->push();
break;