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