diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 0b2182127c..3458623477 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -143,21 +143,14 @@ void AP_MotorsMulticopter::output() // calc filtered battery voltage and lift_max update_lift_max_from_batt_voltage(); - // move throttle_low_comp towards desired throttle low comp - update_throttle_rpy_mix(); + // run spool logic + output_logic(); - if (_flags.armed) { - if (!_flags.interlock) { - output_armed_zero_throttle(); - } else if (_flags.stabilizing) { - output_armed_stabilizing(); - } else { - output_armed_not_stabilizing(); - } - } else { - _multicopter_flags.slow_start_low_end = true; - output_disarmed(); - } + // calculate thrust + output_armed_stabilizing(); + + // convert rpy_thrust values to pwm + output_to_motors(); }; // update the throttle input filter @@ -381,6 +374,163 @@ void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t rad _min_throttle = (float)min_throttle * _throttle_pwm_scalar; } +void AP_MotorsMulticopter::output_logic() +{ + // force desired and current spool mode if disarmed or not interlocked + if (!_flags.armed || !_flags.interlock) { + _multicopter_flags.spool_desired = DESIRED_SHUT_DOWN; + _multicopter_flags.spool_mode = SHUT_DOWN; + _multicopter_flags.slow_start_low_end = true; + } + + 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(_multicopter_flags.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. + // Servoes 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(_multicopter_flags.spool_desired == DESIRED_SHUT_DOWN){ + _throttle_low_end_pct -= spool_step; + } else if(_multicopter_flags.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); + } + _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: + // 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(_multicopter_flags.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(_multicopter_flags.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(_multicopter_flags.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; + } +} + // slow_start - set to true to slew motors from current speed to maximum // Note: this must be set immediately before a step up in throttle void AP_MotorsMulticopter::slow_start(bool true_false) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index d3d1be749f..4bae616bf2 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -39,6 +39,9 @@ #define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 1 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 0.3 second) #endif +// spool definition +#define AP_MOTORS_SPOOL_UP_TIME 0.5f // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle. + /// @class AP_MotorsMulticopter class AP_MotorsMulticopter : public AP_Motors { public: @@ -49,6 +52,9 @@ public: // output - sends commands to the motors virtual void output(); + // output_to_motors - sends commands to the motors + virtual void output_to_motors(){}; + // set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm) void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; } @@ -73,6 +79,26 @@ public: // this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control void set_hover_throttle(uint16_t hov_thr) { _hover_out = hov_thr; } + // spool up states + enum spool_up_down_mode { + SHUT_DOWN = 0, // all motors stop + SPIN_WHEN_ARMED = 1, // all motors at spin when armed + SPOOL_UP = 2, // increasing maximum throttle while stabilizing + THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure + SPOOL_DOWN = 4, // decreasing maximum throttle while stabilizing + }; + + // spool up states + enum spool_up_down_desired { + DESIRED_SHUT_DOWN = 0, // all motors stop + DESIRED_SPIN_WHEN_ARMED = 1, // all motors at spin when armed + DESIRED_SPIN_MIN_THROTTLE = 2, // all motors at min throttle + DESIRED_THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure + }; + + void set_desired_spool_state(enum spool_up_down_desired spool) { _multicopter_flags.spool_desired = spool;} + void output_logic(); + // slow_start - set to true to slew motors from current speed to maximum // Note: this must be set immediately before a step up in throttle void slow_start(bool true_false); @@ -146,6 +172,8 @@ protected: // flag bitmask struct { + spool_up_down_mode spool_mode : 4; // motor's current spool mode + spool_up_down_desired spool_desired : 2; // caller's desired spool mode uint8_t slow_start : 1; // 1 if slow start is active uint8_t slow_start_low_end : 1; // 1 just after arming so we can ramp up the spin_when_armed value } _multicopter_flags; @@ -170,6 +198,10 @@ protected: int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying) int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start) int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000) + 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 // battery voltage, current and air pressure compensation variables float _batt_voltage_resting; // battery voltage reading at minimum throttle