fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 01:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into sd_full

Browse Source
This commit is contained in:
Lorenz Meier
2015-01-04 18:58:45 +01:00
parent 8612189c65 d904a3ca82
commit 615825a632
8 changed files with 103 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+14 -11
View File
@@ -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.
*/
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -75,6 +75,8 @@
#include "mavlink_ftp.h"
#define PX4_EPOCH_SECS 1234567890ULL
class Mavlink;
class MavlinkReceiver
src/modules/sdlog2/sdlog2.c
+31 -18
View File
@@ -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 {