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

AP_AHRS: added AHRS_GPS_MINSATS option

Browse Source 
if the number of visible satellites is below AHRS_GPS_MINSATS then
don't use the GPS for acceleration correction for attitude
This commit is contained in:
Andrew Tridgell 2013-05-05 12:47:49 +10:00
parent 6836964e3c
commit 943a1d8c8d
3 changed files with 28 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -84,6 +84,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @Increment: .01
AP_GROUPINFO("COMP_BETA", 10, AP_AHRS, beta, 0.1f),
// @Param: GPS_MINSATS
// @DisplayName: AHRS GPS Minimum satellites
// @Description: Minimum number of satellites visible to use GPS for velocity based corrections attitude correction. This defaults to 6, which is about the point at which the velocity numbers from a GPS become too unreliable for accurate correction of the accelerometers.
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("GPS_MINSATS", 11, AP_AHRS, _gps_minsats, 6),
AP_GROUPEND
};

1
libraries/AP_AHRS/AP_AHRS.h
View File

@ -169,6 +169,7 @@ public:
AP_Int8 _gps_use;
AP_Int8 _wind_max;
AP_Int8 _board_orientation;
AP_Int8 _gps_minsats;
// for holding parameters
static const struct AP_Param::GroupInfo var_info[];

31
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -415,6 +415,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
Matrix3f temp_dcm = _dcm_matrix;
Vector3f velocity;
uint32_t last_correction_time;
bool gps_based_velocity = false;
// perform yaw drift correction if we have a new yaw reference
// vector
@ -433,7 +434,9 @@ AP_AHRS_DCM::drift_correction(float deltat)
// we have integrated over
_ra_deltat += deltat;
if (!have_gps()) {
if (!have_gps() ||
_gps->status() < GPS::GPS_OK_FIX_3D ||
_gps->num_sats < _gps_minsats) {
// no GPS, or not a good lock. From experience we need at
// least 6 satellites to get a really reliable velocity number
// from the GPS.
@ -492,6 +495,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
gps_based_velocity = true;
}
// see if this is our first time through - in which case we
@ -505,16 +510,20 @@ AP_AHRS_DCM::drift_correction(float deltat)
// equation 9: get the corrected acceleration vector in earth frame. Units
// are m/s/s
Vector3f GA_e;
float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
// limit vertical acceleration correction to 0.5 gravities. The
// barometer sometimes gives crazy acceleration changes.
vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f);
GA_e = Vector3f(0, 0, -1.0f) + vdelta;
GA_e.normalize();
if (GA_e.is_inf()) {
// wait for some non-zero acceleration information
return;
GA_e = Vector3f(0, 0, -1.0f);
if (gps_based_velocity || _fly_forward) {
float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
// limit vertical acceleration correction to 0.5 gravities. The
// barometer sometimes gives crazy acceleration changes.
vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f);
GA_e += vdelta;
GA_e.normalize();
if (GA_e.is_inf()) {
// wait for some non-zero acceleration information
return;
}
}
// calculate the error term in earth frame.