// Calibrate the Accelerometer - "Average" method
void Accel_Calib_Ave() {
Serial.println("Performing a Accelometer calibration using the Averaging method");
Accel_X_Center_Ave = 0;
Accel_X_Center_Ave_Total =0;
Accel_Y_Center_Ave = 0;
Accel_Y_Center_Ave_Total =0;
Accel_Z_Center_Ave = 0;
Accel_Z_Center_Ave_Total =0;
for (Accel_Reading_Count=1; Accel_Reading_Count<=100; Accel_Reading_Count++) {
Serial.print(".");
Read_Accel();
// Summ up the reads for the average
Accel_X_Center_Ave_Total += Accel_X;
Accel_Y_Center_Ave_Total += Accel_Y;
Accel_Z_Center_Ave_Total += Accel_Z;
}
// Calc the average over the sample
Accel_X_Center_Ave = (float)Accel_X_Center_Ave_Total / Accel_Reading_Count;
Accel_Y_Center_Ave = (float)Accel_Y_Center_Ave_Total / Accel_Reading_Count;
Accel_Z_Center_Ave = (float)Accel_Z_Center_Ave_Total / Accel_Reading_Count;
Serial.println("Done");
}
volatile void initialize_IMU(void)
{
twim_init();
Enable_Acc(TWIM);
Enable_Gyro(TWIM);
Enable_Magn(TWIM);
delay_ms(20);
for (int i = 0; i < 32; i++)
{
Read_Gyro();
Read_Accel();
RAW_OFFSET[0] += RAW[0];
RAW_OFFSET[1] += RAW[1];
RAW_OFFSET[2] += RAW[2];
RAW_OFFSET[3] += RAW[3];
RAW_OFFSET[4] += RAW[4];
RAW_OFFSET[5] += RAW[5];
delay_ms(20);
}
RAW_OFFSET[0] = 0;
RAW_OFFSET[1] = 0;
RAW_OFFSET[2] = 0;
RAW_OFFSET[3] = 0;
RAW_OFFSET[4] = 0;
RAW_OFFSET[5] = 0;
timer_current = rtc_get_value(&AVR32_RTC);
delay_ms(20);
get_Angles();
}
void sample_MiniMU9() //Main Loop
{
if((millis()-timer)>=100) //20 // Main loop runs at 50Hz
{
counter++;
timer_old = timer;
timer=millis();
if (timer>timer_old)
G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0;
// *** DCM algorithm
// Data adquisition
Read_Gyro(); // This read gyro data
Read_Accel(); // Read I2C accelerometer
if (counter > 5) // Read compass data at 10Hz... (5 loop runs)
{
counter=0;
Read_Compass(); // Read I2C magnetometer
Compass_Heading(); // Calculate magnetic heading
}
// Calculations...
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// ***
printdata();
}
}
void init_MiniMU9()
{
I2C_Init();
delay(1500);
Accel_Init();
Compass_Init();
Gyro_Init();
delay(20);
for(int i=0;i<32;i++) // We take some readings...
{
Read_Gyro();
Read_Accel();
for(int y=0; y<6; y++) // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
}
for(int y=0; y<6; y++)
AN_OFFSET[y] = AN_OFFSET[y]/32;
AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
delay(2000);
timer=millis();
delay(20);
counter=0;
}
volatile void get_Angles()
{
static float prev_g_d_roll = 0;
timer_old = timer_current;
timer_current = rtc_get_value(&AVR32_RTC);
if (timer_current > timer_old)
{
g_dt = (float)((timer_current-timer_old))*0.001*0.125*0.98; // Time elapsed in seconds -- sensors are in seconds
}
else
{
g_dt = 0;
}
// GET DATA
Read_Gyro();
Read_Accel();
// GYROSCOPE
// Y = roll
float g_d_roll = gyro_x * GYRO_GAIN_RPS; // delta roll in radians/sec
//g_roll = roll + g_d_roll * g_dt;
g_roll = roll + ((g_d_roll+prev_g_d_roll)/2)*g_dt;
prev_g_d_roll = g_d_roll;
// X = pitch
float g_d_pitch = gyro_y * GYRO_GAIN_RPS; // delta pitch in radians/sec
g_pitch = pitch + g_d_pitch * g_dt;
// Z = yaw
float g_d_yaw = gyro_z * GYRO_GAIN_RPS; // delta yaw in radians/sec
g_yaw = yaw + g_d_yaw * g_dt;
// ACCELEROMETER
float acc_x = accel_x * 0.002;
float acc_y = accel_y * 0.002;
float acc_z = accel_z * 0.002;
a_roll = atan2(-acc_y, acc_z);
a_pitch = atan(acc_x/sqrt(acc_y*acc_y + acc_z*acc_z));
// Weigh
roll = TRUST_GAIN * a_roll + (1-TRUST_GAIN) * g_roll;
pitch = TRUST_GAIN * a_pitch + (1-TRUST_GAIN) * g_pitch;
}
// Calibrate the Accelerometer - "Low Pass Filter" method
void Accel_Calib_LPF() {
Serial.println("Performing a Accelometer calibration using the Low Pass Filter method");
Accel_X_Center_LPF = 0;
Accel_Y_Center_LPF = 0;
Accel_Z_Center_LPF = 0;
for (Accel_Reading_Count=1; Accel_Reading_Count<=100; Accel_Reading_Count++) {
Serial.print(".");
Read_Accel();
// Take 90% of the previous cumulative value and 10% on the current reading
// (Bit like averaging
Accel_X_Center_LPF = Accel_X_Center_LPF * 0.9f + Accel_X * 0.1f;
Accel_Y_Center_LPF = Accel_Y_Center_LPF * 0.9f + Accel_Y * 0.1f;
Accel_Z_Center_LPF = Accel_Z_Center_LPF * 0.9f + Accel_Z * 0.1f;
}
Serial.println("Done");
}
/*----------------------------------------------------------------------------
MAIN function
*----------------------------------------------------------------------------*/
int main (void) {
#if USE_VLPR == 1
// enter low power run
SIM->CLKDIV1 = (0x1 << SIM_CLKDIV1_OUTDIV1_SHIFT) | (0x5 << SIM_CLKDIV1_OUTDIV4_SHIFT); // reduce core clock < 4 MHz and flash < 1 MHz
MCG->C6 &= ~MCG_C6_CME0_MASK; // disable MCG clock monitor
MCG->C2 |= MCG_C2_IRCS_MASK; // don't use slow internal reference clock
MCG->C1 |= MCG_C1_CLKS(2); // enter BLPE mode
MCG->C1 &= ~MCG_C1_IREFS_MASK;
MCG->C6 &= ~MCG_C6_PLLS_MASK;
while(!(MCG->S & MCG_S_IREFST_MASK >> MCG_S_IREFST_SHIFT)); // wait to ensure clock change
MCG->C2 |= MCG_C2_LP_MASK;
#endif
Init_RGB_LEDs();
#if DEBUG_SIGNALS == 1
Init_Debug_Signals();
#endif
// I2C and MMA
i2c_init(); /* init i2c */
if (!init_mma()) { /* init mma peripheral */
Control_RGB_LEDs(1, 0, 0); /* Light red error LED */
while (1) /* not able to initialize mma */
;
}
#if RUN_I2C_FAST == 1
// increase i2c baud rate
I2C_DISABLE;
I2C0->F = (I2C_F_ICR(0x00) | I2C_F_MULT(0));
I2C_ENABLE;
#endif
// configure low power modes
SMC->PMPROT = SMC_PMPROT_ALLS_MASK | SMC_PMPROT_AVLP_MASK; // allow low leakage stop mode
SMC->PMCTRL = SMC_PMCTRL_RUNM(2) | SMC_PMCTRL_STOPM(3); // enable low power run mode (10) and low leakage stop mode (011)
SMC->STOPCTRL = SMC_STOPCTRL_PSTOPO(0) | SMC_STOPCTRL_VLLSM(3); // normal stop mode and VLL stop3 (not needed?)
// configure low leakage wakeup unit (LLWU)
LLWU->ME |= LLWU_ME_WUME0_MASK; // internal module 0 is wakeup source which is apparently the LPTMR
// enable stop mode (deep sleep)
SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
// LPTMR
Init_LPTMR();
Start_LPTMR();
__enable_irq();
while (1) {
// read acceleration every 100 ms
if (run_Read_Accel){
run_Read_Accel = 0;
Read_Accel();
}
// update LEDs every 500 ms; keep them on for 10 ms
if (run_Update_LEDs){
run_Update_LEDs = 0;
Update_LEDs();
#if USE_PWM == 1
#if PWM_SLEEP == 1
SCB->SCR &= ~SCB_SCR_SLEEPDEEP_Msk; // switch to regular sleep mode
#if USE_SLEEP_MODES == 1
#if DEBUG_SIGNALS == 1
PTE->PSOR |= MASK(30);
#endif
__wfi(); // PWM does not work in LLS mode
#endif
SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk; // switch back to LLS mode
#else
while(led_on_period); // poll -> bad solution
#endif
#endif
}
#if USE_SLEEP_MODES == 1
#if DEBUG_SIGNALS == 1
PTE->PSOR |= MASK(30);
#endif
__wfi(); // go to sleep
#endif
}
}
void read_sensors()
{
Read_Gyro(); // Read gyroscope
Read_Accel(); // Read accelerometer
Read_Magn(); // Read magnetometer
}
