void Timer0_isr(void)
{
// Clear TIMER1 INTERRUPT
TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
/*
// Send a indicator that ISR has been CALLED through UART0[Serial]
Serial.print("Timer0_isr() called");
Serial.println("");
*/
// Output the GYRO data and ACCEL data (with spaces in between each sample)
// set of 3*2=6 sample values at output on the same line;
// this is done through the Serial (UART) peripheral [Default]
/*
Serial.print(MPU9150_readSensor(MPU9150_CMPS_XOUT_L,MPU9150_CMPS_XOUT_H));
Serial.print(" ");
Serial.print(MPU9150_readSensor(MPU9150_CMPS_YOUT_L,MPU9150_CMPS_YOUT_H));
Serial.print(" ");
Serial.print(MPU9150_readSensor(MPU9150_CMPS_ZOUT_L,MPU9150_CMPS_ZOUT_H));
Serial.print(" ");
Serial.print(MPU9150_readSensor(MPU9150_GYRO_XOUT_L,MPU9150_GYRO_XOUT_H));
Serial.print(" ");
Serial.print(MPU9150_readSensor(MPU9150_GYRO_YOUT_L,MPU9150_GYRO_YOUT_H));
Serial.print(" ");
Serial.print(MPU9150_readSensor(MPU9150_GYRO_ZOUT_L,MPU9150_GYRO_ZOUT_H));
Serial.print(" ");
Serial.print( MPU9150_readSensor( MPU9150_ACCEL_XOUT_L,MPU9150_ACCEL_XOUT_H) );
Serial.print(" ");
Serial.print( MPU9150_readSensor( MPU9150_ACCEL_YOUT_L,MPU9150_ACCEL_YOUT_H) );
Serial.print(" ");
Serial.print( MPU9150_readSensor( MPU9150_ACCEL_ZOUT_L,MPU9150_ACCEL_ZOUT_H) ); */
Serial.print(MPU9150_readSensor(MPU9150_GYRO_XOUT_L,MPU9150_GYRO_XOUT_H));
Serial.print(" ");
Serial.print(MPU9150_readSensor(MPU9150_GYRO_YOUT_L,MPU9150_GYRO_YOUT_H));
Serial.print(" ");
Serial.print(MPU9150_readSensor(MPU9150_GYRO_ZOUT_L,MPU9150_GYRO_ZOUT_H));
Serial.print(" ");
Serial.println("");
}
void get_orientation()
{
g_allow_updating = false;
unsigned int now = 0;
now = millis();
float time_diff = (now-g_last_updated) / 1000.0f;
g_CurrentX_Rotation += ((MPU9150_readSensor(MPU9150_GYRO_XOUT_L, MPU9150_GYRO_XOUT_H) / 131.0f) - get_offsetX())*time_diff;
g_CurrentY_Rotation += ((MPU9150_readSensor(MPU9150_GYRO_YOUT_L, MPU9150_GYRO_YOUT_H) / 131.0f) - get_offsetY())*time_diff;
g_CurrentZ_Rotation += ((MPU9150_readSensor(MPU9150_GYRO_ZOUT_L, MPU9150_GYRO_ZOUT_H) / 131.0f) - get_offsetZ())*time_diff;
g_last_updated = now;
g_allow_updating = true;
}
float calculate_offsetZ()
{
float offsetZ = 0.0f;
for(int i = 0; i < 30; i++)
{
offsetZ += (MPU9150_readSensor(MPU9150_GYRO_ZOUT_L, MPU9150_GYRO_ZOUT_H) / 131.0f);
_delay_ms(20);
}
g_OffsetZ = (offsetZ / 30.0f);
return g_OffsetZ;
}
// void setup(void) runs ONCE when the program just STARTS
void setup()
{
// First SET the SYSTEM CLOCK to 80 [MHz]
// Sets the Clock DIVIDER to 2.5, so that SYS_CLOCK runs at 200/2.5 ==> 80 [MHz]
SysCtlClockSet( SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
uint32_t Timer0_Period;
// Enable & Configure Serial(UART0) to BAUDRATE = 9216000
Serial.begin(BAUDRATE);
// Configure PINS for LED OUTPUT
pinMode(RED,OUTPUT);
pinMode(BLUE,OUTPUT);
pinMode(GREEN,OUTPUT);
// Set LED OUTPUT pins to OFF initially
digitalWrite(RED,LOW);
digitalWrite(BLUE,LOW);
digitalWrite(GREEN,LOW);
state_led=0; // initialize the STATE to STATE0
// Initialize the 'Wire' class for the I2C-bus.
Wire.begin();
// Clear the 'sleep' bit to start the sensor.
MPU9150_writeSensor(MPU9150_PWR_MGMT_1, 0);
int temp0;
temp0= MPU9150_readSensor(0x1c);
temp0 |= (0x10);
temp0 &=~(0x08);
MPU9150_writeSensor(0x1c,temp0);
MPU9150_writeSensor(0x19,0x0f);
MPU9150_setupCompass();
SysCtlPeripheralEnable( SYSCTL_PERIPH_TIMER0); // Enable Timer0
TimerConfigure(TIMER0_BASE, TIMER_CFG_PERIODIC); // Set Timer0 mode PERIODIC
// Timer0_Period ==> 80e6 * A / B, (1/2)=0.5
Timer0_Period = ( 80000000 * A ) / B;
TimerLoadSet( TIMER0_BASE, TIMER_A, Timer0_Period-1);
// REGISTER ISR to TIMER0 INTERRUPT
TimerIntRegister( TIMER0_BASE, TIMER_A, Timer0_isr );
// Enable Interrupts from Timer0_A
IntEnable( INT_TIMER0A);
// Set Timer Interrupt Condition and Enable Timer Interrupt
TimerIntEnable( TIMER0_BASE, TIMER_TIMA_TIMEOUT);
// Enable INTERRUPTS for the SYSTEM
IntMasterEnable();
// Start Timer0
TimerEnable(TIMER0_BASE, TIMER_A);
}
