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

sdlog2: Add external vision data to ekf2 replay

Browse Source 
Put struct definitions in enum order
Fix duplicate enum value
Add ekf2 replay message for external vision data
This commit is contained in:
Paul Riseborough
2016-05-28 14:54:14 +02:00
committed by Lorenz Meier
parent fc4d5ddb67
commit 2e127a4737
2 changed files with 68 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+17 -1
View File
@@ -1251,8 +1251,9 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_EST6_s log_INO3;
struct log_RPL3_s log_RPL3;
struct log_RPL4_s log_RPL4;
struct log_RPL6_s log_RPL6;
struct log_RPL5_s log_RPL5;
struct log_LAND_s log_LAND;
struct log_RPL6_s log_RPL6;
struct log_LOAD_s log_LOAD;
} body;
} log_msg = {
@@ -1605,6 +1606,21 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_RPL6.confidence = buf.replay.confidence;
LOGBUFFER_WRITE_AND_COUNT(RPL6);
}
if (buf.replay.ev_timestamp > 0) {
log_msg.msg_type = LOG_RPL5_MSG;
log_msg.body.log_RPL5.time_ev_usec = buf.replay.ev_timestamp;
log_msg.body.log_RPL5.x = buf.replay.pos_ev[0];
log_msg.body.log_RPL5.y = buf.replay.pos_ev[1];
log_msg.body.log_RPL5.z = buf.replay.pos_ev[2];
log_msg.body.log_RPL5.q0 = buf.replay.quat_ev[0];
log_msg.body.log_RPL5.q1 = buf.replay.quat_ev[1];
log_msg.body.log_RPL5.q2 = buf.replay.quat_ev[2];
log_msg.body.log_RPL5.q3 = buf.replay.quat_ev[3];
log_msg.body.log_RPL5.pos_err = buf.replay.pos_err;
log_msg.body.log_RPL5.ang_err = buf.replay.ang_err;
LOGBUFFER_WRITE_AND_COUNT(RPL5);
}
}
}
src/modules/sdlog2/sdlog2_messages.h
+51 -33
View File
@@ -420,24 +420,6 @@ struct log_EST3_s {
float cov[16];
};
/* --- EST4 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST4_MSG 48
struct log_EST4_s {
float s[12];
};
/* --- EST5 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST5_MSG 49
struct log_EST5_s {
float s[10];
};
/* --- EST6 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST6_MSG 53
struct log_EST6_s {
float s[6];
};
/* --- TEL0..3 - TELEMETRY STATUS --- */
#define LOG_TEL0_MSG 36
#define LOG_TEL1_MSG 37
@@ -518,6 +500,18 @@ struct log_CTS_s {
float yaw_rate;
};
/* --- EST4 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST4_MSG 48
struct log_EST4_s {
float s[12];
};
/* --- EST5 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST5_MSG 49
struct log_EST5_s {
float s[10];
};
#define LOG_OUT1_MSG 50
/* --- EKF2 REPLAY Part 1 --- */
@@ -558,6 +552,13 @@ struct log_RPL2_s {
float vel_d_m_s;
bool vel_ned_valid;
};
/* --- EST6 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST6_MSG 53
struct log_EST6_s {
float s[6];
};
/* --- EKF2 REPLAY Part 3 --- */
#define LOG_RPL3_MSG 54
struct log_RPL3_s {
@@ -570,6 +571,13 @@ struct log_RPL3_s {
uint8_t flow_quality;
};
/* --- CAMERA TRIGGER --- */
#define LOG_CAMT_MSG 55
struct log_CAMT_s {
uint64_t timestamp;
uint32_t seq;
};
/* --- EKF2 REPLAY Part 4 --- */
#define LOG_RPL4_MSG 56
struct log_RPL4_s {
@@ -577,7 +585,16 @@ struct log_RPL4_s {
float range_to_ground;
};
/* --- EKF2 REPLAY Part 4 --- */
/* --- LAND DETECTOR --- */
#define LOG_LAND_MSG 57
struct log_LAND_s {
uint8_t landed;
};
/* 58 used for DGPS message
shares struct with GPS MSG 8*/
/* --- EKF2 REPLAY Part 6 --- */
#define LOG_RPL6_MSG 59
struct log_RPL6_s {
uint64_t time_airs_usec;
@@ -588,23 +605,23 @@ struct log_RPL6_s {
float confidence;
};
/* --- CAMERA TRIGGER --- */
#define LOG_CAMT_MSG 55
struct log_CAMT_s {
uint64_t timestamp;
uint32_t seq;
};
/* --- LAND DETECTOR --- */
#define LOG_LAND_MSG 57
struct log_LAND_s {
uint8_t landed;
/* --- EKF2 REPLAY Part 5 --- */
#define LOG_RPL5_MSG 60
struct log_RPL5_s {
uint64_t time_ev_usec;
float x;
float y;
float z;
float q0;
float q1;
float q2;
float q3;
float pos_err;
float ang_err;
};
/* --- SYSTEM LOAD --- */
#define LOG_LOAD_MSG 58
#define LOG_LOAD_MSG 61
struct log_LOAD_s {
float cpu_load;
};
@@ -694,6 +711,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"),
LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
LOG_FORMAT(RPL5, "Qfffffffff", "Tev,x,y,z,q0,q1,q2,q3,posErr,angErr"),
LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
LOG_FORMAT(LAND, "B", "Landed"),
LOG_FORMAT(LOAD, "f", "CPU"),