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);
}
示例#2
0
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];
	}
	
}
示例#3
0
/* =================================================
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 ------------//
  }

}
示例#4
0
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;
}