diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index fe93be6f3a..ea3959cdd0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -49,6 +49,7 @@ #include "AP_RangeFinder_MSP.h" #include "AP_RangeFinder_USD1_CAN.h" #include "AP_RangeFinder_Benewake_CAN.h" +#include "AP_RangeFinder_RDS02UF.h" #include #include @@ -502,6 +503,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) _add_backend(new AP_RangeFinder_NMEA(state[instance], params[instance]), instance, serial_instance++); } break; + case Type::RDS02UF: + if (AP_RangeFinder_RDS02UF::detect(serial_instance)) { + _add_backend(new AP_RangeFinder_RDS02UF(state[instance], params[instance]), instance, serial_instance++); + } + break; case Type::WASP: if (AP_RangeFinder_Wasp::detect(serial_instance)) { _add_backend(new AP_RangeFinder_Wasp(state[instance], params[instance]), instance, serial_instance++); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 7474c4cadf..8bf2bea3df 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -89,6 +89,7 @@ public: MSP = 32, USD1_CAN = 33, Benewake_CAN = 34, + RDS02UF = 35, SIM = 100, }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp new file mode 100644 index 0000000000..0f4d3ec8b3 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp @@ -0,0 +1,180 @@ +/* + 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_RangeFinder_RDS02UF.h" + +#include +#include + +extern const AP_HAL::HAL& hal; + +// 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; + } + + // 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 (step) + { + case 0: + if (data == RDS02_HEAD1) + { + rev_buffer[step] = data; + step++; + } + break; + case 1: + if (data == RDS02_HEAD2) + { + rev_buffer[step] = data; + step++; + } + else + { + ParseDataFailed(rev_buffer, sizeof(rev_buffer)); + } + break; + case 2: // address + rev_buffer[step] = data; + step++; + break; + case 3: // error_code + rev_buffer[step] = data; + err_code = data; + step++; + break; + case 4: // fc_code low + rev_buffer[step] = data; + fc_code = data; + step++; + break; + case 5: // fc_code high + rev_buffer[step] = data; + fc_code = fc_code | (data << 8); + step++; + break; + case 6: // lengh_low + data_len = data; + rev_buffer[step] = data; + step++; + break; + case 7: // lengh_high + data_len = data << 8 | data_len; + rev_buffer[step] = data; + step++; + break; + case 8: // real_data + rev_buffer[step + data_index] = data; + + data_index++; + if (data_index == data_len) + { + step++; + } + break; + case 9: // crc + crc_data = crc8(&rev_buffer[2], 6 + data_len); + rev_buffer[step + data_index] = data; + if (crc_data == data || data == 0xff) + { + step++; + } + else + { + ParseDataFailed(rev_buffer, sizeof(rev_buffer)); + } + break; + case 10: // + if (data == RDS02_END) + { + rev_buffer[step + data_index] = data; + step++; + } + else + { + ParseDataFailed(rev_buffer, sizeof(rev_buffer)); + } + + break; + case 11: // + if (data == RDS02_END) + { + if (fc_code == 0x03ff && err_code == 0) + { + if(rev_buffer[RDS02_DATA_START_INDEX] == RDS02_TARGET_INFO) + { + float distance = (rev_buffer[RDS02_DATA_Y_INDEX + 1] * 256 + rev_buffer[RDS02_DATA_Y_INDEX]) / 100.0f; + _distance_m = distance; + ret = true; + } + } + ParseDataFailed(rev_buffer, sizeof(rev_buffer)); + } + break; + } + + return ret; +} + +void AP_RangeFinder_RDS02UF::ParseDataFailed(uint8_t *data_in, uint8_t datalengh) +{ + memset(data_in, 0, datalengh); + step = 0; + address = 0; + data_len = 0; + data_index = 0; + crc_data = 0; + fc_code = 0; + err_code = 0; +} + +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; +} \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h new file mode 100644 index 0000000000..ffce4a42cb --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h @@ -0,0 +1,118 @@ +/* + 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.h" +#include "AP_RangeFinder_Backend_Serial.h" + +#define RDS02_HEAD1 0x55 +#define RDS02_HEAD2 0x55 +#define RDS02_END 0xAA + +#define RDS02_BUFFER_SIZE 50 +#define RDS02_DATA_START_INDEX 8 +#define RDS02_DATA_Y_INDEX 13 +#define RDS02_DATA_FC_INDEX RDS02_DATA_START_INDEX + + +#define RDS02_TARGET_INFO 0x0C + +class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial +{ + +public: + + using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; + +protected: + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_ULTRASOUND; + } + + +private: + + uint8_t step; + + uint8_t address; + uint16_t data_len; + uint16_t data_index; + uint8_t crc_data; + uint16_t fc_code; + uint8_t err_code; + uint8_t rev_buffer[RDS02_BUFFER_SIZE]; + float _distance_m; + + void ParseRds02(uint8_t *data_in, uint8_t datalengh); + + /// enum for handled messages + enum sentence_types : uint8_t { + SONAR_UNKNOWN = 0, + SONAR_DBT, + SONAR_DPT, + SONAR_MTW, // mean water temperature + SONAR_HDED, // hondex custom sonar message + }; + + // 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); + void ParseDataFailed(uint8_t *data_in, uint8_t datalengh); + + 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, + }; +};