void loggerInit(void) {
memset((void *)&loggerData, 0, sizeof(loggerData));
loggerSetup();
// skip the first 512 bytes (used exclusively by the USB MSC driver)
loggerData.loggerBuf = (TCHAR *)(filerBuf + 512);
loggerData.bufSize = ((FILER_BUF_SIZE-512) / loggerData.packetSize / FILER_FLUSH_THRESHOLD) * loggerData.packetSize * FILER_FLUSH_THRESHOLD;
loggerData.logHandle = filerGetHandle(LOGGER_FNAME);
filerStream(loggerData.logHandle, loggerData.loggerBuf, loggerData.bufSize);
loggerDoHeader();
}
void motorsSetupLogging(void) {
// setup logging
motorsData.logHandle = filerGetHandle("ESC");
filerStream(motorsData.logHandle, motorsLogBuf, sizeof(motorsLogBuf));
}
void navUkfInit(void) {
float Q[SIM_S]; // state variance
float V[SIM_V]; // process variance
float mag[3];
memset((void *)&navUkfData, 0, sizeof(navUkfData));
navUkfData.v0a[0] = 0.0f;
navUkfData.v0a[1] = 0.0f;
navUkfData.v0a[2] = -1.0f;
// calculate mag vector based on inclination
mag[0] = cosf(p[IMU_MAG_INCL] * DEG_TO_RAD);
mag[1] = 0.0f;
mag[2] = -sinf(p[IMU_MAG_INCL] * DEG_TO_RAD);
// rotate local mag vector to align with true north
navUkfData.v0m[0] = mag[0] * cosf(p[IMU_MAG_DECL] * DEG_TO_RAD) - mag[1] * sinf(p[IMU_MAG_DECL] * DEG_TO_RAD);
navUkfData.v0m[1] = mag[1] * cosf(p[IMU_MAG_DECL] * DEG_TO_RAD) + mag[0] * sinf(p[IMU_MAG_DECL] * DEG_TO_RAD);
navUkfData.v0m[2] = mag[2];
navUkfData.kf = srcdkfInit(SIM_S, SIM_M, SIM_V, SIM_N, navUkfTimeUpdate);
navUkfData.x = srcdkfGetState(navUkfData.kf);
Q[0] = UKF_VEL_Q;
Q[1] = UKF_VEL_Q;
Q[2] = UKF_VEL_ALT_Q;
Q[3] = UKF_POS_Q;
Q[4] = UKF_POS_Q;
Q[5] = UKF_POS_ALT_Q;
Q[6] = UKF_ACC_BIAS_Q;
Q[7] = UKF_ACC_BIAS_Q;
Q[8] = UKF_ACC_BIAS_Q;
Q[9] = UKF_GYO_BIAS_Q;
Q[10] = UKF_GYO_BIAS_Q;
Q[11] = UKF_GYO_BIAS_Q;
Q[12] = UKF_QUAT_Q;
Q[13] = UKF_QUAT_Q;
Q[14] = UKF_QUAT_Q;
Q[15] = UKF_QUAT_Q;
Q[16] = UKF_PRES_ALT_Q;
V[UKF_V_NOISE_ACC_BIAS_X] = UKF_ACC_BIAS_V;
V[UKF_V_NOISE_ACC_BIAS_Y] = UKF_ACC_BIAS_V;
V[UKF_V_NOISE_ACC_BIAS_Z] = UKF_ACC_BIAS_V;
V[UKF_V_NOISE_GYO_BIAS_X] = UKF_GYO_BIAS_V;
V[UKF_V_NOISE_GYO_BIAS_Y] = UKF_GYO_BIAS_V;
V[UKF_V_NOISE_GYO_BIAS_Z] = UKF_GYO_BIAS_V;
V[UKF_V_NOISE_RATE_X] = UKF_RATE_V;
V[UKF_V_NOISE_RATE_Y] = UKF_RATE_V;
V[UKF_V_NOISE_RATE_Z] = UKF_RATE_V;
V[UKF_V_NOISE_VELN] = UKF_VEL_V;
V[UKF_V_NOISE_VELE] = UKF_VEL_V;
V[UKF_V_NOISE_VELD] = UKF_ALT_VEL_V;
srcdkfSetVariance(navUkfData.kf, Q, V, 0, 0);
navUkfInitState();
navUkfData.flowRotCos = cosf(UKF_FLOW_ROT * DEG_TO_RAD);
navUkfData.flowRotSin = sinf(UKF_FLOW_ROT * DEG_TO_RAD);
#ifdef UKF_LOG_FNAME
navUkfData.logHandle = filerGetHandle(UKF_LOG_FNAME);
filerStream(navUkfData.logHandle, ukfLog, UKF_LOG_BUF_SIZE);
#endif
}
