Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: enhanced quaternion attitude controller

This commit is contained in:
Leonard Hall 2016-06-24 16:20:21 +09:00 committed by Randy Mackay
parent 117ae89505
commit 8737f6b62d
5 changed files with 544 additions and 484 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

754
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -97,309 +97,431 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
AP_GROUPEND
};
void AC_AttitudeControl::relax_bf_rate_controller()
// Set output throttle and disable stabilization
void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
{
_throttle_in = throttle_in;
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (reset_attitude_control) {
relax_attitude_controllers();
}
_motors.set_throttle(throttle_in);
_angle_boost = 0.0f;
}
// Ensure attitude controller have zero errors to relax rate controller output
void AC_AttitudeControl::relax_attitude_controllers()
{
// TODO add _ahrs.get_quaternion()
_attitude_target_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
_attitude_target_ang_vel = _ahrs.get_gyro();
_attitude_target_euler_angle = Vector3f(_ahrs.roll, _ahrs.pitch, _ahrs.yaw);
// Set reference angular velocity used in angular velocity controller equal
// to the input angular velocity and reset the angular velocity integrators.
// This zeros the output of the angular velocity controller.
_ang_vel_target_rads = _ahrs.get_gyro();
_rate_target_ang_vel = _ahrs.get_gyro();
get_rate_roll_pid().reset_I();
get_rate_pitch_pid().reset_I();
get_rate_yaw_pid().reset_I();
// Write euler derivatives derived from vehicle angular velocity to
// _att_target_euler_rate_rads. This resets the state of the input shapers.
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
// The attitude controller works around the concept of the desired attitude, target attitude
// and measured attitude. The desired attitude is the attitude input into the attitude controller
// that expresses where the higher level code would like the aircraft to move to. The target attitude is moved
// to the desired attitude with jerk, acceleration, and velocity limits. The target angular velocities are fed
// directly into the rate controllers. The angular error between the measured attitude and the target attitude is
// fed into the angle controller and the output of the angle controller summed at the input of the rate controllers.
// By feeding the target angular velocity directly into the rate controllers the measured and target attitudes
// remain very close together.
//
// All input functions below follow the same procedure
// 1. define the desired attitude the aircraft should attempt to achieve using the input variables
// 2. using the desired attitude and input variables, define the target angular velocity so that it should
// move the target attitude towards the desired attitude
// 3. if _rate_bf_ff_enabled & _use_ff_and_input_shaping are not being used then make the target attitude
// and target angular velocities equal to the desired attitude and desired angular velocities.
// 4. ensure _attitude_target_quat, _attitude_target_euler_angle, _attitude_target_euler_rate and
// _attitude_target_ang_vel have been defined. This ensures input modes can be changed without discontinuity.
// 5. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and
// integrate them into the target attitude. Any errors between the target attitude and the measured attitude are
// corrected by first correcting the thrust vector until the angle between the target thrust vector measured
// trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
// Command a Quaternion attitude with feedforward and smoothing
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain)
{
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + radians(yaw_shift_cd*0.01f));
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_ang_vel.x = input_shaping_angle(attitude_error_angle.x, smoothing_gain, get_accel_roll_max_radss(), _attitude_target_ang_vel.x);
_attitude_target_ang_vel.y = input_shaping_angle(attitude_error_angle.y, smoothing_gain, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y);
_attitude_target_ang_vel.z = input_shaping_angle(attitude_error_angle.z, smoothing_gain, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
_attitude_target_quat = attitude_desired_quat;
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_smooth(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
// Sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
euler_roll_angle += get_roll_trim_rad();
if ((get_accel_roll_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
// When roll acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler roll-axis
// angular velocity that will cause the euler roll angle to smoothly stop at the input angle with limited deceleration
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
float euler_rate_desired_rads = sqrt_controller(euler_roll_angle_rad-_att_target_euler_rad.x, smoothing_gain, get_accel_roll_max_radss());
_attitude_target_euler_rate.x = input_shaping_angle(euler_roll_angle-_attitude_target_euler_angle.x, smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x);
_attitude_target_euler_rate.y = input_shaping_angle(euler_pitch_angle-_attitude_target_euler_angle.y, smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y);
// Acceleration is limited directly to smooth the beginning of the curve.
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_euler_rate_rads.x = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.x-rate_change_limit_rads, _att_target_euler_rate_rads.x+rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_roll(_att_target_euler_rate_rads.x, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
// When acceleration limiting and feedforward are not enabled, the target roll euler angle is simply set to the
// input value and the feedforward rate is zeroed.
_att_target_euler_rad.x = euler_roll_angle_rad;
_att_target_euler_rate_rads.x = 0;
}
_att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
if ((get_accel_pitch_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
// When pitch acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler pitch-axis
// angular velocity that will cause the euler pitch angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
float euler_rate_desired_rads = sqrt_controller(euler_pitch_angle_rad-_att_target_euler_rad.y, smoothing_gain, get_accel_pitch_max_radss());
// Acceleration is limited directly to smooth the beginning of the curve.
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_euler_rate_rads.y = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.y-rate_change_limit_rads, _att_target_euler_rate_rads.y+rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
_att_target_euler_rad.y = euler_pitch_angle_rad;
_att_target_euler_rate_rads.y = 0;
}
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
if (get_accel_yaw_max_radss() > 0.0f) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
// When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
// is fed forward into the rate controller.
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
_attitude_target_euler_angle.z += euler_yaw_rate*_dt;
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
if (_rate_bf_ff_enabled) {
euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
} else {
euler_rate_to_ang_vel(_att_target_euler_rad, Vector3f(0,0,_att_target_euler_rate_rads.z), _att_target_ang_vel_rads);
}
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle = radians(euler_yaw_angle_cd*0.01f);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
euler_roll_angle += get_roll_trim_rad();
// Set roll/pitch attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// Zero the roll and pitch feed-forward rate.
_att_target_euler_rate_rads.x = 0;
_att_target_euler_rate_rads.y = 0;
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_euler_rate.x = input_shaping_angle(euler_roll_angle-_attitude_target_euler_angle.x, smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x);
_attitude_target_euler_rate.y = input_shaping_angle(euler_pitch_angle-_attitude_target_euler_angle.y, smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y);
_attitude_target_euler_rate.z = input_shaping_angle(euler_yaw_angle-_attitude_target_euler_angle.z, smoothing_gain, euler_accel.z, _attitude_target_euler_rate.z);
if (slew_yaw) {
_attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
}
if (get_accel_yaw_max_radss() > 0.0f) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
// When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
// is fed forward into the rate controller.
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), -get_slew_yaw_rads()*_dt, get_slew_yaw_rads()*_dt);
// Update attitude target from constrained angle error
_attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
} else {
_attitude_target_euler_angle.z = euler_yaw_angle;
}
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Convert euler angle derivatives of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle_rad = radians(euler_yaw_angle_cd*0.01f);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
// Set attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.z = euler_yaw_angle_rad;
// If slew_yaw is enabled, constrain yaw target within get_slew_yaw_rads() of _ahrs.yaw
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.z - _ahrs.yaw), -get_slew_yaw_rads(), get_slew_yaw_rads());
// Update attitude target from constrained angle error
_att_target_euler_rad.z = angle_error + _ahrs.yaw;
}
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, Vector3f(0.0f,0.0f,0.0f));
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_rate_rads = radians(euler_roll_rate_cds*0.01f);
float euler_pitch_rate_rads = radians(euler_pitch_rate_cds*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
float euler_roll_rate = radians(euler_roll_rate_cds*0.01f);
float euler_pitch_rate = radians(euler_pitch_rate_cds*0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
// Compute acceleration-limited euler roll rate
if (get_accel_roll_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_euler_rate_rads.x += constrain_float(euler_roll_rate_rads - _att_target_euler_rate_rads.x, -rate_change_limit_rads, rate_change_limit_rads);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing
// the output rate towards the input rate.
_attitude_target_euler_rate.x = input_shaping_ang_vel(_attitude_target_euler_rate.x, euler_roll_rate, euler_accel.x);
_attitude_target_euler_rate.y = input_shaping_ang_vel(_attitude_target_euler_rate.y, euler_pitch_rate, euler_accel.y);
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
_att_target_euler_rate_rads.x = euler_roll_rate_rads;
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
// Pitch angle is restricted to +- 85.0 degrees to avoid gimbal lock discontinuities.
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + euler_roll_rate*_dt);
_attitude_target_euler_angle.y = constrain_float(_attitude_target_euler_angle.y + euler_pitch_rate*_dt, radians(-85.0f), radians(85.0f));
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + euler_yaw_rate*_dt);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
}
// Compute acceleration-limited euler pitch rate
if (get_accel_pitch_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_euler_rate_rads.y += constrain_float(euler_pitch_rate_rads - _att_target_euler_rate_rads.y, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_euler_rate_rads.y = euler_pitch_rate_rads;
}
// Compute acceleration-limited euler yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
}
// Update the attitude target from the computed euler rates
update_att_target_roll(_att_target_euler_rate_rads.x, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
update_att_target_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD);
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// Apply tilt limit
_att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
// Convert euler angle derivatives of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an angular velocity with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_bf_rads = radians(roll_rate_bf_cds*0.01f);
float pitch_rate_bf_rads = radians(pitch_rate_bf_cds*0.01f);
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
float roll_rate_rads = radians(roll_rate_bf_cds*0.01f);
float pitch_rate_rads = radians(pitch_rate_bf_cds*0.01f);
float yaw_rate_rads = radians(yaw_rate_bf_cds*0.01f);
// Compute acceleration-limited body-frame roll rate
if (get_accel_roll_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_ang_vel_rads.x += constrain_float(roll_rate_bf_rads - _att_target_ang_vel_rads.x, -rate_change_limit_rads, rate_change_limit_rads);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// Compute acceleration-limited euler rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
_attitude_target_ang_vel.x = input_shaping_ang_vel(_attitude_target_ang_vel.x, roll_rate_rads, get_accel_roll_max_radss());
_attitude_target_ang_vel.y = input_shaping_ang_vel(_attitude_target_ang_vel.y, pitch_rate_rads, get_accel_pitch_max_radss());
_attitude_target_ang_vel.z = input_shaping_ang_vel(_attitude_target_ang_vel.z, yaw_rate_rads, get_accel_yaw_max_radss());
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
_att_target_ang_vel_rads.x = roll_rate_bf_rads;
// When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed.
Quaternion attitude_target_update_quat;
attitude_target_update_quat.from_axis_angle(Vector3f(roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt));
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Compute acceleration-limited body-frame pitch rate
if (get_accel_pitch_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_ang_vel_rads.y += constrain_float(pitch_rate_bf_rads - _att_target_ang_vel_rads.y, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_ang_vel_rads.y = pitch_rate_bf_rads;
}
// Compute acceleration-limited body-frame yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_ang_vel_rads.z += constrain_float(yaw_rate_bf_rads - _att_target_ang_vel_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_ang_vel_rads.z = yaw_rate_bf_rads;
}
// Compute quaternion target attitude
Quaternion att_target_quat;
att_target_quat.from_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
// Rotate quaternion target attitude using computed rate
att_target_quat.rotate(_att_target_ang_vel_rads*_dt);
att_target_quat.normalize();
// Call attitude controller
attitude_controller_run_quat(att_target_quat, _att_target_ang_vel_rads);
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
void AC_AttitudeControl::input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads)
{
// Call attitude controller
attitude_controller_run_quat(att_target_quat, att_target_ang_vel_rads);
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
void AC_AttitudeControl::attitude_controller_run_euler(const Vector3f& att_target_euler_rad, const Vector3f& att_target_ang_vel_rads)
{
// Compute quaternion target attitude
Quaternion att_target_quat;
att_target_quat.from_euler(att_target_euler_rad.x, att_target_euler_rad.y, att_target_euler_rad.z);
// Call quaternion attitude controller
attitude_controller_run_quat(att_target_quat, att_target_ang_vel_rads);
attitude_controller_run_quat();
}
void AC_AttitudeControl::attitude_controller_run_quat(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads)
// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat()
{
// Update euler attitude target and angular velocity target
att_target_quat.to_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
_att_target_ang_vel_rads = att_target_ang_vel_rads;
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()
Quaternion att_vehicle_quat;
att_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
Quaternion attitude_vehicle_quat;
attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
// Compute attitude error
(att_vehicle_quat.inverse()*att_target_quat).to_axis_angle(_att_error_rot_vec_rad);
Vector3f attitude_error_vector;
thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
_rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z;
_rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z;
// Add the angular velocity feedforward, rotated into vehicle frame
Matrix3f Trv;
get_rotation_reference_to_vehicle(Trv);
_ang_vel_target_rads += Trv * _att_target_ang_vel_rads;
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
Quaternion attitude_error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Quaternion target_ang_vel_quat = attitude_error_quat.inverse()*attitude_target_ang_vel_quat*attitude_error_quat;
// Correct the thrust vector and smoothly add feedforward and yaw input
if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
_rate_target_ang_vel.z = _ahrs.get_gyro().z;
}else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
float flip_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
_rate_target_ang_vel.x += target_ang_vel_quat.q2*flip_scalar;
_rate_target_ang_vel.y += target_ang_vel_quat.q3*flip_scalar;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
_rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-flip_scalar) + _rate_target_ang_vel.z*flip_scalar;
} else {
_rate_target_ang_vel.x += target_ang_vel_quat.q2;
_rate_target_ang_vel.y += target_ang_vel_quat.q3;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
}
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// rotate target and normalize
Quaternion attitude_target_update_quat;
attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
}
}
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
{
Matrix3f att_to_rot_matrix; // earth frame to target frame
att_to_quat.rotation_matrix(att_to_rot_matrix);
Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
Matrix3f att_from_rot_matrix; // earth frame to target frame
att_from_quat.rotation_matrix(att_from_rot_matrix);
Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
// the cross product of the desired and target thrust vector defines the rotation vector
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;
// the dot product is used to calculate the angle between the target and desired thrust vectors
thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec,-1.0f,1.0f));
// Normalize the thrust rotation vector
float thrust_vector_length = thrust_vec_cross.length();
if(is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)){
thrust_vec_cross = Vector3f(0,0,1);
thrust_vec_dot = 0.0f;
}else{
thrust_vec_cross /= thrust_vector_length;
}
Quaternion thrust_vec_correction_quat;
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);
thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;
// calculate the remaining rotation required after thrust vector is rotated
Quaternion heading_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;
Vector3f rotation;
thrust_vec_correction_quat.to_axis_angle(rotation);
att_diff_angle.x = rotation.x;
att_diff_angle.y = rotation.y;
heading_quat.to_axis_angle(rotation);
att_diff_angle.z = rotation.z;
if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
heading_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
att_to_quat = att_from_quat*thrust_vec_correction_quat*heading_quat;
}
}
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain
float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel)
{
error_angle = wrap_PI(error_angle);
// Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss
float ang_vel = sqrt_controller(error_angle, smoothing_gain, accel_max);
// Acceleration is limited directly to smooth the beginning of the curve.
float delta_ang_vel = accel_max * _dt;
return constrain_float(ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel);
}
// limits the acceleration and deceleration of a velocity request
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max)
{
if (accel_max > 0.0f) {
float delta_ang_vel = accel_max * _dt;
target_ang_vel += constrain_float(desired_ang_vel - target_ang_vel, -delta_ang_vel, delta_ang_vel);
} else {
target_ang_vel = desired_ang_vel;
}
return target_ang_vel;
}
// translates body frame acceleration limits to the euler axis
Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel)
{
float sin_phi = constrain_float(fabsf(sinf(euler_rad.x)), 0.1f, 1.0f);
float cos_phi = constrain_float(fabsf(cosf(euler_rad.x)), 0.1f, 1.0f);
float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);
Vector3f rot_accel;
if(is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || (euler_accel.x < 0.0f) || (euler_accel.y < 0.0f) || (euler_accel.z < 0.0f)) {
rot_accel.x = euler_accel.x;
rot_accel.y = euler_accel.y;
rot_accel.z = euler_accel.z;
} else {
rot_accel.x = euler_accel.x;
rot_accel.y = MIN(euler_accel.y/cos_phi, euler_accel.z/sin_phi);
rot_accel.z = MIN(MIN(euler_accel.x/sin_theta, euler_accel.y/sin_phi), euler_accel.z/cos_phi);
}
return rot_accel;
}
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{
float yaw_shift = radians(yaw_shift_cd*0.01f);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + yaw_shift);
Quaternion _attitude_target_update_quat;
_attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, _attitude_target_euler_angle.z));
_attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
}
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)
{
float sin_theta = sinf(euler_rad.y);
@ -412,6 +534,8 @@ void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const
ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z;
}
// Convert an angular velocity vector to a 321-intrinsic euler angle derivative
// Returns false if the vehicle is pitched 90 degrees up or down
bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads)
{
float sin_theta = sinf(euler_rad.y);
@ -430,87 +554,35 @@ bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const
return true;
}
void AC_AttitudeControl::update_att_target_roll(float euler_roll_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.x - _ahrs.roll), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.x = angle_error + _ahrs.roll;
// Increment the attitude target
_att_target_euler_rad.x += euler_roll_rate_rads * _dt;
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x);
}
void AC_AttitudeControl::update_att_target_pitch(float euler_pitch_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.y - _ahrs.pitch), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.y = angle_error + _ahrs.pitch;
// Increment the attitude target
_att_target_euler_rad.y += euler_pitch_rate_rads * _dt;
_att_target_euler_rad.y = wrap_PI(_att_target_euler_rad.y);
}
void AC_AttitudeControl::update_att_target_yaw(float euler_yaw_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.z - _ahrs.yaw), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.z = angle_error + _ahrs.yaw;
// Increment the attitude target
_att_target_euler_rad.z += euler_yaw_rate_rads * _dt;
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z);
}
void AC_AttitudeControl::integrate_bf_rate_error_to_angle_errors()
{
// Integrate the angular velocity error into the attitude error
_att_error_rot_vec_rad += (_att_target_ang_vel_rads - _ahrs.get_gyro()) * _dt;
// Constrain attitude error
_att_error_rot_vec_rad.x = constrain_float(_att_error_rot_vec_rad.x, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
_att_error_rot_vec_rad.y = constrain_float(_att_error_rot_vec_rad.y, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
_att_error_rot_vec_rad.z = constrain_float(_att_error_rot_vec_rad.z, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
}
void AC_AttitudeControl::update_ang_vel_target_from_att_error()
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(Vector3f attitude_error_rot_vec_rad)
{
Vector3f rate_target_ang_vel;
// Compute the roll angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) {
_ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x;
rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x;
}
// Compute the pitch angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) {
_ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y;
rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y;
}
// Compute the yaw angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) {
_ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS));
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z;
rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z;
}
// Add feedforward term that attempts to ensure that the copter yaws about the reference
// Z axis, rather than the vehicle body Z axis.
// NOTE: This is a small-angle approximation.
_ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z;
_ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z;
return rate_target_ang_vel;
}
float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
// Run the roll angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_roll(float rate_target_rads)
{
float current_rate_rads = _ahrs.get_gyro().x;
float rate_error_rads = rate_target_rads - current_rate_rads;
@ -533,7 +605,8 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
return constrain_float(output, -1.0f, 1.0f);
}
float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads)
// Run the pitch angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_pitch(float rate_target_rads)
{
float current_rate_rads = _ahrs.get_gyro().y;
float rate_error_rads = rate_target_rads - current_rate_rads;
@ -556,7 +629,8 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads)
return constrain_float(output, -1.0f, 1.0f);
}
float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads)
// Run the yaw angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_yaw(float rate_target_rads)
{
float current_rate_rads = _ahrs.get_gyro().z;
float rate_error_rads = rate_target_rads - current_rate_rads;
@ -579,6 +653,7 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads)
return constrain_float(output, -1.0f, 1.0f);
}
// Enable or disable body-frame feed forward
void AC_AttitudeControl::accel_limiting(bool enable_limits)
{
if (enable_limits) {
@ -599,18 +674,6 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
}
}
void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
{
_throttle_in = throttle_in;
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (reset_attitude_control) {
relax_bf_rate_controller();
set_yaw_target_to_current_heading();
}
_motors.set_throttle(throttle_in);
_angle_boost = 0.0f;
}
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
float AC_AttitudeControl::get_althold_lean_angle_max() const
{
@ -618,6 +681,7 @@ float AC_AttitudeControl::get_althold_lean_angle_max() const
return ToDeg(_althold_lean_angle_max) * 100.0f;
}
// Proportional controller with piecewise sqrt sections to constrain second derivative
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim)
{
if (second_ord_lim < 0.0f || is_zero(second_ord_lim) || is_zero(p)) {
@ -635,43 +699,35 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
}
}
void AC_AttitudeControl::get_rotation_vehicle_to_ned(Matrix3f& m)
// Inverse proportional controller with piecewise sqrt sections to constrain second derivative
float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float second_ord_lim)
{
m = _ahrs.get_rotation_body_to_ned();
}
void AC_AttitudeControl::get_rotation_ned_to_vehicle(Matrix3f& m)
{
get_rotation_vehicle_to_ned(m);
m = m.transposed();
}
void AC_AttitudeControl::get_rotation_reference_to_ned(Matrix3f& m)
{
m.from_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
}
void AC_AttitudeControl::get_rotation_ned_to_reference(Matrix3f& m)
{
get_rotation_reference_to_ned(m);
m = m.transposed();
}
void AC_AttitudeControl::get_rotation_vehicle_to_reference(Matrix3f& m)
{
Matrix3f Tvn;
Matrix3f Tnr;
get_rotation_vehicle_to_ned(Tvn);
get_rotation_ned_to_reference(Tnr);
m = Tnr * Tvn;
}
void AC_AttitudeControl::get_rotation_reference_to_vehicle(Matrix3f& m)
{
get_rotation_vehicle_to_reference(m);
m = m.transposed();
if (second_ord_lim > 0.0f && !is_zero(second_ord_lim) && is_zero(p)) {
return (first_ord_mag*first_ord_mag)/(2.0f*second_ord_lim);
} else if ((second_ord_lim < 0.0f || is_zero(second_ord_lim)) && !is_zero(p)) {
return first_ord_mag/p;
} else if ((second_ord_lim < 0.0f || is_zero(second_ord_lim)) && is_zero(p)) {
return 0.0f;
}
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
float linear_velocity = second_ord_lim/p;
if (fabsf(first_ord_mag) < linear_velocity) {
// if our current velocity is below the cross-over point we use a linear function
return first_ord_mag/p;
} else {
float linear_dist = second_ord_lim/sq(p);
float overshoot = (linear_dist/2.0f) + sq(first_ord_mag)/(2.0f*second_ord_lim);
if (first_ord_mag > 0){
return overshoot;
} else {
return -overshoot;
}
}
}
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_roll()
{
float alpha = get_rate_roll_pid().get_filt_alpha();
@ -679,6 +735,7 @@ float AC_AttitudeControl::max_rate_step_bf_roll()
return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_roll_pid().kD())/_dt + get_rate_roll_pid().kP());
}
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_pitch()
{
float alpha = get_rate_pitch_pid().get_filt_alpha();
@ -686,6 +743,7 @@ float AC_AttitudeControl::max_rate_step_bf_pitch()
return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_pitch_pid().kD())/_dt + get_rate_pitch_pid().kP());
}
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_yaw()
{
float alpha = get_rate_yaw_pid().get_filt_alpha();

198
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -13,27 +13,21 @@
#include <AC_PID/AC_PID.h>
#include <AC_PID/AC_P.h>
// TODO: change the name or move to AP_Math? eliminate in favor of degrees(100)?
#define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centidegrees
#define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw
#define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(40.0f) // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS radians(720.0f) // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS radians(10.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(360.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS 110000.0f // default maximum acceleration for roll/pitch axis in centidegrees/sec/sec
#define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS 27000.0f // default maximum acceleration for yaw axis in centidegrees/sec/sec
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(40.0f) // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS radians(720.0f) // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS radians(10.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(120.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS 6000 // constraint on yaw angle error in degrees. This should lead to maximum turn rate of 10deg/sed * Stab Rate P so by default will be 45deg/sec.
#define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS 110000.0f // default maximum acceleration for roll/pitch axis in centidegrees/sec/sec
#define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS 27000.0f // default maximum acceleration for yaw axis in centidegrees/sec/sec
#define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT 1.0f // body-frame rate controller timeout in seconds
#define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for roll-pitch axis)
#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for yaw axis)
#define AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD radians(300.0f) // earth-frame rate stabilize controller's maximum overshoot angle (never limited)
#define AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD radians(300.0f) // earth-frame rate stabilize controller's maximum overshoot angle (never limited)
#define AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD radians(10.0f) // earth-frame rate stabilize controller's maximum overshoot angle
#define AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD radians(30.0f) // earth-frame rate stabilize controller's maximum overshoot angle
#define AC_ATTITUDE_THRUST_ERROR_ANGLE radians(30.0f) // Thrust angle error above which yaw corrections are limited
#define AC_ATTITUDE_100HZ_DT 0.0100f // delta time in seconds for 100hz update rate
#define AC_ATTITUDE_400HZ_DT 0.0025f // delta time in seconds for 400hz update rate
@ -43,11 +37,11 @@
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix
#define AC_ATTITUDE_CONTROL_MID_DEFAULT 0.5f // manual throttle mix
#define AC_ATTITUDE_CONTROL_MAX_DEFAULT 0.5f // maximum throttle mix default
#define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix
#define AC_ATTITUDE_CONTROL_MID_DEFAULT 0.5f // manual throttle mix
#define AC_ATTITUDE_CONTROL_MAX_DEFAULT 0.5f // maximum throttle mix default
#define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
#define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
class AC_AttitudeControl {
public:
@ -60,7 +54,7 @@ public:
_p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P),
_dt(dt),
_angle_boost(0),
_att_ctrl_use_accel_limit(true),
_use_ff_and_input_shaping(true),
_throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
_throttle_rpy_mix(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
_ahrs(ahrs),
@ -108,33 +102,30 @@ public:
// Sets and saves the yaw acceleration limit in centidegrees/s/s
void save_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; _accel_yaw_max.save(); }
// Ensure body-frame rate controller has zero errors to relax rate controller output
void relax_bf_rate_controller();
// Ensure attitude controller have zero errors to relax rate controller output
void relax_attitude_controllers();
// Sets yaw target to vehicle heading
void set_yaw_target_to_current_heading() { _att_target_euler_rad.z = _ahrs.yaw; }
void set_yaw_target_to_current_heading() { _attitude_target_euler_angle.z = _ahrs.yaw; }
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void shift_ef_yaw_target(float yaw_shift_cd);
// Command a Quaternion attitude with feedforward and smoothing
void input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain);
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_euler_rate_yaw_smooth(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain);
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain);
// Command an euler roll and pitch angle and an euler yaw rate
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain);
// Command an euler roll, pitch and yaw angle
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw);
// Command an euler roll, pitch, and yaw rate
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
// Command an angular velocity
// Command an angular velocity with angular velocity feedforward and smoothing
virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
// Command a quaternion attitude and a body-frame angular velocity
void input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads);
// Run angular velocity controller and send outputs to the motors
virtual void rate_controller_run() = 0;
@ -146,24 +137,23 @@ public:
bool ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads);
// Configures whether the attitude controller should limit the rate demand to constrain angular acceleration
void limit_angle_to_rate_request(bool limit_request) { _att_ctrl_use_accel_limit = limit_request; }
void use_ff_and_input_shaping(bool use_shaping) { _use_ff_and_input_shaping = use_shaping; }
// Return 321-intrinsic euler angles in centidegrees representing the rotation from NED earth frame to the
// attitude controller's reference attitude.
Vector3f get_att_target_euler_cd() const { return _att_target_euler_rad*degrees(100.0f); }
// attitude controller's target attitude.
Vector3f get_att_target_euler_cd() const { return _attitude_target_euler_angle*degrees(100.0f); }
// Return a rotation vector in centidegrees representing the rotation from vehicle body frame to the
// attitude controller's reference attitude.
Vector3f get_att_error_rot_vec_cd() const { return _att_error_rot_vec_rad*degrees(100.0f); }
// Return the angle between the target thrust vector and the current thrust vector.
float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); }
// Set x-axis angular velocity reference in centidegrees/s
void rate_bf_roll_target(float rate_cds) { _ang_vel_target_rads.x = radians(rate_cds*0.01f); }
// Set x-axis angular velocity in centidegrees/s
void rate_bf_roll_target(float rate_cds) { _rate_target_ang_vel.x = radians(rate_cds*0.01f); }
// Set y-axis angular velocity reference in centidegrees/s
void rate_bf_pitch_target(float rate_cds) { _ang_vel_target_rads.y = radians(rate_cds*0.01f); }
// Set y-axis angular velocity in centidegrees/s
void rate_bf_pitch_target(float rate_cds) { _rate_target_ang_vel.y = radians(rate_cds*0.01f); }
// Set z-axis angular velocity reference in centidegrees/s
void rate_bf_yaw_target(float rate_cds) { _ang_vel_target_rads.z = radians(rate_cds*0.01f); }
// Set z-axis angular velocity in centidegrees/s
void rate_bf_yaw_target(float rate_cds) { _rate_target_ang_vel.z = radians(rate_cds*0.01f); }
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
float max_rate_step_bf_roll();
@ -183,8 +173,8 @@ public:
// Return yaw step size in centidegrees that results in maximum output after 4 time steps
float max_angle_step_bf_yaw() { return max_rate_step_bf_yaw()/_p_angle_yaw.kP(); }
// Return reference angular velocity used in the angular velocity controller
Vector3f rate_bf_targets() const { return _ang_vel_target_rads*degrees(100.0f); }
// Return angular velocity in radians used in the angular velocity controller
Vector3f rate_bf_targets() const { return _rate_target_ang_vel; }
// Enable or disable body-frame feed forward
void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled = enable_or_disable; }
@ -222,57 +212,43 @@ public:
// Proportional controller with piecewise sqrt sections to constrain second derivative
static float sqrt_controller(float error, float p, float second_ord_lim);
// Inverse proportional controller with piecewise sqrt sections to constrain second derivative
static float stopping_point(float first_ord_mag, float p, float second_ord_lim);
// User settable parameters
static const struct AP_Param::GroupInfo var_info[];
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain
float input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel);
// limits the acceleration and deceleration of a velocity request
float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max);
// translates body frame acceleration limits to the euler axis
Vector3f euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel);
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot);
// Calculates the body frame angular velocities to follow the target attitude
void attitude_controller_run_quat();
protected:
// Retrieve a rotation matrix from the vehicle body frame to NED earth frame
void get_rotation_vehicle_to_ned(Matrix3f& m);
// Retrieve a rotation matrix from NED earth frame to the vehicle body frame
void get_rotation_ned_to_vehicle(Matrix3f& m);
// Retrieve a rotation matrix from reference (setpoint) body frame to NED earth frame
void get_rotation_reference_to_ned(Matrix3f& m);
// Retrieve a rotation matrix from NED earth frame to reference (setpoint) body frame
void get_rotation_ned_to_reference(Matrix3f& m);
// Retrieve a rotation matrix from vehicle body frame to reference (setpoint) body frame
void get_rotation_vehicle_to_reference(Matrix3f& m);
// Retrieve a rotation matrix from reference (setpoint) body frame to vehicle body frame
void get_rotation_reference_to_vehicle(Matrix3f& m);
// Command an euler attitude and a body-frame angular velocity
void attitude_controller_run_euler(const Vector3f& att_target_euler_rad, const Vector3f& att_target_ang_vel_rads);
// Command a quaternion attitude and a body-frame angular velocity
void attitude_controller_run_quat(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads);
// Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative
void update_att_target_roll(float euler_roll_rate_rads, float overshoot_max_rad);
// Update _att_target_euler_rad.y by integrating a 321-intrinsic euler pitch angle derivative
void update_att_target_pitch(float euler_pitch_rate_rads, float overshoot_max_rad);
// Update _att_target_euler_rad.z by integrating a 321-intrinsic euler yaw angle derivative
void update_att_target_yaw(float euler_yaw_rate_rads, float overshoot_max_rad);
// Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors();
// Update _ang_vel_target_rads using _att_error_rot_vec_rad
void update_ang_vel_target_from_att_error();
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f update_ang_vel_target_from_att_error(Vector3f attitude_error_rot_vec_rad);
// Run the roll angular velocity PID controller and return the output
float rate_bf_to_motor_roll(float rate_target_rads);
float rate_target_to_motor_roll(float rate_target_rads);
// Run the pitch angular velocity PID controller and return the output
float rate_bf_to_motor_pitch(float rate_target_rads);
float rate_target_to_motor_pitch(float rate_target_rads);
// Run the yaw angular velocity PID controller and return the output
virtual float rate_bf_to_motor_yaw(float rate_target_rads);
virtual float rate_target_to_motor_yaw(float rate_target_rads);
// Return angle in radians to be added to roll angle. Used by heli to counteract
// tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
@ -322,29 +298,30 @@ protected:
// Intersampling period in seconds
float _dt;
// This represents a 321-intrinsic rotation from NED frame to the reference (setpoint)
// attitude used in the attitude controller, in radians. Formerly _angle_ef_target.
Vector3f _att_target_euler_rad;
// This represents a 321-intrinsic rotation from NED frame to the target (setpoint)
// attitude used in the attitude controller, in radians.
Vector3f _attitude_target_euler_angle;
// This represents an euler axis-angle rotation vector from the vehicle’s
// estimated attitude to the reference (setpoint) attitude used in the attitude
// controller, in radians in the vehicle body frame of reference. Formerly
// _angle_bf_error.
Vector3f _att_error_rot_vec_rad;
// This represents the angular velocity of the reference (setpoint) attitude used in
// This represents the angular velocity of the target (setpoint) attitude used in
// the attitude controller as 321-intrinsic euler angle derivatives, in radians per
// second. Formerly _rate_ef_desired.
Vector3f _att_target_euler_rate_rads;
// second.
Vector3f _attitude_target_euler_rate;
// This represents the angular velocity of the reference (setpoint) attitude used in
// This represents a quaternion rotation from NED frame to the target (setpoint)
// attitude used in the attitude controller.
Quaternion _attitude_target_quat;
// This represents the angular velocity of the target (setpoint) attitude used in
// the attitude controller as an angular velocity vector, in radians per second in
// the reference attitude frame. Formerly _rate_bf_desired.
Vector3f _att_target_ang_vel_rads;
// the target attitude frame.
Vector3f _attitude_target_ang_vel;
// This represents the reference (setpoint) angular velocity used in the angular
// velocity controller, in radians per second. Formerly _rate_bf_target.
Vector3f _ang_vel_target_rads;
// This represents the angular velocity in radians per second, used in the angular
// velocity controller.
Vector3f _rate_target_ang_vel;
// The angle between the target thrust vector and the current thrust vector.
float _thrust_error_angle;
// throttle provided as input to attitude controller. This does not include angle boost.
float _throttle_in = 0.0f;
@ -353,14 +330,17 @@ protected:
// Used only for logging.
float _angle_boost;
// Specifies whether the attitude controller should use the acceleration limit
bool _att_ctrl_use_accel_limit;
// Specifies whether the attitude controller should use the input shaping and feedforward
bool _use_ff_and_input_shaping;
// Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold
float _althold_lean_angle_max = 0.0f;
float _throttle_rpy_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _throttle_rpy_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
// desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _throttle_rpy_mix_desired;
// mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_rpy_mix;
// References to external libraries
const AP_AHRS& _ahrs;

58
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -150,17 +150,17 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
_flags_heli.flybar_passthrough = true;
// set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
_att_target_ang_vel_rads.x = _ahrs.get_gyro().x;
_att_target_ang_vel_rads.y = _ahrs.get_gyro().y;
_attitude_target_ang_vel.x = _ahrs.get_gyro().x;
_attitude_target_ang_vel.y = _ahrs.get_gyro().y;
// accel limit desired yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
float rate_change_rads = yaw_rate_bf_rads - _att_target_ang_vel_rads.z;
float rate_change_rads = yaw_rate_bf_rads - _attitude_target_ang_vel.z;
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
_att_target_ang_vel_rads.z += rate_change_rads;
_attitude_target_ang_vel.z += rate_change_rads;
} else {
_att_target_ang_vel_rads.z = yaw_rate_bf_rads;
_attitude_target_ang_vel.z = yaw_rate_bf_rads;
}
integrate_bf_rate_error_to_angle_errors();
@ -173,32 +173,44 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
// convert angle error rotation vector into 321-intrinsic euler angle difference
// NOTE: this results an an approximation linearized about the vehicle's attitude
if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {
_att_target_euler_rad.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_att_target_euler_rad.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_att_target_euler_rad.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
_attitude_target_euler_angle.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_attitude_target_euler_angle.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_attitude_target_euler_angle.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
}
// handle flipping over pitch axis
if (_att_target_euler_rad.y > M_PI/2.0f) {
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI);
_att_target_euler_rad.y = wrap_PI(M_PI - _att_target_euler_rad.x);
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI);
if (_attitude_target_euler_angle.y > M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
if (_att_target_euler_rad.y < -M_PI/2.0f) {
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI);
_att_target_euler_rad.y = wrap_PI(-M_PI - _att_target_euler_rad.x);
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI);
if (_attitude_target_euler_angle.y < -M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(-M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
// convert body-frame angle errors to body-frame rate targets
update_ang_vel_target_from_att_error();
_rate_target_ang_vel = update_ang_vel_target_from_att_error(_att_error_rot_vec_rad);
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
_ang_vel_target_rads.x = _att_target_ang_vel_rads.x;
_ang_vel_target_rads.y = _att_target_ang_vel_rads.y;
_rate_target_ang_vel.x = _attitude_target_ang_vel.x;
_rate_target_ang_vel.y = _attitude_target_ang_vel.y;
// add desired target to yaw
_ang_vel_target_rads.z += _att_target_ang_vel_rads.z;
_rate_target_ang_vel.z += _attitude_target_ang_vel.z;
_thrust_error_angle = norm(_att_error_rot_vec_rad.x, _att_error_rot_vec_rad.y);
}
void AC_AttitudeControl_Heli::integrate_bf_rate_error_to_angle_errors()
{
// Integrate the angular velocity error into the attitude error
_att_error_rot_vec_rad += (_attitude_target_ang_vel - _ahrs.get_gyro()) * _dt;
// Constrain attitude error
_att_error_rot_vec_rad.x = constrain_float(_att_error_rot_vec_rad.x, -AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD, AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD);
_att_error_rot_vec_rad.y = constrain_float(_att_error_rot_vec_rad.y, -AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD, AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD);
_att_error_rot_vec_rad.z = constrain_float(_att_error_rot_vec_rad.z, -AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD, AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD);
}
// subclass non-passthrough too, for external gyro, no flybar
@ -223,12 +235,12 @@ void AC_AttitudeControl_Heli::rate_controller_run()
_motors.set_roll(_passthrough_roll/4500.0f);
_motors.set_pitch(_passthrough_pitch/4500.0f);
} else {
rate_bf_to_motor_roll_pitch(_ang_vel_target_rads.x, _ang_vel_target_rads.y);
rate_bf_to_motor_roll_pitch(_rate_target_ang_vel.x, _rate_target_ang_vel.y);
}
if (_flags_heli.tail_passthrough) {
_motors.set_yaw(_passthrough_yaw/4500.0f);
} else {
_motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
_motors.set_yaw(rate_target_to_motor_yaw(_rate_target_ang_vel.z));
}
}
@ -346,7 +358,7 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
}
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second
float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_rads)
float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_target_rads)
{
float pd,i,vff; // used to capture pid values for logging
float current_rate_rads; // this iteration's rate

12
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -27,6 +27,7 @@
#define AC_ATTITUDE_HELI_RATE_RP_FF_FILTER 10.0f
#define AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER 10.0f
#define AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT 300
#define AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD ToRad(30.0f)
class AC_AttitudeControl_Heli : public AC_AttitudeControl {
public:
@ -68,6 +69,9 @@ public:
// passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds);
// Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors();
// subclass non-passthrough too, for external gyro, no flybar
void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) override;
@ -119,7 +123,7 @@ private:
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in radians/sec) for roll, pitch and yaw
// outputs are sent directly to motor class
void rate_bf_to_motor_roll_pitch(float rate_roll_target_rads, float rate_pitch_target_rads);
float rate_bf_to_motor_yaw(float rate_yaw_rads);
float rate_target_to_motor_yaw(float rate_yaw_rads);
//
// throttle methods
@ -138,6 +142,12 @@ private:
// internal variables
float _hover_roll_trim_scalar = 0; // scalar used to suppress Hover Roll Trim
// This represents an euler axis-angle rotation vector from the vehicles
// estimated attitude to the reference (setpoint) attitude used in the attitude
// controller, in radians in the vehicle body frame of reference.
Vector3f _att_error_rot_vec_rad;
// parameters
AP_Int8 _piro_comp_enabled; // Flybar present or not. Affects attitude controller used during ACRO flight mode
AP_Int16 _hover_roll_trim; // Angle in centi-degrees used to counter tail rotor thrust in hover

6
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -225,9 +225,9 @@ void AC_AttitudeControl_Multi::rate_controller_run()
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();
_motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x));
_motors.set_pitch(rate_bf_to_motor_pitch(_ang_vel_target_rads.y));
_motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
_motors.set_roll(rate_target_to_motor_roll(_rate_target_ang_vel.x));
_motors.set_pitch(rate_target_to_motor_pitch(_rate_target_ang_vel.y));
_motors.set_yaw(rate_target_to_motor_yaw(_rate_target_ang_vel.z));
control_monitor_update();
}