AP_VisualOdom: add align_position_to_ahrs

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -229,6 +229,20 @@ void AP_VisualOdom::align_sensor_to_vehicle()
     }
 }
 
+// update position offsets to align to AHRS position.  Should only be called when this library is not being used as the position source
+void AP_VisualOdom::align_position_to_ahrs(bool align_xy, bool align_z)
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    // call backend
+    if (_driver != nullptr) {
+        _driver->align_position_to_ahrs(align_xy, align_z);
+    }
+}
+
 // 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

@@ -95,6 +95,10 @@ public:
     // calibrate camera attitude to align with vehicle's AHRS/EKF attitude
     void align_sensor_to_vehicle();
 
+    // update position offsets to align to AHRS position
+    // 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);
+
     // 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.h

@@ -39,6 +39,9 @@ public:
     // handle request to align camera's attitude with vehicle's AHRS/EKF attitude
     virtual void align_sensor_to_vehicle() {}
 
+    // handle request to align position with AHRS
+    virtual void align_position_to_ahrs(bool align_xy, bool align_z) {}
+
     // arming check - by default no checks performed
     virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { return true; }
 

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@@ -38,6 +38,11 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
             _align_camera = false;
         }
     }
+    if (_align_posxy || _align_posz) {
+        if (align_position_to_ahrs(pos, _align_posxy, _align_posz)) {
+            _align_posxy = _align_posz = false;
+        }
+    }
 
     // rotate position and attitude to align with vehicle
     rotate_and_correct_position(pos);
@@ -187,6 +192,32 @@ bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position,
     return true;
 }
 
+// align position with ahrs position by updating _pos_correction
+// sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
+bool AP_VisualOdom_IntelT265::align_position_to_ahrs(const Vector3f &sensor_pos, bool align_xy, bool align_z)
+{
+    // fail immediately if ahrs cannot provide position
+    Vector3f ahrs_pos_ned;
+    if (!AP::ahrs().get_relative_position_NED_origin(ahrs_pos_ned)) {
+        return false;
+    }
+
+    // calculate position with current rotation and correction
+    Vector3f pos_orig = sensor_pos;
+    rotate_and_correct_position(pos_orig);
+
+    // update position correction
+    if (align_xy) {
+        _pos_correction.x += (ahrs_pos_ned.x - pos_orig.x);
+        _pos_correction.y += (ahrs_pos_ned.y - pos_orig.y);
+    }
+    if (align_z) {
+        _pos_correction.z += (ahrs_pos_ned.z - pos_orig.z);
+    }
+
+    return 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

@@ -20,6 +20,10 @@ public:
     // handle request to align camera's attitude with vehicle's AHRS/EKF attitude
     void align_sensor_to_vehicle() override { _align_camera = true; }
 
+    // update position offsets to align to AHRS position
+    // 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; }
+
     // arming check
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
 
@@ -42,6 +46,10 @@ protected:
     // only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored
     bool should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter);
 
+    // align position with ahrs position by updating _pos_correction
+    // sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
+    bool align_position_to_ahrs(const Vector3f &sensor_pos, bool align_xy, bool align_z);
+
     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
@@ -50,6 +58,8 @@ protected:
     bool _use_att_rotation;                     // true if _att_rotation should be applied to sensor's attitude data
     bool _use_posvel_rotation;                  // true if _posvel_rotation should be applied to sensor's position and/or velocity data
     bool _align_camera = true;                  // true if camera should be aligned to AHRS/EKF
+    bool _align_posxy;                          // true if sensor xy position should be aligned to AHRS
+    bool _align_posz;                           // true if sensor z position should be aligned to AHRS
     bool _error_orientation;                    // true if the orientation is not supported
     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