ardupilot/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h

41 lines
1.4 KiB
C
Raw Normal View History

2019-03-27 04:18:35 -03:00
#pragma once
#include "AP_OpticalFlow_config.h"
#if AP_OPTICALFLOW_MAV_ENABLED
#include "AP_OpticalFlow_Backend.h"
2019-03-27 04:18:35 -03:00
#include <AP_HAL/utility/OwnPtr.h>
class AP_OpticalFlow_MAV : public OpticalFlow_backend
{
public:
/// constructor
using OpticalFlow_backend::OpticalFlow_backend;
// initialise the sensor
void init() override {}
// read latest values from sensor and fill in x,y and totals.
void update(void) override;
// get update from mavlink
void handle_msg(const mavlink_message_t &msg) override;
2019-03-27 04:18:35 -03:00
// detect if the sensor is available
static AP_OpticalFlow_MAV *detect(AP_OpticalFlow &_frontend);
2019-03-27 04:18:35 -03:00
private:
uint64_t prev_frame_us; // system time of last message when update was last called
uint64_t latest_frame_us; // system time of most recent messages processed
Vector2l flow_sum; // sum of sensor's flow_x and flow_y values since last call to update
uint16_t quality_sum; // sum of sensor's quality values since last call to update
uint16_t count; // number of sensor readings since last call to update
uint8_t sensor_id; // sensor_id received in latest mavlink message
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