diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index dac83df13f..a70d9fa166 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -13,15 +13,15 @@ along with this program. If not, see . */ -#include "AP_VisualOdom.h" +#include "AP_VisualOdom_config.h" #if HAL_VISUALODOM_ENABLED +#include "AP_VisualOdom.h" #include "AP_VisualOdom_Backend.h" #include "AP_VisualOdom_MAV.h" #include "AP_VisualOdom_IntelT265.h" #include -#include extern const AP_HAL::HAL &hal; @@ -128,13 +128,17 @@ void AP_VisualOdom::init() case VisualOdom_Type::None: // do nothing break; +#if AP_VISUALODOM_MAV_ENABLED case VisualOdom_Type::MAV: _driver = new AP_VisualOdom_MAV(*this); break; +#endif +#if AP_VISUALODOM_INTELT265_ENABLED case VisualOdom_Type::IntelT265: case VisualOdom_Type::VOXL: _driver = new AP_VisualOdom_IntelT265(*this); break; +#endif } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 68c051ff7e..5a11a6e9ac 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -44,9 +44,13 @@ public: // external position backend types (used by _TYPE parameter) enum class VisualOdom_Type { None = 0, +#if AP_VISUALODOM_MAV_ENABLED MAV = 1, +#endif +#if AP_VISUALODOM_INTELT265_ENABLED IntelT265 = 2, VOXL = 3, +#endif }; // detect and initialise any sensors diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index 3a01cb7a83..b0974348e1 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -17,11 +17,9 @@ #if HAL_VISUALODOM_ENABLED -#include +#include #include -extern const AP_HAL::HAL &hal; - /* base class constructor. This incorporates initialisation as well. diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index a9c02b7366..853698ce8b 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -68,5 +68,4 @@ protected: uint32_t _reset_timestamp_ms; // time reset counter was received }; -#endif - +#endif // HAL_VISUALODOM_ENABLED diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 62421c82a1..610f49af26 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -13,14 +13,15 @@ along with this program. If not, see . */ -#include "AP_VisualOdom_IntelT265.h" +#include "AP_VisualOdom_config.h" -#if HAL_VISUALODOM_ENABLED +#if AP_VISUALODOM_INTELT265_ENABLED + +#include "AP_VisualOdom_IntelT265.h" #include #include #include -#include #define VISUALODOM_RESET_IGNORE_DURATION_MS 1000 // sensor data is ignored for 1sec after a position reset @@ -344,4 +345,4 @@ void AP_VisualOdom_IntelT265::handle_voxl_camera_reset_jump(const Vector3f &sens _voxl_reset_counter_last = reset_counter; } -#endif +#endif // AP_VISUALODOM_INTELT265_ENABLED diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index 03914499f8..7c5b74e8d0 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -1,8 +1,10 @@ #pragma once -#include "AP_VisualOdom_Backend.h" +#include "AP_VisualOdom_config.h" -#if HAL_VISUALODOM_ENABLED +#if AP_VISUALODOM_INTELT265_ENABLED + +#include "AP_VisualOdom_Backend.h" class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend { @@ -88,4 +90,4 @@ protected: Vector3f _voxl_position_last; // last recorded position (post scaling, offset and orientation corrections) }; -#endif +#endif // AP_VISUALODOM_INTELT265_ENABLED diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index cebd1edf58..b8c6d0ba30 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -13,15 +13,14 @@ along with this program. If not, see . */ -#include "AP_VisualOdom_MAV.h" +#include "AP_VisualOdom_config.h" -#if HAL_VISUALODOM_ENABLED +#if AP_VISUALODOM_MAV_ENABLED + +#include "AP_VisualOdom_MAV.h" #include #include -#include - -extern const AP_HAL::HAL& hal; // 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) @@ -58,4 +57,4 @@ void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, ui Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false); } -#endif +#endif // AP_VISUALODOM_MAV_ENABLED diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h index 7b089d0777..6c09f4589a 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h @@ -1,8 +1,10 @@ #pragma once -#include "AP_VisualOdom_Backend.h" +#include "AP_VisualOdom_config.h" -#if HAL_VISUALODOM_ENABLED +#if AP_VISUALODOM_MAV_ENABLED + +#include "AP_VisualOdom_Backend.h" class AP_VisualOdom_MAV : public AP_VisualOdom_Backend { @@ -18,4 +20,4 @@ public: void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override; }; -#endif +#endif // AP_VISUALODOM_MAV_ENABLED diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_config.h b/libraries/AP_VisualOdom/AP_VisualOdom_config.h index b4cba125a0..f39423384a 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_config.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_config.h @@ -1,7 +1,20 @@ #pragma once #include +#include #ifndef HAL_VISUALODOM_ENABLED #define HAL_VISUALODOM_ENABLED BOARD_FLASH_SIZE > 1024 #endif + +#ifndef AP_VISUALODOM_BACKEND_DEFAULT_ENABLED +#define AP_VISUALODOM_BACKEND_DEFAULT_ENABLED HAL_VISUALODOM_ENABLED +#endif + +#ifndef AP_VISUALODOM_INTELT265_ENABLED +#define AP_VISUALODOM_INTELT265_ENABLED AP_VISUALODOM_BACKEND_DEFAULT_ENABLED && HAL_GCS_ENABLED +#endif + +#ifndef AP_VISUALODOM_MAV_ENABLED +#define AP_VISUALODOM_MAV_ENABLED AP_VISUALODOM_BACKEND_DEFAULT_ENABLED && HAL_GCS_ENABLED +#endif