From bccc37cfb9175cf38f2769c1654f5b7ec3a88618 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 4 Jan 2015 12:31:21 +0100 Subject: send MANUAL_CONTROL stream over usb --- ROMFS/px4fmu_common/init.d/rc.usb | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index b3de8e590..2dfe11ce6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -26,6 +26,8 @@ usleep 100000 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 usleep 100000 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 +usleep 100000 +mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 # Exit shell to make it available to MAVLink exit -- cgit v1.2.3 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(-) 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(-) 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(-) 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 1cc92c03612c504414092bc6742bb17c812ce9c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:24:43 +0100 Subject: GPS driver: Require valid minimum time to allow setting the wall clock. Protection against nulled time fields --- src/drivers/gps/ashtech.cpp | 29 +++++++++++++---------- src/drivers/gps/gps_helper.h | 2 ++ src/drivers/gps/ubx.cpp | 56 ++++++++++++++++++++++++++------------------ 3 files changed, 52 insertions(+), 35 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 99dbe5984..a50767c22 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -99,21 +99,26 @@ int ASHTECH::handle_message(int len) timeinfo.tm_sec = int(ashtech_sec); time_t epoch = mktime(&timeinfo); - uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1e6; - - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + if (epoch > GPS_EPOCH_SECS) { + uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1e6; + + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = usecs * 1000; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += usecs; + } else { + _gps_position->time_utc_usec = 0; } - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += usecs; _gps_position->timestamp_time = hrt_absolute_time(); } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 3623397b2..0ce05fcb5 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -43,6 +43,8 @@ #include #include +#define GPS_EPOCH_SECS 1234567890ULL + class GPS_Helper { public: diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 05809b361..e197059db 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -748,19 +748,23 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; time_t epoch = mktime(&timeinfo); - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; + } } _gps_position->timestamp_time = hrt_absolute_time(); @@ -820,19 +824,25 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + // only set the time if it makes sense - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; + } } _gps_position->timestamp_time = hrt_absolute_time(); -- 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(-) 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(-) 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 From 552ff809693d340ba6f5fed6837b99effe8bf2c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 18:53:34 +0100 Subject: Fix NSH timeout logic --- src/systemcmds/nshterm/nshterm.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index edeb5c624..c2ea2a1cc 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -66,20 +66,18 @@ nshterm_main(int argc, char *argv[]) int fd = -1; int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; - /* we assume the system does not provide arming status feedback */ - bool armed_updated = false; - /* try the first 30 seconds or if arming system is ready */ - while ((retries < 300) || armed_updated) { + /* try to bring up the console - stop doing so if the system gets armed */ + while (true) { /* abort if an arming topic is published and system is armed */ bool updated = false; - if (orb_check(armed_fd, &updated)) { + orb_check(armed_fd, &updated) + if (updated) { /* the system is now providing arming status feedback. * instead of timing out, we resort to abort bringing * up the terminal. */ - armed_updated = true; orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); if (armed.armed) { -- cgit v1.2.3