void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
#ifdef HAS_SURFACE_COMMANDS
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
#endif
}
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
FLOAT_RATES_ZERO( last_body_rate );
FLOAT_RATES_ZERO( body_rate_d );
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
#endif
}
void stabilization_attitude_init(void)
{
/* setpoints */
FLOAT_EULERS_ZERO(stab_att_sp_euler);
float_quat_identity(&stab_att_sp_quat);
/* reference */
attitude_ref_quat_float_init(&att_ref_quat_f);
attitude_ref_quat_float_schedule(&att_ref_quat_f, STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT);
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
#ifdef HAS_SURFACE_COMMANDS
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
#endif
}
float_quat_identity(&stabilization_att_sum_err_quat);
FLOAT_RATES_ZERO(last_body_rate);
FLOAT_RATES_ZERO(body_rate_d);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_FLOAT, send_att_ref);
#endif
}
void ahrs_init(void) {
ahrs_float.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
/*
* Initialises our state
*/
FLOAT_RATES_ZERO(ahrs_impl.gyro_bias);
const float P0_a = 1.;
const float P0_b = 1e-4;
float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. },
{ 0., P0_a, 0., 0., 0., 0. },
{ 0., 0., P0_a, 0., 0., 0. },
{ 0., 0., 0., P0_b, 0., 0. },
{ 0., 0., 0., 0., P0_b, 0. },
{ 0., 0., 0., 0., 0., P0_b}};
memcpy(ahrs_impl.P, P0, sizeof(P0));
}
void stabilization_attitude_ref_init(void) {
FLOAT_EULERS_ZERO(stab_att_sp_euler);
FLOAT_EULERS_ZERO(stab_att_ref_euler);
FLOAT_RATES_ZERO(stab_att_ref_rate);
FLOAT_RATES_ZERO(stab_att_ref_accel);
}
void stabilization_indi_enter(void)
{
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
FLOAT_RATES_ZERO(indi.angular_accel_ref);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
// Re-initialize filters
indi_init_filters();
}
void stabilization_attitude_ref_init(void) {
FLOAT_EULERS_ZERO(stab_att_sp_euler);
FLOAT_QUAT_ZERO( stab_att_sp_quat);
FLOAT_EULERS_ZERO(stab_att_ref_euler);
FLOAT_QUAT_ZERO( stab_att_ref_quat);
FLOAT_RATES_ZERO( stab_att_ref_rate);
FLOAT_RATES_ZERO( stab_att_ref_accel);
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
}
}
void ahrs_fc_init(void)
{
ahrs_fc.status = AHRS_FC_UNINIT;
ahrs_fc.is_aligned = FALSE;
ahrs_fc.ltp_vel_norm_valid = FALSE;
ahrs_fc.heading_aligned = FALSE;
/* init ltp_to_imu quaternion as zero/identity rotation */
float_quat_identity(&ahrs_fc.ltp_to_imu_quat);
FLOAT_RATES_ZERO(ahrs_fc.imu_rate);
/* set default filter cut-off frequency and damping */
ahrs_fc.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_fc.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_fc.mag_omega = AHRS_MAG_OMEGA;
ahrs_fc.mag_zeta = AHRS_MAG_ZETA;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_fc.correct_gravity = TRUE;
#else
ahrs_fc.correct_gravity = FALSE;
#endif
ahrs_fc.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
VECT3_ASSIGN(ahrs_fc.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
ahrs_fc.accel_cnt = 0;
ahrs_fc.mag_cnt = 0;
}
void ahrs_propagate(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate,gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
#if AHRS_PROPAGATE_RMAT
FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt);
float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat);
FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt);
FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
#endif
compute_body_orientation_and_rates();
}
void ahrs_propagate(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
const float dt = 1./512.;
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction);
// DISPLAY_FLOAT_RATES("omega ", omega);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
/* first order integration of rotation matrix */
struct FloatRMat exp_omega_dt = {
{ 1. , dt*omega.r, -dt*omega.q,
-dt*omega.r, 1. , dt*omega.p,
dt*omega.q, -dt*omega.p, 1. }};
struct FloatRMat R_tdt;
FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt);
memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt));
float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat);
// struct FloatRMat test;
// FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat);
// DISPLAY_FLOAT_RMAT("foo", test);
compute_imu_quat_and_euler_from_rmat();
compute_body_orientation_and_rates();
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
}
void stabilization_indi_enter(void)
{
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
FLOAT_RATES_ZERO(indi.rate.x);
FLOAT_RATES_ZERO(indi.rate.dx);
FLOAT_RATES_ZERO(indi.rate.ddx);
FLOAT_RATES_ZERO(indi.angular_accel_ref);
FLOAT_RATES_ZERO(indi.du);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
FLOAT_RATES_ZERO(indi.u.x);
FLOAT_RATES_ZERO(indi.u.dx);
FLOAT_RATES_ZERO(indi.u.ddx);
}
void stabilization_attitude_enter(void)
{
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
FLOAT_RATES_ZERO(indi.filtered_rate);
FLOAT_RATES_ZERO(indi.filtered_rate_deriv);
FLOAT_RATES_ZERO(indi.filtered_rate_2deriv);
FLOAT_RATES_ZERO(indi.angular_accel_ref);
FLOAT_RATES_ZERO(indi.u);
FLOAT_RATES_ZERO(indi.du);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
FLOAT_RATES_ZERO(indi.udot);
FLOAT_RATES_ZERO(indi.udotdot);
}
void stabilization_attitude_ref_update()
{
#if USE_REF
/* dumb integrate reference attitude */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE);
struct FloatEulers delta_angle;
EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
EULERS_ADD(stab_att_ref_euler, delta_angle);
FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi);
/* integrate reference rotational speeds */
struct FloatRates delta_accel;
RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_accel);
/* compute reference attitude error */
struct FloatEulers ref_err;
EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler);
/* wrap it in the shortest direction */
FLOAT_ANGLE_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
stab_att_ref_accel.p = -2.*ZETA_P * OMEGA_P * stab_att_ref_rate.p - OMEGA_P * OMEGA_P * ref_err.phi;
stab_att_ref_accel.q = -2.*ZETA_Q * OMEGA_P * stab_att_ref_rate.q - OMEGA_Q * OMEGA_Q * ref_err.theta;
stab_att_ref_accel.r = -2.*ZETA_R * OMEGA_P * stab_att_ref_rate.r - OMEGA_R * OMEGA_R * ref_err.psi;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
FLOAT_RATES_ZERO(stab_att_ref_rate);
FLOAT_RATES_ZERO(stab_att_ref_accel);
#endif /* USE_REF */
}
void ArduIMU_init( void ) {
FLOAT_EULERS_ZERO(arduimu_eulers);
FLOAT_RATES_ZERO(arduimu_rates);
FLOAT_VECT3_ZERO(arduimu_accel);
ardu_ins_trans.status = I2CTransDone;
ardu_gps_trans.status = I2CTransDone;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
}
/* init state and measurements */
static inline void init_invariant_state(void)
{
// init state
float_quat_identity(&ahrs_float_inv.state.quat);
FLOAT_RATES_ZERO(ahrs_float_inv.state.bias);
ahrs_float_inv.state.as = 1.0f;
ahrs_float_inv.state.cs = 1.0f;
// init measures
FLOAT_VECT3_ZERO(ahrs_float_inv.meas.accel);
FLOAT_VECT3_ZERO(ahrs_float_inv.meas.mag);
}
static void attitude_run_indi(int32_t indi_commands[], struct Int32Quat *att_err, bool_t in_flight)
{
//Calculate required angular acceleration
struct FloatRates *body_rate = stateGetBodyRates_f();
indi.angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- reference_acceleration.rate_p * body_rate->p;
indi.angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- reference_acceleration.rate_q * body_rate->q;
indi.angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
- reference_acceleration.rate_r * body_rate->r;
//Incremented in angular acceleration requires increment in control input
//G1 is the actuator effectiveness. In the yaw axis, we need something additional: G2.
//It takes care of the angular acceleration caused by the change in rotation rate of the propellers
//(they have significant inertia, see the paper mentioned in the header for more explanation)
indi.du.p = 1.0 / g1.p * (indi.angular_accel_ref.p - indi.filtered_rate_deriv.p);
indi.du.q = 1.0 / g1.q * (indi.angular_accel_ref.q - indi.filtered_rate_deriv.q);
indi.du.r = 1.0 / (g1.r + g2) * (indi.angular_accel_ref.r - indi.filtered_rate_deriv.r + g2 * indi.du.r);
//add the increment to the total control input
indi.u_in.p = indi.u.p + indi.du.p;
indi.u_in.q = indi.u.q + indi.du.q;
indi.u_in.r = indi.u.r + indi.du.r;
//bound the total control input
Bound(indi.u_in.p, -4500, 4500);
Bound(indi.u_in.q, -4500, 4500);
Bound(indi.u_in.r, -4500, 4500);
//Propagate input filters
//first order actuator dynamics
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
//sensor filter
stabilization_indi_second_order_filter(&indi.u_act_dyn, &indi.udotdot, &indi.udot, &indi.u,
STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
//Don't increment if thrust is off
if (stabilization_cmd[COMMAND_THRUST] < 300) {
FLOAT_RATES_ZERO(indi.u);
FLOAT_RATES_ZERO(indi.du);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
FLOAT_RATES_ZERO(indi.udot);
FLOAT_RATES_ZERO(indi.udotdot);
} else {
#if STABILIZATION_INDI_USE_ADAPTIVE
#warning "Use caution with adaptive indi. See the wiki for more info"
lms_estimation();
#endif
}
/* INDI feedback */
indi_commands[COMMAND_ROLL] = indi.u_in.p;
indi_commands[COMMAND_PITCH] = indi.u_in.q;
indi_commands[COMMAND_YAW] = indi.u_in.r;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
#if USE_HIGH_ACCEL_FLAG
high_accel_done = FALSE;
high_accel_flag = FALSE;
#endif
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
}
/* init state and measurements */
static inline void init_invariant_state(void) {
// init state
FLOAT_QUAT_ZERO(ins_impl.state.quat);
FLOAT_RATES_ZERO(ins_impl.state.bias);
FLOAT_VECT3_ZERO(ins_impl.state.pos);
FLOAT_VECT3_ZERO(ins_impl.state.speed);
ins_impl.state.as = 1.0f;
ins_impl.state.hb = 0.0f;
// init measures
FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps);
FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps);
ins_impl.meas.baro_alt = 0.0f;
// init baro
ins_baro_initialized = FALSE;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_f(&imu.body_to_imu),
sizeof(struct FloatEulers));
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu));
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
/**
* Initialize the optical flow module for the bottom camera
*/
void opticflow_module_init(void)
{
// Subscribe to the altitude above ground level ABI messages
AbiBindMsgAGL(OPTICFLOW_AGL_ID, &opticflow_agl_ev, opticflow_agl_cb);
// Set the opticflow state to 0
FLOAT_RATES_ZERO(opticflow_state.rates);
opticflow_state.agl = 0;
// Initialize the opticflow calculation
opticflow_got_result = false;
cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
#endif
}
void ahrs_init(void) {
//ahrs_float.status = AHRS_UNINIT;
// set to running for now and only use ahrs.status, not ahrs_float.status
ahrs.status = AHRS_RUNNING;
ahrs_sim_available = FALSE;
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu to same as ltp_to_body, currently no difference simulated */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_body_quat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_body_euler);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_body_rmat);
RATES_COPY(ahrs_float.imu_rate, ahrs_float.body_rate);
}
void ahrs_dcm_init(void)
{
ahrs_dcm.status = AHRS_DCM_UNINIT;
ahrs_dcm.is_aligned = FALSE;
/* init ltp_to_imu euler with zero */
FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_dcm.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
ahrs_dcm.gps_speed = 0;
ahrs_dcm.gps_acceleration = 0;
ahrs_dcm.gps_course = 0;
ahrs_dcm.gps_course_valid = FALSE;
ahrs_dcm.gps_age = 100;
}
/**
* Initialize the opticflow calculator
* @param[out] *opticflow The new optical flow calculator
* @param[in] *w The image width
* @param[in] *h The image height
*/
void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
{
init_median_filter(&vel_x_filt);
init_median_filter(&vel_y_filt);
/* Create the image buffers */
image_create(&opticflow->img_gray, w, h, IMAGE_GRAYSCALE);
image_create(&opticflow->prev_img_gray, w, h, IMAGE_GRAYSCALE);
/* Set the previous values */
opticflow->got_first_img = false;
FLOAT_RATES_ZERO(opticflow->prev_rates);
/* Set the default values */
opticflow->method = OPTICFLOW_METHOD; //0 = LK_fast9, 1 = Edgeflow
opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
opticflow->search_distance = OPTICFLOW_SEARCH_DISTANCE;
opticflow->derotation = OPTICFLOW_DEROTATION; //0 = OFF, 1 = ON
opticflow->derotation_correction_factor_x = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X;
opticflow->derotation_correction_factor_y = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y;
opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
opticflow->pyramid_level = OPTICFLOW_PYRAMID_LEVEL;
opticflow->median_filter = OPTICFLOW_MEDIAN_FILTER;
opticflow->kalman_filter = OPTICFLOW_KALMAN_FILTER;
opticflow->kalman_filter_process_noise = OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE;
opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
opticflow->fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE;
opticflow->fast9_padding = OPTICFLOW_FAST9_PADDING;
opticflow->fast9_rsize = 512;
opticflow->fast9_ret_corners = malloc(sizeof(struct point_t) * opticflow->fast9_rsize);
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
#ifdef HAS_SURFACE_COMMANDS
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
#endif
}
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
FLOAT_RATES_ZERO( last_body_rate );
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set default filter cut-off frequency and damping */
ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu),
sizeof(struct FloatQuat));
memcpy(&ahrs_impl.ltp_to_imu_rmat, orientationGetRMat_f(&imu.body_to_imu),
sizeof(struct FloatRMat));
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set default filter cut-off frequency and damping */
ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
ahrs_impl.accel_cnt = 0;
ahrs_impl.mag_cnt = 0;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
void ahrs_fc_propagate(struct Int32Rates *gyro, float dt)
{
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, *gyro);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_fc.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_fc.imu_rate, gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, gyro_float, ahrs_fc.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_fc.rate_correction);
#if AHRS_PROPAGATE_RMAT
float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt);
float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat);
float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt);
float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
#endif
// increase accel and mag propagation counters
ahrs_fc.accel_cnt++;
ahrs_fc.mag_cnt++;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
EULERS_COPY(ahrs_impl.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat);
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
