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

}
Exemplo n.º 3
0
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_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);

}