From 83f84fdc3ee2fef881209d0e28ae9edf7ab0afb4 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sat, 13 Mar 2021 20:58:25 +0100
Subject: [PATCH] VescDriver: support CAN forwarding

---
 src/drivers/vesc/VescDriver/VescDriver.cpp | 32 ++++++++++++++++------
 src/drivers/vesc/VescDriver/VescDriver.hpp |  9 +++---
 2 files changed, 28 insertions(+), 13 deletions(-)

diff --git a/src/drivers/vesc/VescDriver/VescDriver.cpp b/src/drivers/vesc/VescDriver/VescDriver.cpp
index a53f941100..af03fbeaa9 100644
--- a/src/drivers/vesc/VescDriver/VescDriver.cpp
+++ b/src/drivers/vesc/VescDriver/VescDriver.cpp
@@ -39,43 +39,57 @@
 #include "VescDriver.hpp"
 #include <memory.h>
 
-void VescDriver::commandDutyCycle(float duty_cycle)
+void VescDriver::commandDutyCycle(float duty_cycle, const uint8_t forward_can_id)
 {
 	uint8_t command[5] {VescCommand::SET_DUTY};
 	uint16_t index{1};
 	insertInt32(command, index, static_cast<int32_t>(duty_cycle * 100000.f));
-	sendPacket(command, 5);
+	sendPayload(command, 5, forward_can_id);
 }
 
-void VescDriver::commandCurrent(float current)
+void VescDriver::commandCurrent(float current, const uint8_t forward_can_id)
 {
 	uint8_t command[5] {VescCommand::SET_CURRENT};
 	uint16_t index{1};
 	insertInt32(command, index, static_cast<int32_t>(current * 1000.f));
-	sendPacket(command, 5);
+	sendPayload(command, 5, forward_can_id);
 }
 
-void VescDriver::commandBrakeCurrent(float current)
+void VescDriver::commandBrakeCurrent(float current, const uint8_t forward_can_id)
 {
 	uint8_t command[5] {VescCommand::SET_CURRENT_BRAKE};
 	uint16_t index{1};
 	insertInt32(command, index, static_cast<int32_t>(current * 1000.f));
-	sendPacket(command, 5);
+	sendPayload(command, 5, forward_can_id);
 }
 
 void VescDriver::requestFirmwareVersion()
 {
 	uint8_t command{VescCommand::FW_VERSION};
-	sendPacket(&command, 1);
+	sendPayload(&command, 1);
 }
 
 void VescDriver::requestValues()
 {
 	uint8_t command{VescCommand::GET_VALUES};
-	sendPacket(&command, 1);
+	sendPayload(&command, 1);
 }
 
-size_t VescDriver::sendPacket(const uint8_t *payload, const uint16_t payload_length)
+size_t VescDriver::sendPayload(const uint8_t *payload, const uint16_t payload_length, const uint8_t forward_can_id)
+{
+	if (forward_can_id == 0) {
+		return sendPacket(payload, payload_length);
+
+	} else {
+		uint8_t command[2 + payload_length] {VescCommand::FORWARD_CAN, forward_can_id};
+		uint16_t index{2};
+		memcpy(&command[index], payload, payload_length);
+		index += payload_length;
+		return sendPacket(command, 2 + payload_length);
+	}
+}
+
+size_t VescDriver::sendPacket(const uint8_t *payload, uint16_t payload_length, const uint8_t can_forwarding_id)
 {
 	if (payload_length == 0 || payload_length > MAXIMUM_PAYLOAD_LENGTH) {
 		return 0;
diff --git a/src/drivers/vesc/VescDriver/VescDriver.hpp b/src/drivers/vesc/VescDriver/VescDriver.hpp
index c41009d503..d19c38d4cd 100644
--- a/src/drivers/vesc/VescDriver/VescDriver.hpp
+++ b/src/drivers/vesc/VescDriver/VescDriver.hpp
@@ -51,9 +51,9 @@ public:
 	VescDriver(VescWritable *vesc_writable) : _vesc_writable(vesc_writable) {};
 	~VescDriver() = default;
 
-	void commandDutyCycle(float duty_cycle);
-	void commandCurrent(float current);
-	void commandBrakeCurrent(float current);
+	void commandDutyCycle(float duty_cycle, const uint8_t forward_can_id = 0);
+	void commandCurrent(float current, const uint8_t forward_can_id = 0);
+	void commandBrakeCurrent(float current, const uint8_t forward_can_id = 0);
 
 	void requestFirmwareVersion();
 	void requestValues();
@@ -66,7 +66,8 @@ public:
 
 private:
 	// De-/serialize packets
-	size_t sendPacket(const uint8_t *payload, const uint16_t payload_length);
+	size_t sendPayload(const uint8_t *payload, const uint16_t payload_length, const uint8_t forward_can_id = 0);
+	size_t sendPacket(const uint8_t *payload, uint16_t payload_length, const uint8_t can_forwarding_id = 0);
 	void parsePayload(const uint8_t *payload, const uint16_t payload_length);
 	uint16_t crc16(const uint8_t *buffer, const uint16_t length);