// states, max observations, process noise, max observation noise
srcdkf_t *srcdkfInit(int s, int m, int v, int vMin, int n, SRCDKFTimeUpdate_t *timeUpdate) {
srcdkf_t *f;
int maxN = MAX(v, n);
f = (srcdkf_t *)aqDataCalloc(1, sizeof(srcdkf_t));
f->S = s;
f->V = v;
matrixInit(&f->Sx, s, s);
matrixInit(&f->SxT, s, s);
matrixInit(&f->SxTemp, s, s);
matrixInit(&f->Sv, v, v);
matrixInit(&f->Sn, n, n);
matrixInit(&f->x, s, 1);
matrixInit(&f->Xa, s+maxN, 1+(s+maxN)*2);
matrixInit(&f->qrTempS, s, (s+v)*2);
matrixInit(&f->y, m, 1);
matrixInit(&f->Y, m, 1+(s+n)*2);
matrixInit(&f->qrTempM, m, (s+n)*2);
matrixInit(&f->Sy, m, m);
matrixInit(&f->SyT, m, m);
matrixInit(&f->SyC, m, m);
matrixInit(&f->Pxy, s, m);
matrixInit(&f->C1, m, s);
matrixInit(&f->C1T, s, m);
matrixInit(&f->C2, m, n);
matrixInit(&f->D, m, s+n);
matrixInit(&f->K, s, m);
matrixInit(&f->inov, m, 1);
matrixInit(&f->xUpdate, s, 1);
matrixInit(&f->qrFinal, s, 2*s + 2*n);
matrixInit(&f->Q, s, s+n); // scratch
matrixInit(&f->R, n, n); // scratch
matrixInit(&f->AQ, s, n); // scratch
f->xOut = (float32_t *)aqDataCalloc((uint16_t)s, sizeof(float32_t));
f->xNoise = (float32_t *)aqDataCalloc(maxN, sizeof(float32_t));
f->xIn = (float32_t *)aqDataCalloc((uint16_t)s, sizeof(float32_t));
f->h = (float32_t)SRCDKF_H;
f->hh = f->h*f->h;
// f->w0m = (f->hh - (float32_t)s) / f->hh; // calculated in process
f->wim = 1.0f / (2.0f * f->hh);
f->wic1 = (float32_t)aq_sqrtf(1.0f / (4.0f * f->hh));
f->wic2 = (float32_t)aq_sqrtf((f->hh - 1.0f) / (4.0f * f->hh*f->hh));
f->timeUpdate = timeUpdate;
return f;
}
void matrixInit(arm_matrix_instance_f32 *m, int rows, int cols) {
float32_t *d;
d = (float32_t *)aqDataCalloc(rows*cols, sizeof(float32_t));
arm_mat_init_f32(m, rows, cols, d);
arm_fill_f32(0, d, rows*cols);
}
// size in words
OS_STK *aqStackInit(uint16_t size, char *name) {
OS_STK *sp;
// use memory in the CCM
sp = (OS_STK *)aqDataCalloc(1, size*4);
// fill memory with pattern to ease overflow detection
memset(sp, 0xFF, size*4);
#ifdef UTIL_STACK_CHECK
stackPointers[numStacks] = sp;
stackSizes[numStacks] = size*4;
stackFrees[numStacks] = stackSizes[numStacks];
stackNames[numStacks] = name;
numStacks++;
#endif
return sp;
}
pidStruct_t *pidInit(float *p, float *i, float *d, float *f, float *pMax, float *iMax, float *dMax, float *oMax, int16_t *pTrim, int16_t *iTrim, int16_t *dTrim, int16_t *fTrim) {
pidStruct_t *pid;
pid = (pidStruct_t *)aqDataCalloc(1, sizeof(pidStruct_t));
pid->pMax = pMax;
pid->iMax = iMax;
pid->dMax = dMax;
pid->oMax = oMax;
pid->pGain = p;
pid->iGain = i;
pid->dGain = d;
pid->fGain = f;
pid->pTrim = pTrim;
pid->iTrim = iTrim;
pid->dTrim = dTrim;
pid->fTrim = fTrim;
return pid;
}
digitalPin *digitalInit(GPIO_TypeDef* port, const uint16_t pin) {
digitalPin *p;
GPIO_InitTypeDef GPIO_InitStructure;
p = (digitalPin *)aqDataCalloc(1, sizeof(digitalPin));
p->port = port;
p->pin = pin;
digitalLo(p);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = pin;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(port, &GPIO_InitStructure);
return p;
}
void loggerSetup(void) {
int i;
loggerData.numFields = sizeof(loggerFields) / sizeof(loggerFields_t);
loggerData.packetSize = 3 + 2; // signature + checksum
loggerData.fp = (fieldData_t *)aqDataCalloc(loggerData.numFields, sizeof(fieldData_t));
for (i = 0; i < loggerData.numFields; i++) {
switch (loggerFields[i].fieldId) {
case LOG_LASTUPDATE:
loggerData.fp[i].fieldPointer = (void *)&IMU_LASTUPD;
break;
case LOG_VOLTAGE0:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&mpu6000Data.rawGyo[0];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[0];
#endif
break;
case LOG_VOLTAGE1:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&mpu6000Data.rawGyo[1];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[1];
#endif
break;
case LOG_VOLTAGE2:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&mpu6000Data.rawGyo[2];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[2];
#endif
break;
case LOG_VOLTAGE3:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&hmc5983Data.rawMag[0];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[3];
#endif
break;
case LOG_VOLTAGE4:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&hmc5983Data.rawMag[1];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[4];
#endif
break;
case LOG_VOLTAGE5:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&hmc5983Data.rawMag[2];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[5];
#endif
break;
case LOG_VOLTAGE6:
#ifdef HAS_AIMU
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[6];
#endif
break;
case LOG_VOLTAGE7:
#ifdef HAS_AIMU
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[7];
#else
loggerData.fp[i].fieldPointer = (void *)&analogData.voltages[ANALOG_VOLTS_VIN];
#endif
break;
case LOG_VOLTAGE8:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&mpu6000Data.rawAcc[0];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[8];
#endif
break;
case LOG_VOLTAGE9:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&mpu6000Data.rawAcc[1];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[9];
#endif
break;
case LOG_VOLTAGE10:
#ifdef USE_DIGITAL_IMU
loggerData.fp[i].fieldPointer = (void *)&mpu6000Data.rawAcc[2];
#else
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[10];
#endif
break;
#ifdef HAS_AIMU
case LOG_VOLTAGE11:
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[11];
break;
case LOG_VOLTAGE12:
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[12];
break;
case LOG_VOLTAGE13:
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[13];
break;
case LOG_VOLTAGE14:
loggerData.fp[i].fieldPointer = (void *)&adcData.voltages[14];
break;
#endif
case LOG_IMU_RATEX:
loggerData.fp[i].fieldPointer = (void *)&IMU_RATEX;
break;
case LOG_IMU_RATEY:
loggerData.fp[i].fieldPointer = (void *)&IMU_RATEY;
break;
case LOG_IMU_RATEZ:
loggerData.fp[i].fieldPointer = (void *)&IMU_RATEZ;
break;
case LOG_IMU_ACCX:
loggerData.fp[i].fieldPointer = (void *)&IMU_ACCX;
break;
case LOG_IMU_ACCY:
loggerData.fp[i].fieldPointer = (void *)&IMU_ACCY;
break;
case LOG_IMU_ACCZ:
loggerData.fp[i].fieldPointer = (void *)&IMU_ACCZ;
break;
case LOG_IMU_MAGX:
loggerData.fp[i].fieldPointer = (void *)&IMU_MAGX;
break;
case LOG_IMU_MAGY:
loggerData.fp[i].fieldPointer = (void *)&IMU_MAGY;
break;
case LOG_IMU_MAGZ:
loggerData.fp[i].fieldPointer = (void *)&IMU_MAGZ;
break;
case LOG_GPS_PDOP:
loggerData.fp[i].fieldPointer = (void *)&gpsData.pDOP;
break;
case LOG_GPS_HDOP:
loggerData.fp[i].fieldPointer = (void *)&gpsData.hDOP;
break;
case LOG_GPS_VDOP:
loggerData.fp[i].fieldPointer = (void *)&gpsData.vDOP;
break;
case LOG_GPS_TDOP:
loggerData.fp[i].fieldPointer = (void *)&gpsData.tDOP;
break;
case LOG_GPS_NDOP:
loggerData.fp[i].fieldPointer = (void *)&gpsData.nDOP;
break;
case LOG_GPS_EDOP:
loggerData.fp[i].fieldPointer = (void *)&gpsData.eDOP;
break;
case LOG_GPS_ITOW:
loggerData.fp[i].fieldPointer = (void *)&gpsData.iTOW;
break;
case LOG_GPS_POS_UPDATE:
loggerData.fp[i].fieldPointer = (void *)&gpsData.lastPosUpdate;
break;
case LOG_GPS_LAT:
loggerData.fp[i].fieldPointer = (void *)&gpsData.lat;
break;
case LOG_GPS_LON:
loggerData.fp[i].fieldPointer = (void *)&gpsData.lon;
break;
case LOG_GPS_HEIGHT:
loggerData.fp[i].fieldPointer = (void *)&gpsData.height;
break;
case LOG_GPS_HACC:
loggerData.fp[i].fieldPointer = (void *)&gpsData.hAcc;
break;
case LOG_GPS_VACC:
loggerData.fp[i].fieldPointer = (void *)&gpsData.vAcc;
break;
case LOG_GPS_VEL_UPDATE:
loggerData.fp[i].fieldPointer = (void *)&gpsData.lastVelUpdate;
break;
case LOG_GPS_VELN:
loggerData.fp[i].fieldPointer = (void *)&gpsData.velN;
break;
case LOG_GPS_VELE:
loggerData.fp[i].fieldPointer = (void *)&gpsData.velE;
break;
case LOG_GPS_VELD:
loggerData.fp[i].fieldPointer = (void *)&gpsData.velD;
break;
case LOG_GPS_SACC:
loggerData.fp[i].fieldPointer = (void *)&gpsData.sAcc;
break;
case LOG_ADC_PRESSURE1:
loggerData.fp[i].fieldPointer = (void *)&AQ_PRESSURE;
break;
#ifdef HAS_AIMU
case LOG_ADC_PRESSURE2:
loggerData.fp[i].fieldPointer = (void *)&adcData.pressure2;
break;
#endif
case LOG_ADC_TEMP0:
loggerData.fp[i].fieldPointer = (void *)&IMU_TEMP;
break;
case LOG_ADC_VIN:
loggerData.fp[i].fieldPointer = (void *)&analogData.vIn;
break;
#ifdef HAS_AIMU
case LOG_ADC_MAG_SIGN:
loggerData.fp[i].fieldPointer = (void *)&adcData.magSign;
break;
#endif
case LOG_UKF_Q1:
loggerData.fp[i].fieldPointer = (void *)&UKF_Q1;
break;
case LOG_UKF_Q2:
loggerData.fp[i].fieldPointer = (void *)&UKF_Q2;
break;
case LOG_UKF_Q3:
loggerData.fp[i].fieldPointer = (void *)&UKF_Q3;
break;
case LOG_UKF_Q4:
loggerData.fp[i].fieldPointer = (void *)&UKF_Q4;
break;
case LOG_UKF_POSN:
loggerData.fp[i].fieldPointer = (void *)&UKF_POSN;
break;
case LOG_UKF_POSE:
loggerData.fp[i].fieldPointer = (void *)&UKF_POSE;
break;
case LOG_UKF_POSD:
loggerData.fp[i].fieldPointer = (void *)&UKF_POSD;
break;
case LOG_UKF_PRES_ALT:
loggerData.fp[i].fieldPointer = (void *)&UKF_PRES_ALT;
break;
case LOG_UKF_ALT:
loggerData.fp[i].fieldPointer = (void *)&UKF_ALTITUDE;
break;
case LOG_UKF_VELN:
loggerData.fp[i].fieldPointer = (void *)&UKF_VELN;
break;
case LOG_UKF_VELE:
loggerData.fp[i].fieldPointer = (void *)&UKF_VELE;
break;
case LOG_UKF_VELD:
loggerData.fp[i].fieldPointer = (void *)&UKF_VELD;
break;
case LOG_MOT_MOTOR0:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[0];
break;
case LOG_MOT_MOTOR1:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[1];
break;
case LOG_MOT_MOTOR2:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[2];
break;
case LOG_MOT_MOTOR3:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[3];
break;
case LOG_MOT_MOTOR4:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[4];
break;
case LOG_MOT_MOTOR5:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[5];
break;
case LOG_MOT_MOTOR6:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[6];
break;
case LOG_MOT_MOTOR7:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[7];
break;
case LOG_MOT_MOTOR8:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[8];
break;
case LOG_MOT_MOTOR9:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[9];
break;
case LOG_MOT_MOTOR10:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[10];
break;
case LOG_MOT_MOTOR11:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[11];
break;
case LOG_MOT_MOTOR12:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[12];
break;
case LOG_MOT_MOTOR13:
loggerData.fp[i].fieldPointer = (void *)&motorsData.value[13];
break;
case LOG_MOT_THROTTLE:
loggerData.fp[i].fieldPointer = (void *)&motorsData.throttle;
break;
case LOG_MOT_PITCH:
loggerData.fp[i].fieldPointer = (void *)&motorsData.pitch;
break;
case LOG_MOT_ROLL:
loggerData.fp[i].fieldPointer = (void *)&motorsData.roll;
break;
case LOG_MOT_YAW:
loggerData.fp[i].fieldPointer = (void *)&motorsData.yaw;
break;
case LOG_RADIO_QUALITY:
loggerData.fp[i].fieldPointer = (void *)&RADIO_QUALITY;
break;
case LOG_RADIO_CHANNEL0:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[0];
break;
case LOG_RADIO_CHANNEL1:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[1];
break;
case LOG_RADIO_CHANNEL2:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[2];
break;
case LOG_RADIO_CHANNEL3:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[3];
break;
case LOG_RADIO_CHANNEL4:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[4];
break;
case LOG_RADIO_CHANNEL5:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[5];
break;
case LOG_RADIO_CHANNEL6:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[6];
break;
case LOG_RADIO_CHANNEL7:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[7];
break;
case LOG_RADIO_CHANNEL8:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[8];
break;
case LOG_RADIO_CHANNEL9:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[9];
break;
case LOG_RADIO_CHANNEL10:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[10];
break;
case LOG_RADIO_CHANNEL11:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[11];
break;
case LOG_RADIO_CHANNEL12:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[12];
break;
case LOG_RADIO_CHANNEL13:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[13];
break;
case LOG_RADIO_CHANNEL14:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[14];
break;
case LOG_RADIO_CHANNEL15:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[15];
break;
case LOG_RADIO_CHANNEL16:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[16];
break;
case LOG_RADIO_CHANNEL17:
loggerData.fp[i].fieldPointer = (void *)&radioData.channels[17];
break;
case LOG_RADIO_ERRORS:
loggerData.fp[i].fieldPointer = (void *)&RADIO_ERROR_COUNT;
break;
case LOG_GMBL_TRIGGER:
loggerData.fp[i].fieldPointer = (void *)&gimbalData.triggerLogVal;
break;
case LOG_ACC_BIAS_X:
loggerData.fp[i].fieldPointer = (void *)&UKF_ACC_BIAS_X;
break;
case LOG_ACC_BIAS_Y:
loggerData.fp[i].fieldPointer = (void *)&UKF_ACC_BIAS_Y;
break;
case LOG_ACC_BIAS_Z:
loggerData.fp[i].fieldPointer = (void *)&UKF_ACC_BIAS_Z;
break;
}
switch (loggerFields[i].fieldType) {
case LOG_TYPE_DOUBLE:
loggerData.fp[i].copyFunc = loggerCopy8;
loggerData.packetSize += 8;
break;
case LOG_TYPE_FLOAT:
case LOG_TYPE_U32:
case LOG_TYPE_S32:
loggerData.fp[i].copyFunc = loggerCopy4;
loggerData.packetSize += 4;
break;
case LOG_TYPE_U16:
case LOG_TYPE_S16:
loggerData.fp[i].copyFunc = loggerCopy2;
loggerData.packetSize += 2;
break;
case LOG_TYPE_U8:
case LOG_TYPE_S8:
loggerData.fp[i].copyFunc = loggerCopy1;
loggerData.packetSize += 1;
break;
}
}
}
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);
}
