From f657f46fb7116c04559210c90ee074d63c7dbd60 Mon Sep 17 00:00:00 2001
From: Zhiquan Yeo <zhiquan.yeo@gmail.com>
Date: Sun, 21 Feb 2021 08:08:35 -0500
Subject: [PATCH] feat: Add support for DS packet heartbeat monitoring (#95)

---
 package-lock.json       |  6 +++---
 package.json            |  2 +-
 src/robot/romi-robot.ts | 18 ++++++++++++++++--
 3 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/package-lock.json b/package-lock.json
index 49cc7e9..944c624 100644
--- a/package-lock.json
+++ b/package-lock.json
@@ -916,9 +916,9 @@
       }
     },
     "@wpilib/wpilib-ws-robot": {
-      "version": "1.0.0",
-      "resolved": "https://registry.npmjs.org/@wpilib/wpilib-ws-robot/-/wpilib-ws-robot-1.0.0.tgz",
-      "integrity": "sha512-k6BqCpcZOBy8xMwMxnDzbqP1oAmCkfcmoaN6DEFBDK9HEXwoTaQybnr3t6S9jwyvYX21o2ub3FCXJYeUPBenqg==",
+      "version": "1.1.0",
+      "resolved": "https://registry.npmjs.org/@wpilib/wpilib-ws-robot/-/wpilib-ws-robot-1.1.0.tgz",
+      "integrity": "sha512-k0Nbke2JVVLiE1lYbkmQsrC5qJ1kT+09rXzKmIMtLHMcoyVO1VHG6Nx4g1sfDBJAbRiYJy5lL/CADxJIb37USA==",
       "requires": {
         "@wpilib/node-wpilib-ws": "0.1.1"
       }
diff --git a/package.json b/package.json
index dcf5b39..6a79182 100644
--- a/package.json
+++ b/package.json
@@ -28,7 +28,7 @@
   },
   "homepage": "https://github.com/wpilibsuite/wpilib-ws-robot-romi#readme",
   "dependencies": {
-    "@wpilib/wpilib-ws-robot": "1.0.0",
+    "@wpilib/wpilib-ws-robot": "1.1.0",
     "commander": "^6.1.0",
     "cors": "^2.8.5",
     "express": "^4.17.1",
diff --git a/src/robot/romi-robot.ts b/src/robot/romi-robot.ts
index 0c42257..477eeb1 100644
--- a/src/robot/romi-robot.ts
+++ b/src/robot/romi-robot.ts
@@ -69,6 +69,9 @@ export default class WPILibWSRomiRobot extends WPILibWSRobotBase {
     // Keep track of whether or not the robot is DS enabled/disabled
     private _dsEnabled: boolean = false;
 
+    // Keep track of the DS heartbeat
+    private _dsHeartbeatPresent: boolean = false;
+
     // Take in the abstract bus, since this will allow us to
     // write unit tests more easily
     constructor(bus: QueuedI2CBus, address: number, romiConfig?: RomiConfiguration) {
@@ -134,9 +137,10 @@ export default class WPILibWSRomiRobot extends WPILibWSRobotBase {
                 this._resetToCleanState();
 
                 // Set up the heartbeat. Only send the heartbeat if we have
-                // an active WS connection
+                // an active WS connection, the robot is in enabled state
+                // AND we have a recent-ish DS packet
                 this._heartbeatTimer = setInterval(() => {
-                    if (this._numWsConnections > 0 && this._dsEnabled) {
+                    if (this._numWsConnections > 0 && this._dsEnabled && this._dsHeartbeatPresent) {
                         this._i2cHandle.writeByte(RomiDataBuffer.heartbeat.offset, 1)
                         .catch(err => {
                             this._i2cErrorDetector.addErrorInstance();
@@ -469,6 +473,16 @@ export default class WPILibWSRomiRobot extends WPILibWSRobotBase {
         this._dsEnabled = false;
     }
 
+    public onDSPacketTimeoutOccurred(): void {
+        console.log("[ROMI] DS Packet Heartbeat Lost");
+        this._dsHeartbeatPresent = false;
+    }
+
+    public onDSPacketTimeoutCleared(): void {
+        console.log("[ROMI] DS Packet Heartbeat Acquired");
+        this._dsHeartbeatPresent = true;
+    }
+
     public async queryFirmwareIdent(): Promise<number> {
         return this._i2cHandle.readByte(RomiDataBuffer.firmwareIdent.offset)
         .then(fwIdent => {