/****************************************************************
* This interrupt handler is set up by AcquireInit to check
* for the events that begin a data logging. This is when the voltage
* exceeds a changeable threshold or
* the accelerometer z-axis changes g by more than 0.5.
*****************************************************************/
void MonitorShockISR() {
ROM_TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
//
// Toggle the flag for the second timer.
//
HWREGBITW(&g_ui32Flags, 1) ^= 1;
// If ADC has not converted yet, exit ISR.
if (!ADCIntStatus(ADC1_BASE, 3, false)) {
return;
}
ADCIntClear(ADC1_BASE, 3);
ADCSequenceDataGet(ADC1_BASE, 3, puiADC1Buffer);
ADCProcessorTrigger(ADC1_BASE, 3);
if (first_entry) {
first_entry = false;
prev1_value = ReadAccel(puiADC1Buffer[0]);
} else {
puiADC1Buffer[0] = ReadAccel(puiADC1Buffer[0]);
// Shock monitor detects a change of more than 2.4g
if (abs((int)puiADC1Buffer[0] - prev1_value) > 240) {
// UARTprintf("Shock! : %d \r",puiADC1Buffer[0]);
MonitorStop();
// Start LED
ROM_TimerEnable(TIMER2_BASE, TIMER_A);
// Start logging waveform.
ROM_IntMasterDisable();
psuiConfig->isShocked = true;
ROM_IntMasterEnable();
} else {
ROM_IntMasterDisable();
psuiConfig->isShocked = false;
ROM_IntMasterEnable();
}
prev1_value = puiADC1Buffer[0];
}
}
void IMU::KalmanFiltering()
{
//reading new datas
ReadGyr();
ReadAccel();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
KFData.roll = atan2(AData.y, AData.z) * RAD_TO_DEG;
KFData.pitch = atan(-AData.x / sqrt(AData.y * AData.y + AData.z * AData.z)) * RAD_TO_DEG;
#else // Eq. 28 and 29
KFData.roll = atan(AData.y / sqrt(AData.x * AData.x + AData.z * AData.z)) * RAD_TO_DEG;
KFData.pitch = atan2(-AData.x, AData.z) * RAD_TO_DEG;
#endif
double gyroXrate = GData.x / GYROSCOPE_SENSITIVITY; // 131- Convert to deg/s
double gyroYrate = GData.y / GYROSCOPE_SENSITIVITY; // 131- Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((KFData.roll < -90 && KFData.x > 90) || (KFData.roll > 90 && KFData.x < -90)) {
kalmanX.setAngle(KFData.roll);
KFData.x = KFData.roll;
} else
KFData.x = kalmanX.getAngle(KFData.roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(KFData.x) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
KFData.y = kalmanY.getAngle(KFData.pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((KFData.pitch < -90 && KFData.y > 90) || (KFData.pitch > 90 && KFData.y < -90)) {
kalmanY.setAngle(KFData.pitch);
KFData.y = KFData.pitch;
} else
KFData.y = kalmanY.getAngle(KFData.pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(KFData.y) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
KFData.x = kalmanX.getAngle(KFData.roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
//printf("G: %7.3f %7.3f %7.3f A: %7.3f %7.3f %7.3f KF: %7.3f %7.3f R-P: %7.3f %7.3f\n",GData.x,GData.y,GData.z,AData.x,AData.y,AData.z,KFData.x,KFData.y,KFData.roll,KFData.pitch);
//printf ("KFAngleX:%7.3f \t KFAngleY: %7.3f\n",KFData.x,KFData.y);
}
IMU::IMU(void)
{
char buf[1];
bcm2835_init();
bcm2835_i2c_begin();
bcm2835_i2c_setSlaveAddress(MPU6050_ADDRESS);
WriteRegister(SMPRT_DIV,0x07); // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
WriteRegister(CONFIG,0x00); // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
WriteRegister(GYRO_CONFIG,0x00); //250dpi
WriteRegister(ACCEL_CONFIG,0x00); //2g resolution
WriteRegister(PWR_MGMT_1,0x00); //sleep mode disabled
regaddr[0]=WHO_AM_I;
bcm2835_i2c_write(regaddr, 1);
bcm2835_i2c_read(buf, 1);
if(buf[0]==0x88)
{
printf("sensor config was successful WHO_AM_I: %x\n",buf[0]);
}
else
{
printf("sensor config was unsuccessful, %x\n",buf[0]);
}
bcm2835_i2c_end();
///////////SETUP VARIABLES
ReadGyr();
ReadAccel();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
KFData.roll = atan2(AData.y, AData.z) * RAD_TO_DEG;
KFData.pitch = atan(-AData.x / sqrt(AData.y * AData.y + AData.z * AData.z)) * RAD_TO_DEG;
#else // Eq. 28 and 29
KFData.roll = atan(AData.y / sqrt(AData.x * AData.x + AData.z * AData.z)) * RAD_TO_DEG;
KFData.pitch = atan2(-AData.x, AData.z) * RAD_TO_DEG;
#endif
kalmanX.setAngle(KFData.roll); // Set starting angle
kalmanY.setAngle(KFData.pitch);
}
