AP_HAL: use AP_RTC

Also add a define as to whether set_time / get_time can be used

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

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)));
+    return ((long long)((ts.tv_sec * 1000000) + ts.tv_usec));
-#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#elif HAL_HAVE_GETTIME_SETTIME
-    return ST2MS(chVTGetSystemTime());
-#else
     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<uint32_t>(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<uint32_t>(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<uint32_t>(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<uint32_t>(total_delay_ms += (24*60*60*1000));
-    }
-
-    // total delay in milliseconds
-    return static_cast<uint32_t>(total_delay_ms);
-}

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;
+    virtual uint64_t get_hw_rtc() 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;
 
     /*
       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;
+
 };

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

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

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