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); } }
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); } }
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); } }
/** * 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); } }
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); } }
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 }
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); } }
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;});
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); } }
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); } }
/** * 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); } }
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); }
/** * 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); } }
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); } }
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); } }
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); } }
/** * 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(<p_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu)); stateSetNedToBodyRMat_f(<p_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(<p_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); }
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 } }
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; } */ }