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 {}
+  void HaltAndDiscard() final {}
 
  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 {}
+  void HaltAndDiscard() final {}
 
   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; }
+  void HaltAndDiscard() final {}
   void SetExternalPosition(int motor, int pos) final {
     current_pos_[motor] = pos;
     if (pass_ == ProcessingStep::GenerateOutput) {

src/motion-queue-motor-operations.cc

@@ -337,6 +337,12 @@ bool MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &segment) {
   return ret;
 }
 
+void MotionQueueMotorOperations::HaltAndDiscard() {
+  backend_->HaltAndDiscard();
+  shadow_queue_->clear();
+  shadow_queue_->push_front({});
+}
+
 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;
+  void HaltAndDiscard() final;
 
  private:
   bool EnqueueInternal(const LinearSegmentSteps &param,

src/motion-queue-motor-operations_test.cc

@@ -12,6 +12,8 @@
 #include <stdio.h>
 #include <string.h>
 
+#include <array>
+
 #include "common/container.h"
 #include "common/logging.h"
 #include "hardware-mapping.h"
@@ -37,6 +39,14 @@ class MockMotionQueue final : public MotionQueue {
     return queue_size_;
   }
 
+  void HaltAndDiscard() final {
+    clear_calls_count++;
+    remaining_loops_ = 0;
+    queue_size_ = 0;
+  }
+
+  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);
@@ -49,6 +59,18 @@ class MockMotionQueue final : public MotionQueue {
   unsigned int queue_size_;
 };
 
+// Create a dummy segment to be enqueued. Since the speeds
+// are ignored they are set to zero.
+LinearSegmentSteps CreateMockSegment(
+  const std::array<int, BEAGLEG_NUM_MOTORS> steps) {
+  LinearSegmentSteps segment = {0 /* v0, ignored */,
+                                0 /* v1, ignored */,
+                                0 /* aux, ignored */,
+                                {} /* steps */};
+  memcpy(segment.steps, steps.data(), sizeof(int) * steps.size());
+  return segment;
+}
+
 // Check that on init, the initial position is 0.
 TEST(RealtimePosition, init_pos) {
   HardwareMapping hw;
@@ -69,16 +91,11 @@ TEST(RealtimePosition, back_and_forth) {
   MotionQueueMotorOperations motor_operations(&hw, &motion_backend);
 
   // Enqueue a segment
-  const LinearSegmentSteps kSegment1 = {
-    0 /* v0 */, 0 /* v1 */, 0 /* aux */, {1000, 0, 0, 0, 0, 0, 0, 0} /* steps */
-  };
+  const LinearSegmentSteps kSegment1 =
+    CreateMockSegment({1000, 0, 0, 0, 0, 0, 0, 0});
   // Enqueue a segment
-  const LinearSegmentSteps kSegment2 = {
-    0 /* v0 */,
-    0 /* v1 */,
-    0 /* aux */,
-    {-1000, 0, 0, 0, 0, 0, 0, 0} /* steps */
-  };
+  const LinearSegmentSteps kSegment2 =
+    CreateMockSegment({-1000, 0, 0, 0, 0, 0, 0, 0});
 
   PhysicalStatus status;
   motor_operations.Enqueue(kSegment1);
@@ -112,12 +129,10 @@ TEST(RealtimePosition, empty_element) {
   MotionQueueMotorOperations motor_operations(&hw, &motion_backend);
 
   // Enqueue a segment
-  const LinearSegmentSteps kSegment1 = {
-    0 /* v0 */, 0 /* v1 */, 0xff /* aux */, {0, 0, 0, 0, 0, 0, 0, 0} /* steps */
-  };
-  const LinearSegmentSteps kSegment2 = {
-    0 /* v0 */, 0 /* v1 */, 0 /* aux */, {100, 0, 0, 0, 0, 0, 0, 0} /* steps */
-  };
+  const LinearSegmentSteps kSegment1 =
+    CreateMockSegment({0, 0, 0, 0, 0, 0, 0, 0});
+  const LinearSegmentSteps kSegment2 =
+    CreateMockSegment({100, 0, 0, 0, 0, 0, 0, 0});
 
   motor_operations.Enqueue(kSegment2);
   motor_operations.Enqueue(kSegment1);
@@ -138,18 +153,10 @@ TEST(RealtimePosition, sample_pos) {
   MotionQueueMotorOperations motor_operations(&hw, &motion_backend);
 
   // Enqueue a segment
-  const LinearSegmentSteps kSegment1 = {
-    0 /* v0 */,
-    0 /* v1 */,
-    0 /* aux */,
-    {10, -20, 30, -40, 0, -60, 70, -80} /* steps */
-  };
-  const LinearSegmentSteps kSegment2 = {
-    0 /* v0 */,
-    0 /* v1 */,
-    0 /* aux */,
-    {17, +42, 12, -90, 30, +91, 113, -1000} /* steps */
-  };
+  const LinearSegmentSteps kSegment1 =
+    CreateMockSegment({10, -20, 30, -40, 0, -60, 70, -80});
+  const LinearSegmentSteps kSegment2 =
+    CreateMockSegment({17, +42, 12, -90, 30, +91, 113, -1000});
   motor_operations.Enqueue(kSegment1);
   motor_operations.Enqueue(kSegment2);
 
@@ -180,12 +187,8 @@ TEST(RealtimePosition, zero_loops_edge) {
   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 */
-  };
+  LinearSegmentSteps segment =
+    CreateMockSegment({10, 20, 30, 40, 50, 60, 70, 80});
   const int expected[BEAGLEG_NUM_MOTORS] = {10, 20, 30, 40, 50, 60, 70, 80};
 
   motor_operations.Enqueue(segment);
@@ -202,6 +205,34 @@ 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.HaltAndDiscard()
+// called.
+TEST(RealtimePosition, clear_queue) {
+  HardwareMapping hw;
+  MockMotionQueue motion_backend = MockMotionQueue();
+  MotionQueueMotorOperations motor_operations(&hw, &motion_backend);
+
+  // Enqueue a segment
+  LinearSegmentSteps segment =
+    CreateMockSegment({10, 20, 30, 40, 50, 60, 70, 80});
+  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));
+
+  motor_operations.HaltAndDiscard();
+  EXPECT_EQ(motion_backend.clear_calls_count, 1);
+
+  memset(expected, 0, sizeof(expected));
+  motor_operations.GetPhysicalStatus(&status);
+  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,9 @@ 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;
+
+  // Immediately stop any motion and clear the internal queue.
+  virtual void HaltAndDiscard() = 0;
 };
 
 // Standard implementation.
@@ -137,11 +140,13 @@ class PRUMotionQueue final : public MotionQueue {
   void MotorEnable(bool on) final;
   void Shutdown(bool flush_queue) final;
   int GetPendingElements(uint32_t *head_item_progress) final;
+  void HaltAndDiscard() final;
 
  private:
   bool Init();
 
   void ClearPRUAbort(unsigned int idx);
+  void ClearRingBuffer();
 
   HardwareMapping *const hardware_mapping_;
   PruHardwareInterface *const pru_interface_;
@@ -161,6 +166,7 @@ class DummyMotionQueue final : public MotionQueue {
     if (head_item_progress) *head_item_progress = 0;
     return 1;
   }
+  void HaltAndDiscard() final {}
 };
 
 #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 {}
+  void HaltAndDiscard() final {}
 
   int SegmentsCount() const { return collected_.size(); }
   const std::vector<LinearSegmentSteps> &segments() { return collected_; }

src/pru-hardware-interface.h

@@ -42,6 +42,12 @@ class PruHardwareInterface {
 
   // Halt the PRU
   virtual bool Shutdown() = 0;
+
+  // Halt any program execution. An halted program can be restarted.
+  virtual void Halt() = 0;
+
+  // Restart the execution of the previously halted program.
+  virtual void Restart() = 0;
 };
 
 class UioPrussInterface : public PruHardwareInterface {
@@ -51,6 +57,8 @@ class UioPrussInterface : public PruHardwareInterface {
   bool StartExecution() final;
   unsigned WaitEvent() final;
   bool Shutdown() final;
+  void Halt() final;
+  void Restart() final;
 };
 
 #endif  // BEAGLEG_PRU_HARDWARE_INTERFACE_

src/pru-motion-queue.cc

@@ -177,6 +177,14 @@ void PRUMotionQueue::Shutdown(bool flush_queue) {
   MotorEnable(false);
 }
 
+void PRUMotionQueue::HaltAndDiscard() {
+  MotorEnable(false);
+  pru_interface_->Halt();
+  ClearRingBuffer();
+  queue_pos_ = 0;
+  pru_interface_->Restart();
+}
+
 PRUMotionQueue::~PRUMotionQueue() {}
 
 PRUMotionQueue::PRUMotionQueue(HardwareMapping *hw, PruHardwareInterface *pru)
@@ -187,18 +195,20 @@ PRUMotionQueue::PRUMotionQueue(HardwareMapping *hw, PruHardwareInterface *pru)
   assert(success);
 }
 
+void PRUMotionQueue::ClearRingBuffer() {
+  for (int i = 0; i < QUEUE_LEN; ++i) {
+    pru_data_->ring_buffer[i].state = STATE_EMPTY;
+  }
+}
+
 bool PRUMotionQueue::Init() {
   MotorEnable(false);  // motors off initially.
   if (!pru_interface_->Init()) return false;
 
   if (!pru_interface_->AllocateSharedMem((void **)&pru_data_,
                                          sizeof(*pru_data_)))
     return false;
-
-  for (int i = 0; i < QUEUE_LEN; ++i) {
-    pru_data_->ring_buffer[i].state = STATE_EMPTY;
-  }
+  ClearRingBuffer();
   queue_pos_ = 0;
-
   return pru_interface_->StartExecution();
 }