From 2822f507c131c2095c4fd7823ccd3c62674226a9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 8 Dec 2023 11:19:50 +1100
Subject: [PATCH] AP_RTC: added time and date APIs for lua

and fixed a bug with the ms time return
---
 libraries/AP_RTC/AP_RTC.cpp | 111 ++++++++++++++++++++++++++++--------
 libraries/AP_RTC/AP_RTC.h   |   9 ++-
 2 files changed, 96 insertions(+), 24 deletions(-)

diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp
index 41d9f5b294..77c763b528 100644
--- a/libraries/AP_RTC/AP_RTC.cpp
+++ b/libraries/AP_RTC/AP_RTC.cpp
@@ -94,15 +94,8 @@ bool AP_RTC::get_utc_usec(uint64_t &usec) const
     return true;
 }
 
-bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
+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
 {
-     // get time of day in ms
-    uint64_t time_ms = 0;
-    if (!get_utc_usec(time_ms)) {
-        return false;
-    }
-    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;
@@ -113,32 +106,104 @@ bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uin
     sec = sec_ms / 1000;
     min = min_ms / (60 * 1000);
     hour = hour_ms / (60 * 60 * 1000);
+}
 
+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
+{
+    const time_t utc_sec = utc_sec32;
+    struct tm* tm = gmtime(&utc_sec);
+    if (tm == nullptr) {
+        return false;
+    }
+    year = tm->tm_year+1900;    /* Year, 20xx.  */
+    month = tm->tm_mon;         /* Month.	[0-11] */
+    day = tm->tm_mday;          /* Day.		[1-31] */
+    hour = tm->tm_hour;         /* Hours.	[0-23] */
+    min = tm->tm_min;           /* Minutes.	[0-59] */
+    sec = tm->tm_sec;           /* Seconds.	[0-60] (1 leap second) */
+    wday = tm->tm_wday;         /* week day, [0-6] */
+    return true;
+}
+
+/*
+  return true for leap years
+ */
+bool AP_RTC::_is_leap(uint32_t y)
+{
+    y += 1900;
+    return (y % 4) == 0 && ((y % 100) != 0 || (y % 400) == 0);
+}
+
+/*
+  implementation of timegm() (from Samba)
+*/
+uint32_t AP_RTC::_timegm(struct tm &tm)
+{
+    static const uint8_t ndays[2][12] = {
+		{31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31},
+		{31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}};
+    uint32_t res = 0;
+
+    if (tm.tm_mon > 12 ||
+        tm.tm_mday > 31 ||
+        tm.tm_min > 60 ||
+        tm.tm_sec > 60 ||
+        tm.tm_hour > 24) {
+		/* invalid tm structure */
+		return 0;
+	}
+	
+    for (auto i = 70; i < tm.tm_year; i++) {
+        res += _is_leap(i) ? 366 : 365;
+    }
+	
+    for (auto i = 0; i < tm.tm_mon; i++) {
+        res += ndays[_is_leap(tm.tm_year)][i];
+    }
+    res += tm.tm_mday - 1U;
+    res *= 24U;
+	res += tm.tm_hour;
+    res *= 60U;
+	res += tm.tm_min;
+    res *= 60U;
+	res += tm.tm_sec;
+	return res;
+}
+
+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
+{
+    struct tm tm {};
+    tm.tm_year = year - 1900;
+    tm.tm_mon = month;
+    tm.tm_mday = day;
+    tm.tm_hour = hour;
+    tm.tm_min = min;
+    tm.tm_sec = sec;
+    return _timegm(tm);
+}
+
+bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
+{
+    // get time of day in ms
+    uint64_t time_ms;
+    if (!get_utc_usec(time_ms)) {
+        return false;
+    }
+    time_ms /= 1000U;
+    clock_ms_to_hms_fields(time_ms, hour, min, sec, ms);
     return true;
 }
 
 bool AP_RTC::get_local_time(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const
 {
      // get local time of day in ms
-    uint64_t time_ms = 0;
-    uint64_t ms_local = 0;
+    uint64_t time_ms;
     if (!get_utc_usec(time_ms)) {
         return false;
     }
     time_ms /= 1000U;
-    ms_local = time_ms + (tz_min * 60000);
-
-    // separate time into ms, sec, min, hour and days but all expressed in milliseconds
-    ms = ms_local % 1000;
-    uint32_t sec_ms = (ms_local % (60 * 1000)) - ms;
-    uint32_t min_ms = (ms_local % (60 * 60 * 1000)) - sec_ms - ms;
-    uint32_t hour_ms = (ms_local % (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);
-
+    const uint64_t ms_local = time_ms + (tz_min * 60000);
+    clock_ms_to_hms_fields(ms_local, hour, min, sec, ms);
     return true;
 }
 
diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h
index d03846bf6f..a35887532b 100644
--- a/libraries/AP_RTC/AP_RTC.h
+++ b/libraries/AP_RTC/AP_RTC.h
@@ -61,7 +61,10 @@ public:
     HAL_Semaphore &get_semaphore(void) {
         return rsem;
     }
-    
+
+    bool 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;
+    uint32_t date_fields_to_clock_s(uint16_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec) const;
+
 private:
 
     static AP_RTC *_singleton;
@@ -70,6 +73,10 @@ private:
     source_type rtc_source_type = SOURCE_NONE;
     int64_t rtc_shift;
 
+    void clock_ms_to_hms_fields(const uint64_t time_ms, uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const;
+
+    static bool _is_leap(uint32_t y);
+    static uint32_t _timegm(struct tm &tm);
 };
 
 namespace AP {