From 9dbfb99cc3c68e3a6362c07216a77086999a1888 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 3 Sep 2020 13:49:36 +0900 Subject: [PATCH] AP_VisualOdom: add align_position_to_ahrs --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 14 +++++++++ libraries/AP_VisualOdom/AP_VisualOdom.h | 4 +++ .../AP_VisualOdom/AP_VisualOdom_Backend.h | 3 ++ .../AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 31 +++++++++++++++++++ .../AP_VisualOdom/AP_VisualOdom_IntelT265.h | 10 ++++++ 5 files changed, 62 insertions(+) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index d975e91fac..1aa8dfb7b5 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/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 { diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 66c53eedc2..e36f0235a5 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index 479b357188..fd1a0abf88 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/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; } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index c999a45ab7..4baf8273c6 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/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 { diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index aeea477be7..481b320375 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/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