AP_RangeFinder: update serial driver for JRE

jfbblue0922 · Aug 8, 2023 · 49a1cf2 · 49a1cf2
1 parent b33f2d4
commit 49a1cf2
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 42 deletions.
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
@@ -542,11 +542,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         serial_create_fn = AP_RangeFinder_NoopLoop::create;
 #endif
         break;
-    case Type::JRE_Serial:
 #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
+    case Type::JRE_Serial:
         serial_create_fn = AP_RangeFinder_JRE_Serial::create;
-#endif
         break;
+#endif
 
     case Type::NONE:
         break;

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp
@@ -13,29 +13,29 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "AP_RangeFinder_JRE_Serial.h"
-#include <AP_Math/AP_Math.h>
+#include "AP_RangeFinder_config.h"
 
 #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
 
+#include "AP_RangeFinder_JRE_Serial.h"
+#include <AP_Math/AP_Math.h>
+
 #define FRAME_HEADER_1   'R'    // 0x52
 #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)
 {
     // uart instance check
     if (uart == nullptr) {
         return false;  // not update
     }
-    uint32_t n = uart->available();
-    if (n == 0) {
-        return false;
-    }
 
-    uint16_t count = 0;
-    float reading_cm;
+    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();
 
@@ -46,25 +46,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,27 +62,21 @@ 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 = crc16_ccitt_r(data_buff, ARRAY_SIZE(data_buff) - 2, 0xffff, 0xffff);
-                if ((((crc>>8) & 0xff) == data_buff[15]) && ((crc & 0xff) == data_buff[14])) {
+                if ((HIGHBYTE(crc) == data_buff[15]) && (LOWBYTE(crc) == 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;
-                        }
+                        invalid_count++;
+                    } else { // TRK
+                        reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f;
+                        sum += reading_m;
+                        valid_count++;
                     }
-                    count++;
                 }
                 data_buff_idx = 0; // data index clear
             } else {
@@ -104,9 +85,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
@@ -1,10 +1,12 @@
 #pragma once
 
-#include "AP_RangeFinder.h"
-#include "AP_RangeFinder_Backend_Serial.h"
+#include "AP_RangeFinder_config.h"
 
 #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
 
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
 #define DATA_LENGTH 16
 
 class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial