Added command M181.

src/gcode-machine-control.cc

@@ -75,6 +75,9 @@ class GCodeMachineControl::Impl final : public GCodeParser::EventReceiver {
   HomingState GetHomeStatus();
   bool GetMotorsEnabled();
   void GetCurrentPosition(AxesRegister *pos);
+  bool SetLookahead(int size) { return planner_->SetLookahead(size); }
+  int Lookahead() const { return planner_->Lookahead(); }
+  int GetMaxLookahead() const { return planner_->GetMaxLookahead(); }
 
   // -- GCodeParser::Events interface implementation --
   void gcode_start(GCodeParser *parser) final;
@@ -122,6 +125,7 @@ class GCodeMachineControl::Impl final : public GCodeParser::EventReceiver {
                     int max_steps);
   void home_axis(enum GCodeParserAxis axis);
   void set_output_flags(HardwareMapping::NamedOutput out, bool is_on);
+  const char *handle_set_lookahead(const char *);
   void handle_M105();
   // Parse GCode spindle M3/M4 block.
   const char *set_spindle_on(bool is_ccw, const char *);
@@ -450,6 +454,35 @@ bool GCodeMachineControl::Impl::check_for_pause() {
   return hardware_mapping_->TestPauseSwitch();
 }
 
+const char *GCodeMachineControl::Impl::handle_set_lookahead(
+  const char *remaining) {
+  char letter;
+  float value;
+  const char *const after_pair =
+    parser_->ParsePair(remaining, &letter, &value, msg_stream_);
+  const int max_lookahead = GetMaxLookahead();
+
+  // No argument, set lookahead to max.
+  if (after_pair == NULL) {
+    motor_ops_->WaitQueueEmpty();
+    const bool is_set = SetLookahead(max_lookahead);
+    assert(is_set);
+    return remaining;
+  }
+
+  // Unrecognized pair.
+  if (letter != 'S') return after_pair;
+  const int lookahead = (int)value;
+  if (lookahead <= 0 || lookahead > max_lookahead) {
+    mprintf("// ERROR: allowed range 1 <= S <= %d\n", max_lookahead);
+    return after_pair;
+  }
+  motor_ops_->WaitQueueEmpty();
+  const bool is_set = SetLookahead(lookahead);
+  assert(is_set);
+  return after_pair;
+}
+
 void GCodeMachineControl::Impl::handle_M105() {
   mprintf("// ");
   for (int chan = 0; chan < 8; chan++) {
@@ -534,6 +567,9 @@ const char *GCodeMachineControl::Impl::special_commands(char letter,
   case 119: mprint_endstop_status(); break;
   case 120: pause_enabled_ = true; break;
   case 121: pause_enabled_ = false; break;
+  case 181:
+    remaining = handle_set_lookahead(remaining);
+    break;
   default:
     mprintf("// BeagleG: didn't understand ('%c', %d, '%s')\n",
             letter, code, remaining);
@@ -1079,3 +1115,13 @@ GCodeParser::EventReceiver *GCodeMachineControl::ParseEventReceiver() {
 void GCodeMachineControl::SetMsgOut(FILE *msg_stream) {
   impl_->set_msg_stream(msg_stream);
 }
+
+bool GCodeMachineControl::SetLookahead(int size) {
+  return impl_->SetLookahead(size);
+}
+
+int GCodeMachineControl::Lookahead() const { return impl_->Lookahead(); }
+
+int GCodeMachineControl::GetMaxLookahead() const {
+  return impl_->GetMaxLookahead();
+}

src/gcode-machine-control.h

@@ -123,6 +123,18 @@ class GCodeMachineControl {
   // Can only be called in the same thread that also handles gcode updates.
   void GetCurrentPosition(AxesRegister *pos);
 
+  // Set the internal planner queue size.
+  // Used to trade-off between command sent - executed motion latency
+  // and maximum achievable speed.
+  // Requires 0 < size <= GetMaxLookahead(), returns true on success.
+  bool SetLookahead(int size);
+
+  // Retrieve the internal planning queue size.
+  int Lookahead() const;
+
+  // Get the maximum allowed lookahead size.
+  int GetMaxLookahead() const;
+
  private:
   class Impl;
 

src/gcode-machine-control_test.cc

@@ -54,11 +54,14 @@ class MockMotorOps final : public SegmentQueue {
     return true;
   }
 
+  void WaitQueueEmpty() final { call_count_wait_queue_empty++; }
+
   void MotorEnable(bool on) final {}
-  void WaitQueueEmpty() final {}
   bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
   void SetExternalPosition(int axis, int steps) final {}
 
+  int call_count_wait_queue_empty = 0;
+
  private:
   // Helpers to compare and print MotorMovements.
   static void PrintMovement(const char *msg,
@@ -94,11 +97,11 @@ class Harness {
   // Initialize harness with the expected sequence of motor movements
   // and return the callback struct to receive simulated gcode calls.
   explicit Harness(const LinearSegmentSteps *expected)
-      : expect_motor_ops_(expected) {
+      : expect_motor_ops(expected) {
     struct MachineControlConfig config;
     init_test_config(&config, &hardware_);
     machine_control =
-      GCodeMachineControl::Create(config, &expect_motor_ops_, &hardware_,
+      GCodeMachineControl::Create(config, &expect_motor_ops, &hardware_,
                                   nullptr,   // spindle
                                   nullptr);  // msg-stream
     assert(machine_control != nullptr);
@@ -110,9 +113,11 @@ class Harness {
     return machine_control->ParseEventReceiver();
   }
 
-  MockMotorOps expect_motor_ops_;
-  HardwareMapping hardware_;
+  MockMotorOps expect_motor_ops;
   GCodeMachineControl *machine_control;
+
+ private:
+  HardwareMapping hardware_;
 };
 
 TEST(GCodeMachineControlTest, initial_feedrate_not_set) {
@@ -219,6 +224,67 @@ TEST(GCodeMachineControlTest, straight_segments_speed_change) {
   harness.gcode_emit()->motors_enable(false);  // finish movement.
 }
 
+TEST(GCodeMachineControlTest, m181_stops_and_sets_lookahead) {
+  static const struct LinearSegmentSteps expected[] = {
+    {0.0, 0.0, END_SENTINEL, {}},
+  };
+  Harness harness(expected);
+  GCodeParser::Config config;
+  GCodeParser::Config::ParamMap parameters;
+  config.parameters = &parameters;
+  GCodeParser parser = GCodeParser(config, harness.gcode_emit());
+  EXPECT_EQ(0, parser.error_count());
+
+  // Machine should accept new value and stop.
+  harness.gcode_emit()->gcode_start(&parser);
+  parser.ParseBlock("M181 S100", nullptr);
+
+  // Changing lookahead should enforce a full stop.
+  EXPECT_EQ(harness.expect_motor_ops.call_count_wait_queue_empty, 1);
+  EXPECT_EQ(100, harness.machine_control->Lookahead());
+
+  // Machine should set maximum value.
+  parser.ParseBlock("M181", nullptr);
+  EXPECT_EQ(harness.expect_motor_ops.call_count_wait_queue_empty, 2);
+  EXPECT_EQ(harness.machine_control->GetMaxLookahead(),
+            harness.machine_control->Lookahead());
+
+  // Machine should not change value and not stop.
+  parser.ParseBlock("M181 S-1", nullptr);
+  EXPECT_EQ(harness.expect_motor_ops.call_count_wait_queue_empty, 2);
+  EXPECT_EQ(harness.machine_control->GetMaxLookahead(),
+            harness.machine_control->Lookahead());
+  harness.gcode_emit()->gcode_finished(true);
+}
+
+TEST(GCodeMachineControlTest, set_get_lookahead) {
+  static const struct LinearSegmentSteps expected[] = {
+    {0.0, 0.0, END_SENTINEL, {}},
+  };
+  Harness harness(expected);
+  GCodeParser::Config config;
+  GCodeParser::Config::ParamMap parameters;
+  config.parameters = &parameters;
+  GCodeParser parser = GCodeParser(config, harness.gcode_emit());
+
+  const int max_lookahead = harness.machine_control->GetMaxLookahead();
+
+  // Initial value shall be max.
+  EXPECT_EQ(max_lookahead, harness.machine_control->Lookahead());
+
+  // Negative value, shall fail.
+  EXPECT_FALSE(harness.machine_control->SetLookahead(-1));
+  EXPECT_EQ(max_lookahead, harness.machine_control->Lookahead());
+
+  // Greater than max, shall fail.
+  EXPECT_FALSE(harness.machine_control->SetLookahead(max_lookahead + 1));
+  EXPECT_EQ(max_lookahead, harness.machine_control->Lookahead());
+
+  // Ok value.
+  EXPECT_TRUE(harness.machine_control->SetLookahead(max_lookahead - 1));
+  EXPECT_EQ(max_lookahead - 1, harness.machine_control->Lookahead());
+}
+
 int main(int argc, char *argv[]) {
   Log_init("/dev/stderr");
   ::testing::InitGoogleTest(&argc, argv);

src/gcode-parser/gcode-parser_test.cc

@@ -1035,6 +1035,13 @@ TEST(GCodeParserTest, WhileLoop) {
   EXPECT_EQ(1024, counter.get_parameter(2));
 }
 
+TEST(GCodeParserTest, Unprocessed) {
+  ParseTester counter;
+
+  EXPECT_TRUE(counter.TestParseLine("M181 S300"));
+  EXPECT_EQ(1, counter.call_count[CALL_unprocessed]);
+}
+
 int main(int argc, char *argv[]) {
   ::testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();

src/planner.cc

@@ -234,6 +234,14 @@ class Planner::Impl {
   int DirectDrive(GCodeParserAxis axis, float distance, float v0, float v1);
   void SetExternalPosition(GCodeParserAxis axis, float pos);
 
+  bool SetLookahead(int size) {
+    if (size <= 0 || size > GetMaxLookahead()) return false;
+    lookahead_size_ = size;
+    return true;
+  }
+  int Lookahead() const { return lookahead_size_; }
+  static constexpr int GetMaxLookahead() { return PLANNING_BUFFER_CAPACITY; }
+
  private:
   const struct MachineControlConfig *const cfg_;
   HardwareMapping *const hardware_mapping_;
@@ -277,6 +285,9 @@ class Planner::Impl {
   // segments and update only the last deceleration ramp.
   int num_segments_ready_ = 0;
 
+  // Number of maximum planning steps allow to enqueue.
+  size_t lookahead_size_ = PLANNING_BUFFER_CAPACITY;
+
   // Pre-calculated per axis limits in steps, steps/s, steps/s^2
   // All arrays are indexed by axis.
   AxesRegister max_axis_speed_;  // max travel speed hz
@@ -461,10 +472,11 @@ bool Planner::Impl::issue_motor_move_if_possible(
   fprintf(stderr, "]\n");
 #endif
 
-  // We require a slot to be always present.
-  if (num_segments == 0 &&
-      (planning_buffer_.size() == planning_buffer_.capacity()))
-    ++num_segments;
+  // Count the amount of segments that are extra the lookahead size.
+  const int segments_extra = planning_buffer_.size() - lookahead_size_;
+  if (num_segments == 0 && (segments_extra >= 0))
+    num_segments =
+      segments_extra + 1;  // Always pop one free slot for the next.
 
   // No segments to enqueue, skip.
   if (!num_segments) return ret;
@@ -960,3 +972,12 @@ int Planner::DirectDrive(GCodeParserAxis axis, float distance, float v0,
 void Planner::SetExternalPosition(GCodeParserAxis axis, float pos) {
   impl_->SetExternalPosition(axis, pos);
 }
+
+bool Planner::SetLookahead(int size) { return impl_->SetLookahead(size); }
+
+int Planner::Lookahead() const { return impl_->Lookahead(); }
+
+// Get the maximum allowed lookahead size.
+int Planner::GetMaxLookahead() const {
+  return Planner::Impl::GetMaxLookahead();
+}

src/planner.h

@@ -64,6 +64,18 @@ class Planner {
   // Precondition: BringPathToHalt() had been called before.
   void SetExternalPosition(GCodeParserAxis axis, float pos);
 
+  // Set the internal planner queue size.
+  // Used to trade-off between command sent - executed motion latency
+  // and maximum achievable speed.
+  // Requires 0 < size <= GetMaxLookahead(), returns true on success.
+  bool SetLookahead(int size);
+
+  // Retrieve the internal planning queue size.
+  int Lookahead() const;
+
+  // Get the maximum allowed lookahead size.
+  int GetMaxLookahead() const;
+
  private:
   class Impl;
   Impl *const impl_;