// return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s.
// positive lateral acceleration is to the right.
float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right, float dt)
{
// record desired accel for reporting purposes
_steer_lat_accel_last_ms = AP_HAL::millis();
_desired_lat_accel = desired_accel;
// 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;
}
// enforce minimum speed to stop oscillations when first starting to move
if (fabsf(speed) < AR_ATTCONTROL_STEER_SPEED_MIN) {
if (is_negative(speed)) {
speed = -AR_ATTCONTROL_STEER_SPEED_MIN;
} else {
speed = AR_ATTCONTROL_STEER_SPEED_MIN;
}
}
// Calculate the desired steering rate given desired_accel and speed
const float desired_rate = desired_accel / speed;
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
}
// 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);
}
// return a throttle output from -1 to +1 to perform a controlled stop. returns true once the vehicle has stopped
float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, bool &stopped)
{
// get current system time
const uint32_t now = AP_HAL::millis();
// if we were stopped in the last 300ms, assume we are still stopped
bool _stopped = (_stop_last_ms != 0) && (now - _stop_last_ms) < 300;
// get speed forward
float speed;
if (!get_forward_speed(speed)) {
// could not get speed so assume stopped
_stopped = true;
} else {
// if vehicle drops below _stop_speed consider it stopped
if (fabsf(speed) <= fabsf(_stop_speed)) {
_stopped = true;
}
}
// set stopped status for caller
stopped = _stopped;
// if stopped return zero
if (stopped) {
// update last time we thought we were stopped
_stop_last_ms = now;
return 0.0f;
} else {
// clear stopped system time
_stop_last_ms = 0;
// run speed controller to bring vehicle to stop
return get_throttle_out_speed(0.0f, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle);
}
}
// return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s.
// positive lateral acceleration is to the right.
float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
{
// record desired accel for reporting purposes
_steer_lat_accel_last_ms = AP_HAL::millis();
_desired_lat_accel = desired_accel;
// 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;
}
// only use positive speed. Use reverse flag instead of negative speeds.
speed = fabsf(speed);
// enforce minimum speed to stop oscillations when first starting to move
if (speed < AR_ATTCONTROL_STEER_SPEED_MIN) {
speed = AR_ATTCONTROL_STEER_SPEED_MIN;
}
// Calculate the desired steering rate given desired_accel and speed
float desired_rate = desired_accel / speed;
// invert rate if we are going backwards
if (reversed) {
desired_rate *= -1.0f;
}
return get_steering_out_rate(desired_rate, skid_steering, motor_limit_left, motor_limit_right, reversed);
}
// get actual lateral acceleration in m/s/s. returns true on success
bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const
{
float speed;
if (!get_forward_speed(speed)) {
return false;
}
lat_accel = speed * _ahrs.get_yaw_rate_earth();
return true;
}
// 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;
}
// return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right.
float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
{
// record desired turn rate for reporting purposes
_desired_turn_rate = desired_rate;
// calculate dt
const uint32_t now = AP_HAL::millis();
float dt = (now - _steer_turn_last_ms) / 1000.0f;
if ((_steer_turn_last_ms == 0) || (dt > AR_ATTCONTROL_TIMEOUT_MS)) {
dt = 0.0f;
_steer_rate_pid.reset_filter();
} else {
_steer_rate_pid.set_dt(dt);
}
_steer_turn_last_ms = now;
// 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;
}
// only use positive speed. Use reverse flag instead of negative speeds.
speed = fabsf(speed);
// enforce minimum speed to stop oscillations when first starting to move
bool low_speed = false;
if (speed < AR_ATTCONTROL_STEER_SPEED_MIN) {
low_speed = true;
speed = AR_ATTCONTROL_STEER_SPEED_MIN;
}
// scaler to linearize output because turn rate increases as vehicle speed increases on non-skid steering vehicles
float scaler = 1.0f;
if (!skid_steering) {
scaler = 1.0f / fabsf(speed);
}
// Calculate the steering rate error (rad/sec) and apply gain scaler
// We do this in earth frame to allow for rover leaning over in hard corners
float yaw_rate_earth = _ahrs.get_yaw_rate_earth();
// check if reversing
if (reversed) {
yaw_rate_earth *= -1.0f;
}
const float rate_error = (desired_rate - yaw_rate_earth) * scaler;
// record desired rate for logging purposes only
_steer_rate_pid.set_desired_rate(desired_rate);
// pass error to PID controller
_steer_rate_pid.set_input_filter_all(rate_error);
// get p
const float p = _steer_rate_pid.get_p();
// get i unless non-skid-steering rover at low speed or steering output has hit a limit
float i = _steer_rate_pid.get_integrator();
if ((!low_speed || skid_steering) && ((is_negative(rate_error) && !motor_limit_left) || (is_positive(rate_error) && !motor_limit_right))) {
i = _steer_rate_pid.get_i();
}
// get d
const float d = _steer_rate_pid.get_d();
// constrain and return final output
return constrain_float(p + i + d, -1.0f, 1.0f);
}
