Browse Source

AP_OpticalFlow: create and use AP_OPTICALFLOW_ENABLED

Including a define for each backend.
gps-1.3.1
Peter Barker 3 years ago committed by Andrew Tridgell
parent
commit
99ccbee474
  1. 5
      libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
  2. 18
      libraries/AP_OpticalFlow/AP_OpticalFlow.h
  3. 4
      libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.cpp
  4. 7
      libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.cpp
  5. 9
      libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.h
  6. 9
      libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp
  7. 12
      libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h
  8. 7
      libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp
  9. 9
      libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h
  10. 4
      libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp
  11. 10
      libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.h
  12. 7
      libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp
  13. 9
      libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h
  14. 7
      libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp
  15. 9
      libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h
  16. 7
      libraries/AP_OpticalFlow/AP_OpticalFlow_UPFLOW.cpp
  17. 9
      libraries/AP_OpticalFlow/AP_OpticalFlow_UPFLOW.h

5
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

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

18
libraries/AP_OpticalFlow/AP_OpticalFlow.h

@ -14,6 +14,18 @@ @@ -14,6 +14,18 @@
*/
#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
* Code by Randy Mackay. DIYDrones.com
@ -25,10 +37,6 @@ @@ -25,10 +37,6 @@
#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 AP_AHRS;
@ -143,3 +151,5 @@ namespace AP { @@ -143,3 +151,5 @@ namespace AP {
}
#include "AP_OpticalFlow_Backend.h"
#endif // AP_OPTICALFLOW_ENABLED

4
libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.cpp

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

7
libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.cpp

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

9
libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.h

@ -1,6 +1,13 @@ @@ -1,6 +1,13 @@
#pragma once
#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>
class AP_OpticalFlow_CXOF : public OpticalFlow_backend
@ -27,3 +34,5 @@ private: @@ -27,3 +34,5 @@ private:
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
};
#endif // AP_OPTICALFLOW_CXOF_ENABLED

9
libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp

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

12
libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h

@ -1,7 +1,12 @@ @@ -1,7 +1,12 @@
#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>
@ -32,4 +37,5 @@ private: @@ -32,4 +37,5 @@ private:
void _push_state(void);
};
#endif //HAL_ENABLE_LIBUAVCAN_DRIVERS
#endif // AP_OPTICALFLOW_HEREFLOW_ENABLED

7
libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

@ -13,9 +13,12 @@ @@ -13,9 +13,12 @@
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_AHRS/AP_AHRS.h>
#include "AP_OpticalFlow_MAV.h"
#define OPTFLOW_MAV_TIMEOUT_SEC 0.5f
@ -104,3 +107,5 @@ void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg) @@ -104,3 +107,5 @@ void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg)
// take sensor id from message
sensor_id = packet.sensor_id;
}
#endif // AP_OPTICALFLOW_MAV_ENABLED

9
libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h

@ -1,6 +1,13 @@ @@ -1,6 +1,13 @@
#pragma once
#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>
class AP_OpticalFlow_MAV : public OpticalFlow_backend
@ -32,3 +39,5 @@ private: @@ -32,3 +39,5 @@ private:
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
};
#endif // AP_OPTICALFLOW_MAV_ENABLED

4
libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp

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

10
libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.h

@ -14,6 +14,14 @@ @@ -14,6 +14,14 @@
*/
#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_Math/AP_Math.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
@ -30,3 +38,5 @@ public: @@ -30,3 +38,5 @@ public:
private:
uint32_t _last_read_ms;
};
#endif // AP_OPTICALFLOW_ONBOARD_ENABLED

7
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp

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

9
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h

@ -1,6 +1,13 @@ @@ -1,6 +1,13 @@
#pragma once
#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>
class AP_OpticalFlow_PX4Flow : public OpticalFlow_backend
@ -46,3 +53,5 @@ private: @@ -46,3 +53,5 @@ private:
void timer(void);
};
#endif // AP_OPTICALFLOW_PX4FLOW_ENABLED

7
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp

@ -20,12 +20,15 @@ @@ -20,12 +20,15 @@
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_Math/crc.h>
#include <AP_AHRS/AP_AHRS.h>
#include <utility>
#include "AP_OpticalFlow.h"
#include "AP_OpticalFlow_Pixart.h"
#include "AP_OpticalFlow_Pixart_SROM.h"
#include <stdio.h>
@ -360,3 +363,5 @@ void AP_OpticalFlow_Pixart::update(void) @@ -360,3 +363,5 @@ void AP_OpticalFlow_Pixart::update(void)
// copy results to front end
_update_frontend(state);
}
#endif // AP_OPTICALFLOW_PIXART_ENABLED

9
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h

@ -1,6 +1,13 @@ @@ -1,6 +1,13 @@
#pragma once
#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>
class AP_OpticalFlow_Pixart : public OpticalFlow_backend
@ -75,3 +82,5 @@ private: @@ -75,3 +82,5 @@ private:
uint32_t last_burst_us;
uint32_t last_update_ms;
};
#endif // AP_OPTICALFLOW_PIXART_ENABLED

7
libraries/AP_OpticalFlow/AP_OpticalFlow_UPFLOW.cpp

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

9
libraries/AP_OpticalFlow/AP_OpticalFlow_UPFLOW.h

@ -1,6 +1,13 @@ @@ -1,6 +1,13 @@
#pragma once
#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>
class AP_OpticalFlow_UPFLOW : public OpticalFlow_backend
@ -34,3 +41,5 @@ private: @@ -34,3 +41,5 @@ private:
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
};
#endif // AP_OPTICALFLOW_UPFLOW_ENABLED

Loading…
Cancel
Save