3 files changed, 21 insertions, 0 deletions
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 97493355..c2ad802a 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -83,6 +83,8 @@ defs_trapq = """
     struct trapq *trapq_alloc(void);
     void trapq_free(struct trapq *tq);
     void trapq_finalize_moves(struct trapq *tq, double print_time);
+    void trapq_set_position(struct trapq *tq, double print_time
+        , double pos_x, double pos_y, double pos_z);
     int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
         , double start_time, double end_time);
 """
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index eaeb5949..2d4c2093 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -197,6 +197,23 @@ trapq_finalize_moves(struct trapq *tq, double print_time)
     }
 }
 
+// Note a position change in the trapq history
+void __visible
+trapq_set_position(struct trapq *tq, double print_time
+                   , double pos_x, double pos_y, double pos_z)
+{
+    // Flush all moves from trapq
+    trapq_finalize_moves(tq, NEVER_TIME);
+
+    // Add a marker to the trapq history
+    struct move *m = move_alloc();
+    m->print_time = print_time;
+    m->start_pos.x = pos_x;
+    m->start_pos.y = pos_y;
+    m->start_pos.z = pos_z;
+    list_add_head(&m->node, &tq->history);
+}
+
 // Return history of movement queue
 int __visible
 trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h
index 154fe04d..dbde8d3e 100644
--- a/klippy/chelper/trapq.h
+++ b/klippy/chelper/trapq.h
@@ -44,6 +44,8 @@ void trapq_free(struct trapq *tq);
 void trapq_check_sentinels(struct trapq *tq);
 void trapq_add_move(struct trapq *tq, struct move *m);
 void trapq_finalize_moves(struct trapq *tq, double print_time);
+void trapq_set_position(struct trapq *tq, double print_time
+                        , double pos_x, double pos_y, double pos_z);
 int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
                       , double start_time, double end_time);