void initOrientation()
{
int initLoops = 150;
float accAngle[NUMAXIS] = { 0.0f, 0.0f, 0.0f };
int i;
for (i = 0; i < initLoops; i++)
{
readMPU6050();
computeMPU6050TCBias();
sensors.accel500Hz[XAXIS] = ((float)rawAccel[XAXIS].value - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[YAXIS] = ((float)rawAccel[YAXIS].value - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[ZAXIS] = -((float)rawAccel[ZAXIS].value - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
accAngle[ROLL] += atan2f(-sensors.accel500Hz[YAXIS], -sensors.accel500Hz[ZAXIS]);
accAngle[PITCH] += atan2f(sensors.accel500Hz[XAXIS], -sensors.accel500Hz[ZAXIS]);
accAngleSmooth[ROLL ] = accAngle[ROLL ] / (float)initLoops;
accAngleSmooth[PITCH] = accAngle[PITCH] / (float)initLoops;
delay(2);
}
sensors.evvgcCFAttitude500Hz[PITCH] = accAngleSmooth[PITCH];
sensors.evvgcCFAttitude500Hz[ROLL ] = accAngleSmooth[ROLL ];
sensors.evvgcCFAttitude500Hz[YAW ] = 0.0f;
}
void computeMPU6050RTData(void)
{
uint8_t axis;
uint16_t samples;
double accelSum[3] = { 0.0f, 0.0f, 0.0f };
double gyroSum[3] = { 0.0f, 0.0f, 0.0f };
mpu6050Calibrating = true;
for (samples = 0; samples < 5000; samples++)
{
readMPU6050();
computeMPU6050TCBias();
accelSum[XAXIS] += (float)rawAccel[XAXIS].value - accelTCBias[XAXIS];
accelSum[YAXIS] += (float)rawAccel[YAXIS].value - accelTCBias[YAXIS];
accelSum[ZAXIS] += (float)rawAccel[ZAXIS].value - accelTCBias[ZAXIS];
gyroSum[ROLL ] += (float)rawGyro[ROLL ].value - gyroTCBias[ROLL ];
gyroSum[PITCH] += (float)rawGyro[PITCH].value - gyroTCBias[PITCH];
gyroSum[YAW ] += (float)rawGyro[YAW ].value - gyroTCBias[YAW ];
delayMicroseconds(1000);
}
for (axis = 0; axis < 3; axis++)
{
accelSum[axis] = accelSum[axis] / 5000.0f * ACCEL_SCALE_FACTOR;
gyroRTBias[axis] = gyroSum[axis] / 5000.0f;
}
accelOneG = sqrt(SQR(accelSum[XAXIS]) + SQR(accelSum[YAXIS]) + SQR(accelSum[ZAXIS]));
mpu6050Calibrating = false;
}
void SysTick_Handler(void)
{
uint32_t currentTime;
#ifdef _DTIMING
LA1_ENABLE;
#endif
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
if ((systemReady == true) &&
(mpu6050Calibrating == false) &&
(magCalibrating == false))
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
///////////////////////////////
if ((frameCounter % COUNT_500HZ) == 0)
{
frame_500Hz = true;
readMPU6050();
accelData500Hz[XAXIS] = rawAccel[XAXIS].value;
accelData500Hz[YAXIS] = rawAccel[YAXIS].value;
accelData500Hz[ZAXIS] = rawAccel[ZAXIS].value;
gyroData500Hz[ROLL ] = rawGyro[ROLL ].value;
gyroData500Hz[PITCH] = rawGyro[PITCH].value;
gyroData500Hz[YAW ] = rawGyro[YAW ].value;
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0)
{
frame_100Hz = true;
}
///////////////////////////////
if ((frameCounter % COUNT_50HZ) == 0)
{
frame_50Hz = true;
}
///////////////////////////////
// if (((frameCounter + 1) % COUNT_10HZ) == 0)
// newMagData = readMag();
newMagData = false;
if ((frameCounter % COUNT_10HZ) == 0)
frame_10Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_5HZ) == 0)
frame_5Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_1HZ) == 0)
frame_1Hz = true;
///////////////////////////////////
executionTime1000Hz = micros() - currentTime;
///////////////////////////////
}
#ifdef _DTIMING
LA1_DISABLE;
#endif
}
//--------------- readIMU() -------------------
// Retorna um erro caso não consiga
unsigned char readIMU(IMUdata *data) {
unsigned char error = 0;
error += readMPU6050(&data->MPU);
error += readHMC5883(&data->HMC);
return error;
}
