void App::initial_load()
{
build_femspace();
//autoSetGrainInfo();
readGrainInfo();
Operator::L2Project(&phi_0, *phi_h, Operator::LOCAL_LEAST_SQUARE, 5);
Operator::L2Project(&c_0, *c_h, Operator::LOCAL_LEAST_SQUARE, 5);
for (int i=0;i<3;++i)
{
adapt_mesh();
initOrientation();
// Operator::L2Interpolate(&phi_0, *phi_h);
// Operator::L2Interpolate(&c_0, *c_h);
}
Operator::L2Project(&u_0, *u_h, Operator::LOCAL_LEAST_SQUARE, 5);
Operator::L2Project(&v_0, *v_h, Operator::LOCAL_LEAST_SQUARE, 5);
Operator::L2Project(&p_0, *p_h, Operator::LOCAL_LEAST_SQUARE, 5);
Operator::L2Project(&p_0, *last_p_h, Operator::LOCAL_LEAST_SQUARE, 5);
save_data();
load_balance();
}
void getOrientation(float *smoothAcc, float *orient, float *accData, float *gyroData, float dt)
{
float accAngle[3];
float gyroRate[3];
if (evvgcCFinitialized == false)
{
initOrientation();
evvgcCFinitialized = true;
}
//-------------------------------------------
if (evvgcCFinitialized == true)
{
accAngle[ROLL ] = atan2f(-accData[YAXIS], -accData[ZAXIS]);
accAngle[PITCH] = atan2f(accData[XAXIS], -accData[ZAXIS]);
smoothAcc[ROLL] = ((smoothAcc[ROLL ] * 99.0f) + accAngle[ROLL ]) / 100.0f;
smoothAcc[PITCH] = ((smoothAcc[PITCH] * 99.0f) + accAngle[PITCH]) / 100.0f;
gyroRate[PITCH] = gyroData[PITCH];
orient[PITCH] = (orient[PITCH] + gyroRate[PITCH] * dt) + 0.0002f * (smoothAcc[PITCH] - orient[PITCH]);
gyroRate[ROLL] = gyroData[ROLL] * cosf(fabsf(orient[PITCH])) + gyroData[YAW] * sinf(orient[PITCH]);
orient[ROLL] = (orient[ROLL] + gyroRate[ROLL] * dt) + 0.0002f * (smoothAcc[ROLL] - orient[ROLL]);
gyroRate[YAW] = gyroData[YAW] * cosf(fabsf(orient[PITCH])) - gyroData[ROLL] * sinf(orient[PITCH]);
orient[YAW] = (orient[YAW] + gyroRate[YAW] * dt);
}
}
int main(void)
{
uint32_t currentTime;
systemInit();
initOrientation();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processPointingCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
// HJI if (newMagData == true)
// HJI {
// HJI sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS];
// HJI sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS];
// HJI sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]);
// HJI newMagData = false;
// HJI magDataUpdate = true;
// HJI }
cliCom();
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
#ifdef _DTIMING
LA2_ENABLE;
#endif
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
TIM_Cmd(TIM6, DISABLE);
timerValue = TIM_GetCounter(TIM6);
TIM_SetCounter(TIM6, 0);
TIM_Cmd(TIM6, ENABLE);
dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop
sensors.accel500Hz[XAXIS] = ((float)accelData500Hz[XAXIS] - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[YAXIS] = ((float)accelData500Hz[YAXIS] - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
sensors.accel500Hz[ZAXIS] = -((float)accelData500Hz[ZAXIS] - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
sensors.gyro500Hz[ROLL ] = ((float)gyroData500Hz[ROLL ] - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = ((float)gyroData500Hz[PITCH] - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroData500Hz[YAW ] - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
getOrientation(accAngleSmooth, sensors.evvgcCFAttitude500Hz, sensors.accel500Hz, sensors.gyro500Hz, dt500Hz);
sensors.accel500Hz[ROLL ] = firstOrderFilter(sensors.accel500Hz[ROLL ], &firstOrderFilters[ACCEL_X_500HZ_LOWPASS ]);
sensors.accel500Hz[PITCH] = firstOrderFilter(sensors.accel500Hz[PITCH], &firstOrderFilters[ACCEL_Y_500HZ_LOWPASS]);
sensors.accel500Hz[YAW ] = firstOrderFilter(sensors.accel500Hz[YAW ], &firstOrderFilters[ACCEL_Z_500HZ_LOWPASS ]);
MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
false, //magDataUpdate,
dt500Hz);
//magDataUpdate = false;
computeMotorCommands(dt500Hz);
executionTime500Hz = micros() - currentTime;
#ifdef _DTIMING
LA2_DISABLE;
#endif
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
LED2_TOGGLE;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
LED1_TOGGLE;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
