// read config from uSD
int8_t configReadFile(char *fname) {
char *fileBuf;
int8_t fh;
int ret;
int p1;
if (fname == 0)
fname = CONFIG_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("config: cannot get read file handle\n");
return -1;
}
fileBuf = (char *)aqCalloc(CONFIG_FILE_BUF_SIZE, sizeof(char));
p1 = 0;
while ((ret = filerRead(fh, fileBuf, -1, CONFIG_FILE_BUF_SIZE)) > 0)
p1 = configParseParams((char *)fileBuf, ret, p1);
filerClose(fh);
if (fileBuf)
aqFree(fileBuf, CONFIG_FILE_BUF_SIZE, sizeof(char));
return ret;
}
// write config to uSD
int8_t configWriteFile(char *fname) {
char buf[128];
int8_t fh;
int8_t ret;
int n;
int i;
if (fname == 0)
fname = CONFIG_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("config: cannot get write file handle\n");
return -1;
}
for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
n = configFormatParam(buf, i);
ret = filerWrite(fh, buf, -1, n);
if (ret < n) {
AQ_NOTICE("config: file write error\n");
ret = -1;
break;
}
}
filerClose(fh);
return ret;
}
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();
}
// write config to uSD
int8_t configWriteFile(char *fname) {
char *buf;
int8_t fh;
int8_t ret;
int n;
int i;
if (fname == 0)
fname = CONFIG_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("config: cannot get write file handle\n");
return -1;
}
if (!(buf = (char *)aqCalloc(128, sizeof(char)))) {
AQ_NOTICE("config: Error writing to file, cannot allocate memory.\n");
filerClose(fh);
return -1;
}
for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
n = configFormatParam(buf, i);
if (n)
ret = filerWrite(fh, buf, -1, n);
if (!n || ret < n) {
ret = -1;
break;
}
}
filerClose(fh);
if (buf)
aqFree(buf, 128, sizeof(char));
if (ret > -1)
AQ_NOTICE("config: Parameters saved to local storage file.\n");
else
AQ_NOTICE("config: Error writing parameters to file.\n");
return ret;
}
// read config from uSD
int8_t configReadFile(char *fname) {
char *fileBuf;
int8_t fh;
int ret;
int p1;
if (fname == 0)
fname = CONFIG_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("config: cannot get read file handle\n");
return -1;
}
if (!(fileBuf = (char *)aqCalloc(CONFIG_FILE_BUF_SIZE, sizeof(char)))) {
AQ_NOTICE("config: Error reading from file, cannot allocate memory.\n");
filerClose(fh);
return -1;
}
p1 = 0;
while ((ret = filerRead(fh, fileBuf, -1, CONFIG_FILE_BUF_SIZE)) > 0) {
p1 = configParseParams((char *)fileBuf, ret, p1);
if (p1 < 0) {
ret = -1;
break;
}
}
filerClose(fh);
if (fileBuf)
aqFree(fileBuf, CONFIG_FILE_BUF_SIZE, sizeof(char));
if (ret > -1)
AQ_NOTICE("config: Parameters loaded from local storage file.\n");
else
AQ_NOTICE("config: Failed to read parameters from local file.");
return ret;
}
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
}
// read esc32 params from uSD
static int8_t esc32ReadFile(char *fname, canNodes_t *canNode) {
char *fileBuf;
char *lineBuf;
char param[24];
int paramId;
float value, retValue;
int8_t needsConfigWrite = 0;
int8_t fh;
char c;
int ret;
int i, p1, p2;
int n;
if (fname == 0)
fname = ESC32_FILE_NAME;
if ((fh = filerGetHandle(fname)) < 0) {
AQ_NOTICE("ESC32: cannot get read file handle\n");
return -1;
}
fileBuf = (char *)aqCalloc(ESC32_FILE_BUF_SIZE, sizeof(char));
lineBuf = (char *)aqCalloc(ESC32_LINE_BUF_SIZE, sizeof(char));
needsConfigWrite = 0;
if (fileBuf && lineBuf) {
p1 = 0;
do {
ret = filerRead(fh, fileBuf, -1, ESC32_FILE_BUF_SIZE);
p2 = 0;
for (i = 0; i < ret; i++) {
c = fileBuf[p2++];
if (c == '\n' || p1 == (ESC32_LINE_BUF_SIZE-1)) {
lineBuf[p1] = 0;
n = sscanf(lineBuf, "#define DEFAULT_%23s %f", param, &value);
if (n != 2) {
n = sscanf(lineBuf, "#define %23s %f", param, &value);
if (n != 2)
n = sscanf(lineBuf, "%23s %f", param, &value);
}
if (n == 2) {
// lookup parameter id
if (canNode)
paramId = canGetParamIdByName(canNode->networkId, (uint8_t *)param);
else
paramId = esc32OwGetParamId(param);
// valid id?
if (paramId >= 0) {
// read
if (canNode)
retValue = canGetParamById(canNode->networkId, paramId);
else
retValue = esc32OwReadParamById(paramId);
// current value differs from ours?
if (retValue != value) {
// write
if (canNode)
canSetParamById(canNode->networkId, paramId, value);
else
esc32OwWriteParam(paramId, value);
// read back
if (canNode)
retValue = canGetParamById(canNode->networkId, paramId);
else
retValue = esc32OwReadParamById(paramId);
// successful write?
if (retValue == value) {
needsConfigWrite++;
}
else {
AQ_NOTICE("ESC32: parameter write failed!\n");
}
}
}
}
p1 = 0;
}
else {
lineBuf[p1++] = c;
}
}
} while (ret > 0);
filerClose(fh);
}
if (lineBuf)
aqFree(lineBuf, ESC32_LINE_BUF_SIZE, sizeof(char));
if (fileBuf)
aqFree(fileBuf, ESC32_FILE_BUF_SIZE, sizeof(char));
return needsConfigWrite;
}
