AP_VisualOdom: support position resets

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);
     }
 }
 

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, 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);
+    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();

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

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

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, get_reset_timestamp_ms(reset_counter));
-    AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, reset_timestamp_ms);
 
     // 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;

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; }

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, get_reset_timestamp_ms(reset_counter));
-    AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, attitude, posErr, angErr, time_ms, reset_timestamp_ms);
 
     // 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();

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