yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

sdlog2:

Browse Source 
added separate replay messages for flow and range
This commit is contained in:
Roman
2016-03-09 22:44:58 +01:00
committed by Lorenz Meier
parent 4965926392
commit 37079dc743
2 changed files with 29 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/sdlog2/sdlog2.c
View File

@@ -1202,6 +1202,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_RPL2_s log_RPL2;
struct log_EST6_s log_INO3;
struct log_RPL3_s log_RPL3;
struct log_RPL4_s log_RPL4;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1485,17 +1486,24 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(RPL2);
}
log_msg.msg_type = LOG_RPL3_MSG;
log_msg.body.log_RPL3.time_rng_usec = buf.replay.rng_timestamp;
log_msg.body.log_RPL3.time_flow_usec = buf.replay.flow_timestamp;
log_msg.body.log_RPL3.range_to_ground = buf.replay.range_to_ground;
log_msg.body.log_RPL3.flow_integral_x = buf.replay.flow_pixel_integral[0];
log_msg.body.log_RPL3.flow_integral_y = buf.replay.flow_pixel_integral[1];
log_msg.body.log_RPL3.gyro_integral_x = buf.replay.flow_gyro_integral[0];
log_msg.body.log_RPL3.gyro_integral_y = buf.replay.flow_gyro_integral[1];
log_msg.body.log_RPL3.flow_time_integral = buf.replay.flow_time_integral;
log_msg.body.log_RPL3.flow_quality = buf.replay.flow_quality;
LOGBUFFER_WRITE_AND_COUNT(RPL3);
if (buf.replay.flow_timestamp > 0) {
log_msg.msg_type = LOG_RPL3_MSG;
log_msg.body.log_RPL3.time_flow_usec = buf.replay.flow_timestamp;
log_msg.body.log_RPL3.flow_integral_x = buf.replay.flow_pixel_integral[0];
log_msg.body.log_RPL3.flow_integral_y = buf.replay.flow_pixel_integral[1];
log_msg.body.log_RPL3.gyro_integral_x = buf.replay.flow_gyro_integral[0];
log_msg.body.log_RPL3.gyro_integral_y = buf.replay.flow_gyro_integral[1];
log_msg.body.log_RPL3.flow_time_integral = buf.replay.flow_time_integral;
log_msg.body.log_RPL3.flow_quality = buf.replay.flow_quality;
LOGBUFFER_WRITE_AND_COUNT(RPL3);
}
if (buf.replay.rng_timestamp > 0) {
log_msg.msg_type = LOG_RPL4_MSG;
log_msg.body.log_RPL4.time_rng_usec = buf.replay.rng_timestamp;
log_msg.body.log_RPL4.range_to_ground = buf.replay.range_to_ground;
LOGBUFFER_WRITE_AND_COUNT(RPL4);
}
}
/* --- ATTITUDE --- */

13
src/modules/sdlog2/sdlog2_messages.h
View File

@@ -556,9 +556,7 @@ struct log_RPL2_s {
/* --- EKF2 REPLAY Part 3 --- */
#define LOG_RPL3_MSG 54
struct log_RPL3_s {
uint64_t time_rng_usec;
uint64_t time_flow_usec;
float range_to_ground;
float flow_integral_x;
float flow_integral_y;
float gyro_integral_x;
@@ -567,6 +565,14 @@ struct log_RPL3_s {
uint8_t flow_quality;
};
/* --- EKF2 REPLAY Part 4 --- */
#define LOG_RPL4_MSG 56
struct log_RPL4_s {
uint64_t time_rng_usec;
float range_to_ground;
};
/* --- CAMERA TRIGGER --- */
#define LOG_CAMT_MSG 55
struct log_CAMT_s {
@@ -656,7 +662,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(CAMT, "QI", "timestamp,seq"),
LOG_FORMAT(RPL1, "QQQQQffffffffff", "t,gIdt,aIdt,Tm,Tb,gIx,gIy,gIz,aIx,aIy,aIz,magX,magY,magZ,b_alt"),
LOG_FORMAT(RPL2, "QQLLLMffffffM", "Tpos,Tvel,lat,lon,alt,fix_type,eph,epv,v,vN,vE,vD,v_val"),
LOG_FORMAT(RPL3, "QQfffffIB", "Trng,Tflow,rng,fx,fy,gx,gy,delT,qual"),
LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */
LOG_FORMAT(TIME, "Q", "StartTime"),