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

AC_AttControl: add slew_yaw

This commit is contained in:
Randy Mackay 2014-02-03 13:29:17 +09:00 committed by Andrew Tridgell
parent 959f5ec3b2
commit 2db24659d0
2 changed files with 31 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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();

21
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -17,14 +17,15 @@
#include <APM_PI.h>
// 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?