From 09b6979e1c9989f2d469ebb7611e04d62d4c4291 Mon Sep 17 00:00:00 2001
From: Patrick van Bennekom <P.vanBennekom@alfen.com>
Date: Tue, 29 Aug 2023 16:18:09 +0200
Subject: [PATCH] Added a time sync callback that can be used to syn system
 time

---
 include/ocpp/v201/charge_point.hpp |  5 +++++
 lib/ocpp/v201/charge_point.cpp     | 22 +++++++++++++++++++++-
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/include/ocpp/v201/charge_point.hpp b/include/ocpp/v201/charge_point.hpp
index abca35843..942b4950a 100644
--- a/include/ocpp/v201/charge_point.hpp
+++ b/include/ocpp/v201/charge_point.hpp
@@ -95,6 +95,7 @@ struct Callbacks {
         validate_network_profile_callback;
     std::optional<std::function<bool(const NetworkConnectionProfile& network_connection_profile)>>
         configure_network_connection_profile_callback;
+    std::optional<std::function<void(const ocpp::DateTime &currentTime)>> time_sync_callback;
 };
 
 /// \brief Class implements OCPP2.0.1 Charging Station
@@ -123,6 +124,9 @@ class ChargePoint : ocpp::ChargingStationBase {
     Everest::SteadyTimer boot_notification_timer;
     Everest::SteadyTimer aligned_meter_values_timer;
 
+    // time keeping
+    ocpp::DateTime heartbeat_request_time;
+
     // states
     RegistrationStatusEnum registration_status;
     WebsocketConnectionStatusEnum websocket_connection_status;
@@ -297,6 +301,7 @@ class ChargePoint : ocpp::ChargingStationBase {
 
     // Functional Block G: Availability
     void handle_change_availability_req(Call<ChangeAvailabilityRequest> call);
+    void handle_heartbeat_response(Call<HeartbeatResponse> call);
 
     // Functional Block L: Firmware management
     void handle_firmware_update_req(Call<UpdateFirmwareRequest> call);
diff --git a/lib/ocpp/v201/charge_point.cpp b/lib/ocpp/v201/charge_point.cpp
index 1670134e9..34622dd71 100644
--- a/lib/ocpp/v201/charge_point.cpp
+++ b/lib/ocpp/v201/charge_point.cpp
@@ -678,6 +678,9 @@ void ChargePoint::handle_message(const EnhancedMessage<v201::MessageType>& messa
     case MessageType::TriggerMessage:
         this->handle_trigger_message(json_message);
         break;
+    case MessageType::HeartbeatResponse:
+        this->handle_heartbeat_response(json_message);
+        break;
     case MessageType::SendLocalList:
         this->handle_send_local_authorization_list_req(json_message);
         break;
@@ -1147,6 +1150,7 @@ void ChargePoint::status_notification_req(const int32_t evse_id, const int32_t c
 void ChargePoint::heartbeat_req() {
     HeartbeatRequest req;
 
+    heartbeat_request_time = ocpp::DateTime();
     ocpp::Call<HeartbeatRequest> call(req, this->message_queue->createMessageId());
     this->send<HeartbeatRequest>(call);
 }
@@ -1260,7 +1264,9 @@ void ChargePoint::handle_boot_notification_response(CallResult<BootNotificationR
             this->heartbeat_timer.interval([this]() { this->heartbeat_req(); }, std::chrono::seconds(msg.interval));
         }
         this->update_aligned_data_interval();
-
+        if (this->callbacks.time_sync_callback.has_value()) {
+            this->callbacks.time_sync_callback.value()(msg.currentTime);
+        }
         for (auto const& [evse_id, evse] : this->evses) {
             evse->trigger_status_notification_callbacks();
         }
@@ -1915,6 +1921,20 @@ void ChargePoint::handle_change_availability_req(Call<ChangeAvailabilityRequest>
     }
 }
 
+void ChargePoint::handle_heartbeat_response(Call<HeartbeatResponse> call)
+{
+    const auto msg = call.msg;
+
+    if (this->callbacks.time_sync_callback.has_value()) {
+        // the received currentTime was the time the CSMS received the heartbeat request
+        // to get a system time as accurate as possible keep the time-of-flight into account
+        auto timeOfFlight = (msg.currentTime.to_time_point() - this->heartbeat_request_time.to_time_point()) / 2;
+        ocpp::DateTime currentTimeCompensated(msg.currentTime.to_time_point() + timeOfFlight);
+        this->callbacks.time_sync_callback.value()(currentTimeCompensated);
+    }
+}
+
+
 void ChargePoint::handle_firmware_update_req(Call<UpdateFirmwareRequest> call) {
     EVLOG_debug << "Received UpdateFirmwareRequest: " << call.msg << "\nwith messageId: " << call.uniqueId;
     UpdateFirmwareResponse response = callbacks.update_firmware_request_callback(call.msg);