From ebc630f77931c665f7bf68116f4d7ca23a522b5d Mon Sep 17 00:00:00 2001 From: Oleksiy Protas Date: Wed, 9 Oct 2024 17:32:09 +0300 Subject: [PATCH] AP_RangeFinder: initial support for the DFRobot07 lidar - Only the distance and the temperature are extracted - 50 ms fixed reading interval - Only UART mode implemented --- Tools/scripts/build_options.py | 1 + libraries/AP_RangeFinder/AP_RangeFinder.cpp | 6 + libraries/AP_RangeFinder/AP_RangeFinder.h | 3 + .../AP_RangeFinder_DFRobot_Lidar07.cpp | 222 ++++++++++++++++++ .../AP_RangeFinder_DFRobot_Lidar07.h | 132 +++++++++++ .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_config.h | 4 + 7 files changed, 369 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.h diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 3f5dd534ff8f16..52785c2edd48f9 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -247,6 +247,7 @@ def config_option(self): Feature('Rangefinder', 'RFND_BENEWAKE_TFMINI', 'AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED', "Enable Rangefinder - Benewake - TFMini", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RFND_BENEWAKE_TFMINIPLUS', 'AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED', "Enable Rangefinder - Benewake - TFMiniPlus", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_BLPING', 'AP_RANGEFINDER_BLPING_ENABLED', "Enable Rangefinder - BLPing", 0, "RANGEFINDER"), # NOQA: E501 + Feature('Rangefinder', 'RANGEFINDER_DFROBOT_LIDAR07', 'AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED', "Enable Rangefinder - DFRobot Lidar07", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_GYUS42V2', 'AP_RANGEFINDER_GYUS42V2_ENABLED', "Enable Rangefinder - GYUS42V2", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_HC_SR04', 'AP_RANGEFINDER_HC_SR04_ENABLED', "Enable Rangefinder - HC_SR04", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_JRE_SERIAL', 'AP_RANGEFINDER_JRE_SERIAL_ENABLED', "Enable Rangefinder - JRE_SERIAL", 0, "RANGEFINDER"), # NOQA: E501 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 2134162eab2486..7a5d76f53f6de6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -64,6 +64,7 @@ #include "AP_RangeFinder_JRE_Serial.h" #include "AP_RangeFinder_Ainstein_LR_D1.h" #include "AP_RangeFinder_RDS02UF.h" +#include "AP_RangeFinder_DFRobot_Lidar07.h" #include #include @@ -598,6 +599,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) case Type::RDS02UF: serial_create_fn = AP_RangeFinder_RDS02UF::create; break; +#endif +#if AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED + case Type::DFRobot_Lidar07: + serial_create_fn = AP_RangeFinder_DFRobot_Lidar07::create; + break; #endif case Type::NONE: break; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 4d66101b8b6ab9..fb7d669e6ce1b8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -182,6 +182,9 @@ class RangeFinder #if AP_RANGEFINDER_RDS02UF_ENABLED RDS02UF = 43, #endif +#if AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED + DFRobot_Lidar07 = 44, +#endif #if AP_RANGEFINDER_SIM_ENABLED SIM = 100, #endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.cpp new file mode 100644 index 00000000000000..a259989cc5578f --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.cpp @@ -0,0 +1,222 @@ +/* + 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 "AP_HAL/utility/sparse-endian.h" +#include "AP_RangeFinder_config.h" +#include + +#if AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED + +#include "AP_RangeFinder_DFRobot_Lidar07.h" + +// DFRobot Lidar07 support driver +// Also sold as VSemi Atom +// Documentation: https://dfimg.dfrobot.com/nobody/wiki/1840a7b7b14e02f3566e0cef5b51e9ba.pdf +// https://wiki.dfrobot.com/TOF_IR_Distance_Sensor_0.2_12m_SKU_SEN0413 +// +// Current implementation only is limited to: +// - UART interface +// - Basic distance reading in continuous mode without an inbuilt filter + +#include +#include +extern const AP_HAL::HAL& hal; + +#define LIDAR07_SOF_SENSOR (0xfa) +#define LIDAR07_SOF_CONTROLLER (0xf5) +#define LIDAR07_WRITE_MASK (0x80) +#define LIDAR07_REG_VERSION (0x43) +#define LIDAR07_REG_FILTER (0x59) +#define LIDAR07_REG_MEASUREMENT (0x60) +#define LIDAR07_REG_MODE (0x61) +#define LIDAR07_REG_INTERVAL (0x62) +#define LIDAR07_REG_ERROR (0x65) + +// the sensor supports rates up to 100 Hz +// use the lowest common rate between Copter and Plane: 20 Hz = 50 ms +#define LIDAR07_FRAME_INTERVAL_MS (50) + +// Invalidate the temperature and other readings after 2 measurement intervals +#define LIDAR07_DATA_EXPIRATION_MS (2*LIDAR07_FRAME_INTERVAL_MS) + +// Retry initialization if inactive for 30 seconds +#define LIDAR07_STALE_CONNECTION_MS (30*1000) + +// DFRobot's CRC32 implementation uses unreflected bit order +// apparently it's not the way CRC32 is used elsewhere so it makes sense to hide +// this utility under the define block +template T reflect(const T& p) +{ + // NOTE: straightforward bit reflection, can probably be optimized if used in high frequency code + T res = 0; + size_t bit_len = sizeof(T) * 8; + for (size_t i = 0; i < bit_len; i++) { + res |= bool(p & (1<(buf[i]); + return reflect(crc_crc32(0xffffffff, ubuf, n)); +} + +void AP_RangeFinder_DFRobot_Lidar07::prepare_command(uint8_t reg, uint32_t value) +{ + _packet.start = LIDAR07_SOF_CONTROLLER; + _packet.reg = reg; + _packet.command.value = htole32(value); + _packet.command.checksum = htole32(crc32_unreflected(_buf, offsetof(Packet, command.checksum))); + _buf_pos = 0; + _writing = true; + _exp_reg = reg; +} + +bool AP_RangeFinder_DFRobot_Lidar07::get_reading(float &reading_m) +{ + if (uart == nullptr) { + return false; + } + + // are we coming out of reset? + if (!_exp_reg) { + // no low noise filter to speed things up + prepare_command(LIDAR07_REG_FILTER | LIDAR07_WRITE_MASK, 0); + _last_read_ms = AP_HAL::millis(); + } + + // we have data to write, send what we can and return to get rescheduled + if (_writing) { + size_t left = offsetof(Packet, command.checksum) + sizeof(Packet::command.checksum) - _buf_pos; + size_t written = uart->write(&_buf[_buf_pos], left); + if (written < left) { + _buf_pos += written; + } else { + _buf_pos = 0; + _writing = false; + } + + return false; + } + + // invalidate connection and restart the setup if no data received in a while + if (AP_HAL::millis() - _last_read_ms > LIDAR07_STALE_CONNECTION_MS) { + _exp_reg = 0; + return false; + } + + // try reading a whole message from the serial + for (size_t i = 0; i < sizeof(_buf); i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } + _last_read_ms = AP_HAL::millis(); + + // restart if we are somehow out of buffer + if (_buf_pos == sizeof(_buf)) { + _buf_pos = 0; + continue; + } + + // reject spurious fragments + if (_buf_pos == offsetof(Packet, start) && c != LIDAR07_SOF_SENSOR) { + continue; + } + + // reject things we aren't expecting to get + if (_buf_pos == offsetof(Packet, reg) && c != _exp_reg) { + _buf_pos = 0; + continue; + } + + _buf[_buf_pos++] = c; + + // pull the next byte if we haven't read the length yet + constexpr size_t data_offset = offsetof(Packet, len) + sizeof(Packet::len); + if (_buf_pos < data_offset) { + continue; + } + + // check if data length makes sense after we read all of the value + bool measuring = _exp_reg == (LIDAR07_REG_MEASUREMENT | LIDAR07_WRITE_MASK); + size_t exp_len = measuring? sizeof(Packet::measurement.data) : sizeof(Packet::responce.data); + if (_buf_pos == data_offset && le16toh(_packet.len) != exp_len) { + _buf_pos = 0; + continue; + } + + size_t crc32_offset = measuring? offsetof(Packet, measurement.checksum) : offsetof(Packet, responce.checksum); + // looks like we have a full message, reset the counter and verify it + if (_buf_pos >= crc32_offset + sizeof(uint32_t)) { + _buf_pos = 0; + + // match the checksums + // incoming value on the wire is little endian + uint32_t message_crc32 = crc32_unreflected(_buf, crc32_offset); + uint32_t incoming_crc32 = measuring? _packet.measurement.checksum : _packet.responce.checksum; + if (le32toh(incoming_crc32) == message_crc32) { + // We have a valid message, act in accordance to what we are expecting + // NOTE: we aren't actually checking that the responce values match what we demanded + switch (_exp_reg) { + case LIDAR07_REG_FILTER | LIDAR07_WRITE_MASK: + // continuous mode + prepare_command(LIDAR07_REG_MODE | LIDAR07_WRITE_MASK, 1); + return false; + + case LIDAR07_REG_MODE | LIDAR07_WRITE_MASK: + // frame rate + prepare_command(LIDAR07_REG_INTERVAL | LIDAR07_WRITE_MASK, LIDAR07_FRAME_INTERVAL_MS); + return false; + + case LIDAR07_REG_INTERVAL | LIDAR07_WRITE_MASK: + // start measuring + prepare_command(LIDAR07_REG_MEASUREMENT | LIDAR07_WRITE_MASK, 1); + return false; + + case LIDAR07_REG_MEASUREMENT | LIDAR07_WRITE_MASK: + // got a valid measurement, capture the fields + reading_m = le16toh(_packet.measurement.data.distance)/1000.0; + _temperature = le16toh(_packet.measurement.data.temperature)/100.0; + + // NOTE: other fields are unused right now + // we can probably figure out signal quality by light level/amplitude + return true; + } + } + + // wrong checksum or messed up register? + _buf_pos = 0; + } + } + + // we've got nothing at this time + return false; +} + +bool AP_RangeFinder_DFRobot_Lidar07::get_temp(float &temp) const +{ + if (AP_HAL::millis() - _last_read_ms > LIDAR07_DATA_EXPIRATION_MS) { + return false; + } + + temp = _temperature; + return true; +} + +#endif // AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.h b/libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.h new file mode 100644 index 00000000000000..bb6f6f6f6e48fc --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.h @@ -0,0 +1,132 @@ +/* + 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 . + */ + +#pragma once + +#include "AP_RangeFinder_config.h" + +#if AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED + +#include "AP_RangeFinder.h" +#include "AP_RangeFinder_Backend_Serial.h" + +#include + +class AP_RangeFinder_DFRobot_Lidar07 : public AP_RangeFinder_Backend_Serial +{ + +public: + + static AP_RangeFinder_Backend_Serial *create( + RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params) { + return NEW_NOTHROW AP_RangeFinder_DFRobot_Lidar07(_state, _params); + } + +protected: + + // 850 nm LED + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_INFRARED; + } + +private: + + using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; + + // 115200 baud as per datasheet + uint32_t initial_baudrate(uint8_t serial_instance) const override { return 115200; } + + // get a distance reading + bool get_reading(float &reading_m) override; + + // get the latest encountered temperature + bool get_temp(float &temp) const override; + + // generate a command to the sensor and place it in the message buffer + void prepare_command(uint8_t reg, uint32_t value); + + // unreflected crc32 checksum with 0xffffffff initial value + static uint32_t crc32_unreflected(uint8_t *buf, size_t n); + + // last temperature seen + float _temperature; + + // packet structure + struct Packet + { + uint8_t start; + uint8_t reg; + union + { + // single value for packets sent to the sensor + struct + { + le32_t value; + le32_t checksum; + } command __attribute__((packed)); + // sensor responce + struct + { + // responces include a 16-bit data length although only 2 values are ever used + le16_t len; + union + { + // generic 32-bit reply + struct + { + le32_t data; + le32_t checksum; + } responce __attribute__((packed)); + // measurement response + struct + { + struct { + le16_t distance; + le16_t temperature; + le16_t amplitude; + le16_t ambient_light; + le64_t tof_phase; + } data __attribute__((packed)); + le32_t checksum; + } measurement __attribute__((packed)); + }; + } __attribute__((packed)); + }; + + } __attribute__((packed)); + + // one message buffer + union + { + Packet _packet; + uint8_t _buf[sizeof(Packet)]; + }; + + // current buffer position + size_t _buf_pos = 0; + + // whether we are reading or writing + bool _writing = false; + + // expected message register + // 0 carries a special meaning of a state after reset + uint8_t _exp_reg = 0; + + // last time we've received a valid reading + uint32_t _last_read_ms = 0; +}; + +#endif // AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index ae6812021db6e1..8f437c59351a1e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -14,7 +14,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @Param: TYPE // @DisplayName: Rangefinder type // @Description: Type of connected rangefinder - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,100:SITL + // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,44:DFrobot_Lidar07,100:SITL // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h index 1f5407adaf5e73..5dc925ad6a750b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -190,3 +190,7 @@ #ifndef AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED #define AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 #endif + +#ifndef AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED +#define AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 +#endif