pbio/test/trajectory: Fix skipping start of trajectory.

When walking the trajectory we have to skip the first point because there is no previous point to compare to. We already skipped 0, but there can be new matching starting points when a long trajectory rebases itself.

This was discovered when adding a missing assert for positive time, which is added in this commit as well.

Author: laurensvalk

lib/pbio/src/trajectory.c

@@ -629,6 +629,7 @@ void pbio_trajectory_get_last_vertex(pbio_trajectory_t *trj, uint32_t time_ref,
 
     // Relative time within ongoing maneuver.
     int32_t time = TO_TRAJECTORY_TIME(time_ref - trj->start.time);
+    assert_time(time);
 
     // Find which section of the ongoing maneuver we were in, and take
     // corresponding segment starting point. Acceleration is undefined but not

lib/pbio/test/src/test_trajectory.c

@@ -90,12 +90,18 @@ static void walk_trajectory(pbio_trajectory_t *trj) {
 
     // Loop over all trajectory points and assert results.
     const uint32_t increment = 50;
-    for (uint32_t t = increment; t < duration; t += increment) {
+    for (uint32_t t = 0; t < duration; t += increment) {
 
         // Get current reference.
         uint32_t now = t + time_start;
         pbio_trajectory_get_reference(trj, now, &ref_now);
 
+        // Skip if current time equals start time. Then there is no previous
+        // sample to compare to.
+        if (now == trj->start.time) {
+            continue;
+        }
+
         // Current time should match
         tt_want_int_op(ref_now.time, ==, now);