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