Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Rangefinder: Add support for Garmin LidarLite-V3HP

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

31
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -24,6 +24,7 @@ extern const AP_HAL::HAL& hal;
/* LL40LS Registers addresses */ /* LL40LS Registers addresses */
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ #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_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */
#define LL40LS_COUNT 0x11 #define LL40LS_COUNT 0x11
#define LL40LS_HW_VERSION 0x41 #define LL40LS_HW_VERSION 0x41
@ -86,12 +87,6 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
} }
switch (phase) { switch (phase) {
case PHASE_MEASURE:
if (_dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE)) {
phase = PHASE_COLLECT;
}
break;
case PHASE_COLLECT: { case PHASE_COLLECT: {
be16_t val; be16_t val;
// read the high and low byte distance registers // read the high and low byte distance registers
@ -110,9 +105,18 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
// for v2 hw we use continuous mode // for v2 hw we use continuous mode
phase = PHASE_MEASURE; phase = PHASE_MEASURE;
} }
if (!v3hp_hardware) {
// for v3hp hw we start PHASE_MEASURE immediately after PHASE_COLLECT
break; break;
} }
} }
case PHASE_MEASURE:
if (_dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE)) {
phase = PHASE_COLLECT;
}
break;
}
} }
@ -140,6 +144,13 @@ static const struct settings_table settings_v2[] = {
{ LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }, { 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 initialise the sensor to required settings
*/ */
@ -155,6 +166,8 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3) { if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3) {
v2_hardware = true; v2_hardware = true;
} else if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3HP) {
v3hp_hardware = true;
} else { } else {
// auto-detect v1 vs v2 // auto-detect v1 vs v2
if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) && if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) &&
@ -175,6 +188,10 @@ bool AP_RangeFinder_PulsedLightLRF::init(void)
table = settings_v2; table = settings_v2;
num_settings = sizeof(settings_v2) / sizeof(settings_table); num_settings = sizeof(settings_v2) / sizeof(settings_table);
phase = PHASE_COLLECT; phase = PHASE_COLLECT;
} else if (v3hp_hardware) {
table = settings_v3hp;
num_settings = sizeof(settings_v3hp) / sizeof(settings_table);
phase = PHASE_MEASURE;
} else { } else {
table = settings_v1; table = settings_v1;
num_settings = sizeof(settings_v1) / sizeof(settings_table); num_settings = sizeof(settings_v1) / sizeof(settings_table);
@ -189,7 +206,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(); _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 hw_version;
uint8_t check_reg_counter; uint8_t check_reg_counter;
bool v2_hardware; bool v2_hardware;
bool v3hp_hardware;
uint16_t last_distance_cm; uint16_t last_distance_cm;
RangeFinder::RangeFinder_Type rftype; 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 // @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: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 // @User: Standard
AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0), AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0),
@ -175,7 +175,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: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 // @User: Advanced
AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0), AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0),
@ -300,7 +300,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 3_TYPE // @Param: 3_TYPE
// @DisplayName: Third Rangefinder type // @DisplayName: Third 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: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 // @User: Advanced
AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0), AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0),
@ -425,7 +425,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 4_TYPE // @Param: 4_TYPE
// @DisplayName: Fourth Rangefinder type // @DisplayName: Fourth 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: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 // @User: Advanced
AP_GROUPINFO("4_TYPE", 37, RangeFinder, state[3].type, 0), 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) { switch (_type) {
case RangeFinder_TYPE_PLI2C: case RangeFinder_TYPE_PLI2C:
case RangeFinder_TYPE_PLI2CV3: case RangeFinder_TYPE_PLI2CV3:
case RangeFinder_TYPE_PLI2CV3HP:
if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, state[instance], _type))) { if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, state[instance], _type))) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, state[instance], _type)); _add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, state[instance], _type));
} }

3
libraries/AP_RangeFinder/RangeFinder.h
View File

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