From 1615038e579a1968403f7a475672ff3aabd7e08b Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sun, 6 Aug 2023 22:05:25 -0600 Subject: [PATCH] AP_VisualOdom: Change name from position to pose * The function takes position (linear) + orientation (angular), therefore it's a pose, not a position Signed-off-by: Ryan Friedman --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 8 ++++---- libraries/AP_VisualOdom/AP_VisualOdom.h | 4 ++-- libraries/AP_VisualOdom/AP_VisualOdom_Backend.h | 4 ++-- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 4 ++-- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h | 4 ++-- libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 4 ++-- libraries/AP_VisualOdom/AP_VisualOdom_MAV.h | 4 ++-- 7 files changed, 16 insertions(+), 16 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index 2bc4240ae4..d8cbcb05c4 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -179,7 +179,7 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms // general purpose method to consume position estimate data and send to EKF // distances in meters, roll, pitch and yaw are in radians -void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter) +void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter) { // exit immediately if not enabled if (!enabled()) { @@ -191,12 +191,12 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin // convert attitude to quaternion and call backend Quaternion attitude; attitude.from_euler(roll, pitch, yaw); - _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter); + _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter); } } // general purpose method to consume position estimate data and send to EKF -void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) +void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) { // exit immediately if not enabled if (!enabled()) { @@ -205,7 +205,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin // call backend if (_driver != nullptr) { - _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter); + _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter); } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 5a11a6e9ac..47606abb97 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -90,8 +90,8 @@ public: // general purpose methods to consume position estimate data and send to EKF // distances in meters, roll, pitch and yaw are in radians - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter); - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter); + void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter); + void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter); // general purpose methods to consume velocity estimate data and send to EKF // velocity in NED meters per second diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index 853698ce8b..b17eef76e7 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -30,8 +30,8 @@ public: // consume vision_position_delta mavlink messages void handle_vision_position_delta_msg(const mavlink_message_t &msg); - // consume vision position estimate data and send to EKF. distances in meters - virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0; + // consume vision pose estimate data and send to EKF. distances in meters + virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0; // 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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 610f49af26..8f368817e3 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -27,8 +27,8 @@ extern const AP_HAL::HAL& hal; -// consume vision position estimate data and send to EKF. distances in meters -void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) +// consume vision pose estimate data and send to EKF. distances in meters +void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) { const float scale_factor = _frontend.get_pos_scale(); Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor}; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index 7c5b74e8d0..8a78bd1b7a 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -13,8 +13,8 @@ public: using AP_VisualOdom_Backend::AP_VisualOdom_Backend; - // consume vision position estimate data and send to EKF. distances in meters - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override; + // consume vision pose estimate data and send to EKF. distances in meters + void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override; // 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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index b8c6d0ba30..0117e56af8 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -22,8 +22,8 @@ #include #include -// consume vision position estimate data and send to EKF. distances in meters -void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) +// consume vision pose estimate data and send to EKF. distances in meters +void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) { const float scale_factor = _frontend.get_pos_scale(); Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor}; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h index 6c09f4589a..18d5fed709 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h @@ -13,8 +13,8 @@ public: // constructor using AP_VisualOdom_Backend::AP_VisualOdom_Backend; - // consume vision position estimate data and send to EKF. distances in meters - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override; + // consume vision pose estimate data and send to EKF. distances in meters + void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override; // 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;