Merge branch 'master' into sd_full

7 files changed, 103 insertions, 70 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<uint64_t>((ashtech_sec - static_cast<uint64_t>(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<uint64_t>((ashtech_sec - static_cast<uint64_t>(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<uint64_t>(epoch) * 1000000ULL;
+			_gps_position->time_utc_usec += usecs;
+		} else {
+			_gps_position->time_utc_usec = 0;
 		}
 
-		_gps_position->time_utc_usec = static_cast<uint64_t>(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 <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
+#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<uint64_t>(epoch) * 1000000ULL;
-			_gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
+				_gps_position->time_utc_usec = static_cast<uint64_t>(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<uint64_t>(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<uint64_t>(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();
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 93a297285..97108270c 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.");
+		}
 	}
 
 }
@@ -965,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) {
 
@@ -980,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 > 10000000 || dt < -1000000) { // 10 millisecond skew 
-			_time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; 
-			warnx("[timesync] Resetting.");
-
+		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew 
+			_time_offset = offset_ns;
+			warnx("[timesync] Hard setting offset.");
 		} else {
 			smooth_time_offset(offset_ns);
 		}
@@ -1469,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.
 	 */
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
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 22b178310..963149779 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -59,6 +59,7 @@
 #include <unistd.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
+#include <time.h>
 
 #include <drivers/drv_range_finder.h>
 
@@ -105,6 +106,8 @@
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
 
+#define PX4_EPOCH_SECS 1234567890ULL
+
 /**
  * Logging rate.
  *
@@ -350,13 +353,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);
+	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);
+	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) {
@@ -406,12 +412,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);
+	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);
+	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 {
@@ -455,12 +465,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);
+	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);
+	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 {
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) {