AP_RangeFinder: update serial driver for JRE

jfbblue0922 · Jul 26, 2023 · d3d21a5 · d3d21a5
1 parent b33f2d4
commit d3d21a5
Showing 1 changed file with 22 additions and 24 deletions.
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp
@@ -22,6 +22,7 @@
 #define FRAME_HEADER_2   'A'    // 0x41
 
 #define DIST_MAX_CM 50000
+#define OUT_OF_RANGE_ADD_CM   100
 
 bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
 {
@@ -34,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();
@@ -46,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;
                 }
             }
 
@@ -75,7 +65,7 @@ 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
@@ -84,18 +74,17 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
                 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 {
@@ -104,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