AP_RangeFinder: add support for RDS02UF radar driver on seria

Tools/autotest/arducopter.py

@@ -8396,6 +8396,7 @@ def RangeFinderDrivers(self):
             ("teraranger_serial", 35),
             ("nooploop_tofsense", 37),
             ("ainsteinlrd1", 42),
+            ("rds02uf", 43),
         ]
         while len(drivers):
             do_drivers = drivers[0:3]

Tools/scripts/build_options.py

@@ -248,6 +248,7 @@ def __init__(self,
     Feature('Rangefinder', 'RANGEFINDER_VL53L0X', 'AP_RANGEFINDER_VL53L0X_ENABLED', "Enable Rangefinder - VL53L0X", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_VL53L1X', 'AP_RANGEFINDER_VL53L1X_ENABLED', "Enable Rangefinder - VL53L1X", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_WASP', 'AP_RANGEFINDER_WASP_ENABLED', "Enable Rangefinder - Wasp", 0, "RANGEFINDER"),   # NOQA: E501
+    Feature('Rangefinder', 'RANGEFINDER_RDS02UF', 'AP_RANGEFINDER_RDS02UF_ENABLED', "Enable Rangefinder - RDS02UF", 0, "RANGEFINDER"),   # NOQA: E501
 
     Feature('Sensors', 'OPTICALFLOW', 'AP_OPTICALFLOW_ENABLED', 'Enable Optical Flow', 0, None),
     Feature('Sensors', 'OPTICALFLOW_CXOF', 'AP_OPTICALFLOW_CXOF_ENABLED', 'Enable Optical flow CXOF Sensor', 0, "OPTICALFLOW"),

Tools/scripts/extract_features.py

@@ -84,6 +84,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED', r'AP_RangeFinder_MaxsonarSerialLV::get_reading\b',),
             ('AP_RANGEFINDER_TRI2C_ENABLED', r'AP_RangeFinder_TeraRangerI2C::update\b',),
             ('AP_RANGEFINDER_JRE_SERIAL_ENABLED', r'AP_RangeFinder_JRE_Serial::get_reading\b',),
+            ('AP_RANGEFINDER_RDS02UF_ENABLED', r'AP_RangeFinder_RDS02UF::get_reading\b',),
 
             ('AP_GPS_{type}_ENABLED', r'AP_GPS_(?P<type>.*)::read\b',),
 

libraries/AP_Math/crc.cpp

@@ -227,6 +227,51 @@ uint8_t crc8_sae(const uint8_t *data, uint16_t length)
     return	crc;
 }
 
+// crc table for rangefinder rds02uf
+static const uint8_t crc8_table_rds02uf[256] = {
+    0x93,0x98,0xE4,0x46,0xEB,0xBA,0x04,0x4C,
+    0xFA,0x40,0xB8,0x96,0x0E,0xB2,0xB7,0xC0,
+    0x0C,0x32,0x9B,0x80,0xFF,0x30,0x7F,0x9D,
+    0xB3,0x81,0x58,0xE7,0xF1,0x19,0x7E,0xB6,
+    0xCD,0xF7,0xB4,0xCB,0xBC,0x5C,0xD6,0x09,
+    0x20,0x0A,0xE0,0x37,0x51,0x67,0x24,0x95,
+    0xE1,0x62,0xF8,0x5E,0x38,0x15,0x54,0x77,
+    0x63,0x57,0x6D,0xE9,0x89,0x76,0xBE,0x41,
+    0x5D,0xF9,0xB1,0x4D,0x6C,0x53,0x9C,0xA2,
+    0x23,0xC4,0x8E,0xC8,0x05,0x42,0x61,0x71,
+    0xC5,0x00,0x18,0x6F,0x5F,0xFB,0x7B,0x11,
+    0x65,0x2D,0x8C,0xED,0x14,0xAB,0x88,0xD5,
+    0xD9,0xC2,0x36,0x34,0x7C,0x5B,0x3C,0xF6,
+    0x48,0x0B,0xEE,0x02,0x83,0x79,0x17,0xE6,
+    0xA8,0x78,0xF5,0xD3,0x4E,0x50,0x52,0x91,
+    0xD8,0xC6,0x22,0xEC,0x3B,0xE5,0x3F,0x86,
+    0x06,0xCF,0x2B,0x2F,0x3D,0x59,0x1C,0x87,
+    0xEF,0x4F,0x10,0xD2,0x7D,0xDA,0x72,0xA0,
+    0x9F,0xDE,0x6B,0x75,0x56,0xBD,0xC7,0xC1,
+    0x70,0x1D,0x25,0x92,0xA5,0x31,0xE2,0xD7,
+    0xD0,0x9A,0xAF,0xA9,0xC9,0x97,0x08,0x33,
+    0x5A,0x99,0xC3,0x16,0x84,0x82,0x8A,0xF3,
+    0x4A,0xCE,0xDB,0x29,0x0F,0xAE,0x6E,0xE3,
+    0x8B,0x07,0x3A,0x74,0x47,0xB0,0xBB,0xB5,
+    0x7A,0xAA,0x2C,0xD4,0x03,0x3E,0x1A,0xA7,
+    0x27,0x64,0x06,0xBF,0x55,0x73,0x1E,0xFE,
+    0x49,0x01,0x39,0x28,0xF4,0x26,0xDF,0xDD,
+    0x44,0x0D,0x21,0xF2,0x85,0xB9,0xEA,0x4B,
+    0xDC,0x6A,0xCA,0xAC,0x12,0xFC,0x2E,0x2A,
+    0xA3,0xF0,0x66,0xE8,0x60,0x45,0xA1,0x8D,
+    0x68,0x35,0xFD,0x8F,0x9E,0x1F,0x13,0xD1,
+    0xAD,0x69,0xCC,0xA4,0x94,0x90,0x1B,0x43,
+};
+
+uint8_t crc8_rds02uf(const uint8_t *data, uint16_t length)
+{
+    uint8_t crc = 0;
+    while (length--) {
+        crc = crc8_table_rds02uf[crc^*(data++)];
+    }
+    return crc;
+}
+
 /*
   xmodem CRC thanks to avr-liberty
   https://github.com/dreamiurg/avr-liberty

libraries/AP_Math/crc.h

@@ -13,7 +13,7 @@
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 /*
-  interfaces to ArduPilot collection of CRCs. 
+  interfaces to ArduPilot collection of CRCs.
  */
 #pragma once
 
@@ -28,6 +28,7 @@ uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length);
 uint8_t crc8_dvb_update(uint8_t crc, const uint8_t* buf, const uint16_t buf_len);
 uint8_t crc8_maxim(const uint8_t *data, uint16_t length);
 uint8_t crc8_sae(const uint8_t *data, uint16_t length);
+uint8_t crc8_rds02uf(const uint8_t *data, uint16_t length);
 uint16_t crc_xmodem_update(uint16_t crc, uint8_t data);
 uint16_t crc_xmodem(const uint8_t *data, uint16_t len);
 uint32_t crc_crc32(uint32_t crc, const uint8_t *buf, uint32_t size);

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -60,6 +60,7 @@
 #include "AP_RangeFinder_TOFSenseF_I2C.h"
 #include "AP_RangeFinder_JRE_Serial.h"
 #include "AP_RangeFinder_Ainstein_LR_D1.h"
+#include "AP_RangeFinder_RDS02UF.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -591,6 +592,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         break;
 #endif
 
+#if AP_RANGEFINDER_RDS02UF_ENABLED
+    case Type::RDS02UF:
+        serial_create_fn = AP_RangeFinder_RDS02UF::create;
+        break;
+#endif
     case Type::NONE:
         break;
     }

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -177,6 +177,9 @@ class RangeFinder
 #if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
         Ainstein_LR_D1 = 42,
 #endif
+#if AP_RANGEFINDER_RDS02UF_ENABLED
+        RDS02UF = 43,
+#endif
 #if AP_RANGEFINDER_SIM_ENABLED
         SIM = 100,
 #endif

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -10,7 +10,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,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,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),
 

libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp

@@ -0,0 +1,125 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * RDS02UF Note:
+ * Sensor range scope 1.5m~20.0m
+ * Azimuth Coverage ±17°,Elevation Coverage ±3°
+ * Frame Rate 20Hz
+ */
+
+#include "AP_RangeFinder_config.h"
+
+#if AP_RANGEFINDER_RDS02UF_ENABLED
+
+#include "AP_RangeFinder_RDS02UF.h"
+#include <AP_HAL/AP_HAL.h>
+
+#define RDS02_HEAD                  0x55
+#define RDS02_END                   0xAA
+#define RDS02UF_PRE_DATA_LEN        6
+#define RDS02_TARGET_INFO_FC        0x070C
+#define RDS02UF_IGNORE_ID_BYTE      0x0F0F
+#define RDS02UF_UAV_PRODUCTS_ID     0x03FF
+#define RDS02UF_IGNORE_CRC          0xFF
+#define RDS02UF_NO_ERR              0x00
+
+bool AP_RangeFinder_RDS02UF::get_reading(float &distance_m)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    const uint32_t nbytes = uart->read(&u.parse_buffer[body_length],
+                                 ARRAY_SIZE(u.parse_buffer)-body_length);
+    if (nbytes == 0) {
+        return false;
+    }
+    body_length += nbytes;
+
+    move_header_in_buffer(0);
+
+    // header byte 1 is correct.
+    if (body_length < ARRAY_SIZE(u.parse_buffer)) {
+        // need a full buffer to have a valid message...
+        return false;
+    }
+
+    if (u.packet.headermagic2 != RDS02_HEAD) {
+        move_header_in_buffer(1);
+        return false;
+    }
+
+    const uint16_t read_len = u.packet.length_h << 8 | u.packet.length_l;
+    if (read_len != RDS02UF_DATA_LEN) {
+        // we can only accept the fixed length message
+        move_header_in_buffer(1);
+        return false;
+    }
+
+    // check for the footer signatures:
+    if (u.packet.footermagic1 != RDS02_END) {
+        move_header_in_buffer(1);
+        return false;
+    }
+    if (u.packet.footermagic2 != RDS02_END) {
+        move_header_in_buffer(1);
+        return false;
+    }
+
+    // calculate checksum
+    const uint8_t checksum = crc8_rds02uf(&u.parse_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN);
+    if (u.packet.checksum != checksum && u.packet.checksum != RDS02UF_IGNORE_CRC) {
+        move_header_in_buffer(1);
+        return false;
+    }
+
+    const uint16_t fc_code = (u.packet.fc_high << 8 | u.packet.fc_low);
+    if (fc_code == RDS02UF_UAV_PRODUCTS_ID && u.packet.error_code == RDS02UF_NO_ERR) {
+        // get target information
+        const uint16_t read_info_fc = (u.packet.data[1] << 8 | u.packet.data[0]);
+        if ((read_info_fc & RDS02UF_IGNORE_ID_BYTE) == RDS02_TARGET_INFO_FC) {
+            // read_info_fc = 0x70C + ID * 0x10, ID: 0~0xF
+            distance_m = (u.packet.data[6] * 256 + u.packet.data[5]) * 0.01f;
+            state.last_reading_ms = AP_HAL::millis();
+        }
+    }
+
+    // reset buffer
+    body_length = 0;
+    return true;
+}
+
+// find a RDS02UF message in the buffer, starting at
+// initial_offset.  If found, that message (or partial message) will
+// be moved to the start of the buffer.
+void AP_RangeFinder_RDS02UF::move_header_in_buffer(uint8_t initial_offset)
+{
+    uint8_t* header_ptr = (uint8_t*)memchr(&u.parse_buffer[initial_offset], RDS02_HEAD, body_length - initial_offset);
+    if (header_ptr != nullptr) {
+        size_t header_offset = header_ptr - &u.parse_buffer[0];
+        if (header_offset != 0) {
+            // header was found, but not at index 0; move it back to start of array
+            memmove(u.parse_buffer, header_ptr, body_length - header_offset);
+            body_length -= header_offset;
+        }
+    } else {
+        // no header found; reset buffer
+        body_length = 0;
+    }
+}
+
+#endif  // AP_RANGEFINDER_RDS02UF_ENABLED
+

libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h

@@ -0,0 +1,102 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "AP_RangeFinder_config.h"
+
+#if AP_RANGEFINDER_RDS02UF_ENABLED
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
+#define RDS02_BUFFER_SIZE           50
+#define RDS02UF_DIST_MAX_CM         2000
+#define RDS02UF_DIST_MIN_CM         150
+#define RDS02UF_DATA_LEN            10
+
+class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial
+{
+
+public:
+
+    static AP_RangeFinder_Backend_Serial *create(
+        RangeFinder::RangeFinder_State &_state,
+        AP_RangeFinder_Params &_params) {
+        return new AP_RangeFinder_RDS02UF(_state, _params);
+    }
+
+protected:
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_RADAR;
+    }
+
+private:
+    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
+
+    // find a RDS02UF message in the buffer, starting at
+    // initial_offset.  If found, that message (or partial message) will
+    // be moved to the start of the buffer.
+    void move_header_in_buffer(uint8_t initial_offset);
+
+    // get a distance reading
+    bool get_reading(float &reading_m) override;
+    uint16_t read_timeout_ms() const override { return 500; }
+
+    // make sure readings go out-of-range when necessary
+    int16_t max_distance_cm()const override  { return MIN(params.max_distance_cm, RDS02UF_DIST_MAX_CM); }
+    int16_t min_distance_cm() const override { return MAX(params.min_distance_cm, RDS02UF_DIST_MIN_CM); }
+
+     // Data Format for Benewake Rds02UF
+    // ===============================
+    // 21 bytes total per message:
+    // 1) 0x55
+    // 2) 0x55
+    // 3) address
+    // 4) error_code
+    // 5) FC_CODE_L (low 8bit)
+    // 6) FC_CODE_H (high 8bit)
+    // 7) LENGTH_L (low 8bit)
+    // 8) LENGTH_H (high 8bit)
+    // 9) REAL_DATA (10Byte)
+    // 10) CRC8
+    // 11) END_1 0xAA
+    // 12) END_2 0xAA
+    struct PACKED RDS02UFPacket {
+        uint8_t headermagic1;
+        uint8_t headermagic2;
+        uint8_t address;
+        uint8_t error_code;
+        uint8_t fc_low;
+        uint8_t fc_high;
+        uint8_t length_l;
+        uint8_t length_h;
+        uint8_t data[RDS02UF_DATA_LEN];
+        uint8_t checksum;
+        uint8_t footermagic1;
+        uint8_t footermagic2;
+    };
+
+    union RDS02UF_Union {
+        uint8_t parse_buffer[21];
+        struct RDS02UFPacket packet;
+    };
+    RDS02UF_Union u;
+
+    // number of bytes currently in the buffer
+    uint8_t body_length;
+};
+#endif  // AP_RANGEFINDER_RDS02UF_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_config.h

@@ -133,6 +133,10 @@
 #define AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_RANGEFINDER_RDS02UF_ENABLED
+#define AP_RANGEFINDER_RDS02UF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
+#endif
+
 #ifndef AP_RANGEFINDER_SIM_ENABLED
 #define AP_RANGEFINDER_SIM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
 #endif