From fef47303d2937e838eff4d81c426503de0860626 Mon Sep 17 00:00:00 2001 From: jfbblue0922 Date: Thu, 9 Nov 2023 09:45:22 +0900 Subject: [PATCH] AP_RangeFinder: add serial driver for JRE --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 7 ++ libraries/AP_RangeFinder/AP_RangeFinder.h | 3 + .../AP_RangeFinder_JRE_Serial.cpp | 107 ++++++++++++++++++ .../AP_RangeFinder_JRE_Serial.h | 105 +++++++++++++++++ .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_config.h | 3 + 6 files changed, 226 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 5636656604..1d21089df5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -58,6 +58,7 @@ #include "AP_RangeFinder_TOFSenseP_CAN.h" #include "AP_RangeFinder_NRA24_CAN.h" #include "AP_RangeFinder_TOFSenseF_I2C.h" +#include "AP_RangeFinder_JRE_Serial.h" #include #include @@ -575,6 +576,12 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; } #endif +#if AP_RANGEFINDER_JRE_SERIAL_ENABLED + case Type::JRE_Serial: + serial_create_fn = AP_RangeFinder_JRE_Serial::create; + break; +#endif + case Type::NONE: break; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 1f33536567..b00198644f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -171,6 +171,9 @@ public: #if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED TOFSenseF_I2C = 40, #endif +#if AP_RANGEFINDER_JRE_SERIAL_ENABLED + JRE_Serial = 41, +#endif #if AP_RANGEFINDER_SIM_ENABLED SIM = 100, #endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp new file mode 100644 index 0000000000..e7f6311c8e --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -0,0 +1,107 @@ +/* + 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_config.h" + +#if AP_RANGEFINDER_JRE_SERIAL_ENABLED + +#include "AP_RangeFinder_JRE_Serial.h" +#include + +#define FRAME_HEADER_1 'R' // 0x52 +#define FRAME_HEADER_2_A 'A' // 0x41 +#define FRAME_HEADER_2_B 'B' // 0x42 +#define FRAME_HEADER_2_C 'C' // 0x43 + +#define DIST_MAX_CM 5000 +#define OUT_OF_RANGE_ADD_CM 100 + +bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) +{ + // uart instance check + if (uart == nullptr) { + return false; // not update + } + + uint16_t valid_count = 0; // number of valid readings + uint16_t invalid_count = 0; // number of invalid readings + float sum = 0; + // max distance the sensor can reliably measure - read from parameters + const int16_t distance_cm_max = max_distance_cm(); + + // buffer read + const ssize_t num_read = uart->read(read_buff, ARRAY_SIZE(read_buff)); + for (read_buff_idx = 0; read_buff_idx < num_read; read_buff_idx++) { + if (data_buff_idx == 0) { // header first byte check + // header data search + if (read_buff[read_buff_idx] == FRAME_HEADER_1) { + data_buff[0] = FRAME_HEADER_1; + data_buff_idx = 1; // next data_buff + } + } else if (data_buff_idx == 1) { // header second byte check + data_buff[1] = read_buff[read_buff_idx]; + data_buff_idx = 2; // next data_buff + switch (read_buff[read_buff_idx]) { + case FRAME_HEADER_2_A: + data_buff_len = 16; + break; + case FRAME_HEADER_2_B: + data_buff_len = 32; + break; + case FRAME_HEADER_2_C: + data_buff_len = 48; + break; + default: + data_buff_idx = 0; // data index clear + break; + } + } else { // data set + data_buff[data_buff_idx++] = read_buff[read_buff_idx]; + + if (data_buff_idx >= data_buff_len) { // 1 data set complete + // crc check + uint16_t crc = crc16_ccitt_r(data_buff, data_buff_len - 2, 0xffff, 0xffff); + if ((HIGHBYTE(crc) == data_buff[data_buff_len-1]) && (LOWBYTE(crc) == data_buff[data_buff_len-2])) { + // status check + if (data_buff[data_buff_len-3] & 0x02) { // NTRK + invalid_count++; + } else { // TRK + reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f; + sum += reading_m; + valid_count++; + } + } + data_buff_idx = 0; // data index clear + } + } + } + + // return average of all valid readings + if (valid_count > 0) { + no_signal = false; + reading_m = sum / valid_count; + return true; + } + + // all readings were invalid so return out-of-range-high value + if (invalid_count > 0) { + no_signal = true; + reading_m = MIN(MAX(DIST_MAX_CM, distance_cm_max + OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f; + return true; + } + + return false; +} +#endif // AP_RANGEFINDER_JRE_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h new file mode 100644 index 0000000000..0caa3124b6 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h @@ -0,0 +1,105 @@ +#pragma once + +#include "AP_RangeFinder_config.h" + +#if AP_RANGEFINDER_JRE_SERIAL_ENABLED + +#include "AP_RangeFinder.h" +#include "AP_RangeFinder_Backend_Serial.h" + +/* +The data received from the radio altimeter is as follows. +The format of the received data frame varies depending on the mode, and is stored in "read_buff" with a fixed value of 16 bytes, 32 bytes, or 48 bytes. + +[1] +Measurement mode: 1 data mode +Packet length: 16 bytes +Altitude data used: 4,5 bytes +|----------------------------------------------------------------------------------------------------------------------------------------------------| +| BYTE | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | +|------|---------------------------------------------------------------------------------------------------------------------------------------------| +| NAME | header | header | version | frame | altitude | altitude | reserved | FFT value | FFT value | status | status | CRC | CRC | +| | (H) | (L) | | count | (H) | (L) | | (H) | (L) | (H) | (L) | (L) | (H) | +|------|---------------------------------------------------------------------------------------------------------------------------------------------| +| | | | | | | | | BIT 15to8: reserved | | +| | | | | unsigned | unsigned | | unsigned | BIT 7to4: GAIN detail | CRC result | +| DATA | 'R' | 'A' | 0 to 255 | 0 to 255 | LSB;0.01 m | N/A | LSB:1 | BIT 3-2: reserved | from BYTE | +| | | | | | 0 to 65535 | | 0 to 65535 | BIT 1: TRK / NTRK | 0 to 13 | +| | | | | | | | | BIT 0: GAIN LOW/HIGH | | +|----------------------------------------------------------------------------------------------------------------------------------------------------| + +[2] +Measurement mode: 3 data mode +Packet length: 32 bytes +Altitude data used: 4,5 bytes +|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| BYTE | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14| 15| 16| 17| 18 | 19 | 20 | 21 | 22| 23| 24| 25| 26 | 27 | 28 | 29 | 30 | 31 | +|------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| NAME | header | header | version | frame | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | status | status | CRC | CRC | +| | (H) | (L) | | count | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | (L) | (H) | +|------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| | | | | | | | | | | | | | | BIT 15to8: reserved | | +| | | | | unsigned | unsigned | | unsigned | unsigned | | unsigned | unsigned | | unsigned | BIT 7to4: GAIN detail | CRC result | +| DATA | 'R' | 'B' | 0 to 255 | 0 to 255 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | BIT 3-2: reserved | from BYTE | +| | | | | | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | BIT 1: TRK / NTRK | 0 to 13 | +| | | | | | | | | | | | | | | BIT 0: GAIN LOW/HIGH | | +|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| + +[3] +Measurement mode: 5 data mode +Packet length: 48 bytes +Altitude data used: 4,5 bytes +|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| BYTE | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14| 15| 16| 17| 18 | 19 | 20 | 21 | 22| 23| 24| 25| 26 | 27 | 28 | 29 | 30| 31| 32| 33| 34 | 35 | 36 | 37 | 38| 39| 40| 41| 42 | 43 | 44 | 45 | 46 | 47 | +|------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| NAME | header | header | version | frame | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | altitude | altitude | reserved | FFT value | FFT value | status | status | CRC | CRC | +| | (H) | (L) | | count | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | | (H) | (L) | (H) | (L) | (L) | (H) | +|------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| | | | | | | | | | | | | | | | | | | | | BIT 15to8: reserved | | +| | | | | unsigned | unsigned | | unsigned | unsigned | | unsigned | unsigned | | unsigned | unsigned | | unsigned | unsigned | | unsigned | BIT 7to4: GAIN detail | CRC result | +| DATA | 'R' | 'C' | 0 to 255 | 0 to 255 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | LSB;0.01 m | N/A | LSB:1 | BIT 3-2: reserved | from BYTE | +| | | | | | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | 0 to 65535 | | 0 to 65535 | BIT 1: TRK / NTRK | 0 to 13 | +| | | | | | | | | | | | | | | | | | | | | BIT 0: GAIN LOW/HIGH | | +|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +*/ +#define DATA_LENGTH 16 + +class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial +{ + +public: + static AP_RangeFinder_Backend_Serial *create( + RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params) + { + return new AP_RangeFinder_JRE_Serial(_state, _params); + } + +protected: + + using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override + { + return MAV_DISTANCE_SENSOR_RADAR; + } + + bool get_signal_quality_pct(int8_t &quality_pct) const override { + quality_pct = no_signal ? 0 : 100; + return true; + } + +private: + + // get a reading + bool get_reading(float &reading_m) override; + + uint8_t read_buff[DATA_LENGTH * 10]; + uint8_t read_buff_idx; + uint8_t data_buff[DATA_LENGTH * 3]; // maximum frame length + uint8_t data_buff_idx; + uint8_t data_buff_len; + + bool no_signal; // true if the latest read attempt found no valid distances +}; +#endif // AP_RANGEFINDER_JRE_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 16d30803f9..a0873dec79 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/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,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,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 0800cadd3e..a6c793c340 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -173,3 +173,6 @@ #define AP_RANGEFINDER_WASP_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_RANGEFINDER_JRE_SERIAL_ENABLED +#define AP_RANGEFINDER_JRE_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 +#endif