void imu_init(void) {
/* initialises neutrals */
RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI
struct Int32Eulers body_to_imu_eulers =
{ ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) };
INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
#else
INT32_QUAT_ZERO(imu.body_to_imu_quat);
INT32_RMAT_ZERO(imu.body_to_imu_rmat);
#endif
imu_impl_init();
}
static void test_10(void) {
struct FloatEulers euler;
EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("euler", euler);
struct FloatQuat quat;
FLOAT_QUAT_OF_EULERS(quat, euler);
DISPLAY_FLOAT_QUAT("####quat", quat);
struct Int32Eulers euleri;
EULERS_BFP_OF_REAL(euleri, euler);
DISPLAY_INT32_EULERS("euleri", euleri);
struct Int32Quat quati;
INT32_QUAT_OF_EULERS(quati, euleri);
DISPLAY_INT32_QUAT("####quat", quati);
struct Int32RMat rmati;
INT32_RMAT_OF_EULERS(rmati, euleri);
DISPLAY_INT32_RMAT("####rmat", rmati);
struct Int32Quat quat_ltp_to_body;
struct Int32Quat body_to_imu_quat;
INT32_QUAT_ZERO( body_to_imu_quat);
INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati);
DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* set ltp_to_body to zero */
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
ahrs_impl.use_gravity_heuristic = TRUE;
#else
ahrs_impl.use_gravity_heuristic = FALSE;
#endif
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void stabilization_attitude_enter(void) {
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
//FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void stabilization_attitude_run(bool_t enable_integrator) {
/*
* Update reference
*/
stabilization_attitude_ref_update();
/*
* Compute errors for feedback
*/
/* attitude error */
struct Int32Quat att_err;
struct Int32Quat* att_quat = stateGetNedToBodyQuat_i();
INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
INT32_QUAT_WRAP_SHORTEST(att_err);
INT32_QUAT_NORMALIZE(att_err);
/* rate error */
struct Int32Rates rate_err;
struct Int32Rates* body_rate = stateGetBodyRates_i();
RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
/* integrated error */
if (enable_integrator) {
struct Int32Quat new_sum_err, scaled_att_err;
/* update accumulator */
scaled_att_err.qi = att_err.qi;
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
INT32_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
INT32_QUAT_NORMALIZE(new_sum_err);
QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
/* compute the feed forward command */
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel);
/* compute the feed back command */
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);
/* sum feedforward and feedback */
stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
// FIXME: make ltp_to_imu and ltp_to_body coherent
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
}
void stabilization_attitude_enter(void) {
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
void stabilization_attitude_enter(void) {
#if !USE_SETPOINTS_WITH_TRANSITIONS
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
#endif
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
register_periodic_telemetry(DefaultPeriodic, "AHRS_REF_QUAT", send_ahrs_ref_quat);
#endif
}
void ahrs_init(void) {
int i, j;
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_T[i][j] = 0.;
bafl_P[i][j] = 0.;
}
/* Insert the diagonal elements of the T-Matrix. These will never change. */
bafl_T[i][i] = 1.0;
/* initial covariance values */
if (i<3) {
bafl_P[i][i] = 1.0;
} else {
bafl_P[i][i] = 0.1;
}
}
FLOAT_QUAT_ZERO(bafl_quat);
ahrs.status = AHRS_UNINIT;
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL);
ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG);
bafl_Q_att = BAFL_Q_ATT;
bafl_Q_gyro = BAFL_Q_GYRO;
FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz);
}
void stabilization_attitude_run(bool_t enable_integrator) {
/*
* Update reference
*/
stabilization_attitude_ref_update();
/*
* Compute errors for feedback
*/
/* attitude error */
struct Int32Quat att_err;
INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
INT32_QUAT_WRAP_SHORTEST(att_err);
INT32_QUAT_NORMALIZE(att_err);
/* rate error */
struct Int32Rates rate_err;
RATES_DIFF(rate_err, ahrs.body_rate, stab_att_ref_rate);
/* integrated error */
if (enable_integrator) {
struct Int32Quat new_sum_err, scaled_att_err;
/* update accumulator */
scaled_att_err.qi = att_err.qi;
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
INT32_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
INT32_QUAT_NORMALIZE(new_sum_err);
QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
attitude_run_ff(stabilization_att_ff_cmd, current_stabilization_gains, &stab_att_ref_accel);
attitude_run_fb(stabilization_att_fb_cmd, current_stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);
for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) {
stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
Bound(stabilization_cmd[i], -200, 200);
}
}
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]);
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]);
}
*/
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* set ltp_to_body to zero */
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
ahrs_impl.reinj_1 = FACE_REINJ_1;
#ifdef IMU_MAG_OFFSET
ahrs_mag_offset = IMU_MAG_OFFSET;
#else
ahrs_mag_offset = 0.;
#endif
}
