From 891b85001d82e1cd79a085958fba2bddf948659f Mon Sep 17 00:00:00 2001 From: nagezeng Date: Wed, 8 Jun 2022 14:15:59 +0800 Subject: [PATCH] =?UTF-8?q?Mark=E5=A2=9E=E5=8A=A0=E5=AE=9A=E4=BD=8D?= =?UTF-8?q?=E7=8A=B6=E6=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/version.h | 2 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 3 ++- libraries/AP_Logger/AP_Logger.h | 2 +- libraries/AP_Logger/LogFile.cpp | 5 +++-- libraries/AP_Logger/LogStructure.h | 3 ++- 5 files changed, 9 insertions(+), 6 deletions(-) diff --git a/ArduCopter/version.h b/ArduCopter/version.h index c68603b841..3bfaf19aa3 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -5,7 +5,7 @@ #endif #include "ap_version.h" -#define GIT_VERSION "1" +#define GIT_VERSION "2" #define THISFIRMWARE "ZRUAV v4.1.18" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 4b0caa91b6..f1a62ae49b 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -332,7 +332,8 @@ bool AP_GPS_NMEA::_term_complete() gps_mark.second, llh, sigma, - cam_pos_ned); + cam_pos_ned, + state.status); // gcs().send_text(MAV_SEVERITY_INFO, "mrk:%d, %d, %d, %d", gps_mark.week, gps_mark.second, (int32_t)(gps_mark.lat * (int32_t)10000000UL), (int32_t)(gps_mark.lon * (int32_t)10000000UL)); // gcs().send_text(MAV_SEVERITY_INFO,"rot:%dus,%.3f,%.3f,%.3f",(uint16_t)(tk2 - tk1),cam_pos_ned.x,cam_pos_ned.y,cam_pos_ned.z); } diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index c13ea38e7e..57802df083 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -288,7 +288,7 @@ public: void Write_OABendyRuler(bool active, float target_yaw, float margin, const Location &final_dest, const Location &oa_dest); void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest); void Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2,uint16_t cam3,uint16_t cam4,uint16_t cam5,uint8_t flag); - void Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,const Vector3f& sigma,const Vector3f& offset); + void Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,const Vector3f& sigma,const Vector3f& offset,uint8_t fix_type); void Write(const char *name, const char *labels, const char *fmt, ...); void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 9d157fa840..bd86a9aa41 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -1029,7 +1029,7 @@ void AP_Logger::Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2 WriteBlock(&pkt, sizeof(pkt)); } -void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,const Vector3f& sigma,const Vector3f& offset) +void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,const Vector3f& sigma,const Vector3f& offset,uint8_t fix_type) { const AP_AHRS &ahrs = AP::ahrs(); @@ -1050,7 +1050,8 @@ void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d hgt_sigma :sigma.z , n_offset :offset.x , e_offset :offset.y , - d_offset :offset.z + d_offset :offset.z , + fix_type :fix_type }; WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 0265f04cc1..a47984dcf9 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1236,6 +1236,7 @@ struct PACKED log_rtk_mark_pos float n_offset; float e_offset; float d_offset; + uint8_t fix_type; }; @@ -1438,7 +1439,7 @@ struct PACKED log_rtk_mark_pos { LOG_CAM_FEEDBACK_MSG, sizeof(log_Camera_FeedBack), \ "CAMF","QHHHHHHB","TimeUS,Index,CAM1,CAM2,CAM3,CAM4,CAM5,FLAG", "s-------", "F-------" },\ { LOG_RTK_MARK_POS, sizeof(log_rtk_mark_pos), \ - "MARK","QhhHHIdddffffff","TimeUS,R,P,Y,Wk,ms,Lat,Lon,Hgt,sx,sy,sz,Ns,Es,Ds", "sddh-----------", "F--------------" } + "MARK","QhhHHIdddffffffB","TimeUS,R,P,Y,Wk,ms,Lat,Lon,Hgt,sx,sy,sz,Ns,Es,Ds,Fix", "sddh------------", "F---------------" } // messages for more advanced boards #define LOG_EXTRA_STRUCTURES \ { LOG_IMU2_MSG, sizeof(log_IMU), \