diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d7286e478f..005a133c68 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -110,6 +110,7 @@ #include #include #include +#include #include #include @@ -1112,6 +1113,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct mc_att_ctrl_status_s mc_att_ctrl_status; struct control_state_s ctrl_state; struct ekf2_innovations_s innovations; + struct camera_trigger_s camera_trigger; } buf; memset(&buf, 0, sizeof(buf)); @@ -1163,6 +1165,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_CTS_s log_CTS; struct log_EST4_s log_INO1; struct log_EST5_s log_INO2; + struct log_CAMT_s log_CAMT; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1208,6 +1211,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int mc_att_ctrl_status_sub; int ctrl_state_sub; int innov_sub; + int cam_trig_sub; } subs; subs.cmd_sub = -1; @@ -1245,6 +1249,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.ctrl_state_sub = -1; subs.encoders_sub = -1; subs.innov_sub = -1; + subs.cam_trig_sub = -1; /* add new topics HERE */ @@ -1935,6 +1940,14 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(CTS); } + /* --- CAMERA TRIGGER --- */ + if (copy_if_updated(ORB_ID(camera_trigger), &subs.cam_trig_sub, &buf.camera_trigger)) { + log_msg.msg_type = LOG_CAMT_MSG; + log_msg.body.log_CAMT.timestamp = buf.camera_trigger.timestamp; + log_msg.body.log_CAMT.seq = buf.camera_trigger.seq; + LOGBUFFER_WRITE_AND_COUNT(CAMT); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2e222ec6af..576b9b7870 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -508,6 +508,13 @@ struct log_CTS_s { float yaw_rate; }; +/* --- CAMERA TRIGGER --- */ +#define LOG_CAMT_MSG 48 +struct log_CAMT_s { + uint64_t timestamp; + uint32_t seq; +}; + #define LOG_OUT1_MSG 50 /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -585,6 +592,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"), + LOG_FORMAT(CAMT, "if", "timestamp,seq"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 0286705968..a38b1e7cb3 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -99,6 +99,7 @@ #include #include #include +#include #endif #include #include