Queue clearance for emergency stop

src/determine-print-stats.cc

@@ -117,6 +117,7 @@ class StatsSegmentQueue : public SegmentQueue {
   void WaitQueueEmpty() final {}
   bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
   void SetExternalPosition(int axis, int pos) final {}
+  bool Clear() final { return false; }
 
  private:
   BeagleGPrintStats *const print_stats_;

src/gcode-machine-control_test.cc

@@ -59,6 +59,7 @@ class MockMotorOps final : public SegmentQueue {
   void MotorEnable(bool on) final {}
   bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
   void SetExternalPosition(int axis, int steps) final {}
+  bool Clear() final { return false; }
 
   int call_count_wait_queue_empty = 0;
 

src/gcode2ps.cc

@@ -1037,6 +1037,7 @@ class SegmentQueuePrinter final : public SegmentQueue {
   void MotorEnable(bool on) final {}
   void WaitQueueEmpty() final {}
   bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
+  bool Clear() final { return false; }
   void SetExternalPosition(int motor, int pos) final {
     current_pos_[motor] = pos;
     if (pass_ == ProcessingStep::GenerateOutput) {

src/motion-queue-motor-operations.cc

@@ -337,6 +337,13 @@ bool MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &segment) {
   return ret;
 }
 
+bool MotionQueueMotorOperations::Clear() {
+  const bool ret = backend_->Clear();
+  shadow_queue_->clear();
+  shadow_queue_->push_front({});
+  return ret;
+}
+
 void MotionQueueMotorOperations::MotorEnable(bool on) {
   backend_->WaitQueueEmpty();
   backend_->MotorEnable(on);

src/motion-queue-motor-operations.h

@@ -37,6 +37,7 @@ class MotionQueueMotorOperations : public SegmentQueue {
   void WaitQueueEmpty() final;
   bool GetPhysicalStatus(PhysicalStatus *status) final;
   void SetExternalPosition(int axis, int position_steps) final;
+  bool Clear() final;
 
  private:
   bool EnqueueInternal(const LinearSegmentSteps &param,

src/motion-queue-motor-operations_test.cc

@@ -37,6 +37,15 @@ class MockMotionQueue final : public MotionQueue {
     return queue_size_;
   }
 
+  bool Clear() final {
+    clear_calls_count++;
+    remaining_loops_ = 0;
+    queue_size_ = 0;
+    return true;
+  }
+
+  int clear_calls_count = 0;
+
   void SimRun(const uint32_t executed_loops, const unsigned int buffer_size) {
     assert(buffer_size <= queue_size_);
     if (buffer_size == queue_size_) assert(remaining_loops_ >= executed_loops);
@@ -202,6 +211,35 @@ TEST(RealtimePosition, zero_loops_edge) {
   EXPECT_THAT(expected, ::testing::ContainerEq(status.pos_steps));
 }
 
+// Clear motion queue motor operations.
+// The physical status should be reset and motion_backend.Clear() called.
+TEST(RealtimePosition, clear_queue) {
+  HardwareMapping hw;
+  MockMotionQueue motion_backend = MockMotionQueue();
+  MotionQueueMotorOperations motor_operations(&hw, &motion_backend);
+
+  // Enqueue a segment
+  LinearSegmentSteps segment = {
+    0 /* v0 */,
+    0 /* v1 */,
+    0 /* aux */,
+    {10, 20, 30, 40, 50, 60, 70, 80} /* steps */
+  };
+  int expected[BEAGLEG_NUM_MOTORS] = {10, 20, 30, 40, 50, 60, 70, 80};
+
+  motor_operations.Enqueue(segment);
+  motion_backend.SimRun(0, 1);
+
+  PhysicalStatus status;
+  motor_operations.GetPhysicalStatus(&status);
+  EXPECT_THAT(expected, ::testing::ContainerEq(status.pos_steps));
+  EXPECT_TRUE(motor_operations.Clear());
+  EXPECT_EQ(motion_backend.clear_calls_count, 1);
+  motor_operations.GetPhysicalStatus(&status);
+  memset(expected, 0, sizeof(expected));
+  EXPECT_THAT(expected, ::testing::ContainerEq(status.pos_steps));
+}
+
 int main(int argc, char *argv[]) {
   Log_init("/dev/stderr");
   ::testing::InitGoogleTest(&argc, argv);

src/motion-queue.h

@@ -118,6 +118,10 @@ class MotionQueue {
   // The return parameter head_item_progress is set to the number
   // of not yet executed loops in the item currenly being executed.
   virtual int GetPendingElements(uint32_t *head_item_progress) = 0;
+
+  // Perform an immediate reset of the queue,
+  // even if motors are still moving.
+  virtual bool Clear() = 0;
 };
 
 // Standard implementation.
@@ -137,6 +141,7 @@ class PRUMotionQueue final : public MotionQueue {
   void MotorEnable(bool on) final;
   void Shutdown(bool flush_queue) final;
   int GetPendingElements(uint32_t *head_item_progress) final;
+  bool Clear() final;
 
  private:
   bool Init();
@@ -161,6 +166,7 @@ class DummyMotionQueue final : public MotionQueue {
     if (head_item_progress) *head_item_progress = 0;
     return 1;
   }
+  bool Clear() final { return true; }
 };
 
 #endif  // _BEAGLEG_MOTION_QUEUE_H_

src/planner_test.cc

@@ -124,6 +124,7 @@ class FakeMotorOperations : public SegmentQueue {
   void WaitQueueEmpty() final {}
   bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
   void SetExternalPosition(int axis, int steps) final {}
+  bool Clear() final { return false; }
 
   int SegmentsCount() const { return collected_.size(); }
   const std::vector<LinearSegmentSteps> &segments() { return collected_; }

src/pru-motion-queue.cc

@@ -177,6 +177,11 @@ void PRUMotionQueue::Shutdown(bool flush_queue) {
   MotorEnable(false);
 }
 
+bool PRUMotionQueue::Clear() {
+  pru_interface_->Shutdown();
+  return Init();
+}
+
 PRUMotionQueue::~PRUMotionQueue() {}
 
 PRUMotionQueue::PRUMotionQueue(HardwareMapping *hw, PruHardwareInterface *pru)

src/pru-motion-queue_test.cc

@@ -18,23 +18,30 @@
 #include "pru-hardware-interface.h"
 #include "segment-queue.h"
 
+using ::testing::NiceMock;
+
 // PRU-side mock implementation of the ring buffer.
 struct MockPRUCommunication {
   internal::QueueStatus status;
   MotionSegment ring_buffer[QUEUE_LEN];
 } __attribute__((packed));
 
-class MockPRUInterface final : public PruHardwareInterface {
+class MockPRUInterface : public PruHardwareInterface {
  public:
-  MockPRUInterface() : execution_index_(QUEUE_LEN - 1) { mmap = NULL; }
-  ~MockPRUInterface() final { free(mmap); }
+  MockPRUInterface() : execution_index_(QUEUE_LEN - 1) {
+    mmap = NULL;
+    ON_CALL(*this, Init).WillByDefault([]() { return true; });
+    ON_CALL(*this, Shutdown).WillByDefault([]() { return true; });
+  }
+  ~MockPRUInterface() override { free(mmap); }
 
-  bool Init() final { return true; }
   bool StartExecution() final { return true; }
   unsigned WaitEvent() final { return 1; }
-  bool Shutdown() final { return true; }
+  MOCK_METHOD(bool, Init, (), ());
+  MOCK_METHOD(bool, Shutdown, (), ());
 
   bool AllocateSharedMem(void **pru_mmap, const size_t size) final {
+    if (mmap != NULL) return true;
     mmap = (struct MockPRUCommunication *)malloc(size);
     *pru_mmap = (void *)mmap;
     memset(*pru_mmap, 0x00, size);
@@ -63,16 +70,15 @@ class MockPRUInterface final : public PruHardwareInterface {
 
 TEST(PruMotionQueue, status_init) {
   MotorsRegister absolute_pos_loops;
-  MockPRUInterface pru_interface = MockPRUInterface();
+  NiceMock<MockPRUInterface> pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
-
   EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0);
 }
 
 TEST(PruMotionQueue, single_exec) {
   MotorsRegister absolute_pos_loops;
-  MockPRUInterface pru_interface = MockPRUInterface();
+  NiceMock<MockPRUInterface> pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -85,7 +91,7 @@ TEST(PruMotionQueue, single_exec) {
 
 TEST(PruMotionQueue, full_exec) {
   MotorsRegister absolute_pos_loops;
-  MockPRUInterface pru_interface = MockPRUInterface();
+  NiceMock<MockPRUInterface> pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -98,7 +104,7 @@ TEST(PruMotionQueue, full_exec) {
 
 TEST(PruMotionQueue, single_exec_some_loops) {
   MotorsRegister absolute_pos_loops;
-  MockPRUInterface pru_interface = MockPRUInterface();
+  NiceMock<MockPRUInterface> pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -113,7 +119,7 @@ TEST(PruMotionQueue, single_exec_some_loops) {
 
 TEST(PruMotionQueue, one_round_queue) {
   MotorsRegister absolute_pos_loops;
-  MockPRUInterface pru_interface = MockPRUInterface();
+  NiceMock<MockPRUInterface> pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -132,9 +138,37 @@ TEST(PruMotionQueue, one_round_queue) {
   EXPECT_EQ(motion_backend.GetPendingElements(NULL), QUEUE_LEN);
 }
 
+// Check the PRU is reset and no elements are pending.
+TEST(PruMotionQueue, clear_queue) {
+  MotorsRegister absolute_pos_loops;
+  NiceMock<MockPRUInterface> pru_interface;
+  HardwareMapping hmap = HardwareMapping();
+  PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
+
+  struct MotionSegment segment = {};
+  segment.state = STATE_FILLED;
+  motion_backend.Enqueue(&segment);
+  segment.state = STATE_FILLED;
+  motion_backend.Enqueue(&segment);
+  pru_interface.SimRun(2, 10);
+  EXPECT_EQ(motion_backend.GetPendingElements(NULL), 1);
+
+  // Start recording mocks.
+  ASSERT_TRUE(testing::Mock::VerifyAndClearExpectations(&pru_interface));
+  {
+    testing::InSequence seq;
+    EXPECT_CALL(pru_interface, Shutdown())
+      .Times(1)
+      .WillRepeatedly(testing::Return(true));
+    EXPECT_CALL(pru_interface, Init()).Times(1).WillOnce(testing::Return(true));
+  }
+  EXPECT_TRUE(motion_backend.Clear());
+  EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0);
+}
+
 TEST(PruMotionQueue, exec_index_lt_queue_pos) {
   MotorsRegister absolute_pos_loops;
-  MockPRUInterface pru_interface = MockPRUInterface();
+  NiceMock<MockPRUInterface> pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 

src/segment-queue.h

@@ -77,6 +77,12 @@ class SegmentQueue {
   // source (e.g. homing). This will allow accurate reporting of the
   // PhysicalStatus.
   virtual void SetExternalPosition(int axis, int position_steps) = 0;
+
+  // Clear the queue and reset it to its initial state.
+  // This action is immediate and will discard any running or
+  // enqueued and not yet executed segment.
+  // Current physical status will be lost.
+  virtual bool Clear() = 0;
 };
 
 #endif  // _BEAGLEG_MOTOR_OPERATIONS_H_

src/sim-audio-out.h

@@ -32,6 +32,7 @@ class SimFirmwareAudioQueue : public MotionQueue {
   void WaitQueueEmpty() final {}
   void MotorEnable(bool on) final {}
   void Shutdown(bool flush_queue) final {}
+  bool Clear() final { return false; }
   int GetPendingElements(uint32_t *head_item_progress) final {
     if (head_item_progress) *head_item_progress = 0;
     return 1;

src/sim-firmware.h

@@ -31,6 +31,7 @@ class SimFirmwareQueue : public MotionQueue {
   void WaitQueueEmpty() final {}
   void MotorEnable(bool on) final {}
   void Shutdown(bool flush_queue) final {}
+  bool Clear() final { return false; }
   int GetPendingElements(uint32_t *head_item_progress) final {
     if (head_item_progress) *head_item_progress = 0;
     return 1;