Added clear to motion queues

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

@@ -36,7 +36,15 @@ class MockMotionQueue final : public MotionQueue {
     if (head_item_progress) *head_item_progress = remaining_loops_;
     return queue_size_;
   }
-  bool EmergencyReset() final { return true; }
+
+  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_);
@@ -203,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

@@ -119,9 +119,9 @@ class MotionQueue {
   // of not yet executed loops in the item currenly being executed.
   virtual int GetPendingElements(uint32_t *head_item_progress) = 0;
 
-  // Perform an immediate shutdown, even if motors are still moving, and
-  // reset the queue to its initial state.
-  virtual bool EmergencyReset() = 0;
+  // Perform an immediate reset of the queue,
+  // even if motors are still moving.
+  virtual bool Clear() = 0;
 };
 
 // Standard implementation.
@@ -141,7 +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 EmergencyReset() final;
+  bool Clear() final;
 
  private:
   bool Init();
@@ -166,7 +166,7 @@ class DummyMotionQueue final : public MotionQueue {
     if (head_item_progress) *head_item_progress = 0;
     return 1;
   }
-  bool EmergencyReset() final { return true; }
+  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,8 +177,8 @@ void PRUMotionQueue::Shutdown(bool flush_queue) {
   MotorEnable(false);
 }
 
-bool PRUMotionQueue::EmergencyReset() {
-  Shutdown(false);
+bool PRUMotionQueue::Clear() {
+  pru_interface_->Shutdown();
   return Init();
 }
 

src/pru-motion-queue_test.cc

@@ -30,12 +30,8 @@ class MockPRUInterface : public PruHardwareInterface {
  public:
   MockPRUInterface() : execution_index_(QUEUE_LEN - 1) {
     mmap = NULL;
-    ON_CALL(*this, Init).WillByDefault([]() {
-      return true;
-    });
-    ON_CALL(*this, Shutdown).WillByDefault([]() {
-      return true;
-    });
+    ON_CALL(*this, Init).WillByDefault([]() { return true; });
+    ON_CALL(*this, Shutdown).WillByDefault([]() { return true; });
   }
   ~MockPRUInterface() override { free(mmap); }
 
@@ -45,6 +41,8 @@ class MockPRUInterface : public PruHardwareInterface {
   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);
@@ -141,9 +139,8 @@ TEST(PruMotionQueue, one_round_queue) {
   EXPECT_EQ(motion_backend.GetPendingElements(NULL), QUEUE_LEN);
 }
 
-// Check emergency reset shutsdowns the motors and
-// the PRU and sets to zero the whole queue.
-TEST(PruMotionQueue, emergency_stop) {
+// 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();
@@ -164,11 +161,9 @@ TEST(PruMotionQueue, emergency_stop) {
     EXPECT_CALL(pru_interface, Shutdown())
       .Times(1)
       .WillRepeatedly(testing::Return(true));
-    EXPECT_CALL(pru_interface, Init())
-      .Times(1)
-      .WillOnce(testing::Return(true));
+    EXPECT_CALL(pru_interface, Init()).Times(1).WillOnce(testing::Return(true));
   }
-  EXPECT_TRUE(motion_backend.EmergencyReset());
+  EXPECT_TRUE(motion_backend.Clear());
   EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0);
 }
 

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;