mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 12:14:10 -04:00
AP_OpticalFlow: create and use AP_OPTICALFLOW_ENABLED
Including a define for each backend.
This commit is contained in:
parent
ca8436ba5d
commit
99ccbee474
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
Loading…
Reference in New Issue
Block a user