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

AP_GPS: use get_distance_NE instead of location_diff

This commit is contained in:
Pierre Kancir 2019-04-08 15:16:19 +02:00 committed by Tom Pittenger
parent d6277390f4
commit 4ea10ce5f5
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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)
// 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]);
}