fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 04:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_gps_position: don't work with 64 time with 32 bit floats

Browse Source
This commit is contained in:
Daniel Agar
2022-01-21 09:18:49 -05:00
parent 0607982b23
commit 7cb8ed3a1f
1 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
+13 -4
View File
@@ -105,14 +105,23 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
_np_gps_suitable_for_blending = 0; _np_gps_suitable_for_blending = 0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
const float raw_dt = 1e-6f * ((float)_gps_state[i].timestamp - (float)_time_prev_us[i]);
const float present_dt = 1e-6f * ((float)hrt_now_us - (float)_gps_state[i].timestamp); float raw_dt = 0.f;
if (_gps_state[i].timestamp > _time_prev_us[i]) {
raw_dt = 1e-6f * (_gps_state[i].timestamp - _time_prev_us[i]);
}
float present_dt = 0.f;
if (hrt_now_us > _gps_state[i].timestamp) {
present_dt = 1e-6f * (hrt_now_us - _gps_state[i].timestamp);
}
if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) { if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) {
_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i]; _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
} else if ((present_dt >= GPS_TIMEOUT_S) } else if ((present_dt >= GPS_TIMEOUT_S) && (_gps_state[i].timestamp > 0)) {
&& (_gps_state[i].timestamp > 0)) {
// Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt // Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt
_gps_state[i].timestamp = 0; _gps_state[i].timestamp = 0;
_gps_state[i].fix_type = 0; _gps_state[i].fix_type = 0;