Merge pull request #18 from REVrobotics/improve-heartbeats

Author: ItsHarper

src/canWrapper.cc

@@ -12,6 +12,7 @@
 #include <thread>
 #include <chrono>
 #include <map>
+#include <array>
 #include <vector>
 #include <set>
 #include <exception>
@@ -20,7 +21,11 @@
 #include "canWrapper.h"
 #include "DfuSeFile.h"
 
-#define DEVICE_NOT_FOUND_ERROR "Device not found.  Make sure to run getDevices()"
+#define DEVICE_NOT_FOUND_ERROR "Device not found. Make sure to run getDevices()"
+
+#define REV_COMMON_HEARTBEAT_ID 0x00502C0
+#define SPARK_HEARTBEAT_ID 0x2052C80
+#define HEARTBEAT_PERIOD_MS 20
 
 rev::usb::CandleWinUSBDriver* driver = new rev::usb::CandleWinUSBDriver();
 
@@ -29,11 +34,16 @@ bool halInitialized = false;
 uint32_t m_notifier;
 
 std::mutex canDevicesMtx;
+// These values should only be accessed while holding canDevicesMtx
 std::map<std::string, std::shared_ptr<rev::usb::CANDevice>> canDeviceMap;
 
 std::mutex watchdogMtx;
+// These values should only be accessed while holding watchdogMtx
 std::vector<std::string> heartbeatsRunning;
-auto latestHeartbeatAck = std::chrono::system_clock::now();
+bool heartbeatTimeoutExpired = false; // Should only be changed in heartbeatsWatchdog()
+std::map<std::string, std::array<uint8_t, 1>> revCommonHeartbeatMap;
+std::map<std::string, std::array<uint8_t, 8>> sparkHeartbeatMap;
+auto latestHeartbeatAck = std::chrono::steady_clock::now();
 
 // Only call when holding canDevicesMtx
 void removeExtraDevicesFromDeviceMap(std::vector<std::string> descriptors) {
@@ -617,6 +627,7 @@ void waitForNotifierAlarm(const Napi::CallbackInfo& info) {
     int32_t status;
 
     HAL_UpdateNotifierAlarm(m_notifier, HAL_GetFPGATime(&status) + time, &status);
+    // TODO(Noah): Don't discard the returned value (this function is marked as [nodiscard])
     HAL_WaitForNotifierAlarm(m_notifier, &status);
     cb.Call(info.Env().Global(), {info.Env().Null(), Napi::Number::New(info.Env(), status)});
 }
@@ -644,7 +655,7 @@ void writeDfuToBin(const Napi::CallbackInfo& info) {
 
 void heartbeatsWatchdog() {
     while (true) {
-        std::this_thread::sleep_for (std::chrono::seconds(1));
+        std::this_thread::sleep_for (std::chrono::milliseconds(250));
 
         {
             // Erase removed CAN buses from heartbeatsRunning
@@ -661,22 +672,51 @@ void heartbeatsWatchdog() {
 
         if (heartbeatsRunning.size() < 1) { break; }
 
-        auto now = std::chrono::system_clock::now();
+        auto now = std::chrono::steady_clock::now();
         std::chrono::duration<double> elapsed_seconds = now-latestHeartbeatAck;
-        if (elapsed_seconds.count() > 1) {
-            uint8_t sparkMaxHeartbeat[] = {0, 0, 0, 0, 0, 0, 0, 0};
-            uint8_t revCommonHeartbeat[] = {0};
+        if (elapsed_seconds.count() >= 1 && !heartbeatTimeoutExpired) {
+            // The heartbeat timeout just expired
+            heartbeatTimeoutExpired = true;
+            uint8_t disabledSparkHeartbeat[] = {0, 0, 0, 0, 0, 0, 0, 0};
+            uint8_t disabledRevCommonHeartbeat[] = {0};
             for(int i = 0; i < heartbeatsRunning.size(); i++) {
-                _sendCANMessage(heartbeatsRunning[i], 0x2052C80, sparkMaxHeartbeat, 8, -1);
-                _sendCANMessage(heartbeatsRunning[i], 0x00502C0, revCommonHeartbeat, 1, -1);
+                if (sparkHeartbeatMap.contains(heartbeatsRunning[i])) {
+                    // Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
+                    _sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, 8, -1);
+
+                    _sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, 8, HEARTBEAT_PERIOD_MS);
+                }
+                if (revCommonHeartbeatMap.contains(heartbeatsRunning[i])) {
+                    // Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
+                    _sendCANMessage(descriptor, REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, 1, -1);
+
+                    _sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, 1, HEARTBEAT_PERIOD_MS);
+                }
+            }
+        } else if (elapsed_seconds.count() < 1 && heartbeatTimeoutExpired) {
+            // The heartbeat timeout is newly un-expired
+            heartbeatTimeoutExpired = false;
+            for(int i = 0; i < heartbeatsRunning.size(); i++) {
+                if (auto heartbeatEntry = sparkHeartbeatMap.find(heartbeatsRunning[i]); heartbeatEntry != sparkHeartbeatMap.end()) {
+                    // Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
+                    _sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeatEntry->second.data(), 8, -1);
+
+                    _sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, heartbeatEntry->second.data(), 8, HEARTBEAT_PERIOD_MS);
+                }
+                if (auto heartbeatEntry = revCommonHeartbeatMap.find(heartbeatsRunning[i]); heartbeatEntry != revCommonHeartbeatMap.end()) {
+                    // Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
+                    _sendCANMessage(descriptor, REV_COMMON_HEARTBEAT_ID, heartbeatEntry->second.data(), 1, -1);
+
+                    _sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, heartbeatEntry->second.data(), 1, HEARTBEAT_PERIOD_MS);
+                }
             }
         }
     }
 }
 
 void ackHeartbeats(const Napi::CallbackInfo& info) {
     std::scoped_lock lock{watchdogMtx};
-    latestHeartbeatAck = std::chrono::system_clock::now();
+    latestHeartbeatAck = std::chrono::steady_clock::now();
 }
 
 // Params:
@@ -691,14 +731,19 @@ void startRevCommonHeartbeat(const Napi::CallbackInfo& info) {
         if (deviceIterator == canDeviceMap.end()) return;
     }
 
-    uint8_t payload[] = {1};
-    _sendCANMessage(descriptor, 0x00502C0, payload, 1, 20);
+    std::array<uint8_t, 1> payload = {1};
 
     std::scoped_lock lock{watchdogMtx};
 
+    if (!heartbeatTimeoutExpired) {
+        _sendCANMessage(descriptor, REV_COMMON_HEARTBEAT_ID, payload.data(), 1, HEARTBEAT_PERIOD_MS);
+    }
+
+    revCommonHeartbeatMap[descriptor] = payload;
+
     if (heartbeatsRunning.size() == 0) {
         heartbeatsRunning.push_back(descriptor);
-        latestHeartbeatAck = std::chrono::system_clock::now();
+        latestHeartbeatAck = std::chrono::steady_clock::now();
         std::thread hb(heartbeatsWatchdog);
         hb.detach();
     } else {
@@ -717,41 +762,40 @@ void setSparkMaxHeartbeatData(const Napi::CallbackInfo& info) {
     std::string descriptor = info[0].As<Napi::String>().Utf8Value();
     Napi::Array dataParam = info[1].As<Napi::Array>();
 
-    uint8_t heartbeat[] = {0, 0, 0, 0, 0, 0, 0, 0};
+    std::array<uint8_t, 8> heartbeat = {0, 0, 0, 0, 0, 0, 0, 0};
 
     {
         std::scoped_lock lock{canDevicesMtx};
         auto deviceIterator = canDeviceMap.find(descriptor);
         if (deviceIterator == canDeviceMap.end()) return;
     }
 
-    _sendCANMessage(descriptor, 0x2052C80, heartbeat, 8, -1);
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
-
     int sum = 0;
     for (uint32_t i = 0; i < dataParam.Length(); i++) {
         heartbeat[i] = dataParam.Get(i).As<Napi::Number>().Uint32Value();
         sum+= heartbeat[i];
     }
 
-    if (sum == 0) {
-        _sendCANMessage(descriptor, 0x2052C80, heartbeat, 8, -1);
+    std::scoped_lock lock{watchdogMtx};
+
+    if (!heartbeatTimeoutExpired) {
+        // Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
+        _sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeat.data(), 8, -1);
+
+        _sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeat.data(), 8, HEARTBEAT_PERIOD_MS);
     }
-    else {
-        _sendCANMessage(descriptor, 0x2052C80, heartbeat, 8, 10);
 
-        std::scoped_lock lock{watchdogMtx};
+    sparkHeartbeatMap[descriptor] = heartbeat;
 
-        if (heartbeatsRunning.size() == 0) {
-            heartbeatsRunning.push_back(descriptor);
-            latestHeartbeatAck = std::chrono::system_clock::now();
-            std::thread hb(heartbeatsWatchdog);
-            hb.detach();
-        } else {
-            for(int i = 0; i < heartbeatsRunning.size(); i++) {
-                if (heartbeatsRunning[i].compare(descriptor) == 0) return;
-            }
-            heartbeatsRunning.push_back(descriptor);
+    if (heartbeatsRunning.size() == 0) {
+        heartbeatsRunning.push_back(descriptor);
+        latestHeartbeatAck = std::chrono::steady_clock::now();
+        std::thread hb(heartbeatsWatchdog);
+        hb.detach();
+    } else {
+        for(int i = 0; i < heartbeatsRunning.size(); i++) {
+            if (heartbeatsRunning[i].compare(descriptor) == 0) return;
         }
+        heartbeatsRunning.push_back(descriptor);
     }
 }