pbio/trajectory: fix duration of patched maneuver
This was broken by
Because the starting point is shifted to the past to intersect with ongoing maneuver, we must compensate that difference in the duration. That way, the end time is indeed the current time plus the duration.
@@ -66,7 +66,7 @@ static pbio_error_t pbio_trajectory_patch(pbio_trajectory_t *ref, bool time_base
// Now we can make the new trajectory with a starting point coincident
// with a point on the existing trajectory
if (time_based) {
- return pbio_trajectory_make_time_based(ref, t0, duration, th0, th0_ext, w0, wt, wmax, a);
+ return pbio_trajectory_make_time_based(ref, t0, duration + (nominal.t0 - t0), th0, th0_ext, w0, wt, wmax, a);
}
else {
return pbio_trajectory_make_angle_based(ref, t0, th0, th3, w0, wt, wmax, a);
