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
*/
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)
, _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
*/
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
= new AP_RangeFinder_PulsedLightLRF(bus, _ranger, instance, _state);
= new AP_RangeFinder_PulsedLightLRF(bus, _ranger, instance, _state, rftype);
if (!sensor ||
!sensor->init()) {
delete sensor;
@ -150,18 +153,20 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
// LidarLite needs split transfers
_dev->set_split_transfers(true);
if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) &&
hw_version > 0 &&
_dev->read_registers(LL40LS_SW_VERSION, &sw_version, 1) &&
sw_version > 0)) {
printf("PulsedLightI2C: bad version 0x%02x 0x%02x\n", (unsigned)hw_version, (unsigned)sw_version);
// invalid version information
_dev->get_semaphore()->give();
return false;
if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3) {
v2_hardware = true;
} else {
// auto-detect v1 vs v2
if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) &&
hw_version > 0 &&
_dev->read_registers(LL40LS_SW_VERSION, &sw_version, 1) &&
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;
uint8_t num_settings;
@ -179,11 +184,21 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
_dev->setup_checked_registers(num_settings);
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,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, void));
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:
// static detection function
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
void update(void) override {}
@ -29,7 +30,8 @@ public:
private:
// constructor
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
bool init(void);
@ -43,6 +45,7 @@ private:
uint8_t check_reg_counter;
bool v2_hardware;
uint16_t last_distance_cm;
RangeFinder::RangeFinder_Type rftype;
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
// @DisplayName: Rangefinder type
// @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
AP_GROUPINFO("_TYPE", 0, RangeFinder, _type[0], 0),
@ -163,7 +163,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 2_TYPE
// @DisplayName: Second Rangefinder type
// @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
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)
{
uint8_t type = _type[instance];
enum RangeFinder_Type type = (enum RangeFinder_Type)_type[instance].get();
switch (type) {
case RangeFinder_TYPE_PLI2C:
if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, *this, instance, state[instance]))) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, *this, instance, state[instance]));
case RangeFinder_TYPE_PLI2CV3:
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;
case RangeFinder_TYPE_MBI2C:

3
libraries/AP_RangeFinder/RangeFinder.h
View File

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