C++ (Cpp) int32_quat_inv_comp Beispiele

Beispiel #1

Datei: stabilization_attitude_quat_indi.c Projekt: nvelozsavino/paparazzi

void stabilization_attitude_run(bool_t enable_integrator)
{

    /* Propagate the second order filter on the gyroscopes */
    struct FloatRates *body_rates = stateGetBodyRates_f();
    stabilization_indi_second_order_filter(body_rates, &indi.filtered_rate_2deriv, &indi.filtered_rate_deriv,
                                           &indi.filtered_rate, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);

    /* attitude error                          */
    struct Int32Quat att_err;
    struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
//   INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_sp_quat);
    int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
    /* wrap it in the shortest direction       */
    int32_quat_wrap_shortest(&att_err);
    int32_quat_normalize(&att_err);

    /* compute the INDI command */
    attitude_run_indi(stabilization_att_indi_cmd, &att_err, enable_integrator);

    stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
    stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
    stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_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);
}

Beispiel #2

Datei: stabilization_indi.c Projekt: Merafour/paparazzi

void stabilization_indi_run(bool enable_integrator __attribute__((unused)), bool rate_control)
{
  /* attitude error                          */
  struct Int32Quat att_err;
  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
  int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
  /* wrap it in the shortest direction       */
  int32_quat_wrap_shortest(&att_err);
  int32_quat_normalize(&att_err);

  /* compute the INDI command */
  stabilization_indi_calc_cmd(stabilization_att_indi_cmd, &att_err, rate_control);

  /* copy the INDI command */
  stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
  stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
  stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_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);
}