C++ (Cpp) orientationGetEulers_f Examples

Example #1

File: imu.c Project: elemhsb/paparazzi

void imu_SetBodyToImuCurrent(float set)
{
  imu.b2i_set_current = set;

  if (imu.b2i_set_current) {
    // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
    struct FloatEulers body_to_imu_eulers;
    memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
    if (stateIsAttitudeValid()) {
      // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
      body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
      body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
      orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
      orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
    } else {
      // indicate that we couldn't set to current roll/pitch
      imu.b2i_set_current = FALSE;
    }
  } else {
    // reset to BODY_TO_IMU as defined in airframe file
    struct FloatEulers body_to_imu_eulers =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
    orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
    orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
  }
}

Example #2

File: imu.c Project: Azaddien/paparazzi

void imu_SetBodyToImuPsi(float psi)
{
  struct FloatEulers body_to_imu_eulers;
  memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
  body_to_imu_eulers.psi = psi;
  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}

Example #3

File: ahrs_float_dcm.c Project: jmavi/paparazzi_fix

void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i)
{
  orientationSetQuat_f(&ahrs_dcm.body_to_imu, q_b2i);

  if (!ahrs_dcm.is_aligned) {
    /* Set ltp_to_imu so that body is zero */
    ahrs_dcm.ltp_to_imu_euler = *orientationGetEulers_f(&ahrs_dcm.body_to_imu);
  }
}

Example #4

File: imu.c Project: KISSMonX/paparazzi

void imu_SetBodyToImuPsi(float psi) {
  struct FloatEulers body_to_imu_eulers;
  memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
  body_to_imu_eulers.psi = psi;
  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
  orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
}

Example #5

File: imu.c Project: KISSMonX/paparazzi

void imu_SetBodyToImuTheta(float theta) {
  struct FloatEulers body_to_imu_eulers;
  memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
  body_to_imu_eulers.theta = theta;
  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
  orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
}

Example #6

File: ahrs_float_dcm.c Project: Sniper91/paparazzi

void ahrs_init(void) {
  ahrs.status = AHRS_UNINIT;

  /* Set ltp_to_imu so that body is zero */
  memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_f(&imu.body_to_imu),
         sizeof(struct FloatEulers));

  FLOAT_RATES_ZERO(ahrs_impl.imu_rate);

  /* set inital filter dcm */
  set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu));

  ahrs_impl.gps_speed = 0;
  ahrs_impl.gps_acceleration = 0;
  ahrs_impl.gps_course = 0;
  ahrs_impl.gps_course_valid = FALSE;
  ahrs_impl.gps_age = 100;
}