AP_RangeFinder_NMEA: 增加声呐解析，,SIP

ArduCopter/AP_Arming.cpp

@@ -847,6 +847,9 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che
 
 bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure)
 {
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    return true;
+#else
     uint32_t date;
     if (!copter.zr_app.check_deadline(date))
     {
@@ -857,4 +860,6 @@ bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure)
     }else{
         return true;
     }
+    
+#endif
 }

ArduCopter/GCS_Mavlink.cpp

@@ -607,12 +607,15 @@ void GCS_MAVLINK_Copter::send_banner()
     char frame_and_type_string[30];
     copter.motors->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
     send_text(MAV_SEVERITY_INFO, "%s", frame_and_type_string);
-    
+
+#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
     uint32_t deadline = 0;
     copter.zr_app.get_deadline_params(deadline);
     send_text(MAV_SEVERITY_INFO, "Date:%lu",deadline);
     send_text(MAV_SEVERITY_INFO, "ID:%lu",copter.zr_app.get_zr_sysid());
     
+#endif
+    
 }
 
 void GCS_MAVLINK_Copter::handle_command_ack(const mavlink_message_t &msg)

Tools/ardupilotwaf/boards.py

@@ -278,7 +278,7 @@ class Board:
             '-Werror=unused-value',
             '-Werror=unused-variable',
             '-Werror=delete-non-virtual-dtor',
-            '-Wfatal-errors',
+            # '-Wfatal-errors',
             '-Wno-trigraphs',
             '-Werror=parentheses',
             '-DARDUPILOT_BUILD',
@@ -834,7 +834,7 @@ class chibios(Board):
             '-Wmissing-declarations',
             '-Wno-unused-parameter',
             '-Werror=array-bounds',
-            '-Wfatal-errors',
+            # '-Wfatal-errors',
             '-Werror=uninitialized',
             '-Werror=init-self',
             '-Werror=unused-but-set-variable',

Tools/autotest/locations.txt

@@ -1,4 +1,5 @@
 #NAME=latitude,longitude,absolute-altitude,heading
+CYDS=24.6187612,118.0383748,0,0
 OSRF0=37.4003371,-122.0800351,0,353
 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270
 CMAC=-35.363261,149.165230,584,353

Tools/environment_install/install-prereqs-ubuntu.sh

@@ -303,7 +303,8 @@ echo "Done!"
 
 CCACHE_PATH=$(which ccache)
 if [[ $DO_AP_STM_ENV -eq 1 ]]; then
-  install_arm_none_eabi_toolchain
+#   install_arm_none_eabi_toolchain
+  echo "*********** exit install_arm_none_eabi_toolchain **********!"
 fi
 
 heading "Check if we are inside docker environment..."

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp

@@ -20,6 +20,14 @@
 #include <AP_HAL/AP_HAL.h>
 #include <ctype.h>
 
+#define BOAT_DEBUG 0
+#if BOAT_DEBUG
+#include <GCS_MAVLink/GCS.h>
+ # define Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "Sonar: " fmt "\n", ## args)
+#else
+ # define Debug(fmt, args ...)
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 // return last value measured by sensor
@@ -90,6 +98,7 @@ bool AP_RangeFinder_NMEA::decode(char c)
         return valid_sentence;
     }
 
+    case '@': // sentence begin
     case '$': // sentence begin
         _sentence_type = SONAR_UNKNOWN;
         _term_number = 0;
@@ -119,6 +128,9 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
     // handle the last term in a message
     if (_term_is_checksum) {
         _sentence_done = true;
+        if(_sentence_type == SONAR_EDEP){   // EDEP没有CRC
+            return true;
+        }
         uint8_t nibble_high = 0;
         uint8_t nibble_low  = 0;
         if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) {
@@ -151,6 +163,7 @@ 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) {
@@ -159,6 +172,10 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
             _sentence_type = SONAR_MTW;
         } else if (strcmp(term_type, "ED") == 0) {
             _sentence_type = SONAR_HDED;
+        }else if (strcmp(term_sic, "SIC") == 0) {
+            _sentence_type = SONAR_SIC;
+        }else if (strcmp(term_sic, "EDEP") == 0) {
+            _sentence_type = SONAR_EDEP;
         } else {
             _sentence_type = SONAR_UNKNOWN;
         }
@@ -185,6 +202,30 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
         if (_term_number == 4) {
             _distance_m = strtof(_term, NULL);
         }
+    }else if ((_sentence_type == SONAR_SIC )) {
+
+        if (_term_number == 5) {
+            _distance_m = atof(_term);
+            Debug("dist:%.2f",_distance_m);
+        }
+    }else if (_sentence_type == SONAR_EDEP) {
+        // parse DBT messages
+        switch (_sentence_type + _term_number) {
+            case SONAR_EDEP + 1:
+                _distance_m = atof(_term);
+                // Debug("distance:%.2f",_distance_m);
+                Debug("%.2f,%.2f,%d",_distance_m,delt_dist,sound_speed);
+                break;
+            case SONAR_EDEP + 2:
+                delt_dist = atof(_term);
+                Debug("delt distance:%.2f",delt_dist);
+                break;
+            case SONAR_EDEP + 3:
+                sound_speed = atoi(_term);
+                Debug("sound speed:%d",sound_speed);
+                break;
+        }
+        
     }
 
     return false;

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h

@@ -52,6 +52,8 @@ private:
         SONAR_DPT,
         SONAR_MTW,  // mean water temperature
         SONAR_HDED, // hondex custom sonar message
+        SONAR_SIC,
+        SONAR_EDEP
     };
 
     // get a distance reading
@@ -83,6 +85,9 @@ private:
     bool _term_is_checksum;                 // current term is the checksum
     sentence_types _sentence_type;          // the sentence type currently being processed
     bool _sentence_done;                    // true if this sentence has already been decoded
+
+    float delt_dist;
+    int sound_speed;
 };
 
 #endif  // AP_RANGEFINDER_NMEA_ENABLED