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)); } }