Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Rangefinder: Add support for Garmin LidarLite-V3HP

This commit is contained in:
Nghia Nguyen 2018-07-27 17:22:26 +07:00 committed by Francisco Ferreira
parent 9aa0e3b0ad
commit 513eb1f5db
4 changed files with 43 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -23,18 +23,19 @@
extern const AP_HAL::HAL& hal;
/* LL40LS Registers addresses */
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */
#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */
#define LL40LS_COUNT 0x11
#define LL40LS_HW_VERSION 0x41
#define LL40LS_INTERVAL 0x45
#define LL40LS_SW_VERSION 0x4f
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */
#define LL40LS_SIG_COUNT_VAL 0x02
#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */
#define LL40LS_COUNT 0x11
#define LL40LS_HW_VERSION 0x41
#define LL40LS_INTERVAL 0x45
#define LL40LS_SW_VERSION 0x4f
// bit values
#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */
#define LL40LS_AUTO_INCREMENT 0x80
#define LL40LS_COUNT_CONTINUOUS 0xff
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */
#define LL40LS_AUTO_INCREMENT 0x80
#define LL40LS_COUNT_CONTINUOUS 0xff
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
// i2c address
#define LL40LS_ADDR 0x62
@ -86,12 +87,6 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
}
switch (phase) {
case PHASE_MEASURE:
if (_dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE)) {
phase = PHASE_COLLECT;
}
break;
case PHASE_COLLECT: {
be16_t val;
// read the high and low byte distance registers
@ -111,8 +106,17 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
// for v2 hw we use continuous mode
phase = PHASE_MEASURE;
}
break;
if (!v3hp_hardware) {
// for v3hp hw we start PHASE_MEASURE immediately after PHASE_COLLECT
break;
}
}
case PHASE_MEASURE:
if (_dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE)) {
phase = PHASE_COLLECT;
}
break;
}
}
@ -141,6 +145,13 @@ static const struct settings_table settings_v2[] = {
{ LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE },
};
/*
register setup table for V3HP Lidar
*/
static const struct settings_table settings_v3hp[] = {
{ LL40LS_SIG_COUNT_VAL, 0x80 }, // 0x80 = 128 acquisitions
};
/*
initialise the sensor to required settings
*/
@ -156,6 +167,8 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3) {
v2_hardware = true;
} else if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3HP) {
v3hp_hardware = true;
} else {
// auto-detect v1 vs v2
if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) &&
@ -176,6 +189,10 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
table = settings_v2;
num_settings = sizeof(settings_v2) / sizeof(settings_table);
phase = PHASE_COLLECT;
} else if (v3hp_hardware) {
table = settings_v3hp;
num_settings = sizeof(settings_v3hp) / sizeof(settings_table);
phase = PHASE_MEASURE;
} else {
table = settings_v1;
num_settings = sizeof(settings_v1) / sizeof(settings_table);
@ -190,7 +207,7 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
}
}
printf("Found LidarLite device=0x%x v2=%d\n", _dev->get_bus_id(), (int)v2_hardware);
printf("Found LidarLite device=0x%x v2=%d v3hp=%d\n", _dev->get_bus_id(), (int)v2_hardware, (int)v3hp_hardware);
_dev->get_semaphore()->give();

1
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
View File

@ -49,6 +49,7 @@ private:
uint8_t hw_version;
uint8_t check_reg_counter;
bool v2_hardware;
bool v3hp_hardware;
uint16_t last_distance_cm;
RangeFinder::RangeFinder_Type rftype;

9
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -44,7 +44,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:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C
// @User: Standard
AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0),
@ -175,7 +175,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:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C
// @User: Advanced
AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0),
@ -300,7 +300,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 3_TYPE
// @DisplayName: Third Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C
// @User: Advanced
AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0),
@ -425,7 +425,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 4_TYPE
// @DisplayName: Fourth Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C
// @User: Advanced
AP_GROUPINFO("4_TYPE", 37, RangeFinder, state[3].type, 0),
@ -643,6 +643,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
switch (_type) {
case RangeFinder_TYPE_PLI2C:
case RangeFinder_TYPE_PLI2CV3:
case RangeFinder_TYPE_PLI2CV3HP:
if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, state[instance], _type))) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, state[instance], _type));
}

3
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -65,7 +65,8 @@ public:
RangeFinder_TYPE_NMEA = 17,
RangeFinder_TYPE_WASP = 18,
RangeFinder_TYPE_BenewakeTF02 = 19,
RangeFinder_TYPE_BenewakeTFmini = 20
RangeFinder_TYPE_BenewakeTFmini = 20,
RangeFinder_TYPE_PLI2CV3HP = 21,
};
enum RangeFinder_Function {