Browse Source

AP_OpticalFlow: allow use of OpticalFlow on SimOnHardWare

master
Peter Barker 2 years ago committed by Andrew Tridgell
parent
commit
46aebe3020
  1. 2
      libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
  2. 4
      libraries/AP_OpticalFlow/AP_OpticalFlow.h
  3. 2
      libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h
  4. 21
      libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp
  5. 14
      libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.h

2
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@ -160,7 +160,7 @@ void AP_OpticalFlow::init(uint32_t log_bit)
#endif #endif
break; break;
case Type::SITL: case Type::SITL:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if AP_OPTICALFLOW_SITL_ENABLED
backend = new AP_OpticalFlow_SITL(*this); backend = new AP_OpticalFlow_SITL(*this);
#endif #endif
break; break;

4
libraries/AP_OpticalFlow/AP_OpticalFlow.h

@ -24,6 +24,10 @@
#define HAL_MSP_OPTICALFLOW_ENABLED (AP_OPTICALFLOW_ENABLED && (HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES)) #define HAL_MSP_OPTICALFLOW_ENABLED (AP_OPTICALFLOW_ENABLED && (HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES))
#endif #endif
#ifndef AP_OPTICALFLOW_SITL_ENABLED
#define AP_OPTICALFLOW_SITL_ENABLED AP_SIM_ENABLED
#endif
#if AP_OPTICALFLOW_ENABLED #if AP_OPTICALFLOW_ENABLED
/* /*

2
libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h

@ -32,7 +32,7 @@ public:
CLASS_NO_COPY(OpticalFlow_backend); CLASS_NO_COPY(OpticalFlow_backend);
// init - initialise sensor // init - initialise sensor
virtual void init() = 0; virtual void init() {}
// read latest values from sensor and fill in x,y and totals. // read latest values from sensor and fill in x,y and totals.
virtual void update() = 0; virtual void update() = 0;

21
libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp

@ -17,26 +17,17 @@
* AP_OpticalFlow_SITL.cpp - SITL emulation of optical flow sensor. * AP_OpticalFlow_SITL.cpp - SITL emulation of optical flow sensor.
*/ */
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include "AP_OpticalFlow_SITL.h" #include "AP_OpticalFlow_SITL.h"
extern const AP_HAL::HAL& hal; #if AP_OPTICALFLOW_SITL_ENABLED
AP_OpticalFlow_SITL::AP_OpticalFlow_SITL(AP_OpticalFlow &_frontend) :
OpticalFlow_backend(_frontend),
_sitl(AP::sitl())
{
}
void AP_OpticalFlow_SITL::init(void) #include <AP_HAL/AP_HAL.h>
{ #include <SITL/SITL.h>
}
void AP_OpticalFlow_SITL::update(void) void AP_OpticalFlow_SITL::update(void)
{ {
auto *_sitl = AP::sitl();
if (!_sitl->flow_enable) { if (!_sitl->flow_enable) {
return; return;
} }
@ -128,4 +119,4 @@ void AP_OpticalFlow_SITL::update(void)
_update_frontend(state); _update_frontend(state);
} }
#endif // CONFIG_HAL_BOARD #endif // AP_OPTICALFLOW_SITL_ENABLED

14
libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.h

@ -1,27 +1,25 @@
#pragma once #pragma once
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h> #if AP_OPTICALFLOW_SITL_ENABLED
class AP_OpticalFlow_SITL : public OpticalFlow_backend class AP_OpticalFlow_SITL : public OpticalFlow_backend
{ {
public: public:
/// constructor /// constructor
AP_OpticalFlow_SITL(AP_OpticalFlow &_frontend); using OpticalFlow_backend::OpticalFlow_backend;
// init - initialise the sensor
void init() override;
// update - read latest values from sensor and fill in x,y and totals. // update - read latest values from sensor and fill in x,y and totals.
void update(void) override; void update(void) override;
private: private:
SITL::SIM *_sitl;
uint32_t last_flow_ms; uint32_t last_flow_ms;
uint8_t next_optflow_index; uint8_t next_optflow_index;
uint8_t optflow_delay; uint8_t optflow_delay;
AP_OpticalFlow::OpticalFlow_state optflow_data[20]; AP_OpticalFlow::OpticalFlow_state optflow_data[20];
}; };
#endif // CONFIG_HAL_BOARD
#endif // AP_OPTICALFLOW_SITL_ENABLED

Loading…
Cancel
Save