diff options
-rw-r--r-- | src/CAN/CanInterface.cpp | 22 | ||||
-rw-r--r-- | src/CAN/CanInterface.h | 3 | ||||
-rw-r--r-- | src/Movement/Kinematics/HangprinterKinematics.cpp | 18 |
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. |