diff options
| author | Mohammed Kabir <mhkabir98@gmail.com> | 2015-04-14 12:53:05 +0530 |
|---|---|---|
| committer | Mohammed Kabir <mhkabir98@gmail.com> | 2015-04-14 13:14:15 +0530 |
| commit | 2f164ca34508aee54dc3f01ca389c84ddf282dee (patch) | |
| tree | fbe89fd59816b744d0cc9fdd3800f814b52062c6 /src | |
| parent | 66e6938c6d979b1a955af7dbb3abb4d420d7c241 (diff) | |
| download | px4-firmware-2f164ca34508aee54dc3f01ca389c84ddf282dee.tar.gz px4-firmware-2f164ca34508aee54dc3f01ca389c84ddf282dee.tar.bz2 px4-firmware-2f164ca34508aee54dc3f01ca389c84ddf282dee.zip | |
sdlog2 : field for timesync offset
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/sdlog2/sdlog2.c | 12 | ||||
| -rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 7 |
2 files changed, 19 insertions, 0 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0bfd356a1..2ec733627 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -99,6 +99,7 @@ #include <uORB/topics/wind_estimate.h> #include <uORB/topics/encoders.h> #include <uORB/topics/vtol_vehicle_status.h> +#include <uORB/topics/time_offset.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -1027,6 +1028,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct wind_estimate_s wind_estimate; struct encoders_s encoders; struct vtol_vehicle_status_s vtol_status; + struct time_offset_s time_offset; } buf; memset(&buf, 0, sizeof(buf)); @@ -1071,6 +1073,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; struct log_ENCD_s log_ENCD; + struct log_TSYN_s log_TSYN; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1111,6 +1114,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; + int tsync_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1142,6 +1146,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + subs.tsync_sub = orb_subscribe(ORB_ID(time_offset)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); @@ -1821,6 +1826,13 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ENCD); } + /* --- TIMESYNC OFFSET --- */ + if (copy_if_updated(ORB_ID(time_offset), subs.tsync_sub, &buf.time_offset)) { + log_msg.msg_type = LOG_TSYN_MSG; + log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns; + LOGBUFFER_WRITE_AND_COUNT(TSYN); + } + /* 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 a1fe2c95d..598f12a53 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -449,6 +449,12 @@ struct log_VTOL_s { float airspeed_tot; }; +/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ +#define LOG_TSYN_MSG 43 +struct log_TSYN_s { + uint64_t time_offset; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -517,6 +523,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), + LOG_FORMAT(TSYN, "Q", "TimeOffset"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ |