void accelerometer(){
if (startuptempswitch == true){
while (digitalRead(buttonApin) == LOW){}
}
test_all_meters();
startuptempswitch = true;
int accelx = ( getAccelerometerData (xval) );
int accely = ( getAccelerometerData (yval) );
pospeakcount++;
negpeakcount++;
peak(accely);
Serial.print(0xFE, BYTE); //command flag
Serial.print(128, BYTE);
delay(60);
printBarGraph(accely);
Serial.print(0xFE, BYTE);
Serial.print(192, BYTE);
delay(60);
Serial.print("x:");
printAccelerometerReadout(accelx);
Serial.print(" y: ");
printAccelerometerReadout(accely);
}
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;
}
void usb_logger(){
while (digitalRead(buttonApin) == LOW){}
Serial.print(0xFE, BYTE);
Serial.print(0x01, BYTE);
Serial.print(0xFE, BYTE);
Serial.print(128, BYTE);
Serial.print("USB Logging");
Serial.print(0xFE, BYTE);
Serial.print(192, BYTE);
Serial.print("Press B");
while (digitalRead(buttonBpin) == HIGH){
if (digitalRead(buttonApin)==LOW){
return;
}
}
delay(500);
Serial.print(0xFE, BYTE);
Serial.print(0x01, BYTE);
Serial.print("Logging"); //command flag
//Serial.print(0x08, BYTE); //turn off visual display
//Serial.print("Data logger: \n");
//open a new file based on a millis timestamp, give it a header and start writting
//Serial.print("OPW log");
// Serial.print(logfilecount);
//Serial.print(".txt");
//Serial.print(13, BYTE);
//Serial.print("WRF ");
//Serial.print(42);
Serial.print("T1, T2, Oil Temp, Boost, x accel, y accel\n");
Serial.print(13, BYTE);
while (digitalRead(buttonBpin) == LOW){}
while ( (digitalRead(buttonApin) == HIGH) && (digitalRead(buttonBpin) == HIGH) ){
//logging output
Serial.print("WRF ");
long t1 = ( (lookup_temp(analogRead(t1pin))) );
long t2 = ( (lookup_temp(analogRead(t2pin))) );
long oil = ( lookup_oil_temp(analogRead(tempPin)));
long press = lookup_boost(analogRead(boostPin));
long ax = getAccelerometerData (xval);
long ay = getAccelerometerData (yval);
//calc the number of characters
int linelen = 5 + numberofdigits(t1) + numberofdigits(t2) + numberofdigits(oil) + numberofdigits(press) + numberofdigits(ax) + numberofdigits(ay);
//print the number of characters for this line
Serial.print(linelen);
Serial.print(13, BYTE);
Serial.print(t1);
Serial.print(",");
//get T2 and convert
//Serial.print( lookup_temp(analogRead(t2pin)) );
Serial.print(t2);
Serial.print(",");
//get oil temp and convert
//Serial.print( analogRead(tempPin) );
Serial.print(oil);
Serial.print(",");
//get boost and convert
Serial.print(press);
//Serial.print("###");
Serial.print(",");
//get x accel
Serial.print(ax);
Serial.print(",");
//get y accel
Serial.print(ay);
//Serial.print("\n");
Serial.print(13, BYTE);
}
while ( (digitalRead(buttonApin) == LOW) && (digitalRead(buttonBpin) == LOW) ){}
//close the file
//Serial.print("CLF log%");
//Serial.print(logfilecount);
//Serial.print(".txt");
//Serial.print(13, BYTE);
//logfilecount++;
//Serial.print(0xFE, BYTE); //command flag
//Serial.print(0x0C, BYTE); //turn on visual display
return;
}