From 115d33e300b26515e6b1c1333ee9a51304e3b3d5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 20 Jan 2023 16:38:38 +0900 Subject: [PATCH] AP_VIsualOdom: general purpose align yaw and position methods --- .../AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 28 +++++++++++++------ .../AP_VisualOdom/AP_VisualOdom_IntelT265.h | 9 ++++++ 2 files changed, 29 insertions(+), 8 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index f92d4a3f85..317e12f3bf 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -140,6 +140,13 @@ bool AP_VisualOdom_IntelT265::align_yaw_to_ahrs(const Vector3f &position, const return false; } + align_yaw(position, attitude, AP::ahrs().get_yaw()); + return true; +} + +// align sensor yaw with any new yaw (in radians) +void AP_VisualOdom_IntelT265::align_yaw(const Vector3f &position, const Quaternion &attitude, float yaw_rad) +{ // clear any existing errors _error_orientation = false; @@ -161,7 +168,7 @@ bool AP_VisualOdom_IntelT265::align_yaw_to_ahrs(const Vector3f &position, const // trim yaw by difference between ahrs and sensor yaw const float yaw_trim_orig = _yaw_trim; - _yaw_trim = wrap_2PI(AP::ahrs().get_yaw() - sens_yaw); + _yaw_trim = wrap_2PI(yaw_rad - sens_yaw); gcs().send_text(MAV_SEVERITY_INFO, "VisOdom: yaw shifted %d to %d deg", (int)degrees(_yaw_trim - yaw_trim_orig), (int)wrap_360(degrees(sens_yaw + _yaw_trim))); @@ -186,8 +193,6 @@ bool AP_VisualOdom_IntelT265::align_yaw_to_ahrs(const Vector3f &position, const // update position correction to remove change due to rotation _pos_correction += (pos_orig - pos_new); - - return true; } // align position with ahrs position by updating _pos_correction @@ -200,20 +205,27 @@ bool AP_VisualOdom_IntelT265::align_position_to_ahrs(const Vector3f &sensor_pos, return false; } + align_position(sensor_pos, ahrs_pos_ned, align_xy, align_z); + return true; +} + +// align position with a new 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) +// new_pos should be a NED position offset from the EKF origin +void AP_VisualOdom_IntelT265::align_position(const Vector3f &sensor_pos, const Vector3f &new_pos, bool align_xy, bool align_z) +{ // 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); + _pos_correction.x += (new_pos.x - pos_orig.x); + _pos_correction.y += (new_pos.y - pos_orig.y); } if (align_z) { - _pos_correction.z += (ahrs_pos_ned.z - pos_orig.z); + _pos_correction.z += (new_pos.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 diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index a442f47fd3..f087034235 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -39,8 +39,12 @@ protected: void rotate_attitude(Quaternion &attitude) const; // use sensor provided position and attitude to calculate rotation to align sensor yaw with AHRS/EKF attitude + // calls align_yaw (see below) bool align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude); + // align sensor yaw with any new yaw (in radians) + void align_yaw(const Vector3f &position, const Quaternion &attitude, float yaw_rad); + // returns true if sensor data should be consumed, false if it should be ignored // set vision_position_estimate to true if reset_counter is from the VISION_POSITION_ESTIMATE source, false otherwise // only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored @@ -50,6 +54,11 @@ protected: // 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); + // align position with a new 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) + // 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); + 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