Skip to content

Commit b0ca096

Browse files
Moose1301Moose1301
committed
add serialdriver for other platforms then windows
1 parent b94fade commit b0ca096

File tree

1 file changed

+55
-104
lines changed

1 file changed

+55
-104
lines changed

‎src/canWrapper.cc

Lines changed: 55 additions & 104 deletions
Original file line numberDiff line numberDiff line change
@@ -3,16 +3,22 @@
33
#include <rev/CANMessage.h>
44
#include <rev/CANStatus.h>
55
#include <rev/CANBridgeUtils.h>
6+
#ifdef _WIN32
67
#include <rev/Drivers/CandleWinUSB/CandleWinUSBDriver.h>
78
#include <rev/Drivers/CandleWinUSB/CandleWinUSBDevice.h>
9+
#else
10+
11+
#include "rev/Drivers/SerialPort/SerialDriver.h"
12+
#endif
13+
14+
815
#include <utils/ThreadUtils.h>
916
#include <hal/HAL.h>
1017
#include <hal/CAN.h>
1118
#include <napi.h>
1219
#include <thread>
1320
#include <chrono>
1421
#include <map>
15-
#include <array>
1622
#include <vector>
1723
#include <set>
1824
#include <exception>
@@ -21,34 +27,25 @@
2127
#include "canWrapper.h"
2228
#include "DfuSeFile.h"
2329

24-
#define DEVICE_NOT_FOUND_ERROR "Device not found. Make sure to run getDevices()"
30+
#define DEVICE_NOT_FOUND_ERROR "Device not found. Make sure to run getDevices()"
2531

26-
#define REV_COMMON_HEARTBEAT_ID 0x00502C0
27-
#define SPARK_HEARTBEAT_ID 0x2052C80
28-
#define HEARTBEAT_PERIOD_MS 20
29-
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-
35-
rev::usb::CandleWinUSBDriver* driver = new rev::usb::CandleWinUSBDriver();
32+
rev::usb::CANDriver* driver =
33+
#ifdef _WIN32
34+
new rev::usb::CandleWinUSBDriver();
35+
#else
36+
new rev::usb::SerialDriver();
37+
#endif
3638

3739
std::set<std::string> devicesRegisteredToHal; // TODO(Noah): Protect with mutex
3840
bool halInitialized = false;
3941
uint32_t m_notifier;
4042

4143
std::mutex canDevicesMtx;
42-
// These values should only be accessed while holding canDevicesMtx
4344
std::map<std::string, std::shared_ptr<rev::usb::CANDevice>> canDeviceMap;
4445

4546
std::mutex watchdogMtx;
46-
// These values should only be accessed while holding watchdogMtx
4747
std::vector<std::string> heartbeatsRunning;
48-
bool heartbeatTimeoutExpired = false; // Should only be changed in heartbeatsWatchdog()
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;
51-
auto latestHeartbeatAck = std::chrono::steady_clock::now();
48+
auto latestHeartbeatAck = std::chrono::system_clock::now();
5249

5350
// Only call when holding canDevicesMtx
5451
void removeExtraDevicesFromDeviceMap(std::vector<std::string> descriptors) {
@@ -632,7 +629,6 @@ void waitForNotifierAlarm(const Napi::CallbackInfo& info) {
632629
int32_t status;
633630

634631
HAL_UpdateNotifierAlarm(m_notifier, HAL_GetFPGATime(&status) + time, &status);
635-
// TODO(Noah): Don't discard the returned value (this function is marked as [nodiscard])
636632
HAL_WaitForNotifierAlarm(m_notifier, &status);
637633
cb.Call(info.Env().Global(), {info.Env().Null(), Napi::Number::New(info.Env(), status)});
638634
}
@@ -658,70 +654,41 @@ void writeDfuToBin(const Napi::CallbackInfo& info) {
658654
cb.Call(info.Env().Global(), {info.Env().Null(), Napi::Number::New(info.Env(), status)});
659655
}
660656

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-
672657
void heartbeatsWatchdog() {
673658
while (true) {
674-
std::this_thread::sleep_for (std::chrono::milliseconds(250));
659+
std::this_thread::sleep_for (std::chrono::seconds(1));
675660

676-
cleanupHeartbeatsRunning();
661+
{
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+
}
677671

678672
std::scoped_lock lock{watchdogMtx};
679673

680674
if (heartbeatsRunning.size() < 1) { break; }
681675

682-
auto now = std::chrono::steady_clock::now();
676+
auto now = std::chrono::system_clock::now();
683677
std::chrono::duration<double> elapsed_seconds = now-latestHeartbeatAck;
684-
if (elapsed_seconds.count() >= 1 && !heartbeatTimeoutExpired) {
685-
// The heartbeat timeout just expired
686-
heartbeatTimeoutExpired = true;
687-
for(int i = 0; i < heartbeatsRunning.size(); i++) {
688-
if (sparkHeartbeatMap.contains(heartbeatsRunning[i])) {
689-
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
690-
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, SPARK_HEARTBEAT_LENGTH, -1);
691-
692-
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, disabledSparkHeartbeat, SPARK_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
693-
}
694-
if (revCommonHeartbeatMap.contains(heartbeatsRunning[i])) {
695-
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
696-
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, REV_COMMON_HEARTBEAT_LENGTH, -1);
697-
698-
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, disabledRevCommonHeartbeat, REV_COMMON_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
699-
}
700-
}
701-
} else if (elapsed_seconds.count() < 1 && heartbeatTimeoutExpired) {
702-
// The heartbeat timeout is newly un-expired
703-
heartbeatTimeoutExpired = false;
678+
if (elapsed_seconds.count() > 1) {
679+
uint8_t sparkMaxHeartbeat[] = {0, 0, 0, 0, 0, 0, 0, 0};
680+
uint8_t revCommonHeartbeat[] = {0};
704681
for(int i = 0; i < heartbeatsRunning.size(); i++) {
705-
if (auto heartbeatEntry = sparkHeartbeatMap.find(heartbeatsRunning[i]); heartbeatEntry != sparkHeartbeatMap.end()) {
706-
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
707-
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, heartbeatEntry->second.data(), SPARK_HEARTBEAT_LENGTH, -1);
708-
709-
_sendCANMessage(heartbeatsRunning[i], SPARK_HEARTBEAT_ID, heartbeatEntry->second.data(), SPARK_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
710-
}
711-
if (auto heartbeatEntry = revCommonHeartbeatMap.find(heartbeatsRunning[i]); heartbeatEntry != revCommonHeartbeatMap.end()) {
712-
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
713-
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, heartbeatEntry->second.data(), REV_COMMON_HEARTBEAT_LENGTH, -1);
714-
715-
_sendCANMessage(heartbeatsRunning[i], REV_COMMON_HEARTBEAT_ID, heartbeatEntry->second.data(), REV_COMMON_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
716-
}
682+
_sendCANMessage(heartbeatsRunning[i], 0x2052C80, sparkMaxHeartbeat, 8, -1);
683+
_sendCANMessage(heartbeatsRunning[i], 0x00502C0, revCommonHeartbeat, 1, -1);
717684
}
718685
}
719686
}
720687
}
721688

722689
void ackHeartbeats(const Napi::CallbackInfo& info) {
723690
std::scoped_lock lock{watchdogMtx};
724-
latestHeartbeatAck = std::chrono::steady_clock::now();
691+
latestHeartbeatAck = std::chrono::system_clock::now();
725692
}
726693

727694
// Params:
@@ -736,19 +703,14 @@ void startRevCommonHeartbeat(const Napi::CallbackInfo& info) {
736703
if (deviceIterator == canDeviceMap.end()) return;
737704
}
738705

739-
std::array<uint8_t, REV_COMMON_HEARTBEAT_LENGTH> payload = {1};
706+
uint8_t payload[] = {1};
707+
_sendCANMessage(descriptor, 0x00502C0, payload, 1, 20);
740708

741709
std::scoped_lock lock{watchdogMtx};
742710

743-
if (!heartbeatTimeoutExpired) {
744-
_sendCANMessage(descriptor, REV_COMMON_HEARTBEAT_ID, payload.data(), REV_COMMON_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
745-
}
746-
747-
revCommonHeartbeatMap[descriptor] = payload;
748-
749711
if (heartbeatsRunning.size() == 0) {
750712
heartbeatsRunning.push_back(descriptor);
751-
latestHeartbeatAck = std::chrono::steady_clock::now();
713+
latestHeartbeatAck = std::chrono::system_clock::now();
752714
std::thread hb(heartbeatsWatchdog);
753715
hb.detach();
754716
} else {
@@ -767,53 +729,42 @@ void setSparkMaxHeartbeatData(const Napi::CallbackInfo& info) {
767729
std::string descriptor = info[0].As<Napi::String>().Utf8Value();
768730
Napi::Array dataParam = info[1].As<Napi::Array>();
769731

770-
std::array<uint8_t, SPARK_HEARTBEAT_LENGTH> heartbeat = {0, 0, 0, 0, 0, 0, 0, 0};
732+
uint8_t heartbeat[] = {0, 0, 0, 0, 0, 0, 0, 0};
771733

772734
{
773735
std::scoped_lock lock{canDevicesMtx};
774736
auto deviceIterator = canDeviceMap.find(descriptor);
775737
if (deviceIterator == canDeviceMap.end()) return;
776738
}
777739

740+
_sendCANMessage(descriptor, 0x2052C80, heartbeat, 8, -1);
741+
std::this_thread::sleep_for(std::chrono::milliseconds(50));
742+
778743
int sum = 0;
779744
for (uint32_t i = 0; i < dataParam.Length(); i++) {
780745
heartbeat[i] = dataParam.Get(i).As<Napi::Number>().Uint32Value();
781746
sum+= heartbeat[i];
782747
}
783748

784-
std::scoped_lock lock{watchdogMtx};
785-
786-
if (!heartbeatTimeoutExpired) {
787-
// Clear the scheduled heartbeat that has outdated data so that the updated one gets sent out immediately
788-
_sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeat.data(), SPARK_HEARTBEAT_LENGTH, -1);
789-
790-
_sendCANMessage(descriptor, SPARK_HEARTBEAT_ID, heartbeat.data(), SPARK_HEARTBEAT_LENGTH, HEARTBEAT_PERIOD_MS);
749+
if (sum == 0) {
750+
_sendCANMessage(descriptor, 0x2052C80, heartbeat, 8, -1);
791751
}
752+
else {
753+
_sendCANMessage(descriptor, 0x2052C80, heartbeat, 8, 10);
792754

793-
sparkHeartbeatMap[descriptor] = heartbeat;
755+
std::scoped_lock lock{watchdogMtx};
794756

795-
if (heartbeatsRunning.size() == 0) {
796-
heartbeatsRunning.push_back(descriptor);
797-
latestHeartbeatAck = std::chrono::steady_clock::now();
798-
std::thread hb(heartbeatsWatchdog);
799-
hb.detach();
800-
} else {
801-
for(int i = 0; i < heartbeatsRunning.size(); i++) {
802-
if (heartbeatsRunning[i].compare(descriptor) == 0) return;
757+
if (heartbeatsRunning.size() == 0) {
758+
heartbeatsRunning.push_back(descriptor);
759+
latestHeartbeatAck = std::chrono::system_clock::now();
760+
std::thread hb(heartbeatsWatchdog);
761+
hb.detach();
762+
} else {
763+
for(int i = 0; i < heartbeatsRunning.size(); i++) {
764+
if (heartbeatsRunning[i].compare(descriptor) == 0) return;
765+
}
766+
heartbeatsRunning.push_back(descriptor);
803767
}
804-
heartbeatsRunning.push_back(descriptor);
805768
}
806769
}
807770

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-
}

0 commit comments

Comments
 (0)