diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index d06d9e6cea..93d333749c 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -102,21 +102,12 @@ void Rover::send_extended_status1(mavlink_channel_t chan) void Rover::send_location(mavlink_channel_t chan) { - uint32_t fix_time; - // if we have a GPS fix, take the time as the last fix time. That - // allows us to correctly calculate velocities and extrapolate - // positions. - // If we don't have a GPS fix then we are dead reckoning, and will - // use the current boot time as the fix time. - if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { - fix_time = gps.last_fix_time_ms(); - } else { - fix_time = millis(); - } - const Vector3f &vel = gps.velocity(); + const uint32_t now = AP_HAL::millis(); + Vector3f vel; + ahrs.get_velocity_NED(vel); mavlink_msg_global_position_int_send( chan, - fix_time, + now, current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees current_loc.alt * 10UL, // millimeters above sea level