From 03be6e13b75884a7f073c0ddd7c1296355e5f1d9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 14 Jul 2023 10:58:09 +1000 Subject: [PATCH] AP_VisualOdom: allow compilation with HAL_LOGGING_ENABLED false --- libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp | 2 ++ libraries/AP_VisualOdom/AP_VisualOdom_Backend.h | 4 ++++ libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 4 ++++ libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp | 3 ++- libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 5 +++++ 5 files changed, 17 insertions(+), 1 deletion(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index 568a4b6a01..051981041d 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -66,11 +66,13 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa _frontend.get_pos_offset()); #endif +#if HAL_LOGGING_ENABLED // log sensor data Write_VisualOdom(time_delta_sec, angle_delta, position_delta, packet.confidence); +#endif } #endif // HAL_GCS_ENABLED diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index ce2033ca4c..9f5225aa9e 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -18,6 +18,8 @@ #if HAL_VISUALODOM_ENABLED +#include + class AP_VisualOdom_Backend { public: @@ -57,10 +59,12 @@ protected: return _frontend.get_type(); } +#if HAL_LOGGING_ENABLED // Logging Functions void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence); void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored); void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored); +#endif AP_VisualOdom &_frontend; // reference to frontend uint32_t _last_update_ms; // system time of last update from sensor (used by health checks) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index ebec6eebdd..cc063d8590 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -72,8 +72,10 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint float yaw; att.to_euler(roll, pitch, yaw); +#if HAL_LOGGING_ENABLED // log sensor data Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume); +#endif // store corrected attitude for use in pre-arm checks _attitude_last = att; @@ -99,7 +101,9 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_ // record time for health monitoring _last_update_ms = AP_HAL::millis(); +#if HAL_LOGGING_ENABLED Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume); +#endif } // apply rotation and correction to position diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp index 36486b1e4a..4a92a98360 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp @@ -1,6 +1,7 @@ #include "AP_VisualOdom_Backend.h" +#include -#if HAL_VISUALODOM_ENABLED +#if HAL_VISUALODOM_ENABLED && HAL_LOGGING_ENABLED #include diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index 0117e56af8..5e4af959fa 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -21,6 +21,7 @@ #include #include +#include // 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) @@ -39,8 +40,10 @@ void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t t float yaw; attitude.to_euler(roll, pitch, yaw); +#if HAL_LOGGING_ENABLED // log sensor data Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false); +#endif // record time for health monitoring _last_update_ms = AP_HAL::millis(); @@ -54,7 +57,9 @@ void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, ui // record time for health monitoring _last_update_ms = AP_HAL::millis(); +#if HAL_LOGGING_ENABLED Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false); +#endif } #endif // AP_VISUALODOM_MAV_ENABLED