update to same source as master_JRE

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++) {

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 <[email protected]>
 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

libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp

@@ -14,38 +14,15 @@
  */
 
 #include "AP_RangeFinder_JRE_Serial.h"
+#include <AP_Math/AP_Math.h>
 
 #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
 
-#include <GCS_MAVLink/GCS.h>
-
-#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; read_buff_idx++) {
+            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
-                    if (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

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

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),
 

libraries/AP_RangeFinder/AP_RangeFinder_config.h

@@ -1,6 +1,9 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL_Boards.h>
+#include <AP_Scripting/AP_Scripting_config.h>
+#include <AP_CANManager/AP_CANManager_config.h>
+#include <AP_MSP/AP_MSP_config.h>
 
 #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