From 94bbd261488e07b178ac2a90786fbad214828d65 Mon Sep 17 00:00:00 2001 From: nagezeng Date: Tue, 7 Jun 2022 11:27:40 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=A4=A9=E7=BA=BF=E5=81=8F?= =?UTF-8?q?=E7=A7=BB=E8=AE=A1=E7=AE=97=EF=BC=8C=E5=86=99=E5=85=A5=E6=97=A5?= =?UTF-8?q?=E5=BF=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/version.h | 6 +++--- all.sh | 23 ++++++++++++++--------- hrs100h.sh | 2 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 27 ++++++--------------------- libraries/AP_Logger/AP_Logger.h | 2 +- libraries/AP_Logger/LogFile.cpp | 10 ++++------ libraries/AP_Logger/LogStructure.h | 10 ++++------ 7 files changed, 33 insertions(+), 47 deletions(-) diff --git a/ArduCopter/version.h b/ArduCopter/version.h index ed51470e24..c68603b841 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,14 +6,14 @@ #include "ap_version.h" #define GIT_VERSION "1" -#define THISFIRMWARE "ZRUAV v4.1.17" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.1.18" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,1,17,FIRMWARE_VERSION_TYPE_OFFICIAL +#define FIRMWARE_VERSION 4,1,18,FIRMWARE_VERSION_TYPE_OFFICIAL #define FW_MAJOR 4 #define FW_MINOR 1 -#define FW_PATCH 17 +#define FW_PATCH 18 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL /** * AVDv6: avoid.get_zr_mode() = 3时可以用Loiter模式测试自动避障 diff --git a/all.sh b/all.sh index 9fbecedb53..0c6743417c 100755 --- a/all.sh +++ b/all.sh @@ -4,32 +4,37 @@ starttime=`date +'%Y-%m-%d %H:%M:%S'` ./waf configure --board rs100 > build_log.txt -# ./waf clean >> build_log.txt +./waf clean >> build_log.txt ./waf copter >> build_log.txt -cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/致睿rs100-v4.0.17-RC8.px4 +cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/致睿rs100-v4.1.18.px4 echo "finish build rs100 " ./waf configure --board rs100h >> build_log.txt -# ./waf clean >> build_log.txt +./waf clean >> build_log.txt ./waf copter >> build_log.txt -cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/致睿rs100h-v4.0.17-RC8.px4 +cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/致睿rs100h-v4.1.18.px4 echo "finish build rs100h " ./waf configure --board d100 >> build_log.txt -# ./waf clean >> build_log.txt +./waf clean >> build_log.txt ./waf copter >> build_log.txt -cp ./build/d100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/中海达d100-v4.0.17-RC8.px4 +cp ./build/d100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/中海达d100-v4.1.18.px4 echo "finish build d100 " ./waf configure --board d100h >> build_log.txt -# ./waf clean >> build_log.txt +./waf clean >> build_log.txt ./waf copter >> build_log.txt -cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/中海达d100h-v4.0.17-RC8.px4 +cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/中海达d100h-v4.1.18.px4 echo "finish build d100h " +./waf configure --board E300 >> build_log.txt +./waf clean >> build_log.txt +./waf copter >> build_log.txt +cp ./build/E300/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/Sino-E300-v4.1.18.px4 +echo "finish build E300 " # ./waf configure --board zr-hexa >> build_log.txt # ./waf clean >> build_log.txt # ./waf copter >> build_log.txt -# cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/六轴m66-v4.0.17-RC8.px4 +# cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/六轴m66-v4.1.18.px4 # echo "finish build zr-hexa " endtime=`date +'%Y-%m-%d %H:%M:%S'` diff --git a/hrs100h.sh b/hrs100h.sh index ae630757db..5031ffdf5e 100755 --- a/hrs100h.sh +++ b/hrs100h.sh @@ -1,3 +1,3 @@ ./waf configure --board rs100h ./waf copter -cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.0.16.px4 +cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.1.18.px4 diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 64ab5c48dc..84a1011e19 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -296,14 +296,6 @@ bool AP_GPS_NMEA::_term_complete() #if HAL_WITH_UAVCAN uint8_t can_num_drivers = AP::can().get_num_drivers(); - uint32_t tk1 = AP_HAL::micros(); - Vector3f cam_pos_ned = camera_pos_rotation(); - uint32_t tk2 = AP_HAL::micros(); - - // 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,"ant:%.2f,%.2f,%.2f,euler:%.2f,%.2f,%.2f",_cam_offset.x,_cam_offset.y,_cam_offset.z,euler_deg.x,euler_deg.y,euler_deg.z); - - gcs().send_text(MAV_SEVERITY_INFO,"rot:%dus,%.3f,%.3f,%.3f",tk2 - tk1,cam_pos_ned.x,cam_pos_ned.y,cam_pos_ned.z); for (uint8_t i = 0; i < can_num_drivers; i++) { @@ -330,16 +322,19 @@ bool AP_GPS_NMEA::_term_complete() if (logger != nullptr) { + // uint32_t tk1 = AP_HAL::micros(); + Vector3f cam_pos_ned = camera_pos_rotation(); + // uint32_t tk2 = AP_HAL::micros(); Vector3d llh = Vector3d(gps_mark.lat, gps_mark.lon, gps_mark.hgt); Vector3f sigma = Vector3f(gps_mark.lat_sigma, gps_mark.lon_sigma, gps_mark.hgt_sigma); AP::logger().Write_RTK_Mark_Pos( gps_mark.week, gps_mark.second, llh, - gps_mark.undulation, sigma, - gps_mark.diff_age, - gps_mark.sol_age); + cam_pos_ned); + // 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); } } } @@ -719,19 +714,9 @@ Vector3f AP_GPS_NMEA::camera_pos_rotation() return cam_pos_ned; } const AP_AHRS &ahrs = AP::ahrs(); - Tbn = ahrs.get_rotation_body_to_ned(); cam_pos_ned = Tbn * (_cam_offset); - // Vector3f euler,euler_deg; - if(ahrs.get_secondary_attitude(euler)){ - euler_deg.x = degrees(euler.x); - euler_deg.y = degrees(euler.y); - euler_deg.z = wrap_360(degrees(euler.z)); - } - // Vector3f cam_pos_delt = Tbn.mul_transpose(_cam_offset); - // gcs().send_text(MAV_SEVERITY_INFO,"org:%.2f,%.2f,%.2f,euler:%.2f,%.2f,%.2f",_cam_offset.x,_cam_offset.y,_cam_offset.z,euler_deg.x,euler_deg.y,euler_deg.z); return cam_pos_ned; - // gcs().send_text(MAV_SEVERITY_INFO,"del:%.2f,%.2f,%.2f",cam_pos_delt.x,cam_pos_delt.y,cam_pos_delt.z); } // Calculate and return the CRC for usA binary buffer diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 8aa35a9d2d..c13ea38e7e 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,float undulation,const Vector3f& sigma,float diff_age,float sol_age); + void Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,const Vector3f& sigma,const Vector3f& offset); 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 56d4d1ead4..9d157fa840 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,float undulation,const Vector3f& sigma,float diff_age,float sol_age) +void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d& llh,const Vector3f& sigma,const Vector3f& offset) { const AP_AHRS &ahrs = AP::ahrs(); @@ -1045,14 +1045,12 @@ void AP_Logger::Write_RTK_Mark_Pos(uint16_t week, uint32_t second,const Vector3d lat :llh.x, lon :llh.y , hgt :llh.z , - undulation :undulation , lat_sigma :sigma.x , lon_sigma :sigma.y , hgt_sigma :sigma.z , - diff_age :diff_age , - sol_age :sol_age , - SVs :0 , - solnSVs : 0 + n_offset :offset.x , + e_offset :offset.y , + d_offset :offset.z }; WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 84bd5e34a7..0265f04cc1 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1230,14 +1230,12 @@ struct PACKED log_rtk_mark_pos double lat; double lon; double hgt; - float undulation; float lat_sigma; float lon_sigma; float hgt_sigma; - float diff_age; - float sol_age; - uint8_t SVs; - uint8_t solnSVs; + float n_offset; + float e_offset; + float d_offset; }; @@ -1440,7 +1438,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","QhhHHIdddffffffBB","TimeUS,R,P,Y,Week,Secms,Lat,Lon,Hgt,Undu,Lat_s,Lon_s,Hgt_s,D_age,S_age,SVs,SoSVs", "sddh-------------", "F----------------" } + "MARK","QhhHHIdddffffff","TimeUS,R,P,Y,Wk,ms,Lat,Lon,Hgt,sx,sy,sz,Ns,Es,Ds", "sddh-----------", "F--------------" } // messages for more advanced boards #define LOG_EXTRA_STRUCTURES \ { LOG_IMU2_MSG, sizeof(log_IMU), \