Exemplos de EULERS_BFP_OF_REAL em C++ (Cpp)

Exemplo n.º 1

Arquivo: ahrs_float_invariant_wrapper.c Projeto: AFVlab/DroneCompetition

static void send_att(struct transport_tx *trans, struct link_device *dev)
{
  /* compute eulers in int (IMU frame) */
  struct FloatEulers ltp_to_imu_euler;
  float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_float_inv.state.quat);
  struct Int32Eulers eulers_imu;
  EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);

  /* compute Eulers in int (body frame) */
  struct FloatQuat ltp_to_body_quat;
  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
  struct FloatEulers ltp_to_body_euler;
  float_eulers_of_quat(&ltp_to_body_euler, &ltp_to_body_quat);
  struct Int32Eulers eulers_body;
  EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);

  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
                               &eulers_imu.phi,
                               &eulers_imu.theta,
                               &eulers_imu.psi,
                               &eulers_body.phi,
                               &eulers_body.theta,
                               &eulers_body.psi,
                               &ahrs_finv_id);
}

Exemplo n.º 2

Arquivo: pprz_orientation_conversion.c Projeto: 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);
}

Exemplo n.º 3

Arquivo: test_algebra.c Projeto: byrddev/paparazzi

static void test_10(void) {

    struct FloatEulers euler;
    EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
    DISPLAY_FLOAT_EULERS_DEG("euler", euler);
    struct FloatQuat quat;
    FLOAT_QUAT_OF_EULERS(quat, euler);
    DISPLAY_FLOAT_QUAT("####quat", quat);

    struct Int32Eulers euleri;
    EULERS_BFP_OF_REAL(euleri, euler);
    DISPLAY_INT32_EULERS("euleri", euleri);
    struct Int32Quat quati;
    INT32_QUAT_OF_EULERS(quati, euleri);
    DISPLAY_INT32_QUAT("####quat", quati);
    struct Int32RMat rmati;
    INT32_RMAT_OF_EULERS(rmati, euleri);
    DISPLAY_INT32_RMAT("####rmat", rmati);

    struct Int32Quat quat_ltp_to_body;
    struct Int32Quat body_to_imu_quat;
    INT32_QUAT_ZERO( body_to_imu_quat);


    INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati);
    DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);

}

Exemplo n.º 4

Arquivo: test_algebra.c Projeto: EwoudSmeur/paparazzi

static void test_10(void)
{

  struct FloatEulers euler;
  EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
  DISPLAY_FLOAT_EULERS_DEG("euler", euler);
  struct FloatQuat quat;
  float_quat_of_eulers(&quat, &euler);
  DISPLAY_FLOAT_QUAT("####quat", quat);

  struct Int32Eulers euleri;
  EULERS_BFP_OF_REAL(euleri, euler);
  DISPLAY_INT32_EULERS("euleri", euleri);
  struct Int32Quat quati;
  int32_quat_of_eulers(&quati, &euleri);
  DISPLAY_INT32_QUAT("####quat", quati);
  struct Int32RMat rmati;
  int32_rmat_of_eulers(&rmati, &euleri);
  DISPLAY_INT32_RMAT("####rmat", rmati);

  struct Int32Quat quat_ltp_to_body;
  struct Int32Quat body_to_imu_quat;
  int32_quat_identity(&body_to_imu_quat);


  int32_quat_comp_inv(&quat_ltp_to_body, &body_to_imu_quat, &quati);
  DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);

}

Exemplo n.º 5

Arquivo: test_algebra.c Projeto: EwoudSmeur/paparazzi

float test_INT32_QUAT_OF_RMAT(struct FloatEulers *eul_f, bool display)
{
  struct Int32Eulers eul321_i;
  EULERS_BFP_OF_REAL(eul321_i, (*eul_f));

  struct Int32Eulers eul312_i;
  EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
  if (display) { DISPLAY_INT32_EULERS("eul312_i", eul312_i); }

  struct FloatRMat rmat_f;
  float_rmat_of_eulers_321(&rmat_f, eul_f);
  if (display) { DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); }
  if (display) { DISPLAY_FLOAT_RMAT("rmat float", rmat_f); }

  struct Int32RMat rmat_i;
  int32_rmat_of_eulers_321(&rmat_i, &eul321_i);
  if (display) { DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); }
  if (display) { DISPLAY_INT32_RMAT("rmat int", rmat_i); }
  if (display) { DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); }

  struct FloatQuat qf;
  float_quat_of_rmat(&qf, &rmat_f);
  //FLOAT_QUAT_WRAP_SHORTEST(qf);
  if (display) { DISPLAY_FLOAT_QUAT("qf", qf); }

  struct Int32Quat qi;
  int32_quat_of_rmat(&qi, &rmat_i);
  //int32_quat_wrap_shortest(&qi);
  if (display) { DISPLAY_INT32_QUAT("qi", qi); }
  if (display) { DISPLAY_INT32_QUAT_2("qi", qi); }

  struct FloatQuat qif;
  QUAT_FLOAT_OF_BFP(qif, qi);

  // dot product of two quaternions is 1 if they represent same rotation
  float qi_dot_qf = qif.qi * qf.qi + qif.qx * qf.qx + qif.qy * qf.qy + qif.qz * qf.qz;
  float err_norm = fabs(fabs(qi_dot_qf) - 1.);

  if (display) { printf("err %f\n", err_norm); }
  if (display) { printf("\n"); }

  return err_norm;

}

Exemplo n.º 6

Arquivo: nps_autopilot_rotorcraft.c Projeto: ERAUBB/paparazzi

void sim_overwrite_ahrs(void) {

  EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers);

  QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat);

  RATES_BFP_OF_REAL(ahrs.body_rate, fdm.body_ecef_rotvel);

  INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat);

}

Exemplo n.º 7

Arquivo: test_algebra.c Projeto: EwoudSmeur/paparazzi

static void test_2(void)
{

  struct Int32Vect3 v1 = { 5000, 5000, 5000 };
  DISPLAY_INT32_VECT3("v1", v1);

  struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
  DISPLAY_FLOAT_EULERS("euler_f", euler_f);

  struct Int32Eulers euler_i;
  EULERS_BFP_OF_REAL(euler_i, euler_f);
  DISPLAY_INT32_EULERS("euler_i", euler_i);

  struct Int32Quat quat_i;
  int32_quat_of_eulers(&quat_i, &euler_i);
  DISPLAY_INT32_QUAT("quat_i", quat_i);
  int32_quat_normalize(&quat_i);
  DISPLAY_INT32_QUAT("quat_i_n", quat_i);

  struct Int32Vect3 v2;
  int32_quat_vmult(&v2, &quat_i, &v1);
  DISPLAY_INT32_VECT3("v2", v2);

  struct Int32RMat rmat_i;
  int32_rmat_of_quat(&rmat_i, &quat_i);
  DISPLAY_INT32_RMAT("rmat_i", rmat_i);

  struct Int32Vect3 v3;
  INT32_RMAT_VMULT(v3, rmat_i, v1);
  DISPLAY_INT32_VECT3("v3", v3);

  struct Int32RMat rmat_i2;
  int32_rmat_of_eulers(&rmat_i2, &euler_i);
  DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);

  struct Int32Vect3 v4;
  INT32_RMAT_VMULT(v4, rmat_i2, v1);
  DISPLAY_INT32_VECT3("v4", v4);

  struct FloatQuat quat_f;
  float_quat_of_eulers(&quat_f, &euler_f);
  DISPLAY_FLOAT_QUAT("quat_f", quat_f);

  struct FloatVect3 v5;
  VECT3_COPY(v5, v1);
  DISPLAY_FLOAT_VECT3("v5", v5);
  struct FloatVect3 v6;
  float_quat_vmult(&v6, &quat_f, &v5);
  DISPLAY_FLOAT_VECT3("v6", v6);

}

Exemplo n.º 8

Arquivo: test_algebra.c Projeto: byrddev/paparazzi

static void test_2(void) {

    struct Int32Vect3 v1 = { 5000, 5000, 5000 };
    DISPLAY_INT32_VECT3("v1", v1);

    struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
    DISPLAY_FLOAT_EULERS("euler_f", euler_f);

    struct Int32Eulers euler_i;
    EULERS_BFP_OF_REAL(euler_i, euler_f);
    DISPLAY_INT32_EULERS("euler_i", euler_i);

    struct Int32Quat quat_i;
    INT32_QUAT_OF_EULERS(quat_i, euler_i);
    DISPLAY_INT32_QUAT("quat_i", quat_i);
    INT32_QUAT_NORMALIZE(quat_i);
    DISPLAY_INT32_QUAT("quat_i_n", quat_i);

    struct Int32Vect3 v2;
    INT32_QUAT_VMULT(v2, quat_i, v1);
    DISPLAY_INT32_VECT3("v2", v2);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_QUAT(rmat_i, quat_i);
    DISPLAY_INT32_RMAT("rmat_i", rmat_i);

    struct Int32Vect3 v3;
    INT32_RMAT_VMULT(v3, rmat_i, v1);
    DISPLAY_INT32_VECT3("v3", v3);

    struct Int32RMat rmat_i2;
    INT32_RMAT_OF_EULERS(rmat_i2, euler_i);
    DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);

    struct Int32Vect3 v4;
    INT32_RMAT_VMULT(v4, rmat_i2, v1);
    DISPLAY_INT32_VECT3("v4", v4);

    struct FloatQuat quat_f;
    FLOAT_QUAT_OF_EULERS(quat_f, euler_f);
    DISPLAY_FLOAT_QUAT("quat_f", quat_f);

    struct FloatVect3 v5;
    VECT3_COPY(v5, v1);
    DISPLAY_FLOAT_VECT3("v5", v5);
    struct FloatVect3 v6;
    FLOAT_QUAT_VMULT(v6, quat_f, v5);
    DISPLAY_FLOAT_VECT3("v6", v6);

}

Exemplo n.º 9

Arquivo: ahrs_float_cmpl.c Projeto: 1bitsquared/paparazzi

static void send_att(void) {
  struct FloatEulers ltp_to_imu_euler;
  FLOAT_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat);
  struct Int32Eulers euler_i;
  EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
  struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
  DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
      &euler_i.phi,
      &euler_i.theta,
      &euler_i.psi,
      &(eulers_body->phi),
      &(eulers_body->theta),
      &(eulers_body->psi));
}

Exemplo n.º 10

Arquivo: ahrs_float_cmpl.c Projeto: AULA-PPZ/paparazzi

static void send_att(struct transport_tx *trans, struct link_device *dev) {
  struct FloatEulers ltp_to_imu_euler;
  float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_impl.ltp_to_imu_quat);
  struct Int32Eulers euler_i;
  EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
  struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
      &euler_i.phi,
      &euler_i.theta,
      &euler_i.psi,
      &(eulers_body->phi),
      &(eulers_body->theta),
      &(eulers_body->psi));
}

Exemplo n.º 11

Arquivo: test_algebra.c Projeto: byrddev/paparazzi

float test_rmat_of_eulers_312(void) {

    struct FloatEulers eul312_f;
    EULERS_ASSIGN(eul312_f, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
    DISPLAY_FLOAT_EULERS_DEG("eul312_f", eul312_f);
    struct Int32Eulers eul312_i;
    EULERS_BFP_OF_REAL(eul312_i, eul312_f);
    DISPLAY_INT32_EULERS("eul312_i", eul312_i);

    struct FloatRMat rmat_f;
    FLOAT_RMAT_OF_EULERS_312(rmat_f, eul312_f);
    DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);

    return 0;

}

Exemplo n.º 12

Arquivo: test_algebra.c Projeto: byrddev/paparazzi

float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) {

    struct Int32Eulers eul312_i;
    EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
    if (display)  DISPLAY_INT32_EULERS("eul312_i", eul312_i);

    struct FloatRMat rmat_f;
    FLOAT_RMAT_OF_EULERS_312(rmat_f, (*eul_f));
    if (display) DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
    if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
    if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
    if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i);
    if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i);

    struct FloatQuat qf;
    FLOAT_QUAT_OF_RMAT(qf, rmat_f);
    FLOAT_QUAT_WRAP_SHORTEST(qf);
    if (display) DISPLAY_FLOAT_QUAT("qf", qf);

    struct Int32Quat qi;
    INT32_QUAT_OF_RMAT(qi, rmat_i);
    INT32_QUAT_WRAP_SHORTEST(qi);
    if (display) DISPLAY_INT32_QUAT("qi", qi);
    if (display) DISPLAY_INT32_QUAT_2("qi", qi);

    struct FloatQuat qif;
    QUAT_FLOAT_OF_BFP(qif, qi);
    struct FloatQuat qerr;
    QUAT_DIFF(qerr, qif, qf);

    float err_norm = FLOAT_QUAT_NORM(qerr);
    if (display) printf("err %f\n", err_norm);
    if (display) printf("\n");

    return err_norm;

}

Exemplo n.º 13

Arquivo: test_algebra.c Projeto: byrddev/paparazzi

static void test_1(void) {

    struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
    DISPLAY_FLOAT_EULERS("euler_f", euler_f);

    struct Int32Eulers euler_i;
    EULERS_BFP_OF_REAL(euler_i, euler_f);
    DISPLAY_INT32_EULERS("euler_i", euler_i);

    struct FloatQuat quat_f;
    FLOAT_QUAT_OF_EULERS(quat_f, euler_f);
    DISPLAY_FLOAT_QUAT("quat_f", quat_f);

    struct Int32Quat quat_i;
    INT32_QUAT_OF_EULERS(quat_i, euler_i);
    DISPLAY_INT32_QUAT("quat_i", quat_i);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_QUAT(rmat_i, quat_i);
    DISPLAY_INT32_RMAT("rmat_i", rmat_i);

}

Exemplo n.º 14

Arquivo: pprz_orientation_conversion.c Projeto: 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);
}

Exemplo n.º 15

Arquivo: imu_um6.c Projeto: EwoudSmeur/paparazzi

void UM6_packet_read_message(void)
{
  if ((UM6_status == UM6Running) && PacketLength > 11) {
    switch (PacketAddr) {
      case IMU_UM6_GYRO_PROC:
        UM6_rate.p =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352;
        UM6_rate.q =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352;
        UM6_rate.r =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352;
        RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
        RATES_BFP_OF_REAL(imu.gyro, UM6_rate);
        break;
      case IMU_UM6_ACCEL_PROC:
        UM6_accel.x =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105;
        UM6_accel.y =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105;
        UM6_accel.z =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105;
        VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
        ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
        break;
      case IMU_UM6_MAG_PROC:
        UM6_mag.x =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176;
        UM6_mag.y =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176;
        UM6_mag.z =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176;
        // Assume the same units for magnetic field
        // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
        MAGS_BFP_OF_REAL(imu.mag, UM6_mag);
        break;
      case IMU_UM6_EULER:
        UM6_eulers.phi =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863;
        UM6_eulers.theta =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863;
        UM6_eulers.psi =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863;
        EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad
        EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
        break;
      case IMU_UM6_QUAT:
        UM6_quat.qi =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693;
        UM6_quat.qx =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693;
        UM6_quat.qy =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693;
        UM6_quat.qz =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693;
        QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat);
        break;
      default:
        break;
    }
  }
}