From 7330de86e55fbb462577f11aad1118985782b25d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 24 Nov 2015 13:11:50 -0800 Subject: [PATCH] AC_AttitudeControl: change internals to use radians instead of centidegrees --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 465 ++++++++++-------- .../AC_AttitudeControl/AC_AttitudeControl.h | 129 ++--- .../AC_AttitudeControl_Heli.cpp | 123 ++--- .../AC_AttitudeControl_Heli.h | 11 +- 4 files changed, 393 insertions(+), 335 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 1a08154d82..75838a50e7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -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(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index d18d049d63..df14ffc61f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -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 diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index da4b6ac08c..01bff89880 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -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; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index d2ca8e5bc8..2b0e7be439 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -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