interval_t *
interval_dup (interval_t * interval)
{
interval_t *dinterval;
interval_init (&dinterval, &(interval->start), &(interval->end));
return dinterval;
}
/* executed on the actual hardware */
void main_realtime(int argc, char *argv[])
{
main_init(argc, argv);
main_realtime_init();
interval_t interval;
interval_init(&interval);
DATA_DEFINITION();
PERIODIC_THREAD_LOOP_BEGIN
{
dt = interval_measure(&interval);
sensor_status = platform_read_sensors(&marg_data, &gps_data, &ultra_z, &baro_z, &voltage, ¤t, channels);
main_step(dt, &marg_data, &gps_data, ultra_z, baro_z, voltage, current, channels, sensor_status, 0);
}
PERIODIC_THREAD_LOOP_END
}
SIMPLE_THREAD_END
int scl_gps_init(void)
{
ASSERT_ONCE();
THROW_BEGIN();
scl_socket = scl_get_socket("gps");
THROW_IF(scl_socket == NULL, -ENODEV);
pthread_mutexattr_init(&mutexattr);
pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT);
pthread_mutex_init(&mutex, &mutexattr);
interval_init(&interval);
simple_thread_start(&thread, thread_func, "gps_reader", THREAD_PRIORITY, NULL);
THROW_END();
}
int main(void)
{
i2c_bus_t bus;
int ret = i2c_bus_open(&bus, "/dev/i2c-3");
if (ret < 0)
{
fatal("could not open i2c bus", ret);
return EXIT_FAILURE;
}
/* ITG: */
itg3200_dev_t itg;
itg_again:
ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ);
if (ret < 0)
{
fatal("could not inizialize ITG3200", ret);
if (ret == -EAGAIN)
{
goto itg_again;
}
return EXIT_FAILURE;
}
/* BMA: */
bma180_dev_t bma;
bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ);
/* HMC: */
hmc5883_dev_t hmc;
hmc5883_init(&hmc, &bus);
/* MS: */
ms5611_dev_t ms;
ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096);
if (ret < 0)
{
fatal("could not inizialize MS5611", ret);
return EXIT_FAILURE;
}
pthread_t thread;
pthread_create(&thread, NULL, ms5611_reader, &ms);
/* initialize AHRS filter: */
madgwick_ahrs_t madgwick_ahrs;
madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA);
interval_t interval;
interval_init(&interval);
float init = START_BETA;
udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0);
/* kalman filter: */
kalman_t kalman1, kalman2, kalman3;
kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0);
vec3_t global_acc; /* x = N, y = E, z = D */
int init_done = 0;
int converged = 0;
sliding_avg_t *avg[3];
avg[0] = sliding_avg_create(1000, 0.0);
avg[1] = sliding_avg_create(1000, 0.0);
avg[2] = sliding_avg_create(1000, -9.81);
float alt_rel_last = 0.0;
int udp_cnt = 0;
while (1)
{
int i;
float dt = interval_measure(&interval);
init -= BETA_STEP;
if (init < FINAL_BETA)
{
init = FINAL_BETA;
init_done = 1;
}
madgwick_ahrs.beta = init;
/* sensor data acquisition: */
itg3200_read_gyro(&itg);
bma180_read_acc(&bma);
hmc5883_read(&hmc);
/* state estimates and output: */
euler_t euler;
madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt);
quat_t q_body_to_world;
quat_copy(&q_body_to_world, &madgwick_ahrs.quat);
quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world);
for (i = 0; i < 3; i++)
{
global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]);
}
if (init_done)
{
kalman_in_t kalman_in;
kalman_in.dt = dt;
kalman_in.pos = 0;
kalman_out_t kalman_out;
kalman_in.acc = global_acc.x;
kalman_run(&kalman_out, &kalman1, &kalman_in);
kalman_in.acc = global_acc.y;
kalman_run(&kalman_out, &kalman2, &kalman_in);
kalman_in.acc = -global_acc.z;
pthread_mutex_lock(&mutex);
kalman_in.pos = alt_rel;
pthread_mutex_unlock(&mutex);
kalman_run(&kalman_out, &kalman3, &kalman_in);
if (!converged)
{
if (fabs(kalman_out.pos - alt_rel) < 0.1)
{
converged = 1;
fprintf(stderr, "init done\n");
}
}
if (converged) // && udp_cnt++ > 10)
{
if (udp_cnt++ == 10)
{
char buffer[1024];
udp_cnt = 0;
int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3,
global_acc.x, global_acc.y, global_acc.z);
udp_socket_send(socket, buffer, len);
}
printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos);
fflush(stdout);
}
}
}
return 0;
}
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");
}