From 69560ec147e50677a69780257c32b0dc74ed6507 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 13 Apr 2020 14:04:06 +0900 Subject: [PATCH] AP_VisualOdom: provide delay to ahrs::writeExtNavData --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 8 ++++++++ libraries/AP_VisualOdom/AP_VisualOdom.h | 4 ++++ libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 2 +- libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 2 +- 4 files changed, 14 insertions(+), 2 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index 72990ff04d..2f62b4186b 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/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 }; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 033a193708..62f673a532 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 8b0601c449..62bcc2605f 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index 0b4b7c6e1b..cb94c71185 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/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;