Browse Source

AP_GPS_GSOF-NOVA: fix last_gps_time_ms

mission-4.1.18
Michael Oborne 8 years ago committed by Tom Pittenger
parent
commit
e4ff445ea7
  1. 2
      libraries/AP_GPS/AP_GPS_GSOF.cpp
  2. 4
      libraries/AP_GPS/AP_GPS_NOVA.cpp

2
libraries/AP_GPS/AP_GPS_GSOF.cpp

@ -298,7 +298,7 @@ AP_GPS_GSOF::process_message(void)
state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * (double)1e7); state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * (double)1e7);
state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 100); state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 100);
state.last_gps_time_ms = state.time_week_ms; state.last_gps_time_ms = AP_HAL::millis();
valid++; valid++;
} }

4
libraries/AP_GPS/AP_GPS_NOVA.cpp

@ -194,7 +194,7 @@ AP_GPS_NOVA::process_message(void)
state.time_week = nova_msg.header.nova_headeru.week; state.time_week = nova_msg.header.nova_headeru.week;
state.time_week_ms = (uint32_t) nova_msg.header.nova_headeru.tow; state.time_week_ms = (uint32_t) nova_msg.header.nova_headeru.tow;
state.last_gps_time_ms = state.time_week_ms; state.last_gps_time_ms = AP_HAL::millis();
state.location.lat = (int32_t) (bestposu.lat * (double)1e7); state.location.lat = (int32_t) (bestposu.lat * (double)1e7);
state.location.lng = (int32_t) (bestposu.lng * (double)1e7); state.location.lng = (int32_t) (bestposu.lng * (double)1e7);
@ -270,7 +270,7 @@ AP_GPS_NOVA::process_message(void)
} }
// ensure out position and velocity stay insync // ensure out position and velocity stay insync
if (_new_position && _new_speed && _last_vel_time == state.last_gps_time_ms) { if (_new_position && _new_speed && _last_vel_time == state.time_week_ms) {
_new_speed = _new_position = false; _new_speed = _new_position = false;
return true; return true;

Loading…
Cancel
Save