增加NMEA雷达

APMrover2/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZR_Boat V4.0.3 RC2"
+#define THISFIRMWARE "ZR_Boat V4.0.3 RC3"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,3,FIRMWARE_VERSION_TYPE_DEV
@@ -21,5 +21,6 @@
  * 
  * 4.0.3  去除声呐串口读取，使用uavcan，增加8、9通道控制relay 1、2
  * 4.0.3 rc2 uavcan避障调整
+ *       rc3 增加NMEA声呐
  * 
  */

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp

@@ -18,7 +18,16 @@
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <ctype.h>
 #include "AP_RangeFinder_NMEA.h"
-// #include <GCS_MAVLink/GCS.h>
+#include <GCS_MAVLink/GCS.h>
+
+#define BOAT_DEBUG 1
+#if BOAT_DEBUG
+#include <GCS_MAVLink/GCS.h>
+ # define Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "BOAT:%d: " fmt "\n", ## args)
+#else
+ # define Debug(fmt, args ...)
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 // constructor initialises the rangefinder
@@ -50,8 +59,6 @@ void AP_RangeFinder_NMEA::update(void)
     if (get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
         state.last_reading_ms = now;
-        // gcs().send_text(MAV_SEVERITY_INFO,"t:%ld,dist:%dcm",state.last_reading_ms,state.distance_cm);
-
         update_status();
     } else if ((now - state.last_reading_ms) > 3000) {
         set_status(RangeFinder::RangeFinder_NoData);
@@ -64,24 +71,27 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm)
     if (uart == nullptr) {
         return false;
     }
-    bool ret = false;
 
+    // read any available lines from the lidar
+    float sum = 0.0f;
+    uint16_t count = 0;
     int16_t nbytes = uart->available();
     while (nbytes-- > 0) {
-        uint8_t c = uart->read();
+        char c = uart->read();
         if (decode(c)) {
-            ret = true;
+            sum += _distance_m;
-            _sonar_deep = radar_data[1];
+            count++;
-            _sonar_voltage = radar_data[5];
-
-            // gcs().send_text(MAV_SEVERITY_INFO,"deep:%.2f,volt:%.2f",_sonar_deep,_sonar_voltage);
         }
     }
 
+    // return false on failure
+    if (count == 0) {
+        return false;
+    }
 
     // return average of all measurements
-    reading_cm = 100.0f * _sonar_deep;
+    reading_cm = 100.0f * sum / count;
-    return ret;
+    return true;
 }
 
 // add a single character to the buffer and attempt to decode
@@ -89,25 +99,44 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm)
 bool AP_RangeFinder_NMEA::decode(char c)
 {
     switch (c) {
-        // case '\r':
+        case ',':
+            // end of a term, add to checksum
+            _checksum ^= c;
+            FALLTHROUGH;
+        case '\r':
         case '\n':
+        case '*':
+        {
+            // null terminate and decode latest term
+            _term[_term_offset] = 0;
+            bool valid_sentence = decode_latest_term();
+
+            // move onto next term
+            _term_number++;
+            _term_offset = 0;
+            _term_is_checksum = (c == '*');
+            return valid_sentence;
+        }
+
+        case '@': // sentence begin
+        case '$': // sentence begin
+            _sentence_type = SONAR_UNKNOWN;
+            _term_number = 0;
             _term_offset = 0;
-            decode_step = 0;
+            _checksum = 0;
-                memset(_term,0,sizeof(_term));
+            _term_is_checksum = false;
-            return true;
+            _distance_m = -1.0f;
-        case ' ':
+            return false;
-            if(decode_step < 8){
-                radar_data[decode_step] = atof(_term);
-                _term_offset = 0;
-                decode_step += 1;
-                memset(_term,0,sizeof(_term));
-            }
-            break;
     }
 
-    if (_term_offset < sizeof(_term) - 1)
+    // ordinary characters are added to term
+    if (_term_offset < sizeof(_term) - 1) {
         _term[_term_offset++] = c;
-        
+    }
+    if (!_term_is_checksum) {
+        _checksum ^= c;
+    }
+
     return false;
 }
 
@@ -123,11 +152,14 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
             return false;
         }
         const uint8_t checksum = (nibble_high << 4u) | nibble_low;
-        return ((checksum == _checksum) &&
+
-                !is_negative(_distance_m) &&
+        // Debug("red:%x,_checksum:%x\n",checksum,_checksum);
-                (_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT));
+        // gcs().send_text(MAV_SEVERITY_INFO, "[%d] read:%x,_checksum:%x,dist:%.2f\n",(checksum == _checksum),checksum,_checksum, _distance_m);
+        // return true;
+        return ((checksum == _checksum) && (_sentence_type == SONAR_SIC || _sentence_type == SONAR_DPT));
     }
 
+
     // the first term determines the sentence type
     if (_term_number == 0) {
         // the first two letters of the NMEA term are the talker ID.
@@ -138,17 +170,26 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
             return false;
         }
         const char *term_type = &_term[2];
+        const char *term_sic = &_term[0];
         if (strcmp(term_type, "DBT") == 0) {
             _sentence_type = SONAR_DBT;
         } else if (strcmp(term_type, "DPT") == 0) {
             _sentence_type = SONAR_DPT;
+        }else if (strcmp(term_sic, "SIC") == 0) {
+            _sentence_type = SONAR_SIC;
         } else {
             _sentence_type = SONAR_UNKNOWN;
         }
         return false;
     }
+    if ((_sentence_type == SONAR_SIC )) {
 
-    if (_sentence_type == SONAR_DBT) {
+        if (_term_number == 5) {
+            _distance_m = atof(_term);
+            // Debug("dist:%.2f",_distance_m);
+            // cout << "type " << SONAR_SIC + 5 << ": " << _term << ", " << _term[1] <<", --- " << sonar_deep << endl;
+        }
+    }else if (_sentence_type == SONAR_DBT) {
         // parse DBT messages
         if (_term_number == 3) {
             _distance_m = atof(_term);
@@ -160,5 +201,6 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
         }
     }
 
+    // cout << "term " << int(_term_number) << ": " << _term << endl;
     return false;
 }

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h

@@ -45,6 +45,7 @@ private:
     enum sentence_types : uint8_t {
         SONAR_UNKNOWN = 0,
         SONAR_DBT,
+        SONAR_SIC,
         SONAR_DPT
     };