From 37079dc7437087def1718e2d0d1fb27a7017ed7f Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 9 Mar 2016 22:44:58 +0100 Subject: [PATCH] sdlog2: added separate replay messages for flow and range --- src/modules/sdlog2/sdlog2.c | 30 ++++++++++++++++++---------- src/modules/sdlog2/sdlog2_messages.h | 13 +++++++++--- 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a354de18c6..6d994051ef 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 3c9ef715c5..8393632e45 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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"),