pbio/test/trajectory: Fix test for small speed reductions.

Author: laurensvalk

lib/pbio/test/src/test_trajectory.c

@@ -354,13 +354,14 @@ static void test_position_trajectory(void *env) {
             tt_want_int_op(trj.w3, ==, 0);
         }
 
-        // Initial speed may now be bounded. Verify that the sign is the same.
+        // Initial speed may now be bounded. Verify that the sign is the same,
+        // or more precisely, not opposite. First we get reference to compare.
         pbio_trajectory_reference_t ref;
         pbio_trajectory_get_reference(&trj, command.time_start, &ref);
         get_position_command(i, &command);
-
-        // FIXME: This should pass even for t1 == 0.
-        if (trj.t1 > 0) {
+        int32_t speed_difference = pbio_int_math_abs(ref.speed - command.speed_start);
+        // We avoid doing this check for nonzero but negligible speed differences.
+        if (!speed_difference || speed_difference > 2 * MDEG_PER_DEG) {
             tt_want(pbio_int_math_sign_not_opposite(ref.speed, command.speed_start));
         }