ardupilot/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightL...

69 lines
2.2 KiB
C
Raw Normal View History

#pragma once
2013-12-02 07:02:56 -04:00
#include "AP_RangeFinder.h"
#include "AP_RangeFinder_Backend.h"
#ifndef AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
#define AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
#endif
#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
#include <AP_HAL/I2CDevice.h>
2013-12-02 07:02:56 -04:00
/* Connection diagram
*
* ------------------------------------------------------------------------------------
* | J2-1(LED) J2-2(5V) J2-3(Enable) J2-4(Ref Clk) J2-5(GND) J2-6(GND) |
2013-12-02 07:02:56 -04:00
* | |
* | |
* | J1-3(I2C Clk) J1-2(I2C Data) J1-1(GND) |
2013-12-02 07:02:56 -04:00
* ------------------------------------------------------------------------------------
*/
class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend
2013-12-02 07:02:56 -04:00
{
public:
// static detection function
static AP_RangeFinder_Backend *detect(uint8_t bus,
RangeFinder::RangeFinder_State &_state,
2019-02-03 22:21:58 -04:00
AP_RangeFinder_Params &_params,
RangeFinder::Type rftype);
2013-12-02 07:02:56 -04:00
// update state
void update(void) override {}
2013-12-02 07:02:56 -04:00
protected:
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_LASER;
}
2013-12-02 07:02:56 -04:00
private:
// constructor
AP_RangeFinder_PulsedLightLRF(uint8_t bus,
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
RangeFinder::Type rftype);
// start a reading
bool init(void);
void timer(void);
bool lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint8_t sw_version;
uint8_t hw_version;
uint8_t check_reg_counter;
bool v2_hardware;
bool v3hp_hardware;
uint16_t last_distance_cm;
RangeFinder::Type rftype;
enum { PHASE_MEASURE, PHASE_COLLECT } phase;
2013-12-02 07:02:56 -04:00
};
#endif // AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED