From b3a84966134acab92e23c3b7b320a2271858595c Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Fri, 26 Apr 2019 00:23:45 -0700 Subject: [PATCH] AP_RangeFinder: add support for Benewake TFMini Plus on I2C This was tested with firmware 1.7.0 on the sensor. --- .../AP_RangeFinder_Benewake_TFMiniPlus.cpp | 205 ++++++++++++++++++ .../AP_RangeFinder_Benewake_TFMiniPlus.h | 62 ++++++ .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 4 +- libraries/AP_RangeFinder/RangeFinder.cpp | 9 + libraries/AP_RangeFinder/RangeFinder.h | 3 +- 5 files changed, 280 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp new file mode 100644 index 0000000000..5693cf20fe --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp @@ -0,0 +1,205 @@ +/* + * Copyright (C) 2019 Lucas De Marchi + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "AP_RangeFinder_Benewake_TFMiniPlus.h" + +#include + +#include + +extern const AP_HAL::HAL& hal; + +#define DRIVER "TFMiniPlus" + +/* + * Command format: + * + * uint8_t header; + * uint8_t len; + * uint8_t id; + * uint8_t data[]; + * uint8_t checksum; + */ + +AP_RangeFinder_Benewake_TFMiniPlus::AP_RangeFinder_Benewake_TFMiniPlus( + RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, + AP_HAL::OwnPtr dev) + : AP_RangeFinder_Backend(_state, _params) + , _dev(std::move(dev)) +{ +} + +AP_RangeFinder_Backend *AP_RangeFinder_Benewake_TFMiniPlus::detect( + RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + + AP_RangeFinder_Benewake_TFMiniPlus *sensor + = new AP_RangeFinder_Benewake_TFMiniPlus(_state, _params, std::move(dev)); + + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +bool AP_RangeFinder_Benewake_TFMiniPlus::init() +{ + const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F }; + const uint8_t CMD_SYSTEM_RESET[] = { 0x5A, 0x04, 0x04, 0x62 }; + const uint8_t CMD_OUTPUT_FORMAT_CM[] = { 0x5A, 0x05, 0x05, 0x01, 0x65 }; + const uint8_t CMD_ENABLE_DATA_OUTPUT[] = { 0x5A, 0x05, 0x07, 0x01, 0x67 }; + const uint8_t CMD_FRAME_RATE_100HZ[] = { 0x5A, 0x06, 0x03, 0x64, 0x00, 0xC7 }; + const uint8_t CMD_SAVE_SETTINGS[] = { 0x5A, 0x04, 0x11, 0x6F }; + const uint8_t *cmds[] = { + CMD_OUTPUT_FORMAT_CM, + CMD_FRAME_RATE_100HZ, + CMD_ENABLE_DATA_OUTPUT, + CMD_SAVE_SETTINGS, + }; + uint8_t val[12], i; + bool ret; + + _dev->get_semaphore()->take_blocking(); + + _dev->set_retries(0); + + /* + * Check we get a response for firmware version to detect if sensor is there + */ + ret = _dev->transfer(CMD_FW_VERSION, sizeof(CMD_FW_VERSION), nullptr, 0); + if (!ret) { + goto fail; + } + + hal.scheduler->delay(100); + + ret = _dev->transfer(nullptr, 0, val, 7); + if (!ret || val[0] != 0x5A || val[1] != 0x07 || val[2] != 0x01 || + !check_checksum(val, 7)) { + goto fail; + } + + hal.console->printf(DRIVER ": found fw version %u.%u.%u\n", + val[5], val[4], val[3]); + + for (i = 0; i < ARRAY_SIZE(cmds); i++) { + ret = _dev->transfer(cmds[i], cmds[i][1], nullptr, 0); + if (!ret) { + hal.console->printf(DRIVER ": Unable to set configuration register %u\n", + cmds[i][2]); + goto fail; + } + hal.scheduler->delay(100); + } + + _dev->transfer(CMD_SYSTEM_RESET, sizeof(CMD_SYSTEM_RESET), nullptr, 0); + + _dev->get_semaphore()->give(); + + hal.scheduler->delay(100); + + _dev->register_periodic_callback(10000, + FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Benewake_TFMiniPlus::timer, void)); + + return true; + +fail: + _dev->get_semaphore()->give(); + return false; +} + +void AP_RangeFinder_Benewake_TFMiniPlus::update() +{ + WITH_SEMAPHORE(_sem); + + if (accum.count > 0) { + state.distance_cm = accum.sum / accum.count; + state.last_reading_ms = AP_HAL::millis(); + accum.sum = 0; + accum.count = 0; + update_status(); + } else if (AP_HAL::millis() - state.last_reading_ms > 200) { + set_status(RangeFinder::RangeFinder_NoData); + } +} + +bool AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(be16_t distance_raw, be16_t strength_raw, + uint16_t &output_distance_cm) +{ + uint16_t strength = be16toh(strength_raw); + + output_distance_cm = be16toh(distance_raw); + + if (strength < 100 || strength == 0xFFFF) { + return false; + } + + output_distance_cm = constrain_int16(output_distance_cm, 10, 1200); + + return true; +} + +bool AP_RangeFinder_Benewake_TFMiniPlus::check_checksum(uint8_t *arr, int pkt_len) +{ + uint8_t checksum = 0; + int i; + + /* sum them all except the last (the checksum) */ + for (i = 0; i < pkt_len - 1; i++) + checksum += arr[i]; + + return checksum == arr[pkt_len - 1]; +} + +void AP_RangeFinder_Benewake_TFMiniPlus::timer() +{ + uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 }; + union { + struct PACKED { + uint8_t header; + uint8_t len; + be16_t distance; + be16_t strength; + uint8_t reserved1; + le16_t timestamp; + uint8_t checksum; + } val; + uint8_t arr[10]; + } u; + bool ret; + uint16_t distance; + + ret = _dev->transfer(CMD_READ_MEASUREMENT, sizeof(CMD_READ_MEASUREMENT), nullptr, 0); + if (!ret || !_dev->transfer(nullptr, 0, (uint8_t *)&u, sizeof(u))) { + return; + } + + if (u.val.header != 0x5a || u.val.len != 0x0a || !check_checksum(u.arr, sizeof(u))) + return; + + if (process_raw_measure(u.val.distance, u.val.strength, distance)) { + WITH_SEMAPHORE(_sem); + accum.sum += distance; + accum.count++; + } +} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h new file mode 100644 index 0000000000..6c4792237b --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2019 Lucas De Marchi + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include "RangeFinder.h" +#include "RangeFinder_Backend.h" + +#include +#include + +class AP_RangeFinder_Benewake_TFMiniPlus : public AP_RangeFinder_Backend +{ + +public: + // static detection function + static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, + AP_HAL::OwnPtr dev); + + // update state + void update(void) override; + +protected: + + virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_LASER; + } + +private: + AP_RangeFinder_Benewake_TFMiniPlus(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, + AP_HAL::OwnPtr dev); + + bool init(); + void timer(); + + bool process_raw_measure(le16_t distance_raw, le16_t strength_raw, + uint16_t &output_distance_cm); + + bool check_checksum(uint8_t *arr, int pkt_len); + + AP_HAL::OwnPtr _dev; + + struct { + uint32_t sum; + uint32_t count; + } accum; +}; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 5d0250532d..5f45c90eda 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::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,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN + // @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:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFMiniPlus // @User: Standard AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0), @@ -97,7 +97,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @Param: ADDR // @DisplayName: Bus address of sensor - // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor and UAVCAN Range Sensors to allow for multiple sensors on different addresses. A value of 0 disables the sensor. + // @Description: This sets the bus address of the sensor, where applicable. Used for the I2C and UAVCAN sensors to allow for multiple sensors on different addresses. A value of 0 disables the sensor. // @Range: 0 127 // @Increment: 1 // @User: Standard diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 687d029045..57f8ebfb0f 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -35,6 +35,7 @@ #include "AP_RangeFinder_NMEA.h" #include "AP_RangeFinder_Wasp.h" #include "AP_RangeFinder_Benewake.h" +#include "AP_RangeFinder_Benewake_TFMiniPlus.h" #include "AP_RangeFinder_PWM.h" #include "AP_RangeFinder_BLPing.h" #include "AP_RangeFinder_UAVCAN.h" @@ -414,6 +415,14 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; + case RangeFinder_TYPE_BenewakeTFminiPlus: + FOREACH_I2C(i) { + if (_add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(i, params[instance].address)))) { + break; + } + } + break; #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS case RangeFinder_TYPE_PX4_PWM: // to ease moving from PX4 to ChibiOS we'll lie a little about diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index cd732b06d2..56d0688be1 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -70,7 +70,8 @@ public: RangeFinder_TYPE_PLI2CV3HP = 21, RangeFinder_TYPE_PWM = 22, RangeFinder_TYPE_BLPing = 23, - RangeFinder_TYPE_UAVCAN = 24 + RangeFinder_TYPE_UAVCAN = 24, + RangeFinder_TYPE_BenewakeTFminiPlus = 25, }; enum RangeFinder_Function {