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);

}
Beispiel #3
0
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();

}
Beispiel #6
0
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();
}
Beispiel #7
0
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);
}
Beispiel #17
0
/**
 * 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
}