From 6e812580acf76d5f99aac92cde6a606348053b82 Mon Sep 17 00:00:00 2001 From: jfbblue0922 Date: Fri, 13 Oct 2023 17:45:22 +0900 Subject: [PATCH] add JRE --- libraries/AP_Math/crc.cpp | 47 ++++++++ libraries/AP_Math/crc.h | 1 + libraries/AP_RangeFinder/AP_RangeFinder.cpp | 7 ++ libraries/AP_RangeFinder/AP_RangeFinder.h | 1 + .../AP_RangeFinder_JRE_Serial.cpp | 112 ++++++++++++++++++ .../AP_RangeFinder_JRE_Serial.h | 106 +++++++++++++++++ .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_config.h | 4 + 8 files changed, 279 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_Math/crc.cpp b/libraries/AP_Math/crc.cpp index bc059535daefc4..e538d243758ff5 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -384,6 +384,53 @@ uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc) return crc; } +/* right shift table */ +static const uint16_t crc16tab_r[256] = { + 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF, + 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7, + 0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E, + 0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876, + 0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD, + 0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5, + 0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C, + 0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974, + 0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB, + 0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3, + 0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A, + 0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72, + 0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9, + 0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1, + 0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738, + 0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70, + 0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7, + 0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF, + 0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036, + 0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E, + 0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5, + 0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD, + 0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134, + 0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C, + 0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3, + 0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB, + 0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232, + 0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A, + 0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1, + 0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9, + 0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330, + 0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78, +}; + +// right shift +uint16_t crc16_ccitt_r(const uint8_t *buf, uint32_t len, uint16_t crc, uint16_t out) +{ + for (uint32_t i = 0; i < len; i++) { + crc = (crc >> 8) ^ crc16tab_r[(crc ^ *buf++) & 0x00FF]; + } + crc = crc ^ out; // output xor + + return crc; +} + uint16_t crc16_ccitt_GDL90(const uint8_t *buf, uint32_t len, uint16_t crc) { for (uint32_t i = 0; i < len; i++) { diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h index 6850445b66bc6d..cf994a5c23c8b2 100644 --- a/libraries/AP_Math/crc.h +++ b/libraries/AP_Math/crc.h @@ -41,6 +41,7 @@ uint8_t crc_sum8(const uint8_t *p, uint8_t len); // Copyright (C) 2010 Swift Navigation Inc. // Contact: Fergus Noble uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc); +uint16_t crc16_ccitt_r(const uint8_t *buf, uint32_t len, uint16_t crc, uint16_t out); // CRC16_CCITT algorithm using the GDL90 parser method which is non-standard // https://www.faa.gov/nextgen/programs/adsb/archival/media/gdl90_public_icd_reva.pdf diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 9651b919a4433a..bd75fcaf864036 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -55,6 +55,7 @@ #include "AP_RangeFinder_NoopLoop.h" #include "AP_RangeFinder_TOFSenseP_CAN.h" #include "AP_RangeFinder_NRA24_CAN.h" +#include "AP_RangeFinder_JRE_Serial.h" #include #include @@ -557,6 +558,12 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) #endif break; +#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 7365ed3912cdf5..f9d52dfb9f30ff 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -96,6 +96,7 @@ class RangeFinder NoopLoop_P = 37, TOFSenseP_CAN = 38, NRA24_CAN = 39, + JRE_Serial = 40, SIM = 100, }; 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 00000000000000..9c3ed52199c890 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -0,0 +1,112 @@ +/* + 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 50000 +#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 + for (; read_buff_idx < num_read; read_buff_idx++) { + if (read_buff[read_buff_idx] == FRAME_HEADER_1) { + data_buff[0] = FRAME_HEADER_1; + data_buff_idx = 1; // next data_buff + break; + } + } + + } 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 + 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 + read_buff_idx--; // exclude current index + } else { + data_buff[data_buff_idx++] = read_buff[read_buff_idx]; + } + } + } + + if (valid_count > 0) { + no_signal = false; + reading_m = sum / valid_count; + return true; + } + + 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 00000000000000..c5e2c22294042d --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h @@ -0,0 +1,106 @@ +#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_num; + 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; +}; +#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 3f3cd9511d5c73..198a0aba86bc3f 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, 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: 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 36c5dcf89c98ec..db6f6feb356cc0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -166,3 +166,7 @@ #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 +#endif +