void getIMU(nav_struct* nav_data) { /// getMagnetometerData(magn); No magnetometer getAccelerometerData(acc); getGyroscopeData(gyro); convertTemp(gyro); if (bias_flag == true) { bias_flag = computeBias(acc,acc_bias,ACC_VAR,&acc_bias_count); bias_flag = computeBias(gyro,gyro_bias,GYRO_VAR,&gyro_bias_count); } else { removeBias(acc,acc_bias,ACC_VAR); removeBias(gyro,gyro_bias,GYRO_VAR); setGravity(acc); // Eliminate Hard-Iron offsets magn[0] = magn[0] - MX_OFFSET; magn[1] = magn[1] - MY_OFFSET; /// set IMU values nav_data->imu_lan.abbx = (double)acc[0]; nav_data->imu_lan.abby = (double)acc[1]; nav_data->imu_lan.abbz = -(double)acc[2]; nav_data->imu_lan.wbbx = -(double)gyro[0]; nav_data->imu_lan.wbby = -(double)gyro[1]; nav_data->imu_lan.wbbz = -(double)gyro[2]; } }
/* ================================================= FUNCTION: getIMU CREATED: 16-05-2014 DESCRIPTION: This main loop function for the IMU sensor suite PARAMETERS: vec GLOBAL VARIABLES: None. RETURNS: vec. AUTHOR: P. Kantue ================================================== */ void getIMU(int vec[], unsigned int time_el) { getMagnetometerData(magn); getAccelerometerData(acc); getGyroscopeData(gyro); convertTemp(gyro); if (bias_flag == true){ bias_flag = computeBias(acc,acc_bias,ACC_VAR,&acc_bias_count); bias_flag = computeBias(gyro,gyro_bias,GYRO_VAR,&gyro_bias_count); } else{ removeBias(acc,acc_bias,ACC_VAR); removeBias(gyro,gyro_bias,GYRO_VAR); setGravity(acc); // Eliminate Hard-Iron offsets magn[0] = magn[0] - MX_OFFSET; magn[1] = magn[1] - MY_OFFSET; // Compute Euler angles from acceleration raw values PitchandRoll(acc,temp_euler); // Apply LPF to both pitch and roll angles LPF_euler(Euler,temp_euler,EULER_LPF); // Compute tilt compensation for magnetic heading - NOT WORKING heading = tiltCompensation(magn,Euler); // Store IMU data to be passed to other subsystems storeIMU(acc,gyro,magn,Euler,heading,vec, time_el); //----ADD EXTRA CODE HERE ------------// } }
int main(void) { int16 acc[3]; int16 gyro[3]; int16 mag[3]; int16 temperature = 0; int32 pressure = 0; int32 centimeters = 0; i2c_master_enable(I2C1, I2C_FAST_MODE); delay(200); initAcc(); delay(200); initGyro(); delay(200); zeroCalibrateGyroscope(128,5); compassInit(false); compassCalibrate(1); compassSetMode(0); bmp085Calibration(); while(1) { getAccelerometerData(acc); //Read acceleration SerialUSB.print("Accel: "); SerialUSB.print(acc[0]); SerialUSB.print(" "); SerialUSB.print(acc[1]); SerialUSB.print(" "); SerialUSB.print(acc[2]); getGyroscopeData(gyro); //Read acceleration SerialUSB.print(" Gyro: "); SerialUSB.print(gyro[0]); SerialUSB.print(" "); SerialUSB.print(gyro[1]); SerialUSB.print(" "); SerialUSB.print(gyro[2]); getMagnetometerData(mag); //Read acceleration SerialUSB.print(" Mag: "); SerialUSB.print(mag[0]); SerialUSB.print(" "); SerialUSB.print(mag[1]); SerialUSB.print(" "); SerialUSB.print(mag[2]); temperature = bmp085GetTemperature(bmp085ReadUT()); pressure = bmp085GetPressure(bmp085ReadUP()); centimeters = bmp085GetAltitude(); SerialUSB.print(" Temp: "); SerialUSB.print(temperature, DEC); SerialUSB.print(" *0.1 deg C "); SerialUSB.print("Pressure: "); SerialUSB.print(pressure, DEC); SerialUSB.print(" Pa "); SerialUSB.print("Altitude: "); SerialUSB.print(centimeters, DEC); SerialUSB.print(" cm "); SerialUSB.println(" "); delay(100); } return 0; }