diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index 7780dc242d..f8ec6b4763 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -131,7 +131,10 @@ 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, roll, pitch, yaw); + // 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); } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index 9094b0f844..6e7752f7ad 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -35,15 +35,6 @@ bool AP_VisualOdom_Backend::healthy() const return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS); } -// general purpose method to send position estimate data to EKF -// distances in meters, roll, pitch and yaw are in radians -void AP_VisualOdom_Backend::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) -{ - Quaternion attitude; - attitude.from_euler(roll, pitch, yaw); - handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude); -} - // general purpose method to send position estimate data to EKF void AP_VisualOdom_Backend::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) { diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index 724f43fda5..229b488c64 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -33,7 +33,6 @@ 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); void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude); // calibrate camera attitude to align with vehicle's AHRS/EKF attitude