From 6622927a23b93784e30c05b7967bda57eecee78f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 10 Apr 2020 13:36:26 +0900 Subject: [PATCH] AP_VisualOdom: support position resets --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 8 ++++---- libraries/AP_VisualOdom/AP_VisualOdom.h | 4 ++-- libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp | 12 ++++++++++++ libraries/AP_VisualOdom/AP_VisualOdom_Backend.h | 9 ++++++++- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 7 +++---- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h | 2 +- libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 7 +++---- libraries/AP_VisualOdom/AP_VisualOdom_MAV.h | 2 +- 8 files changed, 34 insertions(+), 17 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index 696a9215c4..72990ff04d 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -140,7 +140,7 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms // general purpose method to consume position estimate data and send to EKF // distances in meters, roll, pitch and yaw are in radians -void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw) +void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter) { // exit immediately if not enabled if (!enabled()) { @@ -152,12 +152,12 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin // convert attitude to quaternion and call backend Quaternion attitude; attitude.from_euler(roll, pitch, yaw); - _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude); + _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter); } } // general purpose method to consume position estimate data and send to EKF -void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) +void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) { // exit immediately if not enabled if (!enabled()) { @@ -166,7 +166,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin // call backend if (_driver != nullptr) { - _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude); + _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter); } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 3f5839c938..033a193708 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -73,8 +73,8 @@ public: // general purpose methods to consume position estimate data and send to EKF // distances in meters, roll, pitch and yaw are in radians - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw); - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude); + void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter); + void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter); // calibrate camera attitude to align with vehicle's AHRS/EKF attitude void align_sensor_to_vehicle(); diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index 9b202c0ba5..c22a993c3e 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -38,4 +38,16 @@ bool AP_VisualOdom_Backend::healthy() const return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS); } +// returns the system time of the last reset if reset_counter has not changed +// updates the reset timestamp to the current system time if the reset_counter has changed +uint32_t AP_VisualOdom_Backend::get_reset_timestamp_ms(uint8_t reset_counter) +{ + // update reset counter and timestamp if reset_counter has changed + if (reset_counter != _last_reset_counter) { + _last_reset_counter = reset_counter; + _reset_timestamp_ms = AP_HAL::millis(); + } + return _reset_timestamp_ms; +} + #endif diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index 20d031c0a9..b695dd7751 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -31,7 +31,7 @@ public: virtual void handle_vision_position_delta_msg(const mavlink_message_t &msg) = 0; // consume vision position estimate data and send to EKF. distances in meters - virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) = 0; + virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) = 0; // handle request to align camera's attitude with vehicle's AHRS/EKF attitude virtual void align_sensor_to_vehicle() {} @@ -41,9 +41,16 @@ public: protected: + // returns the system time of the last reset if reset_counter has not changed + // updates the reset timestamp to the current system time if the reset_counter has changed + uint32_t get_reset_timestamp_ms(uint8_t reset_counter); AP_VisualOdom &_frontend; // reference to frontend uint32_t _last_update_ms; // system time of last update from sensor (used by health checks) + + // reset counter handling + uint8_t _last_reset_counter; // last sensor reset counter received + uint32_t _reset_timestamp_ms; // time reset counter was received }; #endif diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 0a8dc05682..b83fbb79cd 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -24,7 +24,7 @@ extern const AP_HAL::HAL& hal; // consume vision position estimate data and send to EKF. distances in meters -void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) +void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) { const float scale_factor = _frontend.get_pos_scale(); Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor}; @@ -44,8 +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? - const uint32_t reset_timestamp_ms = 0; // no data available - AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, reset_timestamp_ms); + AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter)); // calculate euler orientation for logging float roll; @@ -54,7 +53,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti att.to_euler(roll, pitch, yaw); // log sensor data - AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw))); + AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), reset_counter); // store corrected attitude for use in pre-arm checks _attitude_last = att; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index 4d7ee89ac4..36820eeebd 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -15,7 +15,7 @@ public: void handle_vision_position_delta_msg(const mavlink_message_t &msg) override {}; // consume vision position estimate data and send to EKF. distances in meters - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) override; + void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override; // handle request to align camera's attitude with vehicle's AHRS/EKF attitude void align_sensor_to_vehicle() override { _align_camera = true; } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index 1bbc2c6104..acbb78a5af 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -63,7 +63,7 @@ void AP_VisualOdom_MAV::handle_vision_position_delta_msg(const mavlink_message_t } // consume vision position estimate data and send to EKF. distances in meters -void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) +void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) { const float scale_factor = _frontend.get_pos_scale(); Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor}; @@ -71,8 +71,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? - const uint32_t reset_timestamp_ms = 0; // no data available - AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, attitude, posErr, angErr, time_ms, reset_timestamp_ms); + AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, attitude, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter)); // calculate euler orientation for logging float roll; @@ -81,7 +80,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, attitude.to_euler(roll, pitch, yaw); // log sensor data - AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), degrees(yaw)); + AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), degrees(yaw), reset_counter); // record time for health monitoring _last_update_ms = AP_HAL::millis(); diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h index afdac098d7..6b53bcb205 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h @@ -15,7 +15,7 @@ public: void handle_vision_position_delta_msg(const mavlink_message_t &msg) override; // consume vision position estimate data and send to EKF. distances in meters - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) override; + void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override; }; #endif