Snap for 4716599 from 1b3adf02ce3c7d184c77a764b6b56741ede784ea to pi-release

Change-Id: I23b9438a24c9b7f5a974cd2bf39e5c4b3391e171

4 files changed, 0 insertions, 929 deletions

diff --git a/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.c b/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.c
deleted file mode 100644
index 29a7ba07..00000000
--- a/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.c
+++ /dev/null
@@ -1,271 +0,0 @@
-#include "calibration/magnetometer/mag_gyro_cal.h"
-
-#include <string.h>
-
-#include "calibration/util/cal_log.h"
-#include "common/math/macros.h"
-
-#define SINGULAR_TOL (1e-4f)
-
-static bool allDiagonalsLessThan(struct Mat33 *matrix, float threshold) {
-  return matrix->elem[0][0] < threshold && matrix->elem[1][1] < threshold &&
-         matrix->elem[2][2] < threshold;
-}
-
-static void calculatingAndSettingBias(struct MagGyroCal *mgc,
-                                      struct Mat33 *first_sum_inverse,
-                                      struct Vec3 *bias) {
-  // Calculate bias, since we have high trust into this estimates.
-  mat33Apply(bias, first_sum_inverse, &mgc->mgac.second_sum_average);
-
-  // Setting bias update.
-  mgc->x_bias = bias->x;
-  mgc->y_bias = bias->y;
-  mgc->z_bias = bias->z;
-}
-
-void magGyroCalBatchReset(struct MagGyroBatchCal *mgbc) {
-  mgbc->first_sample = true;
-  mgbc->start_time_ns = 0;
-  mgbc->timestamp_buffer_ns = 0;
-  initZeroMatrix(&mgbc->first_sum_update);
-  initVec3(&mgbc->last_mag_sample, 0.0f, 0.0f, 0.0f);
-  initVec3(&mgbc->second_sum_update, 0.0f, 0.0f, 0.0f);
-}
-
-static void magGyroCalAverageReset(struct MagGyroAverageCal *mgac) {
-  initZeroMatrix(&mgac->first_sum_average);
-  initVec3(&mgac->second_sum_average, 0.0f, 0.0f, 0.0f);
-  initVec3(&mgac->confidence_metric, 0.0f, 0.0f, 0.0f);
-  mgac->medium_accuracy_bias_update = false;
-  initVec3(&mgac->bias_running, 0.0f, 0.0f, 0.0f);
-}
-
-void magGyroCalReset(struct MagGyroCal *mgc) {
-  // Reset Batch Cal.
-  magGyroCalBatchReset(&mgc->mgbc);
-
-  // Reset Avarge Cal.
-  magGyroCalAverageReset(&mgc->mgac);
-
-  initVec3(&mgc->mgac.confidence_metric,
-           mgc->algo_parameters.confidence_threshold_start,
-           mgc->algo_parameters.confidence_threshold_start,
-           mgc->algo_parameters.confidence_threshold_start);
-}
-
-void magGyroCalGetBias(struct MagGyroCal *mgc, float *x, float *y, float *z) {
-  *x = mgc->x_bias;
-  *y = mgc->y_bias;
-  *z = mgc->z_bias;
-}
-
-void magGyroCalInit(struct MagGyroCal *mgc,
-                    const struct MagGyroCalParameters *params) {
-  // Tuning parameters.
-  memcpy(&mgc->algo_parameters, params, sizeof(mgc->algo_parameters));
-
-  // Bias Updates set to zero.
-  mgc->x_bias = 0.0f;
-  mgc->y_bias = 0.0f;
-  mgc->z_bias = 0.0f;
-
-  // Reseting Algo.
-  magGyroCalReset(mgc);
-}
-
-void magGyroCalBatchUpdate(struct MagGyroBatchCal *mgbc,
-                           uint64_t sample_time_ns, float mag_x_ut,
-                           float mag_y_ut, float mag_z_ut, float gyro_x_rps,
-                           float gyro_y_rps, float gyro_z_rps) {
-  // local variables, only needed for math on single samples.
-  struct Vec3 mag_data;
-  struct Vec3 gyro_data;
-  struct Vec3 mag_data_dot;
-  struct Vec3 cross_mag_gyro;
-  struct Vec3 second_sum;
-  struct Vec3 yi;
-  struct Mat33 first_sum;
-  struct Mat33 w;
-  struct Mat33 w_trans;
-  float odr_hz_int;
-  // Creating gyro and mag vector.
-  initVec3(&mag_data, mag_x_ut, mag_y_ut, mag_z_ut);
-  initVec3(&gyro_data, gyro_x_rps, gyro_y_rps, gyro_z_rps);
-
-  // Saving first sample in a batch and skipping the rest, this is done in order
-  // to do the discrete time derivative. x_dot = x(n) - x(n-1).
-  if (mgbc->first_sample) {
-    initVec3(&mgbc->last_mag_sample, mag_x_ut, mag_y_ut, mag_z_ut);
-    mgbc->first_sample = false;
-    mgbc->start_time_ns = sample_time_ns;
-    mgbc->timestamp_buffer_ns = sample_time_ns;
-    return;
-  }
-  // checking if delta time can become zero or negative.
-  if (sample_time_ns <= mgbc->timestamp_buffer_ns) {
-    // Timestamp is not monotonic, resetting batch.
-    magGyroCalBatchReset(mgbc);
-    return;
-  }
-
-  // Calculating time delta between this sample and the sample before.
-  float delta_time_ns = (float)(sample_time_ns - mgbc->timestamp_buffer_ns);
-
-  // Calculating 1/deltaT = ODR [Hz], for time derivative.
-  odr_hz_int = 1 / (delta_time_ns * NANOS_TO_SEC);
-  mgbc->timestamp_buffer_ns = sample_time_ns;
-
-  // creating mag_dot.
-  // mag_data_dot =  (mag_data - last_mag_sample) * odr.
-  vec3SubVecs(&mag_data_dot, &mag_data, &mgbc->last_mag_sample);
-  vec3ScalarMul(&mag_data_dot, odr_hz_int);
-
-  // Cross product (gyro x mag).
-  vec3Cross(&cross_mag_gyro, &gyro_data, &mag_data);
-
-  // Saving data into memory, for next derivative.
-  initVec3(&mgbc->last_mag_sample, mag_x_ut, mag_y_ut, mag_z_ut);
-
-  // calculating yi = mag_dot + (gyro x mag).
-  vec3AddVecs(&yi, &mag_data_dot, &cross_mag_gyro);
-
-  // creating skew-symmetric matrix.
-  //     /   0, -w3,  w2 \.
-  // w = |  w3,   0, -w1 |.
-  //     \ -w2,  w1,  0  /.
-  w.elem[0][0] = 0;
-  w.elem[0][1] = -gyro_z_rps;
-  w.elem[0][2] = gyro_y_rps;
-  w.elem[1][0] = gyro_z_rps;
-  w.elem[1][1] = 0;
-  w.elem[1][2] = -gyro_x_rps;
-  w.elem[2][0] = -gyro_y_rps;
-  w.elem[2][1] = gyro_x_rps;
-  w.elem[2][2] = 0;
-
-  // calculating second_sum:
-  // second_sum = w^T * yi.
-  mat33Transpose(&w_trans, &w);
-  mat33Apply(&second_sum, &w_trans, &yi);
-
-  // calculating first sum:
-  // first_sum = w^T * w.
-  mat33MultiplyTransposed(&first_sum, &w, &w);
-
-  // Updating sums.
-  mat33Add(&mgbc->first_sum_update, &first_sum);
-  vec3Add(&mgbc->second_sum_update, &second_sum);
-}
-
-enum MagUpdate magGyroCalBiasUpdate(struct MagGyroCal *mgc,
-                                    struct Mat33 *first_sum_inverse) {
-  struct Vec3 bias;
-  // Testing for medium accuracy.
-  if (allDiagonalsLessThan(first_sum_inverse,
-                           mgc->algo_parameters.confidence_threshold_medium)) {
-    // Testing for high accuracy, will leave the function here if true.
-    if (allDiagonalsLessThan(first_sum_inverse,
-                             mgc->algo_parameters.confidence_threshold_high)) {
-      // Calculate bias, since we have high trust into this estimates.
-      calculatingAndSettingBias(mgc, first_sum_inverse, &bias);
-
-      // Resetting everything since we have found the bias.
-      magGyroCalReset(mgc);
-
-      // Return high accuracy indicator.
-      return UPDATE_BIAS_MAGGYRO_HIGH;
-    }
-    // Only will reach this point if accuracy is between high and medium, now
-    // we test for if we already have a medium update.
-    if (mgc->mgac.medium_accuracy_bias_update == false) {
-      // Calculate bias, since we have high trust into the estimates.
-      calculatingAndSettingBias(mgc, first_sum_inverse, &bias);
-
-      // Resetting Batch, since we are not done yet.
-      magGyroCalBatchReset(&mgc->mgbc);
-
-      // Setting medium_accuracy_bias_update to "true".
-      mgc->mgac.medium_accuracy_bias_update = true;
-
-      // Return medium accuracy indicator.
-      return UPDATE_BIAS_MAGGYRO_MEDIUM;
-
-      // already have an medium update, still calculate the actual offset, can
-      // be used for log files and manual bias extractions.
-    } else {
-      // Calculate bias, since we have high trust into the estimates.
-      mat33Apply(&bias, first_sum_inverse, &mgc->mgac.second_sum_average);
-
-      // Setting bias update.
-      mgc->mgac.bias_running = bias;
-
-      // Resetting Batch, since we are not done yet.
-      magGyroCalBatchReset(&mgc->mgbc);
-
-      // Return no update indicator.
-      return NO_UPDATE;
-    }
-  }
-  // Have not gotten to medium accuracy, reset the batch.
-  magGyroCalBatchReset(&mgc->mgbc);
-
-  // Return no update indicator.
-  return NO_UPDATE;
-}
-
-enum MagUpdate magGyroCalRun(struct MagGyroCal *mgc, uint64_t sample_time_ns,
-                             float mag_x_ut, float mag_y_ut, float mag_z_ut,
-                             float gyro_x_rps, float gyro_y_rps,
-                             float gyro_z_rps) {
-  struct Mat33 first_sum_inverse;
-  // Running samples into the batch function.
-  magGyroCalBatchUpdate(&mgc->mgbc, sample_time_ns, mag_x_ut, mag_y_ut,
-                        mag_z_ut, gyro_x_rps, gyro_y_rps, gyro_z_rps);
-
-  // Timeout, reset and start over, return MAGGYRO_TIMEOUT.
-  if ((sample_time_ns - mgc->mgbc.start_time_ns) >=
-      mgc->algo_parameters.max_run_time_in_ns) {
-    magGyroCalReset(mgc);
-    return MAGGYRO_TIMEOUT;
-  }
-
-  // Early return if batch not complete.
-  if ((sample_time_ns - mgc->mgbc.start_time_ns) <=
-      mgc->algo_parameters.batch_window_in_ns) {
-    return NO_UPDATE;
-  }
-
-  // Updating the average matrix.
-  mat33Add(&mgc->mgac.first_sum_average, &mgc->mgbc.first_sum_update);
-
-  // Testing for singular matrix.
-  if (fabsf(mat33Determinant(&mgc->mgac.first_sum_average)) > SINGULAR_TOL) {
-    // Inverse of the average system matrix.
-    mat33Invert(&first_sum_inverse, &mgc->mgac.first_sum_average);
-
-    // Testing if the confidence metric has been improved.
-    if (first_sum_inverse.elem[0][0] < mgc->mgac.confidence_metric.x &&
-        first_sum_inverse.elem[1][1] < mgc->mgac.confidence_metric.y &&
-        first_sum_inverse.elem[2][2] < mgc->mgac.confidence_metric.z) {
-      // Updating the second_sum vector.
-      vec3Add(&mgc->mgac.second_sum_average, &mgc->mgbc.second_sum_update);
-
-      // Updating the confidence metric.
-      initVec3(&mgc->mgac.confidence_metric, first_sum_inverse.elem[0][0],
-               first_sum_inverse.elem[1][1], first_sum_inverse.elem[2][2]);
-
-      // Testing for Bias update and leaving the function.
-      return magGyroCalBiasUpdate(mgc, &first_sum_inverse);
-    }
-    // No improvement, removing the last batch from the average.
-    mat33Sub(&mgc->mgac.first_sum_average, &mgc->mgbc.first_sum_update);
-
-    // Resetting Batch, since we are not done yet.
-    magGyroCalBatchReset(&mgc->mgbc);
-
-    // Returning no bias update.
-    return NO_UPDATE;
-  }
-  return NO_UPDATE;
-}
diff --git a/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.h b/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.h
deleted file mode 100644
index edf92e88..00000000
--- a/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.h
+++ /dev/null
@@ -1,188 +0,0 @@
-/*
- * This module contains the algorithms for producing a magnetometer offset
- * calibration. The algorithm is based on the paper “Adaptive Estimation of
- * Measurement Bias in Three-Dimensional Field Sensors with Angular-Rate
- * Sensors: Theory and Comparative Experimental Evaluation” by G. Troni and L.
- * Whitcomb and uses gyroscope data as well as magnetometer data. We use a batch
- * method, resulting in a Linear Least-Squares Fit (Section III B of the paper).
- * We define the following mathematical setup:
- *
- * wi (wx,wy,wx) -> are the gyroscope readings [RAD/s]
- * xi            -> are the magnetometer readings [uT]
- * xi_dot        -> is the derivative of xi
- *
- * Form here we can define:
- *
- *       yi = xi_dot + (wi x xi) (x denotes the cross product)
- *
- * and
- *
- *           /  0   -wz   wy \
- *       Wi =|  wz   0    -wx |
- *           \ -wy  wx     0  /
- *
- * which we can use to define, what we call first sum and second sum
- *
- *       firstSum = sum( Wi^2)
- *
- * and
- *
- *       secondSum = sum(Wi yi)
- *
- * The bias is calculated the following way
- *
- *       b = inverse(firstSum) * secondSum
- *
- * Notice that we use the diagonal of the matrix “inverse(firstSum)” as a
- * confidence metric, to make a accuracy assessment.
- *
- * The algorithm expects time synchronized magnetometer and gyro data, any delay
- * shift between those two greater than 5-10ms will result in poor accuracy.
- * Furthermore, the minimum sample rate of the sensors should be higher than 50
- * Hz.
- */
-
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_GYRO_CAL_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_GYRO_CAL_H_
-
-#ifdef SPHERE_FIT_ENABLED
-#ifndef DIVERSITY_CHECK_ENABLED
-#define DIVERSITY_CHECK_ENABLED
-#endif
-#endif
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <sys/types.h>
-
-#include "calibration/magnetometer/mag_cal.h"
-#include "common/math/mat.h"
-#include "common/math/vec.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// Algo Batch Data Struct.
-struct MagGyroBatchCal {
-  // Batch Start time.
-  uint64_t start_time_ns;
-
-  // Time Stamp Buffer. Variable used to calculate 1/deltaT = odr_in_hz, where
-  // 1/deltaT is used to calculate the derivative of the mag signal.
-  uint64_t timestamp_buffer_ns;
-
-  // First sample in batch indicator.
-  bool first_sample;
-
-  // Matrix and vector definition.
-  struct Mat33 first_sum_update;
-  struct Vec3 last_mag_sample;
-  struct Vec3 second_sum_update;
-};
-
-// Algorithm tuning Parameter struct.
-struct MagGyroCalParameters {
-  // Batch window size.
-  uint64_t batch_window_in_ns;
-
-  // Algo resets after max_run_time.
-  uint64_t max_run_time_in_ns;
-
-  // Confidence Threshold.
-  // The confidence metric is explained in go/magcalp design document.
-  // Furthermore more details are given as well on how to tune it. Confidence
-  // Threshold high accuracy.
-  float confidence_threshold_high;
-
-  // Confidence Threshold medium accuracy.
-  float confidence_threshold_medium;
-
-  // Confidence Threshold start. Don't want to start with a bad matrix.
-  float confidence_threshold_start;
-};
-
-// Data Struct for the Batch average function.
-struct MagGyroAverageCal {
-  // Matrix and vector definition.
-  struct Mat33 first_sum_average;
-  struct Vec3 second_sum_average;
-
-  // Medium accuracy bias update.
-  bool medium_accuracy_bias_update;
-
-  // Confidence Metric.
-  struct Vec3 confidence_metric;
-
-  // Running biases update.
-  struct Vec3 bias_running;
-};
-
-// Main algo data Struct.
-struct MagGyroCal {
-  // Algorithm tuning parameters.
-  struct MagGyroCalParameters algo_parameters;
-
-  // Single batch struct. We use batches, which in turn get again averaged over
-  // time. The single batch is the amount of data that we do the Linear
-  // Least-Squares fit on and decide if we will add this to the averaging based
-  // on the confidence metric. Notice that we do on every batch a matrix
-  // inversion, hence in order to save power it is beneficial to make the batch
-  // size reasonably large. This is controlled by the “batch_window_in_ns”.
-  struct MagGyroBatchCal mgbc;
-
-  // Algo average struct.
-  struct MagGyroAverageCal mgac;
-
-  // New bias update.
-  float x_bias;
-  float y_bias;
-  float z_bias;
-};
-
-// Algorithm initialization.
-void magGyroCalInit(struct MagGyroCal *mgc,
-                    const struct MagGyroCalParameters *params);
-
-// Resets the batch struct.
-void magGyroCalReset(struct MagGyroCal *mgc);
-
-// Gets the current MagGyro bias estimate.
-void magGyroCalGetBias(struct MagGyroCal *mgc, float *x, float *y, float *z);
-
-// Main algorithm runner function.
-// sample_time_ns in [ns].
-// mag_x_ut, mag_y_ut, mag_z_ut are in [uT].
-// gyro_x_rps, gyro_y_rps, gyro_z_rps are in [RAD/s].
-// odr_hz is in [Hz].
-enum MagUpdate magGyroCalRun(struct MagGyroCal *mgc, uint64_t sample_time_ns,
-                             float mag_x_ut, float mag_y_ut, float mag_z_ut,
-                             float gyro_x_rps, float gyro_y_rps,
-                             float gyro_z_rps);
-
-//////// Function exposed for testing ///////////////////////////////////
-// Updating sensor data into the batch.
-// sample_time_ns in [ns].
-// mag_x_ut, mag_y_ut, mag_z_ut are in [uT].
-// gyro_x_rps, gyro_y_rps, gyro_z_rps are in [RAD/s].
-// odr_hz is in [Hz].
-void magGyroCalBatchUpdate(struct MagGyroBatchCal *mgbc,
-                           uint64_t sample_time_ns, float mag_x_ut,
-                           float mag_y_ut, float mag_z_ut, float gyro_x_rps,
-                           float gyro_y_rps, float gyro_z_rps);
-
-// Checks metrics and classifies bias update into either:
-// NO_UPDATE and no bias is updated
-// UPDATE_BIAS_MEDIUM bias is update with medium accuracy.
-// UPDATE_BIAS_HIGH bias is update with high accuracy.
-enum MagUpdate magGyroCalBiasUpdate(struct MagGyroCal *mgc,
-                                    struct Mat33 *first_sum_inverse);
-
-// Resetting the batch struct.
-void magGyroCalBatchReset(struct MagGyroBatchCal *mgbc);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_GYRO_CAL_H_
diff --git a/firmware/os/algos/calibration/synchronizer/synchronizer.cc b/firmware/os/algos/calibration/synchronizer/synchronizer.cc
deleted file mode 100644
index a485389b..00000000
--- a/firmware/os/algos/calibration/synchronizer/synchronizer.cc
+++ /dev/null
@@ -1,370 +0,0 @@
-#include "calibration/synchronizer/synchronizer.h"
-
-#include "calibration/util/cal_log.h"
-
-namespace synchronizer {
-namespace {
-
-// Alias to simplify the access to SensorIndex.
-using SensorIndex = online_calibration::SensorIndex;
-
-// Seeks through data_to_interpolate_to and data_to_interpolate queues so that
-// first two entries of data_to_interpolate have timestamps that straddle the
-// timestamp of the first entry of data_to_interpolate_to. Leading queue items
-// are popped and discarded to perform seeking.
-//
-// Assumes both data_to_interpolate_to and data_to_interpolate sorted by
-// strictly increasing timestamp.
-//
-// Returns True if seeking was achieved. Returns false if seeking could not
-// be achieved, such is if either pointer was null or they contain data
-// streams which cannot be made to straddle in time.
-
-bool SeekStraddle(
-    chre::ArrayQueue<online_calibration::SensorData,
-                     Synchronizer::kInputBufferSize>* data_to_interpolate_to,
-    chre::ArrayQueue<online_calibration::SensorData,
-                     Synchronizer::kInputBufferSize>* data_to_interpolate) {
-  // If either is a null pointer, return false
-  if (data_to_interpolate_to == nullptr || data_to_interpolate == nullptr) {
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                  "Unexpected null pointer during seek!");
-#endif
-    return false;
-  }
-
-  // If data_to_interpolate_to and data_to_interpolate are the same, no seeking.
-  if (reinterpret_cast<void*>(data_to_interpolate_to) ==
-      reinterpret_cast<void*>(data_to_interpolate)) {
-    return true;
-  }
-
-  // Check that queues are not empty and time min/max aren't impossible.
-  if (data_to_interpolate_to->empty() || data_to_interpolate->empty() ||
-      data_to_interpolate->front().timestamp_nanos >
-          data_to_interpolate_to->back().timestamp_nanos ||
-      data_to_interpolate->back().timestamp_nanos <
-          data_to_interpolate_to->front().timestamp_nanos) {
-    return false;
-  }
-
-  // Seek in data_to_interpolate_to until first entry of data_to_interpolate
-  // precedes first of data_to_interpolate_to.
-  while (!data_to_interpolate_to->empty() &&
-         data_to_interpolate->front().timestamp_nanos >
-             data_to_interpolate_to->front().timestamp_nanos) {
-    data_to_interpolate_to->pop();
-  }
-
-  // Check that data_to_interpolate_to still has at least one element.
-  if (data_to_interpolate_to->empty()) {
-    return false;
-  }
-
-  // Seek in data_to_interpolate until 1st and 2nd entries straddle first
-  // data_to_interpolate_to entry.
-  while (data_to_interpolate->size() > 1 &&
-         !((*data_to_interpolate)[0].timestamp_nanos <=
-               data_to_interpolate_to->front().timestamp_nanos &&
-           (*data_to_interpolate)[1].timestamp_nanos >=
-               data_to_interpolate_to->front().timestamp_nanos)) {
-    data_to_interpolate->pop();
-  }
-
-  // data_to_interpolate must either match exactly, or have >=2 elements to
-  // interpolate
-  if (data_to_interpolate->front().timestamp_nanos ==
-      data_to_interpolate_to->front().timestamp_nanos) {
-    return true;
-  } else if (data_to_interpolate->size() < 2) {
-    return false;
-  }
-
-  // Check that data_to_interpolate still has at least two elements and they
-  // straddle the first entry of data_to_interpolate_to.
-  if ((*data_to_interpolate)[0].timestamp_nanos >
-          data_to_interpolate_to->front().timestamp_nanos ||
-      (*data_to_interpolate)[1].timestamp_nanos <
-          data_to_interpolate_to->front().timestamp_nanos) {
-    return false;
-  }
-
-  return true;
-}
-// Performs linear interpolation for a single axis.
-float SingleAxisLinearInterpolation(float tb_minus_ta, float tc_minus_ta,
-                                    float va, float vc) {
-  return va + (vc - va) * (tb_minus_ta / tc_minus_ta);
-}
-
-// Interpolates data_to_interpolate using timestamps in
-// data_to_interpolate_to, writing to interpolated the resulting 3-axis
-// measurement vector and timestamp that matches a timestamp in
-// data_to_interpolate_to. For this to be possible, data_to_interpolate
-// must have at least two timestamps which straddle a timestamp in
-// data_to_interpolate_to.
-//
-// The input queues are assumed to have strictly increasing timestamps.
-//
-// First, this uses SeekStraddle() to pop off entries of each queue until the
-// first two entries of data_to_interpolate have timestamps that straddle the
-// timestamp of the first entry of data_to_interpolate_to. Then linear
-// interpolation proceeds and results are written to interpolated.
-//
-// Returns true if interpolation was possible. Returns false if any of the
-// inputs are nullptr or if interpolation was not possible given the
-// timestamps in the data queues.
-bool SeekAndInterpolateLinear(
-    chre::ArrayQueue<online_calibration::SensorData,
-                     Synchronizer::kInputBufferSize>* data_to_interpolate_to,
-    chre::ArrayQueue<online_calibration::SensorData,
-                     Synchronizer::kInputBufferSize>* data_to_interpolate,
-    online_calibration::SensorData* interpolated) {
-  // Seek until first two entries of data_to_interpolate straddle first entry of
-  // data_to_interpolate_to. Seek function also checks that
-  // data_to_interpolate_to and data_to_interpolate are not nullptr.
-  if (!SeekStraddle(data_to_interpolate_to, data_to_interpolate) ||
-      interpolated == nullptr) {
-    return false;
-  }
-
-  // Use some local variables to avoid repeated pointer dereferencing.
-  // data_to_interpolate_to and data_to_interpolate queues guaranteed by seek
-  // function to be non-empty.
-  online_calibration::SensorData data_to_interpolate_to_0 =
-      data_to_interpolate_to->front();
-  const online_calibration::SensorData data_to_interpolate_0 =
-      data_to_interpolate->front();
-  online_calibration::SensorData data_to_interpolate_1;
-
-  // Avoid floating point math if possible: check if timestamps match exactly.
-  if (data_to_interpolate_to_0.timestamp_nanos ==
-      data_to_interpolate_0.timestamp_nanos) {
-    *interpolated = data_to_interpolate_0;
-    return true;
-  } else if (data_to_interpolate->size() > 1) {
-    data_to_interpolate_1 = (*data_to_interpolate)[1];
-    if (data_to_interpolate_to_0.timestamp_nanos ==
-        data_to_interpolate_1.timestamp_nanos) {
-      *interpolated = data_to_interpolate_1;
-      return true;
-    }
-  } else {
-// Size was 1 and first entries didn't match, so interpolation impossible!
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                  "Unexpected queue state while attempting interpolation!");
-#endif
-    return false;
-  }
-
-  // Linearly interpolate data_to_interpolate at data_to_interpolate_to's 0th
-  // entry timepoint. Interpolate vector vb between va, vc at time tb where
-  // ta < tb < tc.
-  const float tb_minus_ta =
-      static_cast<float>(data_to_interpolate_to_0.timestamp_nanos -
-                         data_to_interpolate_0.timestamp_nanos);
-  const float tc_minus_ta =
-      static_cast<float>(data_to_interpolate_1.timestamp_nanos -
-                         data_to_interpolate_0.timestamp_nanos);
-
-  if (data_to_interpolate_0.type ==
-          online_calibration::SensorType::kAccelerometerMps2 ||
-      data_to_interpolate_0.type ==
-          online_calibration::SensorType::kGyroscopeRps ||
-      data_to_interpolate_0.type ==
-          online_calibration::SensorType::kMagnetometerUt) {
-    // Perform linear interpolation for 3-axis sensor and populate the result
-    // struct.
-    interpolated->data[SensorIndex::kXAxis] = SingleAxisLinearInterpolation(
-        tb_minus_ta, tc_minus_ta,
-        data_to_interpolate_0.data[SensorIndex::kXAxis],
-        data_to_interpolate_1.data[SensorIndex::kXAxis]);
-    interpolated->data[SensorIndex::kYAxis] = SingleAxisLinearInterpolation(
-        tb_minus_ta, tc_minus_ta,
-        data_to_interpolate_0.data[SensorIndex::kYAxis],
-        data_to_interpolate_1.data[SensorIndex::kYAxis]);
-    interpolated->data[SensorIndex::kZAxis] = SingleAxisLinearInterpolation(
-        tb_minus_ta, tc_minus_ta,
-        data_to_interpolate_0.data[SensorIndex::kZAxis],
-        data_to_interpolate_1.data[SensorIndex::kZAxis]);
-    interpolated->timestamp_nanos = data_to_interpolate_to_0.timestamp_nanos;
-    interpolated->type = data_to_interpolate_0.type;
-    return true;
-  } else if (data_to_interpolate_0.type ==
-                 online_calibration::SensorType::kTemperatureCelsius ||
-             data_to_interpolate_0.type ==
-                 online_calibration::SensorType::kBarometerHpa) {
-    // Perform linear interpolation for 1-axis sensor and populate the result
-    // struct.
-    interpolated->data[SensorIndex::kSingleAxis] =
-        SingleAxisLinearInterpolation(
-            tb_minus_ta, tc_minus_ta,
-            data_to_interpolate_0.data[SensorIndex::kSingleAxis],
-            data_to_interpolate_1.data[SensorIndex::kSingleAxis]);
-    interpolated->timestamp_nanos = data_to_interpolate_to_0.timestamp_nanos;
-    interpolated->type = data_to_interpolate_0.type;
-    return true;
-  } else {
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                  "Invalid sensor_type in SeekAndInterpolateLinear");
-#endif
-    return false;
-  }
-}
-
-// Attempts to add a SensorData measurement to a deque of them, returning
-// whether the new measurement was strictly greater in time than the last
-// entry in the queue. If it was not, measurement is not added and false is
-// returned. This function is primarily for code compactness: several queues
-// of measurements are appended-to in this same manner.
-bool AddMeasurementToDeque(
-    const online_calibration::SensorData& measurement,
-    chre::ArrayQueue<online_calibration::SensorData,
-                     Synchronizer::kInputBufferSize>* mutable_deque) {
-  // Push measurement onto the measurement queue if sequential.
-  if (mutable_deque->empty() ||
-      (!mutable_deque->empty() &&
-       measurement.timestamp_nanos > mutable_deque->back().timestamp_nanos)) {
-    mutable_deque->push(measurement);
-    return true;
-  } else {
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG(
-        "[SYNCHRONIZER WARNING]",
-        "measurement timestamp: %lu was not strictly greater than existing"
-        "queued timestamps.",
-        measurement.timestamp_nanos);
-#endif
-    return false;
-  }
-}
-}  // namespace
-
-Synchronizer::Synchronizer()
-    : Synchronizer(online_calibration::SensorType::kGyroscopeRps,
-                   online_calibration::SensorType::kAccelerometerMps2) {}
-
-Synchronizer::Synchronizer(
-    const online_calibration::SensorType& sensor_to_interpolate_to,
-    const online_calibration::SensorType& sensor_to_interpolate) {
-  if (sensor_to_interpolate_to == sensor_to_interpolate) {
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                  "Sensor_types are equal! Using default settings!");
-#endif
-    sensor_to_interpolate_to_ = online_calibration::SensorType::kGyroscopeRps;
-    sensor_to_interpolate_ = online_calibration::SensorType::kAccelerometerMps2;
-  } else {
-    // Setting sensors.
-    sensor_to_interpolate_to_ = sensor_to_interpolate_to;
-    sensor_to_interpolate_ = sensor_to_interpolate;
-  }
-}
-
-bool Synchronizer::SetDataToInterpolateToSensorType(
-    const online_calibration::SensorType& sensor_type) {
-  if (sensor_type == sensor_to_interpolate_) {
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                  "Sensor_type in SetDataToInterpolateToSensorType is "
-                  "already used!");
-#endif
-    return false;
-  }
-  if (sensor_type == online_calibration::SensorType::kUndefined) {
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                  "Invalid sensor_type in SetDataToInterpolateToSensorType");
-#endif
-    return false;
-  }
-  sensor_to_interpolate_to_ = sensor_type;
-  return true;
-}
-
-bool Synchronizer::SetDataToInterpolateSensorType(
-    const online_calibration::SensorType& sensor_type) {
-  if (sensor_type == sensor_to_interpolate_to_) {
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG(
-        "[SYNCHRONIZER WARNING]",
-        "Sensor_type in SetDataToInterpolateSensorType is already used!");
-#endif
-    return false;
-  }
-  if (sensor_type == online_calibration::SensorType::kUndefined) {
-#ifdef SYNCHRONIZER_DBG_ENABLED
-    CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                  "Invalid sensor_type in SetDataToInterpolateSensorType");
-#endif
-    return false;
-  }
-  sensor_to_interpolate_ = sensor_type;
-  return true;
-}
-
-void Synchronizer::AddMeasurement(const online_calibration::SensorData& meas) {
-  online_calibration::SensorType sensor_type = meas.type;
-  if (sensor_type == sensor_to_interpolate_to_) {
-    if (input_sensor_to_interpolate_to_.full()) {
-      CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                    "input_sensor_to_interpolate_to queue is full! "
-                    "Removing front element from queue");
-      input_sensor_to_interpolate_to_.pop();
-    }
-    if (AddMeasurementToDeque(meas, &input_sensor_to_interpolate_to_)) {
-      // Add succeeded. Process applicable measurement queue.
-      ProcessMeasurementQueues();
-    }
-  }
-  if (sensor_type == sensor_to_interpolate_) {
-    if (input_sensor_to_interpolate_.full()) {
-      CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
-                    "input_sensor_to_interpolate queue is full !"
-                    "Removing front element from queue");
-      input_sensor_to_interpolate_.pop();
-    }
-    if (AddMeasurementToDeque(meas, &input_sensor_to_interpolate_)) {
-      // Add succeeded. Process applicable measurement queue.
-      ProcessMeasurementQueues();
-    }
-  }
-}
-
-bool Synchronizer::ConsumeSynchronizedMeasurementQueues(
-    /*first sensor*/ online_calibration::SensorData* sensor_to_interpolate_to,
-    /*second sensor*/ online_calibration::SensorData* sensor_to_interpolate) {
-  if (!synchronized_first_and_second_queues_.empty()) {
-    *sensor_to_interpolate_to =
-        synchronized_first_and_second_queues_.front().first;
-    *sensor_to_interpolate =
-        synchronized_first_and_second_queues_.front().second;
-    synchronized_first_and_second_queues_.pop();
-    return true;
-  }
-  return false;
-}
-
-void Synchronizer::ProcessMeasurementQueues() {
-  // Results of interpolation written here.
-  online_calibration::SensorData interpolated;
-
-  // Process the queues, interpolating and queuing up measurements.
-  while (SeekAndInterpolateLinear(&input_sensor_to_interpolate_to_,
-                                  &input_sensor_to_interpolate_,
-                                  &interpolated)) {
-    synchronized_first_and_second_queues_.push(
-        std::pair<online_calibration::SensorData,
-                  online_calibration::SensorData>(
-            input_sensor_to_interpolate_to_.front(), interpolated));
-
-    // Pop off data_to_interpolate_to measurement used to create this sample.
-    // data_to_interpolate will be cleaned up next seek.
-    input_sensor_to_interpolate_to_.pop();
-  }
-}
-
-}  // namespace synchronizer
diff --git a/firmware/os/algos/calibration/synchronizer/synchronizer.h b/firmware/os/algos/calibration/synchronizer/synchronizer.h
deleted file mode 100644
index 1a0ef4cf..00000000
--- a/firmware/os/algos/calibration/synchronizer/synchronizer.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * This is a sensor data synchronizer module that interpolates buffers (queues)
- * of streamed measurements with respect to a user-selected input master stream
- * to set the timebase. To perform synchronization, a sensor (sensor_data.h)
- * is selected to define the resulting synchronized time axis. The second
- * selected sensor is interpolated to the first sensor's timescale. This
- * produces a synchronized queue of measurement pairs.
- */
-
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SYNCHRONIZER_SYNCHRONIZER_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SYNCHRONIZER_SYNCHRONIZER_H_
-
-#include "calibration/online_calibration/common_data/sensor_data.h"
-#include "chre/util/array_queue.h"
-
-namespace synchronizer {
-
-/*
- * Buffers measurements and synchronizes them using interpolation.
- *
- * Processes input streams of sensors and queues up output streams.
- */
-class Synchronizer {
- public:
-  // Input buffer size = 400 gives 1 second of buffer for a input sensor data
-  // rate of 400 Hz.
-  static constexpr int kInputBufferSize = 400;
-  // The output buffer size can store 10 sample, hist code has to ensure to
-  // grab the data before overflow.
-  static constexpr int kOutputBufferSize = 10;
-
-  Synchronizer();
-
-  Synchronizer(const online_calibration::SensorType& sensor_to_interpolate_to,
-               const online_calibration::SensorType& sensor_to_interpolate);
-
-  // Sets the sensor (first) to which the second sensor is interpolated to.
-  // Hence this sensor timebase is the master of the synchronized data stream.
-  // Notice that "sensor_to_interpolate_to_" cannot be equal
-  // "sensor_to_interpolate_", so this function cannot synchronize two accel's
-  // to each other.
-  // TODO(b/70861106): Change interface to a master slave approach.
-  // Sensors available (sensor_data.h).
-  bool SetDataToInterpolateToSensorType(
-      const online_calibration::SensorType& sensor_type);
-
-  // Sets the sensor (second) which will be interpolated. Notice that
-  // "sensor_to_interpolate_to_" cannot be equal "sensor_to_interpolate_".
-  // Sensors available (sensor_data.h).
-  bool SetDataToInterpolateSensorType(
-      const online_calibration::SensorType& sensor_type);
-
-  // Adds measurements. Uses the type in SensorData to identify
-  // sensor_to_interpolate and sensor_to_interpolate_to sensors.
-  void AddMeasurement(const online_calibration::SensorData& meas);
-
-  // Processes the synchronization queues in chronological order. Processing
-  // involves popping off each queue element and passing it to the specified
-  // function target of corresponding sensor type.
-  //
-  // Returns true if at least one element is in the sensor queue, and populates
-  // the synchronzied sensor data structs.
-  bool ConsumeSynchronizedMeasurementQueues(
-      /*sensor_to_interpolate_to*/
-      online_calibration::SensorData* sensor_to_interpolate_to,
-      /*sensor_to_interpolate*/
-      online_calibration::SensorData* sensor_to_interpolate);
-
- private:
-  // Attempts to pair up "sensor_to_interpolate_to" (first) and
-  // "sensor_to_interpolate" (second)  measurements buffered by the
-  // AddMeasurement() function, interpolating one using the other as a
-  // data_to_interpolate_to timescale if necessary.
-  //
-  // If interpolated measurements are generated, they are queued in this
-  // function.
-
-  void ProcessMeasurementQueues();
-
-  // Queue of synchronized measurements.
-  chre::ArrayQueue<
-      std::pair<online_calibration::SensorData, online_calibration::SensorData>,
-      kOutputBufferSize>
-      synchronized_first_and_second_queues_;
-
-  // Enums set which measurement defines the data_to_interpolate_to (first)
-  // timebase, against which sensor_to_interpolate is interpolated (second).
-  online_calibration::SensorType sensor_to_interpolate_to_;
-  online_calibration::SensorType sensor_to_interpolate_;
-
-  // Queues of time-ordered measurements for interpolation within this class.
-  chre::ArrayQueue<online_calibration::SensorData, kInputBufferSize>
-      input_sensor_to_interpolate_to_;
-  chre::ArrayQueue<online_calibration::SensorData, kInputBufferSize>
-      input_sensor_to_interpolate_;
-};
-
-}  // namespace synchronizer
-
-#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SYNCHRONIZER_SYNCHRONIZER_H_