From bf342fc9269e186e0e16d36b97758a5a9444d633 Mon Sep 17 00:00:00 2001 From: David Crocker Date: Mon, 27 Dec 2021 09:53:46 +0000 Subject: Initial code to support precise stopping on CAN expansion boards --- src/CAN/CanDriversData.cpp | 57 ++++++++ src/CAN/CanDriversData.h | 97 ++++++++++++++ src/CAN/CanInterface.cpp | 43 ------ src/CAN/CanInterface.h | 80 +---------- src/CAN/CanMotion.cpp | 325 ++++++++++++++++++++++++++++++--------------- src/CAN/CanMotion.h | 11 +- src/Movement/DDA.cpp | 20 ++- src/Movement/DDA.h | 4 +- 8 files changed, 386 insertions(+), 251 deletions(-) create mode 100644 src/CAN/CanDriversData.cpp create mode 100644 src/CAN/CanDriversData.h diff --git a/src/CAN/CanDriversData.cpp b/src/CAN/CanDriversData.cpp new file mode 100644 index 00000000..f985110f --- /dev/null +++ b/src/CAN/CanDriversData.cpp @@ -0,0 +1,57 @@ +/* + * CanDriversData.cpp + * + * Created on: 23 Dec 2021 + * Author: David + */ + +#include "CanDriversData.h" + +#if SUPPORT_CAN_EXPANSION + +// Insert a new entry, keeping the list ordered +void CanDriversList::AddEntry(DriverId driver) noexcept +{ + if (numEntries < ARRAY_SIZE(drivers)) + { + // We could do a binary search here but the number of CAN drivers supported isn't huge, so linear search instead + size_t insertPoint = 0; + while (insertPoint < numEntries && drivers[insertPoint] < driver) + { + ++insertPoint; + } + + if (insertPoint == numEntries) + { + drivers[numEntries] = driver; + ++numEntries; + } + else if (drivers[insertPoint] != driver) + { + memmove(drivers + (insertPoint + 1), drivers + insertPoint, (numEntries - insertPoint) * sizeof(drivers[0])); + drivers[insertPoint] = driver; + ++numEntries; + } + } +} + +// Get the details of the drivers on the next board and advance startFrom beyond the entries for this board +CanAddress CanDriversList::GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept +{ + driversBitmap.Clear(); + if (startFrom >= numEntries) + { + return CanId::NoAddress; + } + const CanAddress boardAddress = drivers[startFrom].boardAddress; + do + { + driversBitmap.SetBit(drivers[startFrom].localDriver); + ++startFrom; + } while (startFrom < numEntries && drivers[startFrom].boardAddress == boardAddress); + return boardAddress; +} + +#endif + +// End diff --git a/src/CAN/CanDriversData.h b/src/CAN/CanDriversData.h new file mode 100644 index 00000000..42057bca --- /dev/null +++ b/src/CAN/CanDriversData.h @@ -0,0 +1,97 @@ +/* + * CanDriversData.h + * + * Created on: 23 Dec 2021 + * Author: David + */ + +#ifndef SRC_CAN_CANDRIVERSDATA_H_ +#define SRC_CAN_CANDRIVERSDATA_H_ + +#include "RepRapFirmware.h" + +#if SUPPORT_CAN_EXPANSION + +typedef Bitmap CanDriversBitmap; + +// Class to accumulate a set of values relating to CAN-connected drivers +template class CanDriversData +{ +public: + CanDriversData() noexcept; + void AddEntry(DriverId id, T val) noexcept; + size_t GetNumEntries() const noexcept { return numEntries; } + CanAddress GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept; + T GetElement(size_t n) const pre(n < GetnumEntries()) noexcept { return data[n].val; } + +private: + struct DriverDescriptor + { + DriverId driver; + T val; + }; + + size_t numEntries; + DriverDescriptor data[MaxCanDrivers]; +}; + +// Class to represent a set of CAN-connected drivers with no associated data +class CanDriversList +{ +public: + CanDriversList() noexcept : numEntries(0) { } + void Clear() noexcept { numEntries = 0; } + void AddEntry(DriverId id) noexcept; + size_t GetNumEntries() const noexcept { return numEntries; } + bool IsEmpty() const noexcept { return numEntries == 0; } + CanAddress GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept; + +private: + size_t numEntries; + DriverId drivers[MaxCanDrivers]; +}; + +// Members of template class CanDriversData +template CanDriversData::CanDriversData() noexcept +{ + numEntries = 0; +} + +// Insert a new entry, keeping the list ordered by driver ID +template void CanDriversData::AddEntry(DriverId driver, T val) noexcept +{ + if (numEntries < ARRAY_SIZE(data)) + { + // We could do a binary search here but the number of CAN drivers supported isn't huge, so linear search instead + size_t insertPoint = 0; + while (insertPoint < numEntries && data[insertPoint].driver < driver) + { + ++insertPoint; + } + memmove(data + (insertPoint + 1), data + insertPoint, (numEntries - insertPoint) * sizeof(data[0])); + data[insertPoint].driver = driver; + data[insertPoint].val = val; + ++numEntries; + } +} + +// Get the details of the drivers on the next board and advance startFrom beyond the entries for this board +template CanAddress CanDriversData::GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept +{ + driversBitmap.Clear(); + if (startFrom >= numEntries) + { + return CanId::NoAddress; + } + const CanAddress boardAddress = data[startFrom].driver.boardAddress; + do + { + driversBitmap.SetBit(data[startFrom].driver.localDriver); + ++startFrom; + } while (startFrom < numEntries && data[startFrom].driver.boardAddress == boardAddress); + return boardAddress; +} + +#endif + +#endif /* SRC_CAN_CANDRIVERSDATA_H_ */ diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp index 20c36877..a44d757d 100644 --- a/src/CAN/CanInterface.cpp +++ b/src/CAN/CanInterface.cpp @@ -561,49 +561,6 @@ extern "C" [[noreturn]] void CanClockLoop(void *) noexcept } } -// Insert a new entry, keeping the list ordered -void CanDriversList::AddEntry(DriverId driver) noexcept -{ - if (numEntries < ARRAY_SIZE(drivers)) - { - // We could do a binary search here but the number of CAN drivers supported isn't huge, so linear search instead - size_t insertPoint = 0; - while (insertPoint < numEntries && drivers[insertPoint] < driver) - { - ++insertPoint; - } - - if (insertPoint == numEntries) - { - drivers[numEntries] = driver; - ++numEntries; - } - else if (drivers[insertPoint] != driver) - { - memmove(drivers + (insertPoint + 1), drivers + insertPoint, (numEntries - insertPoint) * sizeof(drivers[0])); - drivers[insertPoint] = driver; - ++numEntries; - } - } -} - -// Get the details of the drivers on the next board and advance startFrom beyond the entries for this board -CanAddress CanDriversList::GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept -{ - driversBitmap.Clear(); - if (startFrom >= numEntries) - { - return CanId::NoAddress; - } - const CanAddress boardAddress = drivers[startFrom].boardAddress; - do - { - driversBitmap.SetBit(drivers[startFrom].localDriver); - ++startFrom; - } while (startFrom < numEntries && drivers[startFrom].boardAddress == boardAddress); - return boardAddress; -} - // Members of namespace CanInterface, and associated local functions template static GCodeResult SetRemoteDriverValues(const CanDriversData& data, const StringRef& reply, CanMessageType mt) noexcept diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h index 5281e879..a1c320ac 100644 --- a/src/CAN/CanInterface.h +++ b/src/CAN/CanInterface.h @@ -14,50 +14,13 @@ #include #include +#include "CanDriversData.h" class CanMessageBuffer; class DDA; class DriveMovement; struct PrepParams; -typedef Bitmap CanDriversBitmap; - -// Class to accumulate a set of values relating to CAN-connected drivers -templateclass CanDriversData -{ -public: - CanDriversData() noexcept; - void AddEntry(DriverId id, T val) noexcept; - size_t GetNumEntries() const noexcept { return numEntries; } - CanAddress GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept; - T GetElement(size_t n) const pre(n < GetnumEntries()) noexcept { return data[n].val; } - -private: - struct DriverDescriptor - { - DriverId driver; - T val; - }; - - size_t numEntries; - DriverDescriptor data[MaxCanDrivers]; -}; - -class CanDriversList -{ -public: - CanDriversList() noexcept : numEntries(0) { } - void Clear() noexcept { numEntries = 0; } - void AddEntry(DriverId id) noexcept; - size_t GetNumEntries() const noexcept { return numEntries; } - bool IsEmpty() const noexcept { return numEntries == 0; } - CanAddress GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept; - -private: - size_t numEntries; - DriverId drivers[MaxCanDrivers]; -}; - namespace CanInterface { // Note: GetCanAddress() in this namespace is now declared in RepRapFirmware.h to overcome ordering issues @@ -155,47 +118,6 @@ namespace ODrive { #endif } -// Members of template class CanDriversData -template CanDriversData::CanDriversData() noexcept -{ - numEntries = 0; -} - -// Insert a new entry, keeping the list ordered -template void CanDriversData::AddEntry(DriverId driver, T val) noexcept -{ - if (numEntries < ARRAY_SIZE(data)) - { - // We could do a binary search here but the number of CAN drivers supported isn't huge, so linear search instead - size_t insertPoint = 0; - while (insertPoint < numEntries && data[insertPoint].driver < driver) - { - ++insertPoint; - } - memmove(data + (insertPoint + 1), data + insertPoint, (numEntries - insertPoint) * sizeof(data[0])); - data[insertPoint].driver = driver; - data[insertPoint].val = val; - ++numEntries; - } -} - -// Get the details of the drivers on the next board and advance startFrom beyond the entries for this board -template CanAddress CanDriversData::GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept -{ - driversBitmap.Clear(); - if (startFrom >= numEntries) - { - return CanId::NoAddress; - } - const CanAddress boardAddress = data[startFrom].driver.boardAddress; - do - { - driversBitmap.SetBit(data[startFrom].driver.localDriver); - ++startFrom; - } while (startFrom < numEntries && data[startFrom].driver.boardAddress == boardAddress); - return boardAddress; -} - #endif #endif /* SRC_CAN_CANINTERFACE_H_ */ diff --git a/src/CAN/CanMotion.cpp b/src/CAN/CanMotion.cpp index 6d311d41..47bb2f27 100644 --- a/src/CAN/CanMotion.cpp +++ b/src/CAN/CanMotion.cpp @@ -12,29 +12,44 @@ #include #include #include "CanInterface.h" +#include -static CanMessageBuffer *movementBufferList = nullptr; -static CanMessageBuffer *urgentMessageBuffer = nullptr; -static uint32_t currentMoveClocks; - -#if 0 -static unsigned int numMotionMessagesSentLast = 0; -#endif +namespace CanMotion +{ + enum class DriverStopState : uint8_t { inactive = 0, active, stopRequested, stopSent }; -static volatile uint32_t hiccupToInsert = 0; -static CanDriversList driversToStop[2]; -static size_t driversToStopIndexBeingFilled = 0; -static size_t indexOfNextDriverToStop = 0; -static volatile bool stopAllFlag = false; -static bool doingStopAll = false; -static LargeBitmap boardsActiveInLastMove; -static uint8_t nextSeq[CanId::MaxCanAddress + 1] = { 0 }; + // Class to record drivers active and requests to stop them + class DriversStopList + { + public: + DECLARE_FREELIST_NEW_DELETE(DriversStopList) + + DriversStopList(DriversStopList *p_next, CanAddress p_ba) noexcept : next(p_next), boardAddress(p_ba) { } + + DriversStopList *next; + CanAddress boardAddress; + uint8_t numDrivers; + volatile DriverStopState stopStates[MaxLinearDriversPerCanSlave]; + volatile int32_t stopSteps[MaxLinearDriversPerCanSlave]; + }; + + static CanMessageBuffer urgentMessageBuffer(nullptr); + static CanMessageBuffer *movementBufferList = nullptr; + static DriversStopList *volatile stopList = nullptr; + static uint32_t currentMoveClocks; + static volatile uint32_t hiccupToInsert = 0; + static Mutex stopListMutex; + static uint8_t nextSeq[CanId::MaxCanAddress + 1] = { 0 }; + + static CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexcept; + static void InternalStopDriverWhenProvisional(DriverId driver) noexcept; + static bool InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noexcept; +} void CanMotion::Init() noexcept { movementBufferList = nullptr; - urgentMessageBuffer = CanMessageBuffer::Allocate(); - boardsActiveInLastMove.ClearAll(); + stopListMutex.Create("stopList"); } // This is called by DDA::Prepare at the start of preparing a movement @@ -51,9 +66,23 @@ void CanMotion::StartMovement() noexcept movementBufferList = p->next; CanMessageBuffer::Free(p); } + + // Free up any stop list items left over from the previous move + MutexLocker lock(stopListMutex); + + for (;;) + { + DriversStopList *p = stopList; + if (p == nullptr) + { + break; + } + stopList = p->next; + delete p; + } } -CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexcept +CanMessageBuffer *CanMotion::GetBuffer(const PrepParams& params, DriverId canDriver) noexcept { if (canDriver.localDriver >= MaxLinearDriversPerCanSlave) { @@ -178,34 +207,18 @@ 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) noexcept +uint32_t CanMotion::FinishMovement(uint32_t moveStartTime, bool simulating, bool checkingEndstops) noexcept { - boardsActiveInLastMove.ClearAll(); CanMessageBuffer *buf = movementBufferList; if (buf == nullptr) { return 0; } -#if 0 - numMotionMessagesSentLast = 0; -#endif + MutexLocker lock(stopListMutex); + do { - boardsActiveInLastMove.SetBit(buf->id.Dst()); //TODO should we set this if there were no steps for drives on the board, just drives to be enabled? -#if USE_REMOTE_INPUT_SHAPING - buf->msg.moveLinearShaped.whenToExecute = moveStartTime; - uint8_t& seq = nextSeq[buf->id.Dst()]; - buf->msg.moveLinearShaped.seq = seq; - seq = (seq + 1) & 0x7F; - buf->dataLength = buf->msg.moveLinearShaped.GetActualDataLength(); -#else - buf->msg.moveLinear.whenToExecute = moveStartTime; - uint8_t& seq = nextSeq[buf->id.Dst()]; - buf->msg.moveLinear.seq = seq; - seq = (seq + 1) & 0x7F; - buf->dataLength = buf->msg.moveLinear.GetActualDataLength(); -#endif CanMessageBuffer * const nextBuffer = buf->next; // must get this before sending the buffer, because sending the buffer releases it if (simulating) { @@ -213,11 +226,30 @@ uint32_t CanMotion::FinishMovement(uint32_t moveStartTime, bool simulating) noex } else { +#if USE_REMOTE_INPUT_SHAPING + CanMessageMovementLinear& msg = buf->msg.moveLinearShaped; +#else + 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) + { + 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 } -#if 0 - ++numMotionMessagesSentLast; -#endif buf = nextBuffer; } while (buf != nullptr); @@ -231,51 +263,31 @@ bool CanMotion::CanPrepareMove() noexcept } // This is called by the CanSender task to check if we have any urgent messages to send +// The only urgent messages we may have currently are messages to stop drivers. CanMessageBuffer *CanMotion::GetUrgentMessage() noexcept { - if (stopAllFlag) - { - // Send a broadcast Stop All message first, followed by individual ones - driversToStop[driversToStopIndexBeingFilled].Clear(); - driversToStop[driversToStopIndexBeingFilled ^ 1].Clear(); - auto msg = urgentMessageBuffer->SetupBroadcastMessage(CanInterface::GetCanAddress()); - msg->whichDrives = 0xFFFF; - doingStopAll = true; - stopAllFlag = false; - indexOfNextDriverToStop = 0; - return urgentMessageBuffer; - } + MutexLocker lock(stopListMutex); // make sure the list isn't being changed while we traverse it - if (doingStopAll) + for (DriversStopList *sl = stopList; sl != nullptr; sl = sl->next) { - const unsigned int nextBoard = boardsActiveInLastMove.FindLowestSetBit(); - if (nextBoard < boardsActiveInLastMove.NumBits()) + auto msg = urgentMessageBuffer.SetupRequestMessage(0, CanInterface::GetCanAddress(), sl->boardAddress); + uint16_t driversBeingStopped = 0; + size_t numDriversStopped = 0; + for (size_t driver = 0; driver < sl->numDrivers; ++driver) { - boardsActiveInLastMove.ClearBit(nextBoard); - auto msg = urgentMessageBuffer->SetupRequestMessage(0, CanInterface::GetCanAddress(), nextBoard); - msg->whichDrives = 0xFFFF; - return urgentMessageBuffer; + if (sl->stopStates[driver] == DriverStopState::stopRequested) + { + driversBeingStopped |= 1u << driver; + msg->finalStepCounts[numDriversStopped++] = sl->stopSteps[driver]; + sl->stopStates[driver] = DriverStopState::stopSent; + } } - doingStopAll = false; - } - - if (driversToStop[driversToStopIndexBeingFilled ^ 1].GetNumEntries() == 0 && driversToStop[driversToStopIndexBeingFilled].GetNumEntries() != 0) - { - driversToStopIndexBeingFilled = driversToStopIndexBeingFilled ^ 1; - } - - if (driversToStop[driversToStopIndexBeingFilled ^ 1].GetNumEntries() != 0) - { - CanDriversBitmap drivers; - const CanAddress board = driversToStop[driversToStopIndexBeingFilled ^ 1].GetNextBoardDriverBitmap(indexOfNextDriverToStop, drivers); - if (board != CanId::NoAddress) + if (driversBeingStopped != 0) { - auto msg = urgentMessageBuffer->SetupRequestMessage(0, CanInterface::GetCanAddress(), board); - msg->whichDrives = drivers.GetRaw(); - return urgentMessageBuffer; + msg->whichDrives = driversBeingStopped; + urgentMessageBuffer.dataLength = msg->GetActualDataLength(numDriversStopped); + return &urgentMessageBuffer; } - driversToStop[driversToStopIndexBeingFilled ^ 1].Clear(); - indexOfNextDriverToStop = 0; } return nullptr; @@ -289,64 +301,116 @@ void CanMotion::InsertHiccup(uint32_t numClocks) noexcept CanInterface::WakeAsyncSenderFromIsr(); } -void CanMotion::StopDriver(bool isBeingPrepared, DriverId driver) noexcept +void CanMotion::InternalStopDriverWhenProvisional(DriverId driver) noexcept { - if (isBeingPrepared) + // Search for the correct movement buffer + CanMessageBuffer* buf = movementBufferList; + while (buf != nullptr && buf->id.Dst() != driver.boardAddress) { - // Search for the correct movement buffer - CanMessageBuffer* buf = movementBufferList; - while (buf != nullptr && buf->id.Dst() != driver.boardAddress) - { - buf = buf->next; - } + buf = buf->next; + } - if (buf != nullptr) - { + if (buf != nullptr) + { #if USE_REMOTE_INPUT_SHAPING - buf->msg.moveLinearShaped.perDrive[driver.localDriver].steps = 0; + buf->msg.moveLinearShaped.perDrive[driver.localDriver].steps = 0; #else - buf->msg.moveLinear.perDrive[driver.localDriver].steps = 0; + buf->msg.moveLinear.perDrive[driver.localDriver].steps = 0; #endif - } } - else +} + +bool CanMotion::InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noexcept +{ + DriversStopList *sl = stopList; + while (sl != nullptr) { - driversToStop[driversToStopIndexBeingFilled].AddEntry(driver); - CanInterface::WakeAsyncSenderFromIsr(); + if (sl->boardAddress == driver.boardAddress) + { + if (sl->stopStates[driver.localDriver] == DriverStopState::active) // if active and stop not yet requested + { + sl->stopSteps[driver.localDriver] = steps; // must assign this one first + sl->stopStates[driver.localDriver] = DriverStopState::stopRequested; + return true; + } + break; // we found the right board, no point in searching further + } + sl = sl->next; } + return false; } -void CanMotion::StopAxis(bool isBeingPrepared, size_t axis) noexcept +// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true +void CanMotion::StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcept { - const AxisDriversConfig& cfg = reprap.GetPlatform().GetAxisDriversConfig(axis); - if (isBeingPrepared) + if (dda.GetState() == DDA::DDAState::provisional) + { + InternalStopDriverWhenProvisional(driver); + } + else { - for (size_t i = 0; i < cfg.numDrivers; ++i) + const DriveMovement * const dm = dda.FindDM(axis); + if (dm != nullptr) { - const DriverId driver = cfg.driverNumbers[i]; - if (driver.IsRemote()) + if (InternalStopDriverWhenMoving(driver, dm->GetNetStepsTaken())) { - StopDriver(true, driver); + CanInterface::WakeAsyncSenderFromIsr(); } } } - else if (!stopAllFlag) +} + +// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true +void CanMotion::StopAxis(const DDA& dda, size_t axis) noexcept +{ + const Platform& p = reprap.GetPlatform(); + if (axis < reprap.GetGCodes().GetTotalAxes()) { - for (size_t i = 0; i < cfg.numDrivers; ++i) + const AxisDriversConfig& cfg = p.GetAxisDriversConfig(axis); + if (dda.GetState() == DDA::DDAState::provisional) { - const DriverId driver = cfg.driverNumbers[i]; - if (driver.IsRemote()) + for (size_t i = 0; i < cfg.numDrivers; ++i) { - driversToStop[driversToStopIndexBeingFilled].AddEntry(driver); + const DriverId driver = cfg.driverNumbers[i]; + if (driver.IsRemote()) + { + InternalStopDriverWhenProvisional(driver); + } + } + } + else + { + const DriveMovement * const dm = dda.FindDM(axis); + if (dm != nullptr) + { + bool somethingStopped = false; + const uint32_t steps = dm->GetNetStepsTaken(); + for (size_t i = 0; i < cfg.numDrivers; ++i) + { + const DriverId driver = cfg.driverNumbers[i]; + if (driver.IsRemote() && InternalStopDriverWhenMoving(driver, steps)) + { + somethingStopped = true; + } + } + if (somethingStopped) + { + CanInterface::WakeAsyncSenderFromIsr(); + } } } - CanInterface::WakeAsyncSenderFromIsr(); + } + else + { + const DriverId driver = p.GetExtruderDriver(LogicalDriveToExtruder(axis)); + StopDriver(dda, axis, driver); } } -void CanMotion::StopAll(bool isBeingPrepared) noexcept +// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true +void CanMotion::StopAll(const DDA& dda) noexcept { - if (isBeingPrepared) + if (dda.GetState() == DDA::DDAState::provisional) { // We still send the messages so that the drives get enabled, but we set the steps to zero for (CanMessageBuffer *buf = movementBufferList; buf != nullptr; buf = buf->next) @@ -368,8 +432,49 @@ void CanMotion::StopAll(bool isBeingPrepared) noexcept } else { - stopAllFlag = true; - CanInterface::WakeAsyncSenderFromIsr(); + // Loop through the axes that are actually moving + const GCodes& gc = reprap.GetGCodes(); + const size_t totalAxes = gc.GetTotalAxes(); + const Platform& p = reprap.GetPlatform(); + bool somethingStopped = false; + for (size_t axis = 0; axis < totalAxes; ++axis) + { + const DriveMovement* const dm = dda.FindDM(axis); + if (dm != nullptr) + { + const uint32_t steps = dm->GetNetStepsTaken(); + const AxisDriversConfig& cfg = p.GetAxisDriversConfig(axis); + for (size_t i = 0; i < cfg.numDrivers; ++i) + { + const DriverId driver = cfg.driverNumbers[i]; + if (driver.IsRemote() && InternalStopDriverWhenMoving(driver, steps)) + { + somethingStopped = true; + } + } + } + } + const size_t numExtruders = gc.GetNumExtruders(); + for (size_t extruder = 0; extruder < numExtruders; ++extruder) + { + const DriverId driver = p.GetExtruderDriver(extruder); + if (driver.IsRemote()) + { + const DriveMovement* const dm = dda.FindDM(extruder); + if (dm != nullptr) + { + if (InternalStopDriverWhenMoving(driver, dm->GetNetStepsTaken())) + { + somethingStopped = true; + } + } + } + } + + if (somethingStopped) + { + CanInterface::WakeAsyncSenderFromIsr(); + } } } diff --git a/src/CAN/CanMotion.h b/src/CAN/CanMotion.h index 7b6b0d32..e02b0f76 100644 --- a/src/CAN/CanMotion.h +++ b/src/CAN/CanMotion.h @@ -22,17 +22,18 @@ namespace CanMotion void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps) noexcept; void AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, bool usePressureAdvance) noexcept; #else - void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance = false) noexcept; + void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance) noexcept; #endif - uint32_t FinishMovement(uint32_t moveStartTime, bool simulating) noexcept; + uint32_t FinishMovement(uint32_t moveStartTime, bool simulating, bool checkingEndstops) noexcept; bool CanPrepareMove() noexcept; CanMessageBuffer *GetUrgentMessage() noexcept; // The next 4 functions may be called from the step ISR, so they can't send CAN messages directly void InsertHiccup(uint32_t numClocks) noexcept; - void StopDriver(bool isBeingPrepared, DriverId driver) noexcept; - void StopAxis(bool isBeingPrepared, size_t axis) noexcept; - void StopAll(bool isBeingPrepared) noexcept; + void StopAll(const DDA& dda) noexcept; + void StopAxis(const DDA& dda, size_t axis) noexcept; + void StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcept + pre(driver.IsRemote()); } #endif diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index 26358f6d..c0a81800 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -1374,7 +1374,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept #if SUPPORT_CAN_EXPANSION if (driver.IsRemote()) { - CanMotion::AddMovement(params, driver, delta); + CanMotion::AddMovement(params, driver, delta, false); } else #endif @@ -1444,7 +1444,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept const DriverId driver = config.driverNumbers[i]; if (driver.IsRemote()) { - CanMotion::AddMovement(params, driver, delta); + CanMotion::AddMovement(params, driver, delta, false); } } #endif @@ -1510,7 +1510,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept const DriverId driver = config.driverNumbers[i]; if (driver.IsRemote()) { - CanMotion::AddMovement(params, driver, delta); + CanMotion::AddMovement(params, driver, delta, false); } } #endif @@ -1614,7 +1614,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept } #if SUPPORT_CAN_EXPANSION - const uint32_t canClocksNeeded = CanMotion::FinishMovement(afterPrepare.moveStartTime, simMode != SimulationMode::off); + const uint32_t canClocksNeeded = CanMotion::FinishMovement(afterPrepare.moveStartTime, simMode != SimulationMode::off, flags.checkEndstops); 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. @@ -1784,10 +1784,6 @@ float DDA::NormaliseLinearMotion(AxesBitmap linearAxes) noexcept // Either this move is currently executing (DDARing.currentDDA == this) and the state is 'executing', or we have almost finished preparing it and the state is 'provisional'. void DDA::CheckEndstops(Platform& platform) noexcept { -#if SUPPORT_CAN_EXPANSION - const bool fromPrepare = (state == DDAState::provisional); // determine this before anything sets the state to 'completed' -#endif - for (;;) { const EndstopHitDetails hitDetails = platform.GetEndstops().CheckEndstops(); @@ -1796,7 +1792,7 @@ void DDA::CheckEndstops(Platform& platform) noexcept case EndstopHitAction::stopAll: MoveAborted(); // set the state to completed and recalculate the endpoints #if SUPPORT_CAN_EXPANSION - CanMotion::StopAll(fromPrepare); + CanMotion::StopAll(*this); #endif if (hitDetails.isZProbe) { @@ -1819,11 +1815,11 @@ void DDA::CheckEndstops(Platform& platform) noexcept #if SUPPORT_CAN_EXPANSION if (state == completed) // if the call to StopDrive flagged the move as completed { - CanMotion::StopAll(fromPrepare); + CanMotion::StopAll(*this); } else { - CanMotion::StopAxis(fromPrepare, hitDetails.axis); + CanMotion::StopAxis(*this, hitDetails.axis); } #endif if (hitDetails.setAxisLow) @@ -1842,7 +1838,7 @@ void DDA::CheckEndstops(Platform& platform) noexcept #if SUPPORT_CAN_EXPANSION if (hitDetails.driver.IsRemote()) { - CanMotion::StopDriver(fromPrepare, hitDetails.driver); + CanMotion::StopDriver(*this, hitDetails.axis, hitDetails.driver); } else #endif diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h index c787b40b..94798d77 100644 --- a/src/Movement/DDA.h +++ b/src/Movement/DDA.h @@ -182,6 +182,7 @@ public: uint32_t GetStepInterval(size_t axis, uint32_t microstepShift) const noexcept; // Get the current full step interval for this axis or extruder #endif + DriveMovement *FindDM(size_t drive) const noexcept; // find the DM for a drive if there is one even if it is completed void CheckEndstops(Platform& platform) noexcept; void DebugPrint(const char *tag) const noexcept; // print the DDA only @@ -231,7 +232,6 @@ public: #endif private: - DriveMovement *FindDM(size_t drive) const noexcept; // find the DM for a drive if there is one even if it is completed DriveMovement *FindActiveDM(size_t drive) const noexcept; // find the DM for a drive if there is one but only if it is active void RecalculateMove(DDARing& ring) noexcept SPEED_CRITICAL; void MatchSpeeds() noexcept SPEED_CRITICAL; @@ -351,7 +351,7 @@ private: // These three could possibly be moved into afterPrepare DriveMovement* activeDMs; // list of associated DMs that need steps, in step time order DriveMovement* completedDMs; // list of associated DMs that don't need any more steps - MoveSegment* shapedSegments; // linked list of move segments used by axis DMs + MoveSegment* shapedSegments; // linked list of move segments used by axis DMs MoveSegment* unshapedSegments; // linked list of move segments used by extruder DMs }; -- cgit v1.2.3