Line data Source code
1 : #include "AP_RTC_config.h"
2 :
3 : #if AP_RTC_ENABLED
4 :
5 : #include "AP_RTC.h"
6 :
7 : #include <AP_HAL/AP_HAL.h>
8 : #include <AP_Math/AP_Math.h>
9 : #include <GCS_MAVLink/GCS.h>
10 : #include <AP_Common/time.h>
11 : #include <AP_Logger/AP_Logger.h>
12 :
13 : extern const AP_HAL::HAL& hal;
14 :
15 2100 : AP_RTC::AP_RTC()
16 : {
17 2100 : AP_Param::setup_object_defaults(this, var_info);
18 2100 : if (_singleton != nullptr) {
19 : // it's an error to get here. But I don't want to include
20 : // AP_HAL here
21 0 : return;
22 : }
23 2100 : _singleton = this;
24 : }
25 :
26 :
27 : // table of user settable parameters
28 : const AP_Param::GroupInfo AP_RTC::var_info[] = {
29 :
30 : // @Param: _TYPES
31 : // @DisplayName: Allowed sources of RTC time
32 : // @Description: Specifies which sources of UTC time will be accepted
33 : // @Bitmask: 0:GPS,1:MAVLINK_SYSTEM_TIME,2:HW
34 : // @User: Advanced
35 : AP_GROUPINFO("_TYPES", 1, AP_RTC, allowed_types, 1),
36 :
37 : // @Param: _TZ_MIN
38 : // @DisplayName: Timezone offset from UTC
39 : // @Description: Adds offset in +- minutes from UTC to calculate local time
40 : // @Range: -720 +840
41 : // @User: Advanced
42 : AP_GROUPINFO("_TZ_MIN", 2, AP_RTC, tz_min, 0),
43 :
44 : AP_GROUPEND
45 : };
46 :
47 6852357 : void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
48 : {
49 6852357 : const uint64_t oldest_acceptable_date_us = 1640995200ULL*1000*1000; // 2022-01-01 0:00
50 :
51 : // only allow time to be moved forward from the same sourcetype
52 : // while the vehicle is disarmed:
53 6852357 : if (hal.util->get_soft_armed()) {
54 4924328 : if (type >= rtc_source_type) {
55 : // e.g. system-time message when we've been set by the GPS
56 5499490 : return;
57 : }
58 : } else {
59 : // vehicle is disarmed; accept (e.g.) GPS time source updates
60 1928029 : if (type > rtc_source_type) {
61 : // e.g. system-time message when we've been set by the GPS
62 2 : return;
63 : }
64 : }
65 :
66 : // check it's from an allowed sources:
67 1928055 : if (!(allowed_types & (1<<type))) {
68 2098 : return;
69 : }
70 :
71 : // don't allow old times
72 1925957 : if (time_utc_usec < oldest_acceptable_date_us) {
73 2 : return;
74 : }
75 :
76 1925955 : const uint64_t now = AP_HAL::micros64();
77 1925955 : const int64_t tmp = int64_t(time_utc_usec) - int64_t(now);
78 1925955 : if (tmp < rtc_shift) {
79 : // can't allow time to go backwards, ever
80 573088 : return;
81 : }
82 :
83 : #if AP_RTC_LOGGING_ENABLED
84 1352867 : if (rtc_source_type != type || tmp != rtc_shift) {
85 7107 : AP::logger().Write_RTC();
86 : }
87 : #endif // AP_RTC_LOGGING_ENABLED
88 :
89 1352867 : WITH_SEMAPHORE(rsem);
90 :
91 1352867 : rtc_shift = tmp;
92 :
93 : // update hardware clock:
94 1352867 : if (type != SOURCE_HW) {
95 1352867 : hal.util->set_hw_rtc(time_utc_usec);
96 : }
97 :
98 1352867 : rtc_source_type = type;
99 :
100 : #if AP_MAVLINK_SIGNING_ENABLED
101 : // update signing timestamp
102 1352867 : GCS_MAVLINK::update_signing_timestamp(time_utc_usec);
103 : #endif // AP_MAVLINK_SIGNING_ENABLED
104 1352867 : }
105 :
106 2458730 : bool AP_RTC::get_utc_usec(uint64_t &usec) const
107 : {
108 2458730 : if (rtc_source_type == SOURCE_NONE) {
109 185720 : return false;
110 : }
111 2273010 : usec = AP_HAL::micros64() + rtc_shift;
112 2273010 : return true;
113 : }
114 :
115 54 : void AP_RTC::clock_ms_to_hms_fields(const uint64_t time_ms, uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
116 : {
117 : // separate time into ms, sec, min, hour and days but all expressed in milliseconds
118 54 : ms = time_ms % 1000;
119 54 : uint32_t sec_ms = (time_ms % (60 * 1000)) - ms;
120 54 : uint32_t min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms;
121 54 : uint32_t hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms;
122 :
123 : // convert times as milliseconds into appropriate units
124 54 : sec = sec_ms / 1000;
125 54 : min = min_ms / (60 * 1000);
126 54 : hour = hour_ms / (60 * 60 * 1000);
127 54 : }
128 :
129 16 : bool AP_RTC::clock_s_to_date_fields(const uint32_t utc_sec32, uint16_t& year, uint8_t& month, uint8_t& day, uint8_t &hour, uint8_t &min, uint8_t &sec, uint8_t &wday) const
130 : {
131 16 : const time_t utc_sec = utc_sec32;
132 16 : struct tm tmd {};
133 16 : struct tm* tm = gmtime_r(&utc_sec, &tmd);
134 16 : if (tm == nullptr) {
135 0 : return false;
136 : }
137 16 : year = tm->tm_year+1900; /* Year, 20xx. */
138 16 : month = tm->tm_mon; /* Month. [0-11] */
139 16 : day = tm->tm_mday; /* Day. [1-31] */
140 16 : hour = tm->tm_hour; /* Hours. [0-23] */
141 16 : min = tm->tm_min; /* Minutes. [0-59] */
142 16 : sec = tm->tm_sec; /* Seconds. [0-60] (1 leap second) */
143 16 : wday = tm->tm_wday; /* week day, [0-6] */
144 16 : return true;
145 : }
146 :
147 : /*
148 : return true for leap years
149 : */
150 966 : bool AP_RTC::_is_leap(uint32_t y)
151 : {
152 966 : y += 1900;
153 966 : return (y % 4) == 0 && ((y % 100) != 0 || (y % 400) == 0);
154 : }
155 :
156 : /*
157 : implementation of timegm() (from Samba)
158 : */
159 16 : uint32_t AP_RTC::_timegm(struct tm &tm)
160 : {
161 : static const uint8_t ndays[2][12] = {
162 : {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31},
163 : {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}};
164 16 : uint32_t res = 0;
165 :
166 16 : if (tm.tm_mon > 12 ||
167 14 : tm.tm_mday > 31 ||
168 14 : tm.tm_min > 60 ||
169 14 : tm.tm_sec > 60 ||
170 14 : tm.tm_hour > 24) {
171 : /* invalid tm structure */
172 2 : return 0;
173 : }
174 :
175 962 : for (auto i = 70; i < tm.tm_year; i++) {
176 948 : res += _is_leap(i) ? 366 : 365;
177 : }
178 :
179 32 : for (auto i = 0; i < tm.tm_mon; i++) {
180 18 : res += ndays[_is_leap(tm.tm_year)][i];
181 : }
182 14 : res += tm.tm_mday - 1U;
183 14 : res *= 24U;
184 14 : res += tm.tm_hour;
185 14 : res *= 60U;
186 14 : res += tm.tm_min;
187 14 : res *= 60U;
188 14 : res += tm.tm_sec;
189 14 : return res;
190 : }
191 :
192 16 : uint32_t AP_RTC::date_fields_to_clock_s(uint16_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec) const
193 : {
194 16 : struct tm tm {};
195 16 : tm.tm_year = year - 1900;
196 16 : tm.tm_mon = month;
197 16 : tm.tm_mday = day;
198 16 : tm.tm_hour = hour;
199 16 : tm.tm_min = min;
200 16 : tm.tm_sec = sec;
201 16 : return _timegm(tm);
202 : }
203 :
204 50 : bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
205 : {
206 : // get time of day in ms
207 : uint64_t time_ms;
208 50 : if (!get_utc_usec(time_ms)) {
209 4 : return false;
210 : }
211 46 : time_ms /= 1000U;
212 46 : clock_ms_to_hms_fields(time_ms, hour, min, sec, ms);
213 46 : return true;
214 : }
215 :
216 10 : bool AP_RTC::get_local_time(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
217 : {
218 : // get local time of day in ms
219 : uint64_t time_ms;
220 10 : if (!get_utc_usec(time_ms)) {
221 2 : return false;
222 : }
223 8 : time_ms /= 1000U;
224 8 : const uint64_t ms_local = time_ms + (tz_min * 60000);
225 8 : clock_ms_to_hms_fields(ms_local, hour, min, sec, ms);
226 8 : return true;
227 : }
228 :
229 : // get milliseconds from now to a target time of day expressed as
230 : // hour, min, sec, ms. Match starts from first value that is not
231 : // -1. I.e. specifying hour=-1, minutes=10 will ignore the hour and
232 : // return time until 10 minutes past 12am (utc) NOTE: if this time has
233 : // just past then you can expect a return value of roughly 86340000 -
234 : // the number of milliseconds in a day.
235 26 : uint32_t AP_RTC::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms)
236 : {
237 : // determine highest value specified (0=none, 1=ms, 2=sec, 3=min, 4=hour)
238 26 : int8_t largest_element = 0;
239 26 : if (hour != -1) {
240 10 : largest_element = 4;
241 16 : } else if (min != -1) {
242 4 : largest_element = 3;
243 12 : } else if (sec != -1) {
244 6 : largest_element = 2;
245 6 : } else if (ms != -1) {
246 4 : largest_element = 1;
247 : } else {
248 : // exit immediately if no time specified
249 2 : return 0;
250 : }
251 :
252 : // get start_time_ms as h, m, s, ms
253 : uint8_t curr_hour, curr_min, curr_sec;
254 : uint16_t curr_ms;
255 24 : if (!get_system_clock_utc(curr_hour, curr_min, curr_sec, curr_ms)) {
256 2 : return 0;
257 : }
258 22 : int32_t total_delay_ms = 0;
259 :
260 : // calculate ms to target
261 22 : if (largest_element >= 1) {
262 22 : total_delay_ms += ms - curr_ms;
263 : }
264 22 : if (largest_element == 1 && total_delay_ms < 0) {
265 2 : return static_cast<uint32_t>(total_delay_ms + 1000);
266 : }
267 :
268 : // calculate sec to target
269 20 : if (largest_element >= 2) {
270 18 : total_delay_ms += (sec - curr_sec)*1000;
271 : }
272 20 : if (largest_element == 2 && total_delay_ms < 0) {
273 2 : return static_cast<uint32_t>(total_delay_ms + (60*1000));
274 : }
275 :
276 : // calculate min to target
277 18 : if (largest_element >= 3) {
278 12 : total_delay_ms += (min - curr_min)*60*1000;
279 : }
280 18 : if (largest_element == 3 && total_delay_ms < 0) {
281 4 : return static_cast<uint32_t>(total_delay_ms + (60*60*1000));
282 : }
283 :
284 : // calculate hours to target
285 14 : if (largest_element >= 4) {
286 8 : total_delay_ms += (hour - curr_hour)*60*60*1000;
287 : }
288 14 : if (largest_element == 4 && total_delay_ms < 0) {
289 4 : return static_cast<uint32_t>(total_delay_ms + (24*60*60*1000));
290 : }
291 :
292 : // total delay in milliseconds
293 10 : return static_cast<uint32_t>(total_delay_ms);
294 : }
295 :
296 : // get date and time. Returns true on success and fills in year, month, day, hour, min, sec and ms
297 : // year is the regular Gregorian year, month is 0~11, day is 1~31, hour is 0~23, minute is 0~59, second is 0~60 (1 leap second), ms is 0~999
298 2003 : bool AP_RTC::get_date_and_time_utc(uint16_t& year, uint8_t& month, uint8_t& day, uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
299 : {
300 : // get local time of day in ms
301 2003 : uint64_t time_us = 0;
302 2003 : if (!get_utc_usec(time_us)) {
303 14 : return false;
304 : }
305 1989 : time_t utc_sec = time_us / (1000U * 1000U);
306 1989 : struct tm tmd {};
307 1989 : struct tm* tm = gmtime_r(&utc_sec, &tmd);
308 1989 : if (tm == nullptr) {
309 0 : return false;
310 : }
311 1989 : year = tm->tm_year+1900; /* Year - 1900. */
312 1989 : month = tm->tm_mon; /* Month. [0-11] */
313 1989 : day = tm->tm_mday; /* Day. [1-31] */
314 1989 : hour = tm->tm_hour; /* Hours. [0-23] */
315 1989 : min = tm->tm_min; /* Minutes. [0-59] */
316 1989 : sec = tm->tm_sec; /* Seconds. [0-60] (1 leap second) */
317 1989 : ms = (time_us / 1000U) % 1000U; /* milliseconds [0-999] */
318 1989 : return true;
319 : }
320 :
321 : // singleton instance
322 : AP_RTC *AP_RTC::_singleton;
323 :
324 : namespace AP {
325 :
326 9320216 : AP_RTC &rtc()
327 : {
328 9320216 : return *AP_RTC::get_singleton();
329 : }
330 :
331 : }
332 :
333 : #endif // AP_RTC_ENABLED
|
