void loop(void *arg){
Matrix outputMatrix;
blackbox_init();
while (running) {
read_inputs();
outputMatrix=periodic_function(io.input_result,io.input_num);
write_outputs(outputMatrix);
}
blackbox_close();
}
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");
}
