From 923520d455a5c0cc6c9ea253e15ee1808e07bc11 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 20 Jan 2023 17:01:29 +0900
Subject: [PATCH] AP_VisualOdom: handle voxl yaw and pos jump on reset

---
 .../AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 47 +++++++++++++++++++
 .../AP_VisualOdom/AP_VisualOdom_IntelT265.h   | 13 +++++
 2 files changed, 60 insertions(+)

diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
index 317e12f3bf..62421c82a1 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
@@ -33,6 +33,9 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
     Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
     Quaternion att = attitude;
 
+    // handle voxl camera reset jumps in attitude and position
+    handle_voxl_camera_reset_jump(pos, att, reset_counter);
+
     // handle request to align sensor's yaw with vehicle's AHRS/EKF attitude
     if (_align_yaw) {
         if (align_yaw_to_ahrs(pos, attitude)) {
@@ -49,6 +52,9 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
     rotate_and_correct_position(pos);
     rotate_attitude(att);
 
+    // record position for voxl reset jump handling
+    record_voxl_position_and_reset_count(pos, reset_counter);
+
     posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
     angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
 
@@ -297,4 +303,45 @@ bool AP_VisualOdom_IntelT265::should_consume_sensor_data(bool vision_position_es
     return (_pos_reset_ignore_start_ms == 0);
 }
 
+// record voxl camera's position and reset counter for reset jump handling
+// position is post scaling, offset and orientation corrections
+void AP_VisualOdom_IntelT265::record_voxl_position_and_reset_count(const Vector3f &position, uint8_t reset_counter)
+{
+    // return immediately if not using VOXL camera
+    if (get_type() != AP_VisualOdom::VisualOdom_Type::VOXL) {
+        return;
+    }
+
+    _voxl_position_last = position;
+    _voxl_reset_counter_last = reset_counter;
+}
+
+// handle voxl camera reset jumps in attitude and position
+// sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
+// sensor_att is similarly the attitude directly from the sensor
+void AP_VisualOdom_IntelT265::handle_voxl_camera_reset_jump(const Vector3f &sensor_pos, const Quaternion &sensor_att, uint8_t reset_counter)
+{
+    // return immediately if not using VOXL camera
+    if (get_type() != AP_VisualOdom::VisualOdom_Type::VOXL) {
+        return;
+    }
+
+    // return immediately if no change in reset counter
+    if (reset_counter == _voxl_reset_counter_last) {
+        return;
+    }
+
+    // warng user of reset
+    gcs().send_text(MAV_SEVERITY_WARNING, "VisOdom: reset");
+
+    // align sensor yaw to match current yaw estimate
+    align_yaw_to_ahrs(sensor_pos, sensor_att);
+
+    // align psoition to match last recorded position
+    align_position(sensor_pos, _voxl_position_last, true, true);
+
+    // record change in reset counter
+    _voxl_reset_counter_last = reset_counter;
+}
+
 #endif
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
index f087034235..03914499f8 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
@@ -59,6 +59,15 @@ protected:
     // new_pos should be a NED position offset from the EKF origin
     void align_position(const Vector3f &sensor_pos, const Vector3f &new_pos, bool align_xy, bool align_z);
 
+    // record voxl camera's position and reset counter for reset jump handling
+    // position is post scaling, offset and orientation corrections
+    void record_voxl_position_and_reset_count(const Vector3f &position, uint8_t reset_counter);
+
+    // handle voxl camera reset jumps in attitude and position
+    // sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
+    // sensor_att is similarly the attitude directly from the sensor
+    void handle_voxl_camera_reset_jump(const Vector3f &sensor_pos, const Quaternion &sensor_att, uint8_t reset_counter);
+
     float _yaw_trim;                            // yaw angle trim (in radians) to align camera's yaw to ahrs/EKF's
     Quaternion _yaw_rotation;                   // earth-frame yaw rotation to align heading of sensor with vehicle.  use when _yaw_trim is non-zero
     Quaternion _att_rotation;                   // body-frame rotation corresponding to ORIENT parameter.  use when get_orientation != NONE
@@ -73,6 +82,10 @@ protected:
     Quaternion _attitude_last;                  // last attitude received from camera (used for arming checks)
     uint8_t _pos_reset_counter_last;            // last vision-position-estimate reset counter value
     uint32_t _pos_reset_ignore_start_ms;        // system time we start ignoring sensor information, 0 if sensor data is not being ignored
+
+    // voxl reset jump handling variables
+    uint8_t _voxl_reset_counter_last;           // last reset counter from voxl camera (only used for origin jump handling)
+    Vector3f _voxl_position_last;               // last recorded position (post scaling, offset and orientation corrections)
 };
 
 #endif