aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_MotorsMulticopter: protect against div-by-zero if MOT_SPIN_ARMED is zero

Browse Source
This commit is contained in:
Randy Mackay 2016-03-25 18:04:52 +09:00
parent b39798ad90
commit 6807b961e2
2 changed files with 18 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -360,25 +360,30 @@ 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){
if (_spool_desired == DESIRED_SHUT_DOWN){
_throttle_low_end_pct -= spool_step;
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED ){
// constrain ramp value and update mode
if (_throttle_low_end_pct <= 0.0f) {
_throttle_low_end_pct = 0.0f;
_multicopter_flags.spool_mode = SHUT_DOWN;
}
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_throttle_low_end_pct += spool_step;
}else{
_throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct()-_throttle_low_end_pct, -spool_step, spool_step);
// constrain ramp value and update mode
if (_throttle_low_end_pct >= 1.0f) {
_throttle_low_end_pct = 1.0f;
_multicopter_flags.spool_mode = SPOOL_UP;
}
} else { // _spool_desired == SPIN_WHEN_ARMED
float spin_when_armed_low_end_pct = 0.0f;
if (_min_throttle > 0) {
spin_when_armed_low_end_pct = (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);
}
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
// constrain ramp value and update mode
if (_throttle_low_end_pct <= 0.0f) {
_throttle_low_end_pct = 0.0f;
_multicopter_flags.spool_mode = SHUT_DOWN;
} else if (_throttle_low_end_pct >= 1.0f) {
_throttle_low_end_pct = 1.0f;
_multicopter_flags.spool_mode = SPOOL_UP;
}
break;
}
case SPOOL_UP:

3
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -132,9 +132,6 @@ protected:
// convert thrust (0~1) range back to pwm range
int16_t calc_thrust_to_pwm(float thrust_in) const;
// spin when armed as a percentage of the 0~1 range from 0 to throttle_min
float spin_when_armed_low_end_pct() { return (float)_spin_when_armed.get() / _min_throttle; }
// flag bitmask
struct {
spool_up_down_mode spool_mode : 3; // motor's current spool mode