update RangeFinder JRE Serial driver (RangeFinderType from 38 to 41)

libraries/AP_Math/crc.cpp

@@ -372,6 +372,26 @@ uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc)
     return crc;
 }
 
+// CRC16_CCITT algorithm using 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 ^= *buf++;                      // XOR byte into least sig. byte of crc
+		for (uint8_t j = 0; j < 8; j++) {   // loop over each bit
+            if ((crc & 0x0001) != 0) {      // if the LSB is set
+                crc >>= 1;                  // shift right and XOR 0x8408
+                crc ^= 0x8408;
+            } else {
+                crc >>= 1;                  // just shift right
+            }
+		}
+	}
+
+    // output xor
+    crc = crc ^ out;
+	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

@@ -38,6 +38,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.h

@@ -104,7 +104,7 @@ class RangeFinder
         Benewake_CAN = 34,
         TeraRanger_Serial = 35,
 
-        JRE_Serial = 38,
+        JRE_Serial = 41,
         SIM = 100,
     };
 

libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp

@@ -17,34 +17,29 @@
 
 #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
 
-#include <GCS_MAVLink/GCS.h>
+#include <AP_Math/AP_Math.h>
 
-#define FRAME_HEADER_1   0x52  // 'R'
-#define FRAME_HEADER_2   0x41  // 'A'
+#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 DIST_MAX_CM 5000
+#define OUT_OF_RANGE_ADD_CM   100
 
-#define CCITT_INITPARA   0xffff
-#define CCITT_POLYNOMIAL 0x8408
-#define CCITT_OUTPARA    0xffff
-
-/* CRC16CCITT */
-static uint16_t CalcCRC16CCITT(uint8_t *cbuffer, uint16_t csize)
+void AP_RangeFinder_JRE_Serial::move_preamble_in_buffer(uint8_t search_start_pos)
 {
-    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;
-            }
+    uint8_t i;
+    for (i=search_start_pos; i<data_buff_ofs; i++) {
+        if (data_buff[i] == FRAME_HEADER_1) {
+            break;
         }
     }
-    crc = crc ^ CCITT_OUTPARA;
-    return crc;
+    if (i == 0) {
+        return;
+    }
+    memmove(data_buff, &data_buff[i], data_buff_ofs-i);
+    data_buff_ofs = data_buff_ofs - i;
 }
 
 bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
@@ -53,84 +48,95 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
     if (uart == nullptr) {
         return false;  // not update
     }
-    uint32_t n = uart->available();
-    if (n == 0) {
-        return false;
-    }
 
-    uint16_t count = 0;
-    float reading_cm;
-    // 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));
-    read_buff_idx = 0; // read_buff start data index
-    while (read_buff_idx < num_read) {
-
-        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
-                    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
-                        }
-                    }
-                }
-            }
-
-        } else if (data_buff_idx == 1) { // header second byte check
-            if (read_buff[read_buff_idx] == FRAME_HEADER_2) {
-                data_buff[1] = FRAME_HEADER_2;
-                data_buff_idx = 2;    // next data_buff
-            } else {
-                data_buff_idx = 0;    // data index clear
-            }
-            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);
-                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;
-                        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;
-                        }
-                    }
-                    count++;
-                }
-                data_buff_idx = 0; // data index clear
-            } else {
-                data_buff[data_buff_idx++] = read_buff[read_buff_idx++];
-            }
+    uint16_t valid_count = 0;   // number of valid readings
+    uint16_t invalid_count = 0; // number of invalid readings
+    float sum = 0;
+
+    // read a maximum of 8192 bytes per call to this function:
+    uint16_t bytes_available = MIN(uart->available(), 8192U);
+
+    while (bytes_available > 0) {
+        // fill buffer
+        const auto num_bytes_to_read = MIN(bytes_available, ARRAY_SIZE(data_buff) - data_buff_ofs);
+        const auto num_bytes_read = uart->read(&data_buff[data_buff_ofs], num_bytes_to_read);
+        if (num_bytes_read == 0) {
+            break;
+        }
+        if (bytes_available < num_bytes_read) {
+            // this is a bug in the uart call.
+            break;
+        }
+        bytes_available -= num_bytes_read;
+        data_buff_ofs += num_bytes_read;
+
+        // move header frame header in buffer
+        move_preamble_in_buffer(0);
+
+        // ensure we have a packet type:
+        if (data_buff_ofs < 2) {
+            continue;
+        }
+
+        // determine packet length for incoming packet:
+        uint8_t packet_length;
+        switch (data_buff[1]) {
+        case FRAME_HEADER_2_A:                   
+            packet_length = 16;
+            break;
+        case FRAME_HEADER_2_B:
+            packet_length = 32;
+            break;
+        case FRAME_HEADER_2_C:
+            packet_length = 48;
+            break;
+        default:
+            move_preamble_in_buffer(1);
+            continue;
         }
+
+        // check there are enough bytes for message type
+        if (data_buff_ofs < packet_length) {
+            continue;
+        }
+
+        // check the checksum
+        const uint16_t crc = crc16_ccitt_r(data_buff, packet_length - 2, 0xffff, 0xffff);
+        if (crc != ((data_buff[packet_length-1] << 8) | data_buff[packet_length-2])) {
+            // CRC failure
+            move_preamble_in_buffer(1);
+            continue;
+        }
+
+        // check random bit to for magic value:
+        if (data_buff[packet_length-3] & 0x02) { // NTRK
+            invalid_count++;
+            // discard entire packet:
+            move_preamble_in_buffer(packet_length);
+            continue;
+        }
+
+        // good message, extract rangefinder reading:
+        reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f;
+        sum += reading_m;
+        valid_count++;
+        move_preamble_in_buffer(packet_length);
     }
 
-    if (count > 0) {
+    // 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, max_distance_cm() + OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
+        return true;
+    }
+
     return false;
 }
 #endif  // AP_RANGEFINDER_JRE_SERIAL_ENABLED