brought some infrastructure for better simulation in place

This commit is contained in:
Johannes Reinhardt 2008-07-13 22:52:08 +02:00
parent 06ecdf245c
commit a2c313fd95
3 changed files with 42 additions and 2 deletions

View File

@ -65,6 +65,42 @@ void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){
m->aw = 2*(w2 - w1)/(dt1 + dt2); m->aw = 2*(w2 - w1)/(dt1 + dt2);
} }
void radar_dgl(trial* t, timestamp ts){
timestamp step = ts- t->sim.tm.ts;
if (step > 0){
dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,1);
t->sim.tm.ts += step;
t->sim.steps++;
}
}
/*updates the simulation state when new telemetry arrives*/
void sim_update(trial* t){
telemetry *tm,*tmp;
tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
/*run simulation up to new telemetry timestamp*/
radar_dgl(t,tm->ts);
/*calculate misspredict*/
fprintf(stderr, "Miss predict: dx=%f, dy=%f, ddir=%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,
t->sim.steps);
/*update simulation vehicle from telemetry: everything and calculate w*/
t->sim.tm.vehicle = tm->vehicle;
g_array_set_size(t->sim.tm.objects, 0);
g_array_append_vals(t->sim.tm.objects, tm->objects->data, tm->objects->len);
t->sim.steps = 0;
if(t->telemetry.length > 1){
tmp = (telemetry*) g_queue_peek_tail_link(&t->telemetry)->prev->data;
t->sim.tm.vehicle.w = (tm->vehicle.dir - tmp->vehicle.dir)/(tm->ts - tmp->ts);
}
}
int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){ int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){
double a,k,aw; double a,k,aw;
double wsoll; double wsoll;

View File

@ -7,4 +7,8 @@ double turn2w(trial* t, turn_t turn);
void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m); void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m);
int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat); int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat);
#endif void sim_update(trial* t);
void radar_dgl(trial* t, timestamp ts);
#endif

View File

@ -35,7 +35,7 @@ void trial_loop(trial *t) {
timestamp step=getcurts(t) + LATENCY - t->sim.tm.ts; timestamp step=getcurts(t) + LATENCY - t->sim.tm.ts;
if (step > 0) { if (step > 0) {
t->sim.tm.vehicle.w = turn2w(t, t->sim.tm.vehicle.turn); t->sim.tm.vehicle.w = turn2w(t, t->sim.tm.vehicle.turn);
dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,step); dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,1);
t->sim.tm.ts += step; t->sim.tm.ts += step;
t->sim.steps++; t->sim.steps++;
} }