// get acceleration limited desired speed float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed, float dt) const { // return input value if no recent calls to speed controller // apply no limiting when ATC_ACCEL_MAX is set to zero const uint32_t now = AP_HAL::millis(); if ((_speed_last_ms == 0) || ((now - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS) || !is_positive(_throttle_accel_max)) { return desired_speed; } // sanity check dt dt = constrain_float(dt, 0.0f, 1.0f); // use previous desired speed as basis for accel limiting float speed_prev = _desired_speed; // if no recent calls to speed controller limit based on current speed if (!speed_control_active()) { get_forward_speed(speed_prev); } // acceleration limit desired speed float speed_change_max; if (fabsf(desired_speed) < fabsf(_desired_speed) && is_positive(_throttle_decel_max)) { speed_change_max = _throttle_decel_max * dt; } else { speed_change_max = _throttle_accel_max * dt; } return constrain_float(desired_speed, speed_prev - speed_change_max, speed_prev + speed_change_max); }
// get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only float AR_AttitudeControl::get_desired_speed() const { // return zero if no recent calls to speed controller if (!speed_control_active()) { return 0.0f; } return _desired_speed; }
// get acceleration limited desired speed float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed, float dt) const { // sanity check dt dt = constrain_float(dt, 0.0f, 1.0f); // use previous desired speed as basis for accel limiting float speed_prev = _desired_speed; // if no recent calls to speed controller limit based on current speed if (!speed_control_active()) { get_forward_speed(speed_prev); } // acceleration limit desired speed float speed_change_max; if (fabsf(desired_speed) < fabsf(_desired_speed) && is_positive(_throttle_decel_max)) { speed_change_max = _throttle_decel_max * dt; } else { speed_change_max = _throttle_accel_max * dt; } return constrain_float(desired_speed, speed_prev - speed_change_max, speed_prev + speed_change_max); }
// return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards) // motor_limit should be true if motors have hit their upper or lower limits // cruise speed should be in m/s, cruise throttle should be a number from -1 to +1 float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt) { // sanity check dt dt = constrain_float(dt, 0.0f, 1.0f); // get speed forward float speed; if (!get_forward_speed(speed)) { // we expect caller will not try to control heading using rate control without a valid speed estimate // on failure to get speed we do not attempt to steer return 0.0f; } // if not called recently, reset input filter and desired speed to actual speed (used for accel limiting) const uint32_t now = AP_HAL::millis(); if (!speed_control_active()) { _throttle_speed_pid.reset_filter(); _desired_speed = speed; } _speed_last_ms = now; // acceleration limit desired speed _desired_speed = get_desired_speed_accel_limited(desired_speed, dt); // set PID's dt _throttle_speed_pid.set_dt(dt); // calculate speed error and pass to PID controller const float speed_error = desired_speed - speed; _throttle_speed_pid.set_input_filter_all(speed_error); // record desired speed for logging purposes only _throttle_speed_pid.set_desired_rate(desired_speed); // get feed-forward const float ff = _throttle_speed_pid.get_ff(desired_speed); // get p const float p = _throttle_speed_pid.get_p(); // get i unless moving at low speed or motors have hit a limit float i = _throttle_speed_pid.get_integrator(); if ((is_negative(speed_error) && !motor_limit_low && !_throttle_limit_low) || (is_positive(speed_error) && !motor_limit_high && !_throttle_limit_high)) { i = _throttle_speed_pid.get_i(); } // get d const float d = _throttle_speed_pid.get_d(); // calculate base throttle (protect against divide by zero) float throttle_base = 0.0f; if (is_positive(cruise_speed) && is_positive(cruise_throttle)) { throttle_base = desired_speed * (cruise_throttle / cruise_speed); } // calculate final output float throttle_out = (ff+p+i+d+throttle_base); // clear local limit flags used to stop i-term build-up as we stop reversed outputs going to motors _throttle_limit_low = false; _throttle_limit_high = false; // protect against reverse output being sent to the motors unless braking has been enabled if (!_brake_enable) { // if both desired speed and actual speed are positive, do not allow negative values if ((desired_speed >= 0.0f) && (throttle_out <= 0.0f)) { throttle_out = 0.0f; _throttle_limit_low = true; } if ((desired_speed <= 0.0f) && (throttle_out >= 0.0f)) { throttle_out = 0.0f; _throttle_limit_high = true; } } // final output throttle in range -1 to 1 return throttle_out; }