Skip to content

Commit 1147222

Browse files
ItsHarperItsHarper
authored
Merge pull request #20 from REVrobotics/add-stopHeartbeatsFunction
2 parents 0b08259 + 4fd62bb commit 1147222

File tree

4 files changed

+50
-27
lines changed

4 files changed

+50
-27
lines changed

‎lib/binding.ts

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,7 @@ export class CanBridge {
6363
setThreadPriority: (descriptor: string, priority: ThreadPriority) => void;
6464
setSparkMaxHeartbeatData: (descriptor: string, heartbeatData: number[]) => void;
6565
startRevCommonHeartbeat: (descriptor: string) => void;
66+
stopHeartbeats: (descriptor: string, sendDisabledHeartbeatsFirst: boolean) => void;
6667
ackHeartbeats: () => void;
6768

6869
constructor() {
@@ -90,6 +91,7 @@ export class CanBridge {
9091
this.setSparkMaxHeartbeatData = addon.setSparkMaxHeartbeatData;
9192
this.startRevCommonHeartbeat = addon.startRevCommonHeartbeat;
9293
this.ackHeartbeats = addon.ackHeartbeats;
94+
this.stopHeartbeats = addon.stopHeartbeats;
9395
} catch (e: any) {
9496
throw new CanBridgeInitializationError(e);
9597
}

‎src/addon.cc

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,8 @@ Napi::Object Init(Napi::Env env, Napi::Object exports) {
4242
Napi::Function::New(env, setSparkMaxHeartbeatData));
4343
exports.Set(Napi::String::New(env, "startRevCommonHeartbeat"),
4444
Napi::Function::New(env, startRevCommonHeartbeat));
45+
exports.Set(Napi::String::New(env, "stopHeartbeats"),
46+
Napi::Function::New(env, stopHeartbeats));
4547
exports.Set(Napi::String::New(env, "ackHeartbeats"),
4648
Napi::Function::New(env, ackHeartbeats));
4749
return exports;

‎src/canWrapper.cc

Lines changed: 45 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,11 @@
2727
#define SPARK_HEARTBEAT_ID 0x2052C80
2828
#define HEARTBEAT_PERIOD_MS 20
2929

30+
#define SPARK_HEARTBEAT_LENGTH 8
31+
#define REV_COMMON_HEARTBEAT_LENGTH 1
32+
uint8_t disabledSparkHeartbeat[] = {0, 0, 0, 0, 0, 0, 0, 0};
33+
uint8_t disabledRevCommonHeartbeat[] = {0};
34+
3035
rev::usb::CandleWinUSBDriver* driver = new rev::usb::CandleWinUSBDriver();
3136

3237
std::set<std::string> devicesRegisteredToHal; // TODO(Noah): Protect with mutex
@@ -41,8 +46,8 @@ std::mutex watchdogMtx;
4146
// These values should only be accessed while holding watchdogMtx
4247
std::vector<std::string> heartbeatsRunning;
4348
bool heartbeatTimeoutExpired = false; // Should only be changed in heartbeatsWatchdog()
44-
std::map<std::string, std::array<uint8_t, 1>> revCommonHeartbeatMap;
45-
std::map<std::string, std::array<uint8_t, 8>> sparkHeartbeatMap;
49+
std::map<std::string, std::array<uint8_t, REV_COMMON_HEARTBEAT_LENGTH>> revCommonHeartbeatMap;
50+
std::map<std::string, std::array<uint8_t, SPARK_HEARTBEAT_LENGTH>> sparkHeartbeatMap;
4651
auto latestHeartbeatAck = std::chrono::steady_clock::now();
4752

4853
// Only call when holding canDevicesMtx
@@ -653,20 +658,22 @@ void writeDfuToBin(const Napi::CallbackInfo& info) {
653658
cb.Call(info.Env().Global(), {info.Env().Null(), Napi::Number::New(info.Env(), status)});
654659
}
655660

661+
void cleanupHeartbeatsRunning() {
662+
// Erase removed CAN buses from heartbeatsRunning
663+
std::scoped_lock lock{watchdogMtx, canDevicesMtx};
664+
for(int i = 0; i < heartbeatsRunning.size(); i++) {
665+
auto deviceIterator = canDeviceMap.find(heartbeatsRunning[i]);
666+
if (deviceIterator == canDeviceMap.end()) {
667+
heartbeatsRunning.erase(heartbeatsRunning.begin() + i);
668+
}
669+
}
670+
}
671+
656672
void heartbeatsWatchdog() {
657673
while (true) {
658674
std::this_thread::sleep_for (std::chrono::milliseconds(250));
659675

660-
{
661-
// Erase removed CAN buses from heartbeatsRunning
662-
std::scoped_lock lock{watchdogMtx, canDevicesMtx};
663-
for(int i = 0; i < heartbeatsRunning.size(); i++) {
664-
auto deviceIterator = canDeviceMap.find(heartbeatsRunning[i]);
665-
if (deviceIterator == canDeviceMap.end()) {
666-
heartbeatsRunning.erase(heartbeatsRunning.begin() + i);
667-
}
668-
}
669-
}
676+
cleanupHeartbeatsRunning();
670677

671678
std::scoped_lock lock{watchdogMtx};
672679

@@ -677,20 +684,18 @@ void heartbeatsWatchdog() {
677684
if (elapsed_seconds.count() >= 1 && !heartbeatTimeoutExpired) {
678685
// The heartbeat timeout just expired
679686
heartbeatTimeoutExpired = true;
680-
uint8_t disabledSparkHeartbeat[] = {0, 0, 0, 0, 0, 0, 0, 0};
681-
uint8_t disabledRevCommonHeartbeat[] = {0};
682687
for(int i = 0; i < heartbeatsRunning.size(); i++) {
683688
if (sparkHeartbeatMap.contains(heartbeatsRunning[i])) {
684689
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
685-
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, 8, -1);
690+
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, SPARK_HEARTBEAT_LENGTH, -1);
686691

687-
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, 8, HEARTBEAT_PERIOD_MS);
692+
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, SPARK_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
688693
}
689694
if (revCommonHeartbeatMap.contains(heartbeatsRunning[i])) {
690695
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
691-
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, 1, -1);
696+
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, REV_COMMON_HEARTBEAT_LENGTH, -1);
692697

693-
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, 1, HEARTBEAT_PERIOD_MS);
698+
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, REV_COMMON_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
694699
}
695700
}
696701
} else if (elapsed_seconds.count() < 1 && heartbeatTimeoutExpired) {
@@ -699,15 +704,15 @@ void heartbeatsWatchdog() {
699704
for(int i = 0; i < heartbeatsRunning.size(); i++) {
700705
if (auto heartbeatEntry = sparkHeartbeatMap.find(heartbeatsRunning[i]); heartbeatEntry != sparkHeartbeatMap.end()) {
701706
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
702-
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, heartbeatEntry->second.data(), 8, -1);
707+
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, heartbeatEntry->second.data(), SPARK_HEARTBEAT_LENGTH, -1);
703708

704-
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, heartbeatEntry->second.data(), 8, HEARTBEAT_PERIOD_MS);
709+
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, heartbeatEntry->second.data(), SPARK_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
705710
}
706711
if (auto heartbeatEntry = revCommonHeartbeatMap.find(heartbeatsRunning[i]); heartbeatEntry != revCommonHeartbeatMap.end()) {
707712
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
708-
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, heartbeatEntry->second.data(), 1, -1);
713+
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, heartbeatEntry->second.data(), REV_COMMON_HEARTBEAT_LENGTH, -1);
709714

710-
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, heartbeatEntry->second.data(), 1, HEARTBEAT_PERIOD_MS);
715+
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, heartbeatEntry->second.data(), REV_COMMON_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
711716
}
712717
}
713718
}
@@ -731,12 +736,12 @@ void startRevCommonHeartbeat(const Napi::CallbackInfo& info) {
731736
if (deviceIterator == canDeviceMap.end()) return;
732737
}
733738

734-
std::array<uint8_t, 1> payload = {1};
739+
std::array<uint8_t, REV_COMMON_HEARTBEAT_LENGTH> payload = {1};
735740

736741
std::scoped_lock lock{watchdogMtx};
737742

738743
if (!heartbeatTimeoutExpired) {
739-
_sendCANMessage(descriptor, REV_COMMON_HEARTBEAT_ID, payload.data(), 1, HEARTBEAT_PERIOD_MS);
744+
_sendCANMessage(descriptor, REV_COMMON_HEARTBEAT_ID, payload.data(), REV_COMMON_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
740745
}
741746

742747
revCommonHeartbeatMap[descriptor] = payload;
@@ -762,7 +767,7 @@ void setSparkMaxHeartbeatData(const Napi::CallbackInfo& info) {
762767
std::string descriptor = info[0].As<Napi::String>().Utf8Value();
763768
Napi::Array dataParam = info[1].As<Napi::Array>();
764769

765-
std::array<uint8_t, 8> heartbeat = {0, 0, 0, 0, 0, 0, 0, 0};
770+
std::array<uint8_t, SPARK_HEARTBEAT_LENGTH> heartbeat = {0, 0, 0, 0, 0, 0, 0, 0};
766771

767772
{
768773
std::scoped_lock lock{canDevicesMtx};
@@ -780,9 +785,9 @@ void setSparkMaxHeartbeatData(const Napi::CallbackInfo& info) {
780785

781786
if (!heartbeatTimeoutExpired) {
782787
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
783-
_sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeat.data(), 8, -1);
788+
_sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeat.data(), SPARK_HEARTBEAT_LENGTH, -1);
784789

785-
_sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeat.data(), 8, HEARTBEAT_PERIOD_MS);
790+
_sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeat.data(), SPARK_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
786791
}
787792

788793
sparkHeartbeatMap[descriptor] = heartbeat;
@@ -799,3 +804,16 @@ void setSparkMaxHeartbeatData(const Napi::CallbackInfo& info) {
799804
heartbeatsRunning.push_back(descriptor);
800805
}
801806
}
807+
808+
void stopHeartbeats(const Napi::CallbackInfo& info) {
809+
std::string descriptor = info[0].As<Napi::String>().Utf8Value();
810+
bool sendDisabledHeartbeatsFirst = info[1].As<Napi::Boolean>().Value();
811+
812+
// 0 sends and then cancels, -1 cancels without sending
813+
const int repeatPeriod = sendDisabledHeartbeatsFirst ? 0 : -1;
814+
815+
std::scoped_lock lock{watchdogMtx};
816+
// Send disabled SPARK and REV common heartbeats and un-schedule them for the future
817+
_sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, SPARK_HEARTBEAT_LENGTH, repeatPeriod);
818+
_sendCANMessage(descriptor, REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, REV_COMMON_HEARTBEAT_LENGTH, repeatPeriod);
819+
}

‎src/canWrapper.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,5 +24,6 @@ void closeHALStreamSession(const Napi::CallbackInfo& info);
2424
void setThreadPriority(const Napi::CallbackInfo& info);
2525
void setSparkMaxHeartbeatData(const Napi::CallbackInfo& info);
2626
void startRevCommonHeartbeat(const Napi::CallbackInfo& info);
27+
void stopHeartbeats(const Napi::CallbackInfo& info);
2728
void ackHeartbeats(const Napi::CallbackInfo& info);
2829
#endif

0 commit comments

Comments
 (0)