void stabilization_rate_run(bool_t in_flight) {
/* reference */
struct Int32Rates _r;
RATES_DIFF(_r, stabilization_rate_sp, stabilization_rate_ref);
RATES_SDIV(stabilization_rate_refdot, _r, STABILIZATION_RATE_REF_TAU);
/* integrate ref */
const struct Int32Rates _delta_ref = {
stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC),
stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC),
stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC)};
RATES_ADD(stabilization_rate_ref, _delta_ref);
/* compute feed-forward command */
RATES_EWMULT_RSHIFT(stabilization_rate_ff_cmd, stabilization_rate_ddgain, stabilization_rate_refdot, 9);
/* compute feed-back command */
/* error for feedback */
const struct Int32Rates _ref_scaled = {
OFFSET_AND_ROUND(stabilization_rate_ref.p, (REF_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates _error;
struct Int32Rates* body_rate = stateGetBodyRates_i();
RATES_DIFF(_error, _ref_scaled, (*body_rate));
if (in_flight) {
/* update integrator */
RATES_ADD(stabilization_rate_sum_err, _error);
RATES_BOUND_CUBE(stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
INT_RATES_ZERO(stabilization_rate_sum_err);
}
/* PI */
stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p +
OFFSET_AND_ROUND2((stabilization_rate_igain.p * stabilization_rate_sum_err.p), 10);
stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q +
OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 10);
stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r +
OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10);
stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 11;
stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 11;
stabilization_rate_fb_cmd.r = stabilization_rate_fb_cmd.r >> 11;
/* sum to final command */
stabilization_cmd[COMMAND_ROLL] = stabilization_rate_ff_cmd.p + stabilization_rate_fb_cmd.p;
stabilization_cmd[COMMAND_PITCH] = stabilization_rate_ff_cmd.q + stabilization_rate_fb_cmd.q;
stabilization_cmd[COMMAND_YAW] = stabilization_rate_ff_cmd.r + stabilization_rate_fb_cmd.r;
/* 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_rate_run(bool_t in_flight)
{
/* compute feed-back command */
struct Int32Rates _error;
struct Int32Rates *body_rate = stateGetBodyRates_i();
RATES_DIFF(_error, stabilization_rate_sp, (*body_rate));
if (in_flight) {
/* update integrator */
RATES_ADD(stabilization_rate_sum_err, _error);
RATES_BOUND_CUBE(stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
} else {
INT_RATES_ZERO(stabilization_rate_sum_err);
}
/* PI */
stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p +
OFFSET_AND_ROUND2((stabilization_rate_igain.p * stabilization_rate_sum_err.p), 10);
stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q +
OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 10);
stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r +
OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10);
stabilization_cmd[COMMAND_ROLL] = stabilization_rate_fb_cmd.p >> 11;
stabilization_cmd[COMMAND_PITCH] = stabilization_rate_fb_cmd.q >> 11;
stabilization_cmd[COMMAND_YAW] = stabilization_rate_fb_cmd.r >> 11;
/* 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_propagate(void)
{
/* convert imu data to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
/* unbias rate measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
/* Uncouple Motions */
#ifdef IMU_GYRO_P_Q
float dp=0,dq=0,dr=0;
dp += ahrs_float.imu_rate.q * IMU_GYRO_P_Q;
dp += ahrs_float.imu_rate.r * IMU_GYRO_P_R;
dq += ahrs_float.imu_rate.p * IMU_GYRO_Q_P;
dq += ahrs_float.imu_rate.r * IMU_GYRO_Q_R;
dr += ahrs_float.imu_rate.p * IMU_GYRO_R_P;
dr += ahrs_float.imu_rate.q * IMU_GYRO_R_Q;
ahrs_float.imu_rate.p += dp;
ahrs_float.imu_rate.q += dq;
ahrs_float.imu_rate.r += dr;
#endif
Matrix_update();
// INFO, ahrs struct only updated in ahrs_update_fw_estimator
Normalize();
}
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 ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates omega;
RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);
/* low pass rate */
//#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
if (gyro_lowpass_filter > 1) {
RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter-1);
RATES_ADD(ahrs_impl.imu_rate, omega);
RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter);
//#else
} else {
RATES_COPY(ahrs_impl.imu_rate, omega);
//#endif
}
/* add correction */
RATES_ADD(omega, ahrs_impl.rate_correction);
/* and zeros it */
INT_RATES_ZERO(ahrs_impl.rate_correction);
/* integrate quaternion */
INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
set_body_state_from_quat();
}
void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt)
{
/* convert imu data to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, *gyro);
/* unbias rate measurement */
RATES_DIFF(ahrs_dcm.imu_rate, gyro_float, ahrs_dcm.gyro_bias);
/* Uncouple Motions */
#ifdef IMU_GYRO_P_Q
float dp = 0, dq = 0, dr = 0;
dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q;
dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R;
dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P;
dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R;
dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P;
dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q;
ahrs_dcm.imu_rate.p += dp;
ahrs_dcm.imu_rate.q += dq;
ahrs_dcm.imu_rate.r += dr;
#endif
Matrix_update(dt);
Normalize();
compute_ahrs_representations();
}
void ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates omega;
RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);
/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
RATES_SMUL(ahrs.imu_rate, ahrs.imu_rate,2);
RATES_ADD(ahrs.imu_rate, omega);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 3);
#else
RATES_COPY(ahrs.imu_rate, omega);
#endif
/* add correction */
RATES_ADD(omega, ahrs_impl.rate_correction);
/* and zeros it */
INT_RATES_ZERO(ahrs_impl.rate_correction);
/* integrate quaternion */
INT32_QUAT_INTEGRATE_FI(ahrs.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat);
compute_imu_euler_and_rmat_from_quat();
compute_body_orientation();
}
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_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 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_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);
}
static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) {
struct Int32Rates diff;
RATES_DIFF(diff, i1, i2);
if (diff.p < -threshold || diff.p > threshold ||
diff.q < -threshold || diff.q > threshold ||
diff.r < -threshold || diff.r > threshold) {
return TRUE;
} else {
return FALSE;
}
}
void stabilization_attitude_run(bool_t enable_integrator) {
/*
* Update reference
*/
stabilization_attitude_ref_update();
/*
* Compute errors for feedback
*/
/* attitude error */
struct FloatQuat att_err;
FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
/* rate error */
struct FloatRates rate_err;
RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate);
/* integrated error */
if (enable_integrator) {
struct FloatQuat 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;
FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat);
// FIXME: this is very dangerous! only works if this really includes all commands
for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
}
/* 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_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 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 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);
}
/**
* copy imu.gyro into gyro_float
* compute imu_rate without bias
* scale values to [rad/s]
* Update Matrix
* Normalize
*/
void ahrs_propagate(void)
{
struct FloatRates gyro_float;
/* convert bfp imu data to floating point */
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); // olri div by (1<<12)
/* unbias rate measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
// scale gyro adc raw to [rad/s] FIXME // olri
ahrs_float.imu_rate.p /= 61.35f;
ahrs_float.imu_rate.q /= 57.96f;
ahrs_float.imu_rate.r /= 60.10f;
/* Update Matrix */
Matrix_update();
/* Normalize */
Normalize();
// INFO, ahrs struct only updated in ahrs_update_fw_estimator
}
void stabilization_rate_run(bool_t in_flight)
{
/* compute feed-back command */
struct Int32Rates _error;
struct Int32Rates *body_rate = stateGetBodyRates_i();
RATES_DIFF(_error, stabilization_rate_sp, (*body_rate));
if (in_flight) {
/* update integrator */
//divide the sum_err_increment to make sure it doesn't accumulate to the max too fast
struct Int32Rates sum_err_increment;
RATES_SDIV(sum_err_increment, _error, 5);
RATES_ADD(stabilization_rate_sum_err, sum_err_increment);
RATES_BOUND_CUBE(stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
} else {
INT_RATES_ZERO(stabilization_rate_sum_err);
}
/* PI */
stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p +
OFFSET_AND_ROUND2((stabilization_rate_igain.p * stabilization_rate_sum_err.p), 6);
stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q +
OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 6);
stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r +
OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 6);
stabilization_cmd[COMMAND_ROLL] = stabilization_rate_fb_cmd.p >> 11;
stabilization_cmd[COMMAND_PITCH] = stabilization_rate_fb_cmd.q >> 11;
stabilization_cmd[COMMAND_YAW] = stabilization_rate_fb_cmd.r >> 11;
/* 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_propagate(void) {
struct NedCoor_f accel;
struct FloatRates body_rates;
struct FloatEulers eulers;
// realign all the filter if needed
// a complete init cycle is required
if (ins_impl.reset) {
ins_impl.reset = FALSE;
ins.status = INS_UNINIT;
ahrs.status = AHRS_UNINIT;
init_invariant_state();
}
// fill command vector
struct Int32Rates gyro_meas_body;
INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro);
RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);
// update correction gains
error_output(&ins_impl);
// propagate model
struct inv_state new_state;
runge_kutta_4_float((float*)&new_state,
(float*)&ins_impl.state, INV_STATE_DIM,
(float*)&ins_impl.cmd, INV_COMMAND_DIM,
invariant_model, dt);
ins_impl.state = new_state;
// normalize quaternion
FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);
// set global state
FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
#if INS_UPDATE_FW_ESTIMATOR
// Some stupid lines of code for neutrals
eulers.phi -= ins_roll_neutral;
eulers.theta -= ins_pitch_neutral;
stateSetNedToBodyEulers_f(&eulers);
#else
stateSetNedToBodyQuat_f(&ins_impl.state.quat);
#endif
RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
stateSetBodyRates_f(&body_rates);
stateSetPositionNed_f(&ins_impl.state.pos);
stateSetSpeedNed_f(&ins_impl.state.speed);
// untilt accel and remove gravity
FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel);
FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
FLOAT_VECT3_ADD(accel, A);
stateSetAccelNed_f(&accel);
//------------------------------------------------------------//
RunOnceEvery(3,{
DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
&ins_impl.state.quat.qi,
&eulers.phi,
&eulers.theta,
&eulers.psi,
&ins_impl.state.speed.x,
&ins_impl.state.speed.y,
&ins_impl.state.speed.z,
&ins_impl.state.pos.x,
&ins_impl.state.pos.y,
&ins_impl.state.pos.z,
&ins_impl.state.bias.p,
&ins_impl.state.bias.q,
&ins_impl.state.bias.r,
&ins_impl.state.as,
&ins_impl.state.hb,
&ins_impl.meas.baro_alt,
&ins_impl.meas.pos_gps.z)
});
#if LOG_INVARIANT_FILTER
if (pprzLogFile.fs != NULL) {
if (!log_started) {
// log file header
sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
log_started = TRUE;
}
else {
sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
ins_impl.cmd.rates.p,
ins_impl.cmd.rates.q,
ins_impl.cmd.rates.r,
ins_impl.cmd.accel.x,
ins_impl.cmd.accel.y,
ins_impl.cmd.accel.z,
ins_impl.meas.pos_gps.x,
ins_impl.meas.pos_gps.y,
ins_impl.meas.pos_gps.z,
ins_impl.meas.speed_gps.x,
ins_impl.meas.speed_gps.y,
ins_impl.meas.speed_gps.z,
ins_impl.meas.mag.x,
ins_impl.meas.mag.y,
ins_impl.meas.mag.z,
ins_impl.meas.baro_alt,
ins_impl.state.quat.qi,
ins_impl.state.quat.qx,
ins_impl.state.quat.qy,
ins_impl.state.quat.qz,
ins_impl.state.bias.p,
ins_impl.state.bias.q,
ins_impl.state.bias.r,
ins_impl.state.speed.x,
ins_impl.state.speed.y,
ins_impl.state.speed.z,
ins_impl.state.pos.x,
ins_impl.state.pos.y,
ins_impl.state.pos.z,
ins_impl.state.hb,
ins_impl.state.as);
}
}
#endif
}
void stabilization_attitude_run(bool enable_integrator)
{
/*
* Update reference
*/
static const float dt = (1./PERIODIC_FREQUENCY);
attitude_ref_quat_float_update(&att_ref_quat_f, &stab_att_sp_quat, dt);
/*
* Compute errors for feedback
*/
/* attitude error */
struct FloatQuat att_err;
struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
float_quat_inv_comp(&att_err, att_quat, &att_ref_quat_f.quat);
/* wrap it in the shortest direction */
float_quat_wrap_shortest(&att_err);
/* rate error */
struct FloatRates rate_err;
struct FloatRates *body_rate = stateGetBodyRates_f();
RATES_DIFF(rate_err, att_ref_quat_f.rate, *body_rate);
/* rate_d error */
RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
RATES_COPY(last_body_rate, *body_rate);
#define INTEGRATOR_BOUND 1.0
/* 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 */
float_quat_identity(&stabilization_att_sum_err_quat);
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &att_ref_quat_f.accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d,
&stabilization_att_sum_err_quat);
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];
#ifdef HAS_SURFACE_COMMANDS
stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] +
stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] +
stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] +
stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
#endif
/* 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 FloatQuat att_err;
struct FloatQuat* att_quat = stateGetNedToBodyQuat_f();
FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
/* rate error */
struct FloatRates rate_err;
struct FloatRates* body_rate = stateGetBodyRates_f();
RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
/* rate_d error */
struct FloatRates body_rate_d;
RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
RATES_COPY(last_body_rate, *body_rate);
/* integrated error */
if (enable_integrator) {
struct FloatQuat 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;
FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat);
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];
#ifdef HAS_SURFACE_COMMANDS
stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
#endif
/* 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 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);
}
void ahrs_propagate(float dt)
{
struct FloatVect3 accel;
struct FloatRates body_rates;
// realign all the filter if needed
// a complete init cycle is required
if (ins_impl.reset) {
ins_impl.reset = FALSE;
ins.status = INS_UNINIT;
ahrs.status = AHRS_UNINIT;
init_invariant_state();
}
// fill command vector
struct Int32Rates gyro_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro);
RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
struct Int32Vect3 accel_meas_body;
int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);
// update correction gains
error_output(&ins_impl);
// propagate model
struct inv_state new_state;
runge_kutta_4_float((float *)&new_state,
(float *)&ins_impl.state, INV_STATE_DIM,
(float *)&ins_impl.cmd, INV_COMMAND_DIM,
invariant_model, dt);
ins_impl.state = new_state;
// normalize quaternion
FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);
// set global state
stateSetNedToBodyQuat_f(&ins_impl.state.quat);
RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
stateSetBodyRates_f(&body_rates);
stateSetPositionNed_f(&ins_impl.state.pos);
stateSetSpeedNed_f(&ins_impl.state.speed);
// untilt accel and remove gravity
struct FloatQuat q_b2n;
float_quat_invert(&q_b2n, &ins_impl.state.quat);
float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel);
VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
VECT3_ADD(accel, A);
stateSetAccelNed_f((struct NedCoor_f *)&accel);
//------------------------------------------------------------//
#if SEND_INVARIANT_FILTER
struct FloatEulers eulers;
FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
RunOnceEvery(3, {
pprz_msg_send_INV_FILTER(trans, dev, AC_ID,
&ins_impl.state.quat.qi,
&eulers.phi,
&eulers.theta,
&eulers.psi,
&ins_impl.state.speed.x,
&ins_impl.state.speed.y,
&ins_impl.state.speed.z,
&ins_impl.state.pos.x,
&ins_impl.state.pos.y,
&ins_impl.state.pos.z,
&ins_impl.state.bias.p,
&ins_impl.state.bias.q,
&ins_impl.state.bias.r,
&ins_impl.state.as,
&ins_impl.state.hb,
&ins_impl.meas.baro_alt,
&ins_impl.meas.pos_gps.z)
});
#endif
#if LOG_INVARIANT_FILTER
if (pprzLogFile.fs != NULL) {
if (!log_started) {
// log file header
sdLogWriteLog(&pprzLogFile,
"p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
log_started = TRUE;
} else {
sdLogWriteLog(&pprzLogFile,
"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
ins_impl.cmd.rates.p,
ins_impl.cmd.rates.q,
ins_impl.cmd.rates.r,
ins_impl.cmd.accel.x,
ins_impl.cmd.accel.y,
ins_impl.cmd.accel.z,
ins_impl.meas.pos_gps.x,
ins_impl.meas.pos_gps.y,
ins_impl.meas.pos_gps.z,
ins_impl.meas.speed_gps.x,
ins_impl.meas.speed_gps.y,
ins_impl.meas.speed_gps.z,
ins_impl.meas.mag.x,
ins_impl.meas.mag.y,
ins_impl.meas.mag.z,
ins_impl.meas.baro_alt,
ins_impl.state.quat.qi,
ins_impl.state.quat.qx,
ins_impl.state.quat.qy,
ins_impl.state.quat.qz,
ins_impl.state.bias.p,
ins_impl.state.bias.q,
ins_impl.state.bias.r,
ins_impl.state.speed.x,
ins_impl.state.speed.y,
ins_impl.state.speed.z,
ins_impl.state.pos.x,
ins_impl.state.pos.y,
ins_impl.state.pos.z,
ins_impl.state.hb,
ins_impl.state.as);
}
}
#endif
}
