コード例 #1
0
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);

}
コード例 #2
0
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);

}
コード例 #3
0
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);

}