From 0f5e3f788fce4fc41f7f698aaad8c360bfb0bbfe Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sat, 13 Mar 2021 10:24:09 +0100
Subject: [PATCH] VescDriver: support commanding motor duty cycle

---
 src/drivers/vesc/VescDriver/VescDriver.cpp | 8 ++++++++
 src/drivers/vesc/VescDriver/VescDriver.hpp | 1 +
 src/drivers/vesc/VescSerialDevice.cpp      | 2 +-
 3 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/drivers/vesc/VescDriver/VescDriver.cpp b/src/drivers/vesc/VescDriver/VescDriver.cpp
index 87c36058f2..a53f941100 100644
--- a/src/drivers/vesc/VescDriver/VescDriver.cpp
+++ b/src/drivers/vesc/VescDriver/VescDriver.cpp
@@ -39,6 +39,14 @@
 #include "VescDriver.hpp"
 #include <memory.h>
 
+void VescDriver::commandDutyCycle(float duty_cycle)
+{
+	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);
+}
+
 void VescDriver::commandCurrent(float current)
 {
 	uint8_t command[5] {VescCommand::SET_CURRENT};
diff --git a/src/drivers/vesc/VescDriver/VescDriver.hpp b/src/drivers/vesc/VescDriver/VescDriver.hpp
index 6025f475b9..c41009d503 100644
--- a/src/drivers/vesc/VescDriver/VescDriver.hpp
+++ b/src/drivers/vesc/VescDriver/VescDriver.hpp
@@ -51,6 +51,7 @@ public:
 	VescDriver(VescWritable *vesc_writable) : _vesc_writable(vesc_writable) {};
 	~VescDriver() = default;
 
+	void commandDutyCycle(float duty_cycle);
 	void commandCurrent(float current);
 	void commandBrakeCurrent(float current);
 
diff --git a/src/drivers/vesc/VescSerialDevice.cpp b/src/drivers/vesc/VescSerialDevice.cpp
index ea7c1cb2cf..6961adfe58 100644
--- a/src/drivers/vesc/VescSerialDevice.cpp
+++ b/src/drivers/vesc/VescSerialDevice.cpp
@@ -79,7 +79,7 @@ void VescDevice::Run()
 	}
 
 	_vesc_driver.requestFirmwareVersion();
-	_vesc_driver.commandCurrent(.5f);
+	_vesc_driver.commandDutyCycle(.05f);
 
 	// Check the number of bytes available in the buffer
 	int bytes_available{0};