From 83368146073a836df115d2dbbe5ca4343842229a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 20 Jan 2023 16:22:04 +0900 Subject: [PATCH] AP_VisualOdom: rename align_sensor_to_vehicle to align_yaw_to_ahrs also separate request to align from code that actually performs the alignment --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 6 +++--- libraries/AP_VisualOdom/AP_VisualOdom.h | 4 ++-- libraries/AP_VisualOdom/AP_VisualOdom_Backend.h | 4 ++-- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 10 +++++----- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h | 10 +++++----- 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index 0dbdde1126..dac83df13f 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -218,8 +218,8 @@ void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32 } } -// calibrate camera attitude to align with vehicle's AHRS/EKF attitude -void AP_VisualOdom::align_sensor_to_vehicle() +// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude +void AP_VisualOdom::request_align_yaw_to_ahrs() { // exit immediately if not enabled if (!enabled()) { @@ -228,7 +228,7 @@ void AP_VisualOdom::align_sensor_to_vehicle() // call backend if (_driver != nullptr) { - _driver->align_sensor_to_vehicle(); + _driver->request_align_yaw_to_ahrs(); } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 601a6a15a3..68c051ff7e 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -93,8 +93,8 @@ public: // velocity in NED meters per second void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter); - // calibrate camera attitude to align with vehicle's AHRS/EKF attitude - void align_sensor_to_vehicle(); + // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude + void request_align_yaw_to_ahrs(); // update position offsets to align to AHRS position // should only be called when this library is not being used as the position source diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index 0d521e29dc..a9c02b7366 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -36,8 +36,8 @@ public: // consume vision velocity estimate data and send to EKF, velocity in NED meters per second virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) = 0; - // handle request to align camera's attitude with vehicle's AHRS/EKF attitude - virtual void align_sensor_to_vehicle() {} + // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude + virtual void request_align_yaw_to_ahrs() {} // handle request to align position with AHRS virtual void align_position_to_ahrs(bool align_xy, bool align_z) {} diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index e38662dfda..f92d4a3f85 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -33,10 +33,10 @@ 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 user request to align camera - if (_align_camera) { - if (align_sensor_to_vehicle(pos, attitude)) { - _align_camera = false; + // handle request to align sensor's yaw with vehicle's AHRS/EKF attitude + if (_align_yaw) { + if (align_yaw_to_ahrs(pos, attitude)) { + _align_yaw = false; } } if (_align_posxy || _align_posz) { @@ -128,7 +128,7 @@ void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const } // use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude -bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude) +bool AP_VisualOdom_IntelT265::align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude) { // do not align to ahrs if we are its yaw source if (AP::ahrs().using_extnav_for_yaw()) { diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index 481b320375..a442f47fd3 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -17,8 +17,8 @@ public: // consume vision velocity estimate data and send to EKF, velocity in NED meters per second void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override; - // handle request to align camera's attitude with vehicle's AHRS/EKF attitude - void align_sensor_to_vehicle() override { _align_camera = true; } + // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude + void request_align_yaw_to_ahrs() override { _align_yaw = true; } // update position offsets to align to AHRS position // should only be called when this library is not being used as the position source @@ -38,8 +38,8 @@ protected: // rotate attitude using _yaw_trim void rotate_attitude(Quaternion &attitude) const; - // use sensor provided position and attitude to calculate rotation to align sensor with AHRS/EKF attitude - bool align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude); + // use sensor provided position and attitude to calculate rotation to align sensor yaw with AHRS/EKF attitude + bool align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude); // 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 @@ -57,7 +57,7 @@ protected: Vector3f _pos_correction; // position correction that should be added to position reported from sensor 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_yaw = true; // true if sensor yaw 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