diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 328cf6c574..b52c3f982b 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -216,6 +216,10 @@ #define HAL_WITH_IO_MCU 0 #endif +#ifndef HAL_HAVE_GETTIME_SETTIME +#define HAL_HAVE_GETTIME_SETTIME 0 +#endif + // this is used as a general mechanism to make a 'small' build by // dropping little used features. We use this to allow us to keep // FMUv2 going for as long as possible diff --git a/libraries/AP_HAL/Util.cpp b/libraries/AP_HAL/Util.cpp index 628656475d..4b67df77d5 100644 --- a/libraries/AP_HAL/Util.cpp +++ b/libraries/AP_HAL/Util.cpp @@ -60,96 +60,31 @@ int AP_HAL::Util::vsnprintf(char* str, size_t size, const char *format, va_list return ret; } -uint64_t AP_HAL::Util::get_system_clock_ms() const +uint64_t AP_HAL::Util::get_hw_rtc() const { #if defined(__APPLE__) && defined(__MACH__) struct timeval ts; gettimeofday(&ts, nullptr); - return ((long long)((ts.tv_sec * 1000) + (ts.tv_usec / 1000))); -#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - return ST2MS(chVTGetSystemTime()); -#else + return ((long long)((ts.tv_sec * 1000000) + ts.tv_usec)); +#elif HAL_HAVE_GETTIME_SETTIME struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); const uint64_t seconds = ts.tv_sec; const uint64_t nanoseconds = ts.tv_nsec; - return (seconds * 1000ULL + nanoseconds/1000000ULL); + return (seconds * 1000000ULL + nanoseconds/1000ULL); +#endif + + // no HW clock (or not one worth bothering with) + return 0; +} + +void AP_HAL::Util::set_hw_rtc(uint64_t time_utc_usec) +{ +#if HAL_HAVE_GETTIME_SETTIME + timespec ts; + ts.tv_sec = time_utc_usec/1000000ULL; + ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000ULL; + clock_settime(CLOCK_REALTIME, &ts); #endif } -void AP_HAL::Util::get_system_clock_utc(int32_t &hour, int32_t &min, int32_t &sec, int32_t &ms) const -{ - // get time of day in ms - uint64_t time_ms = get_system_clock_ms(); - - // separate time into ms, sec, min, hour and days but all expressed in milliseconds - ms = time_ms % 1000; - uint32_t sec_ms = (time_ms % (60 * 1000)) - ms; - uint32_t min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms; - uint32_t hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms; - - // convert times as milliseconds into appropriate units - sec = sec_ms / 1000; - min = min_ms / (60 * 1000); - hour = hour_ms / (60 * 60 * 1000); -} - -// get milliseconds from now to a target time of day expressed as hour, min, sec, ms -// match starts from first value that is not -1. I.e. specifying hour=-1, minutes=10 will ignore the hour and return time until 10 minutes past 12am (utc) -uint32_t AP_HAL::Util::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms) const -{ - // determine highest value specified (0=none, 1=ms, 2=sec, 3=min, 4=hour) - int8_t largest_element = 0; - if (hour != -1) { - largest_element = 4; - } else if (min != -1) { - largest_element = 3; - } else if (sec != -1) { - largest_element = 2; - } else if (ms != -1) { - largest_element = 1; - } else { - // exit immediately if no time specified - return 0; - } - - // get start_time_ms as h, m, s, ms - int32_t curr_hour, curr_min, curr_sec, curr_ms; - get_system_clock_utc(curr_hour, curr_min, curr_sec, curr_ms); - int32_t total_delay_ms = 0; - - // calculate ms to target - if (largest_element >= 1) { - total_delay_ms += ms - curr_ms; - } - if (largest_element == 1 && total_delay_ms < 0) { - return static_cast(total_delay_ms += 1000); - } - - // calculate sec to target - if (largest_element >= 2) { - total_delay_ms += (sec - curr_sec)*1000; - } - if (largest_element == 2 && total_delay_ms < 0) { - return static_cast(total_delay_ms += (60*1000)); - } - - // calculate min to target - if (largest_element >= 3) { - total_delay_ms += (min - curr_min)*60*1000; - } - if (largest_element == 3 && total_delay_ms < 0) { - return static_cast(total_delay_ms += (60*60*1000)); - } - - // calculate hours to target - if (largest_element >= 4) { - total_delay_ms += (hour - curr_hour)*60*60*1000; - } - if (largest_element == 4 && total_delay_ms < 0) { - return static_cast(total_delay_ms += (24*60*60*1000)); - } - - // total delay in milliseconds - return static_cast(total_delay_ms); -} diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h index fb5e28f83a..d76b409dbf 100644 --- a/libraries/AP_HAL/Util.h +++ b/libraries/AP_HAL/Util.h @@ -42,21 +42,14 @@ public: virtual enum safety_state safety_switch_state(void) { return SAFETY_NONE; } /* - set system clock in UTC microseconds + set HW RTC in UTC microseconds */ - virtual void set_system_clock(uint64_t time_utc_usec) {} + virtual void set_hw_rtc(uint64_t time_utc_usec); /* - get system clock in UTC milliseconds + get system clock in UTC microseconds */ - uint64_t get_system_clock_ms() const; - - /* - get system time in UTC hours, minutes, seconds and milliseconds - */ - void get_system_clock_utc(int32_t &hour, int32_t &min, int32_t &sec, int32_t &ms) const; - - uint32_t get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms) const; + virtual uint64_t get_hw_rtc() const; /* get system identifier (eg. serial number) @@ -124,4 +117,5 @@ protected: // values until the vehicle code has fully started bool soft_armed = false; uint64_t capabilities = 0; + }; diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index 7a34955d1b..e336b4d2b0 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -364,6 +364,8 @@ #define HAL_COMPASS_LIS3MDL_NAME "lis3mdl" #define HAL_OPTFLOW_PX4FLOW_I2C_BUS 0 +#define HAL_HAVE_GETTIME_SETTIME 1 + #else #error "no Linux board subtype set" #endif diff --git a/libraries/AP_HAL/board/px4.h b/libraries/AP_HAL/board/px4.h index 7583d92bcd..ab27689d45 100644 --- a/libraries/AP_HAL/board/px4.h +++ b/libraries/AP_HAL/board/px4.h @@ -10,6 +10,8 @@ #define HAL_BARO_DEFAULT HAL_BARO_PX4 #define HAL_COMPASS_DEFAULT HAL_COMPASS_PX4 +#define HAL_HAVE_GETTIME_SETTIME 1 + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_PX4_V1 #define HAL_STORAGE_SIZE 8192 diff --git a/libraries/AP_HAL/board/vrbrain.h b/libraries/AP_HAL/board/vrbrain.h index de8715b6e7..36d677daaa 100644 --- a/libraries/AP_HAL/board/vrbrain.h +++ b/libraries/AP_HAL/board/vrbrain.h @@ -10,6 +10,8 @@ #define HAL_BARO_DEFAULT HAL_BARO_VRBRAIN #define HAL_COMPASS_DEFAULT HAL_COMPASS_VRBRAIN +#define HAL_HAVE_GETTIME_SETTIME 1 + #ifdef CONFIG_ARCH_BOARD_VRBRAIN_V45 #define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_VRBRAIN_V45 #define HAL_STORAGE_SIZE 8192