void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
{
int32_t freq = (int32_t)(1. / dt);
/* unbias gyro */
struct Int32Rates omega;
RATES_DIFF(omega, *gyro, ahrs_icq.gyro_bias);
/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 2);
RATES_ADD(ahrs_icq.imu_rate, omega);
RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 3);
#else
RATES_COPY(ahrs_icq.imu_rate, omega);
#endif
/* add correction */
RATES_ADD(omega, ahrs_icq.rate_correction);
/* and zeros it */
INT_RATES_ZERO(ahrs_icq.rate_correction);
/* integrate quaternion */
int32_quat_integrate_fi(&ahrs_icq.ltp_to_imu_quat, &ahrs_icq.high_rez_quat,
&omega, freq);
int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat);
// increase accel and mag propagation counters
ahrs_icq.accel_cnt++;
ahrs_icq.mag_cnt++;
}
void stabilization_attitude_run(bool_t enable_integrator)
{
/* Propagate the second order filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
stabilization_indi_second_order_filter(body_rates, &indi.filtered_rate_2deriv, &indi.filtered_rate_deriv,
&indi.filtered_rate, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
/* attitude error */
struct Int32Quat att_err;
struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
// INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_sp_quat);
int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
/* wrap it in the shortest direction */
int32_quat_wrap_shortest(&att_err);
int32_quat_normalize(&att_err);
/* compute the INDI command */
attitude_run_indi(stabilization_att_indi_cmd, &att_err, enable_integrator);
stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_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_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);
}
static void test_2(void)
{
struct Int32Vect3 v1 = { 5000, 5000, 5000 };
DISPLAY_INT32_VECT3("v1", v1);
struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
DISPLAY_FLOAT_EULERS("euler_f", euler_f);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, euler_f);
DISPLAY_INT32_EULERS("euler_i", euler_i);
struct Int32Quat quat_i;
int32_quat_of_eulers(&quat_i, &euler_i);
DISPLAY_INT32_QUAT("quat_i", quat_i);
int32_quat_normalize(&quat_i);
DISPLAY_INT32_QUAT("quat_i_n", quat_i);
struct Int32Vect3 v2;
int32_quat_vmult(&v2, &quat_i, &v1);
DISPLAY_INT32_VECT3("v2", v2);
struct Int32RMat rmat_i;
int32_rmat_of_quat(&rmat_i, &quat_i);
DISPLAY_INT32_RMAT("rmat_i", rmat_i);
struct Int32Vect3 v3;
INT32_RMAT_VMULT(v3, rmat_i, v1);
DISPLAY_INT32_VECT3("v3", v3);
struct Int32RMat rmat_i2;
int32_rmat_of_eulers(&rmat_i2, &euler_i);
DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);
struct Int32Vect3 v4;
INT32_RMAT_VMULT(v4, rmat_i2, v1);
DISPLAY_INT32_VECT3("v4", v4);
struct FloatQuat quat_f;
float_quat_of_eulers(&quat_f, &euler_f);
DISPLAY_FLOAT_QUAT("quat_f", quat_f);
struct FloatVect3 v5;
VECT3_COPY(v5, v1);
DISPLAY_FLOAT_VECT3("v5", v5);
struct FloatVect3 v6;
float_quat_vmult(&v6, &quat_f, &v5);
DISPLAY_FLOAT_VECT3("v6", v6);
}
void stabilization_indi_run(bool enable_integrator __attribute__((unused)), bool rate_control)
{
/* attitude error */
struct Int32Quat att_err;
struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
/* wrap it in the shortest direction */
int32_quat_wrap_shortest(&att_err);
int32_quat_normalize(&att_err);
/* compute the INDI command */
stabilization_indi_calc_cmd(stabilization_att_indi_cmd, &att_err, rate_control);
/* copy the INDI command */
stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_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);
}
