单波束修改，uavcan模式和串口模式

APMrover2/Log.cpp

@@ -49,6 +49,15 @@ void Rover::Log_Write_Depth()
     if (!ahrs.get_position(loc)) {
         return;
     }
+    float deepthR = (float)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f);
+    static float last_deep;
+    static uint32_t last_1s;
+    uint32_t tnow = AP_HAL::millis();
+    if (tnow - last_1s > 1000 || fabs(last_deep - deepthR) > 0.01) {
+        last_1s = tnow;
+        last_deep = deepthR;
+        logger.Write_Underwater_Sonar(loc);
+    }
 
     // check if new sensor reading has arrived
     uint32_t reading_ms = rangefinder.last_reading_ms(ROTATION_PITCH_270);
@@ -62,7 +71,7 @@ void Rover::Log_Write_Depth()
                         AP_HAL::micros64(),
                         loc.lat,
                         loc.lng,
-                        (double)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f));
+                        deepthR);
 }
 
 // guided mode logging

APMrover2/Rover.cpp

@@ -263,10 +263,10 @@ void Rover::update_logging2(void)
     
     // underwater_sonar.update(serial_manager,current_loc);
     uint16_t rc8_in = RC_Channels::rc_channel(CH_8)->get_radio_in();
-    uint16_t rc9_in = RC_Channels::rc_channel(CH_9)->get_radio_in();
+    // uint16_t rc9_in = RC_Channels::rc_channel(CH_9)->get_radio_in();
     AP_Relay *_apm_relay = AP::relay();
     rc8_in > 1500?_apm_relay->on(1):_apm_relay->off(1);
-    rc9_in > 1500?_apm_relay->on(2):_apm_relay->off(2);
+    // rc9_in > 1500?_apm_relay->on(2):_apm_relay->off(2);
 
 }
 

APMrover2/Rover.h

@@ -441,7 +441,7 @@ private:
     static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
                   "_failsafe_priorities is missing the sentinel");
 
-    Underwater_Sonar underwater_sonar;
+    // Underwater_Sonar underwater_sonar;
 public:
     void mavlink_delay_cb();
     void failsafe_check();

APMrover2/system.cpp

@@ -163,7 +163,7 @@ void Rover::init_ardupilot()
     // flag that initialisation has completed
     initialised = true;
 
-    underwater_sonar.init(serial_manager);
+    // underwater_sonar.init(serial_manager);
 #if AP_PARAM_KEY_DUMP
     AP_Param::show_all(hal.console, true);
 #endif

libraries/AP_Logger/LogFile.cpp

@@ -594,26 +594,30 @@ void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location &current_l
 
 void AP_Logger::Write_Underwater_Sonar(const Location &current_loc)
 {
-    int32_t altitude_gps;
+    
-    const AP_GPS &gps = AP::gps();
+    const RangeFinder *rangefinder = RangeFinder::get_singleton();
-    if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+    AP_Baro &baro = AP::baro();
-        altitude_gps = gps.location().alt;
+    // only log depth on boats with working downward facing range finders
-    } else {
+    if ( !rangefinder->has_data_orient(ROTATION_PITCH_270)) {
-        altitude_gps = 0;
+        return;
     }
+    float deepthR = (float)(rangefinder->distance_cm_orient(ROTATION_PITCH_270) * 0.01f);
+    
+    const AP_GPS &gps = AP::gps();
 
-    const Underwater_Sonar sonar = AP::underwater_sonar();
     struct log_Underwater_Sonar pkt{
         LOG_PACKET_HEADER_INIT(LOG_UNDERWATER_SONAR_MSG),
         time_us         : AP_HAL::micros64(),
         latitude    : current_loc.lat,
         longitude   : current_loc.lng,
-        altitude    : altitude_gps,
+        altitude    : current_loc.alt,
+        temperature :   baro.get_temperature(0),
+        deepth      :   deepthR,
+        gps_lat:gps.location().lat,
+        gps_lng:gps.location().lng,
+        gps_alt:gps.location().alt,
     };
 
-        for (uint8_t i = 0; i < 8; i++) {
-            pkt.radar_data[i] = sonar.radar_data[i] ;
-        }
     WriteBlock(&pkt, sizeof(pkt));
 }
 

libraries/AP_Logger/LogStructure.h

@@ -732,12 +732,17 @@ struct PACKED log_Camera {
 
 
 struct PACKED log_Underwater_Sonar {
-    LOG_PACKET_HEADER;
+  LOG_PACKET_HEADER;
-    uint64_t time_us;
+  uint64_t time_us;
-    int32_t  latitude;
+  int32_t latitude;
-    int32_t  longitude;
+  int32_t longitude;
-    int32_t  altitude;
+  int32_t altitude;
-    float radar_data[8];
+  // float radar_data[8];
+  float temperature;
+  float deepth;
+  int32_t gps_lat;
+  int32_t gps_lng;
+  int32_t gps_alt;
 };
 
 struct PACKED log_Attitude {
@@ -1431,7 +1436,7 @@ struct PACKED log_Arm_Disarm {
     { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
       "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \
     { LOG_UNDERWATER_SONAR_MSG, sizeof(log_Underwater_Sonar), \
-      "UDSR", "QLLeffffffff","TimeUS,Lat,Lng,Alt,C1,C2,C3,C4,C5,C6,C7,C8", "s--DUm--------", "F-----------" }
+      "UDSR", "QLLeffLLe","TimeUS,Lat,Lng,Alt,Temp,Deep,GLat,GLng,GAlt", "sDUm--DUm", "FGGB--GGB" }
     
 // messages for more advanced boards
 #define LOG_EXTRA_STRUCTURES \

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp

@@ -62,27 +62,22 @@ 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) {
-        char c = uart->read();
+        uint8_t c = uart->read();
         if (decode(c)) {
-            sum += _distance_m;
+            ret = true;
-            count++;
+            _sonar_deep = radar_data[1];
+            _sonar_voltage = radar_data[5];
         }
     }
 
-    // return false on failure
-    if (count == 0) {
-        return false;
-    }
 
     // return average of all measurements
-    reading_cm = 100.0f * sum / count;
+    reading_cm = 100.0f * _sonar_deep;
-    return true;
+    return ret;
 }
 
 // add a single character to the buffer and attempt to decode
@@ -90,43 +85,25 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm)
 bool AP_RangeFinder_NMEA::decode(char c)
 {
     switch (c) {
-    case ',':
+        // case '\r':
-        // end of a term, add to checksum
+        case '\n':
-        _checksum ^= c;
+            _term_offset = 0;
-        FALLTHROUGH;
+            decode_step = 0;
-    case '\r':
+                memset(_term,0,sizeof(_term));
-    case '\n':
+            return true;
-    case '*':
+        case ' ':
-    {
+            if(decode_step < 8){
-        // null terminate and decode latest term
+                radar_data[decode_step] = atof(_term);
-        _term[_term_offset] = 0;
+                _term_offset = 0;
-        bool valid_sentence = decode_latest_term();
+                decode_step += 1;
-
+                memset(_term,0,sizeof(_term));
-        // move onto next term
+            }
-        _term_number++;
+            break;
-        _term_offset = 0;
-        _term_is_checksum = (c == '*');
-        return valid_sentence;
-    }
-
-    case '$': // sentence begin
-        _sentence_type = SONAR_UNKNOWN;
-        _term_number = 0;
-        _term_offset = 0;
-        _checksum = 0;
-        _term_is_checksum = false;
-        _distance_m = -1.0f;
-        return false;
     }
 
-    // ordinary characters are added to term
+    if (_term_offset < sizeof(_term) - 1)
-    if (_term_offset < sizeof(_term) - 1) {
         _term[_term_offset++] = c;
-    }
+        
-    if (!_term_is_checksum) {
-        _checksum ^= c;
-    }
-
     return false;
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h

@@ -63,11 +63,27 @@ private:
     AP_HAL::UARTDriver *uart = nullptr;     // pointer to serial uart
 
     // message decoding related members
-    char _term[15];                         // buffer for the current term within the current sentence
+    // char _term[15];                         // buffer for the current term within the current sentence
-    uint8_t _term_offset;                   // offset within the _term buffer where the next character should be placed
+    // uint8_t _term_offset;                   // offset within the _term buffer where the next character should be placed
     uint8_t _term_number;                   // term index within the current sentence
     float _distance_m;                      // distance in meters parsed from a term, -1 if no distance
     uint8_t _checksum;                      // checksum accumulator
     bool _term_is_checksum;                 // current term is the checksum
     sentence_types _sentence_type;          // the sentence type currently being processed
+
+
+    float radar_data[8];
+
+    uint8_t     ucCRCHi;
+    uint8_t     ucCRCLo;
+    int         iIndex;
+    uint8_t     sonar_data[25];
+    char _term[10];                                                     ///< buffer for the current term within the current sentence
+    uint8_t _term_offset;                                       ///< character offset with the term being received
+    uint8_t _term_num;
+    uint32_t  data_index;
+    float _sonar_deep;
+    float _sonar_voltage;
+    uint8_t decode_step;
+
 };

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4311,9 +4311,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
     case MSG_RANGEFINDER:
         {
         CHECK_PAYLOAD_SIZE(RANGEFINDER);
-        //send_rangefinder();
+        send_rangefinder();
-            Underwater_Sonar sonar = AP::underwater_sonar();
+            // Underwater_Sonar sonar = AP::underwater_sonar();
-            sonar.send_sonar_mavlink(chan);
+            // sonar.send_sonar_mavlink(chan);
         }
         break;