声呐高度替换成GPS

APMrover2/Underwater_Sonar.cpp

@@ -49,16 +49,6 @@ bool Underwater_Sonar::read(void)
         sonar_data[data_len-nbytes] = temp;
         // Debug("read %d:%d",nbytes,temp);
         if(_decode(temp)){
-
-            // // write(&temp,1);
-            // for(int j = 0; j < 8; j++){
-            //     // radar_data = _sonar[j];
-            //     // memcpy(*radar_data,_sonar[j],sizeof(_sonar[j]));
-            //     radar_data[j] = atof(_term);
-            //     // cout << j << " : " << radar_data[j] << endl;
-            // }
-            // // memset(_term,0,sizeof(_term));
-
             ret = true;
         }
     }

libraries/AP_Logger/LogFile.cpp

@@ -594,6 +594,13 @@ 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();
+    if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        altitude_gps = gps.location().alt;
+    } else {
+        altitude_gps = 0;
+    }
 
     const Underwater_Sonar sonar = AP::underwater_sonar();
     struct log_Underwater_Sonar pkt{
@@ -601,7 +608,7 @@ void AP_Logger::Write_Underwater_Sonar(const Location &current_loc)
         time_us         : AP_HAL::micros64(),
         latitude    : current_loc.lat,
         longitude   : current_loc.lng,
-        altitude    : current_loc.alt,
+        altitude    : altitude_gps,
     };
 
         for (uint8_t i = 0; i < 8; i++) {