/* 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 }
int scl_gps_read(gps_data_t *data_out) { ASSERT_NOT_NULL(data_out); int ret_code = 0; pthread_mutex_lock(&mutex); timer += interval_measure(&interval); if (timer > GPS_TIMEOUT) { ret_code = -1; } else { *data_out = gps_data; } pthread_mutex_unlock(&mutex); return ret_code; }
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_step(const float dt, const marg_data_t *marg_data, const gps_data_t *gps_data, const float ultra, const float baro, const float voltage, const float current, const float channels[MAX_CHANNELS], const uint16_t sensor_status, const bool override_hw) { vec2_t ne_pos_err, ne_speed_sp, ne_spd_err; vec2_init(&ne_pos_err); vec2_init(&ne_speed_sp); vec2_init(&ne_spd_err); vec3_t mag_normal; vec3_init(&mag_normal); float u_pos_err = 0.0f; float u_spd_err = 0.0f; int bb_rate; if (calibrate) { ONCE(LOG(LL_INFO, "publishing calibration data; actuators disabled")); bb_rate = 20; goto out; } else bb_rate = 1; pos_in.dt = dt; pos_in.ultra_u = ultra; pos_in.baro_u = baro; if (!(sensor_status & MARG_VALID)) { marg_err += 1; if (marg_err > 500) { /* we are in serious trouble */ memset(setpoints, 0, sizeof(float) * platform.n_motors); platform_write_motors(setpoints); die(); } goto out; } marg_err = 0; float cal_channels[MAX_CHANNELS]; memcpy(cal_channels, channels, sizeof(cal_channels)); if (sensor_status & RC_VALID) { /* apply calibration if remote control input is valid: */ float cal_data[3] = {channels[CH_PITCH], channels[CH_ROLL], channels[CH_YAW]}; cal_sample_apply(&rc_cal, cal_data); cal_channels[CH_PITCH] = cal_data[0]; cal_channels[CH_ROLL] = cal_data[1]; cal_channels[CH_YAW] = cal_data[2]; } /* perform gyro calibration: */ marg_data_t cal_marg_data; marg_data_init(&cal_marg_data); marg_data_copy(&cal_marg_data, marg_data); if (cal_sample_apply(&gyro_cal, cal_marg_data.gyro.ve) == 0 && gyro_moved(&marg_data->gyro)) { if (interval_measure(&gyro_move_interval) > 1.0) LOG(LL_ERROR, "gyro moved during calibration, retrying"); cal_reset(&gyro_cal); } /* update relative gps position, if we have a fix: */ float mag_decl; if (sensor_status & GPS_VALID) { gps_util_update(&gps_rel_data, gps_data); pos_in.pos_n = gps_rel_data.dn; pos_in.pos_e = gps_rel_data.de; pos_in.speed_n = gps_rel_data.speed_n; pos_in.speed_e = gps_rel_data.speed_e; ONCE(gps_start_set(gps_data)); mag_decl = mag_decl_get(); } else { pos_in.pos_n = 0.0f; pos_in.pos_e = 0.0f; pos_in.speed_n = 0.0f; pos_in.speed_e = 0.0f; mag_decl = 0.0f; } /* apply acc/mag calibration: */ acc_mag_cal_apply(&cal_marg_data.acc, &cal_marg_data.mag); vec_copy(&mag_normal, &cal_marg_data.mag); /* apply current magnetometer compensation: */ cmc_apply(&cal_marg_data.mag, current); //printf("%f %f %f %f\n", current, cal_marg_data.mag.x, cal_marg_data.mag.y, cal_marg_data.mag.z); /* determine flight state: */ bool flying = flight_state_update(&cal_marg_data.acc.ve[0]); if (!flying && pos_in.ultra_u == 7.0) pos_in.ultra_u = 0.2; /* compute orientation estimate: */ euler_t euler; int ahrs_state = cal_ahrs_update(&euler, &cal_marg_data, mag_decl, dt); if (ahrs_state < 0 || !cal_complete(&gyro_cal)) goto out; ONCE(init = 1; LOG(LL_DEBUG, "system initialized; orientation = yaw: %f pitch: %f roll: %f", euler.yaw, euler.pitch, euler.roll)); /* rotate local ACC measurements into global NEU reference frame: */ vec3_t world_acc; vec3_init(&world_acc); body_to_neu(&world_acc, &euler, &cal_marg_data.acc); /* center global ACC readings: */ filter1_run(&lp_filter, &world_acc.ve[0], &acc_vec[0]); FOR_N(i, 3) pos_in.acc.ve[i] = world_acc.ve[i] - acc_vec[i]; /* compute next 3d position estimate using Kalman filters: */ pos_t pos_est; vec2_init(&pos_est.ne_pos); vec2_init(&pos_est.ne_speed); pos_update(&pos_est, &pos_in); /* execute flight logic (sets cm_x parameters used below): */ bool hard_off = false; bool motors_enabled = flight_logic_run(&hard_off, sensor_status, flying, cal_channels, euler.yaw, &pos_est.ne_pos, pos_est.baro_u.pos, pos_est.ultra_u.pos, platform.max_thrust_n, platform.mass_kg, dt); /* execute up position/speed controller(s): */ float a_u = 0.0f; if (cm_u_is_pos()) { if (cm_u_is_baro_pos()) a_u = u_ctrl_step(&u_pos_err, cm_u_sp(), pos_est.baro_u.pos, pos_est.baro_u.speed, dt); else /* ultra pos */ a_u = u_ctrl_step(&u_pos_err, cm_u_sp(), pos_est.ultra_u.pos, pos_est.ultra_u.speed, dt); } else if (cm_u_is_spd()) a_u = u_speed_step(&u_spd_err, cm_u_sp(), pos_est.baro_u.speed, dt); else a_u = cm_u_sp(); /* execute north/east navigation and/or read speed vector input: */ if (cm_att_is_gps_pos()) { vec2_t pos_sp; vec2_init(&pos_sp); cm_att_sp(&pos_sp); navi_run(&ne_speed_sp, &ne_pos_err, &pos_sp, &pos_est.ne_pos, dt); } else if (cm_att_is_gps_spd()) cm_att_sp(&ne_speed_sp); /* execute north/east speed controller: */ vec2_t a_ne; vec2_init(&a_ne); ne_speed_ctrl_run(&a_ne, &ne_spd_err, &ne_speed_sp, dt, &pos_est.ne_speed); vec3_t a_neu; vec3_set(&a_neu, a_ne.x, a_ne.y, a_u); vec3_t f_neu; vec3_init(&f_neu); vec_scalar_mul(&f_neu, &a_neu, platform.mass_kg); /* f[i] = a[i] * m, makes ctrl device-independent */ float hover_force = platform.mass_kg * 9.81f; f_neu.z += hover_force; /* execute NEU forces optimizer: */ float thrust; vec2_t pr_pos_sp; vec2_init(&pr_pos_sp); att_thrust_calc(&pr_pos_sp, &thrust, &f_neu, euler.yaw, platform.max_thrust_n, 0); /* execute direct attitude angle control, if requested: */ if (cm_att_is_angles()) cm_att_sp(&pr_pos_sp); /* execute attitude angles controller: */ vec2_t att_err; vec2_init(&att_err); vec2_t pr_spd; vec2_set(&pr_spd, -cal_marg_data.gyro.y, cal_marg_data.gyro.x); vec2_t pr_pos, pr_pos_ctrl; vec2_set(&pr_pos, -euler.pitch, euler.roll); vec2_init(&pr_pos_ctrl); att_ctrl_step(&pr_pos_ctrl, &att_err, dt, &pr_pos, &pr_spd, &pr_pos_sp); float piid_sp[3] = {0.0f, 0.0f, 0.0f}; piid_sp[PIID_PITCH] = pr_pos_ctrl.ve[0]; piid_sp[PIID_ROLL] = pr_pos_ctrl.ve[1]; /* execute direct attitude rate control, if requested:*/ if (cm_att_is_rates()) { vec2_t rates_sp; vec2_init(&rates_sp); cm_att_sp(&rates_sp); piid_sp[PIID_PITCH] = rates_sp.ve[0]; piid_sp[PIID_ROLL] = rates_sp.ve[1]; } /* execute yaw position controller: */ float yaw_speed_sp, yaw_err; if (cm_yaw_is_pos()) yaw_speed_sp = yaw_ctrl_step(&yaw_err, cm_yaw_sp(), euler.yaw, cal_marg_data.gyro.z, dt); else yaw_speed_sp = cm_yaw_sp(); /* direct yaw speed control */ //yaw_speed_sp = yaw_ctrl_step(&yaw_err, 0.0, euler.yaw, cal_marg_data.gyro.z, dt); piid_sp[PIID_YAW] = yaw_speed_sp; /* execute stabilizing PIID controller: */ f_local_t f_local = {{thrust, 0.0f, 0.0f, 0.0f}}; float piid_gyros[3] = {cal_marg_data.gyro.x, -cal_marg_data.gyro.y, cal_marg_data.gyro.z}; piid_run(&f_local.ve[1], piid_gyros, piid_sp, dt); /* computate rpm ^ 2 out of the desired forces: */ inv_coupling_calc(rpm_square, f_local.ve); /* compute motor setpoints out of rpm ^ 2: */ piid_int_enable(platform_ac_calc(setpoints, motors_spinning(), voltage, rpm_square)); /* enables motors, if flight logic requests it: */ motors_state_update(flying, dt, motors_enabled); /* reset controllers, if motors are not controllable: */ if (!motors_controllable()) { navi_reset(); ne_speed_ctrl_reset(); u_ctrl_reset(); att_ctrl_reset(); piid_reset(); } /* handle special cases for motor setpoints: */ if (motors_starting()) FOR_N(i, platform.n_motors) setpoints[i] = platform.ac.min; if (hard_off || motors_stopping()) FOR_N(i, platform.n_motors) setpoints[i] = platform.ac.off_val; printf("%f %f %f\n", rad2deg(euler.pitch), rad2deg(euler.roll), rad2deg(euler.yaw)); /* write motors: */ if (!override_hw) { //platform_write_motors(setpoints); } /* set monitoring data: */ mon_data_set(ne_pos_err.x, ne_pos_err.y, u_pos_err, yaw_err); out: EVERY_N_TIMES(bb_rate, blackbox_record(dt, marg_data, gps_data, ultra, baro, voltage, current, channels, sensor_status, /* sensor inputs */ &ne_pos_err, u_pos_err, /* position errors */ &ne_spd_err, u_spd_err /* speed errors */, &mag_normal)); }