Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: added type 15 for LidareLiteV3

Browse Source
This commit is contained in:
Andrew Tridgell 2017-03-01 15:22:32 +11:00 committed by Randy Mackay
parent cf3db7560c
commit 05cbc19ad6
4 changed files with 43 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -45,9 +45,11 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder already know that we should setup the rangefinder
*/ */
AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &_ranger, uint8_t instance, AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state) RangeFinder::RangeFinder_State &_state,
RangeFinder::RangeFinder_Type _rftype)
: AP_RangeFinder_Backend(_ranger, instance, _state) : AP_RangeFinder_Backend(_ranger, instance, _state)
, _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR)) , _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR))
, rftype(_rftype)
{ {
} }
@ -56,10 +58,11 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeF
look for the version registers look for the version registers
*/ */
AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus, RangeFinder &_ranger, uint8_t instance, AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state) RangeFinder::RangeFinder_State &_state,
RangeFinder::RangeFinder_Type rftype)
{ {
AP_RangeFinder_PulsedLightLRF *sensor AP_RangeFinder_PulsedLightLRF *sensor
= new AP_RangeFinder_PulsedLightLRF(bus, _ranger, instance, _state); = new AP_RangeFinder_PulsedLightLRF(bus, _ranger, instance, _state, rftype);
if (!sensor || if (!sensor ||
!sensor->init()) { !sensor->init()) {
delete sensor; delete sensor;
@ -150,18 +153,20 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
// LidarLite needs split transfers // LidarLite needs split transfers
_dev->set_split_transfers(true); _dev->set_split_transfers(true);
if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) && if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3) {
hw_version > 0 && v2_hardware = true;
_dev->read_registers(LL40LS_SW_VERSION, &sw_version, 1) && } else {
sw_version > 0)) { // auto-detect v1 vs v2
printf("PulsedLightI2C: bad version 0x%02x 0x%02x\n", (unsigned)hw_version, (unsigned)sw_version); if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) &&
// invalid version information hw_version > 0 &&
_dev->get_semaphore()->give(); _dev->read_registers(LL40LS_SW_VERSION, &sw_version, 1) &&
return false; sw_version > 0)) {
printf("PulsedLightI2C: bad version 0x%02x 0x%02x\n", (unsigned)hw_version, (unsigned)sw_version);
// invalid version information
goto failed;
}
v2_hardware = (hw_version >= 0x15);
} }
_dev->get_semaphore()->give();
v2_hardware = (hw_version >= 0x15);
const struct settings_table *table; const struct settings_table *table;
uint8_t num_settings; uint8_t num_settings;
@ -179,11 +184,21 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
_dev->setup_checked_registers(num_settings); _dev->setup_checked_registers(num_settings);
for (uint8_t i = 0; i < num_settings; i++) { for (uint8_t i = 0; i < num_settings; i++) {
_dev->write_register(table[i].reg, table[i].value, true); if (!_dev->write_register(table[i].reg, table[i].value, true)) {
goto failed;
}
} }
printf("Found LidarLite device=0x%x v2=%d\n", _dev->get_bus_id(), (int)v2_hardware);
_dev->get_semaphore()->give();
_dev->register_periodic_callback(20000, _dev->register_periodic_callback(20000,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, void)); FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, void));
return true; return true;
failed:
_dev->get_semaphore()->give();
return false;
} }

7
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
View File

@ -20,7 +20,8 @@ class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend
public: public:
// static detection function // static detection function
static AP_RangeFinder_Backend *detect(uint8_t bus, RangeFinder &ranger, uint8_t instance, static AP_RangeFinder_Backend *detect(uint8_t bus, RangeFinder &ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state); RangeFinder::RangeFinder_State &_state,
RangeFinder::RangeFinder_Type rftype);
// update state // update state
void update(void) override {} void update(void) override {}
@ -29,7 +30,8 @@ public:
private: private:
// constructor // constructor
AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &ranger, uint8_t instance, AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state); RangeFinder::RangeFinder_State &_state,
RangeFinder::RangeFinder_Type rftype);
// start a reading // start a reading
bool init(void); bool init(void);
@ -43,6 +45,7 @@ private:
uint8_t check_reg_counter; uint8_t check_reg_counter;
bool v2_hardware; bool v2_hardware;
uint16_t last_distance_cm; uint16_t last_distance_cm;
RangeFinder::RangeFinder_Type rftype;
enum { PHASE_MEASURE, PHASE_COLLECT } phase; enum { PHASE_MEASURE, PHASE_COLLECT } phase;
}; };

11
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -36,7 +36,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: _TYPE // @Param: _TYPE
// @DisplayName: Rangefinder type // @DisplayName: Rangefinder type
// @Description: What type of rangefinder device that is connected // @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:PulsedLightI2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TrOneI2C // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TrOneI2C,15:LidarLiteV3
// @User: Standard // @User: Standard
AP_GROUPINFO("_TYPE", 0, RangeFinder, _type[0], 0), AP_GROUPINFO("_TYPE", 0, RangeFinder, _type[0], 0),
@ -163,7 +163,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 2_TYPE // @Param: 2_TYPE
// @DisplayName: Second Rangefinder type // @DisplayName: Second Rangefinder type
// @Description: What type of rangefinder device that is connected // @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:PulsedLightI2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TrOneI2C // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TrOneI2C,15:LidarLiteV3
// @User: Advanced // @User: Advanced
AP_GROUPINFO("2_TYPE", 12, RangeFinder, _type[1], 0), AP_GROUPINFO("2_TYPE", 12, RangeFinder, _type[1], 0),
@ -608,11 +608,12 @@ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
*/ */
void RangeFinder::detect_instance(uint8_t instance) void RangeFinder::detect_instance(uint8_t instance)
{ {
uint8_t type = _type[instance]; enum RangeFinder_Type type = (enum RangeFinder_Type)_type[instance].get();
switch (type) { switch (type) {
case RangeFinder_TYPE_PLI2C: case RangeFinder_TYPE_PLI2C:
if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, *this, instance, state[instance]))) { case RangeFinder_TYPE_PLI2CV3:
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, *this, instance, state[instance])); if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, *this, instance, state[instance], type))) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, *this, instance, state[instance], type));
} }
break; break;
case RangeFinder_TYPE_MBI2C: case RangeFinder_TYPE_MBI2C:

3
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -51,7 +51,8 @@ public:
RangeFinder_TYPE_ULANDING= 11, RangeFinder_TYPE_ULANDING= 11,
RangeFinder_TYPE_LEDDARONE = 12, RangeFinder_TYPE_LEDDARONE = 12,
RangeFinder_TYPE_MBSER = 13, RangeFinder_TYPE_MBSER = 13,
RangeFinder_TYPE_TRONE = 14 RangeFinder_TYPE_TRONE = 14,
RangeFinder_TYPE_PLI2CV3= 15,
}; };
enum RangeFinder_Function { enum RangeFinder_Function {