From 05cbc19ad6a1d5e45a974c5743cdc5ff3725a9f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 1 Mar 2017 15:22:32 +1100 Subject: [PATCH] AP_RangeFinder: added type 15 for LidareLiteV3 --- .../AP_RangeFinder_PulsedLightLRF.cpp | 45 ++++++++++++------- .../AP_RangeFinder_PulsedLightLRF.h | 7 ++- libraries/AP_RangeFinder/RangeFinder.cpp | 11 ++--- libraries/AP_RangeFinder/RangeFinder.h | 3 +- 4 files changed, 43 insertions(+), 23 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 19d8cfa795..8dc89d85db 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -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; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index f9437d3958..24157832ee 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -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; }; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 8c5ef28a6f..d2f09c6358 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -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: diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 45a2c361a3..93e980bbbb 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -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 {