// 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;
}
spiClient_t *spiClientInit(SPI_TypeDef *spi, uint16_t baud, GPIO_TypeDef *csPort, uint16_t csPin, volatile uint32_t *flag, spiCallback_t *callback) {
spiClient_t *client;
client = (spiClient_t *)aqCalloc(1, sizeof(spiClient_t));
#ifdef SPI_SPI1_CLOCK
if (spi == SPI1) {
client->interface = 0;
spi1Init();
}
#endif
#ifdef SPI_SPI2_CLOCK
if (spi == SPI2) {
client->interface = 1;
spi2Init();
}
#endif
#ifdef SPI_SPI3_CLOCK
if (spi == SPI3) {
client->interface = 2;
spi3Init();
}
#endif
client->cs = digitalInit(csPort, csPin, 1);
spiDeselect(client);
spiChangeBaud(client, baud);
client->flag = flag;
client->callback = callback;
return client;
}
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;
}
serialPort_t *serialUSART6(unsigned int flowControl, unsigned int rxBufSize, unsigned int txBufSize) {
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
serialPort_t *s;
s = serialPort6 = (serialPort_t *)aqCalloc(1, sizeof(serialPort_t));
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
// Enable USART6 clock
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART6, ENABLE);
#ifdef SERIAL_UART6_RX_PIN
s->rxBufSize = (rxBufSize) ? rxBufSize : SERIAL_DEFAULT_RX_BUFSIZE;
s->rxBuf = (volatile unsigned char*)aqCalloc(1, s->rxBufSize);
GPIO_PinAFConfig(SERIAL_UART6_PORT, SERIAL_UART6_RX_SOURCE, GPIO_AF_USART6);
GPIO_InitStructure.GPIO_Pin = SERIAL_UART6_RX_PIN;
GPIO_Init(SERIAL_UART6_PORT, &GPIO_InitStructure);
#ifdef SERIAL_UART6_RX_DMA_ST
s->rxDMAStream = SERIAL_UART6_RX_DMA_ST;
s->rxDMAChannel = SERIAL_UART6_RX_DMA_CH;
s->rxDmaFlags = SERIAL_UART6_RX_TC_FLAG | SERIAL_UART6_RX_HT_FLAG | SERIAL_UART6_RX_TE_FLAG | SERIAL_UART6_RX_DM_FLAG | SERIAL_UART6_RX_FE_FLAG;
#else
// otherwise use interrupts
NVIC_InitStructure.NVIC_IRQChannel = USART6_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif // SERIAL_UART6_RX_DMA_ST
#endif // SERIAL_UART6_RX_PIN
#ifdef SERIAL_UART6_TX_PIN
s->txBufSize = txBufSize;
s->txBuf = (volatile unsigned char*)aqCalloc(1, s->txBufSize);
GPIO_PinAFConfig(SERIAL_UART6_PORT, SERIAL_UART6_TX_SOURCE, GPIO_AF_USART6);
GPIO_InitStructure.GPIO_Pin = SERIAL_UART6_TX_PIN;
GPIO_Init(SERIAL_UART6_PORT, &GPIO_InitStructure);
#ifdef SERIAL_UART6_TX_DMA_ST
s->txDMAStream = SERIAL_UART6_TX_DMA_ST;
s->txDMAChannel = SERIAL_UART6_TX_DMA_CH;
s->txDmaFlags = SERIAL_UART6_TX_TC_FLAG | SERIAL_UART6_TX_HT_FLAG | SERIAL_UART6_TX_TE_FLAG | SERIAL_UART6_TX_DM_FLAG | SERIAL_UART6_TX_FE_FLAG;
// Enable the DMA global Interrupt
NVIC_InitStructure.NVIC_IRQChannel = SERIAL_UART6_TX_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#else
// otherwise use interrupts
NVIC_InitStructure.NVIC_IRQChannel = USART6_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif // SERIAL_UART6_TX_DMA_ST
#endif // SERIAL_UART6_TX_PIN
#ifdef SERIAL_UART6_CTS_PIN
if (flowControl == USART_HardwareFlowControl_RTS_CTS) {
GPIO_PinAFConfig(SERIAL_UART6_PORT, SERIAL_UART6_RTS_SOURCE, GPIO_AF_USART6);
GPIO_PinAFConfig(SERIAL_UART6_PORT, SERIAL_UART6_CTS_SOURCE, GPIO_AF_USART6);
GPIO_InitStructure.GPIO_Pin = SERIAL_UART6_CTS_PIN | SERIAL_UART6_RTS_PIN;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
GPIO_Init(SERIAL_UART6_PORT, &GPIO_InitStructure);
}
#endif // SERIAL_UART6_CTS_PIN
return s;
}
// 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;
}
void logSetup(struct gyro_report* gyro_report,
struct mag_report* mag_report,
struct battery_status_s* battery_status,
struct accel_report* accel_report,
struct baro_report* barometer,
struct sensor_combined_s* raw,
struct ukf_state_vector_s* ukfState) {
int i;
logData.numFields = sizeof(logFields) / sizeof(logFields_t);
logData.packetSize = 3 + 2; // signature + checksum
logData.fp = (fieldData_t *)aqDataCalloc(logData.numFields, sizeof(fieldData_t));
for (i = 0; i < logData.numFields; i++) {
switch (logFields[i].fieldId) {
case LOG_LASTUPDATE:
logData.fp[i].fieldPointer = (void *)&gyro_report->timestamp;
break;
case LOG_VOLTAGE0:
logData.fp[i].fieldPointer = (void *)&gyro_report->x;
break;
case LOG_VOLTAGE1:
logData.fp[i].fieldPointer = (void *)&gyro_report->y;
break;
case LOG_VOLTAGE2:
logData.fp[i].fieldPointer = (void *)&gyro_report->z;
break;
case LOG_VOLTAGE3:
logData.fp[i].fieldPointer = (void *)&mag_report->x;
break;
case LOG_VOLTAGE4:
logData.fp[i].fieldPointer = (void *)&mag_report->y;
break;
case LOG_VOLTAGE5:
logData.fp[i].fieldPointer = (void *)&mag_report->z;
break;
case LOG_VOLTAGE6:
logData.fp[i].fieldPointer = (void *)&gyro_report->temperature;
break;
case LOG_VOLTAGE7:
logData.fp[i].fieldPointer = (void *)&battery_status->voltage_v;
break;
case LOG_VOLTAGE8:
logData.fp[i].fieldPointer = (void *)&accel_report->x;
break;
case LOG_VOLTAGE9:
logData.fp[i].fieldPointer = (void *)&accel_report->y;
break;
case LOG_VOLTAGE10:
logData.fp[i].fieldPointer = (void *)&accel_report->z;
break;
case LOG_VOLTAGE11:
logData.fp[i].fieldPointer = (void *)&barometer->pressure;
break;
case LOG_VOLTAGE12:
logData.fp[i].fieldPointer = (void *)&barometer->pressure;
break;
case LOG_VOLTAGE13:
logData.fp[i].fieldPointer = (void *)&barometer->temperature;
break;
case LOG_VOLTAGE14:
logData.fp[i].fieldPointer = (void *)&accel_report->temperature;
break;
case LOG_IMU_RATEX:
logData.fp[i].fieldPointer = (void *)&raw->gyro_rad_s[0];//(void *)&IMU_RATEX;
break;
case LOG_IMU_RATEY:
logData.fp[i].fieldPointer = (void *)&raw->gyro_rad_s[1];//(void *)&IMU_RATEY;
break;
case LOG_IMU_RATEZ:
logData.fp[i].fieldPointer = (void *)&raw->gyro_rad_s[2];//(void *)&IMU_RATEZ;
break;
case LOG_IMU_ACCX:
logData.fp[i].fieldPointer = (void *)&raw->accelerometer_m_s2[0];//(void *)&IMU_ACCX;
break;
case LOG_IMU_ACCY:
logData.fp[i].fieldPointer = (void *)&raw->accelerometer_m_s2[1];//(void *)&IMU_ACCY;
break;
case LOG_IMU_ACCZ:
logData.fp[i].fieldPointer = (void *)&raw->accelerometer_m_s2[2];//(void *)&IMU_ACCZ;
break;
case LOG_IMU_MAGX:
logData.fp[i].fieldPointer = (void *)&raw->magnetometer_ga[0];//(void *)&IMU_MAGX;
break;
case LOG_IMU_MAGY:
logData.fp[i].fieldPointer = (void *)&raw->magnetometer_ga[1];//(void *)&IMU_MAGY;
break;
case LOG_IMU_MAGZ:
logData.fp[i].fieldPointer = (void *)&raw->magnetometer_ga[2];//(void *)&IMU_MAGZ;
break;
case LOG_GPS_PDOP:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.pDOP;
break;
case LOG_GPS_HDOP:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.hDOP;
break;
case LOG_GPS_VDOP:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.vDOP;
break;
case LOG_GPS_TDOP:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.tDOP;
break;
case LOG_GPS_NDOP:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.nDOP;
break;
case LOG_GPS_EDOP:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.eDOP;
break;
case LOG_GPS_ITOW:
logData.fp[i].fieldPointer = (void *)&dummyUint32;//(void *)&gpsData.iTOW;
break;
case LOG_GPS_POS_UPDATE:
logData.fp[i].fieldPointer = (void *)&dummyUint32;//(void *)&gpsData.lastPosUpdate;
break;
case LOG_GPS_LAT:
logData.fp[i].fieldPointer = (void *)&dummyDouble;//(void *)&gpsData.lat;
break;
case LOG_GPS_LON:
logData.fp[i].fieldPointer = (void *)&dummyDouble;//(void *)&gpsData.lon;
break;
case LOG_GPS_HEIGHT:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.height;
break;
case LOG_GPS_HACC:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.hAcc;
break;
case LOG_GPS_VACC:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.vAcc;
break;
case LOG_GPS_VEL_UPDATE:
logData.fp[i].fieldPointer = (void *)&dummyUint32;//(void *)&gpsData.lastVelUpdate;
break;
case LOG_GPS_VELN:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.velN;
break;
case LOG_GPS_VELE:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.velE;
break;
case LOG_GPS_VELD:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.velD;
break;
case LOG_GPS_SACC:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&gpsData.sAcc;
break;
case LOG_ADC_PRESSURE1:
logData.fp[i].fieldPointer = (void *)&barometer->pressure;
break;
case LOG_ADC_PRESSURE2:
logData.fp[i].fieldPointer = (void *)&barometer->pressure;//(void *)&adcData.pressure2;
break;
case LOG_ADC_TEMP0:
logData.fp[i].fieldPointer = (void *)&gyro_report->temperature;//(void *)&IMU_TEMP; //original board temp used for all calculations
break;
case LOG_ADC_VIN:
logData.fp[i].fieldPointer = (void *)&battery_status->voltage_v;//(void *)&adcData.vIn;
break;
case LOG_ADC_MAG_SIGN:
logData.fp[i].fieldPointer = (void *)&dummyint8_Value1;//(void *)&adcData.magSign;
break;
case LOG_UKF_Q1:
logData.fp[i].fieldPointer = (void *)&ukfState->q1;
break;
case LOG_UKF_Q2:
logData.fp[i].fieldPointer = (void *)&ukfState->q2;//(void *)&UKF_Q2;
break;
case LOG_UKF_Q3:
logData.fp[i].fieldPointer = (void *)&ukfState->q3;//(void *)&UKF_Q3;
break;
case LOG_UKF_Q4:
logData.fp[i].fieldPointer = (void *)&ukfState->q4;//(void *)&UKF_Q4;
break;
case LOG_UKF_POSN:
logData.fp[i].fieldPointer = (void *)&ukfState->pos_x;//(void *)&UKF_POSN;
break;
case LOG_UKF_POSE:
logData.fp[i].fieldPointer = (void *)&ukfState->pos_y;//(void *)&UKF_POSE;
break;
case LOG_UKF_POSD:
logData.fp[i].fieldPointer = (void *)&ukfState->pos_d;//(void *)&UKF_POSD;
break;
case LOG_UKF_PRES_ALT:
logData.fp[i].fieldPointer = (void *)&raw->baro_pres_mbar;//(void *)&UKF_PRES_ALT;
break;
case LOG_UKF_ALT:
logData.fp[i].fieldPointer = (void *)&raw->baro_alt_meter;//(void *)&UKF_ALTITUDE;
break;
case LOG_UKF_VELN:
logData.fp[i].fieldPointer = (void *)&ukfState->vel_x;//(void *)&UKF_VELN;
break;
case LOG_UKF_VELE:
logData.fp[i].fieldPointer = (void *)&ukfState->vel_y;//(void *)&UKF_VELE;
break;
case LOG_UKF_VELD:
logData.fp[i].fieldPointer = (void *)&ukfState->vel_d;//(void *)&UKF_VELD;
break;
case LOG_MOT_MOTOR0:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[0];
break;
case LOG_MOT_MOTOR1:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[1];
break;
case LOG_MOT_MOTOR2:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[2];
break;
case LOG_MOT_MOTOR3:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[3];
break;
case LOG_MOT_MOTOR4:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[4];
break;
case LOG_MOT_MOTOR5:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[5];
break;
case LOG_MOT_MOTOR6:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[6];
break;
case LOG_MOT_MOTOR7:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[7];
break;
case LOG_MOT_MOTOR8:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[8];
break;
case LOG_MOT_MOTOR9:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[9];
break;
case LOG_MOT_MOTOR10:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[10];
break;
case LOG_MOT_MOTOR11:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[11];
break;
case LOG_MOT_MOTOR12:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[12];
break;
case LOG_MOT_MOTOR13:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&motorsData.value[13];
break;
case LOG_MOT_THROTTLE:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&motorsData.throttle;
break;
case LOG_MOT_PITCH:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&motorsData.pitch;
break;
case LOG_MOT_ROLL:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&motorsData.roll;
break;
case LOG_MOT_YAW:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&motorsData.yaw;
break;
case LOG_RADIO_QUALITY:
logData.fp[i].fieldPointer = (void *)&dummyFloat;//(void *)&RADIO_QUALITY;
break;
case LOG_RADIO_CHANNEL0:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[0];
break;
case LOG_RADIO_CHANNEL1:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[1];
break;
case LOG_RADIO_CHANNEL2:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[2];
break;
case LOG_RADIO_CHANNEL3:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[3];
break;
case LOG_RADIO_CHANNEL4:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[4];
break;
case LOG_RADIO_CHANNEL5:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[5];
break;
case LOG_RADIO_CHANNEL6:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[6];
break;
case LOG_RADIO_CHANNEL7:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[7];
break;
case LOG_RADIO_CHANNEL8:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[8];
break;
case LOG_RADIO_CHANNEL9:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[9];
break;
case LOG_RADIO_CHANNEL10:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[10];
break;
case LOG_RADIO_CHANNEL11:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[11];
break;
case LOG_RADIO_CHANNEL12:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[12];
break;
case LOG_RADIO_CHANNEL13:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[13];
break;
case LOG_RADIO_CHANNEL14:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[14];
break;
case LOG_RADIO_CHANNEL15:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[15];
break;
case LOG_RADIO_CHANNEL16:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[16];
break;
case LOG_RADIO_CHANNEL17:
logData.fp[i].fieldPointer = (void *)&dummyint16;//(void *)&radioData.channels[17];
break;
case LOG_RADIO_ERRORS:
logData.fp[i].fieldPointer = (void *)&dummyUint16;//RADIO_ERROR_COUNT;
break;
}
switch (logFields[i].fieldType) {
case LOG_TYPE_DOUBLE:
logData.fp[i].copyFunc = logCopy8;
logData.packetSize += 8;
break;
case LOG_TYPE_FLOAT:
case LOG_TYPE_U32:
case LOG_TYPE_S32:
logData.fp[i].copyFunc = logCopy4;
logData.packetSize += 4;
break;
case LOG_TYPE_U16:
case LOG_TYPE_S16:
logData.fp[i].copyFunc = logCopy2;
logData.packetSize += 2;
break;
case LOG_TYPE_U8:
case LOG_TYPE_S8:
logData.fp[i].copyFunc = logCopy1;
logData.packetSize += 1;
break;
}
}
logData.logBuf = (char *)aqCalloc(LOGGER_BUF_SIZE, logData.packetSize);
}