Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS: added AHRS_GPS_GAIN parameter

Browse Source 
this allows control of how much GPS velocity information is used in
doing centripetal correction in DCM
This commit is contained in:
Andrew Tridgell 2012-07-10 16:22:48 +10:00
parent 2bc530d04f
commit 9b6acf168e
3 changed files with 24 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Parameters.pde
View File

@ -314,6 +314,9 @@ static void load_parameters(void)
}
}
// change the default for the AHRS_GPS_GAIN for ArduCopter
ahrs.gps_gain.set(0.0);
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {

24
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -31,13 +31,26 @@
// table of user settable parameters
const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @Param: YAW_P
// @DisplayName: Yaw P
// @Description: This controls the weight the compass has on the overall heading
// @Range: 0 .4
// @Increment: .01
// @Param: AHRS_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", 0, AP_AHRS_DCM, _kp_yaw),
// @Param: AHRS_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),
// @Param: AHRS_GPS_GAIN
// @DisplayName: AHRS GPS gain
// @Description: This controls how how much to use the GPS to correct the attitude
// @Range: 0.0 1.0
// @Increment: .01
AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS_DCM, gps_gain),
AP_GROUPEND
};
@ -437,6 +450,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// are m/s/s
Vector3f GA_e;
float v_scale = 1.0/(_ra_deltat*_gravity);
v_scale *= gps_gain;
GA_e = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
GA_e.normalize();
if (GA_e.is_inf()) {

2
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -25,6 +25,7 @@ public:
_kp.set(0.4);
_kp_yaw.set(0.4);
gps_gain.set(1.0);
}
// return the smoothed gyro vector corrected for drift
@ -45,6 +46,7 @@ public:
// settable parameters
AP_Float _kp_yaw;
AP_Float _kp;
AP_Float gps_gain;
private:
float _ki;