AP_RangeFinder:add support for RDS02UF radar driver on serial

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -51,6 +51,7 @@
 #include "AP_RangeFinder_MSP.h"
 #include "AP_RangeFinder_USD1_CAN.h"
 #include "AP_RangeFinder_Benewake_CAN.h"
+#include "AP_RangeFinder_RDS02UF.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -608,6 +609,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
 #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
         _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), 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

@@ -103,6 +103,7 @@ class RangeFinder
         USD1_CAN = 33,
         Benewake_CAN = 34,
         TeraRanger_Serial = 35,
+        RDS02UF = 36,
         SIM = 100,
     };
 

libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.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 <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_RDS02UF.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <ctype.h>
+
+#define RDS02_HEAD1 0x55
+#define RDS02_HEAD2 0x55
+#define RDS02_END 0xAA
+
+#define RDS02UF_HEAD_LEN 2
+#define RDS02UF_PRE_DATA_LEN 6
+#define RDS02UF_DATA_LEN 10
+#define RDS02_DATA_Y_INDEX_L 13
+#define RDS02_DATA_Y_INDEX_H 14
+#define RDS02_DATA_FC_L    8
+#define RDS02_DATA_FC_H    9
+#define RDS02_TARGET_INFO  0x70C
+#define RDS02UF_DIST_MAX_CM           2000
+#define RDS02UF_DIST_MIN_CM           150
+#define AP_RANGEFINDER_RDS02UF_TIMEOUT_MS   200
+
+
+extern const AP_HAL::HAL& hal;
+
+AP_RangeFinder_RDS02UF::AP_RangeFinder_RDS02UF(
+    RangeFinder::RangeFinder_State &_state,
+    AP_RangeFinder_Params &_params):
+    AP_RangeFinder_Backend_Serial(_state, _params)
+{
+
+}
+
+// return last value measured by sensor
+bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    // read any available lines from the lidar
+    float sum = 0.0f;
+    uint16_t count = 0;
+    int16_t nbytes = uart->available();
+    while (nbytes-- > 0) {
+        uint8_t c = uart->read();
+        if (decode(c)) {
+            sum += _distance_m;
+            count++;
+        }
+    }
+
+    // return false on failure
+    if (count == 0) {
+        return false;
+    }
+
+    if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_RDS02UF_TIMEOUT_MS) {
+        set_status(RangeFinder::Status::NoData);
+        reading_m = 0.0f;
+    } else {
+        // return average of all measurements
+        reading_m = sum / count;
+        update_status();
+    }
+
+    // return average of all measurements
+    reading_m = sum / count;
+    return true;
+}
+
+
+// add a single character to the buffer and attempt to decode
+// returns true if a distance was successfully decoded
+bool AP_RangeFinder_RDS02UF::decode(uint8_t c)
+{
+    uint8_t data = c;
+    bool ret = false;
+    switch (decode_state)
+    {
+        case STATE0_SYNC_1:
+            if (data == RDS02_HEAD1) {
+                rev_buffer[parserbuf_index] = data;
+                parserbuf_index++;
+                decode_state = STATE1_SYNC_2;
+            }
+            break;
+        case STATE1_SYNC_2:
+            if (data == RDS02_HEAD2) {
+                rev_buffer[parserbuf_index] = data;
+                parserbuf_index++;
+                decode_state = STATE2_ADDRESS;
+            } else {
+                parserbuf_index = 0;
+                decode_state = STATE0_SYNC_1;
+            }
+            break;
+        case STATE2_ADDRESS: // address
+            rev_buffer[parserbuf_index] = data;
+            parserbuf_index++;
+            decode_state = STATE3_ERROR_CODE;
+            break;
+        case STATE3_ERROR_CODE: // error_code
+            rev_buffer[parserbuf_index] = data;
+            parserbuf_index++;
+            decode_state = STATE4_FC_CODE_L;
+            break;
+        case STATE4_FC_CODE_L: // fc_code low
+            rev_buffer[parserbuf_index] = data;
+            parserbuf_index++;
+            decode_state = STATE5_FC_CODE_H;
+            break;
+        case STATE5_FC_CODE_H: // fc_code high
+            rev_buffer[parserbuf_index] = data;
+            parserbuf_index++;
+            decode_state = STATE6_LENGTH_L;
+            break;
+        case STATE6_LENGTH_L: // lengh_low
+            rev_buffer[parserbuf_index] = data;
+            parserbuf_index++;
+            decode_state = STATE7_LENGTH_H;
+            break;
+        case STATE7_LENGTH_H: // lengh_high
+        {
+            uint8_t read_len = data << 8 | rev_buffer[parserbuf_index-1];
+            if ( read_len == RDS02UF_DATA_LEN)	// rds02uf data length is 10
+            {
+                rev_buffer[parserbuf_index] = data;
+                parserbuf_index++;
+                decode_state = STATE8_REAL_DATA;
+            }else{
+                parserbuf_index = 0;
+                decode_state = STATE0_SYNC_1;
+            }
+            break;
+        }
+        case STATE8_REAL_DATA: // real_data
+            rev_buffer[parserbuf_index] = data;
+            parserbuf_index++;
+            if ( parserbuf_index == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN) ) {
+                decode_state = STATE9_CRC;
+            }
+            break;
+        case STATE9_CRC: // crc
+        {
+	        uint8_t crc_data = 0;
+            crc_data = crc8(&rev_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN);
+            rev_buffer[parserbuf_index] = data;
+            if (crc_data == data || data == 0xff) {
+                parserbuf_index++;
+                decode_state = STATE10_END_1;
+            } else {
+                parserbuf_index = 0;
+                decode_state = STATE0_SYNC_1;
+            }
+            break;
+        }
+        case STATE10_END_1: //
+            if (data == RDS02_END) {
+                rev_buffer[parserbuf_index] = data;
+                parserbuf_index++;
+                decode_state = STATE11_END_2;
+            } else {
+                parserbuf_index = 0;
+                decode_state = STATE0_SYNC_1;
+            }
+            break;
+        case STATE11_END_2: //
+        {
+            uint16_t fc_code = (rev_buffer[STATE5_FC_CODE_H] << 8 | rev_buffer[STATE4_FC_CODE_L]);
+            uint8_t err_code = rev_buffer[STATE3_ERROR_CODE];
+            if (data == RDS02_END)
+            {
+                if (fc_code == 0x03ff && err_code == 0) {// get targer information
+                    uint16_t data_fc = (rev_buffer[RDS02_DATA_FC_H] << 8 | rev_buffer[RDS02_DATA_FC_L]);
+                    if((data_fc & 0xf0f) == RDS02_TARGET_INFO){	// data_fc = 0x70C + ID * 0x10, ID: 0~0xF
+                        float distance = (rev_buffer[RDS02_DATA_Y_INDEX_H] * 256 + rev_buffer[RDS02_DATA_Y_INDEX_L]) / 100.0f;
+                        _distance_m = distance;
+                        _distance_m = MIN(MAX(RDS02UF_DIST_MIN_CM, uint16_t(_distance_m*100)), RDS02UF_DIST_MAX_CM) * 0.01f;
+                        ret = true;
+                        state.last_reading_ms = AP_HAL::millis();
+                    }
+
+                }
+            }
+			parserbuf_index = 0;
+			decode_state = STATE0_SYNC_1;
+			break;
+        }
+    }
+
+    return ret;
+}
+
+uint8_t AP_RangeFinder_RDS02UF::crc8(uint8_t* pbuf, int32_t len)
+{
+     uint8_t* data = pbuf;
+     uint8_t crc = 0;
+     while ( len-- )
+     crc = crc8_table[crc^*(data++)];
+     return crc;
+}

libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h

@@ -0,0 +1,135 @@
+/*
+   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.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
+#ifndef AP_RANGEFINDER_RDS02UF_ENABLED
+#define AP_RANGEFINDER_RDS02UF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
+#endif
+
+#if AP_RANGEFINDER_RDS02UF_ENABLED
+
+#define RDS02_BUFFER_SIZE 50
+
+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_ULTRASOUND;
+    }
+
+
+private:
+    AP_RangeFinder_RDS02UF(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
+
+    uint8_t decode_state;
+    uint8_t parserbuf_index;
+    uint8_t rev_buffer[RDS02_BUFFER_SIZE];
+    float _distance_m;
+
+    void ParseRds02(uint8_t *data_in, uint8_t datalengh);
+
+    // 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
+    /// enum for handled messages
+    enum RDS02UF_PARSE_STATE {
+        STATE0_SYNC_1 = 0,
+        STATE1_SYNC_2,
+        STATE2_ADDRESS,
+        STATE3_ERROR_CODE,
+        STATE4_FC_CODE_L,
+        STATE5_FC_CODE_H,
+        STATE6_LENGTH_L,
+        STATE7_LENGTH_H,
+        STATE8_REAL_DATA,
+        STATE9_CRC,
+        STATE10_END_1,
+        STATE11_END_2
+    };
+
+    // get a distance reading
+    bool get_reading(float &reading_m) override;
+
+    // get temperature reading in C.  returns true on success and populates temp argument
+    // bool get_temp(float &temp) const override;
+
+    uint16_t read_timeout_ms() const override { return 3000; }
+
+    uint8_t crc8(uint8_t *pbuf, int32_t len);
+
+    bool decode(uint8_t c);
+
+    const uint8_t crc8_table[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,
+    };
+};
+#endif  // AP_RANGEFINDER_NMEA_ENABLED