AP_VisualOdom: support align_position_and_yaw

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -273,6 +273,22 @@ void AP_VisualOdom::align_position_to_ahrs(bool align_xy, bool align_z)
     }
 }
 
+// align position to the given position expressed in meters offset from EKF origin
+// and the given yaw expressed in radians from North
+void AP_VisualOdom::align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad)
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    // call backend
+    if (_driver != nullptr) {
+        _driver->align_position_and_yaw(pos_ned, yaw_rad);
+    }
+}
+
+
 // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
 bool AP_VisualOdom::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
 {

libraries/AP_VisualOdom/AP_VisualOdom.h

@@ -113,6 +113,10 @@ class AP_VisualOdom
     // should only be called when this library is not being used as the position source
     void align_position_to_ahrs(bool align_xy, bool align_z);
 
+    // align position to the given position expressed in meters offset from EKF origin
+    // and the given yaw expressed in radians from North
+    void align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad);
+
     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
 

libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp

@@ -90,4 +90,10 @@ uint32_t AP_VisualOdom_Backend::get_reset_timestamp_ms(uint8_t reset_counter)
     return _reset_timestamp_ms;
 }
 
+// force an update of the reset timestamp (used when external caller updates position or yaw)
+void AP_VisualOdom_Backend::force_update_reset_timestamp_ms()
+{
+    _reset_timestamp_ms = AP_HAL::millis();
+}
+
 #endif

libraries/AP_VisualOdom/AP_VisualOdom_Backend.h

@@ -52,6 +52,10 @@ class AP_VisualOdom_Backend
     // handle request to align position with AHRS
     virtual void align_position_to_ahrs(bool align_xy, bool align_z) {}
 
+    // align position to the given position expressed in meters offset from EKF origin
+    // and the given yaw expressed in radians from North
+    virtual void align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad) {};
+
     // arming check - by default no checks performed
     virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { return true; }
 
@@ -61,6 +65,9 @@ class AP_VisualOdom_Backend
     // 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);
 
+    // force an update of the reset timestamp (used when external caller updates position or yaw)
+    void force_update_reset_timestamp_ms();
+
     AP_VisualOdom::VisualOdom_Type get_type(void) const {
         return _frontend.get_type();
     }

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@@ -50,6 +50,14 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
         }
     }
 
+    // handle external request to align position and yaw
+    if (ext_align_request.align) {
+        align_yaw(pos, attitude, ext_align_request.yaw_rad);
+        align_position(pos, ext_align_request.pos_ned, true, true);
+        ext_align_request.align = false;
+        force_update_reset_timestamp_ms();
+    }
+
     // rotate position and attitude to align with vehicle
     rotate_and_correct_position(pos);
     rotate_attitude(att);
@@ -251,6 +259,15 @@ void AP_VisualOdom_IntelT265::align_position(const Vector3f &sensor_pos, const V
     }
 }
 
+// align position to the given position expressed in meters offset from EKF origin
+// and the given yaw expressed in radians from North
+void AP_VisualOdom_IntelT265::align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad)
+{
+    // record new position and yaw
+    // offsets will be adjusted on the next update from the sensor
+    ext_align_request = {pos_ned, yaw_rad, true};
+}
+
 // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
 bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
 {

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h

@@ -28,6 +28,10 @@ class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend
     // should only be called when this library is not being used as the position source
     void align_position_to_ahrs(bool align_xy, bool align_z) override { _align_posxy = align_xy; _align_posz = align_z; }
 
+    // align position to the given position expressed in meters offset from EKF origin
+    // and the given yaw expressed in radians from North
+    void align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad) override;
+
     // arming check
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
 
@@ -86,6 +90,11 @@ class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend
     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
+    struct {
+        Vector3f pos_ned;                       // externally requested position in NED frame (meters offset from EKF origin)
+        float yaw_rad;                          // externally requested yaw in radians
+        bool align;                             // true when an extenrally requested pos_ned and yaw_rad have been provided
+    } ext_align_request;
 
     // voxl reset jump handling variables
     uint8_t _voxl_reset_counter_last;           // last reset counter from voxl camera (only used for origin jump handling)