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

AHRS: added set_fast_gains() method 

this will be used by arducopter on the ground
This commit is contained in:
Andrew Tridgell 2012-08-21 15:38:06 +10:00
parent 9fbdcdcf84
commit 89db3458c8
2 changed files with 31 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_AHRS/AP_AHRS.h
View File

@ -112,6 +112,11 @@ public:
return _have_initial_yaw; return _have_initial_yaw;
} }
// set the fast gains flag
void set_fast_gains(bool setting) {
_fast_ground_gains = setting;
}
protected: protected:
// whether the yaw value has been intialised with a reference // whether the yaw value has been intialised with a reference
bool _have_initial_yaw; bool _have_initial_yaw;
@ -131,6 +136,10 @@ protected:
GPS *&_gps; GPS *&_gps;
AP_Baro *_barometer; AP_Baro *_barometer;
// should we raise the gain on the accelerometers for faster
// convergence, used when disarmed for ArduCopter
bool _fast_ground_gains;
// true if we can assume the aircraft will be flying forward // true if we can assume the aircraft will be flying forward
// on its X axis // on its X axis
bool _fly_forward; bool _fly_forward;

36
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -31,19 +31,7 @@
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo AP_AHRS_DCM::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_AHRS_DCM::var_info[] PROGMEM = {
// @Param: YAW_P // index 0 and 1 are for old parameters that are no longer used
// @DisplayName: Yaw P
// @Description: This controls the weight the compass has on the overall heading
// @Range: 0 .4
// @Increment: .01
AP_GROUPINFO("YAW_P", 0, AP_AHRS_DCM, _kp_yaw, 0.4),
// @Param: RP_P
// @DisplayName: AHRS RP_P
// @Description: This controls how fast the accelerometers correct the attitude
// @Range: 0 .4
// @Increment: .01
AP_GROUPINFO("RP_P", 1, AP_AHRS_DCM, _kp, 0.4),
// @Param: GPS_GAIN // @Param: GPS_GAIN
// @DisplayName: AHRS GPS gain // @DisplayName: AHRS GPS gain
@ -58,6 +46,20 @@ const AP_Param::GroupInfo AP_AHRS_DCM::var_info[] PROGMEM = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("GPS_USE", 3, AP_AHRS_DCM, _gps_use, 1), AP_GROUPINFO("GPS_USE", 3, AP_AHRS_DCM, _gps_use, 1),
// @Param: YAW_P
// @DisplayName: Yaw P
// @Description: This controls the weight the compass has on the overall heading
// @Range: 0 .4
// @Increment: .01
AP_GROUPINFO("YAW_P", 4, AP_AHRS_DCM, _kp_yaw, 0.4),
// @Param: RP_P
// @DisplayName: AHRS RP_P
// @Description: This controls how fast the accelerometers correct the attitude
// @Range: 0 .4
// @Increment: .01
AP_GROUPINFO("RP_P", 5, AP_AHRS_DCM, _kp, 0.4),
AP_GROUPEND AP_GROUPEND
}; };
@ -375,7 +377,10 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// that depends on the spin rate. See the fastRotations.pdf // that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani // paper from Bill Premerlani
_omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw.get(); _omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw;
if (_fast_ground_gains) {
_omega_yaw_P.z *= 8;
}
// don't update the drift term if we lost the yaw reference // don't update the drift term if we lost the yaw reference
// for more than 2 seconds // for more than 2 seconds
@ -569,6 +574,9 @@ AP_AHRS_DCM::drift_correction(float deltat)
// _omega_P value is what drags us quickly to the // _omega_P value is what drags us quickly to the
// accelerometer reading. // accelerometer reading.
_omega_P = error * _P_gain(spin_rate) * _kp; _omega_P = error * _P_gain(spin_rate) * _kp;
if (_fast_ground_gains) {
_omega_P *= 8;
}
// accumulate some integrator error // accumulate some integrator error
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {