void att_ctrl_init(void)
{
ASSERT_ONCE();
/* load parameters: */
opcd_param_t params[] =
{
{"p", &angle_p.value},
{"i", &angle_i.value},
{"i_max", &angle_i_max.value},
{"d", &angle_d.value},
{"filt_fg", &filt_fg},
{"pitch_bias", &biases[0]},
{"roll_bias", &biases[1]},
OPCD_PARAMS_END
};
opcd_params_apply("controllers.attitude.", params);
/* initialize filter: */
filter1_lp_init(&filter, tsfloat_get(&filt_fg), 0.0033333, 2);
/* initialize controllers: */
FOR_EACH(i, controllers)
{
pid_init(&controllers[i], &angle_p, &angle_i, &angle_d, &angle_i_max);
}
void u_ctrl_init(const float dt)
{
ASSERT_ONCE();
opcd_param_t params[] =
{
{"p", &pos_p},
{"d", &pos_d},
{"i", &pos_i},
{"imax", &pos_imax},
{"lpfg", &lpfg},
OPCD_PARAMS_END
};
opcd_params_apply("controllers.u_pos.", params);
pid_init(&ctrl, &pos_p, &pos_i, &pos_d, &pos_imax);
filter1_lp_init(&filter, tsfloat_get(&lpfg), dt, 1);
}
void ne_speed_ctrl_init(float dt)
{
ASSERT_ONCE();
/* load parameters: */
opcd_param_t params[] =
{
{"p", &speed_p.value},
{"i", &speed_i.value},
{"i_max", &speed_i_max.value},
{"d", &speed_d.value},
{"lpfg", &lpfg},
OPCD_PARAMS_END
};
opcd_params_apply("controllers.ne_speed.", params);
filter1_lp_init(&filter, tsfloat_get(&lpfg), dt, 2);
/* initialize controllers: */
FOR_EACH(i, controllers)
{
pid_init(&controllers[i], &speed_p, &speed_i, &speed_d, &speed_i_max);
}
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");
}
