From 98224db1e4a6f420b2cf1516f7d44d2d01ad4088 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 10 Feb 2014 11:59:51 +0900 Subject: [PATCH] AC_AttControl: Leaonard's rate feedforward Also saves 24bytes of RAM --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 481 ++++++++---------- .../AC_AttitudeControl/AC_AttitudeControl.h | 106 ++-- 2 files changed, 248 insertions(+), 339 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index c634b69e79..014cc11461 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -51,30 +51,58 @@ void AC_AttitudeControl::init_targets() _angle_ef_target.z = _ahrs.yaw_sensor; // clear body frame angle errors - _rate_stab_bf_error.x = 0; - _rate_stab_bf_error.y = 0; - _rate_stab_bf_error.z = 0; + _angle_bf_error.x = 0; + _angle_bf_error.y = 0; + _angle_bf_error.z = 0; + + // clear body frame feed forward rates + _rate_bf_feedforward.x = 0; + _rate_bf_feedforward.y = 0; + _rate_bf_feedforward.z = 0; } +// +// methods to be called by upper controllers to request and implement a desired attitude +// + // angleef_rp_rateef_y - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) void AC_AttitudeControl::angleef_rp_rateef_y(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef) { - // set earth-frame angle targets + Vector3f rate_ef_feedforward; // earth frame feedforward rate + Vector3f angle_ef_error; // earth frame angle errors + + // set earth-frame angle targets for roll and pitch and calculate angle error _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.y = pitch_angle_ef; + angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor); - // convert earth-frame angle targets to earth-frame rate targets - angle_to_rate_ef_roll(); - angle_to_rate_ef_pitch(); + // set earth-frame feed forward rate for yaw + rate_ef_feedforward.z = yaw_rate_ef; - // set earth-frame rate stabilize target for yaw - _rate_stab_ef_target.z = yaw_rate_ef; + // increment the yaw angle target + _angle_ef_target.z += yaw_rate_ef * _dt; + _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z); - // convert earth-frame stabilize rate to regular rate target - rate_stab_ef_to_rate_ef_yaw(); + // calculate angle error with maximum of +- max_angle_overshoot + angle_ef_error.z = wrap_180_cd_float(_angle_ef_target.z - _ahrs.yaw_sensor); + angle_ef_error.z = constrain_float(angle_ef_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); - // convert earth-frame rates to body-frame rates - rate_ef_targets_to_bf(_rate_ef_target, _rate_bf_target); + // update yaw angle target to be within max_angle_overshoot of our current heading + _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); + + // convert earth-frame angle errors to body-frame angle errors + rate_ef_targets_to_bf(angle_ef_error, _angle_bf_error); + + // convert earth-frame feed forward rates to body-frame feed forward rates + rate_ef_targets_to_bf(rate_ef_feedforward, _rate_bf_feedforward); + + // convert body-frame angle errors to body-frame rate targets + update_stab_rate_bf_targets(); + + // add body frame rate feed forward + _rate_bf_target += _rate_bf_feedforward; // body-frame to motor outputs should be called separately } @@ -83,40 +111,101 @@ void AC_AttitudeControl::angleef_rp_rateef_y(float roll_angle_ef, float pitch_an // 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::angleef_rpy(float roll_angle_ef, float pitch_angle_ef, float yaw_angle_ef, bool slew_yaw) { + Vector3f angle_ef_error; + // set earth-frame angle targets _angle_ef_target.x = roll_angle_ef; _angle_ef_target.y = pitch_angle_ef; - if (slew_yaw && _angle_ef_target.z != yaw_angle_ef) { - float slew = _slew_yaw * _dt; - _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + constrain_float(wrap_180_cd_float(yaw_angle_ef - _angle_ef_target.z), -slew, slew)); + _angle_ef_target.z = yaw_angle_ef; + + // 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); + + // convert earth-frame angle errors to body-frame angle errors + rate_ef_targets_to_bf(angle_ef_error, _angle_bf_error); + + // convert body-frame angle errors to body-frame rate targets + update_stab_rate_bf_targets(); + + if (slew_yaw) { + _rate_bf_target.z = constrain_float(_rate_bf_target.z,-_slew_yaw,_slew_yaw); } - // convert earth-frame angle targets to earth-frame rate targets - angle_to_rate_ef_roll(); - angle_to_rate_ef_pitch(); - angle_to_rate_ef_yaw(); - - // convert earth-frame rates to body-frame rates - rate_ef_targets_to_bf(_rate_ef_target, _rate_bf_target); - // body-frame to motor outputs should be called separately } // rateef_rpy - attempts to maintain a roll, pitch and yaw rate (all earth frame) void AC_AttitudeControl::rateef_rpy(float roll_rate_ef, float pitch_rate_ef, float yaw_rate_ef) { - // set stabilized earth-frame rate targets - _rate_stab_ef_target.x = roll_rate_ef; - _rate_stab_ef_target.y = pitch_rate_ef; - _rate_stab_ef_target.z = yaw_rate_ef; + Vector3f rate_ef_feedforward; + Vector3f angle_ef_error; - // convert stabilized earth-frame rates to (regular) earth-frames rates - rate_stab_ef_to_rate_ef_roll(); - rate_stab_ef_to_rate_ef_pitch(); - rate_stab_ef_to_rate_ef_yaw(); + // update the rate feed forward + rate_ef_feedforward.x = roll_rate_ef; + rate_ef_feedforward.y = pitch_rate_ef; + rate_ef_feedforward.z = yaw_rate_ef; + + // increment the roll angle target + _angle_ef_target.x += roll_rate_ef * _dt; + _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x); + + // ensure targets are within the lean angle limits + // To-Do: make angle_max part of the AP_Vehicle class + _angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max); + + // 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, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle check for traditional heli's motors.motor_runup_complete + // To-Do: reset target angle to current angle if motors not spinning + + // update roll angle target to be within max angle overshoot of our roll angle + _angle_ef_target.x = wrap_180_cd(angle_ef_error.x + _ahrs.roll_sensor); + + // increment the pitch angle target + _angle_ef_target.y += pitch_rate_ef * _dt; + _angle_ef_target.y = wrap_180_cd(_angle_ef_target.y); + + // ensure targets are within the lean angle limits + // To-Do: make angle_max part of the AP_Vehicle class + _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max); + + // 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, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle check for traditional heli's motors.motor_runup_complete + // To-Do: reset target angle to current angle if motors not spinning + + // update pitch angle target to be within max angle overshoot of our pitch angle + _angle_ef_target.y = wrap_180_cd(angle_ef_error.y + _ahrs.pitch_sensor); + + // increment the yaw angle target + _angle_ef_target.z += yaw_rate_ef * _dt; + _angle_ef_target.z = wrap_360_cd(_angle_ef_target.z); + + // 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, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); + + // update yaw angle target to be within max angle overshoot of our current heading + _angle_ef_target.z = wrap_360_cd(angle_ef_error.z + _ahrs.yaw_sensor); + + // convert earth-frame angle errors to body-frame angle errors + rate_ef_targets_to_bf(angle_ef_error, _angle_bf_error); // convert earth-frame rates to body-frame rates - rate_ef_targets_to_bf(_rate_ef_target, _rate_bf_target); + rate_ef_targets_to_bf(rate_ef_feedforward, _rate_bf_feedforward); + + // convert body-frame angle errors to body-frame rate targets + update_stab_rate_bf_targets(); + + // add body frame rate feed forward + _rate_bf_target += _rate_bf_feedforward; // body-frame to motor outputs should be called separately } @@ -125,246 +214,28 @@ void AC_AttitudeControl::rateef_rpy(float roll_rate_ef, float pitch_rate_ef, flo void AC_AttitudeControl::ratebf_rpy(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf) { // Update angle error - rate_stab_bf_update_error(); - - // set earth-frame angle targets - _rate_stab_bf_target.x = roll_rate_bf; - _rate_stab_bf_target.y = pitch_rate_bf; - _rate_stab_bf_target.z = yaw_rate_bf; + update_stab_rate_bf_errors(); - // convert stabilize rates to regular rates - rate_stab_bf_to_rate_bf_roll(); - rate_stab_bf_to_rate_bf_pitch(); - rate_stab_bf_to_rate_bf_yaw(); + // convert body-frame angle errors to body-frame rate targets + update_stab_rate_bf_targets(); + + // update the rate feed forward + _rate_bf_feedforward.x = roll_rate_bf; + _rate_bf_feedforward.y = pitch_rate_bf; + _rate_bf_feedforward.z = yaw_rate_bf; + + // body-frame rate commands added + _rate_bf_target += _rate_bf_feedforward; // body-frame to motor outputs should be called separately } - // -// angle controller methods +// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors +// should be called at 100hz or more // - -// angle_to_rate_ef_roll - ask the angle controller to calculate the earth frame rate targets for roll -void AC_AttitudeControl::angle_to_rate_ef_roll() -{ - // calculate angle error - // To-Do: is this being converted to int32_t as part of wrap_180_cd? - float angle_error_cd = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor); - - // convert to desired earth-frame rate - // To-Do: replace PI controller with just a single gain? - _rate_ef_target.x = _pi_angle_roll.kP() * angle_error_cd; - - // constrain rate request - if (_flags.limit_angle_to_rate_request) { - _rate_ef_target.x = constrain_float(_rate_ef_target.x,-_angle_rate_rp_max,_angle_rate_rp_max); - } -} - -// angle_to_rate_ef_pitch - ask the angle controller to calculate the earth frame rate targets for pitch -void AC_AttitudeControl::angle_to_rate_ef_pitch() -{ - // calculate angle error - // To-Do: is this being converted to int32_t as part of wrap_180_cd? - float angle_error_cd = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor); - - // convert to desired earth-frame rate - // To-Do: replace PI controller with just a single gain? - _rate_ef_target.y = _pi_angle_pitch.kP() * angle_error_cd; - - // constrain rate request - if (_flags.limit_angle_to_rate_request) { - _rate_ef_target.y = constrain_float(_rate_ef_target.y,-_angle_rate_rp_max,_angle_rate_rp_max); - } -} - -// angle_to_rate_ef_yaw - ask the angle controller to calculate the earth-frame yaw rate in centi-degrees/second -void AC_AttitudeControl::angle_to_rate_ef_yaw() -{ - // calculate angle error - // To-Do: is this being converted to int32_t as part of wrap_180_cd? - float angle_error_cd = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor); - - // convert to desired earth-frame rate in centi-degrees/second - // To-Do: replace PI controller with just a single gain? - _rate_ef_target.z = _pi_angle_yaw.kP() * angle_error_cd; - - // constrain rate request - if (_flags.limit_angle_to_rate_request) { - _rate_ef_target.z = constrain_float(_rate_ef_target.z,-_angle_rate_y_max,_angle_rate_y_max); - } - - // To-Do: deal with trad helicopter which do not use yaw rate controllers if using external gyros -} - -// -// stabilized rate controller (earth-frame) methods -// stabilized rate controllers are better at maintaining a desired rate than the simpler earth-frame rate controllers -// because they also maintain angle-targets and increase/decrease the rate request passed to the earth-frame rate controller -// upon the errors between the actual angle and angle-target. -// -// -// rate_stab_ef_to_rate_ef_roll - converts earth-frame stabilized rate targets to regular earth-frame rate targets for roll, pitch and yaw axis -// targets rates in centi-degrees/second taken from _rate_stab_ef_target -// results in centi-degrees/sec put into _rate_ef_target -void AC_AttitudeControl::rate_stab_ef_to_rate_ef_roll() -{ - float angle_error; - - // convert the input to the desired roll rate - _angle_ef_target.x += _rate_stab_ef_target.x * _dt; - _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x); - - // ensure targets are within the lean angle limits - // To-Do: make angle_max part of the AP_Vehicle class - _angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max); - - // calculate angle error with maximum of +- max_angle_overshoot - angle_error = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor); - angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); - - // To-Do: handle check for traditional heli's motors.motor_runup_complete - // To-Do: reset target angle to current angle if motors not spinning - - // update acro_roll to be within max_angle_overshoot of our current heading - _angle_ef_target.x = wrap_180_cd(angle_error + _ahrs.roll_sensor); - - // set earth frame rate controller targets - _rate_ef_target.x = _pi_angle_roll.get_p(angle_error) + _rate_stab_ef_target.x; -} - -void AC_AttitudeControl::rate_stab_ef_to_rate_ef_pitch() -{ - float angle_error; - - // convert the input to the desired roll rate - _angle_ef_target.y += _rate_stab_ef_target.y * _dt; - _angle_ef_target.y = wrap_180_cd(_angle_ef_target.y); - - // ensure targets are within the lean angle limits - // To-Do: make angle_max part of the AP_Vehicle class - _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max); - - // calculate angle error with maximum of +- max_angle_overshoot - // To-Do: should we do something better as we cross 90 degrees? - angle_error = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor); - angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); - - // To-Do: handle check for traditional heli's motors.motor_runup_complete - // To-Do: reset target angle to current angle if motors not spinning - - // update acro_roll to be within max_angle_overshoot of our current heading - _angle_ef_target.y = wrap_180_cd(angle_error + _ahrs.pitch_sensor); - - // set earth frame rate controller targets - _rate_ef_target.y = _pi_angle_pitch.get_p(angle_error) + _rate_stab_ef_target.y; -} - -void AC_AttitudeControl::rate_stab_ef_to_rate_ef_yaw() -{ - float angle_error; - - // convert the input to the desired roll rate - _angle_ef_target.z += _rate_stab_ef_target.z * _dt; - _angle_ef_target.z = wrap_360_cd(_angle_ef_target.z); - - // calculate angle error with maximum of +- max_angle_overshoot - angle_error = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor); - angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); - - // To-Do: handle check for traditional heli's motors.motor_runup_complete - // To-Do: reset target angle to current angle if motors not spinning - - // update acro_roll to be within max_angle_overshoot of our current heading - _angle_ef_target.z = wrap_360_cd(angle_error + _ahrs.yaw_sensor); - - // set earth frame rate controller targets - _rate_ef_target.z = _pi_angle_yaw.get_p(angle_error) + _rate_stab_ef_target.z; -} - -// -// stabilized rate controller (body-frame) methods -// - - -// rate_stab_bf_to_rate_ef_roll - converts body-frame stabilized rate targets to regular body-frame rate targets for roll, pitch and yaw axis -// targets rates in centi-degrees/second taken from _rate_stab_bf_target -// results in centi-degrees/sec put into _rate_bf_target -void AC_AttitudeControl::rate_stab_bf_update_error() -{ - // roll - calculate body-frame angle error by integrating body-frame rate error - _rate_stab_bf_error.x += (_rate_stab_bf_target.x - (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; - - // roll - limit maximum error - _rate_stab_bf_error.x = constrain_float(_rate_stab_bf_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); - - - // pitch - calculate body-frame angle error by integrating body-frame rate error - _rate_stab_bf_error.y += (_rate_stab_bf_target.y - (_ins.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; - - // pitch - limit maximum error - _rate_stab_bf_error.y = constrain_float(_rate_stab_bf_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); - - - // yaw - calculate body-frame angle error by integrating body-frame rate error - _rate_stab_bf_error.z += (_rate_stab_bf_target.z - (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; - - // yaw - limit maximum error - _rate_stab_bf_error.z = constrain_float(_rate_stab_bf_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); - - // To-Do: handle case of motors being disarmed or g.rc_3.servo_out == 0 and set error to zero -} -void AC_AttitudeControl::rate_stab_bf_to_rate_bf_roll() -{ - // calculate rate correction from angle errors - // To-Do: do we still need to have this rate correction calculated from the previous iteration's errors? - float rate_correction = _pi_angle_roll.get_p(_rate_stab_bf_error.x); - - // set body frame targets for rate controller - _rate_bf_target.x = _rate_stab_bf_target.x + rate_correction; -} - -void AC_AttitudeControl::rate_stab_bf_to_rate_bf_pitch() -{ - // calculate rate correction from angle errors - // To-Do: do we still need to have this rate correction calculated from the previous iteration's errors? - float rate_correction = _pi_angle_pitch.get_p(_rate_stab_bf_error.y); - - // set body frame targets for rate controller - _rate_bf_target.y = _rate_stab_bf_target.y + rate_correction; -} - -void AC_AttitudeControl::rate_stab_bf_to_rate_bf_yaw() -{ - // calculate rate correction from angle errors - float rate_correction = _pi_angle_yaw.get_p(_rate_stab_bf_error.z); - - // set body frame targets for rate controller - _rate_bf_target.y = _rate_stab_bf_target.y + rate_correction; -} - -// -// rate controller (earth-frame) methods -// - -// rate_ef_targets_to_bf - converts earth frame rate targets to body frame rate targets -void AC_AttitudeControl::rate_ef_targets_to_bf(const Vector3f& rate_ef_target, Vector3f& rate_bf_target) -{ - // convert earth frame rates to body frame rates - rate_bf_target.x = rate_ef_target.x - _ahrs.sin_pitch() * rate_ef_target.z; - rate_bf_target.y = _ahrs.cos_roll() * rate_ef_target.y + _ahrs.sin_roll() * _ahrs.cos_pitch() * rate_ef_target.z; - rate_bf_target.z = _ahrs.cos_pitch() * _ahrs.cos_roll() * rate_ef_target.z - _ahrs.sin_roll() * rate_ef_target.y; -} - -// -// rate controller (body-frame) methods -// - -// rate_controller_run - run lowest level rate controller and send outputs to the motors -// should be called at 100hz or more 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? @@ -374,8 +245,86 @@ void AC_AttitudeControl::rate_controller_run() } // -// private methods +// earth-frame <-> body-frame conversion functions // +// rate_ef_targets_to_bf - converts earth frame rate targets to body frame rate targets +void AC_AttitudeControl::rate_ef_targets_to_bf(const Vector3f& rate_ef_target, Vector3f& rate_bf_target) +{ + // convert earth frame rates to body frame rates + rate_bf_target.x = rate_ef_target.x - _ahrs.sin_pitch() * rate_ef_target.z; + rate_bf_target.y = _ahrs.cos_roll() * rate_ef_target.y + _ahrs.sin_roll() * _ahrs.cos_pitch() * rate_ef_target.z; + rate_bf_target.z = -_ahrs.sin_roll() * rate_ef_target.y + _ahrs.cos_pitch() * _ahrs.cos_roll() * rate_ef_target.z; +} + +// rate_bf_targets_to_ef - converts body frame rate targets to earth frame rate targets +void AC_AttitudeControl::rate_bf_targets_to_ef(const Vector3f& rate_bf_target, Vector3f& rate_ef_target) +{ + // convert earth frame rates to body frame rates + rate_ef_target.x = rate_bf_target.x - _ahrs.sin_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * rate_bf_target.y - _ahrs.cos_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * rate_bf_target.z; + rate_ef_target.y = _ahrs.cos_roll() * rate_bf_target.y - _ahrs.sin_roll() * rate_bf_target.z; + rate_ef_target.z = (_ahrs.sin_roll() / _ahrs.cos_pitch()) * rate_bf_target.y + (_ahrs.cos_roll() / _ahrs.cos_pitch()) * rate_bf_target.z; +} + +// +// protected methods +// + +// +// stabilized rate controller (body-frame) methods +// + +// update_stab_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 +void AC_AttitudeControl::update_stab_rate_bf_errors() +{ + // roll - calculate body-frame angle error by integrating body-frame rate error + _angle_bf_error.x += (_rate_bf_feedforward.x - (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + // roll - limit maximum error + _angle_bf_error.x = constrain_float(_angle_bf_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); + + + // pitch - calculate body-frame angle error by integrating body-frame rate error + _angle_bf_error.y += (_rate_bf_feedforward.y - (_ins.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + // pitch - limit maximum error + _angle_bf_error.y = constrain_float(_angle_bf_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); + + + // yaw - calculate body-frame angle error by integrating body-frame rate error + _angle_bf_error.z += (_rate_bf_feedforward.z - (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + // yaw - limit maximum error + _angle_bf_error.z = constrain_float(_angle_bf_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle case of motors being disarmed or g.rc_3.servo_out == 0 and set error to zero +} + +// update_stab_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 +void AC_AttitudeControl::update_stab_rate_bf_targets() +{ + // stab roll calculation + _rate_bf_target.x = _pi_angle_roll.kP() * _angle_bf_error.x; + // constrain roll rate request + if (_flags.limit_angle_to_rate_request) { + _rate_bf_target.x = constrain_float(_rate_bf_target.x,-_angle_rate_rp_max,_angle_rate_rp_max); + } + + // stab pitch calculation + _rate_bf_target.y = _pi_angle_pitch.kP() * _angle_bf_error.y; + // constrain pitch rate request + if (_flags.limit_angle_to_rate_request) { + _rate_bf_target.y = constrain_float(_rate_bf_target.y,-_angle_rate_rp_max,_angle_rate_rp_max); + } + + // stab yaw calculation + _rate_bf_target.z = _pi_angle_yaw.kP() * _angle_bf_error.z; + // constrain yaw rate request + if (_flags.limit_angle_to_rate_request) { + _rate_bf_target.z = constrain_float(_rate_bf_target.z,-_angle_rate_y_max,_angle_rate_y_max); + } +} + // // body-frame rate controller // diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 44c9fcd7e9..00f647c622 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -73,11 +73,14 @@ 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) { _dt = delta_sec; } - float get_dt() { return _dt; } // init_targets - resets target angles to current angles void init_targets(); + // + // methods to be called by upper controllers to request and implement a desired attitude + // + // angleef_rp_rateef_y - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) void angleef_rp_rateef_y(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef); @@ -91,87 +94,36 @@ public: // ratebf_rpy - attempts to maintain a roll, pitch and yaw rate (all body frame) void ratebf_rpy(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf); - // limit_angle_to_rate_request - void limit_angle_to_rate_request(bool limit_request) { _flags.limit_angle_to_rate_request = limit_request; } + // + // rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors + // should be called at 100hz or more + // + virtual void rate_controller_run(); // - // angle controller (earth-frame) methods + // earth-frame <-> body-frame conversion functions // - - // angle_ef_targets - get and set angle controller earth-frame-targets - Vector3f angle_ef_targets() const { return _angle_ef_target; } - void angle_ef_targets(Vector3f& angle_cd) { _angle_ef_target = angle_cd; } - - // angle_to_rate_ef_roll - converts earth-frame angle targets to earth-frame rate targets for roll, pitch and yaw axis - // targets angles in centi-degrees should be placed in _angle_ef_targets - // results in centi-degrees/sec put directly into _rate_ef_target - void angle_to_rate_ef_roll(); - void angle_to_rate_ef_pitch(); - void angle_to_rate_ef_yaw(); - - // - // stabilized rate controller (earth-frame) methods - // - - // rate_stab_ef_targets - gets and sets the stabilized rate controller earth-frame targets - // stabilized rate controllers are better at maintaining a desired rate than the simpler earth-frame rate controllers - // because they also maintain angle-targets and increase/decrease the rate request passed to the earth-frame rate controller - // upon the errors between the actual angle and angle-target. - // - Vector3f rate_stab_ef_targets() const { return _rate_stab_ef_target; } - void rate_stab_ef_targets(Vector3f& rates_cds) { _rate_stab_ef_target = rates_cds; } - - // rate_stab_ef_to_rate_ef_roll - converts earth-frame stabilized rate targets to regular earth-frame rate targets for roll, pitch and yaw axis - // targets rates in centi-degrees/second taken from _rate_stab_ef_target - // results in centi-degrees/sec put into _rate_ef_target - void rate_stab_ef_to_rate_ef_roll(); - void rate_stab_ef_to_rate_ef_pitch(); - void rate_stab_ef_to_rate_ef_yaw(); - - // - // stabilized rate controller (body-frame) methods - // - - // rate_stab_bf_targets - gets and sets the stabilized rate controller body-frame targets - Vector3f rate_stab_bf_targets() const { return _rate_stab_bf_target; } - void rate_stab_bf_targets(Vector3f& rates_cds) { _rate_stab_bf_target = rates_cds; } - - // rate_stab_bf_to_rate_ef_roll - converts body-frame stabilized rate targets to regular body-frame rate targets for roll, pitch and yaw axis - // targets rates in centi-degrees/second taken from _rate_stab_bf_target - // results in centi-degrees/sec put into _rate_bf_target - void rate_stab_bf_update_error(); - void rate_stab_bf_to_rate_bf_roll(); - void rate_stab_bf_to_rate_bf_pitch(); - void rate_stab_bf_to_rate_bf_yaw(); - - // - // rate controller (earth-frame) methods - // - - // rate_ef_targets - gets and sets rate controller earth-frame-targets - Vector3f rate_ef_targets() const { return _rate_ef_target; } - void rate_ef_targets(Vector3f& rates_cds) { _rate_ef_target = rates_cds; } - // rate_ef_targets_to_bf - converts earth frame rate targets to body frame rate targets void rate_ef_targets_to_bf(const Vector3f& rate_ef_target, Vector3f &rate_bf_target); - + // rate_bf_targets_to_ef - converts body frame rate targets to earth frame rate targets + void rate_bf_targets_to_ef(const Vector3f& rate_ef_target, Vector3f &rate_bf_target); + // - // rate controller (body-frame) methods + // public accessor functions // - // rate_bf_targets - gets and sets rate controller body-frame targets - // To-Do: can we return these Vector3fs by reference? i.e. using "&"? - Vector3f rate_bf_targets() const { return _rate_bf_target; } - void rate_bf_targets(Vector3f& rates_cds) { _rate_bf_target = rates_cds; } + // limit_angle_to_rate_request + 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) + Vector3f angle_ef_targets() const { return _angle_ef_target; } + + // rate_bf_targets - gets 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; } - // rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors - // should be called at 100hz or more - virtual void rate_controller_run(); - // // throttle functions // @@ -200,6 +152,16 @@ protected: uint8_t limit_angle_to_rate_request : 1; // 1 if the earth frame angle controller is limiting it's maximum rate request } _flags; + // update_stab_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 + void update_stab_rate_bf_errors(); + + // update_stab_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 + void update_stab_rate_bf_targets(); + // // body-frame rate controller // @@ -259,11 +221,9 @@ protected: // 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 _rate_stab_ef_target; // stabilized rate controller earth-frame rate targets - Vector3f _rate_ef_target; // rate controller earth-frame targets - Vector3f _rate_stab_bf_target; // stabilized rate controller body-frame rate targets - Vector3f _rate_stab_bf_error; // stabilized rate controller body-frame angle errors + Vector3f _angle_bf_error; // angle controller earth-frame targets Vector3f _rate_bf_target; // rate controller body-frame targets + Vector3f _rate_bf_feedforward; // rate controller body-frame targets int16_t _angle_boost; // used only for logging };