Don't fill buffer when flushing CAN receive hw (#550)

* Don't fill buffer when flushing CAN receive hw

* Use nullptr when flushing CAN receive hw

* Don't flush CAN receive hardware unless we expect an answer

3 files changed, 24 insertions, 19 deletions

diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index 75d7e763..2b71ecee 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -1458,18 +1458,13 @@ CanId CanInterface::ODrive::ArbitrationId(DriverId const driver, uint8_t const c
 #endif
 
 #if DUAL_CAN
-CanMessageBuffer * CanInterface::ODrive::PrepareSimpleMessage(DriverId const driver, uint8_t const cmd, const StringRef& reply) noexcept
+CanMessageBuffer * CanInterface::ODrive::PrepareSimpleMessage(DriverId const driver, const StringRef& reply) noexcept
 {
 	// Detect any early return conditions
 	if (can1dev == nullptr)
 	{
 		return nullptr;
 	}
-	if (cmd & 0xE0) // Top three bits must be zero
-	{
-		reply.copy("Simple CAN command not supported");
-		return nullptr;
-	}
 	CanMessageBuffer * buf = CanMessageBuffer::Allocate();
 	if (buf == nullptr)
 	{
@@ -1477,19 +1472,12 @@ CanMessageBuffer * CanInterface::ODrive::PrepareSimpleMessage(DriverId const dri
 		return nullptr;
 	}
 
-	// Find the correct arbitration id
-
- 	// Flush CAN receive hardware
-	while (CanInterface::ReceivePlainMessage(buf , 0)) { }
-
 	// Build the message
-	buf->id = ArbitrationId(driver, cmd);
 	buf->marker = 0;
 	buf->extId = false; // ODrive uses 11-bit IDs
 	buf->fdMode = false;
 	buf->useBrs = false;
 	buf->dataLength = 0;
-	buf->remote = true; // set RTR bit
 	buf->reportInFifo = false;
 
 	return buf;
@@ -1497,6 +1485,14 @@ CanMessageBuffer * CanInterface::ODrive::PrepareSimpleMessage(DriverId const dri
 #endif
 
 #if DUAL_CAN
+void CanInterface::ODrive::FlushCanReceiveHardware() noexcept
+{
+	while (CanInterface::ReceivePlainMessage(nullptr, 0)) { }
+}
+#endif
+
+
+#if DUAL_CAN
 bool CanInterface::ODrive::GetExpectedSimpleMessage(CanMessageBuffer *buf, DriverId const driver, uint8_t const cmd, const StringRef& reply) noexcept
 {
 	CanId const expectedId = ArbitrationId(driver, cmd);
diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h
index 6100784a..b0344f16 100644
--- a/src/CAN/CanInterface.h
+++ b/src/CAN/CanInterface.h
@@ -144,7 +144,8 @@ namespace CanInterface
 #if DUAL_CAN
 namespace ODrive {
 	CanId ArbitrationId(DriverId driver, uint8_t cmd) noexcept;
-	CanMessageBuffer * PrepareSimpleMessage(DriverId const driver, uint8_t const cmd, const StringRef& reply) noexcept;
+	CanMessageBuffer * PrepareSimpleMessage(DriverId const driver, const StringRef& reply) noexcept;
+	void FlushCanReceiveHardware() noexcept;
 	bool GetExpectedSimpleMessage(CanMessageBuffer *buf, DriverId const driver, uint8_t const cmd, const StringRef& reply) noexcept;
 }
 #endif
diff --git a/src/Movement/Kinematics/HangprinterKinematics.cpp b/src/Movement/Kinematics/HangprinterKinematics.cpp
index 41fe3464..54a035bf 100644
--- a/src/Movement/Kinematics/HangprinterKinematics.cpp
+++ b/src/Movement/Kinematics/HangprinterKinematics.cpp
@@ -580,12 +580,16 @@ HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEsti
 		}
 	}
 
-	CanMessageBuffer * buf = CanInterface::ODrive::PrepareSimpleMessage(driver, cmd, reply);
+	CanMessageBuffer * buf = CanInterface::ODrive::PrepareSimpleMessage(driver, reply);
 	if (buf == nullptr)
 	{
 		return {};
 	}
 
+	buf->id = CanInterface::ODrive::ArbitrationId(driver, cmd);
+	buf->remote = true; // Indicates that we expect an answer
+	CanInterface::ODrive::FlushCanReceiveHardware();
+
 	CanInterface::SendPlainMessageNoFree(buf);
 
 	bool ok = CanInterface::ODrive::GetExpectedSimpleMessage(buf, driver, cmd, reply);
@@ -612,6 +616,7 @@ HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEsti
 			reply.printf("Unexpected response length: %d", buf->dataLength);
 		}
 	}
+	CanMessageBuffer::Free(buf);
 
 	if (newOne && !ok)
 	{
@@ -619,7 +624,6 @@ HangprinterKinematics::ODriveAnswer HangprinterKinematics::GetODrive3EncoderEsti
 		numSeenDrives--;
 	}
 
-	CanMessageBuffer::Free(buf);
 	if (ok)
 	{
 		return {true, encoderEstimate};
@@ -650,12 +654,15 @@ GCodeResult HangprinterKinematics::ReadODrive3Encoder(DriverId const driver, GCo
 #if DUAL_CAN
 GCodeResult HangprinterKinematics::SetODrive3TorqueModeInner(DriverId const driver, float const torque, const StringRef& reply) noexcept
 {
-	// Set the right target torque
-	CanMessageBuffer * buf = CanInterface::ODrive::PrepareSimpleMessage(driver, CANSimple::MSG_SET_INPUT_TORQUE, reply);
+	// Get a buffer
+	CanMessageBuffer * buf = CanInterface::ODrive::PrepareSimpleMessage(driver, reply);
 	if (buf == nullptr)
 	{
 		return GCodeResult::error;
 	}
+
+	// Set the right target torque
+	buf->id = CanInterface::ODrive::ArbitrationId(driver, CANSimple::MSG_SET_INPUT_TORQUE);
 	buf->dataLength = 4;
 	buf->remote = false;
 	memcpy(buf->msg.raw, &torque, sizeof(torque));
@@ -681,11 +688,12 @@ GCodeResult HangprinterKinematics::SetODrive3PosMode(DriverId const driver, cons
 	if (estimate.valid)
 	{
 		float const desiredPos = estimate.value;
-		CanMessageBuffer * buf = CanInterface::ODrive::PrepareSimpleMessage(driver, CANSimple::MSG_SET_INPUT_POS, reply);
+		CanMessageBuffer * buf = CanInterface::ODrive::PrepareSimpleMessage(driver, reply);
 		if (buf == nullptr)
 		{
 			return GCodeResult::error;
 		}
+		buf->id = CanInterface::ODrive::ArbitrationId(driver, CANSimple::MSG_SET_INPUT_POS);
 		buf->dataLength = 8;
 		buf->remote = false;
 		memset(buf->msg.raw32, 0, buf->dataLength); // four last bytes are velocity and torque setpoints. Zero them.