diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 4f3ef10..edc6e79 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -43,8 +43,8 @@ void Motion::StallGuardReset(Axis axis) {
 void Motion::Home(Axis axis, bool direction) {
 }
 
-void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {
-    if (axisData[axis].ctrl.PlanMoveTo(pos, feedrate)) {
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) {
+    if (axisData[axis].ctrl.PlanMoveTo(pos, feed_rate, end_rate)) {
         // move was queued, prepare the axis
         if (!axisData[axis].enabled)
             SetEnabled(axis, true);
diff --git a/src/modules/motion.h b/src/modules/motion.h
index cd78ce2..05d6f10 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -105,15 +105,17 @@ public:
     /// feedrate. Moves can only be enqueued if the axis is not Full().
     /// @param axis axis affected
     /// @param pos target position
-    /// @param feedrate maximum feedrate
-    void PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate);
+    /// @param feed_rate maximum feedrate
+    /// @param end_rate endding feedrate (may not be reached!)
+    void PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate = 0);
 
     /// Enqueue a single axis move using PlanMoveTo, but using AxisUnit. The Axis needs to
     /// be supplied as the first template argument: PlanMoveTo<axis>(pos, rate).
     /// @see PlanMoveTo, unitToSteps
     template <Axis A>
-    constexpr void PlanMoveTo(AxisUnit<pos_t, A, Lenght> pos, AxisUnit<steps_t, A, Speed> feedrate) {
-        PlanMoveTo(A, pos.v, feedrate.v);
+    constexpr void PlanMoveTo(AxisUnit<pos_t, A, Lenght> pos,
+        AxisUnit<steps_t, A, Speed> feed_rate, AxisUnit<steps_t, A, Speed> end_rate = { 0 }) {
+        PlanMoveTo(A, pos.v, feed_rate.v, end_rate.v);
     }
 
     /// Enqueue a single axis move using PlanMoveTo, but using physical units. The Axis
@@ -121,27 +123,30 @@ public:
     /// @see PlanMoveTo, unitToSteps
     template <Axis A, config::UnitBase B>
     constexpr void PlanMoveTo(config::Unit<long double, B, Lenght> pos,
-        config::Unit<long double, B, Speed> feedrate) {
+        config::Unit<long double, B, Speed> feed_rate, config::Unit<long double, B, Speed> end_rate = { 0 }) {
         PlanMoveTo<A>(
             unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(pos),
-            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feedrate));
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(end_rate));
     }
 
     /// Enqueue a single axis move in steps starting and ending at zero speed with maximum
     /// feedrate. Moves can only be enqueued if the axis is not Full().
     /// @param axis axis affected
     /// @param delta relative to current position
-    /// @param feedrate maximum feedrate
-    void PlanMove(Axis axis, pos_t delta, steps_t feedrate) {
-        PlanMoveTo(axis, Position(axis) + delta, feedrate);
+    /// @param feed_rate maximum feedrate
+    /// @param end_rate endding feedrate (may not be reached!)
+    void PlanMove(Axis axis, pos_t delta, steps_t feed_rate, steps_t end_rate = 0) {
+        PlanMoveTo(axis, Position(axis) + delta, feed_rate, end_rate);
     }
 
     /// Enqueue a single axis move using PlanMove, but using AxisUnit. The Axis needs to
     /// be supplied as the first template argument: PlanMove<axis>(pos, rate).
     /// @see PlanMove, unitToSteps
     template <Axis A>
-    constexpr void PlanMove(AxisUnit<pos_t, A, Lenght> delta, AxisUnit<steps_t, A, Speed> feedrate) {
-        PlanMove(A, delta.v, feedrate.v);
+    constexpr void PlanMove(AxisUnit<pos_t, A, Lenght> delta,
+        AxisUnit<steps_t, A, Speed> feed_rate, AxisUnit<steps_t, A, Speed> end_rate = { 0 }) {
+        PlanMove(A, delta.v, feed_rate.v, end_rate.v);
     }
 
     /// Enqueue a single axis move using PlanMove, but using physical units. The Axis needs to
@@ -149,10 +154,11 @@ public:
     /// @see PlanMove, unitToSteps
     template <Axis A, config::UnitBase B>
     constexpr void PlanMove(config::Unit<long double, B, Lenght> delta,
-        config::Unit<long double, B, Speed> feedrate) {
+        config::Unit<long double, B, Speed> feed_rate, config::Unit<long double, B, Speed> end_rate = { 0 }) {
         PlanMove<A>(
             unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(delta),
-            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feedrate));
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(end_rate));
     }
 
     /// @returns head position of an axis (last enqueued position)
diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp
index a48a16f..0510316 100644
--- a/src/modules/pulse_gen.cpp
+++ b/src/modules/pulse_gen.cpp
@@ -66,8 +66,10 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e
                 accelerate_steps += acceleration_x2;
             accelerate_steps /= acceleration_x4;
             accelerate_steps += (block->steps >> 1);
-            if (accelerate_steps > block->steps)
+            if (accelerate_steps > block->steps) {
                 accelerate_steps = block->steps;
+                final_rate = sqrt(acceleration_x2 * accelerate_steps + initial_rate_sqr);
+            }
         } else {
             decelerate_steps = initial_rate_sqr - final_rate_sqr;
             if (block->steps & 1)
@@ -86,7 +88,7 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e
     block->final_rate = final_rate;
 }
 
-bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) {
+bool PulseGen::PlanMoveTo(pos_t target, rate_t feed_rate, rate_t end_rate) {
     // Prepare to set up new block
     if (block_index.full())
         return false;
@@ -107,6 +109,13 @@ bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) {
     block->acceleration = acceleration;
     block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / config::stepTimerFrequencyDivider));
 
+    // Simplified forward jerk: do not handle reversals
+    steps_t entry_speed;
+    if (feed_rate >= last_rate)
+        entry_speed = last_rate + min(feed_rate - last_rate, max_jerk);
+    else
+        entry_speed = last_rate - min(last_rate - feed_rate, max_jerk);
+
     // Perform the trapezoid calculations
     CalculateTrapezoid(block, entry_speed, end_rate);
     last_rate = block->final_rate;
diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h
index 1322b05..7012342 100644
--- a/src/modules/pulse_gen.h
+++ b/src/modules/pulse_gen.h
@@ -39,9 +39,10 @@ public:
     /// Enqueue a single move in steps starting and ending at zero speed with maximum
     /// feedrate. Moves can only be enqueued if the axis is not Full().
     /// @param pos target position
-    /// @param feedrate maximum feedrate
+    /// @param feed_rate maximum feedrate
+    /// @param end_rate endding feedrate (may not be reached!)
     /// @returns true if the move has been enqueued
-    bool PlanMoveTo(pos_t pos, steps_t feedrate);
+    bool PlanMoveTo(pos_t pos, rate_t feed_rate, rate_t end_rate = 0);
 
     /// Stop whatever moves are being done
     /// @param halt When true, also abruptly stop axis movement.
diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp
index 4d9f506..0dacd75 100644
--- a/tests/unit/logic/stubs/stub_motion.cpp
+++ b/tests/unit/logic/stubs/stub_motion.cpp
@@ -32,7 +32,7 @@ void Motion::StallGuardReset(Axis axis) {
     axes[axis].stallGuard = false;
 }
 
-void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) {
     axes[axis].targetPos = pos;
 }