Example #1
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);
  }

}
Example #2
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);
    }
}
Example #3
0
void imu_krooz_event(void)
{
  if (imu_krooz.mpu_eoc) {
    mpu60x0_i2c_read(&imu_krooz.mpu);
    imu_krooz.mpu_eoc = false;
  }

  // If the MPU6050 I2C transaction has succeeded: convert the data
  mpu60x0_i2c_event(&imu_krooz.mpu);
  if (imu_krooz.mpu.data_available) {
    RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
    VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
    imu_krooz.meas_nb++;
    imu_krooz.mpu.data_available = false;
  }

  if (imu_krooz.hmc_eoc) {
    hmc58xx_read(&imu_krooz.hmc);
    imu_krooz.hmc_eoc = false;
  }

  // If the HMC5883 I2C transaction has succeeded: convert the data
  hmc58xx_event(&imu_krooz.hmc);
  if (imu_krooz.hmc.data_available) {
    VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
    UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
    imu_krooz.hmc.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, get_sys_time_usec(), &imu.mag);
  }
}
Example #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);
  }
}
Example #5
0
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
  }
}
Example #6
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);
  }
}
Example #7
0
/* Parse the InterMCU message */
static inline void mag_pitot_parse_msg(void)
{
  uint32_t now_ts = get_sys_time_usec();

  /* Parse the mag-pitot message */
  uint8_t msg_id = pprzlink_get_msg_id(mp_msg_buf);

#if PPRZLINK_DEFAULT_VER == 2
  // Check that the message is really a intermcu message
  if (pprzlink_get_msg_class_id(mp_msg_buf) == DL_intermcu_CLASS_ID) {
#endif
  switch (msg_id) {
  /* Got a magneto message */
  case DL_IMCU_REMOTE_MAG: {
    struct Int32Vect3 raw_mag;

    // Read the raw magneto information
    raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf);
    raw_mag.y = DL_IMCU_REMOTE_MAG_mag_y(mp_msg_buf);
    raw_mag.z = DL_IMCU_REMOTE_MAG_mag_z(mp_msg_buf);

    // Rotate the magneto
    struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag);
    int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag);

    // Send and set the magneto IMU data
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag);
    break;
  }

  /* Got a barometer message */
  case DL_IMCU_REMOTE_BARO: {
    float pitot_stat = DL_IMCU_REMOTE_BARO_pitot_stat(mp_msg_buf);
    float pitot_temp = DL_IMCU_REMOTE_BARO_pitot_temp(mp_msg_buf);

    AbiSendMsgBARO_ABS(IMU_MAG_PITOT_ID, pitot_stat);
    AbiSendMsgTEMPERATURE(IMU_MAG_PITOT_ID, pitot_temp);
    break;
  }

  /* Got an airspeed message */
  case DL_IMCU_REMOTE_AIRSPEED: {
    // Should be updated to differential pressure
    float pitot_ias = DL_IMCU_REMOTE_AIRSPEED_pitot_IAS(mp_msg_buf);
    AbiSendMsgAIRSPEED(IMU_MAG_PITOT_ID, pitot_ias);
    break;
  }

    default:
      break;
  }
#if PPRZLINK_DEFAULT_VER == 2
  }
#endif
}
Example #8
0
uint8_t
imu_event_task(void)
{
    uint8_t valid = 0;

    if ( max1168_event() )
    {
        valid = IMU_ACC | IMU_GYR;

        booz_imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN];
        booz_imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN];
        booz_imu.gyro_unscaled.r = max1168_values[IMU_GYRO_R_CHAN];
        booz_imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN];
        booz_imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
        booz_imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];

        imu_scale_gyro();
        imu_scale_accel();

        max1168_reset();
    }

    if (do_max1168_read && imu_spi_selected == SPI_NONE) 
    {
        Booz2ImuSetSpi16bits();
        imu_spi_selected = SPI_SLAVE_MAX1168;
        do_max1168_read = FALSE;

        max1168_read();
    }
    if (do_micromag_read && imu_spi_selected == SPI_NONE) 
    {
        Booz2ImuSetSpi8bits();
        imu_spi_selected = SPI_SLAVE_MM;
        do_micromag_read = FALSE;
    }
    if (imu_spi_selected == SPI_SLAVE_MM)
        micromag_read();


    if ( micromag_event() )
    {
        valid |= IMU_MAG;

        booz_imu.mag_unscaled.x = micromag_values[IMU_MAG_X_CHAN];
        booz_imu.mag_unscaled.y = micromag_values[IMU_MAG_Y_CHAN];
        booz_imu.mag_unscaled.z = micromag_values[IMU_MAG_Z_CHAN];

        imu_scale_mag();

        micromag_reset();
    }

    return valid;
}
Example #9
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
}
Example #10
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);
  }

}
Example #11
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);
  }
}
Example #12
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);
  }
}
Example #13
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);
  }
}
Example #14
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);
  }
}
Example #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);
  }
}
Example #16
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
  }
}