Added command M181.

src/gcode-machine-control.cc

@@ -122,6 +122,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);
+  void set_lookahead(int size);
   void handle_M105();
   // Parse GCode spindle M3/M4 block.
   const char *set_spindle_on(bool is_ccw, const char *);
@@ -450,6 +451,22 @@ bool GCodeMachineControl::Impl::check_for_pause() {
   return hardware_mapping_->TestPauseSwitch();
 }
 
+void GCodeMachineControl::Impl::set_lookahead(int size) {
+  // It is necessary to have a fully stopped machine to change lookahead.
+  // This is to avoid introducing a delay between the requested change and
+  // actual machine behavior with previously enqueued segments.
+  motor_ops_->WaitQueueEmpty();
+  const bool set_max = size < 0;
+  size = (size <= 0) ? -1 : size;
+  const bool set = planner_->SetLookAhead(&size);
+  if (set_max) {
+    planner_->SetLookAhead(&size);
+  }
+  if (!set) {
+    mprintf("// ERROR: allowed range 1 <= S <= %d\n", size);
+  }
+}
+
 void GCodeMachineControl::Impl::handle_M105() {
   mprintf("// ");
   for (int chan = 0; chan < 8; chan++) {
@@ -534,6 +551,14 @@ 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: {
+    const char *const after_pair = parser_->ParsePair(remaining, &letter, &value, msg_stream_);
+    if (remaining == NULL) set_lookahead(-1);
+    else if (letter == 'S') set_lookahead((unsigned)value);
+    else break;
+    remaining = after_pair;
+    break;
+  }
   default:
     mprintf("// BeagleG: didn't understand ('%c', %d, '%s')\n",
             letter, code, remaining);

src/gcode-machine-control_test.cc

@@ -35,6 +35,9 @@ static void init_test_config(struct MachineControlConfig *c,
 }
 
 namespace {
+// Record Segment queue calls we care avout.
+enum { CALL_wait_queue_empty, NUM_COUNTED_CALLS };
+
 class MockMotorOps final : public SegmentQueue {
  public:
   explicit MockMotorOps(const LinearSegmentSteps *expected)
@@ -54,12 +57,17 @@ class MockMotorOps final : public SegmentQueue {
     return true;
   }
 
+  void WaitQueueEmpty() final { Count(CALL_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[NUM_COUNTED_CALLS] = {};
+
  private:
+  void Count(int what) { call_count[what]++; }
+
   // Helpers to compare and print MotorMovements.
   static void PrintMovement(const char *msg,
                             const struct LinearSegmentSteps *m) {
@@ -94,11 +102,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_);
+    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,8 +118,8 @@ class Harness {
     return machine_control->ParseEventReceiver();
   }
 
-  MockMotorOps expect_motor_ops_;
-  HardwareMapping hardware_;
+  MockMotorOps expect_motor_ops;
+  HardwareMapping hardware;
   GCodeMachineControl *machine_control;
 };
 
@@ -219,6 +227,25 @@ TEST(GCodeMachineControlTest, straight_segments_speed_change) {
   harness.gcode_emit()->motors_enable(false);  // finish movement.
 }
 
+TEST(GCodeMachineControlTest, m181_stops_and_set_planner_queue) {
+  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());
+
+  harness.gcode_emit()->gcode_start(&parser);
+  parser.ParseBlock("M181 S100", nullptr);
+  harness.gcode_emit()->gcode_finished(true);
+
+  // Changing lookahead should enforce a full stop.
+  EXPECT_EQ(harness.expect_motor_ops.call_count[CALL_wait_queue_empty], 1);
+}
+
 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

@@ -233,6 +233,7 @@ class Planner::Impl {
   void GetCurrentPosition(AxesRegister *pos);
   int DirectDrive(GCodeParserAxis axis, float distance, float v0, float v1);
   void SetExternalPosition(GCodeParserAxis axis, float pos);
+  bool SetLookAhead(int *size);
 
  private:
   const struct MachineControlConfig *const cfg_;
@@ -277,6 +278,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.
+  int 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
@@ -463,7 +467,7 @@ bool Planner::Impl::issue_motor_move_if_possible(
 
   // We require a slot to be always present.
   if (num_segments == 0 &&
-      (planning_buffer_.size() == planning_buffer_.capacity()))
+      (planning_buffer_.size() == (unsigned)lookahead_size_))
     ++num_segments;
 
   // No segments to enqueue, skip.
@@ -934,6 +938,23 @@ void Planner::Impl::SetExternalPosition(GCodeParserAxis axis, float pos) {
   }
 }
 
+bool Planner::Impl::SetLookAhead(int *size) {
+  if (size == nullptr) {
+    return false;
+  }
+  const int value = *size;
+  if (value == 0) {
+    *size = lookahead_size_;
+    return false;
+  }
+  if (value < 0 || value > PLANNING_BUFFER_CAPACITY) {
+    *size = PLANNING_BUFFER_CAPACITY;
+    return false;
+  }
+  lookahead_size_ = value;
+  return true;
+}
+
 // -- public interface
 
 Planner::Planner(const MachineControlConfig *config,
@@ -960,3 +981,5 @@ 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); }

src/planner.h

@@ -64,6 +64,16 @@ class Planner {
   // Precondition: BringPathToHalt() had been called before.
   void SetExternalPosition(GCodeParserAxis axis, float pos);
 
+  // Set/get internal planning buffer size. Trade-off between
+  // command sent - executed motion latency and maximum achievable speed.
+  // Set the internal planning queue to the value pointed by size.
+  // If size is nullptr, this is a no-op and false is returned.
+  // Special size 0 returns false and sets size to the current value.
+  // Negative size values, or size values above the internal maximum
+  // will not change the internal state and set size to the maximum value
+  // instead.
+  bool SetLookAhead(int *size);
+
  private:
   class Impl;
   Impl *const impl_;