mirror of https://github.com/ArduPilot/ardupilot
AP_OpticalFlow: allow separate enable of MSP opticalflow
This commit is contained in:
parent
34430e9d6c
commit
3517ab9c45
|
@ -17,12 +17,14 @@
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
#include "AP_OpticalFlow_MSP.h"
|
#include "AP_OpticalFlow_MSP.h"
|
||||||
|
|
||||||
#if HAL_MSP_ENABLED
|
#if HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
|
|
||||||
#define OPTFLOW_MSP_TIMEOUT_SEC 0.5f // 2Hz
|
#define OPTFLOW_MSP_TIMEOUT_SEC 0.5f // 2Hz
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
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(OpticalFlow &_frontend)
|
||||||
{
|
{
|
||||||
|
@ -89,7 +91,7 @@ void AP_OpticalFlow_MSP::update(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
// handle OPTICAL_FLOW msp messages
|
// handle OPTICAL_FLOW msp messages
|
||||||
void AP_OpticalFlow_MSP::handle_msp(const msp_opflow_sensor_t &pkt)
|
void AP_OpticalFlow_MSP::handle_msp(const MSP::msp_opflow_sensor_t &pkt)
|
||||||
{
|
{
|
||||||
// record time message was received
|
// record time message was received
|
||||||
// ToDo: add jitter correction
|
// ToDo: add jitter correction
|
||||||
|
@ -102,4 +104,5 @@ void AP_OpticalFlow_MSP::handle_msp(const msp_opflow_sensor_t &pkt)
|
||||||
count++;
|
count++;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif //HAL_MSP_ENABLED
|
#endif // HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
|
|
||||||
|
|
|
@ -3,7 +3,7 @@
|
||||||
#include "OpticalFlow.h"
|
#include "OpticalFlow.h"
|
||||||
#include <AP_HAL/utility/OwnPtr.h>
|
#include <AP_HAL/utility/OwnPtr.h>
|
||||||
|
|
||||||
#if HAL_MSP_ENABLED
|
#if HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
|
|
||||||
class AP_OpticalFlow_MSP : public OpticalFlow_backend
|
class AP_OpticalFlow_MSP : public OpticalFlow_backend
|
||||||
{
|
{
|
||||||
|
@ -18,7 +18,7 @@ public:
|
||||||
void update(void) override;
|
void update(void) override;
|
||||||
|
|
||||||
// get update from msp
|
// get update from msp
|
||||||
void handle_msp(const msp_opflow_sensor_t &pkt) override;
|
void handle_msp(const MSP::msp_opflow_sensor_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(OpticalFlow &_frontend);
|
||||||
|
@ -34,4 +34,4 @@ private:
|
||||||
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 //HAL_MSP_ENABLED
|
#endif // HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
|
|
|
@ -137,9 +137,9 @@ void OpticalFlow::init(uint32_t log_bit)
|
||||||
#endif
|
#endif
|
||||||
break;
|
break;
|
||||||
case OpticalFlowType::MSP:
|
case OpticalFlowType::MSP:
|
||||||
#if HAL_MSP_ENABLED
|
#if HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
backend = AP_OpticalFlow_MSP::detect(*this);
|
backend = AP_OpticalFlow_MSP::detect(*this);
|
||||||
#endif //HAL_MSP_ENABLED
|
#endif
|
||||||
break;
|
break;
|
||||||
case OpticalFlowType::SITL:
|
case OpticalFlowType::SITL:
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
@ -179,8 +179,8 @@ void OpticalFlow::handle_msg(const mavlink_message_t &msg)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#if HAL_MSP_ENABLED
|
#if HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
void OpticalFlow::handle_msp(const msp_opflow_sensor_t &pkt)
|
void OpticalFlow::handle_msp(const MSP::msp_opflow_sensor_t &pkt)
|
||||||
{
|
{
|
||||||
// exit immediately if not enabled
|
// exit immediately if not enabled
|
||||||
if (!enabled()) {
|
if (!enabled()) {
|
||||||
|
@ -191,7 +191,7 @@ void OpticalFlow::handle_msp(const msp_opflow_sensor_t &pkt)
|
||||||
backend->handle_msp(pkt);
|
backend->handle_msp(pkt);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif //HAL_MSP_ENABLED
|
#endif //HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
|
|
||||||
void OpticalFlow::update_state(const OpticalFlow_state &state)
|
void OpticalFlow::update_state(const OpticalFlow_state &state)
|
||||||
{
|
{
|
||||||
|
|
|
@ -24,9 +24,10 @@
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||||
|
|
||||||
#if HAL_MSP_ENABLED
|
|
||||||
using namespace MSP;
|
#ifndef HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
#endif //HAL_MSP_ENABLED
|
#define HAL_MSP_OPTICALFLOW_ENABLED HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES
|
||||||
|
#endif
|
||||||
|
|
||||||
class OpticalFlow_backend;
|
class OpticalFlow_backend;
|
||||||
class AP_AHRS_NavEKF;
|
class AP_AHRS_NavEKF;
|
||||||
|
@ -74,10 +75,10 @@ public:
|
||||||
// handle optical flow mavlink messages
|
// handle optical flow mavlink messages
|
||||||
void handle_msg(const mavlink_message_t &msg);
|
void handle_msg(const mavlink_message_t &msg);
|
||||||
|
|
||||||
#if HAL_MSP_ENABLED
|
#if HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
// handle optical flow msp messages
|
// handle optical flow msp messages
|
||||||
void handle_msp(const msp_opflow_sensor_t &pkt);
|
void handle_msp(const MSP::msp_opflow_sensor_t &pkt);
|
||||||
#endif //HAL_MSP_ENABLED
|
#endif
|
||||||
|
|
||||||
// quality - returns the surface quality as a measure from 0 ~ 255
|
// quality - returns the surface quality as a measure from 0 ~ 255
|
||||||
uint8_t quality() const { return _state.surface_quality; }
|
uint8_t quality() const { return _state.surface_quality; }
|
||||||
|
|
|
@ -38,10 +38,11 @@ public:
|
||||||
// handle optical flow mavlink messages
|
// handle optical flow mavlink messages
|
||||||
virtual void handle_msg(const mavlink_message_t &msg) {}
|
virtual void handle_msg(const mavlink_message_t &msg) {}
|
||||||
|
|
||||||
#if HAL_MSP_ENABLED
|
#if HAL_MSP_OPTICALFLOW_ENABLED
|
||||||
// handle optical flow msp messages
|
// handle optical flow msp messages
|
||||||
virtual void handle_msp(const MSP::msp_opflow_sensor_t &pkt) {}
|
virtual void handle_msp(const MSP::msp_opflow_sensor_t &pkt) {}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// access to frontend
|
// access to frontend
|
||||||
OpticalFlow &frontend;
|
OpticalFlow &frontend;
|
||||||
|
|
Loading…
Reference in New Issue