fix
This commit is contained in:
parent
66d5b5f065
commit
070a6a1417
@ -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
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user