// 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;
}
int8_t configFormatParam(char *buf, int n) {
char *str;
int8_t ret = 0;
if (!(str = (char *)aqCalloc(16, sizeof(char))))
return ret;
ftoa(str, p[n], 10);
ret = sprintf(buf, "%-17s\t\t%s\n", configParameterStrings[n], str);
if (str)
aqFree(str, 16, sizeof(char));
return ret;
}
int configParseParams(char *fileBuf, int size, int p1) {
static char lineBuf[CONFIG_LINE_BUF_SIZE];
char *param;
float value;
char c;
int p2;
int n;
int i, j;
if (!(param = (char *)aqCalloc(17, sizeof(char))))
return -1;
p2 = 0;
for (i = 0; i < size; i++) {
c = fileBuf[p2++];
if (c == '\n' || p1 == (CONFIG_LINE_BUF_SIZE-1)) {
lineBuf[p1] = 0;
n = sscanf(lineBuf, "#define DEFAULT_%17s %f", param, &value);
if (n != 2) {
n = sscanf(lineBuf, "%17s %f", param, &value);
if (n != 2) {
n = sscanf(lineBuf, "#define %17s %f", param, &value);
}
}
if (n == 2) {
for (j = 0; j < CONFIG_NUM_PARAMS; j++) {
if (!strncasecmp(param, configParameterStrings[j], sizeof(char)*17))
p[j] = value;
}
}
p1 = 0;
}
else {
lineBuf[p1++] = c;
}
}
if (param)
aqFree(param, 17, sizeof(char));
return p1;
}
// 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;
}
static void dIMUWriteCalib(void) {
#ifdef DIMU_HAVE_EEPROM
char *lineBuf;
uint8_t *buf;
int n;
int i, j, k;
if (!(lineBuf = (char *)aqCalloc(128, sizeof(char)))) {
AQ_NOTICE("DIMU: Error writing to EEPROM, cannot allocate memory.\n");
return;
}
buf = eepromOpenWrite();
k = 0;
for (i = 0; i < sizeof(dImuCalibParameters) / sizeof(uint16_t); i++) {
n = configFormatParam(lineBuf, dImuCalibParameters[i]);
for (j = 0; j < n; j++) {
buf[k++] = lineBuf[j];
if (k == DIMU_EEPROM_BLOCK_SIZE) {
eepromWrite();
k = 0;
}
}
}
if (k != 0)
eepromWrite();
if (lineBuf)
aqFree(lineBuf, 128, sizeof(char));
AQ_NOTICE("DIMU: wrote calibration parameters to EEPROM\n");
eepromClose();
#endif
}
uint8_t configFlashWrite(void) {
configRec_t *recs;
uint8_t ret = 0;
int i;
recs = (void *)aqCalloc(CONFIG_NUM_PARAMS, sizeof(configRec_t));
if (recs) {
configToken_t *tr = (configToken_t *)recs;
configToken_t *tf = 0;
// read all tokens
do {
tf = configTokenIterate(tf);
// copy to RAM
if (tf) {
// only one instance per key
do {
if (tr->key == 0 || tr->key == tf->key) {
memcpy(tr, tf, sizeof(configToken_t));
break;
}
tr++;
} while (1);
}
} while (tf);
ret = flashErase(flashStartAddr(), CONFIG_NUM_PARAMS*sizeof(configRec_t)/sizeof(uint32_t));
// invalidate the flash data cache
FLASH_DataCacheCmd(DISABLE);
FLASH_DataCacheReset();
FLASH_DataCacheCmd(ENABLE);
if (ret) {
tr = (configToken_t *)recs;
// copy tokens back to flash
while (tr->key)
configTokenStore(tr++);
// create param list in RAM
for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
memcpy(recs[i].name, configParameterStrings[i], 16);
recs[i].val = p[i];
}
ret = flashAddress(flashStartAddr(), (uint32_t *)recs, CONFIG_NUM_PARAMS*sizeof(configRec_t)/sizeof(uint32_t));
}
aqFree(recs, CONFIG_NUM_PARAMS, sizeof(configRec_t));
AQ_NOTICE("config: Parameters saved to flash memory.\n");
}
else {
AQ_NOTICE("config: Error writing params to flash, cannot allocate memory.\n");
}
return ret;
}
void spiClientFree(spiClient_t *spi) {
if (spi)
aqFree(spi, 1, sizeof(spiClient_t));
}
// 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;
}
