diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp new file mode 100644 index 0000000000..9a7effbc12 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -0,0 +1,165 @@ +/* + This program 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 program 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 +#include "AP_RangeFinder_Benewake.h" +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define BENEWAKE_FRAME_HEADER 0x59 +#define BENEWAKE_FRAME_LENGTH 9 + +// format of serial packets received from benewake lidar +// +// Data Bit Definition Description +// ------------------------------------------------ +// byte 0 Frame header 0x59 +// byte 1 Frame header 0x59 +// byte 2 DIST_L Distance (in cm) low 8 bits +// byte 3 DIST_H Distance (in cm) high 8 bits +// byte 4 STRENGTH_L Strength low 8 bits +// byte 5 STRENGTH_H Strength high 8 bits +// byte 6 (TF02) SIG Reliability in 8 levels, 7 & 8 means reliable +// byte 6 (TFmini) Distance Mode 0x02 for short distance (mm), 0x07 for long distance (cm) +// byte 7 (TF02 only) TIME Exposure time in two levels 0x03 and 0x06 +// byte 8 Checksum Checksum byte, sum of bytes 0 to bytes 7 + +/* + The constructor also initialises the rangefinder. Note that this + constructor is not called until detect() returns true, so we + already know that we should setup the rangefinder +*/ +AP_RangeFinder_Benewake::AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state, + AP_SerialManager &serial_manager, + uint8_t serial_instance, + benewake_model_type model) : + AP_RangeFinder_Backend(_state), + model_type(model) +{ + uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); + if (uart != nullptr) { + uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance)); + } +} + +/* + detect if a Benewake rangefinder is connected. We'll detect by + trying to take a reading on Serial. If we get a result the sensor is + there. +*/ +bool AP_RangeFinder_Benewake::detect(AP_SerialManager &serial_manager, uint8_t serial_instance) +{ + return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr; +} + +// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal +bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok) +{ + if (uart == nullptr) { + return false; + } + + float sum_cm = 0; + uint16_t count = 0; + bool dist_reliable = false; + + // read any available lines from the lidar + int16_t nbytes = uart->available(); + while (nbytes-- > 0) { + char c = uart->read(); + // if buffer is empty and this byte is 0x59, add to buffer + if (linebuf_len == 0) { + if (c == BENEWAKE_FRAME_HEADER) { + linebuf[linebuf_len++] = c; + } + } else if (linebuf_len == 1) { + // if buffer has 1 element and this byte is 0x59, add it to buffer + // if not clear the buffer + if (c == BENEWAKE_FRAME_HEADER) { + linebuf[linebuf_len++] = c; + } else { + linebuf_len = 0; + dist_reliable = false; + } + } else { + // add character to buffer + linebuf[linebuf_len++] = c; + // if buffer now has 9 items try to decode it + if (linebuf_len == BENEWAKE_FRAME_LENGTH) { + // calculate checksum + uint16_t checksum = 0; + for (uint8_t i=0; i= 7); + } + if (dist_reliable) { + sum_cm += dist; + count++; + } + } + } + // clear buffer + linebuf_len = 0; + dist_reliable = false; + } + } + } + + if (count == 0) { + return false; + } + reading_cm = sum_cm / count; + return true; +} + +/* + update the state of the sensor +*/ +void AP_RangeFinder_Benewake::update(void) +{ + bool signal_ok; + if (get_reading(state.distance_cm, signal_ok)) { + // update range_valid state based on distance measured + last_reading_ms = AP_HAL::millis(); + if (signal_ok) { + update_status(); + } else { + // if signal is weak set status to out-of-range + set_status(RangeFinder::RangeFinder_OutOfRangeHigh); + } + } else if (AP_HAL::millis() - last_reading_ms > 200) { + set_status(RangeFinder::RangeFinder_NoData); + } +} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h new file mode 100644 index 0000000000..b61a66ebb7 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h @@ -0,0 +1,45 @@ +#pragma once + +#include "RangeFinder.h" +#include "RangeFinder_Backend.h" + +class AP_RangeFinder_Benewake : public AP_RangeFinder_Backend +{ + +public: + + enum benewake_model_type { + BENEWAKE_TF02 = 0, + BENEWAKE_TFmini = 1 + }; + + // constructor + AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state, + AP_SerialManager &serial_manager, + uint8_t serial_instance, + benewake_model_type model); + + // static detection function + static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance); + + // update state + void update(void); + +protected: + + virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_LASER; + } + +private: + + // get a reading + // distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal + bool get_reading(uint16_t &reading_cm, bool &signal_ok); + + AP_HAL::UARTDriver *uart = nullptr; + benewake_model_type model_type; + uint32_t last_reading_ms; + char linebuf[10]; + uint8_t linebuf_len; +}; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 99b7e519c6..4ca9ccec3d 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -34,6 +34,7 @@ #include "AP_RangeFinder_VL53L0X.h" #include "AP_RangeFinder_NMEA.h" #include "AP_RangeFinder_Wasp.h" +#include "AP_RangeFinder_Benewake.h" #include extern const AP_HAL::HAL &hal; @@ -43,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-200 + // @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-200,19:BenewakeTF02,20:BenewakeTFmini // @User: Standard AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0), @@ -174,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-200 + // @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-200,19:BenewakeTF02,20:BenewakeTFmini // @User: Advanced AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0), @@ -299,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-200 + // @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-200,19:BenewakeTF02,20:BenewakeTFmini // @User: Advanced AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0), @@ -424,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-200 + // @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-200,19:BenewakeTF02,20:BenewakeTFmini // @User: Advanced AP_GROUPINFO("4_TYPE", 37, RangeFinder, state[3].type, 0), @@ -746,6 +747,16 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) drivers[instance] = new AP_RangeFinder_Wasp(state[instance], serial_manager, serial_instance++); } break; + case RangeFinder_TYPE_BenewakeTF02: + if (AP_RangeFinder_Benewake::detect(serial_manager, serial_instance)) { + drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02); + } + break; + case RangeFinder_TYPE_BenewakeTFmini: + if (AP_RangeFinder_Benewake::detect(serial_manager, serial_instance)) { + drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini); + } + break; default: break; } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 1a9b8d7013..240bb04cb7 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -60,6 +60,8 @@ public: RangeFinder_TYPE_VL53L0X = 16, RangeFinder_TYPE_NMEA = 17, RangeFinder_TYPE_WASP = 18, + RangeFinder_TYPE_BenewakeTF02 = 19, + RangeFinder_TYPE_BenewakeTFmini = 20 }; enum RangeFinder_Function {