// 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; } }