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

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

}