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

AC_AttControl: RATE_FF_ENAB param to disable rate feed forward

This commit is contained in:
Randy Mackay 2014-06-06 14:04:06 +09:00
parent 4d4c7a2118
commit d9c966c927
2 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -52,6 +52,13 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl, _accel_y_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT),
// @Param: ATC_RATE_FF_ENAB
// @DisplayName: Rate Feedforward Enable
// @Description: Controls whether body-frame rate feedfoward is enabled or disabled
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("RATE_FF_ENAB", 5, AC_AttitudeControl, _rate_bf_ff_enabled, AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT),
AP_GROUPEND
};
@ -96,7 +103,7 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
float rate_ef_desired;
float angle_to_target;
if (_accel_rp_max > 0) {
if (_rate_bf_ff_enabled) {
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = roll_angle_ef - _angle_ef_target.x;
@ -138,7 +145,7 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
_rate_ef_desired.y = 0;
}
if (_accel_y_max > 0) {
if (_rate_bf_ff_enabled) {
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_y_max * _dt;
@ -168,9 +175,9 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
update_rate_bf_targets();
// add body frame rate feed forward
// if(do feedforward){
_rate_bf_target += _rate_bf_desired;
// }
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
@ -213,9 +220,9 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, fl
update_rate_bf_targets();
// add body frame rate feed forward
// if(do feedforward){
_rate_bf_target += _rate_bf_desired;
// }
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
@ -291,9 +298,9 @@ void AC_AttitudeControl::rate_ef_roll_pitch_yaw(float roll_rate_ef, float pitch_
update_rate_bf_targets();
// add body frame rate feed forward
// if(do feedforward){
_rate_bf_target += _rate_bf_desired;
// }
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
@ -341,8 +348,7 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
float rate_change, rate_change_limit;
// update the rate feed forward with angular acceleration limits
if (_accel_rp_max > 0) {
if (_rate_bf_ff_enabled) {
rate_change_limit = _accel_rp_max * _dt;
rate_change = roll_rate_bf - _rate_bf_desired.x;
@ -357,7 +363,7 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
_rate_bf_desired.y = pitch_rate_bf;
}
if (_accel_y_max > 0) {
if (_rate_bf_ff_enabled) {
rate_change_limit = _accel_y_max * _dt;
rate_change = yaw_rate_bf - _rate_bf_desired.z;
@ -368,9 +374,9 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
}
// body-frame rate commands added
// if(do feedforward){
_rate_bf_target += _rate_bf_desired;
// }
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
}
}
//

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -40,6 +40,8 @@
#define AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER 20 // D-term filter rate cutoff frequency for Roll and Pitch rate controllers
#define AC_ATTITUDE_RATE_Y_PID_DTERM_FILTER 5 // D-term filter rate cutoff frequency for Yaw rate controller
#define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default
class AC_AttitudeControl {
public:
AC_AttitudeControl( AP_AHRS &ahrs,
@ -228,6 +230,7 @@ protected:
AP_Float _slew_yaw; // maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
AP_Float _accel_rp_max; // maximum rotation acceleration for earth-frame roll and pitch axis
AP_Float _accel_y_max; // maximum rotation acceleration for earth-frame yaw axis
AP_Int8 _rate_bf_ff_enabled; // Enable/Disable body frame rate feed forward
// internal variables
// To-Do: make rate targets a typedef instead of Vector3f?