|
|
|
@ -1443,7 +1443,7 @@ void AP_GPS::calc_blended_state(void)
@@ -1443,7 +1443,7 @@ void AP_GPS::calc_blended_state(void)
|
|
|
|
|
blended_NE_offset_m.zero(); |
|
|
|
|
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) { |
|
|
|
|
if (_blend_weights[i] > 0.0f && i != best_index) { |
|
|
|
|
blended_NE_offset_m += location_diff(state[GPS_BLENDED_INSTANCE].location, state[i].location) * _blend_weights[i]; |
|
|
|
|
blended_NE_offset_m += state[GPS_BLENDED_INSTANCE].location.get_distance_NE(state[i].location) * _blend_weights[i]; |
|
|
|
|
blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_BLENDED_INSTANCE].location.alt) * _blend_weights[i]; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -1480,7 +1480,7 @@ void AP_GPS::calc_blended_state(void)
@@ -1480,7 +1480,7 @@ void AP_GPS::calc_blended_state(void)
|
|
|
|
|
|
|
|
|
|
// Calculate the offset from each GPS solution to the blended solution
|
|
|
|
|
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) { |
|
|
|
|
_NE_pos_offset_m[i] = location_diff(state[i].location, state[GPS_BLENDED_INSTANCE].location) * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); |
|
|
|
|
_NE_pos_offset_m[i] = state[i].location.get_distance_NE(state[GPS_BLENDED_INSTANCE].location) * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); |
|
|
|
|
_hgt_offset_cm[i] = (float)(state[GPS_BLENDED_INSTANCE].location.alt - state[i].location.alt) * alpha[i] + _hgt_offset_cm[i] * (1.0f - alpha[i]); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|