AP_VisualOdom: provide delay to ahrs::writeExtNavData

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -74,6 +74,14 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_SCALE", 3, AP_VisualOdom, _pos_scale, 1.0f),
 
+    // @Param: _DELAY_MS
+    // @DisplayName: Visual odometry sensor delay
+    // @Description: Visual odometry sensor delay relative to inertial measurements
+    // @Units: ms
+    // @Range: 0 250
+    // @User: Advanced
+    AP_GROUPINFO("_DELAY_MS", 4, AP_VisualOdom, _delay_ms, 10),
+
     AP_GROUPEND
 };
 

libraries/AP_VisualOdom/AP_VisualOdom.h

@@ -68,6 +68,9 @@ public:
     // return a 3D vector defining the position offset of the camera in meters relative to the body frame origin
     const Vector3f &get_pos_offset(void) const { return _pos_offset; }
 
+    // return the sensor delay in milliseconds (see _DELAY_MS parameter)
+    uint16_t get_delay_ms() const { return MAX(0, _delay_ms); }
+
     // consume vision_position_delta mavlink messages
     void handle_vision_position_delta_msg(const mavlink_message_t &msg);
 
@@ -93,6 +96,7 @@ private:
     AP_Vector3f _pos_offset;    // position offset of the camera in the body frame
     AP_Int8 _orientation;       // camera orientation on vehicle frame
     AP_Float _pos_scale;        // position scale factor applied to sensor values
+    AP_Int16 _delay_ms;         // average delay relative to inertial measurements
 
     // reference to backends
     AP_VisualOdom_Backend *_driver;

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@@ -44,7 +44,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
     // send attitude and position to EKF
     const float posErr = 0; // parameter required?
     const float angErr = 0; // parameter required?
-    AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
+    AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
 
     // calculate euler orientation for logging
     float roll;

libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp

@@ -38,7 +38,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
     // send attitude and position to EKF
     const float posErr = 0; // parameter required?
     const float angErr = 0; // parameter required?
-    AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
+    AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
 
     // calculate euler orientation for logging
     float roll;