aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: fixed antenna offset for blending

Browse Source 
we were accessing beyond the end of the antenna_offset array
This commit is contained in:
Andrew Tridgell 2020-03-13 16:29:54 +11:00
parent 0665c379de
commit 9fa034d1de
1 changed files with 4 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_GPS/AP_GPS.cpp
View File

@ -795,8 +795,10 @@ void AP_GPS::update(void)
#if defined(GPS_BLENDED_INSTANCE)
// copy the primary instance to the blended instance in case it is enabled later
state[GPS_BLENDED_INSTANCE] = state[primary_instance];
_blended_antenna_offset = _antenna_offset[primary_instance];
if (primary_instance != GPS_BLENDED_INSTANCE) {
state[GPS_BLENDED_INSTANCE] = state[primary_instance];
_blended_antenna_offset = _antenna_offset[primary_instance];
}
#endif
#ifndef HAL_BUILD_AP_PERIPH
@ -1654,14 +1656,6 @@ void AP_GPS::calc_blended_state(void)
_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]);
}
// Calculate a corrected location for each GPS
Location corrected_location[GPS_MAX_RECEIVERS];
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
corrected_location[i] = state[i].location;
corrected_location[i].offset(_NE_pos_offset_m[i].x, _NE_pos_offset_m[i].y);
corrected_location[i].alt += (int)(_hgt_offset_cm[i]);
}
// If the GPS week is the same then use a blended time_week_ms
// If week is different, then use time stamp from GPS with largest weighting
// detect inconsistent week data