From 3616c83f88078ecf827fb844ffdbacdd442f9bad Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 17:38:19 +0530 Subject: missing zero --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 93a297285..9b3274431 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -983,7 +983,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; - if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew + if (dt > 10000000 || dt < -10000000) { // 10 millisecond skew _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; warnx("[timesync] Resetting."); -- cgit v1.2.3 From 1f181fb03f5eb467d5e1f0933164363269986844 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:14:41 +0100 Subject: SDLOG2: Use RTC, not GPS time for file naming --- src/modules/sdlog2/sdlog2.c | 48 ++++++++++++++++++++++++++++----------------- 1 file changed, 30 insertions(+), 18 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2ce3d0097..10b6c903f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -103,6 +103,8 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" +#define PX4_EPOCH_SECS 1234567890ULL + /** * Logging rate. * @@ -338,13 +340,16 @@ int create_log_dir() uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - if (log_name_timestamp && gps_time != 0) { - /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); - strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == OK) { @@ -395,12 +400,16 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - if (log_name_timestamp && gps_time != 0) { - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); - strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + /* start logging if we have a valid time and the time is not in the past */ + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { @@ -446,12 +455,15 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - if (log_name_timestamp && gps_time != 0) { - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); - strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); } else { -- cgit v1.2.3 From 0d103dcb383f29d77753ee1e7ab49cc7a0f76fb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:24:02 +0100 Subject: sdlog2: Use correct struct --- src/modules/sdlog2/sdlog2.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 10b6c903f..c3952b77d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -340,7 +341,7 @@ int create_log_dir() uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); @@ -400,7 +401,7 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); @@ -455,7 +456,7 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); -- cgit v1.2.3 From c47e8c22688ad3ac358c71337ef8cb0feeeba409 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 21:29:05 +0530 Subject: Add suffixes, constant --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++++------ src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 12 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9b3274431..d84b70e46 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -947,14 +947,18 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) clock_gettime(CLOCK_REALTIME, &tv); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = tv.tv_sec > 1234567890ULL; - bool ofb_unix_valid = time.time_unix_usec > 1234567890ULL * 1000ULL; + bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS; + bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; if (!onb_unix_valid && ofb_unix_valid) { tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; - clock_settime(CLOCK_REALTIME, &tv); - warnx("[timesync] synced.."); + if(clock_settime(CLOCK_REALTIME, &tv)) { + warn("failed setting clock"); + } + else { + warnx("[timesync] UTC time synced."); + } } } @@ -983,9 +987,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; - if (dt > 10000000 || dt < -10000000) { // 10 millisecond skew + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Resetting."); + warnx("[timesync] Resetting offset sync."); } else { smooth_time_offset(offset_ns); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a677e75cd..4afc9b372 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -75,6 +75,8 @@ #include "mavlink_ftp.h" +#define PX4_EPOCH_SECS 1234567890ULL + class Mavlink; class MavlinkReceiver -- cgit v1.2.3 From 3df73cde3e3bd092cdf28990c0736001681a9289 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 22:38:47 +0530 Subject: Fix offset calculation. --- src/modules/mavlink/mavlink_receiver.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d84b70e46..97108270c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -969,7 +969,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); - uint64_t now_ns = hrt_absolute_time() * 1000 ; + uint64_t now_ns = hrt_absolute_time() * 1000LL ; if (tsync.tc1 == 0) { @@ -984,13 +984,12 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; int64_t dt = _time_offset - offset_ns; if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew - _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Resetting offset sync."); - + _time_offset = offset_ns; + warnx("[timesync] Hard setting offset."); } else { smooth_time_offset(offset_ns); } @@ -1473,7 +1472,7 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) { - /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, + /* alpha = 0.6 fixed for now. The closer alpha is to 1.0, * the faster the moving average updates in response to * new offset samples. */ -- cgit v1.2.3