diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 9835f49595ad03..bfc9b1dedfc5c4 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -374,6 +374,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 7cd9d380e4ba67..7c7b908c7bc372 100644 --- a/libraries/AP_Math/crc.h +++ b/libraries/AP_Math/crc.h @@ -40,6 +40,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_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp index b1ab0a01ac710f..5766ac219d8d5e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -14,38 +14,15 @@ */ #include "AP_RangeFinder_JRE_Serial.h" +#include #if AP_RANGEFINDER_JRE_SERIAL_ENABLED -#include - -#define FRAME_HEADER_1 0x52 // 'R' -#define FRAME_HEADER_2 0x41 // 'A' +#define FRAME_HEADER_1 'R' // 0x52 +#define FRAME_HEADER_2 'A' // 0x41 #define DIST_MAX_CM 50000 - -#define CCITT_INITPARA 0xffff -#define CCITT_POLYNOMIAL 0x8408 -#define CCITT_OUTPARA 0xffff - -/* CRC16CCITT */ -static uint16_t CalcCRC16CCITT(uint8_t *cbuffer, uint16_t csize) -{ - uint16_t crc = CCITT_INITPARA; - for (int i = 0; i < csize; i++) { - crc ^= *cbuffer++; - for (int j = 0; j < 8; j++) { - if ((crc & 0x0001) != 0) { - crc >>= 1; - crc ^= CCITT_POLYNOMIAL; - } else { - crc >>= 1; - } - } - } - crc = crc ^ CCITT_OUTPARA; - return crc; -} +#define OUT_OF_RANGE_ADD_CM 100 bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) { @@ -58,7 +35,9 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) return false; } - uint16_t count = 0; + uint16_t valid_count = 0; // number of valid readings + uint16_t invalid_count = 0; // number of invalid readings + float sum = 0; float reading_cm; // max distance the sensor can reliably measure - read from parameters const int16_t distance_cm_max = max_distance_cm(); @@ -70,25 +49,12 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) if (data_buff_idx == 0) { // header first byte check // header data search - for (; read_buff_idx= num_read - 1) { // read end byte - if (count > 0) { - return true; - } - return false; // next packet to header second byte - } else { - if (read_buff[read_buff_idx + 1] == FRAME_HEADER_2) { - data_buff[1] = FRAME_HEADER_2; - data_buff_idx = 2; // next data_buff - read_buff_idx += 2; // next read_buff - break; // next data set - } else { - data_buff_idx = 0; // data index clear - } - } + read_buff_idx++; // next read_buff + break; } } @@ -99,27 +65,26 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) } else { data_buff_idx = 0; // data index clear } - read_buff_idx++; // next read_buff + read_buff_idx++; // next read_buff } else { // data set if (data_buff_idx >= ARRAY_SIZE(data_buff)) { // 1 data set complete // crc check - uint16_t crc = CalcCRC16CCITT(data_buff, ARRAY_SIZE(data_buff) - 2); + uint16_t crc = crc16_ccitt_r(data_buff, ARRAY_SIZE(data_buff) - 2, 0xffff, 0xffff); if ((((crc>>8) & 0xff) == data_buff[15]) && ((crc & 0xff) == data_buff[14])) { // status check if (data_buff[13] & 0x02) { // NTRK - no_signal = true; - reading_m = MIN(MAX(DIST_MAX_CM, distance_cm_max), UINT16_MAX) * 0.01f; - } else { // UPDATE DATA - no_signal = false; + invalid_count++; + } else { // TRK reading_cm = data_buff[4] * 256 + data_buff[5]; if (reading_cm < distance_cm_max) { reading_m = reading_cm * 0.01f; } else { reading_m = distance_cm_max * 0.01f; } + sum += reading_m; + valid_count++; } - count++; } data_buff_idx = 0; // data index clear } else { @@ -128,9 +93,18 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) } } - if (count > 0) { + 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 index 0249919543a447..03115917a5c005 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h @@ -3,10 +3,6 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" -#ifndef AP_RANGEFINDER_JRE_SERIAL_ENABLED -#define AP_RANGEFINDER_JRE_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED -#endif - #if AP_RANGEFINDER_JRE_SERIAL_ENABLED #define DATA_LENGTH 16 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 2cdcffb6664289..04d9bb4b853cd6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -15,7 +15,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:JRE_Serial, 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: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 c3fdf94cdef0ee..173bad858dc8c9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -1,6 +1,9 @@ #pragma once #include +#include +#include +#include #ifndef AP_RANGEFINDER_ENABLED #define AP_RANGEFINDER_ENABLED 1 @@ -9,3 +12,150 @@ #ifndef AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED #define AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED AP_RANGEFINDER_ENABLED #endif + +#ifndef AP_RANGEFINDER_ANALOG_ENABLED +#define AP_RANGEFINDER_ANALOG_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_BBB_PRU_ENABLED +#define AP_RANGEFINDER_BBB_PRU_ENABLED ( \ + AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI \ + ) +#endif + +#ifndef AP_RANGEFINDER_BENEWAKE_ENABLED +#define AP_RANGEFINDER_BENEWAKE_ENABLED AP_RANGEFINDER_ENABLED +#endif + +#ifndef AP_RANGEFINDER_BENEWAKE_CAN_ENABLED +#define AP_RANGEFINDER_BENEWAKE_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#ifndef AP_RANGEFINDER_BENEWAKE_TF02_ENABLED +#define AP_RANGEFINDER_BENEWAKE_TF02_ENABLED (AP_RANGEFINDER_BENEWAKE_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#ifndef AP_RANGEFINDER_BENEWAKE_TF03_ENABLED +#define AP_RANGEFINDER_BENEWAKE_TF03_ENABLED (AP_RANGEFINDER_BENEWAKE_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#ifndef AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED +#define AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED (AP_RANGEFINDER_BENEWAKE_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#ifndef AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED +#define AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_BLPING_ENABLED +#define AP_RANGEFINDER_BLPING_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_BEBOP_ENABLED +#define AP_RANGEFINDER_BEBOP_ENABLED \ + AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && \ + (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && \ + defined(HAVE_LIBIIO) +#endif + +#ifndef AP_RANGEFINDER_DRONECAN_ENABLED +#define AP_RANGEFINDER_DRONECAN_ENABLED (HAL_ENABLE_DRONECAN_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#ifndef AP_RANGEFINDER_GYUS42V2_ENABLED +#define AP_RANGEFINDER_GYUS42V2_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_HC_SR04_ENABLED +#define AP_RANGEFINDER_HC_SR04_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_LANBAO_ENABLED +#define AP_RANGEFINDER_LANBAO_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_LEDDARONE_ENABLED +#define AP_RANGEFINDER_LEDDARONE_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_LEDDARVU8_ENABLED +#define AP_RANGEFINDER_LEDDARVU8_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_LWI2C_ENABLED +#define AP_RANGEFINDER_LWI2C_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED +#define AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_LUA_ENABLED +#define AP_RANGEFINDER_LUA_ENABLED AP_SCRIPTING_ENABLED +#endif + +#ifndef AP_RANGEFINDER_MAVLINK_ENABLED +#define AP_RANGEFINDER_MAVLINK_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED +#define AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_MAXSONARI2CXL_ENABLED +#define AP_RANGEFINDER_MAXSONARI2CXL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef HAL_MSP_RANGEFINDER_ENABLED +#define HAL_MSP_RANGEFINDER_ENABLED HAL_MSP_ENABLED +#endif + +#ifndef AP_RANGEFINDER_NMEA_ENABLED +#define AP_RANGEFINDER_NMEA_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_NOOPLOOP_ENABLED +#define AP_RANGEFINDER_NOOPLOOP_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 +#endif + +#ifndef AP_RANGEFINDER_PWM_ENABLED +#define AP_RANGEFINDER_PWM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#ifndef AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED +#define AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_SIM_ENABLED +#define AP_RANGEFINDER_SIM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#ifndef AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED +#define AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_TRI2C_ENABLED +#define AP_RANGEFINDER_TRI2C_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_USD1_CAN_ENABLED +#define AP_RANGEFINDER_USD1_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#ifndef AP_RANGEFINDER_VL53L0X_ENABLED +#define AP_RANGEFINDER_VL53L0X_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_VL53L1X_ENABLED +#define AP_RANGEFINDER_VL53L1X_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RANGEFINDER_WASP_ENABLED +#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