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 timestamp getcurts(trial *t);
|
||||||
INLINE double fixangle(double a);
|
INLINE double fixangle(double a);
|
||||||
INLINE double fixdirection(double a);
|
|
||||||
|
|
||||||
/* needed by parser */
|
/* needed by parser */
|
||||||
telemetry* telemetry_new();
|
telemetry* telemetry_new();
|
||||||
@ -232,15 +231,9 @@ INLINE timestamp getcurts(trial *t) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
INLINE double fixangle(double a) {
|
INLINE double fixangle(double a) {
|
||||||
while (a < -180) a += 360;
|
while (a <= -180) a += 360;
|
||||||
while (a > 180) a -= 360;
|
while (a > 180) a -= 360;
|
||||||
return a;
|
return a;
|
||||||
}
|
}
|
||||||
|
|
||||||
INLINE double fixdirection(double a) {
|
|
||||||
while (a < 0) a += 360;
|
|
||||||
while (a > 360) a -= 360;
|
|
||||||
return a;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@ -85,10 +85,10 @@ void sim_update(trial* t){
|
|||||||
radar_dgl(t,tm->ts);
|
radar_dgl(t,tm->ts);
|
||||||
|
|
||||||
/*calculate misspredict*/
|
/*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.x - t->vehicle.x,
|
||||||
t->sim.tm.vehicle.y - t->vehicle.y,
|
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);
|
t->sim.steps);
|
||||||
// fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir);
|
// 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);
|
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->y = tmp.y;
|
||||||
after->speed = tmp.speed;
|
after->speed = tmp.speed;
|
||||||
after->w = tmp.w;
|
after->w = tmp.w;
|
||||||
after->dir = fixdirection(tmp.dir);
|
after->dir = fixangle(tmp.dir);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user