From ece7ff874c037cf6c4c5f6fa9a96613bee080cfd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 10 Apr 2018 21:43:03 +1000 Subject: [PATCH] AP_RTC: a library to handle epoch-time --- libraries/AP_RTC/AP_RTC.cpp | 142 ++++++++++++++++++++++++++++++++++++ libraries/AP_RTC/AP_RTC.h | 62 ++++++++++++++++ 2 files changed, 204 insertions(+) create mode 100644 libraries/AP_RTC/AP_RTC.cpp create mode 100644 libraries/AP_RTC/AP_RTC.h diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp new file mode 100644 index 0000000000..5b60e40a5f --- /dev/null +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -0,0 +1,142 @@ +#include "AP_RTC.h" + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +const char *AP_RTC::_clock_source_types[] = { + "GPS", + "SYSTEM_TIME", + "HW", + "NONE", +}; + +void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type) +{ + if (type >= rtc_source_type) { + // e.g. system-time message when we've been set by the GPS + return; + } + + const uint64_t now = AP_HAL::micros64(); + const int64_t tmp = int64_t(time_utc_usec) - int64_t(now); + if (tmp < rtc_shift) { + // can't allow time to go backwards, ever + return; + } + + rtc_shift = tmp; + + // update hardware clock: + if (type != SOURCE_HW) { + hal.util->set_hw_rtc(time_utc_usec); + } + + rtc_source_type = type; + + // update signing timestamp + GCS_MAVLINK::update_signing_timestamp(time_utc_usec); +} + +bool AP_RTC::get_utc_usec(uint64_t &usec) const +{ + if (rtc_source_type == SOURCE_NONE) { + return false; + } + usec = AP_HAL::micros64() + rtc_shift; + return true; +} + +void AP_RTC::get_system_clock_utc(int32_t &hour, int32_t &min, int32_t &sec, int32_t &ms) +{ + // get time of day in ms + uint64_t time_ms = 0; + get_utc_usec(time_ms); // may fail, leaving time_ms at 0 + time_ms /= 1000U; + + // 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_RTC::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms) +{ + // 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); +} + + +// singleton instance +AP_RTC *AP_RTC::_singleton; + +namespace AP { + +AP_RTC &rtc() +{ + return *AP_RTC::get_singleton(); +} + +} diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h new file mode 100644 index 0000000000..424208bded --- /dev/null +++ b/libraries/AP_RTC/AP_RTC.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +class AP_RTC { + +public: + + AP_RTC() { + if (_singleton != nullptr) { + // it's an error to get here. But I don't want to include + // AP_HAL here + return; + } + _singleton = this; + } + + // ordering is important in source_type; lower-numbered is + // considered a better time source. + enum source_type : uint8_t { + SOURCE_GPS, + SOURCE_MAVLINK_SYSTEM_TIME, + SOURCE_HW, + SOURCE_NONE, + }; + + /* + get clock in UTC microseconds. Returns false if it is not available. + */ + bool get_utc_usec(uint64_t &usec) const; + + // set the system time. If the time has already been set by + // something better (according to source_type), this set will be + // ignored. + void set_utc_usec(uint64_t time_utc_usec, source_type type); + + /* + get 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); + + uint32_t get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms); + + // get singleton instance + static AP_RTC *get_singleton() { + return _singleton; + } + + static const char *_clock_source_types[]; + +private: + + static AP_RTC *_singleton; + + source_type rtc_source_type = SOURCE_NONE; + int64_t rtc_shift; + +}; + +namespace AP { + AP_RTC &rtc(); +};