// 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); }