当前位置: 首页>>代码示例>>C++>>正文

C++ VECT3_COPY函数代码示例

本文整理汇总了C++中VECT3_COPY函数的典型用法代码示例。如果您正苦于以下问题：C++ VECT3_COPY函数的具体用法？C++ VECT3_COPY怎么用？C++ VECT3_COPY使用的例子？那么, 这里精选的函数代码示例或许可以为您提供帮助。


在下文中一共展示了VECT3_COPY函数的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: nav_init

void nav_init(void)
{
  waypoints_init();

  nav_block = 0;
  nav_stage = 0;
  nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
  nav_flight_altitude = nav_altitude;
  flight_altitude = SECURITY_ALT;
  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
  VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i);

  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
  vertical_mode = VERTICAL_MODE_ALT;

  nav_roll = 0;
  nav_pitch = 0;
  nav_heading = 0;
  nav_radius = DEFAULT_CIRCLE_RADIUS;
  nav_throttle = 0;
  nav_climb = 0;
  nav_leg_progress = 0;
  nav_leg_length = 1;

  too_far_from_home = FALSE;
  dist2_to_home = 0;
  dist2_to_wp = 0;

#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status);
  register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved);
#endif
}
开发者ID:dohamann，项目名称:paparazzi，代码行数:33，代码来源:navigation.c

示例2: ins_update_gps

void ins_update_gps(void) {

  if (gps_state.fix == GPS_FIX_3D) {
    if (!ins.ltp_initialised) {
      ltp_def_from_ecef_i(&ins.ltp_def, &gps_state.ecef_pos);
      ins.ltp_initialised = TRUE;
    }
    ned_of_ecef_point_i(&ins.gps_pos_cm_ned, &ins.ltp_def, &gps_state.ecef_pos);
    ned_of_ecef_vect_i(&ins.gps_speed_cm_s_ned, &ins.ltp_def, &gps_state.ecef_speed);
#ifdef USE_HFF
    b2ins_update_gps();
    VECT2_SDIV(ins.ltp_pos, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)), b2ins_pos_ltp);
    VECT2_SDIV(ins.ltp_speed, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)), b2ins_speed_ltp);
#else
    INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, ins.gps_pos_cm_ned, 
		INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); 
    INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, ins.gps_speed_cm_s_ned,
		INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); 
    VECT3_COPY(ins.ltp_pos,   b2ins_meas_gps_pos_ned);
    VECT3_COPY(ins.ltp_speed, b2ins_meas_gps_speed_ned);
#endif
    INT32_VECT3_ENU_OF_NED(ins.enu_pos, ins.ltp_pos);
    INT32_VECT3_ENU_OF_NED(ins.enu_speed, ins.ltp_speed);
    INT32_VECT3_ENU_OF_NED(ins.enu_accel, ins.ltp_accel);
  }

}
开发者ID:OpenUAS，项目名称:wasp，代码行数:27，代码来源:booz2_ins.c

示例3: imu_aspirin_event

void imu_aspirin_event(void)
{
  adxl345_spi_event(&imu_aspirin.acc_adxl);
  if (imu_aspirin.acc_adxl.data_available) {
    VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
    imu_aspirin.acc_adxl.data_available = FALSE;
    imu_aspirin.accel_valid = TRUE;
  }

  /* If the itg3200 I2C transaction has succeeded: convert the data */
  itg3200_event(&imu_aspirin.gyro_itg);
  if (imu_aspirin.gyro_itg.data_available) {
    RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates);
    imu_aspirin.gyro_itg.data_available = FALSE;
    imu_aspirin.gyro_valid = TRUE;
  }

  /* HMC58XX event task */
  hmc58xx_event(&imu_aspirin.mag_hmc);
  if (imu_aspirin.mag_hmc.data_available) {
#ifdef IMU_ASPIRIN_VERSION_1_0
    VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect);
#else // aspirin 1.5 with hmc5883
    imu.mag_unscaled.x =  imu_aspirin.mag_hmc.data.vect.y;
    imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x;
    imu.mag_unscaled.z =  imu_aspirin.mag_hmc.data.vect.z;
#endif
    imu_aspirin.mag_hmc.data_available = FALSE;
    imu_aspirin.mag_valid = TRUE;
  }
}
开发者ID:jornanke，项目名称:paparazzi，代码行数:31，代码来源:imu_aspirin.c

示例4: nps_sensor_gps_run_step

void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {

  if (time < gps->next_update)
    return;


  /*
   * simulate speed sensor
   */
  struct DoubleVect3 cur_speed_reading;
  VECT3_COPY(cur_speed_reading, fdm.ecef_ecef_vel);
  /* add a gaussian noise */
  double_vect3_add_gaussian_noise(&cur_speed_reading, &gps->speed_noise_std_dev);

  /* store that for later and retrieve a previously stored data */
  UpdateSensorLatency(time, &cur_speed_reading, &gps->speed_history, gps->speed_latency, &gps->ecef_vel);


  /*
   * simulate position sensor
   */
  /* compute gps error readings */
  struct DoubleVect3 pos_error;
  VECT3_COPY(pos_error, gps->pos_bias_initial);
  /* add a gaussian noise */
  double_vect3_add_gaussian_noise(&pos_error, &gps->pos_noise_std_dev);
  /* update random walk bias and add it to error*/
  double_vect3_update_random_walk(&gps->pos_bias_random_walk_value, &gps->pos_bias_random_walk_std_dev, NPS_GPS_DT, 5.);
  VECT3_ADD(pos_error, gps->pos_bias_random_walk_value);

  /* add error to current pos reading */
  struct DoubleVect3 cur_pos_reading;
  VECT3_COPY(cur_pos_reading, fdm.ecef_pos);
  VECT3_ADD(cur_pos_reading, pos_error);

  /* store that for later and retrieve a previously stored data */
  UpdateSensorLatency(time, &cur_pos_reading, &gps->pos_history, gps->pos_latency, &gps->ecef_pos);


  /*
   * simulate lla pos
   */
  /* convert current ecef reading to lla */
  struct LlaCoor_d cur_lla_reading;
  lla_of_ecef_d(&cur_lla_reading, (EcefCoor_d*) &cur_pos_reading);

  /* store that for later and retrieve a previously stored data */
  UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos);

  double cur_hmsl_reading = fdm.hmsl;
  UpdateSensorLatency_Single(time, &cur_hmsl_reading, &gps->hmsl_history, gps->pos_latency, &gps->hmsl);

  gps->next_update += NPS_GPS_DT;
  gps->data_available = TRUE;

}
开发者ID:AntoineBlais，项目名称:paparazzi，代码行数:56，代码来源:nps_sensor_gps.c

示例5: sim_overwrite_ins

void sim_overwrite_ins(void) {

  struct NedCoor_f ltp_pos;
  VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
  stateSetPositionNed_f(&ltp_pos);

  struct NedCoor_f ltp_speed;
  VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
  stateSetSpeedNed_f(&ltp_speed);

  struct NedCoor_f ltp_accel;
  VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
  stateSetAccelNed_f(&ltp_accel);

}
开发者ID:Azaddien，项目名称:paparazzi，代码行数:15，代码来源:nps_autopilot_rotorcraft.c

示例6: imu_aspirin2_event

void imu_aspirin2_event(void)
{
  mpu60x0_spi_event(&imu_aspirin2.mpu);
  if (imu_aspirin2.mpu.data_available) {
    /* HMC5883 has xzy order of axes in returned data */
    struct Int32Vect3 mag;
    mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
    mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
    mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#ifdef LISA_M_LONGITUDINAL_X
    RATES_ASSIGN(imu.gyro_unscaled,
                 imu_aspirin2.mpu.data_rates.rates.q,
                 -imu_aspirin2.mpu.data_rates.rates.p,
                 imu_aspirin2.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled,
                 imu_aspirin2.mpu.data_accel.vect.y,
                 -imu_aspirin2.mpu.data_accel.vect.x,
                 imu_aspirin2.mpu.data_accel.vect.z);
    VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#else
#ifdef LISA_S
#ifdef LISA_S_UPSIDE_DOWN
    RATES_ASSIGN(imu.gyro_unscaled,
                 imu_aspirin2.mpu.data_rates.rates.p,
                 -imu_aspirin2.mpu.data_rates.rates.q,
                 -imu_aspirin2.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled,
                 imu_aspirin2.mpu.data_accel.vect.x,
                 -imu_aspirin2.mpu.data_accel.vect.y,
                 -imu_aspirin2.mpu.data_accel.vect.z);
    VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
#else
    RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
    VECT3_COPY(imu.mag_unscaled, mag);
#endif
#else
    RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
    VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
#endif
    imu_aspirin2.mpu.data_available = FALSE;
    imu_aspirin2.gyro_valid = TRUE;
    imu_aspirin2.accel_valid = TRUE;
    imu_aspirin2.mag_valid = TRUE;
  }
}
开发者ID:1bitsquared，项目名称:paparazzi，代码行数:48，代码来源:imu_aspirin_2_spi.c

示例7: nav_init_stage

void nav_init_stage(void)
{
  VECT3_COPY(nav_last_point, *stateGetPositionEnu_i());
  stage_time = 0;
  nav_circle_radians = 0;
  //horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
}
开发者ID:dohamann，项目名称:paparazzi，代码行数:7，代码来源:navigation.c

示例8: imu_navstik_event

/**
 * Handle all the events of the Navstik IMU components.
 * When there is data available convert it to the correct axis and save it in the imu structure.
 */
void imu_navstik_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  /* MPU-60x0 event taks */
  mpu60x0_i2c_event(&imu_navstik.mpu);

  if (imu_navstik.mpu.data_available) {
    /* default orientation as should be printed on the pcb, z-down, ICs down */
    RATES_COPY(imu.gyro_unscaled, imu_navstik.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_navstik.mpu.data_accel.vect);

    imu_navstik.mpu.data_available = false;
    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
  }

  /* HMC58XX event task */
  hmc58xx_event(&imu_navstik.hmc);
  if (imu_navstik.hmc.data_available) {
    imu.mag_unscaled.x =  imu_navstik.hmc.data.vect.y;
    imu.mag_unscaled.y = -imu_navstik.hmc.data.vect.x;
    imu.mag_unscaled.z =  imu_navstik.hmc.data.vect.z;
    imu_navstik.hmc.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
  }
}
开发者ID:enacuavlab，项目名称:paparazzi，代码行数:34，代码来源:imu_navstik.c

示例9: nps_sensor_gyro_run_step

void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct DoubleRMat* body_to_imu) {

  if (time < gyro->next_update)
    return;

  /* transform body rates to IMU frame */
  struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_inertial_rotvel);
  struct DoubleVect3 rate_imu;
  MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body );
  /* compute gyros readings */
  MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu);
  VECT3_ADD(gyro->value, gyro->neutral);
  /* compute gyro error readings */
  struct DoubleVect3 gyro_error;
  VECT3_COPY(gyro_error, gyro->bias_initial);
  double_vect3_add_gaussian_noise(&gyro_error, &gyro->noise_std_dev);
  double_vect3_update_random_walk(&gyro->bias_random_walk_value, &gyro->bias_random_walk_std_dev,
				  NPS_GYRO_DT, 5.);
  VECT3_ADD(gyro_error, gyro->bias_random_walk_value);

  struct DoubleVect3 gain = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR};
  VECT3_EW_MUL(gyro_error, gyro_error, gain);

  VECT3_ADD(gyro->value, gyro_error);

  /* round signal to account for adc discretisation */
  DOUBLE_VECT3_ROUND(gyro->value);
  /* saturate                                       */
  VECT3_BOUND_CUBE(gyro->value, gyro->min, gyro->max);

  gyro->next_update += NPS_GYRO_DT;
  gyro->data_available = TRUE;
}
开发者ID:jziesing，项目名称:paparazzi，代码行数:33，代码来源:nps_sensor_gyro.c

示例10: imu_mpu_hmc_event

void imu_mpu_hmc_event(void)
{
    uint32_t now_ts = get_sys_time_usec();

    mpu60x0_spi_event(&imu_mpu_hmc.mpu);
    if (imu_mpu_hmc.mpu.data_available) {
        RATES_COPY(imu.gyro_unscaled, imu_mpu_hmc.mpu.data_rates.rates);
        VECT3_COPY(imu.accel_unscaled, imu_mpu_hmc.mpu.data_accel.vect);
        imu_mpu_hmc.mpu.data_available = false;
        imu_scale_gyro(&imu);
        imu_scale_accel(&imu);
        AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
        AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
    }

    /* HMC58XX event task */
    hmc58xx_event(&imu_mpu_hmc.hmc);
    if (imu_mpu_hmc.hmc.data_available) {
        /* mag rotated by 90deg around z axis relative to MPU */
        imu.mag_unscaled.x =  imu_mpu_hmc.hmc.data.vect.y;
        imu.mag_unscaled.y = -imu_mpu_hmc.hmc.data.vect.x;
        imu.mag_unscaled.z =  imu_mpu_hmc.hmc.data.vect.z;
        imu_mpu_hmc.hmc.data_available = false;
        imu_scale_mag(&imu);
        AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
    }
}
开发者ID:coppolam，项目名称:paparazzi，代码行数:27，代码来源:imu_mpu6000_hmc5883.c

示例11: mag_hmc58xx_module_event

void mag_hmc58xx_module_event(void)
{
  hmc58xx_event(&mag_hmc58xx);

  if (mag_hmc58xx.data_available) {
#if MODULE_HMC58XX_UPDATE_AHRS
    // current timestamp
    uint32_t now_ts = get_sys_time_usec();

    // set channel order
    struct Int32Vect3 mag = {
      (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
      (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
      (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
    };
    // unscaled vector
    VECT3_COPY(imu.mag_unscaled, mag);
    // scale vector
    imu_scale_mag(&imu);

    AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag);
#endif
#if MODULE_HMC58XX_SYNC_SEND
    mag_hmc58xx_report();
#endif
#if MODULE_HMC58XX_UPDATE_AHRS ||  MODULE_HMC58XX_SYNC_SEND
    mag_hmc58xx.data_available = FALSE;
#endif
  }
}
开发者ID:Azaddien，项目名称:paparazzi，代码行数:30，代码来源:mag_hmc58xx.c

示例12: imu_periodic

void imu_periodic( void )
{
  // Start reading the latest gyroscope data
  if (!imu_krooz.mpu.config.initialized)
    mpu60x0_i2c_start_configure(&imu_krooz.mpu);

  if (!imu_krooz.hmc.initialized)
    hmc58xx_start_configure(&imu_krooz.hmc);

  if (imu_krooz.meas_nb) {
    RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
    VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);

#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
    UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
    VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
    VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
    VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
    VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);

    RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
    VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
    imu_krooz.meas_nb = 0;

    imu_krooz.gyr_valid = TRUE;
    imu_krooz.acc_valid = TRUE;
  }

  //RunOnceEvery(10,imu_krooz_downlink_raw());
}
开发者ID:1bitsquared，项目名称:paparazzi，代码行数:31，代码来源:imu_krooz.c

示例13: ltp_def_from_ecef_i

void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {

  /* store the origin of the tangeant plane */
  VECT3_COPY(def->ecef, *ecef);
  /* compute the lla representation of the origin */
  lla_of_ecef_i(&def->lla, &def->ecef);
  /* store the rotation matrix                    */

#if 1
  int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#else
  int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
  int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
  int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#endif


  def->ltp_of_ecef.m[0] = -sin_lon;
  def->ltp_of_ecef.m[1] =  cos_lon;
  def->ltp_of_ecef.m[2] =  0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
  def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[5] =  cos_lat;
  def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
  def->ltp_of_ecef.m[8] =  sin_lat;

}
开发者ID:HolyFox812，项目名称:paparazzi，代码行数:32，代码来源:pprz_geodetic_int.c

示例14: imu_mpu9250_event

void imu_mpu9250_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  // If the MPU9250 I2C transaction has succeeded: convert the data
  mpu9250_i2c_event(&imu_mpu9250.mpu);

  if (imu_mpu9250.mpu.data_available) {
    // set channel order
    struct Int32Vect3 accel = {
      IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
      IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
    };
    struct Int32Rates rates = {
      IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
      IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
    };
    // unscaled vector
    VECT3_COPY(imu.accel_unscaled, accel);
    RATES_COPY(imu.gyro_unscaled, rates);

    imu_mpu9250.mpu.data_available = false;

    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
  }
#if IMU_MPU9250_READ_MAG
  // Test if mag data are updated
  if (imu_mpu9250.mpu.akm.data_available) {
    struct Int32Vect3 mag = {
      IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]),
      -IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z])
    };
    VECT3_COPY(imu.mag_unscaled, mag);
    imu_mpu9250.mpu.akm.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);

  }
#endif
}
开发者ID:HWal，项目名称:paparazzi，代码行数:46，代码来源:imu_mpu9250_i2c.c

示例15: imu_mpu9250_event

void imu_mpu9250_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  // If the MPU9250 SPI transaction has succeeded: convert the data
  mpu9250_spi_event(&imu_mpu9250.mpu);

  if (imu_mpu9250.mpu.data_available) {
    // set channel order
    struct Int32Vect3 accel = {
      IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
      IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
    };
    struct Int32Rates rates = {
      IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
      IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
    };
    // unscaled vector
    VECT3_COPY(imu.accel_unscaled, accel);
    RATES_COPY(imu.gyro_unscaled, rates);

#if IMU_MPU9250_READ_MAG
    if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0
      /** FIXME: assumes that we get new mag data each time instead of reading drdy bit */
      struct Int32Vect3 mag;
      mag.x =  (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y);
      mag.y =  (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X);
      mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z);
      VECT3_COPY(imu.mag_unscaled, mag);
      imu_scale_mag(&imu);
      AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
    }
#endif

    imu_mpu9250.mpu.data_available = false;

    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
  }

}
开发者ID:enacuavlab，项目名称:paparazzi，代码行数:45，代码来源:imu_mpu9250_spi.c


注：本文中的VECT3_COPY函数示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。