From d1ced8a51bb9c0636b9794130744c2c7106b4a91 Mon Sep 17 00:00:00 2001 From: zbr Date: Fri, 4 Jun 2021 17:00:07 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A3=B0=E5=91=90=E9=AB=98=E5=BA=A6=E6=9B=BF?= =?UTF-8?q?=E6=8D=A2=E6=88=90GPS?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- APMrover2/Underwater_Sonar.cpp | 10 ---------- libraries/AP_Logger/LogFile.cpp | 9 ++++++++- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/APMrover2/Underwater_Sonar.cpp b/APMrover2/Underwater_Sonar.cpp index 80bb8571ee..9b987f9d27 100644 --- a/APMrover2/Underwater_Sonar.cpp +++ b/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; } } diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 49d3a03fbb..cc3ddf1b79 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -594,6 +594,13 @@ void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location ¤t_l void AP_Logger::Write_Underwater_Sonar(const Location ¤t_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 ¤t_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++) {