// (Internal) Find stationary points (using either a player or direct data)
static omcalibrate_stationary_points_t *OmCalibrateFindStationaryPoints(omcalibrate_config_t *config, omdata_t *data, om_convert_player_t *player)
{
double sampleRate, startTime;
double firstSampleTime = 0;
int lastWindow = -1;
int numSamples;
int samplesInPreviousSegments = 0;
omdata_segment_t *dataSegment = NULL;
if (player != NULL)
{
sampleRate = player->sampleRate;
startTime = player->arrangement->startTime;
numSamples = player->numSamples;
}
else if (data != NULL)
{
omdata_stream_t *stream = &data->stream['a'];
if (!stream->inUse)
{
fprintf(stderr, "ERROR: Calibration failed as accelerometer stream not found.\n");
return NULL;
}
dataSegment = stream->segmentFirst;
sampleRate = dataSegment->sampleRate;
startTime = dataSegment->startTime;
// Determine total number of samples
numSamples = 0;
omdata_segment_t *seg;
for (seg = dataSegment; seg != NULL; seg = seg->segmentNext)
{
numSamples += seg->numSamples;
}
}
else
{
return NULL;
}
omcalibrate_stationary_points_t *stationaryPoints = (omcalibrate_stationary_points_t *)malloc(sizeof(omcalibrate_stationary_points_t));
memset(stationaryPoints, 0, sizeof(omcalibrate_stationary_points_t));
// For 'ignore repeated' option
double initialMean[OMCALIBRATE_AXES] = { 0 };
double initialTemp = 0.0;
int consecutive = 0;
// Trackers
#ifdef ALT_SD
// See Knuth TAOCP vol 2, 3rd edition, page 232
double oldM[OMCALIBRATE_AXES] = { 0 };
double newM[OMCALIBRATE_AXES] = { 0 };
double oldS[OMCALIBRATE_AXES] = { 0 };
double newS[OMCALIBRATE_AXES] = { 0 };
int n[OMCALIBRATE_AXES] = { 0 };
#else
double axisSum[OMCALIBRATE_AXES] = { 0 };
double axisSumSquared[OMCALIBRATE_AXES] = { 0 };
#endif
double tempSum = 0;
int sampleCount = 0;
// Time interpolation for direct data
int lastSectorIndex = -1;
int nextTimestampSample = -1, lastTimestampSample = -1;
double nextTimestampValue = 0, lastTimestampValue = 0;
int sample;
for (sample = 0; sample < numSamples; sample++)
{
int c;
double values[OMCALIBRATE_AXES];
double temp;
double currentTime;
bool windowFilled = false;
if (player != NULL)
{
OmConvertPlayerSeek(player, sample);
if (!player->valid) { sampleCount = 0; continue; }
// Convert to units
for (c = 0; c < OMCALIBRATE_AXES; c++) // player.arrangement->numChannels
{
values[c] = player->scale[c] * player->values[c];
}
temp = player->temp;
currentTime = ((double)sample / sampleRate) + startTime;
// Have we filled a window?
int numStationary = (int)(config->stationaryTime * sampleRate + 0.5);
if (sampleCount >= numStationary) { windowFilled = true; }
}
else if (data != NULL)
{
int sampleWithinSegment = sample - samplesInPreviousSegments;
// Check we have data
if (dataSegment == NULL)
{
fprintf(stderr, "WARNING: Less data than was expected.\n");
break;
}
int sectorWithinSegment = sampleWithinSegment / dataSegment->samplesPerSector;
if (sectorWithinSegment >= dataSegment->sectorCount)
{
fprintf(stderr, "WARNING: Invalid sector within segment.\n");
break;
}
// Advance to next segment?
if (sampleWithinSegment >= dataSegment->numSamples)
{
dataSegment = dataSegment->segmentNext;
samplesInPreviousSegments = sample;
sampleWithinSegment = 0;
// Update rates/times
if (dataSegment != NULL)
{
sampleRate = dataSegment->sampleRate;
startTime = dataSegment->startTime;
}
// Reset accumulator on segment change (break in sample stream)
sampleCount = 0;
firstSampleTime = 0; // Trigger time reset
lastSectorIndex = -1; // Force no time interpolation
nextTimestampSample = -1; // Force restart of time interpolation
lastWindow = -1;
continue;
}
// Sector index
int sectorIndex = dataSegment->sectorIndex[sectorWithinSegment];
#if 1
if (sectorIndex != lastSectorIndex)
{
lastSectorIndex = sectorIndex;
// Get current sector time
int sampleIndexOffset = 0;
double newTimestampValue = OmDataTimestampForSector(data, sectorIndex, &sampleIndexOffset);
int newTimestampSample = sample + sampleIndexOffset;
// Replace 'next' timestamp if different
if (newTimestampSample != nextTimestampSample)
{
if (nextTimestampSample < 0)
{
nextTimestampSample = newTimestampSample;
nextTimestampValue = newTimestampValue;
}
lastTimestampSample = nextTimestampSample;
lastTimestampValue = nextTimestampValue;
nextTimestampSample = newTimestampSample;
nextTimestampValue = newTimestampValue;
}
}
// If we only have one timestamp to estimate from
int elapsedSamples = sample - lastTimestampSample + 1;
if (nextTimestampSample <= lastTimestampSample)
{
currentTime = lastTimestampValue + (elapsedSamples / sampleRate);
}
else
{
currentTime = lastTimestampValue + (elapsedSamples * (nextTimestampValue - lastTimestampValue) / (nextTimestampSample - lastTimestampSample));
}
#else
// HACK: This is a bogus time as the sample rate is not fixed
currentTime = ((double)sampleWithinSegment / sampleRate) + startTime;
#endif
#if 0
// Debug out of time delta
static double lastTime = -1;
if (lastTime < 0) { lastTime = currentTime; }
double deltaTime = currentTime - lastTime;
printf("%f\n", deltaTime);
lastTime = currentTime;
#endif
// Get samples
int16_t intvalues[OMCALIBRATE_AXES];
OmDataGetValues(data, dataSegment, sampleWithinSegment, intvalues);
// Get temperature
temp = 0;
if (dataSegment->offset == 30)
{
const unsigned char *p = (const unsigned char *)data->buffer + (OMDATA_SECTOR_SIZE * sectorIndex);
int16_t inttemp = p[20] | ((int16_t)p[21] << 8); // @20 WORD Temperature
// Convert
temp = ((int)inttemp * 150 - 20500) / 1000.0;
//temp = (double)inttemp * 75 / 256.0 - 50;
}
// Scale values
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
values[c] = intvalues[c] * dataSegment->scaling;
}
// Check whether a window is filled
if (firstSampleTime <= 0) { firstSampleTime = currentTime; lastWindow = -1; }
int currentWindow = (int)((currentTime - firstSampleTime) / config->stationaryTime);
if (lastWindow < 0) { lastWindow = currentWindow; }
if (currentWindow != lastWindow)
{
windowFilled = true;
lastWindow = currentWindow;
}
}
else
{
break;
}
// Filled window
if (windowFilled)
{
bool stationary = true;
if (sampleCount <= 0) { sampleCount = 1; stationary = false; }
// Check whether stationary
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
//double count = n[c];
//double mean = (n[c] > 0) ? newM[c] : 0.0;
double variance = (n[c] > 1) ? newS[c] / (n[c] - 1) : 0.0;
double standardDeviation = sqrt(variance);
#else
double mean = axisSum[c] / sampleCount;
double squareOfMean = mean * mean;
double averageOfSquares = (axisSumSquared[c] / sampleCount);
double standardDeviation = sqrt(averageOfSquares - squareOfMean);
//double standardDeviation = sqrt(sampleCount * axisSumSquared[c] - (axisSum[c] * axisSum[c])) / sampleCount;
#endif
if (standardDeviation > config->stationaryMaxDeviation)
{
stationary = false;
}
}
// Calculate mean values
double time = currentTime - ((((double)sampleCount / 2)) / sampleRate);
double mean[OMCALIBRATE_AXES];
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
mean[c] = (n[c] > 0) ? newM[c] : 0.0;
#else
mean[c] = axisSum[c] / sampleCount;
#endif
}
// Check if this is a repeat
if (stationary && config->stationaryRepeated)
{
if (consecutive == 0)
{
// Update 'initial' values
for (c = 0; c < OMCALIBRATE_AXES; c++) { initialMean[c] = mean[c]; }
initialTemp = temp;
consecutive = 1;
}
else
{
// Compare 'initial' values
double diff = 0;
for (c = 0; c < OMCALIBRATE_AXES; c++) { diff += (initialMean[c] - mean[c]) * (initialMean[c] - mean[c]); }
diff = sqrt(diff);
if (diff < config->stationaryRepeatedAccel && fabs(initialTemp - temp) <= config->stationaryRepeatedTemp)
{
consecutive++;
stationary = false;
}
else
{
consecutive = 0;
}
}
}
else
{
consecutive = 0;
}
#if 0
// Debug out of time delta
static int windowNumber = 0;
static double lastTime = -1;
if (lastTime < 0) { lastTime = currentTime; }
double deltaTime = currentTime - lastTime;
if (stationary)
printf("WINDOW: %d, from=%.3f, to=%.3f, period=%.3f\n", windowNumber++, lastTime, currentTime, deltaTime);
lastTime = currentTime;
windowNumber++;
#endif
// Create new point
if (stationary)
{
omcalibrate_point_t point;
point.time = time;
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
point.mean[c] = mean[c];
}
point.actualTemperature = tempSum / sampleCount;
//point.temperature = 0;
// Check whether we have to grow the buffer
if (stationaryPoints->numValues >= stationaryPoints->capacity)
{
stationaryPoints->capacity = (15 * stationaryPoints->capacity / 10) + 1;
stationaryPoints->values = (omcalibrate_point_t *)realloc(stationaryPoints->values, stationaryPoints->capacity * sizeof(omcalibrate_point_t));
}
stationaryPoints->values[stationaryPoints->numValues] = point;
stationaryPoints->numValues++;
}
// Trigger reset of accumulators
sampleCount = 0;
}
// If the accumulators need to be reset...
if (sampleCount == 0)
{
if (firstSampleTime <= 0) { firstSampleTime = currentTime; }
// Clear accumulators
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
oldM[c] = newM[c] = oldS[c] = newS[c] = 0;
n[c] = 0;
#else
axisSum[c] = 0;
axisSumSquared[c] = 0;
#endif
}
tempSum = 0;
}
sampleCount++;
// Accumulate
for (c = 0; c < OMCALIBRATE_AXES; c++) // player.arrangement->numChannels
{
double x = values[c];
#ifdef ALT_SD
n[c]++;
if (n[c] == 1)
{
oldM[c] = newM[c] = x;
oldS[c] = 0.0;
}
else
{
newM[c] = oldM[c] + (x - oldM[c]) / n[c];
newS[c] = oldS[c] + (x - oldM[c]) * (x - newM[c]);
oldM[c] = newM[c];
oldS[c] = newS[c];
}
#else
axisSum[c] += x;
axisSumSquared[c] += x * x;
#endif
}
tempSum += temp;
}
return stationaryPoints;
}
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 stationary points
omcalibrate_stationary_points_t *OmCalibrateFindStationaryPoints(omcalibrate_config_t *config, om_convert_player_t *player)
{
omcalibrate_stationary_points_t *stationaryPoints = (omcalibrate_stationary_points_t *)malloc(sizeof(omcalibrate_stationary_points_t));
memset(stationaryPoints, 0, sizeof(omcalibrate_stationary_points_t));
// For 'ignore repeated' option
double initialMean[OMCALIBRATE_AXES] = { 0 };
double initialTemp = 0.0;
int consecutive = 0;
// Trackers
#ifdef ALT_SD
// See Knuth TAOCP vol 2, 3rd edition, page 232
double oldM[OMCALIBRATE_AXES] = { 0 };
double newM[OMCALIBRATE_AXES] = { 0 };
double oldS[OMCALIBRATE_AXES] = { 0 };
double newS[OMCALIBRATE_AXES] = { 0 };
int n[OMCALIBRATE_AXES] = { 0 };
#else
double axisSum[OMCALIBRATE_AXES] = { 0 };
double axisSumSquared[OMCALIBRATE_AXES] = { 0 };
#endif
double tempSum = 0;
int numStationary = (int)(config->stationaryTime * player->sampleRate + 0.5);
int sampleCount = 0;
int sample;
for (sample = 0; sample < player->numSamples; sample++)
{
int c;
OmConvertPlayerSeek(player, sample);
if (!player->valid) { sampleCount = 0; continue; }
// If the accumulators need to be reset...
if (sampleCount == 0)
{
// Clear accumulators
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
oldM[c] = newM[c] = oldS[c] = newS[c] = 0;
n[c] = 0;
#else
axisSum[c] = 0;
axisSumSquared[c] = 0;
#endif
}
tempSum = 0;
}
sampleCount++;
// Convert to units
double values[OMCALIBRATE_AXES];
for (c = 0; c < OMCALIBRATE_AXES; c++) // player.arrangement->numChannels
{
values[c] = player->scale[c] * player->values[c];
}
double temp = player->temp;
// Accumulate
for (c = 0; c < OMCALIBRATE_AXES; c++) // player.arrangement->numChannels
{
double x = values[c];
#ifdef ALT_SD
n[c]++;
if (n[c] == 1)
{
oldM[c] = newM[c] = x;
oldS[c] = 0.0;
}
else
{
newM[c] = oldM[c] + (x - oldM[c]) / n[c];
newS[c] = oldS[c] + (x - oldM[c]) * (x - newM[c]);
oldM[c] = newM[c];
oldS[c] = newS[c];
}
#else
axisSum[c] += x;
axisSumSquared[c] += x * x;
#endif
}
tempSum += temp;
// Filled window
if (sampleCount >= numStationary)
{
bool stationary = true;
// Check whether stationary
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
//double count = n[c];
//double mean = (n[c] > 0) ? newM[c] : 0.0;
double variance = (n[c] > 1) ? newS[c] / (n[c] - 1) : 0.0;
double standardDeviation = sqrt(variance);
#else
double mean = axisSum[c] / numStationary;
double squareOfMean = mean * mean;
double averageOfSquares = (axisSumSquared[c] / numStationary);
double standardDeviation = sqrt(averageOfSquares - squareOfMean);
//double standardDeviation = sqrt(numStationary * axisSumSquared[c] - (axisSum[c] * axisSum[c])) / numStationary;
#endif
if (standardDeviation > config->stationaryMaxDeviation)
{
stationary = false;
}
}
// Calculate mean values
double time = ((sample - (numStationary / 2)) / player->sampleRate) + player->arrangement->startTime;
double mean[OMCALIBRATE_AXES];
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
#ifdef ALT_SD
mean[c] = (n[c] > 0) ? newM[c] : 0.0;
#else
mean[c] = axisSum[c] / numStationary;
#endif
}
// Check if this is a repeat
if (stationary && config->stationaryRepeated)
{
if (consecutive == 0)
{
// Update 'initial' values
for (c = 0; c < OMCALIBRATE_AXES; c++) { initialMean[c] = mean[c]; }
initialTemp = temp;
consecutive = 1;
}
else
{
// Compare 'initial' values
double diff = 0;
for (c = 0; c < OMCALIBRATE_AXES; c++) { diff += (initialMean[c] - mean[c]) * (initialMean[c] - mean[c]); }
diff = sqrt(diff);
if (diff < config->stationaryRepeatedAccel && fabs(initialTemp - temp) <= config->stationaryRepeatedTemp)
{
consecutive++;
stationary = false;
}
else
{
consecutive = 0;
}
}
}
else
{
consecutive = 0;
}
// Create new point
if (stationary)
{
omcalibrate_point_t point;
point.time = time;
for (c = 0; c < OMCALIBRATE_AXES; c++)
{
point.mean[c] = mean[c];
}
point.actualTemperature = tempSum / numStationary;
//point.temperature = 0;
// Check whether we have to grow the buffer
if (stationaryPoints->numValues >= stationaryPoints->capacity)
{
stationaryPoints->capacity = (15 * stationaryPoints->capacity / 10) + 1;
stationaryPoints->values = (omcalibrate_point_t *)realloc(stationaryPoints->values, stationaryPoints->capacity * sizeof(omcalibrate_point_t));
}
stationaryPoints->values[stationaryPoints->numValues] = point;
stationaryPoints->numValues++;
}
// Trigger reset of accumulators
sampleCount = 0;
}
}
return stationaryPoints;
}
