mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AC_AttitudeControl: change internals to use radians instead of centidegrees
This commit is contained in:
Jonathan Challinger
Randy Mackay
parent
fff275fd99
commit
7330de86e5
@ -9,6 +9,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
||||
|
||||
// 0, 1 were RATE_RP_MAX, RATE_Y_MAX
|
||||
|
||||
// BUG: SLEW_YAW's behavior does not match its parameter documentation
|
||||
// @Param: SLEW_YAW
|
||||
// @DisplayName: Yaw target slew rate
|
||||
// @Description: Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
|
||||
@ -16,7 +17,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
||||
// @Range: 500 18000
|
||||
// @Increment: 100
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT),
|
||||
AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS),
|
||||
|
||||
// 3 was for ACCEL_RP_MAX
|
||||
|
||||
@ -28,7 +29,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
||||
// @Values: 0:Disabled, 18000:Slow, 36000:Medium, 54000:Fast
|
||||
// @Increment: 1000
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl, _accel_yaw_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT),
|
||||
AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl, _accel_yaw_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS),
|
||||
|
||||
// @Param: RATE_FF_ENAB
|
||||
// @DisplayName: Rate Feedforward Enable
|
||||
@ -45,7 +46,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
||||
// @Increment: 1000
|
||||
// @Values: 0:Disabled, 72000:Slow, 108000:Medium, 162000:Fast
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ACCEL_R_MAX", 6, AC_AttitudeControl, _accel_roll_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT),
|
||||
AP_GROUPINFO("ACCEL_R_MAX", 6, AC_AttitudeControl, _accel_roll_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS),
|
||||
|
||||
// @Param: ACCEL_P_MAX
|
||||
// @DisplayName: Acceleration Max for Pitch
|
||||
@ -55,7 +56,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
||||
// @Increment: 1000
|
||||
// @Values: 0:Disabled, 72000:Slow, 108000:Medium, 162000:Fast
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ACCEL_P_MAX", 7, AC_AttitudeControl, _accel_pitch_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT),
|
||||
AP_GROUPINFO("ACCEL_P_MAX", 7, AC_AttitudeControl, _accel_pitch_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS),
|
||||
|
||||
// IDs 8,9,10,11 RESERVED (in use on Solo)
|
||||
|
||||
@ -80,19 +81,19 @@ void AC_AttitudeControl::set_dt(float delta_sec)
|
||||
void AC_AttitudeControl::relax_bf_rate_controller()
|
||||
{
|
||||
// ensure zero error in body frame rate controllers
|
||||
const Vector3f& gyro = _ahrs.get_gyro();
|
||||
_rate_bf_target = gyro * AC_ATTITUDE_CONTROL_DEGX100;
|
||||
frame_conversion_bf_to_ef(_rate_bf_target, _rate_ef_desired);
|
||||
_rate_bf_target_rads = _ahrs.get_gyro();
|
||||
frame_conversion_bf_to_ef(_rate_bf_target_rads, _rate_ef_desired_rads);
|
||||
|
||||
_pid_rate_roll.reset_I();
|
||||
_pid_rate_pitch.reset_I();
|
||||
_pid_rate_yaw.reset_I();
|
||||
}
|
||||
|
||||
// shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centi-degreesa and is added to the current target heading
|
||||
// shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centi-degrees and is added to the current target heading
|
||||
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
|
||||
{
|
||||
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + yaw_shift_cd);
|
||||
// convert from centi-degrees on public interface to radians
|
||||
_angle_ef_target_rad.z = wrap_2PI(_angle_ef_target_rad.z + radians(yaw_shift_cd*0.01f));
|
||||
}
|
||||
|
||||
//
|
||||
@ -101,82 +102,87 @@ void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
|
||||
|
||||
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
|
||||
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
|
||||
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
|
||||
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_rate_ef_cds, float smoothing_gain)
|
||||
{
|
||||
float rate_ef_desired;
|
||||
float rate_change_limit;
|
||||
Vector3f angle_ef_error; // earth frame angle errors
|
||||
// convert from centi-degrees on public interface to radians
|
||||
float roll_angle_ef_rad = radians(roll_angle_ef_cd*0.01f);
|
||||
float pitch_angle_ef_rad = radians(pitch_angle_ef_cd*0.01f);
|
||||
float yaw_rate_ef_rads = radians(yaw_rate_ef_cds*0.01f);
|
||||
|
||||
float rate_ef_desired_rads;
|
||||
float rate_change_limit_rads;
|
||||
Vector3f angle_ef_error_rad; // earth frame angle errors
|
||||
|
||||
// sanity check smoothing gain
|
||||
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
|
||||
|
||||
// add roll trim to compensate tail rotor thrust in heli (should return zero for multirotors)
|
||||
roll_angle_ef += get_roll_trim();
|
||||
roll_angle_ef_rad += get_roll_trim_rad();
|
||||
|
||||
// if accel limiting and feed forward enabled
|
||||
if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled) {
|
||||
rate_change_limit = _accel_roll_max * _dt;
|
||||
if ((get_accel_roll_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
|
||||
rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
|
||||
|
||||
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
|
||||
rate_ef_desired = sqrt_controller(roll_angle_ef-_angle_ef_target.x, smoothing_gain, _accel_roll_max);
|
||||
rate_ef_desired_rads = sqrt_controller(roll_angle_ef_rad-_angle_ef_target_rad.x, smoothing_gain, get_accel_roll_max_radss());
|
||||
|
||||
// apply acceleration limit to feed forward roll rate
|
||||
_rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit);
|
||||
_rate_ef_desired_rads.x = constrain_float(rate_ef_desired_rads, _rate_ef_desired_rads.x-rate_change_limit_rads, _rate_ef_desired_rads.x+rate_change_limit_rads);
|
||||
|
||||
// update earth-frame roll angle target using desired roll rate
|
||||
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_roll_angle_and_error(_rate_ef_desired_rads.x, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
} else {
|
||||
// target roll and pitch to desired input roll and pitch
|
||||
_angle_ef_target.x = roll_angle_ef;
|
||||
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
|
||||
_angle_ef_target_rad.x = roll_angle_ef_rad;
|
||||
angle_ef_error_rad.x = wrap_180_cd_float(_angle_ef_target_rad.x - _ahrs.roll);
|
||||
|
||||
// set roll and pitch feed forward to zero
|
||||
_rate_ef_desired.x = 0;
|
||||
_rate_ef_desired_rads.x = 0;
|
||||
}
|
||||
// constrain earth-frame angle targets
|
||||
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
|
||||
_angle_ef_target_rad.x = constrain_float(_angle_ef_target_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
|
||||
|
||||
// if accel limiting and feed forward enabled
|
||||
if ((_accel_pitch_max > 0.0f) && _rate_bf_ff_enabled) {
|
||||
rate_change_limit = _accel_pitch_max * _dt;
|
||||
if ((get_accel_pitch_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
|
||||
rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
|
||||
|
||||
// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away
|
||||
rate_ef_desired = sqrt_controller(pitch_angle_ef-_angle_ef_target.y, smoothing_gain, _accel_pitch_max);
|
||||
rate_ef_desired_rads = sqrt_controller(pitch_angle_ef_rad-_angle_ef_target_rad.y, smoothing_gain, get_accel_pitch_max_radss());
|
||||
|
||||
// apply acceleration limit to feed forward pitch rate
|
||||
_rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit);
|
||||
_rate_ef_desired_rads.y = constrain_float(rate_ef_desired_rads, _rate_ef_desired_rads.y-rate_change_limit_rads, _rate_ef_desired_rads.y+rate_change_limit_rads);
|
||||
|
||||
// update earth-frame pitch angle target using desired pitch rate
|
||||
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_pitch_angle_and_error(_rate_ef_desired_rads.y, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
} else {
|
||||
// target roll and pitch to desired input roll and pitch
|
||||
_angle_ef_target.y = pitch_angle_ef;
|
||||
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
|
||||
// target pitch and pitch to desired input pitch and pitch
|
||||
_angle_ef_target_rad.y = pitch_angle_ef_rad;
|
||||
angle_ef_error_rad.y = wrap_180_cd_float(_angle_ef_target_rad.y - _ahrs.pitch);
|
||||
|
||||
// set roll and pitch feed forward to zero
|
||||
_rate_ef_desired.y = 0;
|
||||
// set pitch and pitch feed forward to zero
|
||||
_rate_ef_desired_rads.y = 0;
|
||||
}
|
||||
// constrain earth-frame angle targets
|
||||
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
|
||||
_angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
|
||||
|
||||
if (_accel_yaw_max > 0.0f) {
|
||||
if (get_accel_yaw_max_radss() > 0.0f) {
|
||||
// set earth-frame feed forward rate for yaw
|
||||
rate_change_limit = _accel_yaw_max * _dt;
|
||||
rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
|
||||
|
||||
// update yaw rate target with acceleration limit
|
||||
_rate_ef_desired.z += constrain_float(yaw_rate_ef - _rate_ef_desired.z, -rate_change_limit, rate_change_limit);
|
||||
_rate_ef_desired_rads.z += constrain_float(yaw_rate_ef_rads - _rate_ef_desired_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
|
||||
// calculate yaw target angle and angle error
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired_rads.z, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
} else {
|
||||
// set yaw feed forward to zero
|
||||
_rate_ef_desired.z = yaw_rate_ef;
|
||||
_rate_ef_desired_rads.z = yaw_rate_ef_rads;
|
||||
// calculate yaw target angle and angle error
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired_rads.z, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
}
|
||||
|
||||
// convert earth-frame angle errors to body-frame angle errors
|
||||
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
|
||||
frame_conversion_ef_to_bf(angle_ef_error_rad, _angle_bf_error_rad);
|
||||
|
||||
|
||||
// convert body-frame angle errors to body-frame rate targets
|
||||
@ -185,12 +191,12 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
|
||||
// add body frame rate feed forward
|
||||
if (_rate_bf_ff_enabled) {
|
||||
// convert earth-frame feed forward rates to body-frame feed forward rates
|
||||
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
|
||||
_rate_bf_target += _rate_bf_desired;
|
||||
frame_conversion_ef_to_bf(_rate_ef_desired_rads, _rate_bf_desired_rads);
|
||||
_rate_bf_target_rads += _rate_bf_desired_rads;
|
||||
} else {
|
||||
// convert earth-frame feed forward rates to body-frame feed forward rates
|
||||
frame_conversion_ef_to_bf(Vector3f(0,0,_rate_ef_desired.z), _rate_bf_desired);
|
||||
_rate_bf_target += _rate_bf_desired;
|
||||
frame_conversion_ef_to_bf(Vector3f(0,0,_rate_ef_desired_rads.z), _rate_bf_desired_rads);
|
||||
_rate_bf_target_rads += _rate_bf_desired_rads;
|
||||
}
|
||||
|
||||
// body-frame to motor outputs should be called separately
|
||||
@ -201,78 +207,89 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
|
||||
//
|
||||
|
||||
// angle_ef_roll_pitch_rate_ef_yaw - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)
|
||||
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef)
|
||||
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_rate_ef_cds)
|
||||
{
|
||||
Vector3f angle_ef_error; // earth frame angle errors
|
||||
// convert from centidegrees on public interface to radians
|
||||
float roll_angle_ef_rad = radians(roll_angle_ef_cd*0.01f);
|
||||
float pitch_angle_ef_rad = radians(pitch_angle_ef_cd*0.01f);
|
||||
float yaw_rate_ef_rads = radians(yaw_rate_ef_cds*0.01f);
|
||||
|
||||
Vector3f angle_ef_error_rad; // earth frame angle errors
|
||||
|
||||
// add roll trim to compensate tail rotor thrust in heli (should return zero for multirotors)
|
||||
roll_angle_ef += get_roll_trim();
|
||||
roll_angle_ef_rad += get_roll_trim_rad();
|
||||
|
||||
// set earth-frame angle targets for roll and pitch and calculate angle error
|
||||
_angle_ef_target.x = constrain_float(roll_angle_ef, -_aparm.angle_max, _aparm.angle_max);
|
||||
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
|
||||
_angle_ef_target_rad.x = constrain_float(roll_angle_ef_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
|
||||
angle_ef_error_rad.x = wrap_PI(_angle_ef_target_rad.x - _ahrs.roll);
|
||||
|
||||
_angle_ef_target.y = constrain_float(pitch_angle_ef, -_aparm.angle_max, _aparm.angle_max);
|
||||
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
|
||||
_angle_ef_target_rad.y = constrain_float(pitch_angle_ef_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
|
||||
angle_ef_error_rad.y = wrap_PI(_angle_ef_target_rad.y - _ahrs.pitch);
|
||||
|
||||
if (_accel_yaw_max > 0.0f) {
|
||||
if (get_accel_yaw_max_radss() > 0.0f) {
|
||||
// set earth-frame feed forward rate for yaw
|
||||
float rate_change_limit = _accel_yaw_max * _dt;
|
||||
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
|
||||
|
||||
float rate_change = yaw_rate_ef - _rate_ef_desired.z;
|
||||
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
|
||||
_rate_ef_desired.z += rate_change;
|
||||
float rate_change_rads = yaw_rate_ef_rads - _rate_ef_desired_rads.z;
|
||||
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
_rate_ef_desired_rads.z += rate_change_rads;
|
||||
// calculate yaw target angle and angle error
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired_rads.z, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
} else {
|
||||
// set yaw feed forward to zero
|
||||
_rate_ef_desired.z = yaw_rate_ef;
|
||||
_rate_ef_desired_rads.z = yaw_rate_ef_rads;
|
||||
// calculate yaw target angle and angle error
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired_rads.z, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
}
|
||||
|
||||
// convert earth-frame angle errors to body-frame angle errors
|
||||
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
|
||||
frame_conversion_ef_to_bf(angle_ef_error_rad, _angle_bf_error_rad);
|
||||
|
||||
// convert body-frame angle errors to body-frame rate targets
|
||||
update_rate_bf_targets();
|
||||
|
||||
// set roll and pitch feed forward to zero
|
||||
_rate_ef_desired.x = 0;
|
||||
_rate_ef_desired.y = 0;
|
||||
_rate_ef_desired_rads.x = 0;
|
||||
_rate_ef_desired_rads.y = 0;
|
||||
// convert earth-frame feed forward rates to body-frame feed forward rates
|
||||
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
|
||||
_rate_bf_target += _rate_bf_desired;
|
||||
frame_conversion_ef_to_bf(_rate_ef_desired_rads, _rate_bf_desired_rads);
|
||||
_rate_bf_target_rads += _rate_bf_desired_rads;
|
||||
|
||||
// body-frame to motor outputs should be called separately
|
||||
}
|
||||
|
||||
// angle_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw angle (all earth frame)
|
||||
// if yaw_slew is true then target yaw movement will be gradually moved to the new target based on the SLEW_YAW parameter
|
||||
void AC_AttitudeControl::angle_ef_roll_pitch_yaw(float roll_angle_ef, float pitch_angle_ef, float yaw_angle_ef, bool slew_yaw)
|
||||
void AC_AttitudeControl::angle_ef_roll_pitch_yaw(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_angle_ef_cd, bool slew_yaw)
|
||||
{
|
||||
Vector3f angle_ef_error;
|
||||
// convert from centidegrees on public interface to radians
|
||||
float roll_angle_ef_rad = radians(roll_angle_ef_cd*0.01f);
|
||||
float pitch_angle_ef_rad = radians(pitch_angle_ef_cd*0.01f);
|
||||
float yaw_angle_ef_rad = radians(yaw_angle_ef_cd*0.01f);
|
||||
|
||||
Vector3f angle_ef_error_rad;
|
||||
|
||||
// add roll trim to compensate tail rotor thrust in heli (should return zero for multirotors)
|
||||
roll_angle_ef += get_roll_trim();
|
||||
roll_angle_ef_rad += get_roll_trim_rad();
|
||||
|
||||
// set earth-frame angle targets
|
||||
_angle_ef_target.x = constrain_float(roll_angle_ef, -_aparm.angle_max, _aparm.angle_max);
|
||||
_angle_ef_target.y = constrain_float(pitch_angle_ef, -_aparm.angle_max, _aparm.angle_max);
|
||||
_angle_ef_target.z = yaw_angle_ef;
|
||||
_angle_ef_target_rad.x = constrain_float(roll_angle_ef_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
|
||||
_angle_ef_target_rad.y = constrain_float(pitch_angle_ef_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
|
||||
_angle_ef_target_rad.z = yaw_angle_ef_rad;
|
||||
|
||||
// calculate earth frame errors
|
||||
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
|
||||
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
|
||||
angle_ef_error.z = wrap_180_cd_float(_angle_ef_target.z - _ahrs.yaw_sensor);
|
||||
angle_ef_error_rad.x = wrap_PI(_angle_ef_target_rad.x - _ahrs.roll);
|
||||
angle_ef_error_rad.y = wrap_PI(_angle_ef_target_rad.y - _ahrs.pitch);
|
||||
angle_ef_error_rad.z = wrap_PI(_angle_ef_target_rad.z - _ahrs.yaw);
|
||||
|
||||
// constrain the yaw angle error
|
||||
// BUG: SLEW_YAW's behavior does not match its parameter documentation
|
||||
if (slew_yaw) {
|
||||
angle_ef_error.z = constrain_float(angle_ef_error.z,-_slew_yaw,_slew_yaw);
|
||||
angle_ef_error_rad.z = constrain_float(angle_ef_error_rad.z,-get_slew_yaw_rads(),get_slew_yaw_rads());
|
||||
}
|
||||
|
||||
// convert earth-frame angle errors to body-frame angle errors
|
||||
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
|
||||
frame_conversion_ef_to_bf(angle_ef_error_rad, _angle_bf_error_rad);
|
||||
|
||||
// convert body-frame angle errors to body-frame rate targets
|
||||
update_rate_bf_targets();
|
||||
@ -281,137 +298,147 @@ void AC_AttitudeControl::angle_ef_roll_pitch_yaw(float roll_angle_ef, float pitc
|
||||
}
|
||||
|
||||
// rate_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all earth frame)
|
||||
void AC_AttitudeControl::rate_ef_roll_pitch_yaw(float roll_rate_ef, float pitch_rate_ef, float yaw_rate_ef)
|
||||
void AC_AttitudeControl::rate_ef_roll_pitch_yaw(float roll_rate_ef_cds, float pitch_rate_ef_cds, float yaw_rate_ef_cds)
|
||||
{
|
||||
Vector3f angle_ef_error;
|
||||
float rate_change_limit, rate_change;
|
||||
// convert from centidegrees on public interface to radians
|
||||
float roll_rate_ef_rads = radians(roll_rate_ef_cds*0.01f);
|
||||
float pitch_rate_ef_rads = radians(pitch_rate_ef_cds*0.01f);
|
||||
float yaw_rate_ef_rads = radians(yaw_rate_ef_cds*0.01f);
|
||||
|
||||
if (_accel_roll_max > 0.0f) {
|
||||
rate_change_limit = _accel_roll_max * _dt;
|
||||
Vector3f angle_ef_error_rad;
|
||||
float rate_change_limit_rads, rate_change_rads;
|
||||
|
||||
if (get_accel_roll_max_radss() > 0.0f) {
|
||||
rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
|
||||
|
||||
// update feed forward roll rate after checking it is within acceleration limits
|
||||
rate_change = roll_rate_ef - _rate_ef_desired.x;
|
||||
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
|
||||
_rate_ef_desired.x += rate_change;
|
||||
rate_change_rads = roll_rate_ef_rads - _rate_ef_desired_rads.x;
|
||||
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
_rate_ef_desired_rads.x += rate_change_rads;
|
||||
} else {
|
||||
_rate_ef_desired.x = roll_rate_ef;
|
||||
_rate_ef_desired_rads.x = roll_rate_ef_rads;
|
||||
}
|
||||
|
||||
if (_accel_pitch_max > 0.0f) {
|
||||
rate_change_limit = _accel_pitch_max * _dt;
|
||||
if (get_accel_pitch_max_radss() > 0.0f) {
|
||||
rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
|
||||
|
||||
// update feed forward pitch rate after checking it is within acceleration limits
|
||||
rate_change = pitch_rate_ef - _rate_ef_desired.y;
|
||||
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
|
||||
_rate_ef_desired.y += rate_change;
|
||||
rate_change_rads = pitch_rate_ef_rads - _rate_ef_desired_rads.y;
|
||||
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
_rate_ef_desired_rads.y += rate_change_rads;
|
||||
} else {
|
||||
_rate_ef_desired.y = pitch_rate_ef;
|
||||
_rate_ef_desired_rads.y = pitch_rate_ef_rads;
|
||||
}
|
||||
|
||||
if (_accel_yaw_max > 0.0f) {
|
||||
rate_change_limit = _accel_yaw_max * _dt;
|
||||
if (get_accel_yaw_max_radss() > 0.0f) {
|
||||
rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
|
||||
|
||||
// update feed forward yaw rate after checking it is within acceleration limits
|
||||
rate_change = yaw_rate_ef - _rate_ef_desired.z;
|
||||
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
|
||||
_rate_ef_desired.z += rate_change;
|
||||
rate_change_rads = yaw_rate_ef_rads - _rate_ef_desired_rads.z;
|
||||
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
_rate_ef_desired_rads.z += rate_change_rads;
|
||||
} else {
|
||||
_rate_ef_desired.z = yaw_rate_ef;
|
||||
_rate_ef_desired_rads.z = yaw_rate_ef_rads;
|
||||
}
|
||||
|
||||
// update earth frame angle targets and errors
|
||||
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_roll_angle_and_error(_rate_ef_desired_rads.x, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
update_ef_pitch_angle_and_error(_rate_ef_desired_rads.y, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired_rads.z, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
|
||||
// constrain earth-frame angle targets
|
||||
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
|
||||
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
|
||||
_angle_ef_target_rad.x = constrain_float(_angle_ef_target_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
|
||||
_angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
|
||||
|
||||
// convert earth-frame angle errors to body-frame angle errors
|
||||
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
|
||||
frame_conversion_ef_to_bf(angle_ef_error_rad, _angle_bf_error_rad);
|
||||
|
||||
// convert body-frame angle errors to body-frame rate targets
|
||||
update_rate_bf_targets();
|
||||
|
||||
// convert earth-frame rates to body-frame rates
|
||||
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
|
||||
frame_conversion_ef_to_bf(_rate_ef_desired_rads, _rate_bf_desired_rads);
|
||||
|
||||
// add body frame rate feed forward
|
||||
_rate_bf_target += _rate_bf_desired;
|
||||
_rate_bf_target_rads += _rate_bf_desired_rads;
|
||||
|
||||
// body-frame to motor outputs should be called separately
|
||||
}
|
||||
|
||||
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame)
|
||||
void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf)
|
||||
void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
|
||||
{
|
||||
Vector3f angle_ef_error;
|
||||
// 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 rate_change, rate_change_limit;
|
||||
Vector3f angle_ef_error_rad;
|
||||
|
||||
float rate_change_rads, rate_change_limit_rads;
|
||||
|
||||
// update the rate feed forward with angular acceleration limits
|
||||
if (_accel_roll_max > 0.0f) {
|
||||
rate_change_limit = _accel_roll_max * _dt;
|
||||
if (get_accel_roll_max_radss() > 0.0f) {
|
||||
rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
|
||||
|
||||
rate_change = roll_rate_bf - _rate_bf_desired.x;
|
||||
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
|
||||
_rate_bf_desired.x += rate_change;
|
||||
rate_change_rads = roll_rate_bf_rads - _rate_bf_desired_rads.x;
|
||||
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
_rate_bf_desired_rads.x += rate_change_rads;
|
||||
} else {
|
||||
_rate_bf_desired.x = roll_rate_bf;
|
||||
_rate_bf_desired_rads.x = roll_rate_bf_rads;
|
||||
}
|
||||
|
||||
// update the rate feed forward with angular acceleration limits
|
||||
if (_accel_pitch_max > 0.0f) {
|
||||
rate_change_limit = _accel_pitch_max * _dt;
|
||||
if (get_accel_pitch_max_radss() > 0.0f) {
|
||||
rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
|
||||
|
||||
rate_change = pitch_rate_bf - _rate_bf_desired.y;
|
||||
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
|
||||
_rate_bf_desired.y += rate_change;
|
||||
rate_change_rads = pitch_rate_bf_rads - _rate_bf_desired_rads.y;
|
||||
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
_rate_bf_desired_rads.y += rate_change_rads;
|
||||
} else {
|
||||
_rate_bf_desired.y = pitch_rate_bf;
|
||||
_rate_bf_desired_rads.y = pitch_rate_bf_rads;
|
||||
}
|
||||
|
||||
if (_accel_yaw_max > 0.0f) {
|
||||
rate_change_limit = _accel_yaw_max * _dt;
|
||||
if (get_accel_yaw_max_radss() > 0.0f) {
|
||||
rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
|
||||
|
||||
rate_change = yaw_rate_bf - _rate_bf_desired.z;
|
||||
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
|
||||
_rate_bf_desired.z += rate_change;
|
||||
rate_change_rads = yaw_rate_bf_rads - _rate_bf_desired_rads.z;
|
||||
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
_rate_bf_desired_rads.z += rate_change_rads;
|
||||
} else {
|
||||
_rate_bf_desired.z = yaw_rate_bf;
|
||||
_rate_bf_desired_rads.z = yaw_rate_bf_rads;
|
||||
}
|
||||
|
||||
// Update angle error
|
||||
if (labs(_ahrs.pitch_sensor)<_acro_angle_switch) {
|
||||
_acro_angle_switch = 6000;
|
||||
if (fabsf(_ahrs.pitch)<_acro_angle_switch_rad) {
|
||||
_acro_angle_switch_rad = radians(60.0f);
|
||||
// convert body-frame rates to earth-frame rates
|
||||
frame_conversion_bf_to_ef(_rate_bf_desired, _rate_ef_desired);
|
||||
frame_conversion_bf_to_ef(_rate_bf_desired_rads, _rate_ef_desired_rads);
|
||||
|
||||
// update earth frame angle targets and errors
|
||||
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
|
||||
update_ef_roll_angle_and_error(_rate_ef_desired_rads.x, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
update_ef_pitch_angle_and_error(_rate_ef_desired_rads.y, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
update_ef_yaw_angle_and_error(_rate_ef_desired_rads.z, angle_ef_error_rad, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
|
||||
// convert earth-frame angle errors to body-frame angle errors
|
||||
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
|
||||
frame_conversion_ef_to_bf(angle_ef_error_rad, _angle_bf_error_rad);
|
||||
} else {
|
||||
_acro_angle_switch = 4500;
|
||||
_acro_angle_switch_rad = radians(45.0f);
|
||||
integrate_bf_rate_error_to_angle_errors();
|
||||
if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) {
|
||||
_angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
|
||||
_angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
|
||||
_angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
|
||||
if (frame_conversion_bf_to_ef(_angle_bf_error_rad, angle_ef_error_rad)) {
|
||||
_angle_ef_target_rad.x = wrap_PI(angle_ef_error_rad.x + _ahrs.roll);
|
||||
_angle_ef_target_rad.y = wrap_PI(angle_ef_error_rad.y + _ahrs.pitch);
|
||||
_angle_ef_target_rad.z = wrap_2PI(angle_ef_error_rad.z + _ahrs.yaw);
|
||||
}
|
||||
if (_angle_ef_target.y > 9000.0f) {
|
||||
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
|
||||
_angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.y);
|
||||
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
|
||||
if (_angle_ef_target_rad.y > M_PI/2.0f) {
|
||||
_angle_ef_target_rad.x = wrap_PI(_angle_ef_target_rad.x + M_PI);
|
||||
_angle_ef_target_rad.y = wrap_PI(M_PI - _angle_ef_target_rad.y);
|
||||
_angle_ef_target_rad.z = wrap_2PI(_angle_ef_target_rad.z + M_PI);
|
||||
}
|
||||
if (_angle_ef_target.y < -9000.0f) {
|
||||
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
|
||||
_angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.y);
|
||||
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
|
||||
if (_angle_ef_target_rad.y < -M_PI/2.0f) {
|
||||
_angle_ef_target_rad.x = wrap_PI(_angle_ef_target_rad.x + M_PI);
|
||||
_angle_ef_target_rad.y = wrap_PI(-M_PI - _angle_ef_target_rad.y);
|
||||
_angle_ef_target_rad.z = wrap_2PI(_angle_ef_target_rad.z + M_PI);
|
||||
}
|
||||
}
|
||||
|
||||
@ -419,7 +446,7 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
|
||||
update_rate_bf_targets();
|
||||
|
||||
// body-frame rate commands added
|
||||
_rate_bf_target += _rate_bf_desired;
|
||||
_rate_bf_target_rads += _rate_bf_desired_rads;
|
||||
}
|
||||
|
||||
//
|
||||
@ -431,9 +458,9 @@ void AC_AttitudeControl::rate_controller_run()
|
||||
// call rate controllers and send output to motors object
|
||||
// To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
|
||||
// To-Do: skip this step if the throttle out is zero?
|
||||
_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x));
|
||||
_motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y));
|
||||
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z));
|
||||
_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target_rads.x));
|
||||
_motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target_rads.y));
|
||||
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target_rads.z));
|
||||
}
|
||||
|
||||
//
|
||||
@ -471,130 +498,133 @@ bool AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Ve
|
||||
//
|
||||
|
||||
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
|
||||
void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max)
|
||||
void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad)
|
||||
{
|
||||
// calculate angle error with maximum of +- max angle overshoot
|
||||
angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);
|
||||
angle_ef_error.x = constrain_float(angle_ef_error.x, -overshoot_max, overshoot_max);
|
||||
angle_ef_error_rad.x = wrap_PI(_angle_ef_target_rad.x - _ahrs.roll);
|
||||
angle_ef_error_rad.x = constrain_float(angle_ef_error_rad.x, -overshoot_max_rad, overshoot_max_rad);
|
||||
|
||||
// update roll angle target to be within max angle overshoot of our roll angle
|
||||
_angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;
|
||||
_angle_ef_target_rad.x = angle_ef_error_rad.x + _ahrs.roll;
|
||||
|
||||
// increment the roll angle target
|
||||
_angle_ef_target.x += roll_rate_ef * _dt;
|
||||
_angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);
|
||||
_angle_ef_target_rad.x += roll_rate_ef_rads * _dt;
|
||||
_angle_ef_target_rad.x = wrap_PI(_angle_ef_target_rad.x);
|
||||
}
|
||||
|
||||
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request
|
||||
void AC_AttitudeControl::update_ef_pitch_angle_and_error(float pitch_rate_ef, Vector3f &angle_ef_error, float overshoot_max)
|
||||
void AC_AttitudeControl::update_ef_pitch_angle_and_error(float pitch_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad)
|
||||
{
|
||||
// calculate angle error with maximum of +- max angle overshoot
|
||||
// To-Do: should we do something better as we cross 90 degrees?
|
||||
angle_ef_error.y = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor);
|
||||
angle_ef_error.y = constrain_float(angle_ef_error.y, -overshoot_max, overshoot_max);
|
||||
angle_ef_error_rad.y = wrap_PI(_angle_ef_target_rad.y - _ahrs.pitch);
|
||||
angle_ef_error_rad.y = constrain_float(angle_ef_error_rad.y, -overshoot_max_rad, overshoot_max_rad);
|
||||
|
||||
// update pitch angle target to be within max angle overshoot of our pitch angle
|
||||
_angle_ef_target.y = angle_ef_error.y + _ahrs.pitch_sensor;
|
||||
_angle_ef_target_rad.y = angle_ef_error_rad.y + _ahrs.pitch;
|
||||
|
||||
// increment the pitch angle target
|
||||
_angle_ef_target.y += pitch_rate_ef * _dt;
|
||||
_angle_ef_target.y = wrap_180_cd(_angle_ef_target.y);
|
||||
_angle_ef_target_rad.y += pitch_rate_ef_rads * _dt;
|
||||
_angle_ef_target_rad.y = wrap_PI(_angle_ef_target_rad.y);
|
||||
}
|
||||
|
||||
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request
|
||||
void AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error, float overshoot_max)
|
||||
void AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad)
|
||||
{
|
||||
// calculate angle error with maximum of +- max angle overshoot
|
||||
angle_ef_error.z = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor);
|
||||
angle_ef_error.z = constrain_float(angle_ef_error.z, -overshoot_max, overshoot_max);
|
||||
angle_ef_error_rad.z = wrap_PI(_angle_ef_target_rad.z - _ahrs.yaw);
|
||||
angle_ef_error_rad.z = constrain_float(angle_ef_error_rad.z, -overshoot_max_rad, overshoot_max_rad);
|
||||
|
||||
// update yaw angle target to be within max angle overshoot of our current heading
|
||||
_angle_ef_target.z = angle_ef_error.z + _ahrs.yaw_sensor;
|
||||
_angle_ef_target_rad.z = angle_ef_error_rad.z + _ahrs.yaw;
|
||||
|
||||
// increment the yaw angle target
|
||||
_angle_ef_target.z += yaw_rate_ef * _dt;
|
||||
_angle_ef_target.z = wrap_360_cd(_angle_ef_target.z);
|
||||
_angle_ef_target_rad.z += yaw_rate_ef_rads * _dt;
|
||||
_angle_ef_target_rad.z = wrap_2PI(_angle_ef_target_rad.z);
|
||||
}
|
||||
|
||||
// update_rate_bf_errors - calculates body frame angle errors
|
||||
// body-frame feed forward rates (centi-degrees / second) taken from _angle_bf_error
|
||||
// angle errors in centi-degrees placed in _angle_bf_error
|
||||
// body-frame feed forward rates (radians/s) taken from _angle_bf_error
|
||||
// angle errors in radians placed in _angle_bf_error
|
||||
void AC_AttitudeControl::integrate_bf_rate_error_to_angle_errors()
|
||||
{
|
||||
// roll - calculate body-frame angle error by integrating body-frame rate error
|
||||
_angle_bf_error.x += (_rate_bf_desired.x - (_ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100)) * _dt;
|
||||
_angle_bf_error_rad.x += (_rate_bf_desired_rads.x - _ahrs.get_gyro().x) * _dt;
|
||||
// roll - limit maximum error
|
||||
_angle_bf_error.x = constrain_float(_angle_bf_error.x, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
|
||||
_angle_bf_error_rad.x = constrain_float(_angle_bf_error_rad.x, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
|
||||
|
||||
// pitch - calculate body-frame angle error by integrating body-frame rate error
|
||||
_angle_bf_error.y += (_rate_bf_desired.y - (_ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100)) * _dt;
|
||||
_angle_bf_error_rad.y += (_rate_bf_desired_rads.y - _ahrs.get_gyro().y) * _dt;
|
||||
// pitch - limit maximum error
|
||||
_angle_bf_error.y = constrain_float(_angle_bf_error.y, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
|
||||
_angle_bf_error_rad.y = constrain_float(_angle_bf_error_rad.y, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
|
||||
|
||||
// yaw - calculate body-frame angle error by integrating body-frame rate error
|
||||
_angle_bf_error.z += (_rate_bf_desired.z - (_ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100)) * _dt;
|
||||
_angle_bf_error_rad.z += (_rate_bf_desired_rads.z - _ahrs.get_gyro().z) * _dt;
|
||||
// yaw - limit maximum error
|
||||
_angle_bf_error.z = constrain_float(_angle_bf_error.z, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
|
||||
_angle_bf_error_rad.z = constrain_float(_angle_bf_error_rad.z, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
|
||||
|
||||
// To-Do: handle case of motors being disarmed or channel_throttle == 0 and set error to zero
|
||||
}
|
||||
|
||||
// update_rate_bf_targets - converts body-frame angle error to body-frame rate targets for roll, pitch and yaw axis
|
||||
// targets rates in centi-degrees taken from _angle_bf_error
|
||||
// results in centi-degrees/sec put into _rate_bf_target
|
||||
// targets rates in radians/s taken from _angle_bf_error
|
||||
// results in radians/s put into _rate_bf_target
|
||||
void AC_AttitudeControl::update_rate_bf_targets()
|
||||
{
|
||||
|
||||
// stab roll calculation
|
||||
// constrain roll rate request
|
||||
|
||||
if (_flags.limit_angle_to_rate_request && _accel_roll_max > 0.0f) {
|
||||
_rate_bf_target.x = sqrt_controller(_angle_bf_error.x, _p_angle_roll.kP(), constrain_float(_accel_roll_max/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX));
|
||||
_rate_bf_target_rads.x = sqrt_controller(_angle_bf_error_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{
|
||||
_rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x;
|
||||
_rate_bf_target_rads.x = _p_angle_roll.kP() * _angle_bf_error_rad.x;
|
||||
}
|
||||
|
||||
// stab pitch calculation
|
||||
// constrain pitch rate request
|
||||
if (_flags.limit_angle_to_rate_request && _accel_pitch_max > 0.0f) {
|
||||
_rate_bf_target.y = sqrt_controller(_angle_bf_error.y, _p_angle_pitch.kP(), constrain_float(_accel_pitch_max/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX));
|
||||
_rate_bf_target_rads.y = sqrt_controller(_angle_bf_error_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{
|
||||
_rate_bf_target.y = _p_angle_pitch.kP() * _angle_bf_error.y;
|
||||
_rate_bf_target_rads.y = _p_angle_pitch.kP() * _angle_bf_error_rad.y;
|
||||
}
|
||||
|
||||
// stab yaw calculation
|
||||
// constrain yaw rate request
|
||||
if (_flags.limit_angle_to_rate_request && _accel_yaw_max > 0.0f) {
|
||||
_rate_bf_target.z = sqrt_controller(_angle_bf_error.z, _p_angle_yaw.kP(), constrain_float(_accel_yaw_max/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX));
|
||||
_rate_bf_target_rads.z = sqrt_controller(_angle_bf_error_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{
|
||||
_rate_bf_target.z = _p_angle_yaw.kP() * _angle_bf_error.z;
|
||||
_rate_bf_target_rads.z = _p_angle_yaw.kP() * _angle_bf_error_rad.z;
|
||||
}
|
||||
|
||||
// include roll and pitch rate required to account for precession of the desired attitude about the body frame yaw axes
|
||||
_rate_bf_target.x += _angle_bf_error.y * _ahrs.get_gyro().z;
|
||||
_rate_bf_target.y += -_angle_bf_error.x * _ahrs.get_gyro().z;
|
||||
_rate_bf_target_rads.x += _angle_bf_error_rad.y * _ahrs.get_gyro().z;
|
||||
_rate_bf_target_rads.y += -_angle_bf_error_rad.x * _ahrs.get_gyro().z;
|
||||
}
|
||||
|
||||
//
|
||||
// body-frame rate controller
|
||||
//
|
||||
|
||||
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
|
||||
float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds)
|
||||
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/s
|
||||
float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
|
||||
{
|
||||
float p,i,d; // used to capture pid values for logging
|
||||
float current_rate; // this iteration's rate
|
||||
float rate_error; // simply target_rate - current_rate
|
||||
float current_rate_rads; // this iteration's rate
|
||||
float rate_error_rads; // simply target_rate - current_rate
|
||||
|
||||
// get current rate
|
||||
// To-Do: make getting gyro rates more efficient?
|
||||
current_rate = (_ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100);
|
||||
current_rate_rads = _ahrs.get_gyro().x;
|
||||
|
||||
// calculate error and call pid controller
|
||||
rate_error = rate_target_cds - current_rate;
|
||||
_pid_rate_roll.set_input_filter_d(rate_error);
|
||||
_pid_rate_roll.set_desired_rate(rate_target_cds);
|
||||
rate_error_rads = rate_target_rads - current_rate_rads;
|
||||
|
||||
// For legacy reasons, we convert to centi-degrees before inputting to the PID
|
||||
_pid_rate_roll.set_input_filter_d(degrees(rate_error_rads)*100.0f);
|
||||
_pid_rate_roll.set_desired_rate(degrees(rate_target_rads)*100.0f);
|
||||
|
||||
// get p value
|
||||
p = _pid_rate_roll.get_p();
|
||||
@ -603,7 +633,7 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds)
|
||||
i = _pid_rate_roll.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!_motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
|
||||
if (!_motors.limit.roll_pitch || ((i>0&&rate_error_rads<0)||(i<0&&rate_error_rads>0))) {
|
||||
i = _pid_rate_roll.get_i();
|
||||
}
|
||||
|
||||
@ -614,21 +644,23 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds)
|
||||
return constrain_float((p+i+d), -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
|
||||
}
|
||||
|
||||
// rate_bf_to_motor_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
|
||||
float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_cds)
|
||||
// rate_bf_to_motor_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/s
|
||||
float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads)
|
||||
{
|
||||
float p,i,d; // used to capture pid values for logging
|
||||
float current_rate; // this iteration's rate
|
||||
float rate_error; // simply target_rate - current_rate
|
||||
float current_rate_rads; // this iteration's rate
|
||||
float rate_error_rads; // simply target_rate - current_rate
|
||||
|
||||
// get current rate
|
||||
// To-Do: make getting gyro rates more efficient?
|
||||
current_rate = (_ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100);
|
||||
current_rate_rads = _ahrs.get_gyro().y;
|
||||
|
||||
// calculate error and call pid controller
|
||||
rate_error = rate_target_cds - current_rate;
|
||||
_pid_rate_pitch.set_input_filter_d(rate_error);
|
||||
_pid_rate_pitch.set_desired_rate(rate_target_cds);
|
||||
rate_error_rads = rate_target_rads - current_rate_rads;
|
||||
|
||||
// For legacy reasons, we convert to centi-degrees before inputting to the PID
|
||||
_pid_rate_pitch.set_input_filter_d(degrees(rate_error_rads)*100.0f);
|
||||
_pid_rate_pitch.set_desired_rate(degrees(rate_target_rads)*100.0f);
|
||||
|
||||
// get p value
|
||||
p = _pid_rate_pitch.get_p();
|
||||
@ -637,7 +669,7 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_cds)
|
||||
i = _pid_rate_pitch.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!_motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
|
||||
if (!_motors.limit.roll_pitch || ((i>0&&rate_error_rads<0)||(i<0&&rate_error_rads>0))) {
|
||||
i = _pid_rate_pitch.get_i();
|
||||
}
|
||||
|
||||
@ -648,21 +680,23 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_cds)
|
||||
return constrain_float((p+i+d), -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
|
||||
}
|
||||
|
||||
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
|
||||
float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_cds)
|
||||
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/s
|
||||
float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads)
|
||||
{
|
||||
float p,i,d; // used to capture pid values for logging
|
||||
float current_rate; // this iteration's rate
|
||||
float rate_error; // simply target_rate - current_rate
|
||||
float current_rate_rads; // this iteration's rate
|
||||
float rate_error_rads; // simply target_rate - current_rate
|
||||
|
||||
// get current rate
|
||||
// To-Do: make getting gyro rates more efficient?
|
||||
current_rate = (_ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100);
|
||||
current_rate_rads = _ahrs.get_gyro().z;
|
||||
|
||||
// calculate error and call pid controller
|
||||
rate_error = rate_target_cds - current_rate;
|
||||
_pid_rate_yaw.set_input_filter_all(rate_error);
|
||||
_pid_rate_yaw.set_desired_rate(rate_target_cds);
|
||||
rate_error_rads = rate_target_rads - current_rate_rads;
|
||||
|
||||
// For legacy reasons, we convert to centi-degrees before inputting to the PID
|
||||
_pid_rate_yaw.set_input_filter_all(degrees(rate_error_rads)*100.0f);
|
||||
_pid_rate_yaw.set_desired_rate(degrees(rate_target_rads)*100.0f);
|
||||
|
||||
// get p value
|
||||
p = _pid_rate_yaw.get_p();
|
||||
@ -671,7 +705,7 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_cds)
|
||||
i = _pid_rate_yaw.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!_motors.limit.yaw || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
|
||||
if (!_motors.limit.yaw || ((i>0&&rate_error_rads<0)||(i<0&&rate_error_rads>0))) {
|
||||
i = _pid_rate_yaw.get_i();
|
||||
}
|
||||
|
||||
@ -757,7 +791,8 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
|
||||
}
|
||||
}
|
||||
|
||||
// Maximum roll rate step size that results in maximum output after 4 time steps
|
||||
// Maximum roll rate step size in centidegrees that results in maximum output after 4 time steps
|
||||
// NOTE: for legacy reasons, the output of this function is in centidegrees
|
||||
float AC_AttitudeControl::max_rate_step_bf_roll()
|
||||
{
|
||||
float alpha = _pid_rate_roll.get_filt_alpha();
|
||||
@ -766,6 +801,7 @@ float AC_AttitudeControl::max_rate_step_bf_roll()
|
||||
}
|
||||
|
||||
// Maximum pitch rate step size that results in maximum output after 4 time steps
|
||||
// NOTE: for legacy reasons, the output of this function is in centidegrees
|
||||
float AC_AttitudeControl::max_rate_step_bf_pitch()
|
||||
{
|
||||
float alpha = _pid_rate_pitch.get_filt_alpha();
|
||||
@ -774,6 +810,7 @@ float AC_AttitudeControl::max_rate_step_bf_pitch()
|
||||
}
|
||||
|
||||
// Maximum yaw rate step size that results in maximum output after 4 time steps
|
||||
// NOTE: for legacy reasons, the output of this function is in centidegrees
|
||||
float AC_AttitudeControl::max_rate_step_bf_yaw()
|
||||
{
|
||||
float alpha = _pid_rate_yaw.get_filt_alpha();
|
||||
|
||||
@ -17,13 +17,14 @@
|
||||
|
||||
// To-Do: change the name or move to AP_Math?
|
||||
#define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
|
||||
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN 36000.0f // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
|
||||
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX 72000.0f // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
|
||||
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN 9000.0f // minimum body-frame acceleration limit for the stability controller (for yaw axis)
|
||||
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX 36000.0f // maximum body-frame acceleration limit for the stability controller (for yaw axis)
|
||||
#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT 1000 // 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 110000.0f // default maximum acceleration for roll/pitch axis in centi-degrees/sec/sec
|
||||
#define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT 27000.0f // default maximum acceleration for yaw axis in centi-degrees/sec/sec
|
||||
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(360.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(90.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)
|
||||
// BUG: SLEW_YAW's behavior does not match its parameter documentation
|
||||
#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS 1000 // 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 centi-degrees/sec/sec
|
||||
#define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS 27000.0f // default maximum acceleration for yaw axis in centi-degrees/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 5000.0f // body-frame rate controller maximum output (for roll-pitch axis)
|
||||
@ -31,10 +32,10 @@
|
||||
#define AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX 4500.0f // earth-frame angle controller maximum output (for yaw axis)
|
||||
#define AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX 4500.0f // earth-frame angle controller maximum input angle (To-Do: replace with reference to aparm.angle_max)
|
||||
|
||||
#define AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX 30000.0f // earth-frame rate stabilize controller's maximum overshoot angle (never limited)
|
||||
#define AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX 30000.0f // earth-frame rate stabilize controller's maximum overshoot angle (never limited)
|
||||
#define AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX 1000.0f // earth-frame rate stabilize controller's maximum overshoot angle
|
||||
#define AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle
|
||||
#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_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
|
||||
@ -62,7 +63,7 @@ public:
|
||||
_pid_rate_yaw(pid_rate_yaw),
|
||||
_dt(AC_ATTITUDE_100HZ_DT),
|
||||
_angle_boost(0),
|
||||
_acro_angle_switch(0),
|
||||
_acro_angle_switch_rad(0),
|
||||
_throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
@ -81,40 +82,40 @@ public:
|
||||
// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
|
||||
void set_dt(float delta_sec);
|
||||
|
||||
// get_accel_roll_max - gets the roll acceleration limit
|
||||
// get_accel_roll_max - gets the roll acceleration limit in centidegrees/s/s
|
||||
float get_accel_roll_max() { return _accel_roll_max; }
|
||||
|
||||
// set_accel_roll_max - sets the roll acceleration limit
|
||||
// set_accel_roll_max - sets the roll acceleration limit in centidegrees/s/s
|
||||
void set_accel_roll_max(float accel_roll_max) { _accel_roll_max = accel_roll_max; }
|
||||
|
||||
// save_accel_roll_max - sets and saves the roll acceleration limit
|
||||
// save_accel_roll_max - sets and saves the roll acceleration limit in centidegrees/s/s
|
||||
void save_accel_roll_max(float accel_roll_max) { _accel_roll_max = accel_roll_max; _accel_roll_max.save(); }
|
||||
|
||||
// get_accel_pitch_max - gets the pitch acceleration limit
|
||||
// get_accel_pitch_max - gets the pitch acceleration limit in centidegrees/s/s
|
||||
float get_accel_pitch_max() { return _accel_pitch_max; }
|
||||
|
||||
// set_accel_pitch_max - sets the pitch acceleration limit
|
||||
// set_accel_pitch_max - sets the pitch acceleration limit in centidegrees/s/s
|
||||
void set_accel_pitch_max(float accel_pitch_max) { _accel_pitch_max = accel_pitch_max; }
|
||||
|
||||
// save_accel_pitch_max - sets and saves the pitch acceleration limit
|
||||
// save_accel_pitch_max - sets and saves the pitch acceleration limit in centidegrees/s/s
|
||||
void save_accel_pitch_max(float accel_pitch_max) { _accel_pitch_max = accel_pitch_max; _accel_pitch_max.save(); }
|
||||
|
||||
// get_accel_yaw_max - gets the yaw acceleration limit
|
||||
// get_accel_yaw_max - gets the yaw acceleration limit in centidegrees/s/s
|
||||
float get_accel_yaw_max() { return _accel_yaw_max; }
|
||||
|
||||
// set_accel_yaw_max - sets the yaw acceleration limit
|
||||
// set_accel_yaw_max - sets the yaw acceleration limit in centidegrees/s/s
|
||||
void set_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; }
|
||||
|
||||
// save_accel_yaw_max - sets and saves the yaw acceleration limit
|
||||
// save_accel_yaw_max - 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(); }
|
||||
|
||||
// relax_bf_rate_controller - ensure body-frame rate controller has zero errors to relax rate controller output
|
||||
void relax_bf_rate_controller();
|
||||
|
||||
// set_yaw_target_to_current_heading - sets yaw target to current heading
|
||||
void set_yaw_target_to_current_heading() { _angle_ef_target.z = _ahrs.yaw_sensor; }
|
||||
void set_yaw_target_to_current_heading() { _angle_ef_target_rad.z = _ahrs.yaw; }
|
||||
|
||||
// shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centi-degreesa and is added to the current target heading
|
||||
// shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centi-degrees and is added to the current target heading
|
||||
void shift_ef_yaw_target(float yaw_shift_cd);
|
||||
|
||||
//
|
||||
@ -123,20 +124,20 @@ public:
|
||||
|
||||
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
|
||||
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
|
||||
void angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain);
|
||||
void angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_rate_ef_cds, float smoothing_gain);
|
||||
|
||||
// angle_ef_roll_pitch_rate_ef_yaw - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)
|
||||
void angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef);
|
||||
void angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_rate_ef_cds);
|
||||
|
||||
// angle_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw angle (all earth frame)
|
||||
// if yaw_slew is true then target yaw movement will be gradually moved to the new target based on the YAW_SLEW parameter
|
||||
void angle_ef_roll_pitch_yaw(float roll_angle_ef, float pitch_angle_ef, float yaw_angle_ef, bool slew_yaw);
|
||||
void angle_ef_roll_pitch_yaw(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_angle_ef_cd, bool slew_yaw);
|
||||
|
||||
// rate_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all earth frame)
|
||||
void rate_ef_roll_pitch_yaw(float roll_rate_ef, float pitch_rate_ef, float yaw_rate_ef);
|
||||
void rate_ef_roll_pitch_yaw(float roll_rate_ef_cds, float pitch_rate_ef_cds, float yaw_rate_ef_cds);
|
||||
|
||||
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame)
|
||||
virtual void rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf);
|
||||
virtual void rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
|
||||
|
||||
//
|
||||
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
|
||||
@ -162,32 +163,36 @@ public:
|
||||
void limit_angle_to_rate_request(bool limit_request) { _flags.limit_angle_to_rate_request = limit_request; }
|
||||
|
||||
// angle_ef_targets - returns angle controller earth-frame targets (for reporting)
|
||||
const Vector3f& angle_ef_targets() const { return _angle_ef_target; }
|
||||
// convert from radians to centi-degrees on public interface
|
||||
Vector3f angle_ef_targets() const { return _angle_ef_target_rad*degrees(100.0f); }
|
||||
|
||||
// angle_bf_error - returns angle controller body-frame errors (for stability checking)
|
||||
const Vector3f& angle_bf_error() const { return _angle_bf_error; }
|
||||
// convert from radians to centi-degrees on public interface
|
||||
Vector3f angle_bf_error() const { return _angle_bf_error_rad*degrees(100.0f); }
|
||||
|
||||
// rate_bf_targets - sets rate controller body-frame targets
|
||||
void rate_bf_roll_target(float rate_cds) { _rate_bf_target.x = rate_cds; }
|
||||
void rate_bf_pitch_target(float rate_cds) { _rate_bf_target.y = rate_cds; }
|
||||
void rate_bf_yaw_target(float rate_cds) { _rate_bf_target.z = rate_cds; }
|
||||
// convert from centi-degrees on public interface to radians
|
||||
void rate_bf_roll_target(float rate_cds) { _rate_bf_target_rads.x = radians(rate_cds*0.01f); }
|
||||
void rate_bf_pitch_target(float rate_cds) { _rate_bf_target_rads.y = radians(rate_cds*0.01f); }
|
||||
void rate_bf_yaw_target(float rate_cds) { _rate_bf_target_rads.z = radians(rate_cds*0.01f); }
|
||||
|
||||
// Maximum roll rate step size that results in maximum output after 4 time steps
|
||||
// Maximum roll rate step size in centi-degrees/s that results in maximum output after 4 time steps
|
||||
float max_rate_step_bf_roll();
|
||||
// Maximum pitch rate step size that results in maximum output after 4 time steps
|
||||
// Maximum pitch rate step size in centi-degrees/s that results in maximum output after 4 time steps
|
||||
float max_rate_step_bf_pitch();
|
||||
// Maximum yaw rate step size that results in maximum output after 4 time steps
|
||||
// Maximum yaw rate step size in centi-degrees/s that results in maximum output after 4 time steps
|
||||
float max_rate_step_bf_yaw();
|
||||
|
||||
// Maximum roll step size that results in maximum output after 4 time steps
|
||||
// Maximum roll step size in centi-degrees that results in maximum output after 4 time steps
|
||||
float max_angle_step_bf_roll() { return max_rate_step_bf_roll()/_p_angle_roll.kP(); }
|
||||
// Maximum pitch step size that results in maximum output after 4 time steps
|
||||
// Maximum pitch step size in centi-degrees that results in maximum output after 4 time steps
|
||||
float max_angle_step_bf_pitch() { return max_rate_step_bf_pitch()/_p_angle_pitch.kP(); }
|
||||
// Maximum yaw step size that results in maximum output after 4 time steps
|
||||
// Maximum yaw step size in centi-degrees 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(); }
|
||||
|
||||
// rate_ef_targets - returns rate controller body-frame targets (for reporting)
|
||||
const Vector3f& rate_bf_targets() const { return _rate_bf_target; }
|
||||
// convert from radians/s to centi-degrees/s on public interface
|
||||
Vector3f rate_bf_targets() const { return _rate_bf_target_rads*degrees(100.0f); }
|
||||
|
||||
// bf_feedforward - enable or disable body-frame feed forward
|
||||
void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled = enable_or_disable; }
|
||||
@ -238,31 +243,31 @@ protected:
|
||||
} _flags;
|
||||
|
||||
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
|
||||
void update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max);
|
||||
void update_ef_roll_angle_and_error(float roll_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
|
||||
|
||||
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request
|
||||
void update_ef_pitch_angle_and_error(float pitch_rate_ef, Vector3f &angle_ef_error, float overshoot_max);
|
||||
void update_ef_pitch_angle_and_error(float pitch_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
|
||||
|
||||
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request
|
||||
void update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error, float overshoot_max);
|
||||
void update_ef_yaw_angle_and_error(float yaw_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
|
||||
|
||||
// integrate_bf_rate_error_to_angle_errors - calculates body frame angle errors
|
||||
// body-frame feed forward rates (centi-degrees / second) taken from _angle_bf_error
|
||||
// angle errors in centi-degrees placed in _angle_bf_error
|
||||
// body-frame feed forward rates (radians/s) taken from _angle_bf_error
|
||||
// angle errors in radians placed in _angle_bf_error
|
||||
void integrate_bf_rate_error_to_angle_errors();
|
||||
|
||||
// update_rate_bf_targets - converts body-frame angle error to body-frame rate targets for roll, pitch and yaw axis
|
||||
// targets rates in centi-degrees taken from _angle_bf_error
|
||||
// results in centi-degrees/sec put into _rate_bf_target
|
||||
// targets rates in radians taken from _angle_bf_error
|
||||
// results in radians/s put into _rate_bf_target
|
||||
void update_rate_bf_targets();
|
||||
|
||||
//
|
||||
// body-frame rate controller
|
||||
//
|
||||
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in centi-degrees/sec) for roll, pitch and yaw
|
||||
float rate_bf_to_motor_roll(float rate_target_cds);
|
||||
float rate_bf_to_motor_pitch(float rate_target_cds);
|
||||
virtual float rate_bf_to_motor_yaw(float rate_target_cds);
|
||||
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in radians/s) for roll, pitch and yaw
|
||||
float rate_bf_to_motor_roll(float rate_target_rads);
|
||||
float rate_bf_to_motor_pitch(float rate_target_rads);
|
||||
virtual float rate_bf_to_motor_yaw(float rate_target_rads);
|
||||
|
||||
//
|
||||
// throttle methods
|
||||
@ -273,7 +278,13 @@ protected:
|
||||
|
||||
// get_roll_trim - angle in centi-degrees to be added to roll angle. Used by helicopter to counter tail rotor thrust in hover
|
||||
// Overloaded by AC_Attitude_Heli to return angle. Should be left to return zero for multirotors.
|
||||
virtual int16_t get_roll_trim() { return 0;}
|
||||
virtual int16_t get_roll_trim_rad() { return 0;}
|
||||
|
||||
float get_accel_roll_max_radss() { return radians(_accel_roll_max*0.01f); }
|
||||
float get_accel_pitch_max_radss() { return radians(_accel_pitch_max*0.01f); }
|
||||
float get_accel_yaw_max_radss() { return radians(_accel_yaw_max*0.01f); }
|
||||
float get_slew_yaw_rads() { return radians(_slew_yaw*0.01f); }
|
||||
float get_tilt_limit_rad() { return radians(_aparm.angle_max*0.01f); }
|
||||
|
||||
// references to external libraries
|
||||
const AP_AHRS& _ahrs;
|
||||
@ -296,13 +307,13 @@ protected:
|
||||
// internal variables
|
||||
// To-Do: make rate targets a typedef instead of Vector3f?
|
||||
float _dt; // time delta in seconds
|
||||
Vector3f _angle_ef_target; // angle controller earth-frame targets
|
||||
Vector3f _angle_bf_error; // angle controller body-frame error
|
||||
Vector3f _rate_bf_target; // rate controller body-frame targets
|
||||
Vector3f _rate_ef_desired; // earth-frame feed forward rates
|
||||
Vector3f _rate_bf_desired; // body-frame feed forward rates
|
||||
int16_t _angle_boost; // used only for logging
|
||||
int16_t _acro_angle_switch; // used only for logging
|
||||
Vector3f _angle_ef_target_rad; // angle controller earth-frame targets
|
||||
Vector3f _angle_bf_error_rad; // angle controller body-frame error
|
||||
Vector3f _rate_bf_target_rads; // rate controller body-frame targets
|
||||
Vector3f _rate_ef_desired_rads; // earth-frame feed forward rates
|
||||
Vector3f _rate_bf_desired_rads; // body-frame feed forward rates
|
||||
int16_t _angle_boost; // used only for logging
|
||||
float _acro_angle_switch_rad;
|
||||
|
||||
// throttle based angle limits
|
||||
LowPassFilterFloat _throttle_in_filt; // throttle input from pilot or alt hold controller
|
||||
|
||||
@ -27,63 +27,67 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
|
||||
};
|
||||
|
||||
// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
|
||||
void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf)
|
||||
void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds)
|
||||
{
|
||||
// convert from centidegrees on public interface to radians
|
||||
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
|
||||
|
||||
// store roll, pitch and passthroughs
|
||||
// NOTE: this abuses yaw_rate_bf_rads
|
||||
_passthrough_roll = roll_passthrough;
|
||||
_passthrough_pitch = pitch_passthrough;
|
||||
_passthrough_yaw = yaw_rate_bf;
|
||||
_passthrough_yaw = degrees(yaw_rate_bf_rads)*100.0f;
|
||||
|
||||
// set rate controller to use pass through
|
||||
_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)
|
||||
_rate_bf_desired.x = _ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100;
|
||||
_rate_bf_desired.y = _ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100;
|
||||
_rate_bf_desired_rads.x = _ahrs.get_gyro().x;
|
||||
_rate_bf_desired_rads.y = _ahrs.get_gyro().y;
|
||||
|
||||
// accel limit desired yaw rate
|
||||
if (_accel_yaw_max > 0.0f) {
|
||||
float rate_change_limit = _accel_yaw_max * _dt;
|
||||
float rate_change = yaw_rate_bf - _rate_bf_desired.z;
|
||||
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
|
||||
_rate_bf_desired.z += rate_change;
|
||||
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 - _rate_bf_desired_rads.z;
|
||||
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
|
||||
_rate_bf_desired_rads.z += rate_change_rads;
|
||||
} else {
|
||||
_rate_bf_desired.z = yaw_rate_bf;
|
||||
_rate_bf_desired_rads.z = yaw_rate_bf_rads;
|
||||
}
|
||||
|
||||
integrate_bf_rate_error_to_angle_errors();
|
||||
_angle_bf_error.x = 0;
|
||||
_angle_bf_error.y = 0;
|
||||
_angle_bf_error_rad.x = 0;
|
||||
_angle_bf_error_rad.y = 0;
|
||||
|
||||
// update our earth-frame angle targets
|
||||
Vector3f angle_ef_error;
|
||||
if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) {
|
||||
_angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
|
||||
_angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
|
||||
_angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
|
||||
Vector3f angle_ef_error_rad;
|
||||
if (frame_conversion_bf_to_ef(_angle_bf_error_rad, angle_ef_error_rad)) {
|
||||
_angle_ef_target_rad.x = wrap_PI(angle_ef_error_rad.x + _ahrs.roll);
|
||||
_angle_ef_target_rad.y = wrap_PI(angle_ef_error_rad.y + _ahrs.pitch);
|
||||
_angle_ef_target_rad.z = wrap_2PI(angle_ef_error_rad.z + _ahrs.yaw);
|
||||
}
|
||||
|
||||
// handle flipping over pitch axis
|
||||
if (_angle_ef_target.y > 9000.0f) {
|
||||
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
|
||||
_angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
|
||||
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
|
||||
if (_angle_ef_target_rad.y > M_PI/2.0f) {
|
||||
_angle_ef_target_rad.x = wrap_PI(_angle_ef_target_rad.x + M_PI);
|
||||
_angle_ef_target_rad.y = wrap_PI(M_PI - _angle_ef_target_rad.x);
|
||||
_angle_ef_target_rad.z = wrap_2PI(_angle_ef_target_rad.z + M_PI);
|
||||
}
|
||||
if (_angle_ef_target.y < -9000.0f) {
|
||||
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
|
||||
_angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x);
|
||||
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
|
||||
if (_angle_ef_target_rad.y < -M_PI/2.0f) {
|
||||
_angle_ef_target_rad.x = wrap_PI(_angle_ef_target_rad.x + M_PI);
|
||||
_angle_ef_target_rad.y = wrap_PI(-M_PI - _angle_ef_target_rad.x);
|
||||
_angle_ef_target_rad.z = wrap_2PI(_angle_ef_target_rad.z + M_PI);
|
||||
}
|
||||
|
||||
// convert body-frame angle errors to body-frame rate targets
|
||||
update_rate_bf_targets();
|
||||
|
||||
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
|
||||
_rate_bf_target.x = _rate_bf_desired.x;
|
||||
_rate_bf_target.y = _rate_bf_desired.y;
|
||||
_rate_bf_target_rads.x = _rate_bf_desired_rads.x;
|
||||
_rate_bf_target_rads.y = _rate_bf_desired_rads.y;
|
||||
|
||||
// add desired target to yaw
|
||||
_rate_bf_target.z += _rate_bf_desired.z;
|
||||
_rate_bf_target_rads.z += _rate_bf_desired_rads.z;
|
||||
}
|
||||
|
||||
// subclass non-passthrough too, for external gyro, no flybar
|
||||
@ -108,12 +112,12 @@ void AC_AttitudeControl_Heli::rate_controller_run()
|
||||
_motors.set_roll(_passthrough_roll);
|
||||
_motors.set_pitch(_passthrough_pitch);
|
||||
} else {
|
||||
rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y);
|
||||
rate_bf_to_motor_roll_pitch(_rate_bf_target_rads.x, _rate_bf_target_rads.y);
|
||||
}
|
||||
if (_flags_heli.tail_passthrough) {
|
||||
_motors.set_yaw(_passthrough_yaw);
|
||||
} else {
|
||||
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z));
|
||||
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target_rads.z));
|
||||
}
|
||||
}
|
||||
|
||||
@ -121,7 +125,10 @@ void AC_AttitudeControl_Heli::rate_controller_run()
|
||||
float AC_AttitudeControl_Heli::get_althold_lean_angle_max() const
|
||||
{
|
||||
// calc maximum tilt angle based on throttle
|
||||
return ToDeg(acos(constrain_float(_throttle_in_filt.get()/900.0f, 0.0f, 1000.0f) / 1000.0f)) * 100.0f;
|
||||
float ret = acosf(constrain_float(_throttle_in_filt.get()/900.0f, 0.0f, 1000.0f) / 1000.0f);
|
||||
|
||||
// TEMP: convert to centi-degrees for public interface
|
||||
return degrees(ret) * 100.0f;
|
||||
}
|
||||
|
||||
//
|
||||
@ -132,24 +139,24 @@ float AC_AttitudeControl_Heli::get_althold_lean_angle_max() const
|
||||
// body-frame rate controller
|
||||
//
|
||||
|
||||
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
|
||||
void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target_cds, float rate_pitch_target_cds)
|
||||
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second
|
||||
void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target_rads, float rate_pitch_target_rads)
|
||||
{
|
||||
float roll_pd, roll_i, roll_ff; // used to capture pid values
|
||||
float pitch_pd, pitch_i, pitch_ff; // used to capture pid values
|
||||
float rate_roll_error, rate_pitch_error; // simply target_rate - current_rate
|
||||
float rate_roll_error_rads, rate_pitch_error_rads; // simply target_rate - current_rate
|
||||
float roll_out, pitch_out;
|
||||
const Vector3f& gyro = _ahrs.get_gyro(); // get current rates
|
||||
|
||||
// calculate error
|
||||
rate_roll_error = rate_roll_target_cds - gyro.x * AC_ATTITUDE_CONTROL_DEGX100;
|
||||
rate_pitch_error = rate_pitch_target_cds - gyro.y * AC_ATTITUDE_CONTROL_DEGX100;
|
||||
rate_roll_error_rads = rate_roll_target_rads - gyro.x;
|
||||
rate_pitch_error_rads = rate_pitch_target_rads - gyro.y;
|
||||
|
||||
// input to PID controller
|
||||
_pid_rate_roll.set_input_filter_all(rate_roll_error);
|
||||
_pid_rate_roll.set_desired_rate(rate_roll_target_cds);
|
||||
_pid_rate_pitch.set_input_filter_all(rate_pitch_error);
|
||||
_pid_rate_pitch.set_desired_rate(rate_pitch_target_cds);
|
||||
// For legacy reasons, we convert to centi-degrees before inputting to the PID
|
||||
_pid_rate_roll.set_input_filter_all(degrees(rate_roll_error_rads)*100.0f);
|
||||
_pid_rate_roll.set_desired_rate(degrees(rate_roll_target_rads)*100.0f);
|
||||
_pid_rate_pitch.set_input_filter_all(degrees(rate_pitch_error_rads)*100.0f);
|
||||
_pid_rate_pitch.set_desired_rate(degrees(rate_pitch_target_rads)*100.0f);
|
||||
|
||||
// call p and d controllers
|
||||
roll_pd = _pid_rate_roll.get_p() + _pid_rate_roll.get_d();
|
||||
@ -159,7 +166,7 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
|
||||
roll_i = _pid_rate_roll.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!_flags_heli.limit_roll || ((roll_i>0&&rate_roll_error<0)||(roll_i<0&&rate_roll_error>0))){
|
||||
if (!_flags_heli.limit_roll || ((roll_i>0&&rate_roll_error_rads<0)||(roll_i<0&&rate_roll_error_rads>0))){
|
||||
if (_flags_heli.leaky_i){
|
||||
roll_i = ((AC_HELI_PID&)_pid_rate_roll).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
|
||||
}else{
|
||||
@ -171,7 +178,7 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
|
||||
pitch_i = _pid_rate_pitch.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!_flags_heli.limit_pitch || ((pitch_i>0&&rate_pitch_error<0)||(pitch_i<0&&rate_pitch_error>0))){
|
||||
if (!_flags_heli.limit_pitch || ((pitch_i>0&&rate_pitch_error_rads<0)||(pitch_i<0&&rate_pitch_error_rads>0))){
|
||||
if (_flags_heli.leaky_i) {
|
||||
pitch_i = ((AC_HELI_PID&)_pid_rate_pitch).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
|
||||
}else{
|
||||
@ -179,8 +186,9 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
|
||||
}
|
||||
}
|
||||
|
||||
roll_ff = roll_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_roll).get_vff(rate_roll_target_cds), _dt);
|
||||
pitch_ff = pitch_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_pitch).get_vff(rate_pitch_target_cds), _dt);
|
||||
// For legacy reasons, we convert to centi-degrees before inputting to the feedforward
|
||||
roll_ff = roll_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_roll).get_vff(degrees(rate_roll_target_rads)*100.0f), _dt);
|
||||
pitch_ff = pitch_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_pitch).get_vff(degrees(rate_pitch_target_rads)*100.0f), _dt);
|
||||
|
||||
// add feed forward and final output
|
||||
roll_out = roll_pd + roll_i + roll_ff;
|
||||
@ -229,24 +237,24 @@ 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 centi-degrees / second
|
||||
float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
|
||||
// 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 pd,i,vff,aff; // used to capture pid values for logging
|
||||
float current_rate; // this iteration's rate
|
||||
float rate_error; // simply target_rate - current_rate
|
||||
float current_rate_rads; // this iteration's rate
|
||||
float rate_error_rads; // simply target_rate - current_rate
|
||||
float yaw_out;
|
||||
|
||||
// get current rate
|
||||
// To-Do: make getting gyro rates more efficient?
|
||||
current_rate = (_ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100);
|
||||
current_rate_rads = _ahrs.get_gyro().z;
|
||||
|
||||
// calculate error and call pid controller
|
||||
rate_error = rate_target_cds - current_rate;
|
||||
rate_error_rads = rate_target_rads - current_rate_rads;
|
||||
|
||||
// send input to PID controller
|
||||
_pid_rate_yaw.set_input_filter_all(rate_error);
|
||||
_pid_rate_yaw.set_desired_rate(rate_target_cds);
|
||||
// For legacy reasons, we convert to centi-degrees before inputting to the PID
|
||||
_pid_rate_yaw.set_input_filter_all(degrees(rate_error_rads)*100.0f);
|
||||
_pid_rate_yaw.set_desired_rate(degrees(rate_target_rads)*100.0f);
|
||||
|
||||
// get p and d
|
||||
pd = _pid_rate_yaw.get_p() + _pid_rate_yaw.get_d();
|
||||
@ -255,7 +263,7 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
|
||||
i = _pid_rate_yaw.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!_flags_heli.limit_yaw || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
|
||||
if (!_flags_heli.limit_yaw || ((i>0&&rate_error_rads<0)||(i<0&&rate_error_rads>0))) {
|
||||
if (((AP_MotorsHeli&)_motors).rotor_runup_complete()) {
|
||||
i = _pid_rate_yaw.get_i();
|
||||
} else {
|
||||
@ -263,8 +271,9 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
|
||||
}
|
||||
}
|
||||
|
||||
vff = yaw_velocity_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_yaw).get_vff(rate_target_cds), _dt);
|
||||
aff = yaw_acceleration_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_yaw).get_aff(rate_target_cds), _dt);
|
||||
// For legacy reasons, we convert to centi-degrees before inputting to the feedforward
|
||||
vff = yaw_velocity_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_yaw).get_vff(degrees(rate_target_rads)*100.0f), _dt);
|
||||
aff = yaw_acceleration_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_yaw).get_aff(degrees(rate_target_rads)*100.0f), _dt);
|
||||
|
||||
// add feed forward
|
||||
yaw_out = pd + i + vff + aff;
|
||||
|
||||
@ -47,7 +47,7 @@ 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);
|
||||
void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds);
|
||||
|
||||
// subclass non-passthrough too, for external gyro, no flybar
|
||||
void rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf) override;
|
||||
@ -57,6 +57,7 @@ public:
|
||||
virtual void rate_controller_run();
|
||||
|
||||
// get lean angle max for pilot input that prioritises altitude hold over lean angle
|
||||
// NOTE: returns centi-degrees
|
||||
float get_althold_lean_angle_max() const;
|
||||
|
||||
// use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage
|
||||
@ -94,10 +95,10 @@ private:
|
||||
//
|
||||
// body-frame rate controller
|
||||
//
|
||||
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in centi-degrees/sec) for roll, pitch and yaw
|
||||
// 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_cds, float rate_pitch_target_cds);
|
||||
virtual float rate_bf_to_motor_yaw(float rate_yaw_cds);
|
||||
void rate_bf_to_motor_roll_pitch(float rate_roll_target_rads, float rate_pitch_target_rads);
|
||||
virtual float rate_bf_to_motor_yaw(float rate_yaw_rads);
|
||||
|
||||
//
|
||||
// throttle methods
|
||||
@ -114,7 +115,7 @@ private:
|
||||
int16_t _passthrough_yaw;
|
||||
|
||||
// get_roll_trim - angle in centi-degrees to be added to roll angle. Used by helicopter to counter tail rotor thrust in hover
|
||||
int16_t get_roll_trim() { return constrain_int16(_hover_roll_trim_scalar * _hover_roll_trim, -1000, 1000);}
|
||||
int16_t get_roll_trim_rad() { return radians(constrain_int16(_hover_roll_trim_scalar * _hover_roll_trim, -1000, 1000)*0.01f);}
|
||||
|
||||
// internal variables
|
||||
float _hover_roll_trim_scalar = 0; // scalar used to suppress Hover Roll Trim
|
||||
|
||||
Loading…
Reference in New Issue
Block a user