void ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates uf_rate;
RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias);
#if USE_NOISE_CUT
static struct Int32Rates last_uf_rate = { 0, 0, 0 };
if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
/* low pass rate */
#if USE_NOISE_FILTER
RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1);
#else
RATES_ADD(ahrs.imu_rate, uf_rate);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
#endif
#if USE_NOISE_CUT
}
RATES_COPY(last_uf_rate, uf_rate);
#endif
/* integrate eulers */
struct Int32Eulers euler_dot;
INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot);
/* low pass measurement */
EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement);
EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2);
/* compute residual */
EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.hi_res_euler);
INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi);
struct Int32Eulers correction;
/* compute a correction */
EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1);
/* correct estimation */
EULERS_ADD(ahrs_impl.hi_res_euler, correction);
INTEG_EULER_NORMALIZE(ahrs_impl.hi_res_euler.psi);
/* Compute LTP to IMU eulers */
EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
compute_imu_quat_and_rmat_from_euler();
compute_body_orientation();
}
void ahrs_ice_propagate(struct Int32Rates *gyro)
{
/* unbias gyro */
struct Int32Rates uf_rate;
RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias);
#if USE_NOISE_CUT
static struct Int32Rates last_uf_rate = { 0, 0, 0 };
if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
/* low pass rate */
#if USE_NOISE_FILTER
RATES_SUM_SCALED(ahrs_ice.imu_rate, ahrs_ice.imu_rate, uf_rate, NOISE_FILTER_GAIN);
RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, NOISE_FILTER_GAIN + 1);
#else
RATES_ADD(ahrs_ice.imu_rate, uf_rate);
RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, 2);
#endif
#if USE_NOISE_CUT
}
RATES_COPY(last_uf_rate, uf_rate);
#endif
/* integrate eulers */
struct Int32Eulers euler_dot;
int32_eulers_dot_of_rates(&euler_dot, &ahrs_ice.ltp_to_imu_euler, &ahrs_ice.imu_rate);
EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot);
/* low pass measurement */
EULERS_ADD(ahrs_ice.measure, ahrs_ice.measurement);
EULERS_SDIV(ahrs_ice.measure, ahrs_ice.measure, 2);
/* compute residual */
EULERS_DIFF(ahrs_ice.residual, ahrs_ice.measure, ahrs_ice.hi_res_euler);
INTEG_EULER_NORMALIZE(ahrs_ice.residual.psi);
struct Int32Eulers correction;
/* compute a correction */
EULERS_SDIV(correction, ahrs_ice.residual, ahrs_ice.reinj_1);
/* correct estimation */
EULERS_ADD(ahrs_ice.hi_res_euler, correction);
INTEG_EULER_NORMALIZE(ahrs_ice.hi_res_euler.psi);
/* Compute LTP to IMU eulers */
EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE);
}
void stabilization_attitude_run(bool_t in_flight) {
stabilization_attitude_ref_update();
/* Compute feedforward */
stabilization_att_ff_cmd[COMMAND_ROLL] =
stabilization_gains.dd.x * stab_att_ref_accel.p / 32.;
stabilization_att_ff_cmd[COMMAND_PITCH] =
stabilization_gains.dd.y * stab_att_ref_accel.q / 32.;
stabilization_att_ff_cmd[COMMAND_YAW] =
stabilization_gains.dd.z * stab_att_ref_accel.r / 32.;
/* Compute feedback */
/* attitude error */
struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
struct FloatEulers att_err;
EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
FLOAT_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
/* update integrator */
EULERS_ADD(stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
FLOAT_EULERS_ZERO(stabilization_att_sum_err);
}
/* rate error */
struct FloatRates* rate_float = stateGetBodyRates_f();
struct FloatRates rate_err;
RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);
/* PID */
stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p +
stabilization_gains.i.x * stabilization_att_sum_err.phi / 1024.;
stabilization_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q +
stabilization_gains.i.y * stabilization_att_sum_err.theta / 1024.;
stabilization_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r +
stabilization_gains.i.z * stabilization_att_sum_err.psi / 1024.;
stabilization_cmd[COMMAND_ROLL] =
(stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
stabilization_cmd[COMMAND_PITCH] =
(stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
stabilization_cmd[COMMAND_YAW] =
(stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW])/16.;
}
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 stabilization_attitude_run(bool_t in_flight) {
/* update reference */
stabilization_attitude_ref_update();
/* compute feedforward command */
stabilization_att_ff_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND(stabilization_gains.dd.x * stab_att_ref_accel.p, 5);
stabilization_att_ff_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND(stabilization_gains.dd.y * stab_att_ref_accel.q, 5);
stabilization_att_ff_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND(stabilization_gains.dd.z * stab_att_ref_accel.r, 5);
/* compute feedback command */
/* attitude error */
const struct Int32Eulers att_ref_scaled = {
OFFSET_AND_ROUND(stab_att_ref_euler.phi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC))
};
struct Int32Eulers att_err;
struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i();
EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler));
INT32_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
/* update integrator */
EULERS_ADD(stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
INT_EULERS_ZERO(stabilization_att_sum_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));
/* PID */
stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p +
OFFSET_AND_ROUND2((stabilization_gains.i.x * stabilization_att_sum_err.phi), 10);
stabilization_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q +
OFFSET_AND_ROUND2((stabilization_gains.i.y * stabilization_att_sum_err.theta), 10);
stabilization_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r +
OFFSET_AND_ROUND2((stabilization_gains.i.z * stabilization_att_sum_err.psi), 10);
/* with P gain of 100, att_err of 180deg (3.14 rad)
* fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628
* max possible command is 9600
*/
#define CMD_SHIFT 11
/* sum feedforward and feedback */
stabilization_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]), CMD_SHIFT);
stabilization_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]), CMD_SHIFT);
stabilization_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), CMD_SHIFT);
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
