C++ (Cpp) INT32_RMAT_ZERO Beispiele

Beispiel #1

Datei: imu.c Projekt: FW-M/paparazzi

void imu_init(void) {

  /* initialises neutrals */
  RATES_ASSIGN(imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);
  VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
  VECT3_ASSIGN(imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);

  /*
    Compute quaternion and rotation matrix
    for conversions between body and imu frame
  */
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined  IMU_BODY_TO_IMU_PSI
  struct Int32Eulers body_to_imu_eulers =
    { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) };
  INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
  INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
  INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
#else
  INT32_QUAT_ZERO(imu.body_to_imu_quat);
  INT32_RMAT_ZERO(imu.body_to_imu_rmat);
#endif

  imu_impl_init();
}

Beispiel #2

Datei: ahrs_int_cmpl_quat.c Projekt: ERAUBB/paparazzi

void ahrs_init(void) {

  ahrs.status = AHRS_UNINIT;
  ahrs_impl.ltp_vel_norm_valid = FALSE;
  ahrs_impl.heading_aligned = FALSE;

  /* set ltp_to_body to zero */
  INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
  INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
  INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
  INT_RATES_ZERO(ahrs.body_rate);

  /* set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
  RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
  INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
  INT_RATES_ZERO(ahrs.imu_rate);

  INT_RATES_ZERO(ahrs_impl.gyro_bias);
  INT_RATES_ZERO(ahrs_impl.rate_correction);
  INT_RATES_ZERO(ahrs_impl.high_rez_bias);

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  ahrs_impl.correct_gravity = TRUE;
#else
  ahrs_impl.correct_gravity = FALSE;
#endif

#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  ahrs_impl.use_gravity_heuristic = TRUE;
#else
  ahrs_impl.use_gravity_heuristic = FALSE;
#endif

}

Beispiel #3

Datei: ahrs_int_cmpl.c Projekt: cbeighley/paparazzi

void ahrs_init(void) {

  ahrs.status = AHRS_UNINIT;

  // FIXME: make ltp_to_imu and ltp_to_body coherent
  INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
  INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
  INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
  INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
  INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
  INT_RATES_ZERO(ahrs.body_rate);
  INT_RATES_ZERO(ahrs.imu_rate);

  INT_RATES_ZERO(ahrs_impl.gyro_bias);
  INT_RATES_ZERO(ahrs_impl.rate_correction);
  INT_RATES_ZERO(ahrs_impl.high_rez_bias);

}

Beispiel #4

Datei: ahrs_int_cmpl_euler.c Projekt: Paolo-Maffei/lxyppc-tetrix

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

  /* set ltp_to_body to zero */
  INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
  INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
  INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
  INT_RATES_ZERO(ahrs.body_rate);

  /* set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
  RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
  INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
  INT_RATES_ZERO(ahrs.imu_rate);

  INT_RATES_ZERO(ahrs_impl.gyro_bias);
  ahrs_impl.reinj_1 = FACE_REINJ_1;

#ifdef IMU_MAG_OFFSET
  ahrs_mag_offset = IMU_MAG_OFFSET;
#else
  ahrs_mag_offset = 0.;
#endif
}