/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
/// called by update_z_controller if z-axis speed or accelerations are changed
void AC_PosControl::calc_leash_length_z()
{
if (_flags.recalc_leash_z) {
_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP());
_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP());
_flags.recalc_leash_z = false;
}
}
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
/// should be called whenever the speed, acceleration or position kP is modified
void AC_PosControl::calc_leash_length_xy()
{
if (_flags.recalc_leash_xy) {
_leash = calc_leash_length(_speed_cms, _accel_cms, _p_pos_xy.kP());
_flags.recalc_leash_xy = false;
}
}
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
/// should be called whenever the speed, acceleration or position kP is modified
void AC_PosControl::calc_leash_length_xy()
{
// todo: remove _flags.recalc_leash_xy or don't call this function after each variable change.
if (_flags.recalc_leash_xy) {
_leash = calc_leash_length(_speed_cms, _accel_cms, _p_pos_xy.kP());
_flags.recalc_leash_xy = false;
}
}
// update the controller
// dt: time dt
// user_rate: user desired climb rate, usually from stick.
int altitude_controller::update(float dt, float user_rate)
{
// sonar switching
if (m_sonar_ticker < 0.5f)
{
m_sonar_ticker += dt;
if (m_sonar_ticker > 0.5f)
{
// sonar state changed more than 0.5 second.
if (isnan(m_sonar))
{
m_sonar_target = NAN;
}
else
{
m_sonar_target = m_sonar + target_altitude - m_states[0];
}
LOGE("sonar changed: %f/%f,%f,%f\n", m_sonar_target, m_sonar, target_altitude, m_states[0]);
}
}
if (!isnan(target_altitude))
{
float leash_up = calc_leash_length(quadcopter_max_climb_rate, quadcopter_max_acceleration, pid_quad_altitude[0]);
float leash_down = calc_leash_length(quadcopter_max_descend_rate, quadcopter_max_acceleration, pid_quad_altitude[0]);
// only move altitude target if throttle and target climb rate didn't hit limits
if ((!(m_motor_state & MOTOR_LIMIT_MAX) && user_rate > 0 && (target_climb_rate < quadcopter_max_climb_rate)) ||
(!(m_motor_state & MOTOR_LIMIT_MIN) && user_rate < 0 && (target_climb_rate > -quadcopter_max_descend_rate))
)
{
target_altitude += user_rate * dt;
}
target_altitude = limit(target_altitude, m_states[0]-leash_down, m_states[0]+leash_up);
// new target rate, directly use linear approach since we use very tight limit
// TODO: use sqrt approach on large errors (see get_throttle_althold() in Attitude.pde)
altitude_error_pid[0] = target_altitude - m_states[0];
altitude_error_pid[0] = limit(altitude_error_pid[0], -2.5f, 2.5f);
target_climb_rate = pid_quad_altitude[0] * altitude_error_pid[0];
}
else
{
target_climb_rate = 0;
}
// feed forward
//
feed_forward_factor += m_airborne ? -dt : dt;
feed_forward_factor = limit(feed_forward_factor, 0.35f, 0.8f);
target_climb_rate += user_rate * feed_forward_factor;
TRACE("\rtarget_climb_rate=%.2f climb from alt =%.2f, user=%.2f, out=%2f. ", target_climb_rate, target_climb_rate-user_rate * feed_forward_factor, user_rate, throttle_result);
// new climb rate error
float climb_rate_error = target_climb_rate - m_states[1];
climb_rate_error = limit(climb_rate_error, -quadcopter_max_descend_rate, quadcopter_max_climb_rate);
// apply a 2Hz LPF to rate error
const float RC = 1.0f/(2*3.1415926 * 5.0f);
float alpha = dt / (dt + RC);
// 5Hz LPF filter
const float RC5 = 1.0f/(2*3.1415926 * 2.0f);
float alpha5 = dt / (dt + RC5);
// 30Hz LPF for derivative factor
const float RC30 = 1.0f/(2*3.1415926 * 30.0f);
float alpha30 = dt / (dt + RC30);
// TODO: add feed forward
// reference: get_throttle_rate()
climb_rate_error_pid[0] = isnan(climb_rate_error_pid[0]) ? (climb_rate_error) : (climb_rate_error_pid[0] * (1-alpha) + alpha * climb_rate_error);
target_accel = climb_rate_error_pid[0] * pid_quad_alt_rate[0];
target_accel = limit(target_accel, m_airborne ? -quadcopter_max_acceleration : -2 * quadcopter_max_acceleration, quadcopter_max_acceleration);
// new accel error, +2Hz LPF
float accel_error = target_accel - m_states[2];
if (isnan(accel_error_pid[0]))
{
accel_error_pid[0] = accel_error;
}
else
{
accel_error = accel_error_pid[0] * (1-alpha) + alpha * accel_error;
}
// core pid
// only integrate if throttle didn't hit limits or I term will reduce
if (m_airborne)
if ((!(m_motor_state & MOTOR_LIMIT_MAX) && accel_error_pid[0] > 0) || (!(m_motor_state & MOTOR_LIMIT_MIN) && accel_error_pid[0] < 0))
{
accel_error_pid[1] += accel_error_pid[0] * dt;
accel_error_pid[1] = limit(accel_error_pid[1], -pid_quad_accel[3], pid_quad_accel[3]);
}
float D = (accel_error - accel_error_pid[0])/dt;
accel_error_pid[2] = isnan(accel_error_pid[2])? D : (accel_error_pid[2] * (1-alpha30) + alpha30 * D);
accel_error_pid[0] = accel_error;
float output = 0;
output += accel_error_pid[0] * pid_quad_accel[0];
output += accel_error_pid[1] * pid_quad_accel[1];
output += accel_error_pid[2] * pid_quad_accel[2];
output *= throttle_hover / default_throttle_hover; // normalize throttle output PID, from throttle percentage to acceleration
throttle_result = output + throttle_hover;
float angle_boost_factor = limit(1/ cos(m_attitude[0]) / cos(m_attitude[1]), 1.0f, 1.5f);
throttle_result = throttle_result * angle_boost_factor;
if (throttle_result > 1 - QUADCOPTER_THROTTLE_RESERVE)
{
throttle_result = 1 - QUADCOPTER_THROTTLE_RESERVE;
m_motor_state = MOTOR_LIMIT_MAX;
}
else if (throttle_result < (m_airborne ? QUADCOPTER_THROTTLE_RESERVE : 0))
{
throttle_result = m_airborne ? QUADCOPTER_THROTTLE_RESERVE : 0;
m_motor_state = MOTOR_LIMIT_MIN;
}
else
{
m_motor_state = 0;
}
LOGE("\rthrottle=%f, altitude = %.2f/%.2f, pid=%.2f,%.2f,%.2f, limit=%d", throttle_result, m_states[0], target_altitude,
accel_error_pid[0], accel_error_pid[1], accel_error_pid[2], m_motor_state);
// update throttle_real_crusing if we're in near level state and no violent climbing/descending action
if (m_airborne && m_throttle_realized>0 && fabs(m_states[1]) < 0.5f && fabs(m_states[2])<0.5f && fabs(m_attitude[0])<5*PI/180 && fabs(m_attitude[1])<5*PI/180
&& fabs(user_rate) < 0.001f)
{
// 0.2Hz low pass filter
const float RC02 = 1.0f/(2*3.1415926 * 0.2f);
float alpha02 = dt / (dt + RC02);
throttle_hover = throttle_hover * (1-alpha02) + alpha02 * m_throttle_realized;
// TODO: estimate throttle cursing correctly
TRACE("\rthrottle_hover=%f", throttle_hover);
}
return 0;
}
// update the controller
// dt: time dt
// user_rate: user desired climb rate, usually from stick.
int altitude_controller::update(float dt, float user_rate)
{
if (systimer->gettime() - last_update > 1000000)
reset();
if (throttle_hover <= 0.01f || isnan(throttle_hover))
throttle_hover = default_throttle_hover;
// sonar switching
if (isnan(m_sonar) == isnan(m_sonar_target)) // reset ticker if sonar state didn't changed
m_sonar_ticker = 0;
if (m_sonar_ticker < 0.3f)
{
m_sonar_ticker += dt;
if (m_sonar_ticker > 0.3f)
{
// sonar state changed more than 0.5 second.
if (isnan(m_sonar))
{
baro_target = m_baro_states[0] + m_sonar_target - m_last_valid_sonar;
m_sonar_target = NAN;
LOGE("changed to baro: %f/%f,%f,%f\n", m_sonar_target, m_sonar, baro_target, m_baro_states[0]);
}
else
{
m_sonar_target = m_sonar + baro_target - m_baro_states[0];
LOGE("changed to sonar: %f/%f,%f,%f\n", m_sonar_target, m_sonar, baro_target, m_baro_states[0]);
}
}
}
float leash_up = calc_leash_length(quadcopter_max_climb_rate, quadcopter_max_acceleration, pid_quad_altitude[0]);
float leash_down = calc_leash_length(quadcopter_max_descend_rate, quadcopter_max_acceleration, pid_quad_altitude[0]);
float &alt_target = isnan(m_sonar_target) ? baro_target : m_sonar_target;
float &alt_state = isnan(m_sonar_target) ? m_baro_states[0]: m_last_valid_sonar;
// only move altitude target if throttle and target climb rate didn't hit limits, and no climb target override exists
if (isnan(climb_rate_override) &&
(
(!(m_motor_state & LIMIT_POSITIVE_HARD) && user_rate > 0 && (target_climb_rate < quadcopter_max_climb_rate)) ||
(!(m_motor_state & LIMIT_NEGATIVE_HARD) && user_rate < 0 && (target_climb_rate > -quadcopter_max_descend_rate))
)
)
{
alt_target += user_rate * dt;
}
alt_target = limit(alt_target, alt_state-leash_down, alt_state+leash_up);
// new target rate, directly use linear approach since we use very tight limit
// TODO: use sqrt approach on large errors (see get_throttle_althold() in Attitude.pde)
altitude_error_pid[0] = alt_target - alt_state;
altitude_error_pid[0] = limit(altitude_error_pid[0], -leash_down, leash_up);
target_climb_rate = pid_quad_altitude[0] * altitude_error_pid[0];
if (!isnan(climb_rate_override))
target_climb_rate = climb_rate_override;
// feed forward
//
feed_forward_factor += m_airborne ? -dt : dt;
feed_forward_factor = limit(feed_forward_factor, 0.35f, 0.8f);
target_climb_rate += user_rate * feed_forward_factor;
TRACE("\rtarget_climb_rate=%.2f climb from alt =%.2f, user=%.2f, out=%2f. ", target_climb_rate, target_climb_rate-user_rate * feed_forward_factor, user_rate, throttle_result);
// a better outter loop for centered throttle stick remote controller.
if (int(alt_braking_enabled) && m_airborne)
{
bool near_ground = !isnan(m_sonar_target) && m_last_valid_sonar < 0.15f;
static bool last_near_ground;
if (!last_near_ground && near_ground)
m_sonar_target = m_last_valid_sonar + m_baro_states[1] / 2;
last_near_ground = near_ground;
if (!near_ground)
{
bool stick_centered = fabs(user_rate) < 0.1f;
if (!stick_centered)
{
m_controller_state = alt_override;
m_braking_timer = 0;
target_climb_rate = user_rate;
}
else
{
if (m_controller_state == alt_override)
{
m_controller_state = alt_braking;
m_braking_timer = 0;
target_climb_rate = 0;
}
else if (m_controller_state == alt_braking)
{
if (fabs(m_baro_states[1]) < 0.5f)
m_braking_timer += dt;
else
m_braking_timer = 0;
if (m_braking_timer > 0.5f)
{
float &target = isnan(m_sonar_target) ? baro_target : m_sonar_target;
target = (isnan(m_sonar_target) ? m_baro_states[0] : m_last_valid_sonar) + m_baro_states[1] /2;
LOGE("alt braking done, target set to %.2f(%s)", target, isnan(m_sonar_target) ? "baro" : "sonar");
m_controller_state = alt_hold;
}
target_climb_rate = 0;
}
else if (m_controller_state == alt_hold)
{
// do nothing
}
}
}
}
target_climb_rate = limit(target_climb_rate, -quadcopter_max_descend_rate, quadcopter_max_climb_rate);
// new climb rate error
float climb_rate_error = target_climb_rate - m_baro_states[1];
//climb_rate_error = limit(climb_rate_error, -quadcopter_max_descend_rate, quadcopter_max_climb_rate);
// apply a 2Hz LPF to rate error
const float RC2 = 1.0f/(2*3.1415926 * 3.5f);
float alpha2 = dt / (dt + RC2);
// 5Hz LPF filter
const float RC5 = 1.0f/(2*3.1415926 * 5.0f);
float alpha5 = dt / (dt + RC5);
// 30Hz LPF for derivative factor
const float RC30 = 1.0f/(2*3.1415926 * 30.0f);
float alpha30 = dt / (dt + RC30);
// TODO: add feed forward
// reference: get_throttle_rate()
climb_rate_error_pid[0] = isnan(climb_rate_error_pid[0]) ? (climb_rate_error) : (climb_rate_error_pid[0] * (1-alpha2) + alpha2 * climb_rate_error);
target_accel = climb_rate_error_pid[0] * pid_quad_alt_rate[0];
target_accel = limit(target_accel, m_airborne ? -quadcopter_max_acceleration : -2 * quadcopter_max_acceleration, quadcopter_max_acceleration);
// new accel error, +2Hz LPF
float accel_error = target_accel - m_baro_states[2];
if (isnan(accel_error_pid[0]))
{
accel_error_pid[0] = accel_error;
}
else
{
accel_error = accel_error_pid[0] * (1-alpha2) + alpha2 * accel_error;
}
// core pid
// only integrate if throttle didn't hit limits or I term will reduce
if (m_airborne)
if ((!(m_motor_state & LIMIT_POSITIVE_HARD) && accel_error_pid[0] > 0) || (!(m_motor_state & LIMIT_NEGATIVE_HARD) && accel_error_pid[0] < 0))
{
accel_error_pid[1] += accel_error_pid[0] * dt;
accel_error_pid[1] = limit(accel_error_pid[1], -pid_quad_accel[3], pid_quad_accel[3]);
}
float D = (accel_error - accel_error_pid[0])/dt;
accel_error_pid[2] = isnan(accel_error_pid[2])? D : (accel_error_pid[2] * (1-alpha30) + alpha30 * D);
accel_error_pid[0] = accel_error;
float output = 0;
output += accel_error_pid[0] * pid_quad_accel[0];
output += accel_error_pid[1] * pid_quad_accel[1];
output += accel_error_pid[2] * pid_quad_accel[2];
output *= throttle_hover / default_throttle_hover; // normalize throttle output PID, from throttle percentage to acceleration
throttle_result = output + throttle_hover;
float angle_boost_factor = 1/ cos(m_attitude[0]) / cos(m_attitude[1]);
if (angle_boost_factor < 0)
angle_boost_factor = 0; // if we got reverted, kill thrust
else if (angle_boost_factor > 1.5f)
angle_boost_factor = 1.5f;
else if (angle_boost_factor < 1.0f)
angle_boost_factor = 1.0f;
throttle_result = throttle_result * angle_boost_factor;
if (throttle_result > 1 - QUADCOPTER_THROTTLE_RESERVE)
{
m_motor_state = LIMIT_POSITIVE_HARD;
}
else if (throttle_result < 0)
{
m_motor_state = LIMIT_NEGATIVE_HARD;
}
else
{
m_motor_state = 0;
}
throttle_result = limit(throttle_result, 0.0f, 1.0f);
TRACE("\rthrottle=%f, altitude = %.2f/%.2f, pid=%.2f,%.2f,%.2f, limit=%d", throttle_result, m_baro_states[0], baro_target,
accel_error_pid[0], accel_error_pid[1], accel_error_pid[2], m_motor_state);
// update throttle_real_crusing if we're in near level state and no violent climbing/descending action
if (m_airborne && m_throttle_realized>0 && fabs(m_baro_states[1]) < 0.5f && fabs(m_baro_states[2])<0.5f && fabs(m_attitude[0])<5*PI/180 && fabs(m_attitude[1])<5*PI/180
&& fabs(user_rate) < 0.001f)
{
// 0.2Hz low pass filter
const float RC02 = 1.0f/(2*3.1415926 * 0.02f);
float alpha02 = dt / (dt + RC02);
throttle_hover = throttle_hover * (1-alpha02) + alpha02 * m_throttle_realized;
// TODO: estimate throttle cursing correctly
TRACE("\rthrottle_hover=%f", throttle_hover);
}
last_update = systimer->gettime();
return 0;
}