static void *
critical_init (Display *dpy, Window window)
{
struct state *st = (struct state *) calloc (1, sizeof(*st));
int model_w, model_h;
st->dpy = dpy;
st->window = window;
/* Find window attributes */
XGetWindowAttributes (st->dpy, st->window, &st->wattr);
st->batchcount = get_integer_resource (st->dpy, "batchcount", "Integer");
if (st->batchcount < 5)
st->batchcount = 5;
st->lines_per_color = 10;
/* For the moment the model size is just fixed -- making it vary
with the screen size just makes the hack boring on large
screens. */
model_w = 80;
st->settings.cell_size = st->wattr.width / model_w;
model_h = st->settings.cell_size ?
st->wattr.height / st->settings.cell_size : 1;
/* Construct the initial model state. */
st->settings.trail = clip(2, get_integer_resource (st->dpy, "trail", "Integer"), 1000);
st->history = calloc (st->settings.trail, sizeof (st->history[0]));
if (!st->history)
{
fprintf (stderr, "critical: "
"couldn't allocate trail history of %d cells\n",
st->settings.trail);
abort();
}
st->model = model_allocate (model_w, model_h);
if (!st->model)
{
fprintf (stderr, "critical: error preparing the model\n");
abort();
}
/* make a black gc for the background */
st->gcv.foreground = get_pixel_resource (st->dpy, st->wattr.colormap,
"background", "Background");
st->bgc = XCreateGC (st->dpy, st->window, GCForeground, &st->gcv);
st->fgc = XCreateGC (st->dpy, st->window, 0, &st->gcv);
#ifdef HAVE_JWXYZ
jwxyz_XSetAntiAliasing (dpy, st->fgc, False);
jwxyz_XSetAntiAliasing (dpy, st->bgc, False);
#endif
st->delay_usecs = get_integer_resource (st->dpy, "delay", "Integer");
st->n_restart = get_integer_resource (st->dpy, "restart", "Integer");
setup_colormap (st, &st->d_colors, &st->d_n_colors);
model_initialize (st->model);
model_step (st->model, &st->history[0]);
st->d_pos = 1;
st->d_wrapped = 0;
st->i_restart = 0;
st->d_i_batch = st->batchcount;
return st;
}
static unsigned long
critical_draw (Display *dpy, Window window, void *closure)
{
struct state *st = (struct state *) closure;
if (st->eraser) {
st->eraser = erase_window (st->dpy, st->window, st->eraser);
return st->delay_usecs;
}
/* for (d_i_batch = batchcount; d_i_batch; d_i_batch--) */
{
/* Set color */
if ((st->d_i_batch % st->lines_per_color) == 0)
{
st->d_i_color = (st->d_i_color + 1) % st->d_n_colors;
st->gcv.foreground = st->d_colors[st->d_i_color].pixel;
XChangeGC (st->dpy, st->fgc, GCForeground, &st->gcv);
}
assert(st->d_pos >= 0 && st->d_pos < st->settings.trail);
model_step (st->model, &st->history[st->d_pos]);
draw_step (st, st->fgc, st->d_pos);
/* we use the history as a ring buffer, but don't start erasing until
we've d_wrapped around once. */
if (++st->d_pos >= st->settings.trail)
{
st->d_pos -= st->settings.trail;
st->d_wrapped = 1;
}
if (st->d_wrapped)
{
draw_step (st, st->bgc, st->d_pos+1);
}
}
st->d_i_batch--;
if (st->d_i_batch < 0)
st->d_i_batch = st->batchcount;
else
return st->delay_usecs;
st->i_restart = (st->i_restart + 1) % st->n_restart;
if (st->i_restart == 0)
{
/* Time to start a new simulation, this one has probably got
to be a bit boring. */
free_colormap (st, &st->d_colors, st->d_n_colors);
setup_colormap (st, &st->d_colors, &st->d_n_colors);
st->eraser = erase_window (st->dpy, st->window, st->eraser);
model_initialize (st->model);
model_step (st->model, &st->history[0]);
st->d_pos = 1;
st->d_wrapped = 0;
st->d_i_batch = st->batchcount;
}
return st->delay_usecs;
}
void _main(int argc, char *argv[])
{
(void)argc;
(void)argv;
syslog(LOG_INFO, "initializing core");
/* init SCL subsystem: */
syslog(LOG_INFO, "initializing signaling and communication link (SCL)");
if (scl_init("core") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
exit(EXIT_FAILURE);
}
/* init params subsystem: */
syslog(LOG_INFO, "initializing opcd interface");
opcd_params_init("core.", 1);
/* initialize logger: */
syslog(LOG_INFO, "opening logger");
if (logger_open() != 0)
{
syslog(LOG_CRIT, "could not open logger");
exit(EXIT_FAILURE);
}
syslog(LOG_CRIT, "logger opened");
sleep(1); /* give scl some time to establish
a link between publisher and subscriber */
LOG(LL_INFO, "+------------------+");
LOG(LL_INFO, "| core startup |");
LOG(LL_INFO, "+------------------+");
LOG(LL_INFO, "initializing system");
/* set-up real-time scheduling: */
struct sched_param sp;
sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
sched_setscheduler(getpid(), SCHED_FIFO, &sp);
if (mlockall(MCL_CURRENT | MCL_FUTURE))
{
LOG(LL_ERROR, "mlockall() failed");
exit(EXIT_FAILURE);
}
/* initialize hardware/drivers: */
omap_i2c_bus_init();
baro_altimeter_init();
ultra_altimeter_init();
ahrs_init();
motors_init();
voltage_reader_start();
//gps_init();
LOG(LL_INFO, "initializing model/controller");
model_init();
ctrl_init();
/* initialize command interface */
LOG(LL_INFO, "initializing cmd interface");
cmd_init();
/* prepare main loop: */
for (int i = 0; i < NUM_AVG; i++)
{
output_avg[i] = sliding_avg_create(OUTPUT_RATIO, 0.0f);
}
LOG(LL_INFO, "system up and running");
struct timespec ts_curr;
struct timespec ts_prev;
struct timespec ts_diff;
clock_gettime(CLOCK_REALTIME, &ts_curr);
/* run model and controller: */
while (1)
{
/* calculate dt: */
ts_prev = ts_curr;
clock_gettime(CLOCK_REALTIME, &ts_curr);
TIMESPEC_SUB(ts_diff, ts_curr, ts_prev);
float dt = (float)ts_diff.tv_sec + (float)ts_diff.tv_nsec / (float)NSEC_PER_SEC;
/* read sensor values into model input structure: */
model_input_t model_input;
model_input.dt = dt;
ahrs_read(&model_input.ahrs_data);
gps_read(&model_input.gps_data);
model_input.ultra_z = ultra_altimeter_read();
model_input.baro_z = baro_altimeter_read();
/* execute model step: */
model_state_t model_state;
model_step(&model_state, &model_input);
/* execute controller step: */
mixer_in_t mixer_in;
ctrl_step(&mixer_in, dt, &model_state);
/* set up mixer input: */
mixer_in.pitch = sliding_avg_calc(output_avg[AVG_PITCH], mixer_in.pitch);
mixer_in.roll = sliding_avg_calc(output_avg[AVG_ROLL], mixer_in.roll);
mixer_in.yaw = sliding_avg_calc(output_avg[AVG_YAW], mixer_in.yaw);
mixer_in.gas = sliding_avg_calc(output_avg[AVG_GAS], mixer_in.gas);
/* write data to motor mixer: */
EVERY_N_TIMES(OUTPUT_RATIO, motors_write(&mixer_in));
}
}
