// run spool logic
void AP_MotorsMulticopter::output_logic()
{
if (_flags.armed) {
_disarm_safety_timer = 100;
} else if (_disarm_safety_timer != 0) {
_disarm_safety_timer--;
}
// force desired and current spool mode if disarmed or not interlocked
if (!_flags.armed || !_flags.interlock) {
_spool_desired = DESIRED_SHUT_DOWN;
_spool_mode = SHUT_DOWN;
}
if (_spool_up_time < 0.05) {
// prevent float exception
_spool_up_time.set(0.05);
}
switch (_spool_mode) {
case SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) {
_spool_mode = SPIN_WHEN_ARMED;
break;
}
// set and increment ramp variables
_spin_up_ratio = 0.0f;
_throttle_thrust_max = 0.0f;
break;
case SPIN_WHEN_ARMED: {
// Motors should be stationary or at spin when armed.
// Servos should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// set and increment ramp variables
float spool_step = 1.0f/(_spool_up_time*_loop_rate);
if (_spool_desired == DESIRED_SHUT_DOWN){
_spin_up_ratio -= spool_step;
// constrain ramp value and update mode
if (_spin_up_ratio <= 0.0f) {
_spin_up_ratio = 0.0f;
_spool_mode = SHUT_DOWN;
}
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_spin_up_ratio += spool_step;
// constrain ramp value and update mode
if (_spin_up_ratio >= 1.0f) {
_spin_up_ratio = 1.0f;
_spool_mode = SPOOL_UP;
}
} else { // _spool_desired == SPIN_WHEN_ARMED
float spin_up_armed_ratio = 0.0f;
if (_spin_min > 0.0f) {
spin_up_armed_ratio = _spin_arm / _spin_min;
}
_spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step);
}
_throttle_thrust_max = 0.0f;
break;
}
case SPOOL_UP:
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){
_spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max += 1.0f/(_spool_up_time*_loop_rate);
// constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
_throttle_thrust_max = get_current_limit_max_throttle();
_spool_mode = THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f;
}
break;
case THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) {
_spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
break;
case SPOOL_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_spool_mode = SPOOL_UP;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max -= 1.0f/(_spool_up_time*_loop_rate);
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f){
_throttle_thrust_max = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max)) {
_spool_mode = SPIN_WHEN_ARMED;
}
break;
}
}
void AP_MotorsMulticopter::output_logic()
{
// force desired and current spool mode if disarmed or not interlocked
if (!_flags.armed || !_flags.interlock) {
_spool_desired = DESIRED_SHUT_DOWN;
_multicopter_flags.spool_mode = SHUT_DOWN;
}
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 0.0f;
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
break;
case SPIN_WHEN_ARMED: {
// Motors should be stationary or at spin when armed.
// Servos should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// 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;
// 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;
// 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;
break;
}
case SPOOL_UP:
// Maximum throttle should move from minimum to maximum.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){
_multicopter_flags.spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max += 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
// constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
_throttle_thrust_max = get_current_limit_max_throttle();
_multicopter_flags.spool_mode = THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f;
}
break;
case THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) {
_multicopter_flags.spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
update_throttle_rpy_mix();
break;
case SPOOL_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_multicopter_flags.spool_mode = SPOOL_UP;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix_desired = _throttle_rpy_mix;
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f){
_throttle_thrust_max = 0.0f;
}
if (_throttle_rpy_mix <= 0.0f){
_throttle_rpy_mix = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max) && is_zero(_throttle_rpy_mix)) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
}
break;
}
}