diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 963d3f9197..4a940dd6cb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -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 @@ -110,8 +105,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; } } @@ -140,6 +144,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 */ @@ -155,6 +166,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) && @@ -175,6 +188,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); @@ -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(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index 53a5be357c..7de2b81325 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -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; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 2f300c62fd..3525be1f91 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -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)); } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 240bb04cb7..a87aca50a3 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -61,7 +61,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 {