C++ (Cpp) RMAT_BFP_OF_REAL Examples

Example #1

File: test_algebra.c Project: EwoudSmeur/paparazzi

float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display)
{

  struct FloatRMat ma2b_f;
  float_rmat_comp_inv(&ma2b_f, &ma2c_f, &mb2c_f);
  struct Int32RMat ma2c_i;
  RMAT_BFP_OF_REAL(ma2c_i, ma2c_f);
  struct Int32RMat mb2c_i;
  RMAT_BFP_OF_REAL(mb2c_i, mb2c_f);
  struct Int32RMat ma2b_i;
  int32_rmat_comp_inv(&ma2b_i, &ma2c_i, &mb2c_i);

  struct FloatRMat err;
  RMAT_DIFF(err, ma2b_f, ma2b_i);
  float norm_err = FLOAT_RMAT_NORM(err);

  if (display) {
    printf("rmap comp_inv\n");
    DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2b_f);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2b_i);
  }

  return norm_err;

}

Example #2

File: pprz_orientation_conversion.c Project: CheBuzz/paparazzi

void orientationCalcRMat_i(struct OrientationReps* orientation) {
  if (bit_is_set(orientation->status, ORREP_RMAT_I))
    return;

  if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
    RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
  }
  else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
    INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
  }
  else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
    INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
  }
  else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
    QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
    SetBit(orientation->status, ORREP_QUAT_I);
    INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
  }
  else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
    EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
    SetBit(orientation->status, ORREP_EULER_I);
    INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
  }
  /* set bit to indicate this representation is computed */
  SetBit(orientation->status, ORREP_RMAT_I);
}

Example #3

File: test_algebra.c Project: EwoudSmeur/paparazzi

float test_eulers_of_rmat(struct FloatRMat frm, int display)
{

  struct FloatEulers fe;
  float_eulers_of_rmat(&fe, &frm);
  struct Int32RMat irm;
  RMAT_BFP_OF_REAL(irm, frm);
  struct Int32Eulers ie;
  int32_eulers_of_rmat(&ie, &irm);
  struct FloatEulers fe2;
  EULERS_FLOAT_OF_BFP(fe2, ie);
  EULERS_SUB(fe2, ie);
  float norm_err = FLOAT_EULERS_NORM(fe2);

  if (display) {
    printf("euler of rmat\n");
    //    DISPLAY_FLOAT_RMAT("fr", fr);
    DISPLAY_FLOAT_EULERS_DEG("fe", fe);
    DISPLAY_INT32_EULERS("ie", ie);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie);
  }

  return norm_err;

}

Example #4

File: pprz_orientation_conversion.c Project: 2770862886/paparazzi

void orientationCalcEulers_i(struct OrientationReps* orientation)
{
  if (bit_is_set(orientation->status, ORREP_EULER_I)) {
    return;
  }

  if (bit_is_set(orientation->status, ORREP_EULER_F)) {
    EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
  } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
    int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
  } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
    int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
  } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
    RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
    SetBit(orientation->status, ORREP_RMAT_I);
    int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
  } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
    QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
    SetBit(orientation->status, ORREP_QUAT_I);
    int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
  }
  /* set bit to indicate this representation is computed */
  SetBit(orientation->status, ORREP_EULER_I);
}