fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 12:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

applying dual gps patch

Browse Source 
resolve transfer errors

reformatting

implementing multi-topics
This commit is contained in:
Andreas Bircher
2016-05-22 12:19:47 +02:00
committed by Lorenz Meier
parent 5be1f3a856
commit 556851a511
3 changed files with 151 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+24 -2
View File
@@ -1188,6 +1188,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct ekf2_replay_s replay;
struct vehicle_land_detected_s land_detected;
struct cpuload_s cpuload;
struct vehicle_gps_position_s dual_gps_pos;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -1368,7 +1369,7 @@ int sdlog2_thread_main(int argc, char *argv[])
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos)) {
gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
}
}
@@ -1482,7 +1483,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- GPS POSITION - LOG MANAGEMENT --- */
bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos);
if (gps_pos_updated && log_name_timestamp) {
gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
@@ -1718,6 +1719,27 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
/* --- GPS POSITION - UNIT #2 --- */
if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub, &buf.dual_gps_pos)) {
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec;
log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type;
log_msg.body.log_GPS.eph = buf.dual_gps_pos.eph;
log_msg.body.log_GPS.epv = buf.dual_gps_pos.epv;
log_msg.body.log_GPS.lat = buf.dual_gps_pos.lat;
log_msg.body.log_GPS.lon = buf.dual_gps_pos.lon;
log_msg.body.log_GPS.alt = buf.dual_gps_pos.alt * 0.001f;
log_msg.body.log_GPS.vel_n = buf.dual_gps_pos.vel_n_m_s;
log_msg.body.log_GPS.vel_e = buf.dual_gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf.dual_gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf.dual_gps_pos.cog_rad;
log_msg.body.log_GPS.sats = buf.dual_gps_pos.satellites_used;
log_msg.body.log_GPS.snr_mean = snr_mean;
log_msg.body.log_GPS.noise_per_ms = buf.dual_gps_pos.noise_per_ms;
log_msg.body.log_GPS.jamming_indicator = buf.dual_gps_pos.jamming_indicator;
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
/* --- SATELLITE INFO - UNIT #1 --- */
if (_extended_logging) {
src/modules/sdlog2/sdlog2_messages.h
+2
View File
@@ -146,6 +146,7 @@ struct log_LPSP_s {
/* --- GPS - GPS POSITION --- */
#define LOG_GPS_MSG 8
#define LOG_DGPS_MSG 58
struct log_GPS_s {
uint64_t gps_time;
uint8_t fix_type;
@@ -647,6 +648,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffffffffff", "X,Y,Z,Yaw,VX,VY,VZ,AX,AY,AZ"),
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(DGPS, GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBB", "MainState,NavState,ArmS,Failsafe"),