From 80927d79fe2dc0f628a8d4d55ce3e671fece4a8c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 9 Mar 2016 17:33:06 +1100 Subject: [PATCH] sdlog2: Add replay logging of flow and range finder messages for ekf2 --- src/modules/sdlog2/sdlog2.c | 12 ++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 ++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2e4541aa0c..3e19d88c4a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1201,6 +1201,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_RPL1_s log_RPL1; struct log_RPL2_s log_RPL2; struct log_EST6_s log_INO3; + struct log_RPL3_s log_RPL3; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1479,6 +1480,17 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_RPL2.vel_d_m_s = buf.replay.vel_d_m_s; log_msg.body.log_RPL2.vel_ned_valid = buf.replay.vel_ned_valid; 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); } /* --- ATTITUDE --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index bb87e7fcc0..3c9ef715c5 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -553,6 +553,19 @@ struct log_RPL2_s { float vel_d_m_s; bool vel_ned_valid; }; +/* --- 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; + float gyro_integral_y; + uint32_t flow_time_integral; + uint8_t flow_quality; +}; /* --- CAMERA TRIGGER --- */ #define LOG_CAMT_MSG 55 @@ -643,6 +656,7 @@ 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"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"),