From 355a1a0306d3e3c6420e9d3d9782e8377115f8d1 Mon Sep 17 00:00:00 2001 From: Leonardo Romor Date: Sat, 29 Apr 2023 13:23:46 +0200 Subject: [PATCH 1/6] Added emergency reset for motion queue --- src/motion-queue-motor-operations_test.cc | 1 + src/motion-queue.h | 6 +++ src/pru-motion-queue.cc | 5 ++ src/pru-motion-queue_test.cc | 64 ++++++++++++++++++----- 4 files changed, 64 insertions(+), 12 deletions(-) diff --git a/src/motion-queue-motor-operations_test.cc b/src/motion-queue-motor-operations_test.cc index ae7cef7..489e4e9 100644 --- a/src/motion-queue-motor-operations_test.cc +++ b/src/motion-queue-motor-operations_test.cc @@ -36,6 +36,7 @@ class MockMotionQueue final : public MotionQueue { if (head_item_progress) *head_item_progress = remaining_loops_; return queue_size_; } + bool EmergencyReset() final { return true; } void SimRun(const uint32_t executed_loops, const unsigned int buffer_size) { assert(buffer_size <= queue_size_); diff --git a/src/motion-queue.h b/src/motion-queue.h index c8e7937..bbcfdfd 100644 --- a/src/motion-queue.h +++ b/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 shutdown, even if motors are still moving, and + // reset the queue to its initial state. + virtual bool EmergencyReset() = 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 EmergencyReset() final; private: bool Init(); @@ -161,6 +166,7 @@ class DummyMotionQueue final : public MotionQueue { if (head_item_progress) *head_item_progress = 0; return 1; } + bool EmergencyReset() final { return true; } }; #endif // _BEAGLEG_MOTION_QUEUE_H_ diff --git a/src/pru-motion-queue.cc b/src/pru-motion-queue.cc index 7e8868f..3f9112d 100644 --- a/src/pru-motion-queue.cc +++ b/src/pru-motion-queue.cc @@ -177,6 +177,11 @@ void PRUMotionQueue::Shutdown(bool flush_queue) { MotorEnable(false); } +bool PRUMotionQueue::EmergencyReset() { + Shutdown(false); + return Init(); +} + PRUMotionQueue::~PRUMotionQueue() {} PRUMotionQueue::PRUMotionQueue(HardwareMapping *hw, PruHardwareInterface *pru) diff --git a/src/pru-motion-queue_test.cc b/src/pru-motion-queue_test.cc index e42095a..acbeac0 100644 --- a/src/pru-motion-queue_test.cc +++ b/src/pru-motion-queue_test.cc @@ -18,21 +18,31 @@ #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 { mmap = (struct MockPRUCommunication *)malloc(size); @@ -63,16 +73,15 @@ class MockPRUInterface final : public PruHardwareInterface { TEST(PruMotionQueue, status_init) { MotorsRegister absolute_pos_loops; - MockPRUInterface pru_interface = MockPRUInterface(); + NiceMock 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 pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -85,7 +94,7 @@ TEST(PruMotionQueue, single_exec) { TEST(PruMotionQueue, full_exec) { MotorsRegister absolute_pos_loops; - MockPRUInterface pru_interface = MockPRUInterface(); + NiceMock pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -98,7 +107,7 @@ TEST(PruMotionQueue, full_exec) { TEST(PruMotionQueue, single_exec_some_loops) { MotorsRegister absolute_pos_loops; - MockPRUInterface pru_interface = MockPRUInterface(); + NiceMock pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -113,7 +122,7 @@ TEST(PruMotionQueue, single_exec_some_loops) { TEST(PruMotionQueue, one_round_queue) { MotorsRegister absolute_pos_loops; - MockPRUInterface pru_interface = MockPRUInterface(); + NiceMock pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -132,9 +141,40 @@ 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) { + MotorsRegister absolute_pos_loops; + NiceMock 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.EmergencyReset()); + EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0); +} + TEST(PruMotionQueue, exec_index_lt_queue_pos) { MotorsRegister absolute_pos_loops; - MockPRUInterface pru_interface = MockPRUInterface(); + NiceMock pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); From 449a9f663afe08b865c3ac82d2887ad1d0ce0247 Mon Sep 17 00:00:00 2001 From: Leonardo Romor Date: Sat, 29 Apr 2023 20:30:24 +0200 Subject: [PATCH 2/6] Added clear to motion queues --- src/determine-print-stats.cc | 1 + src/gcode-machine-control_test.cc | 1 + src/gcode2ps.cc | 1 + src/motion-queue-motor-operations.cc | 7 ++++ src/motion-queue-motor-operations.h | 1 + src/motion-queue-motor-operations_test.cc | 39 ++++++++++++++++++++++- src/motion-queue.h | 10 +++--- src/planner_test.cc | 1 + src/pru-motion-queue.cc | 4 +-- src/pru-motion-queue_test.cc | 20 ++++-------- src/segment-queue.h | 6 ++++ src/sim-audio-out.h | 1 + src/sim-firmware.h | 1 + 13 files changed, 72 insertions(+), 21 deletions(-) diff --git a/src/determine-print-stats.cc b/src/determine-print-stats.cc index 13629b4..3342b25 100644 --- a/src/determine-print-stats.cc +++ b/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_; diff --git a/src/gcode-machine-control_test.cc b/src/gcode-machine-control_test.cc index 92f48e1..e906716 100644 --- a/src/gcode-machine-control_test.cc +++ b/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; diff --git a/src/gcode2ps.cc b/src/gcode2ps.cc index 5656ab5..ef1096c 100644 --- a/src/gcode2ps.cc +++ b/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) { diff --git a/src/motion-queue-motor-operations.cc b/src/motion-queue-motor-operations.cc index 0433595..7b7acee 100644 --- a/src/motion-queue-motor-operations.cc +++ b/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); diff --git a/src/motion-queue-motor-operations.h b/src/motion-queue-motor-operations.h index 6a0e8f2..b311f00 100644 --- a/src/motion-queue-motor-operations.h +++ b/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 ¶m, diff --git a/src/motion-queue-motor-operations_test.cc b/src/motion-queue-motor-operations_test.cc index 489e4e9..9059d03 100644 --- a/src/motion-queue-motor-operations_test.cc +++ b/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); diff --git a/src/motion-queue.h b/src/motion-queue.h index bbcfdfd..612d6cd 100644 --- a/src/motion-queue.h +++ b/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_ diff --git a/src/planner_test.cc b/src/planner_test.cc index 601036a..71b2f0e 100644 --- a/src/planner_test.cc +++ b/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 &segments() { return collected_; } diff --git a/src/pru-motion-queue.cc b/src/pru-motion-queue.cc index 3f9112d..40b751b 100644 --- a/src/pru-motion-queue.cc +++ b/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(); } diff --git a/src/pru-motion-queue_test.cc b/src/pru-motion-queue_test.cc index acbeac0..ec46e4a 100644 --- a/src/pru-motion-queue_test.cc +++ b/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,7 @@ 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 +138,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 pru_interface; HardwareMapping hmap = HardwareMapping(); @@ -164,11 +160,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); } diff --git a/src/segment-queue.h b/src/segment-queue.h index bc258a8..7b6fd67 100644 --- a/src/segment-queue.h +++ b/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_ diff --git a/src/sim-audio-out.h b/src/sim-audio-out.h index 2ef5f58..0f6951e 100644 --- a/src/sim-audio-out.h +++ b/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; diff --git a/src/sim-firmware.h b/src/sim-firmware.h index 3f8c9c5..b5091b1 100644 --- a/src/sim-firmware.h +++ b/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; From 846dfb845c034c3f74b13c98beecfafd7a0c44d8 Mon Sep 17 00:00:00 2001 From: Leonardo Romor Date: Sat, 29 Apr 2023 22:31:45 +0200 Subject: [PATCH 3/6] added comments for unused segment values --- src/motion-queue-motor-operations_test.cc | 69 ++++++++++------------- 1 file changed, 30 insertions(+), 39 deletions(-) diff --git a/src/motion-queue-motor-operations_test.cc b/src/motion-queue-motor-operations_test.cc index 9059d03..ec09da6 100644 --- a/src/motion-queue-motor-operations_test.cc +++ b/src/motion-queue-motor-operations_test.cc @@ -12,6 +12,8 @@ #include #include +#include + #include "common/container.h" #include "common/logging.h" #include "hardware-mapping.h" @@ -58,6 +60,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 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; @@ -78,16 +92,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); @@ -121,12 +130,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); @@ -147,18 +154,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); @@ -189,12 +188,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); @@ -219,12 +214,8 @@ TEST(RealtimePosition, clear_queue) { 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}); int expected[BEAGLEG_NUM_MOTORS] = {10, 20, 30, 40, 50, 60, 70, 80}; motor_operations.Enqueue(segment); From 17baad1d9076ffe791f939063b7d16e3f335fb94 Mon Sep 17 00:00:00 2001 From: Leonardo Romor Date: Sat, 29 Apr 2023 22:39:14 +0200 Subject: [PATCH 4/6] Improved comments describing interfaces --- src/motion-queue.h | 5 +++-- src/segment-queue.h | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/motion-queue.h b/src/motion-queue.h index 612d6cd..6a89189 100644 --- a/src/motion-queue.h +++ b/src/motion-queue.h @@ -119,8 +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 reset of the queue, - // even if motors are still moving. + // Immediately stop any motion and clear the internal queue. + // Returns true if the queue was successfully cleared, + // false if an internal error occurred. virtual bool Clear() = 0; }; diff --git a/src/segment-queue.h b/src/segment-queue.h index 7b6fd67..334f7ca 100644 --- a/src/segment-queue.h +++ b/src/segment-queue.h @@ -78,10 +78,10 @@ class SegmentQueue { // 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. + // Immediately stop any motion and clear the internal queue. // Current physical status will be lost. + // Returns true if the queue was successfully cleared, + // false if an internal error occurred. virtual bool Clear() = 0; }; From 7291bdd081a95d6cc54ff710d22c1a4ef56a84cc Mon Sep 17 00:00:00 2001 From: Leonardo Romor Date: Sat, 29 Apr 2023 23:32:40 +0200 Subject: [PATCH 5/6] Changed pru interface to support halting and restarting --- src/determine-print-stats.cc | 2 +- src/gcode-machine-control_test.cc | 2 +- src/gcode2ps.cc | 2 +- src/motion-queue-motor-operations.cc | 5 ++- src/motion-queue-motor-operations.h | 2 +- src/motion-queue-motor-operations_test.cc | 8 ++--- src/motion-queue.h | 9 +++--- src/planner_test.cc | 2 +- src/pru-hardware-interface.h | 8 +++++ src/pru-motion-queue.cc | 21 ++++++++----- src/pru-motion-queue_test.cc | 37 ++++++++++------------- src/segment-queue.h | 4 +-- src/sim-audio-out.h | 2 +- src/sim-firmware.h | 2 +- src/uio-pruss-interface.cc | 4 +++ 15 files changed, 59 insertions(+), 51 deletions(-) diff --git a/src/determine-print-stats.cc b/src/determine-print-stats.cc index 3342b25..6cfb697 100644 --- a/src/determine-print-stats.cc +++ b/src/determine-print-stats.cc @@ -117,7 +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; } + void HaltAndDiscard() final {} private: BeagleGPrintStats *const print_stats_; diff --git a/src/gcode-machine-control_test.cc b/src/gcode-machine-control_test.cc index e906716..95487ac 100644 --- a/src/gcode-machine-control_test.cc +++ b/src/gcode-machine-control_test.cc @@ -59,7 +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; } + void HaltAndDiscard() final {} int call_count_wait_queue_empty = 0; diff --git a/src/gcode2ps.cc b/src/gcode2ps.cc index ef1096c..3dd945b 100644 --- a/src/gcode2ps.cc +++ b/src/gcode2ps.cc @@ -1037,7 +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 HaltAndDiscard() final {} void SetExternalPosition(int motor, int pos) final { current_pos_[motor] = pos; if (pass_ == ProcessingStep::GenerateOutput) { diff --git a/src/motion-queue-motor-operations.cc b/src/motion-queue-motor-operations.cc index 7b7acee..a7856d6 100644 --- a/src/motion-queue-motor-operations.cc +++ b/src/motion-queue-motor-operations.cc @@ -337,11 +337,10 @@ bool MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &segment) { return ret; } -bool MotionQueueMotorOperations::Clear() { - const bool ret = backend_->Clear(); +void MotionQueueMotorOperations::HaltAndDiscard() { + backend_->HaltAndDiscard(); shadow_queue_->clear(); shadow_queue_->push_front({}); - return ret; } void MotionQueueMotorOperations::MotorEnable(bool on) { diff --git a/src/motion-queue-motor-operations.h b/src/motion-queue-motor-operations.h index b311f00..62741ec 100644 --- a/src/motion-queue-motor-operations.h +++ b/src/motion-queue-motor-operations.h @@ -37,7 +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; + void HaltAndDiscard() final; private: bool EnqueueInternal(const LinearSegmentSteps ¶m, diff --git a/src/motion-queue-motor-operations_test.cc b/src/motion-queue-motor-operations_test.cc index ec09da6..d9846d4 100644 --- a/src/motion-queue-motor-operations_test.cc +++ b/src/motion-queue-motor-operations_test.cc @@ -39,11 +39,10 @@ class MockMotionQueue final : public MotionQueue { return queue_size_; } - bool Clear() final { + void HaltAndDiscard() final { clear_calls_count++; remaining_loops_ = 0; queue_size_ = 0; - return true; } int clear_calls_count = 0; @@ -207,7 +206,8 @@ TEST(RealtimePosition, zero_loops_edge) { } // Clear motion queue motor operations. -// The physical status should be reset and motion_backend.Clear() called. +// The physical status should be reset and motion_backend.HaltAndDiscard() +// called. TEST(RealtimePosition, clear_queue) { HardwareMapping hw; MockMotionQueue motion_backend = MockMotionQueue(); @@ -224,7 +224,7 @@ TEST(RealtimePosition, clear_queue) { PhysicalStatus status; motor_operations.GetPhysicalStatus(&status); EXPECT_THAT(expected, ::testing::ContainerEq(status.pos_steps)); - EXPECT_TRUE(motor_operations.Clear()); + motor_operations.HaltAndDiscard(); EXPECT_EQ(motion_backend.clear_calls_count, 1); motor_operations.GetPhysicalStatus(&status); memset(expected, 0, sizeof(expected)); diff --git a/src/motion-queue.h b/src/motion-queue.h index 6a89189..aaae9f1 100644 --- a/src/motion-queue.h +++ b/src/motion-queue.h @@ -120,9 +120,7 @@ class MotionQueue { virtual int GetPendingElements(uint32_t *head_item_progress) = 0; // Immediately stop any motion and clear the internal queue. - // Returns true if the queue was successfully cleared, - // false if an internal error occurred. - virtual bool Clear() = 0; + virtual void HaltAndDiscard() = 0; }; // Standard implementation. @@ -142,12 +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; - bool Clear() final; + void HaltAndDiscard() final; private: bool Init(); void ClearPRUAbort(unsigned int idx); + void ClearRingBuffer(); HardwareMapping *const hardware_mapping_; PruHardwareInterface *const pru_interface_; @@ -167,7 +166,7 @@ class DummyMotionQueue final : public MotionQueue { if (head_item_progress) *head_item_progress = 0; return 1; } - bool Clear() final { return true; } + void HaltAndDiscard() final {} }; #endif // _BEAGLEG_MOTION_QUEUE_H_ diff --git a/src/planner_test.cc b/src/planner_test.cc index 71b2f0e..9a6a1e0 100644 --- a/src/planner_test.cc +++ b/src/planner_test.cc @@ -124,7 +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; } + void HaltAndDiscard() final {} int SegmentsCount() const { return collected_.size(); } const std::vector &segments() { return collected_; } diff --git a/src/pru-hardware-interface.h b/src/pru-hardware-interface.h index 940258b..4ec71c0 100644 --- a/src/pru-hardware-interface.h +++ b/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_ diff --git a/src/pru-motion-queue.cc b/src/pru-motion-queue.cc index 40b751b..91a3e76 100644 --- a/src/pru-motion-queue.cc +++ b/src/pru-motion-queue.cc @@ -177,9 +177,12 @@ void PRUMotionQueue::Shutdown(bool flush_queue) { MotorEnable(false); } -bool PRUMotionQueue::Clear() { - pru_interface_->Shutdown(); - return Init(); +void PRUMotionQueue::HaltAndDiscard() { + MotorEnable(false); + pru_interface_->Halt(); + ClearRingBuffer(); + queue_pos_ = 0; + pru_interface_->Restart(); } PRUMotionQueue::~PRUMotionQueue() {} @@ -192,6 +195,12 @@ 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; @@ -199,11 +208,7 @@ bool PRUMotionQueue::Init() { 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(); } diff --git a/src/pru-motion-queue_test.cc b/src/pru-motion-queue_test.cc index ec46e4a..22226fc 100644 --- a/src/pru-motion-queue_test.cc +++ b/src/pru-motion-queue_test.cc @@ -18,8 +18,6 @@ #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; @@ -28,17 +26,16 @@ struct MockPRUCommunication { 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; }); - } + MockPRUInterface() : execution_index_(QUEUE_LEN - 1) { mmap = NULL; } ~MockPRUInterface() override { free(mmap); } + bool Init() final { return true; } bool StartExecution() final { return true; } unsigned WaitEvent() final { return 1; } - MOCK_METHOD(bool, Init, (), ()); - MOCK_METHOD(bool, Shutdown, (), ()); + bool Shutdown() final { return true; } + + MOCK_METHOD(void, Halt, (), (final)); + MOCK_METHOD(void, Restart, (), (final)); bool AllocateSharedMem(void **pru_mmap, const size_t size) final { if (mmap != NULL) return true; @@ -70,7 +67,7 @@ class MockPRUInterface : public PruHardwareInterface { TEST(PruMotionQueue, status_init) { MotorsRegister absolute_pos_loops; - NiceMock pru_interface; + MockPRUInterface pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0); @@ -78,7 +75,7 @@ TEST(PruMotionQueue, status_init) { TEST(PruMotionQueue, single_exec) { MotorsRegister absolute_pos_loops; - NiceMock pru_interface; + MockPRUInterface pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -91,7 +88,7 @@ TEST(PruMotionQueue, single_exec) { TEST(PruMotionQueue, full_exec) { MotorsRegister absolute_pos_loops; - NiceMock pru_interface; + MockPRUInterface pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -104,7 +101,7 @@ TEST(PruMotionQueue, full_exec) { TEST(PruMotionQueue, single_exec_some_loops) { MotorsRegister absolute_pos_loops; - NiceMock pru_interface; + MockPRUInterface pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -119,7 +116,7 @@ TEST(PruMotionQueue, single_exec_some_loops) { TEST(PruMotionQueue, one_round_queue) { MotorsRegister absolute_pos_loops; - NiceMock pru_interface; + MockPRUInterface pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -141,7 +138,7 @@ TEST(PruMotionQueue, one_round_queue) { // Check the PRU is reset and no elements are pending. TEST(PruMotionQueue, clear_queue) { MotorsRegister absolute_pos_loops; - NiceMock pru_interface; + MockPRUInterface pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); @@ -157,18 +154,16 @@ TEST(PruMotionQueue, clear_queue) { 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_CALL(pru_interface, Halt()).Times(1); + EXPECT_CALL(pru_interface, Restart()).Times(1); } - EXPECT_TRUE(motion_backend.Clear()); + motion_backend.HaltAndDiscard(); EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0); } TEST(PruMotionQueue, exec_index_lt_queue_pos) { MotorsRegister absolute_pos_loops; - NiceMock pru_interface; + MockPRUInterface pru_interface; HardwareMapping hmap = HardwareMapping(); PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface); diff --git a/src/segment-queue.h b/src/segment-queue.h index 334f7ca..6aebedf 100644 --- a/src/segment-queue.h +++ b/src/segment-queue.h @@ -80,9 +80,7 @@ class SegmentQueue { // Immediately stop any motion and clear the internal queue. // Current physical status will be lost. - // Returns true if the queue was successfully cleared, - // false if an internal error occurred. - virtual bool Clear() = 0; + virtual void HaltAndDiscard() = 0; }; #endif // _BEAGLEG_MOTOR_OPERATIONS_H_ diff --git a/src/sim-audio-out.h b/src/sim-audio-out.h index 0f6951e..0bfcaa0 100644 --- a/src/sim-audio-out.h +++ b/src/sim-audio-out.h @@ -32,7 +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; } + void HaltAndDiscard() final {} int GetPendingElements(uint32_t *head_item_progress) final { if (head_item_progress) *head_item_progress = 0; return 1; diff --git a/src/sim-firmware.h b/src/sim-firmware.h index b5091b1..a4e67c0 100644 --- a/src/sim-firmware.h +++ b/src/sim-firmware.h @@ -31,7 +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; } + void HaltAndDiscard() final {} int GetPendingElements(uint32_t *head_item_progress) final { if (head_item_progress) *head_item_progress = 0; return 1; diff --git a/src/uio-pruss-interface.cc b/src/uio-pruss-interface.cc index 081105e..25a97af 100644 --- a/src/uio-pruss-interface.cc +++ b/src/uio-pruss-interface.cc @@ -84,6 +84,10 @@ unsigned UioPrussInterface::WaitEvent() { return num_events; } +void UioPrussInterface::Halt() { prussdrv_pru_disable(PRU_NUM); } + +void UioPrussInterface::Restart() { prussdrv_pru_enable_at(PRU_NUM, 0); } + bool UioPrussInterface::Shutdown() { prussdrv_pru_disable(PRU_NUM); prussdrv_exit(); From 6f01af1fe52a7c077dbaf47a521cccb715a1e625 Mon Sep 17 00:00:00 2001 From: Leonardo Romor Date: Sat, 3 Jun 2023 15:20:39 +0200 Subject: [PATCH 6/6] Improved test readability. --- src/motion-queue-motor-operations_test.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/motion-queue-motor-operations_test.cc b/src/motion-queue-motor-operations_test.cc index d9846d4..920ac69 100644 --- a/src/motion-queue-motor-operations_test.cc +++ b/src/motion-queue-motor-operations_test.cc @@ -224,10 +224,12 @@ TEST(RealtimePosition, clear_queue) { 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); - motor_operations.GetPhysicalStatus(&status); + memset(expected, 0, sizeof(expected)); + motor_operations.GetPhysicalStatus(&status); EXPECT_THAT(expected, ::testing::ContainerEq(status.pos_steps)); }