C++ (Cpp) FLOAT_RMAT_ZERO示例

示例#1

文件： ahrs_float_dcm.c 项目： maxcht/paparazzi

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

  /*
   * Initialises our IMU alignement variables
   * This should probably done in the IMU code instead
   */
  struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

  /* set ltp_to_body to zero */
  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
  FLOAT_RATES_ZERO(ahrs_float.body_rate);

  /* set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
  RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
  EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
  FLOAT_RATES_ZERO(ahrs_float.imu_rate);

  /* set inital filter dcm */
  set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
}

示例#2

文件： imu.c 项目： FW-M/paparazzi

void imu_float_init(struct ImuFloat* imuf) {

  /*
    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
  EULERS_ASSIGN(imuf->body_to_imu_eulers,
		IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
  FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers);
  FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat);
  FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
#else
  EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.);
  FLOAT_QUAT_ZERO(imuf->body_to_imu_quat);
  FLOAT_RMAT_ZERO(imuf->body_to_imu_rmat);
#endif

}

示例#3

文件： ahrs_sim.c 项目： Ludo6431/paparazzi

void ahrs_init(void) {
  //ahrs_float.status = AHRS_UNINIT;
  // set to running for now and only use ahrs.status, not ahrs_float.status
  ahrs.status = AHRS_RUNNING;

  ahrs_sim_available = FALSE;

  /* set ltp_to_body to zero */
  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
  FLOAT_RATES_ZERO(ahrs_float.body_rate);

  /* set ltp_to_imu to same as ltp_to_body, currently no difference simulated */
  QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_body_quat);
  EULERS_COPY(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_body_euler);
  RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_body_rmat);
  RATES_COPY(ahrs_float.imu_rate, ahrs_float.body_rate);
}

示例#4

文件： csc_xsens.c 项目： 0lri/paparazzi

void xsens_init( void )
{
  for (int i = 0; i < XSENS_COUNT; i++) {
    send_ck[i] = 0;
    xsens_msg_status[i] = 0;
    xsens_time_stamp[i] = 0;
    xsens_output_mode[i] = XSENS_DEFAULT_OUTPUT_MODE;
    xsens_output_settings[i] = XSENS_DEFAULT_OUTPUT_SETTINGS;
    xsens_msg_buf_count[i] = 0;
    xsens_msg_buf_pi[i] = 0;
    xsens_msg_buf_ci[i] = 0;
    FLOAT_RMAT_ZERO(xsens_rmat_neutral[i]);
  }
  // Also TODO: set scenario to aerospace
  // set magnetic declination angle
  // Probably quicker to just set everything once via MT Manager software
  // instead of setting via paparazzi (MT-G should store setting in flash)
  //XSENS_GoToConfig();
  //XSENS_SetOutputMode(xsens2_output_mode);
  //XSENS_SetOutputSettings(xsens2_output_settings);
  //XSENS_GoToMeasurment();
}

示例#5

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

    /*
     * Initialises our IMU alignement variables
     * This should probably done in the IMU code instead
     */
    struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
    FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
    FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

    /* set ltp_to_body to zero */
    FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
    FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
    FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
    FLOAT_RATES_ZERO(ahrs_float.body_rate);

    /* set ltp_to_imu so that body is zero */
    QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
    RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
    EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
    FLOAT_RATES_ZERO(ahrs_float.imu_rate);

    /* set inital filter dcm */
    set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);

#if USE_HIGH_ACCEL_FLAG
    high_accel_done = FALSE;
    high_accel_flag = FALSE;
#endif

    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;
}

示例#6

文件： ahrs_float_cmpl_rmat.c 项目： Ludo6431/paparazzi

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

  /* Initialises IMU alignement */
  struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

  /* Set ltp_to_body to zero */
  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
  FLOAT_RATES_ZERO(ahrs_float.body_rate);

  /* Set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
  RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
  EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);

  FLOAT_RATES_ZERO(ahrs_float.imu_rate);

}