2016-05-04 00:02:44 -03:00
|
|
|
#pragma once
|
|
|
|
|
2019-11-10 22:47:38 -04:00
|
|
|
#include "AP_RangeFinder.h"
|
|
|
|
#include "AP_RangeFinder_Backend.h"
|
2016-05-04 00:02:44 -03:00
|
|
|
|
|
|
|
// Data timeout
|
2016-05-03 23:55:05 -03:00
|
|
|
#define AP_RANGEFINDER_MAVLINK_TIMEOUT_MS 500
|
2016-05-04 00:02:44 -03:00
|
|
|
|
2016-05-03 23:55:05 -03:00
|
|
|
class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend
|
2016-05-04 00:02:44 -03:00
|
|
|
{
|
|
|
|
|
|
|
|
public:
|
|
|
|
// constructor
|
2018-07-04 11:22:17 -03:00
|
|
|
AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
|
2016-05-04 00:02:44 -03:00
|
|
|
|
|
|
|
// static detection function
|
2017-08-07 00:41:01 -03:00
|
|
|
static bool detect();
|
2016-05-04 00:02:44 -03:00
|
|
|
|
|
|
|
// update state
|
2018-11-07 07:01:51 -04:00
|
|
|
void update(void) override;
|
2016-05-04 00:02:44 -03:00
|
|
|
|
|
|
|
// Get update from mavlink
|
2019-04-30 07:22:49 -03:00
|
|
|
void handle_msg(const mavlink_message_t &msg) override;
|
2016-05-04 00:02:44 -03:00
|
|
|
|
2017-08-08 04:32:53 -03:00
|
|
|
protected:
|
|
|
|
|
|
|
|
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
|
|
|
|
return sensor_type;
|
|
|
|
}
|
|
|
|
|
2016-05-04 00:02:44 -03:00
|
|
|
private:
|
2016-05-03 23:55:05 -03:00
|
|
|
uint16_t distance_cm;
|
2016-05-04 00:02:44 -03:00
|
|
|
|
|
|
|
// start a reading
|
|
|
|
static bool start_reading(void);
|
|
|
|
static bool get_reading(uint16_t &reading_cm);
|
2017-08-08 04:32:53 -03:00
|
|
|
|
|
|
|
MAV_DISTANCE_SENSOR sensor_type = MAV_DISTANCE_SENSOR_UNKNOWN;
|
2016-05-04 00:02:44 -03:00
|
|
|
};
|