Esempio n. 1
0
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);
    }
}
Esempio n. 2
0
void imu_hbmini_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  max1168_event();

  if (max1168_status == MAX1168_DATA_AVAILABLE) {
    imu.gyro_unscaled.p  = max1168_values[IMU_GYRO_P_CHAN];
    imu.gyro_unscaled.q  = max1168_values[IMU_GYRO_Q_CHAN];
    imu.gyro_unscaled.r  = max1168_values[IMU_GYRO_R_CHAN];
    imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN];
    imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
    imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];
    max1168_status = MAX1168_IDLE;
    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_hbmini.hmc);
  if (imu_hbmini.hmc.data_available) {
    imu.mag_unscaled.z = imu_hbmini.hmc.data.value[IMU_MAG_X_CHAN];
    imu.mag_unscaled.y = imu_hbmini.hmc.data.value[IMU_MAG_Y_CHAN];
    imu.mag_unscaled.x = imu_hbmini.hmc.data.value[IMU_MAG_Z_CHAN];

    imu_hbmini.hmc.data_available = FALSE;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
  }

}
Esempio n. 3
0
void imu_px4fmu_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  mpu60x0_spi_event(&imu_px4fmu.mpu);
  if (imu_px4fmu.mpu.data_available) {
    RATES_ASSIGN(imu.gyro_unscaled,
                 imu_px4fmu.mpu.data_rates.rates.q,
                 imu_px4fmu.mpu.data_rates.rates.p,
                 -imu_px4fmu.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled,
                 imu_px4fmu.mpu.data_accel.vect.y,
                 imu_px4fmu.mpu.data_accel.vect.x,
                 -imu_px4fmu.mpu.data_accel.vect.z);
    imu_px4fmu.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_px4fmu.hmc);
  if (imu_px4fmu.hmc.data_available) {
    imu.mag_unscaled.x =  imu_px4fmu.hmc.data.vect.y;
    imu.mag_unscaled.y =  imu_px4fmu.hmc.data.vect.x;
    imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z;
    imu_px4fmu.hmc.data_available = FALSE;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
  }
}
Esempio n. 4
0
/**
 * 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);
  }
}
Esempio n. 5
0
void imu_mpu_spi_event(void)
{
  mpu60x0_spi_event(&imu_mpu_spi.mpu);
  if (imu_mpu_spi.mpu.data_available) {
    uint32_t now_ts = get_sys_time_usec();
    RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
    imu_mpu_spi.mpu.data_available = false;
    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel);
  }
}
Esempio n. 6
0
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
}
Esempio n. 7
0
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);
  }

}
Esempio n. 8
0
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);
  }

  uint32_t now_ts = get_sys_time_usec();

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

    RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
    imu_krooz.meas_nb = 0;
    imu_scale_gyro(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
  }

  if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
    imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
    imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
    imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;

#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);

    INT_VECT3_ZERO(imu_krooz.accel_sum);
    INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
  }

  RunOnceEvery(128, {axis_nb = 5;});
Esempio n. 9
0
void imu_drotek2_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  // If the MPU6050 I2C transaction has succeeded: convert the data
  mpu60x0_i2c_event(&imu_drotek2.mpu);

  if (imu_drotek2.mpu.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
    /* change orientation, so if ICs face up, z-axis is down */
    imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p;
    imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q;
    imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r;
    imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x;
    imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y;
    imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z;
#else
    /* default orientation as should be printed on the pcb, z-down, ICs down */
    RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect);
#endif

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

  /* HMC58XX event task */
  hmc58xx_event(&imu_drotek2.hmc);
  if (imu_drotek2.hmc.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
    imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x;
    imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y;
    imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z;
#else
    VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
#endif
    imu_drotek2.hmc.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_DROTEK_ID, now_ts, &imu.mag);
  }
}
Esempio n. 10
0
void imu_nps_event(void)
{
  uint32_t now_ts = get_sys_time_usec();
  if (imu_nps.gyro_available) {
    imu_nps.gyro_available = FALSE;
    imu_scale_gyro(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
  }
  if (imu_nps.accel_available) {
    imu_nps.accel_available = FALSE;
    imu_scale_accel(&imu);
    AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
  }
  if (imu_nps.mag_available) {
    imu_nps.mag_available = FALSE;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
  }
}
Esempio n. 11
0
/**
 * 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_bebop_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

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

  if (imu_bebop.mpu.data_available) {
    /* default orientation of the MPU is upside down sor corrigate this here */
    RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q,
                 -imu_bebop.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y,
                 -imu_bebop.mpu.data_accel.vect.z);

    imu_bebop.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);
  }

  /* AKM8963 event task */
  ak8963_event(&imu_bebop.ak);

  if (imu_bebop.ak.data_available) {
#if BEBOP_VERSION2
    struct Int32Vect3 mag_temp;
    // In the second bebop version the magneto is turned 90 degrees
    VECT3_ASSIGN(mag_temp, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z);

    // Rotate the magneto
    struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop);
    int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp);
#else //BEBOP regular first verion
    VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z);
#endif

    imu_bebop.ak.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
  }
}
Esempio n. 12
0
void imu_periodic(void)
{
  // Actual Nr of ADC measurements per channel per periodic loop
  static int last_head = 0;

  uint32_t now_ts = get_sys_time_usec();

  imu_overrun = analog_imu_adc_buf[0].head - last_head;
  if (imu_overrun < 0) {
    imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES;
  }
  last_head = analog_imu_adc_buf[0].head;

  // Read All Measurements
#ifdef ADC_CHANNEL_GYRO_P
  imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_GYRO_Q
  imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_GYRO_R
  imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_X
  imu.accel_unscaled.x = analog_imu_adc_buf[3].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_Y
  imu.accel_unscaled.y = analog_imu_adc_buf[4].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_Z
  imu.accel_unscaled.z = analog_imu_adc_buf[5].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif

  imu_scale_gyro(&imu);
  imu_scale_accel(&imu);
  AbiSendMsgIMU_GYRO_INT32(IMU_ANALOG_ID, now_ts, &imu.gyro);
  AbiSendMsgIMU_ACCEL_INT32(IMU_ANALOG_ID, now_ts, &imu.accel);
}
Esempio n. 13
0
/**
 * Event handling for Vectornav
 */
void ahrs_vectornav_event(void)
{
  // receive data
  vn200_event(&(ahrs_vn.vn_packet));

  // read message
  if (ahrs_vn.vn_packet.msg_available) {
    vn200_read_message(&(ahrs_vn.vn_packet),&(ahrs_vn.vn_data));
    ahrs_vn.vn_packet.msg_available = false;

    if (ahrs_vectornav_is_enabled()) {
      ahrs_vectornav_propagate();
    }

    // send ABI messages
    uint32_t now_ts = get_sys_time_usec();
    // in fixed point for sending as ABI and telemetry msgs
    RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro);
    AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i);
    ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel);
    AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i);
  }
}
Esempio n. 14
0
void imu_umarim_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  // If the itg3200 I2C transaction has succeeded: convert the data
  itg3200_event(&imu_umarim.itg);
  if (imu_umarim.itg.data_available) {
    RATES_COPY(imu.gyro_unscaled, imu_umarim.itg.data.rates);
    imu_umarim.itg.data_available = FALSE;
    imu_scale_gyro(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
  }

  // If the adxl345 I2C transaction has succeeded: convert the data
  adxl345_i2c_event(&imu_umarim.adxl);
  if (imu_umarim.adxl.data_available) {
    VECT3_ASSIGN(imu.accel_unscaled, imu_umarim.adxl.data.vect.y, -imu_umarim.adxl.data.vect.x,
                 imu_umarim.adxl.data.vect.z);
    imu_umarim.adxl.data_available = FALSE;
    imu_scale_accel(&imu);
    AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
  }
}
Esempio n. 15
0
void imu_aspirin_i2c_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  adxl345_i2c_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_scale_accel(&imu);
    AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &imu.accel);
  }

  /* 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_scale_gyro(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &imu.gyro);
  }

  /* 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_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN_ID, now_ts, &imu.mag);
  }
}
Esempio n. 16
0
void imu_ppzuav_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  adxl345_i2c_event(&imu_ppzuav.acc_adxl);
  if (imu_ppzuav.acc_adxl.data_available) {
    imu.accel_unscaled.x = -imu_ppzuav.acc_adxl.data.vect.x;
    imu.accel_unscaled.y =  imu_ppzuav.acc_adxl.data.vect.y;
    imu.accel_unscaled.z = -imu_ppzuav.acc_adxl.data.vect.z;
    imu_ppzuav.acc_adxl.data_available = false;
    imu_scale_accel(&imu);
    AbiSendMsgIMU_ACCEL_INT32(IMU_PPZUAV_ID, now_ts, &imu.accel);
  }

  /* If the itg3200 I2C transaction has succeeded: convert the data */
  itg3200_event(&imu_ppzuav.gyro_itg);
  if (imu_ppzuav.gyro_itg.data_available) {
    imu.gyro_unscaled.p = -imu_ppzuav.gyro_itg.data.rates.p;
    imu.gyro_unscaled.q =  imu_ppzuav.gyro_itg.data.rates.q;
    imu.gyro_unscaled.r = -imu_ppzuav.gyro_itg.data.rates.r;
    imu_ppzuav.gyro_itg.data_available = false;
    imu_scale_gyro(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_PPZUAV_ID, now_ts, &imu.gyro);
  }

  /* HMC58XX event task */
  hmc58xx_event(&imu_ppzuav.mag_hmc);
  if (imu_ppzuav.mag_hmc.data_available) {
    imu.mag_unscaled.x = -imu_ppzuav.mag_hmc.data.vect.y;
    imu.mag_unscaled.y = -imu_ppzuav.mag_hmc.data.vect.x;
    imu.mag_unscaled.z = -imu_ppzuav.mag_hmc.data.vect.z;
    imu_ppzuav.mag_hmc.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_PPZUAV_ID, now_ts, &imu.mag);
  }
}
Esempio n. 17
0
/**
 *  Propagate the received states into the vehicle
 *  state machine
 */
void ins_vectornav_propagate()
{
  // Acceleration [m/s^2]
  // in fixed point for sending as ABI and telemetry msgs
  ACCELS_BFP_OF_REAL(ins_vn.accel_i, ins_vn.accel);

  // Rates [rad/s]
  static struct FloatRates body_rate;
  // in fixed point for sending as ABI and telemetry msgs
  RATES_BFP_OF_REAL(ins_vn.gyro_i, ins_vn.gyro);
  float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.gyro); // compute body rates
  stateSetBodyRates_f(&body_rate);   // Set state [rad/s]

  // Attitude [deg]
  ins_vectornav_yaw_pitch_roll_to_attitude(&ins_vn.attitude); // convert to correct units and axis [rad]
  static struct FloatQuat imu_quat; // convert from euler to quat
  float_quat_of_eulers(&imu_quat, &ins_vn.attitude);
  static struct FloatRMat imu_rmat; // convert from quat to rmat
  float_rmat_of_quat(&imu_rmat, &imu_quat);
  static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
  float_rmat_comp(&ltp_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu));
  stateSetNedToBodyRMat_f(&ltp_to_body_rmat); // set body states [rad]

  // NED (LTP) velocity [m/s]
  // North east down (NED), also known as local tangent plane (LTP),
  // is a geographical coordinate system for representing state vectors that is commonly used in aviation.
  // It consists of three numbers: one represents the position along the northern axis,
  // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to
  // up in order to comply with the right-hand rule.
  // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity.
  // x = North
  // y = East
  // z = Down
  stateSetSpeedNed_f(&ins_vn.vel_ned); // set state

  // NED (LTP) acceleration [m/s^2]
  static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame
  float_rmat_transp_vmult(&accel_meas_ltp, orientationGetRMat_f(&ins_vn.body_to_imu), &(ins_vn.lin_accel));
  static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct
  VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z);
  stateSetAccelNed_f(&ltp_accel); // then set the states
  ins_vn.ltp_accel_f = ltp_accel;

  // LLA position [rad, rad, m]
  //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float
  ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.pos_lla[0]); // ins_impl.pos_lla[0] = lat
  ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.pos_lla[1]); // ins_impl.pos_lla[1] = lon
  ins_vn.lla_pos.alt = ((float)ins_vn.pos_lla[2]); // ins_impl.pos_lla[2] = alt
  LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos);
  stateSetPositionLla_i(&gps.lla_pos);

  // ECEF position
  struct LtpDef_f def;
  ltp_def_from_lla_f(&def, &ins_vn.lla_pos);
  struct EcefCoor_f ecef_vel;
  ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vel_ned);
  ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel);

  // ECEF velocity
  gps.ecef_pos.x = stateGetPositionEcef_i()->x;
  gps.ecef_pos.y = stateGetPositionEcef_i()->y;
  gps.ecef_pos.z = stateGetPositionEcef_i()->z;


#if GPS_USE_LATLONG
  // GPS UTM
  /* Computes from (lat, long) in the referenced UTM zone */
  struct UtmCoor_f utm_f;
  utm_f.zone = nav_utm_zone0;
  /* convert to utm */
  //utm_of_lla_f(&utm_f, &lla_f);
  utm_of_lla_f(&utm_f, &ins_vn.lla_pos);
  /* copy results of utm conversion */
  gps.utm_pos.east = (int32_t)(utm_f.east * 100);
  gps.utm_pos.north = (int32_t)(utm_f.north * 100);
  gps.utm_pos.alt = (int32_t)(utm_f.alt * 1000);
  gps.utm_pos.zone = (uint8_t)nav_utm_zone0;
#endif

  // GPS Ground speed
  float speed = sqrt(ins_vn.vel_ned.x * ins_vn.vel_ned.x + ins_vn.vel_ned.y * ins_vn.vel_ned.y);
  gps.gspeed = ((uint16_t)(speed * 100));

  // GPS course
  gps.course = (int32_t)(1e7 * (atan2(ins_vn.vel_ned.y, ins_vn.vel_ned.x)));

  // Because we have not HMSL data from Vectornav, we are using LLA-Altitude
  // as a workaround
  gps.hmsl = (uint32_t)(gps.lla_pos.alt);

  // set position uncertainty
  ins_vectornav_set_pacc();

  // set velocity uncertainty
  ins_vectornav_set_sacc();

  // check GPS status
  gps.last_msg_time = sys_time.nb_sec;
  gps.last_msg_ticks = sys_time.nb_sec_rem;
  if (gps.fix == GPS_FIX_3D) {
    gps.last_3dfix_time = sys_time.nb_sec;
    gps.last_3dfix_ticks = sys_time.nb_sec_rem;
  }

  // read INS status
  ins_vectornav_check_status();

  // update internal states for telemetry purposes
  // TODO: directly convert vectornav output instead of using state interface
  // to support multiple INS running at the same time
  ins_vn.ltp_pos_i = *stateGetPositionNed_i();
  ins_vn.ltp_speed_i = *stateGetSpeedNed_i();
  ins_vn.ltp_accel_i = *stateGetAccelNed_i();

  // send ABI messages
  uint32_t now_ts = get_sys_time_usec();
  AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps);
  AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.gyro_i);
  AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.accel_i);
}
Esempio n. 18
0
void imu_aspirin2_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  mpu60x0_spi_event(&imu_aspirin2.mpu);
  if (imu_aspirin2.mpu.data_available) {
#if !ASPIRIN_2_DISABLE_MAG
    /* 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);
#endif

    /* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */
#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);
#if !ASPIRIN_2_DISABLE_MAG
    VECT3_COPY(imu.mag_unscaled, mag);
#endif
#endif
#else

    /* Handle axis assignement for Lisa/M or Lisa/MX V2.1 integrated Aspirin like
     * IMU.
     */
#ifdef LISA_M_OR_MX_21
    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);
#if !ASPIRIN_2_DISABLE_MAG
    VECT3_ASSIGN(imu.mag_unscaled, -mag.y, mag.x, mag.z);
#endif
#else

    /* Handle real Aspirin IMU axis assignement. */
#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);
#if !ASPIRIN_2_DISABLE_MAG
    VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#endif
#else
    RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
#if !ASPIRIN_2_DISABLE_MAG
    VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
#endif
#endif
#endif

    imu_aspirin2.mpu.data_available = false;

    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.accel);
#if !ASPIRIN_2_DISABLE_MAG
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.mag);
#endif
  }
}
Esempio n. 19
0
void aspirin2_subsystem_event(void)
{
  int32_t x, y, z;
  uint32_t now_ts = get_sys_time_usec();

  // If the itg3200 I2C transaction has succeeded: convert the data
  if (aspirin2_mpu60x0.status == I2CTransSuccess) {
#define MPU_OFFSET_GYRO 9
    x = (int16_t)((aspirin2_mpu60x0.buf[0 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[1 + MPU_OFFSET_GYRO]);
    y = (int16_t)((aspirin2_mpu60x0.buf[2 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[3 + MPU_OFFSET_GYRO]);
    z = (int16_t)((aspirin2_mpu60x0.buf[4 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[5 + MPU_OFFSET_GYRO]);

    RATES_ASSIGN(imu.gyro_unscaled, x, y, z);

#define MPU_OFFSET_ACC 1
    x = (int16_t)((aspirin2_mpu60x0.buf[0 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[1 + MPU_OFFSET_ACC]);
    y = (int16_t)((aspirin2_mpu60x0.buf[2 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[3 + MPU_OFFSET_ACC]);
    z = (int16_t)((aspirin2_mpu60x0.buf[4 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[5 + MPU_OFFSET_ACC]);

    VECT3_ASSIGN(imu.accel_unscaled, x, y, z);

    // Is this is new data
    if (aspirin2_mpu60x0.buf[0] & 0x01) {
      imu_scale_gyro(&imu);
      imu_scale_accel(&imu);
      AbiSendMsgIMU_GYRO_INT32(IMU_PPZUAV_ID, now_ts, &imu.gyro);
      AbiSendMsgIMU_ACCEL_INT32(IMU_PPZUAV_ID, now_ts, &imu.accel);
    }

    aspirin2_mpu60x0.status =
      I2CTransDone;  // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
  }
  /*
    // If the adxl345 I2C transaction has succeeded: convert the data
    if (ppzuavimu_adxl345.status == I2CTransSuccess)
    {
      x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
      y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
      z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);

  #ifdef ASPIRIN_IMU
      VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
  #else // PPZIMU
      VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
  #endif

      acc_valid = TRUE;
      ppzuavimu_adxl345.status = I2CTransDone;
    }

    // If the hmc5843 I2C transaction has succeeded: convert the data
    if (ppzuavimu_hmc5843.status == I2CTransSuccess)
    {
      x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
      y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
      z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);

  #ifdef ASPIRIN_IMU
      VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
  #else // PPZIMU
      VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
  #endif

      mag_valid = TRUE;
      ppzuavimu_hmc5843.status = I2CTransDone;
    }
  */
}