From 2db24659d00972c27b6dd3f8dfbeeca8b70811df Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Feb 2014 13:29:17 +0900 Subject: [PATCH] AC_AttControl: add slew_yaw --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 23 +++++++++++++++---- .../AC_AttitudeControl/AC_AttitudeControl.h | 21 +++++++++-------- 2 files changed, 31 insertions(+), 13 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index a4baac9e43..24d5045770 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -11,18 +11,29 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { // @Param: RATE_RP_MAX // @DisplayName: Angle Rate Roll-Pitch max // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes + // @Unit: Centi-Degrees/Sec // @Range: 90000 250000 // @Increment: 500 // @User: Advanced - AP_GROUPINFO("RATE_RP_MAX", 0, AC_AttitudeControl, _angle_rate_rp_max, AC_ATTITUDE_CONTROL_ANGLE_RATE_RP_MAX_DEFAULT), + AP_GROUPINFO("RATE_RP_MAX", 0, AC_AttitudeControl, _angle_rate_rp_max, AC_ATTITUDE_CONTROL_RATE_RP_MAX_DEFAULT), // @Param: RATE_Y_MAX // @DisplayName: Angle Rate Yaw max // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes + // @Unit: Centi-Degrees/Sec // @Range: 90000 250000 // @Increment: 500 // @User: Advanced - AP_GROUPINFO("RATE_Y_MAX", 1, AC_AttitudeControl, _angle_rate_y_max, AC_ATTITUDE_CONTROL_ANGLE_RATE_Y_MAX_DEFAULT), + AP_GROUPINFO("RATE_Y_MAX", 1, AC_AttitudeControl, _angle_rate_y_max, AC_ATTITUDE_CONTROL_RATE_Y_MAX_DEFAULT), + + // @Param: SLEW_YAW + // @DisplayName: Yaw target slew rate + // @Description: Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes + // @Unit: Centi-Degrees/Sec + // @Range: 500 18000 + // @Increment: 100 + // @User: Advanced + AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT), AP_GROUPEND }; @@ -63,12 +74,16 @@ void AC_AttitudeControl::angleef_rp_rateef_y(float roll_angle_ef, float pitch_an } // angleef_rpy - attempts to maintain a roll, pitch and yaw angle (all earth frame) -void AC_AttitudeControl::angleef_rpy(float roll_angle_ef, float pitch_angle_ef, float yaw_angle_ef) +// 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) { // set earth-frame angle targets _angle_ef_target.x = roll_angle_ef; _angle_ef_target.y = pitch_angle_ef; - _angle_ef_target.z = yaw_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)); + } // convert earth-frame angle targets to earth-frame rate targets angle_to_rate_ef_roll(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index d7383e6907..340cbad217 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -17,14 +17,15 @@ #include // 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_CONTROL_ANGLE_RATE_RP_MAX_DEFAULT 18000 // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes -#define AC_ATTITUDE_CONTROL_ANGLE_RATE_Y_MAX_DEFAULT 18000 // maximum rotation rate on yaw axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes -#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) -#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 4500.0f // body-frame rate controller maximum output (for yaw axis) -#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_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees +#define AC_ATTITUDE_CONTROL_RATE_RP_MAX_DEFAULT 18000 // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes +#define AC_ATTITUDE_CONTROL_RATE_Y_MAX_DEFAULT 18000 // maximum rotation rate on yaw axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes +#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT 6000 // default yaw slew rate in centi-degrees/sec (i.e. maximum yaw target change in 1second) +#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) +#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 4500.0f // body-frame rate controller maximum output (for yaw axis) +#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 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle #define AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle @@ -85,7 +86,8 @@ public: void angleef_rp_rateef_y(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef); // angleef_rpy - attempts to maintain a roll, pitch and yaw angle (all earth frame) - void angleef_rpy(float roll_angle_ef, float pitch_angle_ef, float yaw_angle_ef); + // if yaw_slew is true then target yaw movement will be gradually moved to the new target based on the YAW_SLEW parameter + void angleef_rpy(float roll_angle_ef, float pitch_angle_ef, float yaw_angle_ef, bool slew_yaw); // rateef_rpy - attempts to maintain a roll, pitch and yaw rate (all earth frame) void rateef_rpy(float roll_rate_ef, float pitch_rate_ef, float yaw_rate_ef); @@ -264,6 +266,7 @@ protected: // parameters AP_Float _angle_rate_rp_max; // maximum rate request output from the earth-frame angle controller for roll and pitch axis AP_Float _angle_rate_y_max; // maximum rate request output from the earth-frame angle controller for yaw axis + AP_Float _slew_yaw; // maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes // internal variables // To-Do: make rate targets a typedef instead of Vector3f?