// Copy a calibration void OmCalibrateCopy(omcalibrate_calibration_t *calibration, omcalibrate_calibration_t *source) { if (source == NULL) { OmCalibrateInit(calibration); } else { memcpy(calibration, source, sizeof(omcalibrate_calibration_t)); } }
int OmConvertRunConvert(omconvert_settings_t *settings, calc_t *calc) { int retVal = EXIT_OK; omdata_t omdata = { 0 }; om_convert_arrangement_t arrangement = { 0 }; // Output information file FILE *infofp = NULL; if (settings->infoFilename != NULL) { infofp = fopen(settings->infoFilename, "wt"); if (infofp == NULL) { fprintf(stderr, "ERROR: Cannot open output information file: %s\n", settings->infoFilename); return EXIT_CANTCREAT; } } // Load input data if (!OmDataLoad(&omdata, settings->filename)) { const char *msg = "ERROR: Problem loading file.\n"; fprintf(stderr, msg); fprintf(stdout, msg); return EXIT_DATAERR; } fprintf(stderr, "Data loaded!\n"); OmDataDump(&omdata); // For each session: omdata_session_t *session; int sessionCount = 0; for (session = omdata.firstSession; session != NULL; session = session->sessionNext) { sessionCount++; fprintf(stderr, "=== SESSION %d ===\n", sessionCount); if (sessionCount > 1) { fprintf(stderr, "NOTE: Skipping session %d...\n", sessionCount); continue; } // Find a configuration OmConvertFindArrangement(&arrangement, settings, &omdata, session, defaultChannelPriority); // Calibration configuration omcalibrate_config_t calibrateConfig = { 0 }; OmCalibrateConfigInit(&calibrateConfig); calibrateConfig.stationaryTime = settings->stationaryTime; // 10.0; calibrateConfig.stationaryRepeated = settings->repeatedStationary; // Start a player om_convert_player_t player = { 0 }; OmConvertPlayerInitialize(&player, &arrangement, settings->sampleRate, settings->interpolate); // Initialize here for find stationary points // Initialize calibration omcalibrate_calibration_t calibration; OmCalibrateInit(&calibration); if (settings->calibrate) { // Find stationary points omcalibrate_stationary_points_t *stationaryPoints; fprintf(stderr, "Finding stationary points...\n"); stationaryPoints = OmCalibrateFindStationaryPoints(&calibrateConfig, &player); // Player already initialized fprintf(stderr, "Found stationary points: %d\n", stationaryPoints->numValues); // Dump no calibration OmCalibrateDump(&calibration, stationaryPoints, 0); // Auto-calibrate fprintf(stderr, "Auto-calibrating...\n"); int calibrationResult = OmCalibrateFindAutoCalibration(&calibrateConfig, stationaryPoints, &calibration); OmCalibrateDump(&calibration, stationaryPoints, 1); if (calibrationResult < 0) { fprintf(stderr, "Auto-calibration: using identity calibration...\n"); int ec = calibration.errorCode; // Copy error code int na = calibration.numAxes; // ...and num-axes OmCalibrateInit(&calibration); calibration.errorCode = ec; // Copy error code to identity calibration calibration.numAxes = na; // ...and num-axes } // Free stationary points OmCalibrateFreeStationaryPoints(stationaryPoints); } // Output range scalings int outputAccelRange = 8; // TODO: Possibly allow for +/- 16 outputs (currently always +/-8g -> 16-bit signed)? if (outputAccelRange < 8) { outputAccelRange = 8; } // Minimum of +/-2, +/-4, +/-8 all get output coded as +/-8 int outputAccelScale = 65536 / (2 * outputAccelRange); int outputChannels = arrangement.numChannels + 1; int outputRate = (int)(player.sampleRate + 0.5); int outputSamples = player.numSamples; // Metadata - [Artist�"IART" WAV chunk] Data about the device that made the recording char artist[WAV_META_LENGTH] = { 0 }; sprintf(artist, "Id: %u\n" "Device: %s\n" "Revision: %d\n" "Firmware: %d", omdata.metadata.deviceId, omdata.metadata.deviceTypeString, omdata.metadata.deviceVersion, omdata.metadata.firmwareVer ); // Metadata - [Title�"INAM" WAV chunk] Data about the recording configuration char name[WAV_META_LENGTH] = { 0 }; sprintf(name, "Session: %u\n" "Start: %s\n" "Stop: %s\n" "Config-A: %d,%d\n" "Metadata: %s", (unsigned int)omdata.metadata.sessionId, TimeString(omdata.metadata.recordingStart), TimeString(omdata.metadata.recordingStop), omdata.metadata.configAccel.frequency, omdata.metadata.configAccel.sensitivity, omdata.metadata.metadata ); // Metadata - [Creation�date�"ICRD"�WAV�chunk] - Specify�the�time of the first sample (also in the comment for Matlab) char datetime[WAV_META_LENGTH] = { 0 }; sprintf(datetime, "%s", TimeString(arrangement.startTime)); // Metadata - [Comment�"ICMT" WAV chunk] Data about this file representation char comment[WAV_META_LENGTH] = { 0 }; sprintf(comment, "Time: %s\n" "Channel-1: Accel-X\n" "Scale-1: %d\n" "Channel-2: Accel-Y\n" "Scale-2: %d\n" "Channel-3: Accel-Z\n" "Scale-3: %d\n" "Channel-4: Aux", TimeString(arrangement.startTime), outputAccelRange, outputAccelRange, outputAccelRange ); // Create output WAV file FILE *ofp = NULL; if (settings->outFilename != NULL && strlen(settings->outFilename) > 0) { fprintf(stderr, "Generating WAV file: %s\n", settings->outFilename); ofp = fopen(settings->outFilename, "wb"); if (ofp == NULL) { fprintf(stderr, "Cannot open output WAV file: %s\n", settings->outFilename); retVal = EXIT_CANTCREAT; break; } WavInfo wavInfo = { 0 }; wavInfo.bytesPerChannel = 2; wavInfo.chans = outputChannels; wavInfo.freq = outputRate; wavInfo.numSamples = outputSamples; wavInfo.infoArtist = artist; wavInfo.infoName = name; wavInfo.infoDate = datetime; wavInfo.infoComment = comment; // Try to start the data at 1k offset (create a dummy 'JUNK' header) wavInfo.offset = 1024; if (WavWrite(&wavInfo, ofp) <= 0) { fprintf(stderr, "ERROR: Problem writing WAV file.\n"); retVal = EXIT_IOERR; break; } } // Re-start the player OmConvertPlayerInitialize(&player, &arrangement, settings->sampleRate, settings->interpolate); int outputOk = CalcInit(calc, player.sampleRate, player.arrangement->startTime); // Whether any processing outputs are used // Calculate each output sample between the start/end time of session if (!outputOk && ofp == NULL) { fprintf(stderr, "ERROR: No output.\n"); retVal = EXIT_CONFIG; break; } else { // Write other information to info file if (infofp != NULL) { fprintf(infofp, ":\n"); fprintf(infofp, "::: Data about the conversion process\n"); fprintf(infofp, "Result-file-version: %d\n", 1); fprintf(infofp, "Convert-version: %d\n", CONVERT_VERSION); fprintf(infofp, "Processed: %s\n", TimeString(TimeNow())); fprintf(infofp, "File-input: %s\n", settings->filename); fprintf(infofp, "File-output: %s\n", settings->outFilename); fprintf(infofp, "Results-output: %s\n", settings->infoFilename); fprintf(infofp, "Auto-calibration: %d\n", settings->calibrate); fprintf(infofp, "Calibration-Result: %d\n", calibration.errorCode); fprintf(infofp, "Calibration: %.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\n", calibration.scale[0], calibration.scale[1], calibration.scale[2], calibration.offset[0], calibration.offset[1], calibration.offset[2], calibration.tempOffset[0], calibration.tempOffset[1], calibration.tempOffset[2], calibration.referenceTemperature); fprintf(infofp, "Input-sectors-total: %d\n", omdata.statsTotalSectors); fprintf(infofp, "Input-sectors-data: %d\n", omdata.statsDataSectors); fprintf(infofp, "Input-sectors-bad: %d\n", omdata.statsBadSectors); fprintf(infofp, "Output-rate: %d\n", outputRate); fprintf(infofp, "Output-channels: %d\n", outputChannels); fprintf(infofp, "Output-duration: %f\n", (float)outputSamples / outputRate); fprintf(infofp, "Output-samples: %d\n", outputSamples); fprintf(infofp, ":\n"); fprintf(infofp, "::: Data about the device that made the recording\n"); fprintf(infofp, "%s\n", artist); fprintf(infofp, ":\n"); fprintf(infofp, "::: Data about the recording itself\n"); fprintf(infofp, "%s\n", name); fprintf(infofp, ":\n"); fprintf(infofp, "::: Data about this file representation\n"); fprintf(infofp, "%s\n", comment); } signed short values[OMDATA_MAX_CHANNELS + 1]; int sample; for (sample = 0; sample < outputSamples; sample++) { OmConvertPlayerSeek(&player, sample); // Convert to integers int c; double temp = player.temp; char clipped = player.clipped; double accel[OMCALIBRATE_AXES]; for (c = 0; c < player.arrangement->numChannels; c++) { double interpVal = player.values[c]; double v = player.scale[c] * interpVal; // Apply calibration if (c < OMCALIBRATE_AXES) { // Rescaling is: v = (v + offset) * scale + (temp - referenceTemperature) * tempOffset v = (v + calibration.offset[c]) * calibration.scale[c] + (temp - calibration.referenceTemperature) * calibration.tempOffset[c]; } if (c < OMCALIBRATE_AXES) { accel[c] = v; } // Output range scaled double ov = v * outputAccelScale; // Saturate if (ov < -32768.0) { ov = -32768.0; clipped = 1; } if (ov > 32767.0) { ov = 32767.0; clipped = 1; } // Save values[c] = (signed short)(ov); } // Auxilliary channel uint16_t aux = 0; if (!player.valid) { aux |= WAV_AUX_UNAVAILABLE; } if (clipped) { aux |= WAV_AUX_CLIPPING; } int cycle = sample % (int)player.sampleRate; if (cycle == 0) { aux |= WAV_AUX_SENSOR_BATTERY | (player.aux[0] & 0x3ff); } if (cycle == 1) { aux |= WAV_AUX_SENSOR_LIGHT | (player.aux[1] & 0x3ff); } if (cycle == 2) { aux |= WAV_AUX_SENSOR_TEMPERATURE | (player.aux[2] & 0x3ff); } //player.ettings.auxChannel values[player.arrangement->numChannels] = aux; if (!CalcAddValue(calc, accel, 0.0, player.valid ? true : false)) { fprintf(stderr, "ERROR: Problem writing calculations.\n"); retVal = EXIT_IOERR; break; } #if 0 // TEMPORARY: Write SVM (before filtering) to fourth channel double outSvm = svm * 4096.0; if (outSvm < -32768.0) { outSvm = -32768.0; } if (outSvm > 32767.0) { outSvm = 32767.0; } values[player.arrangement->numChannels] = (signed short)outSvm; #endif // Output //for (c = 0; c < numChannels + 1; c++) { printf("%s%d", (c > 0) ? "," : "", values[c]); } //printf("\n"); if (ofp != NULL) { int bytesToWrite = sizeof(int16_t) * (arrangement.numChannels + 1); static unsigned char cache[1024 * 1024]; // TODO: Don't do this - not thread safe static int cachePosition = 0; // ...or this... memcpy(cache + cachePosition, values, bytesToWrite); cachePosition += bytesToWrite; if (cachePosition + bytesToWrite >= sizeof(cache) || sample + 1 >= outputSamples) { if (fwrite(cache, 1, cachePosition, ofp) != cachePosition) { fprintf(stderr, "ERROR: Problem writing output.\n"); retVal = EXIT_IOERR; break; } cachePosition = 0; fprintf(stderr, "."); } } } } if (ofp != NULL) { fclose(ofp); } CalcClose(calc); fprintf(stderr, "\n"); fprintf(stderr, "Finished.\n"); } OmDataFree(&omdata); if (sessionCount < 1) { fprintf(stderr, "ERROR: No sessions to write.\n"); retVal = EXIT_DATAERR; } if (infofp != NULL) { // Write other information to info file fprintf(infofp, ":\n"); fprintf(infofp, "::: Data about the final state\n"); fprintf(infofp, "Exit: %d\n", retVal); fclose(infofp); infofp = NULL; } return retVal; }
// Find calibration int OmCalibrateFindAutoCalibration(omcalibrate_config_t *config, omcalibrate_stationary_points_t *stationaryPoints, omcalibrate_calibration_t *calibration) { int i; int c; // Reset calibration OmCalibrateInit(calibration); // Calcuate mean temperature if (config->useTemp) { double tempSum = 0; for (i = 0; i < stationaryPoints->numValues; i++) { tempSum += stationaryPoints->values[i].actualTemperature; } calibration->referenceTemperature = (stationaryPoints->numValues > 0) ? (tempSum / stationaryPoints->numValues) : 0.0; } else { calibration->referenceTemperature = 0.0; } // Check number of stationary points if (stationaryPoints->numValues < 4) { const char *msg = "CALIBRATE: Fewer than 4 stationary points for calibration estimation.\n"; fprintf(stderr, msg); fprintf(stdout, msg); if (calibration->errorCode == 0) { calibration->errorCode = -1; } } else if (stationaryPoints->numValues < 10) { const char *msg = "CALIBRATE: Fewer than 10 stationary points for calibration estimation.\n"; fprintf(stderr, msg); fprintf(stdout, msg); } // Measure per-axis min/max range double axisMin[OMCALIBRATE_AXES] = { 0 }; double axisMax[OMCALIBRATE_AXES] = { 0 }; for (c = 0; c < OMCALIBRATE_AXES; c++) { for (i = 0; i < stationaryPoints->numValues; i++) { double v = stationaryPoints->values[i].mean[c]; if (i == 0 || v < axisMin[c]) { axisMin[c] = v; } if (i == 0 || v > axisMax[c]) { axisMax[c] = v; } } } // Check range on each axis calibration->numAxes = 0; for (c = 0; c < OMCALIBRATE_AXES; c++) { if (axisMin[c] <= -config->axisRange && axisMax[c] >= config->axisRange) { calibration->numAxes++; } } // Warn if not distributed on either side of each axis (+/- 0.3 g) if (calibration->numAxes <= 0) { const char *msg = "CALIBRATE: Unit sphere - no axis fits criterion.\n"; fprintf(stderr, msg); fprintf(stdout, msg); if (calibration->errorCode == 0) { calibration->errorCode = -2; } } else if (calibration->numAxes <= 1) { const char *msg = "CALIBRATE: Unit sphere - one axis fits criterion.\n"; fprintf(stderr, msg); fprintf(stdout, msg); if (calibration->errorCode == 0) { calibration->errorCode = -3; } } else if (calibration->numAxes <= 2) { const char *msg = "CALIBRATE: Unit sphere - two axes fulfill criterion.\n"; fprintf(stderr, msg); fprintf(stdout, msg); if (calibration->errorCode == 0) { calibration->errorCode = -4; } } // ---------- Auto-calibration ---------- int numPoints = stationaryPoints->numValues; double *temp = (double *)malloc(sizeof(double) * numPoints); double *weights = (double *)malloc(sizeof(double) * numPoints); double *values[OMCALIBRATE_AXES]; double *target[OMCALIBRATE_AXES]; for (c = 0; c < OMCALIBRATE_AXES; c++) { values[c] = (double *)malloc(sizeof(double) * numPoints); target[c] = (double *)malloc(sizeof(double) * numPoints); } // Initialize weights to 1 for (i = 0; i < numPoints; i++) { weights[i] = 1.0; } // Set the temperature relative to the reference temperature for (i = 0; i < numPoints; i++) { if (config->useTemp) { temp[i] = stationaryPoints->values[i].actualTemperature - calibration->referenceTemperature; } else { temp[i] = 0.0; } } // Main loop to estimate unit sphere int iter; for (iter = 0; iter < config->maxIter; iter++) { int c; // Scale input data with current parameters // model: (offset + D_in) * scale + T * tempOffset) // D = (repmat(offset,N,1) + D_in) .* repmat(scale,N,1) + repmat(temp,1,3) .* repmat(tempOffset,N,1); for (c = 0; c < OMCALIBRATE_AXES; c++) { for (i = 0; i < numPoints; i++) { values[c][i] = (calibration->offset[c] + stationaryPoints->values[i].mean[c]) * calibration->scale[c] + temp[i] * calibration->tempOffset[c]; } } // targets: points on unit sphere // target = D ./ repmat(sqrt(sum(D.^2,2)),1,size(D,2)); for (i = 0; i < numPoints; i++) { double sumSquares = 0; for (c = 0; c < OMCALIBRATE_AXES; c++) { sumSquares += values[c][i] * values[c][i]; } double vectorLength = sqrt(sumSquares); if (vectorLength == 0) { vectorLength = 1.0; } for (c = 0; c < OMCALIBRATE_AXES; c++) { target[c][i] = values[c][i] / vectorLength; } } // Initialise vars for optimisation double gradient[OMCALIBRATE_AXES], off[OMCALIBRATE_AXES], tOff[OMCALIBRATE_AXES]; // Do linear regression per input axis to estimate scale offset (and tempOffset) for (c = 0; c < OMCALIBRATE_AXES; c++) { gradient[c] = 1; off[c] = 0; tOff[c] = 0; double *coef; if (config->useTemp) { // mdl = LinearModel.fit([D(:,j) temp], target(:,j), 'linear', 'Weights', weights); //coef = LinearModelFitTwoIndependent(numPoints, target[c], values[c], temp); #ifdef ENABLE_GSL coef = LinearModelFitTwoIndependentWeighted(numPoints, target[c], values[c], temp, weights); #else coef = LinearModelFitTwoIndependent(numPoints, target[c], values[c], temp); //coef = LinearModelFitTwoIndependentWeightedApproximately(numPoints, target[c], values[c], temp, weights); #endif } else { // mdl = LinearModel.fit([D(:,j)], target(:,j), 'linear', 'Weights', weights); coef = LinearModelFitOneIndependent(numPoints, target[c], values[c]); } off[c] = coef[0]; // offset = intersect if (coef[1] != 0.0) { gradient[c] = coef[1]; } // scale = gradient if (config->useTemp) tOff[c] = coef[2]; // tempOffset = last coeff. else tOff[c] = 0; } // Change current parameters // sc = scale; double sc[OMCALIBRATE_AXES]; // previous values (saved for convergence comparison) for (c = 0; c < OMCALIBRATE_AXES; c++) { sc[c] = calibration->scale[c]; } // adapt offset: offset = offset + off . / (scale.*gradient); for (c = 0; c < OMCALIBRATE_AXES; c++) { double div = calibration->scale[c] * gradient[c]; if (div == 0.0) { div = 1; } calibration->offset[c] = calibration->offset[c] + off[c] / div; } // adapt scaling: scale = scale.*gradient; for (c = 0; c < OMCALIBRATE_AXES; c++) { calibration->scale[c] = calibration->scale[c] * gradient[c]; } // Apply temperature offset // if p.useTemp if (config->useTemp) { // tempOffset = tempOffset .* gradient + tOff; for (c = 0; c < OMCALIBRATE_AXES; c++) { calibration->tempOffset[c] = calibration->tempOffset[c] * gradient[c] + tOff[c]; } } // Update weightings for linear regression (ignores outliers, overall limited to a maximum of 100) // weights = min([1 ./ sqrt(sum((D-target).^2,2)), repmat(100,N,1)],[],2); for (i = 0; i < numPoints; i++) { double rowSum = 0; for (c = 0; c < OMCALIBRATE_AXES; c++) { double v = values[c][i] - target[c][i]; // (D-target) rowSum += (v * v); // sum((D-target).^2) } double vlen = sqrt(rowSum); double vv = vlen != 0 ? 1 / vlen : 0; if (vv > 100) { vv = 100; } weights[i] = vv; } // no more scaling change -> assume it has converged double cE = 0; for (c = 0; c < OMCALIBRATE_AXES; c++) { cE += fabs(calibration->scale[c] - sc[c]); } bool converged = cE < config->convCrit; // RMS error to unit sphere // E = sqrt(mean(sum((D - target). ^ 2, 2))); double colSum = 0; for (i = 0; i < numPoints; i++) { double rowSum = 0; for (c = 0; c < OMCALIBRATE_AXES; c++) { double v = values[c][i] - target[c][i]; // (D-target) rowSum += (v * v); // sum((D-target).^2) } colSum += rowSum; } double meanSum = numPoints > 0 ? colSum / numPoints : 0; double E = sqrt(meanSum); // Debug progress if (iter == 0 || iter >= config->maxIter - 1 || converged) { fprintf(stderr, "CALIBRATE: Iteration %d, error: %.4f, convergence: %.6f %s\n", iter + 1, E, cE, converged ? "CONVERGED" : ""); } // Check for convergence if (converged) { break; } // Warning if no convergence if (!converged && iter + 1 >= config->maxIter) { const char *msg = "WARNING: Maximum number of iterations reached without convergence.\n"; fprintf(stderr, msg); fprintf(stdout, msg); } } // Free resources free(temp); free(weights); for (c = 0; c < OMCALIBRATE_AXES; c++) { free(values[c]); free(target[c]); } // Sanity check the calibration char axisFailed = 0; for (c = 0; c < OMCALIBRATE_AXES; c++) { double scaleDiff = fabs(calibration->scale[c] - 1.0); double offsetDiff = calibration->offset[c]; double tempOffsetDiff = calibration->tempOffset[c]; if ((config->maximumScaleDiff > 0 && scaleDiff > config->maximumScaleDiff) || (config->maximumOffsetDiff > 0 && offsetDiff > config->maximumOffsetDiff) || (config->maximumTempOffsetDiff > 0 && tempOffsetDiff > config->maximumTempOffsetDiff)) { axisFailed++; } } if (axisFailed > 0) { char msg[128]; sprintf(msg, "CALIBRATE: Range check on %d axes unmet.\n", axisFailed); fprintf(stderr, msg); fprintf(stdout, msg); if (calibration->errorCode == 0) calibration->errorCode = -5; } return calibration->errorCode; }