diff --git a/src/control.h b/src/control.h index 4f46259..1b6bf4f 100644 --- a/src/control.h +++ b/src/control.h @@ -134,7 +134,6 @@ INLINE void vehicle_hard_right(trial *t); INLINE timestamp getcurts(trial *t); INLINE double fixangle(double a); -INLINE double fixdirection(double a); /* needed by parser */ telemetry* telemetry_new(); @@ -232,15 +231,9 @@ INLINE timestamp getcurts(trial *t) { } INLINE double fixangle(double a) { - while (a < -180) a += 360; + while (a <= -180) a += 360; while (a > 180) a -= 360; return a; } -INLINE double fixdirection(double a) { - while (a < 0) a += 360; - while (a > 360) a -= 360; - return a; -} - #endif diff --git a/src/lookahead.c b/src/lookahead.c index 43d9e73..ea16c07 100644 --- a/src/lookahead.c +++ b/src/lookahead.c @@ -85,10 +85,10 @@ void sim_update(trial* t){ radar_dgl(t,tm->ts); /*calculate misspredict*/ - fprintf(stderr, "Miss predict: dx=%f, dy=%f, ddir=%f, steps=%u\n", + fprintf(stderr, "Miss predict: dx=%f, dy=%f, ddir=%f (%f %f), steps=%u\n", t->sim.tm.vehicle.x - t->vehicle.x, t->sim.tm.vehicle.y - t->vehicle.y, - t->sim.tm.vehicle.dir - t->vehicle.dir, + fixangle(t->sim.tm.vehicle.dir - t->vehicle.dir), t->sim.steps); // fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir); fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed); @@ -158,7 +158,7 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat after->y = tmp.y; after->speed = tmp.speed; after->w = tmp.w; - after->dir = fixdirection(tmp.dir); + after->dir = fixangle(tmp.dir); return 0; }