AP_ExternalAHRS: added location extrapolation

this copes better with slow backends

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -167,6 +167,19 @@ bool AP_ExternalAHRS::get_location(Location &loc)
     }
     WITH_SEMAPHORE(state.sem);
     loc = state.location;
+
+    if (state.last_location_update_us != 0 &&
+        state.have_velocity) {
+        // extrapolate position based on velocity to cope with slow backends
+        const float dt = (AP_HAL::micros() - state.last_location_update_us)*1.0e-6;
+        if (dt < 1) {
+            // only extrapolate for 1s max
+            Vector3p ofs = state.velocity.topostype();
+            ofs *= dt;
+            loc.offset(ofs);
+        }
+    }
+
     return true;
 }
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -96,6 +96,8 @@ public:
         bool have_origin;
         bool have_location;
         bool have_velocity;
+
+        uint32_t last_location_update_us;
     } state;
 
     // accessors for AP_AHRS

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp

@@ -163,6 +163,7 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const
 
         state.location = Location{filter_data.lat, filter_data.lon, gnss_data[gnss_instance].msl_altitude, Location::AltFrame::ABSOLUTE};
         state.have_location = true;
+        state.last_location_update_us = AP_HAL::micros();
     }
 
     AP_ExternalAHRS::gps_data_message_t gps {

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp

@@ -475,6 +475,7 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b)
                                   int32_t(pkt1.positionLLA[1] * 1.0e7),
                                   int32_t(pkt1.positionLLA[2] * 1.0e2),
                                   Location::AltFrame::ABSOLUTE};
+        state.last_location_update_us = AP_HAL::micros();
         state.have_location = true;
     }