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_identity(&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 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 */
const struct Int32Rates rate_ref_scaled = {
OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))
};
struct Int32Rates rate_err;
struct Int32Rates *body_rate = stateGetBodyRates_i();
RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
#define INTEGRATOR_BOUND 100000
/* integrated error */
if (enable_integrator) {
stabilization_att_sum_err_quat.qx += att_err.qx / IERROR_SCALE;
stabilization_att_sum_err_quat.qy += att_err.qy / IERROR_SCALE;
stabilization_att_sum_err_quat.qz += att_err.qz / IERROR_SCALE;
Bound(stabilization_att_sum_err_quat.qx, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qy, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qz, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
} else {
/* reset accumulator */
int32_quat_identity(&stabilization_att_sum_err_quat);
}
/* 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 stabilization_attitude_enter(void)
{
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
attitude_ref_quat_int_enter(&att_ref_quat_i, stab_att_sp_euler.psi);
int32_quat_identity(&stabilization_att_sum_err_quat);
}
void stabilization_attitude_init(void)
{
attitude_ref_quat_int_init(&att_ref_quat_i);
int32_quat_identity(&stabilization_att_sum_err_quat);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INT, send_att);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_INT, send_att_ref);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat);
#endif
}
void stabilization_attitude_init(void)
{
stabilization_attitude_ref_init();
int32_quat_identity(&stabilization_att_sum_err_quat);
#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_icq_init(void)
{
ahrs_icq.status = AHRS_ICQ_UNINIT;
ahrs_icq.is_aligned = FALSE;
ahrs_icq.ltp_vel_norm_valid = FALSE;
ahrs_icq.heading_aligned = FALSE;
/* init ltp_to_imu quaternion as zero/identity rotation */
int32_quat_identity(&ahrs_icq.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs_icq.imu_rate);
INT_RATES_ZERO(ahrs_icq.gyro_bias);
INT_RATES_ZERO(ahrs_icq.rate_correction);
INT_RATES_ZERO(ahrs_icq.high_rez_bias);
/* set default filter cut-off frequency and damping */
ahrs_icq.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_icq.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_icq_set_accel_gains();
ahrs_icq.mag_omega = AHRS_MAG_OMEGA;
ahrs_icq.mag_zeta = AHRS_MAG_ZETA;
ahrs_icq_set_mag_gains();
/* set default gravity heuristic */
ahrs_icq.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_icq.correct_gravity = TRUE;
#else
ahrs_icq.correct_gravity = FALSE;
#endif
VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
ahrs_icq.accel_cnt = 0;
ahrs_icq.mag_cnt = 0;
}
