t_pos *read_file(char *file) {
int size, x, y, ry, p, inctile;
char line[MAX_LINE_SIZE];
t_pos *ret = (t_pos *) malloc(sizeof(t_pos));
t_hex *hex = &ret->hex;
t_tiles *tiles = &ret->tiles;
t_done *done = &ret->done;
FILE *input;
tiles_init(tiles);
input = fopen(file, "rb");
if (input == NULL) {
free(ret);
return NULL;
}
fgets(line, MAX_LINE_SIZE, input);
sscanf(line, "%d", &size);
hex_init(hex, size);
done_init(done, hex->count);
for (y = 0; y < size; ++y) {
if (fgets(line, MAX_LINE_SIZE, input) == NULL) {
printf("ERROR READING LINE %d!\n", y);
free(ret);
return NULL;
}
p = size - 1 - y;
for (x = 0; x < size + y; ++x) {
char *tile = line + p;
p += 2;
inctile = 0;
if (tile[1] == '.') {
inctile = 7;
} else {
inctile = tile[1] - '0';
}
if (tile[0] == '+') {
printf("Adding locked tile: %d at pos %d, %d, id=%d\n",
inctile, x, y, get_by_pos(hex, make_point(x, y))->id);
add_done(done, get_by_pos(hex, make_point(x, y))->id, inctile);
} else {
(*tiles)[inctile] += 1;
}
}
}
for (y = 1; y < size; ++y) {
ry = size - 1 + y;
if (fgets(line, MAX_LINE_SIZE, input) == NULL) {
printf("ERROR READING LINE %d!\n", ry);
free(ret);
return NULL;
}
p = y;
for (x = y; x < 2 * size - 1; ++x) {
char *tile = line + p;
p += 2;
inctile = 0;
if (tile[1] == '.') {
inctile = 7;
} else {
inctile = tile[1] - '0';
}
if (tile[0] == '+') {
printf("Adding locked tile: %d at pos %d, %d, id=%d\n",
inctile, x, ry, get_by_pos(hex, make_point(x, ry))->id);
add_done(done, get_by_pos(hex, make_point(x, ry))->id, inctile);
} else {
(*tiles)[inctile] += 1;
}
}
}
fclose(input);
pos_init(ret);
link_nodes(hex);
return ret;
}
/**
* Initialize fragment shader input attribute info.
*/
void
lp_build_interp_soa_init(struct lp_build_interp_soa_context *bld,
struct gallivm_state *gallivm,
unsigned num_inputs,
const struct lp_shader_input *inputs,
boolean pixel_center_integer,
LLVMBuilderRef builder,
struct lp_type type,
LLVMValueRef a0_ptr,
LLVMValueRef dadx_ptr,
LLVMValueRef dady_ptr,
LLVMValueRef x0,
LLVMValueRef y0)
{
struct lp_type coeff_type;
struct lp_type setup_type;
unsigned attrib;
unsigned chan;
memset(bld, 0, sizeof *bld);
memset(&coeff_type, 0, sizeof coeff_type);
coeff_type.floating = TRUE;
coeff_type.sign = TRUE;
coeff_type.width = 32;
coeff_type.length = type.length;
memset(&setup_type, 0, sizeof setup_type);
setup_type.floating = TRUE;
setup_type.sign = TRUE;
setup_type.width = 32;
setup_type.length = TGSI_NUM_CHANNELS;
/* XXX: we don't support interpolating into any other types */
assert(memcmp(&coeff_type, &type, sizeof coeff_type) == 0);
lp_build_context_init(&bld->coeff_bld, gallivm, coeff_type);
lp_build_context_init(&bld->setup_bld, gallivm, setup_type);
/* For convenience */
bld->pos = bld->attribs[0];
bld->inputs = (const LLVMValueRef (*)[TGSI_NUM_CHANNELS]) bld->attribs[1];
/* Position */
bld->mask[0] = TGSI_WRITEMASK_XYZW;
bld->interp[0] = LP_INTERP_LINEAR;
/* Inputs */
for (attrib = 0; attrib < num_inputs; ++attrib) {
bld->mask[1 + attrib] = inputs[attrib].usage_mask;
bld->interp[1 + attrib] = inputs[attrib].interp;
}
bld->num_attribs = 1 + num_inputs;
/* Ensure all masked out input channels have a valid value */
for (attrib = 0; attrib < bld->num_attribs; ++attrib) {
for (chan = 0; chan < TGSI_NUM_CHANNELS; ++chan) {
bld->attribs[attrib][chan] = bld->coeff_bld.undef;
}
}
if (pixel_center_integer) {
bld->pos_offset = 0.0;
} else {
bld->pos_offset = 0.5;
}
pos_init(bld, x0, y0);
/*
* Simple method (single step interpolation) may be slower if vector length
* is just 4, but the results are different (generally less accurate) with
* the other method, so always use more accurate version.
*/
if (1) {
bld->simple_interp = TRUE;
{
/* XXX this should use a global static table */
unsigned i;
unsigned num_loops = 16 / type.length;
LLVMValueRef pixoffx, pixoffy, index;
LLVMValueRef ptr;
bld->xoffset_store = lp_build_array_alloca(gallivm,
lp_build_vec_type(gallivm, type),
lp_build_const_int32(gallivm, num_loops),
"");
bld->yoffset_store = lp_build_array_alloca(gallivm,
lp_build_vec_type(gallivm, type),
lp_build_const_int32(gallivm, num_loops),
"");
for (i = 0; i < num_loops; i++) {
index = lp_build_const_int32(gallivm, i);
calc_offsets(&bld->coeff_bld, i*type.length/4, &pixoffx, &pixoffy);
ptr = LLVMBuildGEP(builder, bld->xoffset_store, &index, 1, "");
LLVMBuildStore(builder, pixoffx, ptr);
ptr = LLVMBuildGEP(builder, bld->yoffset_store, &index, 1, "");
LLVMBuildStore(builder, pixoffy, ptr);
}
}
coeffs_init_simple(bld, a0_ptr, dadx_ptr, dady_ptr);
}
else {
bld->simple_interp = FALSE;
coeffs_init(bld, a0_ptr, dadx_ptr, dady_ptr);
}
}
void main_init(int argc, char *argv[])
{
bool override_hw = false;
if (argc > 1)
{
if (strcmp(argv[1], "calibrate") == 0)
calibrate = true;
else
override_hw = true;
}
/* init data structures: */
memset(&pos_in, 0, sizeof(pos_in_t));
vec3_init(&pos_in.acc);
/* init SCL subsystem: */
syslog(LOG_INFO, "initializing signaling and communication link (SCL)");
if (scl_init("pilot") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
die();
}
/* init params subsystem: */
syslog(LOG_INFO, "initializing opcd interface");
opcd_params_init("pilot.", 1);
/* initialize logger: */
syslog(LOG_INFO, "opening logger");
if (logger_open() != 0)
{
syslog(LOG_CRIT, "could not open logger");
die();
}
syslog(LOG_CRIT, "logger opened");
LOG(LL_INFO, "initializing platform");
if (arcade_quad_init(&platform, override_hw) < 0)
{
LOG(LL_ERROR, "could not initialize platform");
die();
}
acc_mag_cal_init();
cmc_init();
const size_t array_len = sizeof(float) * platform.n_motors;
setpoints = malloc(array_len);
ASSERT_NOT_NULL(setpoints);
memset(setpoints, 0, array_len);
rpm_square = malloc(array_len);
ASSERT_NOT_NULL(rpm_square);
memset(rpm_square, 0, array_len);
LOG(LL_INFO, "initializing model/controller");
pos_init();
ne_speed_ctrl_init(REALTIME_PERIOD);
att_ctrl_init();
yaw_ctrl_init();
u_ctrl_init();
u_speed_init();
navi_init();
LOG(LL_INFO, "initializing command interface");
cmd_init();
motors_state_init();
blackbox_init();
/* init flight logic: */
flight_logic_init();
/* init calibration data: */
cal_init(&gyro_cal, 3, 1000);
cal_ahrs_init();
flight_state_init(50, 150, 4.0);
piid_init(REALTIME_PERIOD);
interval_init(&gyro_move_interval);
gps_data_init(&gps_data);
mag_decl_init();
cal_init(&rc_cal, 3, 500);
tsfloat_t acc_fg;
opcd_param_t params[] =
{
{"acc_fg", &acc_fg},
OPCD_PARAMS_END
};
opcd_params_apply("main.", params);
filter1_lp_init(&lp_filter, tsfloat_get(&acc_fg), 0.06, 3);
cm_init();
mon_init();
LOG(LL_INFO, "entering main loop");
}