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

AP_GPS: fix blending when accuracy reported as 1mm

Browse Source 
Thanks to Michael DuBreuil for suggesting the fix
This commit is contained in:
Randy Mackay 2017-03-11 11:25:46 +09:00
parent f340c118d4
commit 3e628f30ff
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1198,7 +1198,7 @@ bool AP_GPS::calc_blend_weights(void)
// calculate the weights using the inverse of the variances
float sum_of_hpos_weights = 0.0f;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (state[i].status >= GPS_OK_FIX_2D && state[i].horizontal_accuracy > 0.001f) {
if (state[i].status >= GPS_OK_FIX_2D && state[i].horizontal_accuracy >= 0.001f) {
hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (state[i].horizontal_accuracy * state[i].horizontal_accuracy);
sum_of_hpos_weights += hpos_blend_weights[i];
}
@ -1218,7 +1218,7 @@ bool AP_GPS::calc_blend_weights(void)
// calculate the weights using the inverse of the variances
float sum_of_vpos_weights = 0.0f;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (state[i].status >= GPS_OK_FIX_3D && state[i].vertical_accuracy > 0.001f) {
if (state[i].status >= GPS_OK_FIX_3D && state[i].vertical_accuracy >= 0.001f) {
vpos_blend_weights[i] = vertical_accuracy_sum_sq / (state[i].vertical_accuracy * state[i].vertical_accuracy);
sum_of_vpos_weights += vpos_blend_weights[i];
}
@ -1238,7 +1238,7 @@ bool AP_GPS::calc_blend_weights(void)
// calculate the weights using the inverse of the variances
float sum_of_spd_weights = 0.0f;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (state[i].status >= GPS_OK_FIX_3D && state[i].speed_accuracy > 0.001f) {
if (state[i].status >= GPS_OK_FIX_3D && state[i].speed_accuracy >= 0.001f) {
spd_blend_weights[i] = speed_accuracy_sum_sq / (state[i].speed_accuracy * state[i].speed_accuracy);
sum_of_spd_weights += spd_blend_weights[i];
}