zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

增加天线偏移计算，写入日志

zr-v4.1.17
nagezeng 3 years ago
parent
ea0210ee76
commit
94bbd26148
7 changed files with 33 additions and 47 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      ArduCopter/version.h
  2. 23
      all.sh
  3. 2
      hrs100h.sh
  4. 27
      libraries/AP_GPS/AP_GPS_NMEA.cpp
  5. 2
      libraries/AP_Logger/AP_Logger.h
  6. 10
      libraries/AP_Logger/LogFile.cpp
  7. 10
      libraries/AP_Logger/LogStructure.h

6
ArduCopter/version.h
Unescape View File

@ -6,14 +6,14 @@ @@ -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模式测试自动避障

23
all.sh
Unescape View File

@ -4,32 +4,37 @@ starttime=`date +'%Y-%m-%d %H:%M:%S'` @@ -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'`

2
hrs100h.sh
Unescape View File

@ -1,3 +1,3 @@ @@ -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

27
libraries/AP_GPS/AP_GPS_NMEA.cpp
Unescape View File

@ -296,14 +296,6 @@ bool AP_GPS_NMEA::_term_complete() @@ -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() @@ -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() @@ -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

2
libraries/AP_Logger/AP_Logger.h
Unescape View File

@ -288,7 +288,7 @@ public: @@ -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, ...);

10
libraries/AP_Logger/LogFile.cpp
Unescape View File

@ -1029,7 +1029,7 @@ void AP_Logger::Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2 @@ -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 @@ -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));
}

10
libraries/AP_Logger/LogStructure.h
Unescape View File

@ -1230,14 +1230,12 @@ struct PACKED log_rtk_mark_pos @@ -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 @@ -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), \

Write Preview
Loading…
Cancel
Save