AP_OpticalFlow: create and use AP_OPTICALFLOW_ENABLED

Including a define for each backend.
This commit is contained in:
Peter Barker 2021-12-24 16:47:21 +11:00 committed by Andrew Tridgell
parent ca8436ba5d
commit 99ccbee474
17 changed files with 126 additions and 18 deletions

View File

@ -1,5 +1,8 @@
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#if AP_OPTICALFLOW_ENABLED
#include "AP_OpticalFlow_Onboard.h" #include "AP_OpticalFlow_Onboard.h"
#include "AP_OpticalFlow_SITL.h" #include "AP_OpticalFlow_SITL.h"
#include "AP_OpticalFlow_Pixart.h" #include "AP_OpticalFlow_Pixart.h"
@ -247,3 +250,5 @@ OpticalFlow *opticalflow()
} }
} }
#endif // AP_OPTICALFLOW_ENABLED

View File

@ -14,6 +14,18 @@
*/ */
#pragma once #pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef AP_OPTICALFLOW_ENABLED
#define AP_OPTICALFLOW_ENABLED 1
#endif
#ifndef HAL_MSP_OPTICALFLOW_ENABLED
#define HAL_MSP_OPTICALFLOW_ENABLED (AP_OPTICALFLOW_ENABLED && (HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES))
#endif
#if AP_OPTICALFLOW_ENABLED
/* /*
* AP_OpticalFlow.h - OpticalFlow Base Class for Ardupilot * AP_OpticalFlow.h - OpticalFlow Base Class for Ardupilot
* Code by Randy Mackay. DIYDrones.com * Code by Randy Mackay. DIYDrones.com
@ -25,10 +37,6 @@
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#ifndef HAL_MSP_OPTICALFLOW_ENABLED
#define HAL_MSP_OPTICALFLOW_ENABLED HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES
#endif
class OpticalFlow_backend; class OpticalFlow_backend;
class AP_AHRS; class AP_AHRS;
@ -143,3 +151,5 @@ namespace AP {
} }
#include "AP_OpticalFlow_Backend.h" #include "AP_OpticalFlow_Backend.h"
#endif // AP_OPTICALFLOW_ENABLED

View File

@ -15,6 +15,8 @@
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#if AP_OPTICALFLOW_ENABLED
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
OpticalFlow_backend::OpticalFlow_backend(OpticalFlow &_frontend) : OpticalFlow_backend::OpticalFlow_backend(OpticalFlow &_frontend) :
@ -46,3 +48,5 @@ void OpticalFlow_backend::_applyYaw(Vector2f &v)
v.x = cosYaw * x - sinYaw * y; v.x = cosYaw * x - sinYaw * y;
v.y = sinYaw * x + cosYaw * y; v.y = sinYaw * x + cosYaw * y;
} }
#endif

View File

@ -30,8 +30,11 @@
sensor sends packets at 25hz sensor sends packets at 25hz
*/ */
#include <AP_HAL/AP_HAL.h>
#include "AP_OpticalFlow_CXOF.h" #include "AP_OpticalFlow_CXOF.h"
#if AP_OPTICALFLOW_CXOF_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/crc.h> #include <AP_Math/crc.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
@ -199,3 +202,5 @@ void AP_OpticalFlow_CXOF::update(void)
gyro_sum.zero(); gyro_sum.zero();
gyro_sum_count = 0; gyro_sum_count = 0;
} }
#endif // AP_OPTICALFLOW_CXOF_ENABLED

View File

@ -1,6 +1,13 @@
#pragma once #pragma once
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#ifndef AP_OPTICALFLOW_CXOF_ENABLED
#define AP_OPTICALFLOW_CXOF_ENABLED AP_OPTICALFLOW_ENABLED
#endif
#if AP_OPTICALFLOW_CXOF_ENABLED
#include <AP_HAL/utility/OwnPtr.h> #include <AP_HAL/utility/OwnPtr.h>
class AP_OpticalFlow_CXOF : public OpticalFlow_backend class AP_OpticalFlow_CXOF : public OpticalFlow_backend
@ -27,3 +34,5 @@ private:
Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor
uint16_t gyro_sum_count; // number of gyro sensor values in sum uint16_t gyro_sum_count; // number of gyro sensor values in sum
}; };
#endif // AP_OPTICALFLOW_CXOF_ENABLED

View File

@ -1,9 +1,9 @@
#include <AP_HAL/AP_HAL.h>
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include "AP_OpticalFlow_HereFlow.h" #include "AP_OpticalFlow_HereFlow.h"
#if AP_OPTICALFLOW_HEREFLOW_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_CANManager/AP_CANManager.h> #include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_UAVCAN/AP_UAVCAN.h>
@ -101,5 +101,4 @@ void AP_OpticalFlow_HereFlow::_push_state(void)
new_data = false; new_data = false;
} }
#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS #endif // AP_OPTICALFLOW_HEREFLOW_ENABLED

View File

@ -1,7 +1,12 @@
#pragma once #pragma once
#include "AP_OpticalFlow_Backend.h"
#if HAL_ENABLE_LIBUAVCAN_DRIVERS #include "AP_OpticalFlow.h"
#ifndef AP_OPTICALFLOW_HEREFLOW_ENABLED
#define AP_OPTICALFLOW_HEREFLOW_ENABLED (AP_OPTICALFLOW_ENABLED && HAL_ENABLE_LIBUAVCAN_DRIVERS)
#endif
#if AP_OPTICALFLOW_HEREFLOW_ENABLED
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_UAVCAN/AP_UAVCAN.h>
@ -32,4 +37,5 @@ private:
void _push_state(void); void _push_state(void);
}; };
#endif //HAL_ENABLE_LIBUAVCAN_DRIVERS
#endif // AP_OPTICALFLOW_HEREFLOW_ENABLED

View File

@ -13,9 +13,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "AP_OpticalFlow_MAV.h"
#if AP_OPTICALFLOW_MAV_ENABLED
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include "AP_OpticalFlow_MAV.h"
#define OPTFLOW_MAV_TIMEOUT_SEC 0.5f #define OPTFLOW_MAV_TIMEOUT_SEC 0.5f
@ -104,3 +107,5 @@ void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg)
// take sensor id from message // take sensor id from message
sensor_id = packet.sensor_id; sensor_id = packet.sensor_id;
} }
#endif // AP_OPTICALFLOW_MAV_ENABLED

View File

@ -1,6 +1,13 @@
#pragma once #pragma once
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#ifndef AP_OPTICALFLOW_MAV_ENABLED
#define AP_OPTICALFLOW_MAV_ENABLED AP_OPTICALFLOW_ENABLED
#endif
#if AP_OPTICALFLOW_MAV_ENABLED
#include <AP_HAL/utility/OwnPtr.h> #include <AP_HAL/utility/OwnPtr.h>
class AP_OpticalFlow_MAV : public OpticalFlow_backend class AP_OpticalFlow_MAV : public OpticalFlow_backend
@ -32,3 +39,5 @@ private:
Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor
uint16_t gyro_sum_count; // number of gyro sensor values in sum uint16_t gyro_sum_count; // number of gyro sensor values in sum
}; };
#endif // AP_OPTICALFLOW_MAV_ENABLED

View File

@ -14,6 +14,8 @@
*/ */
#include "AP_OpticalFlow_Onboard.h" #include "AP_OpticalFlow_Onboard.h"
#if AP_OPTICALFLOW_ONBOARD_ENABLED
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
@ -98,3 +100,5 @@ void AP_OpticalFlow_Onboard::update()
} }
#endif #endif
#endif // AP_OPTICALFLOW_ONBOARD_ENABLED

View File

@ -14,6 +14,14 @@
*/ */
#pragma once #pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef AP_OPTICALFLOW_ONBOARD_ENABLED
#define AP_OPTICALFLOW_ONBOARD_ENABLED AP_OPTICALFLOW_ENABLED
#endif
#if AP_OPTICALFLOW_ONBOARD_ENABLED
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_NavEKF2/AP_NavEKF2.h> #include <AP_NavEKF2/AP_NavEKF2.h>
@ -30,3 +38,5 @@ public:
private: private:
uint32_t _last_read_ms; uint32_t _last_read_ms;
}; };
#endif // AP_OPTICALFLOW_ONBOARD_ENABLED

View File

@ -16,8 +16,11 @@
driver for PX4Flow optical flow sensor driver for PX4Flow optical flow sensor
*/ */
#include <AP_HAL/AP_HAL.h>
#include "AP_OpticalFlow_PX4Flow.h" #include "AP_OpticalFlow_PX4Flow.h"
#if AP_OPTICALFLOW_PX4FLOW_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/crc.h> #include <AP_Math/crc.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_HAL/I2CDevice.h> #include <AP_HAL/I2CDevice.h>
@ -135,3 +138,5 @@ void AP_OpticalFlow_PX4Flow::timer(void)
_update_frontend(state); _update_frontend(state);
} }
#endif // AP_OPTICALFLOW_PX4FLOW_ENABLED

View File

@ -1,6 +1,13 @@
#pragma once #pragma once
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#ifndef AP_OPTICALFLOW_PX4FLOW_ENABLED
#define AP_OPTICALFLOW_PX4FLOW_ENABLED AP_OPTICALFLOW_ENABLED
#endif
#if AP_OPTICALFLOW_PX4FLOW_ENABLED
#include <AP_HAL/utility/OwnPtr.h> #include <AP_HAL/utility/OwnPtr.h>
class AP_OpticalFlow_PX4Flow : public OpticalFlow_backend class AP_OpticalFlow_PX4Flow : public OpticalFlow_backend
@ -46,3 +53,5 @@ private:
void timer(void); void timer(void);
}; };
#endif // AP_OPTICALFLOW_PX4FLOW_ENABLED

View File

@ -20,12 +20,15 @@
timing for register reads and writes is critical timing for register reads and writes is critical
*/ */
#include "AP_OpticalFlow_Pixart.h"
#if AP_OPTICALFLOW_PIXART_ENABLED
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/crc.h> #include <AP_Math/crc.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <utility> #include <utility>
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#include "AP_OpticalFlow_Pixart.h"
#include "AP_OpticalFlow_Pixart_SROM.h" #include "AP_OpticalFlow_Pixart_SROM.h"
#include <stdio.h> #include <stdio.h>
@ -360,3 +363,5 @@ void AP_OpticalFlow_Pixart::update(void)
// copy results to front end // copy results to front end
_update_frontend(state); _update_frontend(state);
} }
#endif // AP_OPTICALFLOW_PIXART_ENABLED

View File

@ -1,6 +1,13 @@
#pragma once #pragma once
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#ifndef AP_OPTICALFLOW_PIXART_ENABLED
#define AP_OPTICALFLOW_PIXART_ENABLED AP_OPTICALFLOW_ENABLED
#endif
#if AP_OPTICALFLOW_PIXART_ENABLED
#include <AP_HAL/utility/OwnPtr.h> #include <AP_HAL/utility/OwnPtr.h>
class AP_OpticalFlow_Pixart : public OpticalFlow_backend class AP_OpticalFlow_Pixart : public OpticalFlow_backend
@ -75,3 +82,5 @@ private:
uint32_t last_burst_us; uint32_t last_burst_us;
uint32_t last_update_ms; uint32_t last_update_ms;
}; };
#endif // AP_OPTICALFLOW_PIXART_ENABLED

View File

@ -33,8 +33,11 @@
byte13:footer (0x55) byte13:footer (0x55)
*/ */
#include <AP_HAL/AP_HAL.h>
#include "AP_OpticalFlow_UPFLOW.h" #include "AP_OpticalFlow_UPFLOW.h"
#if AP_OPTICALFLOW_UPFLOW_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
#include <utility> #include <utility>
@ -187,3 +190,5 @@ void AP_OpticalFlow_UPFLOW::update(void)
gyro_sum.zero(); gyro_sum.zero();
gyro_sum_count = 0; gyro_sum_count = 0;
} }
#endif // AP_OPTICALFLOW_UPFLOW_ENABLED

View File

@ -1,6 +1,13 @@
#pragma once #pragma once
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#ifndef AP_OPTICALFLOW_UPFLOW_ENABLED
#define AP_OPTICALFLOW_UPFLOW_ENABLED AP_OPTICALFLOW_ENABLED
#endif
#if AP_OPTICALFLOW_UPFLOW_ENABLED
#include <AP_HAL/utility/OwnPtr.h> #include <AP_HAL/utility/OwnPtr.h>
class AP_OpticalFlow_UPFLOW : public OpticalFlow_backend class AP_OpticalFlow_UPFLOW : public OpticalFlow_backend
@ -34,3 +41,5 @@ private:
Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor
uint16_t gyro_sum_count; // number of gyro sensor values in sum uint16_t gyro_sum_count; // number of gyro sensor values in sum
}; };
#endif // AP_OPTICALFLOW_UPFLOW_ENABLED