AP_OpticalFlow: rename OpticalFlow class to AP_OpticalFlow

Brings us in-line with other classes in ArduPilot.

Removes ambiguity with AP_HAL::OpticalFlow which can cause compilation errors as we start to make code more portable across targets
This commit is contained in:
Peter Barker 2022-08-15 11:31:14 +10:00 committed by Peter Barker
parent c95ff4b82b
commit e45f938056
23 changed files with 92 additions and 98 deletions

View File

@ -19,22 +19,22 @@ extern const AP_HAL::HAL& hal;
#ifndef OPTICAL_FLOW_TYPE_DEFAULT #ifndef OPTICAL_FLOW_TYPE_DEFAULT
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 || defined(HAL_HAVE_PIXARTFLOW_SPI) #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 || defined(HAL_HAVE_PIXARTFLOW_SPI)
#define OPTICAL_FLOW_TYPE_DEFAULT OpticalFlowType::PIXART #define OPTICAL_FLOW_TYPE_DEFAULT Type::PIXART
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#define OPTICAL_FLOW_TYPE_DEFAULT OpticalFlowType::BEBOP #define OPTICAL_FLOW_TYPE_DEFAULT Type::BEBOP
#else #else
#define OPTICAL_FLOW_TYPE_DEFAULT OpticalFlowType::NONE #define OPTICAL_FLOW_TYPE_DEFAULT Type::NONE
#endif #endif
#endif #endif
const AP_Param::GroupInfo OpticalFlow::var_info[] = { const AP_Param::GroupInfo AP_OpticalFlow::var_info[] = {
// @Param: _TYPE // @Param: _TYPE
// @DisplayName: Optical flow sensor type // @DisplayName: Optical flow sensor type
// @Description: Optical flow sensor type // @Description: Optical flow sensor type
// @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink, 6:DroneCAN, 7:MSP, 8:UPFLOW // @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink, 6:DroneCAN, 7:MSP, 8:UPFLOW
// @User: Standard // @User: Standard
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO_FLAGS("_TYPE", 0, OpticalFlow, _type, (int8_t)OPTICAL_FLOW_TYPE_DEFAULT, AP_PARAM_FLAG_ENABLE), AP_GROUPINFO_FLAGS("_TYPE", 0, AP_OpticalFlow, _type, (float)OPTICAL_FLOW_TYPE_DEFAULT, AP_PARAM_FLAG_ENABLE),
// @Param: _FXSCALER // @Param: _FXSCALER
// @DisplayName: X axis optical flow scale factor correction // @DisplayName: X axis optical flow scale factor correction
@ -42,7 +42,7 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = {
// @Range: -200 +200 // @Range: -200 +200
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
AP_GROUPINFO("_FXSCALER", 1, OpticalFlow, _flowScalerX, 0), AP_GROUPINFO("_FXSCALER", 1, AP_OpticalFlow, _flowScalerX, 0),
// @Param: _FYSCALER // @Param: _FYSCALER
// @DisplayName: Y axis optical flow scale factor correction // @DisplayName: Y axis optical flow scale factor correction
@ -50,7 +50,7 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = {
// @Range: -200 +200 // @Range: -200 +200
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
AP_GROUPINFO("_FYSCALER", 2, OpticalFlow, _flowScalerY, 0), AP_GROUPINFO("_FYSCALER", 2, AP_OpticalFlow, _flowScalerY, 0),
// @Param: _ORIENT_YAW // @Param: _ORIENT_YAW
// @DisplayName: Flow sensor yaw alignment // @DisplayName: Flow sensor yaw alignment
@ -59,7 +59,7 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = {
// @Range: -17999 +18000 // @Range: -17999 +18000
// @Increment: 10 // @Increment: 10
// @User: Standard // @User: Standard
AP_GROUPINFO("_ORIENT_YAW", 3, OpticalFlow, _yawAngle_cd, 0), AP_GROUPINFO("_ORIENT_YAW", 3, AP_OpticalFlow, _yawAngle_cd, 0),
// @Param: _POS_X // @Param: _POS_X
// @DisplayName: X position offset // @DisplayName: X position offset
@ -84,44 +84,44 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = {
// @Range: -5 5 // @Range: -5 5
// @Increment: 0.01 // @Increment: 0.01
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_POS", 4, OpticalFlow, _pos_offset, 0.0f), AP_GROUPINFO("_POS", 4, AP_OpticalFlow, _pos_offset, 0.0f),
// @Param: _ADDR // @Param: _ADDR
// @DisplayName: Address on the bus // @DisplayName: Address on the bus
// @Description: This is used to select between multiple possible I2C addresses for some sensor types. For PX4Flow you can choose 0 to 7 for the 8 possible addresses on the I2C bus. // @Description: This is used to select between multiple possible I2C addresses for some sensor types. For PX4Flow you can choose 0 to 7 for the 8 possible addresses on the I2C bus.
// @Range: 0 127 // @Range: 0 127
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_ADDR", 5, OpticalFlow, _address, 0), AP_GROUPINFO("_ADDR", 5, AP_OpticalFlow, _address, 0),
AP_GROUPEND AP_GROUPEND
}; };
// default constructor // default constructor
OpticalFlow::OpticalFlow() AP_OpticalFlow::AP_OpticalFlow()
{ {
_singleton = this; _singleton = this;
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
void OpticalFlow::init(uint32_t log_bit) void AP_OpticalFlow::init(uint32_t log_bit)
{ {
_log_bit = log_bit; _log_bit = log_bit;
// return immediately if not enabled or backend already created // return immediately if not enabled or backend already created
if ((_type == (int8_t)OpticalFlowType::NONE) || (backend != nullptr)) { if ((_type == Type::NONE) || (backend != nullptr)) {
return; return;
} }
switch ((OpticalFlowType)_type.get()) { switch ((Type)_type) {
case OpticalFlowType::NONE: case Type::NONE:
break; break;
case OpticalFlowType::PX4FLOW: case Type::PX4FLOW:
#if AP_OPTICALFLOW_PX4FLOW_ENABLED #if AP_OPTICALFLOW_PX4FLOW_ENABLED
backend = AP_OpticalFlow_PX4Flow::detect(*this); backend = AP_OpticalFlow_PX4Flow::detect(*this);
#endif #endif
break; break;
case OpticalFlowType::PIXART: case Type::PIXART:
#if AP_OPTICALFLOW_PIXART_ENABLED #if AP_OPTICALFLOW_PIXART_ENABLED
backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this); backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this);
if (backend == nullptr) { if (backend == nullptr) {
@ -129,37 +129,37 @@ void OpticalFlow::init(uint32_t log_bit)
} }
#endif #endif
break; break;
case OpticalFlowType::BEBOP: case Type::BEBOP:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
backend = new AP_OpticalFlow_Onboard(*this); backend = new AP_OpticalFlow_Onboard(*this);
#endif #endif
break; break;
case OpticalFlowType::CXOF: case Type::CXOF:
#if AP_OPTICALFLOW_CXOF_ENABLED #if AP_OPTICALFLOW_CXOF_ENABLED
backend = AP_OpticalFlow_CXOF::detect(*this); backend = AP_OpticalFlow_CXOF::detect(*this);
#endif #endif
break; break;
case OpticalFlowType::MAVLINK: case Type::MAVLINK:
#if AP_OPTICALFLOW_MAV_ENABLED #if AP_OPTICALFLOW_MAV_ENABLED
backend = AP_OpticalFlow_MAV::detect(*this); backend = AP_OpticalFlow_MAV::detect(*this);
#endif #endif
break; break;
case OpticalFlowType::UAVCAN: case Type::UAVCAN:
#if AP_OPTICALFLOW_HEREFLOW_ENABLED #if AP_OPTICALFLOW_HEREFLOW_ENABLED
backend = new AP_OpticalFlow_HereFlow(*this); backend = new AP_OpticalFlow_HereFlow(*this);
#endif #endif
break; break;
case OpticalFlowType::MSP: case Type::MSP:
#if HAL_MSP_OPTICALFLOW_ENABLED #if HAL_MSP_OPTICALFLOW_ENABLED
backend = AP_OpticalFlow_MSP::detect(*this); backend = AP_OpticalFlow_MSP::detect(*this);
#endif #endif
break; break;
case OpticalFlowType::UPFLOW: case Type::UPFLOW:
#if AP_OPTICALFLOW_UPFLOW_ENABLED #if AP_OPTICALFLOW_UPFLOW_ENABLED
backend = AP_OpticalFlow_UPFLOW::detect(*this); backend = AP_OpticalFlow_UPFLOW::detect(*this);
#endif #endif
break; break;
case OpticalFlowType::SITL: case Type::SITL:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
backend = new AP_OpticalFlow_SITL(*this); backend = new AP_OpticalFlow_SITL(*this);
#endif #endif
@ -171,7 +171,7 @@ void OpticalFlow::init(uint32_t log_bit)
} }
} }
void OpticalFlow::update(void) void AP_OpticalFlow::update(void)
{ {
// exit immediately if not enabled // exit immediately if not enabled
if (!enabled()) { if (!enabled()) {
@ -200,7 +200,7 @@ void OpticalFlow::update(void)
} }
} }
void OpticalFlow::handle_msg(const mavlink_message_t &msg) void AP_OpticalFlow::handle_msg(const mavlink_message_t &msg)
{ {
// exit immediately if not enabled // exit immediately if not enabled
if (!enabled()) { if (!enabled()) {
@ -213,7 +213,7 @@ void OpticalFlow::handle_msg(const mavlink_message_t &msg)
} }
#if HAL_MSP_OPTICALFLOW_ENABLED #if HAL_MSP_OPTICALFLOW_ENABLED
void OpticalFlow::handle_msp(const MSP::msp_opflow_data_message_t &pkt) void AP_OpticalFlow::handle_msp(const MSP::msp_opflow_data_message_t &pkt)
{ {
// exit immediately if not enabled // exit immediately if not enabled
if (!enabled()) { if (!enabled()) {
@ -227,7 +227,7 @@ void OpticalFlow::handle_msp(const MSP::msp_opflow_data_message_t &pkt)
#endif //HAL_MSP_OPTICALFLOW_ENABLED #endif //HAL_MSP_OPTICALFLOW_ENABLED
// start calibration // start calibration
void OpticalFlow::start_calibration() void AP_OpticalFlow::start_calibration()
{ {
if (_calibrator == nullptr) { if (_calibrator == nullptr) {
_calibrator = new AP_OpticalFlow_Calibrator(); _calibrator = new AP_OpticalFlow_Calibrator();
@ -242,14 +242,14 @@ void OpticalFlow::start_calibration()
} }
// stop calibration // stop calibration
void OpticalFlow::stop_calibration() void AP_OpticalFlow::stop_calibration()
{ {
if (_calibrator != nullptr) { if (_calibrator != nullptr) {
_calibrator->stop(); _calibrator->stop();
} }
} }
void OpticalFlow::update_state(const OpticalFlow_state &state) void AP_OpticalFlow::update_state(const OpticalFlow_state &state)
{ {
_state = state; _state = state;
_last_update_ms = AP_HAL::millis(); _last_update_ms = AP_HAL::millis();
@ -263,7 +263,7 @@ void OpticalFlow::update_state(const OpticalFlow_state &state)
Log_Write_Optflow(); Log_Write_Optflow();
} }
void OpticalFlow::Log_Write_Optflow() void AP_OpticalFlow::Log_Write_Optflow()
{ {
AP_Logger *logger = AP_Logger::get_singleton(); AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) { if (logger == nullptr) {
@ -289,13 +289,13 @@ void OpticalFlow::Log_Write_Optflow()
// singleton instance // singleton instance
OpticalFlow *OpticalFlow::_singleton; AP_OpticalFlow *AP_OpticalFlow::_singleton;
namespace AP { namespace AP {
OpticalFlow *opticalflow() AP_OpticalFlow *opticalflow()
{ {
return OpticalFlow::get_singleton(); return AP_OpticalFlow::get_singleton();
} }
} }

View File

@ -37,23 +37,21 @@
class OpticalFlow_backend; class OpticalFlow_backend;
class OpticalFlow class AP_OpticalFlow
{ {
friend class OpticalFlow_backend; friend class OpticalFlow_backend;
public: public:
OpticalFlow(); AP_OpticalFlow();
/* Do not allow copies */ CLASS_NO_COPY(AP_OpticalFlow);
OpticalFlow(const OpticalFlow &other) = delete;
OpticalFlow &operator=(const OpticalFlow&) = delete;
// get singleton instance // get singleton instance
static OpticalFlow *get_singleton() { static AP_OpticalFlow *get_singleton() {
return _singleton; return _singleton;
} }
enum class OpticalFlowType { enum class Type {
NONE = 0, NONE = 0,
PX4FLOW = 1, PX4FLOW = 1,
PIXART = 2, PIXART = 2,
@ -70,7 +68,7 @@ public:
void init(uint32_t log_bit); void init(uint32_t log_bit);
// enabled - returns true if optical flow is enabled // enabled - returns true if optical flow is enabled
bool enabled() const { return _type != (int8_t)OpticalFlowType::NONE; } bool enabled() const { return _type != Type::NONE; }
// healthy - return true if the sensor is healthy // healthy - return true if the sensor is healthy
bool healthy() const { return backend != nullptr && _flags.healthy; } bool healthy() const { return backend != nullptr && _flags.healthy; }
@ -118,7 +116,7 @@ public:
private: private:
static OpticalFlow *_singleton; static AP_OpticalFlow *_singleton;
OpticalFlow_backend *backend; OpticalFlow_backend *backend;
@ -127,7 +125,7 @@ private:
} _flags; } _flags;
// parameters // parameters
AP_Int8 _type; // user configurable sensor type AP_Enum<Type> _type; // user configurable sensor type
AP_Int16 _flowScalerX; // X axis flow scale factor correction - parts per thousand AP_Int16 _flowScalerX; // X axis flow scale factor correction - parts per thousand
AP_Int16 _flowScalerY; // Y axis flow scale factor correction - parts per thousand AP_Int16 _flowScalerY; // Y axis flow scale factor correction - parts per thousand
AP_Int16 _yawAngle_cd; // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees AP_Int16 _yawAngle_cd; // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees
@ -151,7 +149,7 @@ private:
}; };
namespace AP { namespace AP {
OpticalFlow *opticalflow(); AP_OpticalFlow *opticalflow();
} }
#include "AP_OpticalFlow_Backend.h" #include "AP_OpticalFlow_Backend.h"

View File

@ -19,7 +19,7 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
OpticalFlow_backend::OpticalFlow_backend(OpticalFlow &_frontend) : OpticalFlow_backend::OpticalFlow_backend(AP_OpticalFlow &_frontend) :
frontend(_frontend) frontend(_frontend)
{ {
} }
@ -29,7 +29,7 @@ OpticalFlow_backend::~OpticalFlow_backend(void)
} }
// update the frontend // update the frontend
void OpticalFlow_backend::_update_frontend(const struct OpticalFlow::OpticalFlow_state &state) void OpticalFlow_backend::_update_frontend(const struct AP_OpticalFlow::OpticalFlow_state &state)
{ {
frontend.update_state(state); frontend.update_state(state);
} }

View File

@ -15,20 +15,22 @@
#pragma once #pragma once
/* /*
OpticalFlow backend class for ArduPilot AP_OpticalFlow backend class for ArduPilot
*/ */
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
class OpticalFlow_backend class OpticalFlow_backend
{ {
friend class OpticalFlow; friend class AP_OpticalFlow;
public: public:
// constructor // constructor
OpticalFlow_backend(OpticalFlow &_frontend); OpticalFlow_backend(AP_OpticalFlow &_frontend);
virtual ~OpticalFlow_backend(void); virtual ~OpticalFlow_backend(void);
CLASS_NO_COPY(OpticalFlow_backend);
// init - initialise sensor // init - initialise sensor
virtual void init() = 0; virtual void init() = 0;
@ -45,10 +47,10 @@ public:
protected: protected:
// access to frontend // access to frontend
OpticalFlow &frontend; AP_OpticalFlow &frontend;
// update the frontend // update the frontend
void _update_frontend(const struct OpticalFlow::OpticalFlow_state &state); void _update_frontend(const struct AP_OpticalFlow::OpticalFlow_state &state);
// get the flow scaling parameters // get the flow scaling parameters
Vector2f _flowScaler(void) const { return Vector2f(frontend._flowScalerX, frontend._flowScalerY); } Vector2f _flowScaler(void) const { return Vector2f(frontend._flowScalerX, frontend._flowScalerY); }

View File

@ -51,14 +51,14 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// constructor // constructor
AP_OpticalFlow_CXOF::AP_OpticalFlow_CXOF(OpticalFlow &_frontend, AP_HAL::UARTDriver *_uart) : AP_OpticalFlow_CXOF::AP_OpticalFlow_CXOF(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *_uart) :
OpticalFlow_backend(_frontend), OpticalFlow_backend(_frontend),
uart(_uart) uart(_uart)
{ {
} }
// detect the device // detect the device
AP_OpticalFlow_CXOF *AP_OpticalFlow_CXOF::detect(OpticalFlow &_frontend) AP_OpticalFlow_CXOF *AP_OpticalFlow_CXOF::detect(AP_OpticalFlow &_frontend)
{ {
AP_SerialManager *serial_manager = AP::serialmanager().get_singleton(); AP_SerialManager *serial_manager = AP::serialmanager().get_singleton();
if (serial_manager == nullptr) { if (serial_manager == nullptr) {
@ -159,7 +159,7 @@ void AP_OpticalFlow_CXOF::update(void)
return; return;
} }
struct OpticalFlow::OpticalFlow_state state {}; struct AP_OpticalFlow::OpticalFlow_state state {};
// average surface quality scaled to be between 0 and 255 // average surface quality scaled to be between 0 and 255
state.surface_quality = (constrain_int16(qual_sum / count, 64, 78) - 64) * 255 / 14; state.surface_quality = (constrain_int16(qual_sum / count, 64, 78) - 64) * 255 / 14;

View File

@ -14,7 +14,7 @@ class AP_OpticalFlow_CXOF : public OpticalFlow_backend
{ {
public: public:
/// constructor /// constructor
AP_OpticalFlow_CXOF(OpticalFlow &_frontend, AP_HAL::UARTDriver *uart); AP_OpticalFlow_CXOF(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *uart);
// initialise the sensor // initialise the sensor
void init() override; void init() override;
@ -23,7 +23,7 @@ public:
void update(void) override; void update(void) override;
// detect if the sensor is available // detect if the sensor is available
static AP_OpticalFlow_CXOF *detect(OpticalFlow &_frontend); static AP_OpticalFlow_CXOF *detect(AP_OpticalFlow &_frontend);
private: private:

View File

@ -20,7 +20,7 @@ AP_UAVCAN* AP_OpticalFlow_HereFlow::_ap_uavcan = nullptr;
/* /*
constructor - registers instance at top Flow driver constructor - registers instance at top Flow driver
*/ */
AP_OpticalFlow_HereFlow::AP_OpticalFlow_HereFlow(OpticalFlow &flow) : AP_OpticalFlow_HereFlow::AP_OpticalFlow_HereFlow(AP_OpticalFlow &flow) :
OpticalFlow_backend(flow) OpticalFlow_backend(flow)
{ {
if (_driver) { if (_driver) {
@ -83,7 +83,7 @@ void AP_OpticalFlow_HereFlow::_push_state(void)
if (!new_data) { if (!new_data) {
return; return;
} }
struct OpticalFlow::OpticalFlow_state state; struct AP_OpticalFlow::OpticalFlow_state state;
const Vector2f flowScaler = _flowScaler(); const Vector2f flowScaler = _flowScaler();
//setup scaling based on parameters //setup scaling based on parameters
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;

View File

@ -14,7 +14,7 @@ class MeasurementCb;
class AP_OpticalFlow_HereFlow : public OpticalFlow_backend { class AP_OpticalFlow_HereFlow : public OpticalFlow_backend {
public: public:
AP_OpticalFlow_HereFlow(OpticalFlow &flow); AP_OpticalFlow_HereFlow(AP_OpticalFlow &flow);
void init() override {} void init() override {}

View File

@ -23,7 +23,7 @@
#define OPTFLOW_MAV_TIMEOUT_SEC 0.5f #define OPTFLOW_MAV_TIMEOUT_SEC 0.5f
// detect the device // detect the device
AP_OpticalFlow_MAV *AP_OpticalFlow_MAV::detect(OpticalFlow &_frontend) AP_OpticalFlow_MAV *AP_OpticalFlow_MAV::detect(AP_OpticalFlow &_frontend)
{ {
// we assume mavlink messages will be sent into this driver // we assume mavlink messages will be sent into this driver
AP_OpticalFlow_MAV *sensor = new AP_OpticalFlow_MAV(_frontend); AP_OpticalFlow_MAV *sensor = new AP_OpticalFlow_MAV(_frontend);
@ -47,7 +47,7 @@ void AP_OpticalFlow_MAV::update(void)
return; return;
} }
struct OpticalFlow::OpticalFlow_state state {}; struct AP_OpticalFlow::OpticalFlow_state state {};
state.surface_quality = quality_sum / count; state.surface_quality = quality_sum / count;

View File

@ -26,7 +26,7 @@ public:
void handle_msg(const mavlink_message_t &msg) override; void handle_msg(const mavlink_message_t &msg) override;
// detect if the sensor is available // detect if the sensor is available
static AP_OpticalFlow_MAV *detect(OpticalFlow &_frontend); static AP_OpticalFlow_MAV *detect(AP_OpticalFlow &_frontend);
private: private:

View File

@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal;
using namespace MSP; using namespace MSP;
// detect the device // detect the device
AP_OpticalFlow_MSP *AP_OpticalFlow_MSP::detect(OpticalFlow &_frontend) AP_OpticalFlow_MSP *AP_OpticalFlow_MSP::detect(AP_OpticalFlow &_frontend)
{ {
// we assume msp messages will be sent into this driver // we assume msp messages will be sent into this driver
return new AP_OpticalFlow_MSP(_frontend); return new AP_OpticalFlow_MSP(_frontend);
@ -49,7 +49,7 @@ void AP_OpticalFlow_MSP::update(void)
return; return;
} }
struct OpticalFlow::OpticalFlow_state state {}; struct AP_OpticalFlow::OpticalFlow_state state {};
state.surface_quality = quality_sum / count; state.surface_quality = quality_sum / count;

View File

@ -21,7 +21,7 @@ public:
void handle_msp(const MSP::msp_opflow_data_message_t &pkt) override; void handle_msp(const MSP::msp_opflow_data_message_t &pkt) override;
// detect if the sensor is available // detect if the sensor is available
static AP_OpticalFlow_MSP *detect(OpticalFlow &_frontend); static AP_OpticalFlow_MSP *detect(AP_OpticalFlow &_frontend);
private: private:

View File

@ -30,10 +30,6 @@
#define OPTICALFLOW_ONBOARD_ID 1 #define OPTICALFLOW_ONBOARD_ID 1
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_OpticalFlow_Onboard::AP_OpticalFlow_Onboard(OpticalFlow &_frontend) :
OpticalFlow_backend(_frontend)
{}
void AP_OpticalFlow_Onboard::init(void) void AP_OpticalFlow_Onboard::init(void)
{ {
/* register callback to get gyro data */ /* register callback to get gyro data */
@ -54,7 +50,7 @@ void AP_OpticalFlow_Onboard::update()
return; return;
} }
struct OpticalFlow::OpticalFlow_state state; struct AP_OpticalFlow::OpticalFlow_state state;
state.surface_quality = data_frame.quality; state.surface_quality = data_frame.quality;
if (data_frame.delta_time > 0) { if (data_frame.delta_time > 0) {
const Vector2f flowScaler = _flowScaler(); const Vector2f flowScaler = _flowScaler();

View File

@ -32,7 +32,9 @@
class AP_OpticalFlow_Onboard : public OpticalFlow_backend class AP_OpticalFlow_Onboard : public OpticalFlow_backend
{ {
public: public:
AP_OpticalFlow_Onboard(OpticalFlow &_frontend);
using OpticalFlow_backend::OpticalFlow_backend;
void init(void) override; void init(void) override;
void update(void) override; void update(void) override;
private: private:

View File

@ -34,15 +34,9 @@ extern const AP_HAL::HAL& hal;
#define PX4FLOW_BASE_I2C_ADDR 0x42 #define PX4FLOW_BASE_I2C_ADDR 0x42
#define PX4FLOW_INIT_RETRIES 10 // attempt to initialise the sensor up to 10 times at startup #define PX4FLOW_INIT_RETRIES 10 // attempt to initialise the sensor up to 10 times at startup
// constructor
AP_OpticalFlow_PX4Flow::AP_OpticalFlow_PX4Flow(OpticalFlow &_frontend) :
OpticalFlow_backend(_frontend)
{
}
// detect the device // detect the device
AP_OpticalFlow_PX4Flow *AP_OpticalFlow_PX4Flow::detect(OpticalFlow &_frontend) AP_OpticalFlow_PX4Flow *AP_OpticalFlow_PX4Flow::detect(AP_OpticalFlow &_frontend)
{ {
AP_OpticalFlow_PX4Flow *sensor = new AP_OpticalFlow_PX4Flow(_frontend); AP_OpticalFlow_PX4Flow *sensor = new AP_OpticalFlow_PX4Flow(_frontend);
if (!sensor) { if (!sensor) {
@ -119,7 +113,7 @@ void AP_OpticalFlow_PX4Flow::timer(void)
if (!dev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame))) { if (!dev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame))) {
return; return;
} }
struct OpticalFlow::OpticalFlow_state state {}; struct AP_OpticalFlow::OpticalFlow_state state {};
if (frame.integration_timespan > 0) { if (frame.integration_timespan > 0) {
const Vector2f flowScaler = _flowScaler(); const Vector2f flowScaler = _flowScaler();

View File

@ -14,7 +14,9 @@ class AP_OpticalFlow_PX4Flow : public OpticalFlow_backend
{ {
public: public:
/// constructor /// constructor
AP_OpticalFlow_PX4Flow(OpticalFlow &_frontend); using OpticalFlow_backend::OpticalFlow_backend;
CLASS_NO_COPY(AP_OpticalFlow_PX4Flow);
// init - initialise the sensor // init - initialise the sensor
void init() override {} void init() override {}
@ -23,7 +25,7 @@ public:
void update(void) override; void update(void) override;
// detect if the sensor is available // detect if the sensor is available
static AP_OpticalFlow_PX4Flow *detect(OpticalFlow &_frontend); static AP_OpticalFlow_PX4Flow *detect(AP_OpticalFlow &_frontend);
private: private:
AP_HAL::OwnPtr<AP_HAL::Device> dev; AP_HAL::OwnPtr<AP_HAL::Device> dev;

View File

@ -83,14 +83,14 @@ extern const AP_HAL::HAL& hal;
#define PIXART_SROM_CRC_RESULT 0xBEEF #define PIXART_SROM_CRC_RESULT 0xBEEF
// constructor // constructor
AP_OpticalFlow_Pixart::AP_OpticalFlow_Pixart(const char *devname, OpticalFlow &_frontend) : AP_OpticalFlow_Pixart::AP_OpticalFlow_Pixart(const char *devname, AP_OpticalFlow &_frontend) :
OpticalFlow_backend(_frontend) OpticalFlow_backend(_frontend)
{ {
_dev = std::move(hal.spi->get_device(devname)); _dev = std::move(hal.spi->get_device(devname));
} }
// detect the device // detect the device
AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, OpticalFlow &_frontend) AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, AP_OpticalFlow &_frontend)
{ {
AP_OpticalFlow_Pixart *sensor = new AP_OpticalFlow_Pixart(devname, _frontend); AP_OpticalFlow_Pixart *sensor = new AP_OpticalFlow_Pixart(devname, _frontend);
if (!sensor) { if (!sensor) {
@ -329,7 +329,7 @@ void AP_OpticalFlow_Pixart::update(void)
} }
last_update_ms = now; last_update_ms = now;
struct OpticalFlow::OpticalFlow_state state; struct AP_OpticalFlow::OpticalFlow_state state;
state.surface_quality = burst.squal; state.surface_quality = burst.squal;
if (integral.sum_us > 0) { if (integral.sum_us > 0) {

View File

@ -14,7 +14,7 @@ class AP_OpticalFlow_Pixart : public OpticalFlow_backend
{ {
public: public:
/// constructor /// constructor
AP_OpticalFlow_Pixart(const char *devname, OpticalFlow &_frontend); AP_OpticalFlow_Pixart(const char *devname, AP_OpticalFlow &_frontend);
// init - initialise the sensor // init - initialise the sensor
void init() override {} void init() override {}
@ -23,7 +23,7 @@ public:
void update(void) override; void update(void) override;
// detect if the sensor is available // detect if the sensor is available
static AP_OpticalFlow_Pixart *detect(const char *devname, OpticalFlow &_frontend); static AP_OpticalFlow_Pixart *detect(const char *devname, AP_OpticalFlow &_frontend);
private: private:
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev; AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;

View File

@ -25,7 +25,7 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_OpticalFlow_SITL::AP_OpticalFlow_SITL(OpticalFlow &_frontend) : AP_OpticalFlow_SITL::AP_OpticalFlow_SITL(AP_OpticalFlow &_frontend) :
OpticalFlow_backend(_frontend), OpticalFlow_backend(_frontend),
_sitl(AP::sitl()) _sitl(AP::sitl())
{ {
@ -52,7 +52,7 @@ void AP_OpticalFlow_SITL::update(void)
radians(_sitl->state.pitchRate), radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate)); radians(_sitl->state.yawRate));
OpticalFlow::OpticalFlow_state state; AP_OpticalFlow::OpticalFlow_state state;
// NED velocity vector in m/s // NED velocity vector in m/s
Vector3f velocity(_sitl->state.speedN, Vector3f velocity(_sitl->state.speedN,

View File

@ -8,7 +8,7 @@ class AP_OpticalFlow_SITL : public OpticalFlow_backend
{ {
public: public:
/// constructor /// constructor
AP_OpticalFlow_SITL(OpticalFlow &_frontend); AP_OpticalFlow_SITL(AP_OpticalFlow &_frontend);
// init - initialise the sensor // init - initialise the sensor
void init() override; void init() override;
@ -22,6 +22,6 @@ private:
uint8_t next_optflow_index; uint8_t next_optflow_index;
uint8_t optflow_delay; uint8_t optflow_delay;
OpticalFlow::OpticalFlow_state optflow_data[20]; AP_OpticalFlow::OpticalFlow_state optflow_data[20];
}; };
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -53,14 +53,14 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// constructor // constructor
AP_OpticalFlow_UPFLOW::AP_OpticalFlow_UPFLOW(OpticalFlow &_frontend, AP_HAL::UARTDriver *_uart) : AP_OpticalFlow_UPFLOW::AP_OpticalFlow_UPFLOW(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *_uart) :
OpticalFlow_backend(_frontend), OpticalFlow_backend(_frontend),
uart(_uart) uart(_uart)
{ {
} }
// detect the device // detect the device
AP_OpticalFlow_UPFLOW *AP_OpticalFlow_UPFLOW::detect(OpticalFlow &_frontend) AP_OpticalFlow_UPFLOW *AP_OpticalFlow_UPFLOW::detect(AP_OpticalFlow &_frontend)
{ {
AP_SerialManager *serial_manager = AP::serialmanager().get_singleton(); AP_SerialManager *serial_manager = AP::serialmanager().get_singleton();
if (serial_manager == nullptr) { if (serial_manager == nullptr) {
@ -156,7 +156,7 @@ void AP_OpticalFlow_UPFLOW::update(void)
return; return;
} }
struct OpticalFlow::OpticalFlow_state state {}; struct AP_OpticalFlow::OpticalFlow_state state {};
state.surface_quality = updata.quality; state.surface_quality = updata.quality;

View File

@ -14,7 +14,7 @@ class AP_OpticalFlow_UPFLOW : public OpticalFlow_backend
{ {
public: public:
/// constructor /// constructor
AP_OpticalFlow_UPFLOW(OpticalFlow &_frontend, AP_HAL::UARTDriver *uart); AP_OpticalFlow_UPFLOW(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *uart);
// initialise the sensor // initialise the sensor
void init() override; void init() override;
@ -23,7 +23,7 @@ public:
void update(void) override; void update(void) override;
// detect if the sensor is available // detect if the sensor is available
static AP_OpticalFlow_UPFLOW *detect(OpticalFlow &_frontend); static AP_OpticalFlow_UPFLOW *detect(AP_OpticalFlow &_frontend);
private: private:
struct PACKED UpixelsOpticalFlow { struct PACKED UpixelsOpticalFlow {

View File

@ -32,7 +32,7 @@ public:
static DummyVehicle vehicle; static DummyVehicle vehicle;
#if AP_OPTICALFLOW_ENABLED #if AP_OPTICALFLOW_ENABLED
static OpticalFlow optflow; static AP_OpticalFlow optflow;
#endif #endif
void setup() void setup()