From 63f3d0b723ee819191e55a3712e787c24bf66755 Mon Sep 17 00:00:00 2001 From: David Crocker Date: Thu, 17 Feb 2022 14:48:13 +0000 Subject: Fixes for homing CAN drives when the endstop switch is already triggered --- src/CAN/CanMotion.cpp | 99 +++++++++++++++++++++++++++++---------------------- src/CAN/CanMotion.h | 2 +- src/Movement/DDA.cpp | 2 +- 3 files changed, 58 insertions(+), 45 deletions(-) (limited to 'src') diff --git a/src/CAN/CanMotion.cpp b/src/CAN/CanMotion.cpp index ebf98fc6..7aece70d 100644 --- a/src/CAN/CanMotion.cpp +++ b/src/CAN/CanMotion.cpp @@ -48,6 +48,7 @@ namespace CanMotion static CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexcept; static void InternalStopDriverWhenProvisional(DriverId driver) noexcept; static bool InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noexcept; + static void FreeMovementBuffers() noexcept; } void CanMotion::Init() noexcept @@ -56,10 +57,8 @@ void CanMotion::Init() noexcept stopListMutex.Create("stopList"); } -// This is called by DDA::Prepare at the start of preparing a movement -void CanMotion::StartMovement() noexcept +void CanMotion::FreeMovementBuffers() noexcept { - // There shouldn't be any movement buffers in the list, but free any that there may be for (;;) { CanMessageBuffer *p = movementBufferList; @@ -70,6 +69,12 @@ void CanMotion::StartMovement() noexcept movementBufferList = p->next; CanMessageBuffer::Free(p); } +} + +// This is called by DDA::Prepare at the start of preparing a movement +void CanMotion::StartMovement() noexcept +{ + FreeMovementBuffers(); // there shouldn't be any movement buffers in the list, but free any that there may be // Free up any stop list items left over from the previous move MutexLocker lock(stopListMutex); @@ -212,54 +217,60 @@ void CanMotion::AddExtruderMovement(const PrepParams& params, DriverId canDriver #endif // This is called by DDA::Prepare when all DMs for CAN drives have been processed. Return the calculated move time in steps, or 0 if there are no CAN moves -uint32_t CanMotion::FinishMovement(uint32_t moveStartTime, bool simulating, bool checkingEndstops) noexcept +uint32_t CanMotion::FinishMovement(const DDA& dda, uint32_t moveStartTime, bool simulating) noexcept { - CanMessageBuffer *buf = movementBufferList; - if (buf == nullptr) + uint32_t clocks = 0; + if (simulating || dda.GetState() == DDA::completed) { - return 0; + FreeMovementBuffers(); // it turned out that there was nothing to move } - - MutexLocker lock(stopListMutex); - - do + else { - CanMessageBuffer * const nextBuffer = buf->next; // must get this before sending the buffer, because sending the buffer releases it - if (simulating) - { - CanMessageBuffer::Free(buf); - } - else + CanMessageBuffer *buf = movementBufferList; + if (buf != nullptr) { + MutexLocker lock((dda.IsCheckingEndstops()) ? &stopListMutex : nullptr); + do + { + CanMessageBuffer * const nextBuffer = buf->next; // must get this before sending the buffer, because sending the buffer releases it #if USE_REMOTE_INPUT_SHAPING - CanMessageMovementLinear& msg = buf->msg.moveLinearShaped; + CanMessageMovementLinear& msg = buf->msg.moveLinearShaped; #else - CanMessageMovementLinear& msg = buf->msg.moveLinear; + CanMessageMovementLinear& msg = buf->msg.moveLinear; #endif - msg.whenToExecute = moveStartTime; - uint8_t& seq = nextSeq[buf->id.Dst()]; - msg.seq = seq; - seq = (seq + 1) & 0x7F; - buf->dataLength = msg.GetActualDataLength(); - if (checkingEndstops) - { - // Set up the stop list - DriversStopList * const sl = new DriversStopList(stopList, buf->id.Dst()); - const size_t nd = msg.numDrivers; - sl->numDrivers = (uint8_t)nd; - for (size_t i = 0; i < nd; ++i) + if (msg.HasMotion()) { - sl->stopStates[i] = (msg.perDrive[i].steps != 0) ? DriverStopState::active : DriverStopState::inactive; + msg.whenToExecute = moveStartTime; + uint8_t& seq = nextSeq[buf->id.Dst()]; + msg.seq = seq; + seq = (seq + 1) & 0x7F; + buf->dataLength = msg.GetActualDataLength(); + if (dda.IsCheckingEndstops()) + { + // Set up the stop list + DriversStopList * const sl = new DriversStopList(stopList, buf->id.Dst()); + const size_t nd = msg.numDrivers; + sl->numDrivers = (uint8_t)nd; + for (size_t i = 0; i < nd; ++i) + { + sl->stopStates[i] = (msg.perDrive[i].steps != 0) ? DriverStopState::active : DriverStopState::inactive; + } + stopList = sl; + } + CanInterface::SendMotion(buf); // queues the buffer for sending and frees it when done + clocks = currentMoveClocks; } - stopList = sl; - } - CanInterface::SendMotion(buf); // queues the buffer for sending and frees it when done - } - buf = nextBuffer; - } while (buf != nullptr); + else + { + CanMessageBuffer::Free(buf); + } + buf = nextBuffer; + } while (buf != nullptr); - movementBufferList = nullptr; - return currentMoveClocks; + movementBufferList = nullptr; + } + } + return clocks; } bool CanMotion::CanPrepareMove() noexcept @@ -340,6 +351,7 @@ void CanMotion::InsertHiccup(uint32_t numClocks) noexcept CanInterface::WakeAsyncSenderFromIsr(); } +// Flag a CAN-connected driver as not moving when we haven't sent the movement message yet void CanMotion::InternalStopDriverWhenProvisional(DriverId driver) noexcept { // Search for the correct movement buffer @@ -359,6 +371,7 @@ void CanMotion::InternalStopDriverWhenProvisional(DriverId driver) noexcept } } +// Tell a CAN-connected driver to stop moving after we have sent the movement message bool CanMotion::InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noexcept { DriversStopList *sl = stopList; @@ -379,7 +392,7 @@ bool CanMotion::InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noe return false; } -// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true +// This is called from the step ISR with DDA state executing, or from the Move task with DDA state provisional void CanMotion::StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcept { if (dda.GetState() == DDA::DDAState::provisional) @@ -399,7 +412,7 @@ void CanMotion::StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcep } } -// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true +// This is called from the step ISR with DDA state executing, or from the Move task with DDA state provisional void CanMotion::StopAxis(const DDA& dda, size_t axis) noexcept { const Platform& p = reprap.GetPlatform(); @@ -446,7 +459,7 @@ void CanMotion::StopAxis(const DDA& dda, size_t axis) noexcept } } -// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true +// This is called from the step ISR with DDA state executing, or from the Move task with DDA state provisional void CanMotion::StopAll(const DDA& dda) noexcept { if (dda.GetState() == DDA::DDAState::provisional) diff --git a/src/CAN/CanMotion.h b/src/CAN/CanMotion.h index 9dfcb758..aae4d562 100644 --- a/src/CAN/CanMotion.h +++ b/src/CAN/CanMotion.h @@ -24,7 +24,7 @@ namespace CanMotion #else void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance) noexcept; #endif - uint32_t FinishMovement(uint32_t moveStartTime, bool simulating, bool checkingEndstops) noexcept; + uint32_t FinishMovement(const DDA& dda, uint32_t moveStartTime, bool simulating) noexcept; bool CanPrepareMove() noexcept; CanMessageBuffer *GetUrgentMessage() noexcept; diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index 880f02f5..f0adb8ac 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -1624,7 +1624,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept } #if SUPPORT_CAN_EXPANSION - const uint32_t canClocksNeeded = CanMotion::FinishMovement(afterPrepare.moveStartTime, simMode != SimulationMode::off, flags.checkEndstops); + const uint32_t canClocksNeeded = CanMotion::FinishMovement(*this, afterPrepare.moveStartTime, simMode != SimulationMode::off); if (canClocksNeeded > clocksNeeded) { // Due to rounding error in the calculations, we quite often calculate the CAN move as being longer than our previously-calculated value, normally by just one clock. -- cgit v1.2.3