diff --git a/Android.bp b/Android.bp
new file mode 100644
index 0000000..1e0d8c8
--- /dev/null
+++ b/Android.bp
@@ -0,0 +1,3 @@
+subdirs = [
+    "lib",
+]
diff --git a/contexthubhal/Android.mk b/contexthubhal/Android.mk
index 1600e1b..be27d47 100644
--- a/contexthubhal/Android.mk
+++ b/contexthubhal/Android.mk
@@ -14,7 +14,10 @@
 # Include target-specific files.
 LOCAL_SRC_FILES += nanohubhal_default.cpp
 
-LOCAL_STATIC_LIBRARIES += libnanohub_common
+LOCAL_HEADER_LIBRARIES := \
+    libhardware_headers \
+    libnanohub_common_headers \
+    libutils_headers
 
 LOCAL_MODULE := context_hub.default
 LOCAL_MODULE_TAGS := optional
diff --git a/contexthubhal/nanohubhal.cpp b/contexthubhal/nanohubhal.cpp
index fa3c5f8..21b6374 100644
--- a/contexthubhal/nanohubhal.cpp
+++ b/contexthubhal/nanohubhal.cpp
@@ -26,8 +26,8 @@
 #include <hardware/context_hub.h>
 #include <hardware/hardware.h>
 
-#include <utils/Log.h>
 #include <cutils/properties.h>
+#include <log/log.h>
 
 #include <nanohub/nanohub.h>
 
@@ -139,8 +139,11 @@
     discard_inotify_evt(pfd);
     while (access(NANOHUB_LOCK_FILE, F_OK) == 0) {
         ALOGW("Nanohub is locked; blocking read thread");
-        int ret = poll(&pfd, 1, 5000);
-        if (ret > 0) {
+        int ret = TEMP_FAILURE_RETRY(poll(&pfd, 1, 5000));
+        if (ret < 0) {
+            ALOGE("poll returned with an error: %s", strerror(errno));
+            break;
+        } else if (ret > 0) {
             discard_inotify_evt(pfd);
         }
     }
@@ -226,10 +229,12 @@
     setDebugFlags(property_get_int32("persist.nanohub.debug", 0));
 
     while (1) {
-        int ret = poll(myFds, numPollFds, -1);
-        if (ret <= 0) {
-            ALOGD("poll returned with an error: %s", strerror(errno));
+        int ret = TEMP_FAILURE_RETRY(poll(myFds, numPollFds, -1));
+        if (ret == 0)
             continue;
+        else if (ret < 0) {
+            ALOGE("poll returned with an error: %s", strerror(errno));
+            break;
         }
 
         if (hasInotify) {
diff --git a/contexthubhal/nanohubhal_default.cpp b/contexthubhal/nanohubhal_default.cpp
index 2645c61..fb5231e 100644
--- a/contexthubhal/nanohubhal_default.cpp
+++ b/contexthubhal/nanohubhal_default.cpp
@@ -18,7 +18,7 @@
 #include <hardware/context_hub.h>
 #include "nanohub_perdevice.h"
 #include "nanohubhal.h"
-#include <utils/Log.h>
+#include <log/log.h>
 
 namespace android {
 
diff --git a/contexthubhal/system_comms.cpp b/contexthubhal/system_comms.cpp
index c4e9b62..4a67a66 100644
--- a/contexthubhal/system_comms.cpp
+++ b/contexthubhal/system_comms.cpp
@@ -24,7 +24,7 @@
 
 #include <vector>
 
-#include <utils/Log.h>
+#include <log/log.h>
 
 #include <endian.h>
 
diff --git a/contexthubhal/test/main.cpp b/contexthubhal/test/main.cpp
index fe21e99..f8399ce 100644
--- a/contexthubhal/test/main.cpp
+++ b/contexthubhal/test/main.cpp
@@ -16,8 +16,8 @@
 #include <signal.h>
 #include <unistd.h>
 
+#include <log/log.h>
 #include <sys/endian.h>
-#include <utils/Log.h>
 
 #include <hardware/hardware.h>
 #include <hardware/context_hub.h>
diff --git a/firmware/app/chre/common/chre_app.c b/firmware/app/chre/common/chre_app.c
index aae23cf..55bbdf2 100644
--- a/firmware/app/chre/common/chre_app.c
+++ b/firmware/app/chre/common/chre_app.c
@@ -176,7 +176,7 @@
         return;
 
     si = eOsSensorFind(SENSOR_TYPE(evt), 0, &sensorHandle);
-    if (si) {
+    if (si && eOsSensorGetReqRate(sensorHandle)) {
         switch (si->numAxis) {
         case NUM_AXIS_EMBEDDED:
             processEmbeddedData(eventData, sensorHandle, SENSOR_TYPE(evt));
diff --git a/firmware/lib/builtins/Android.mk b/firmware/lib/builtins/Android.mk
index aa8663c..b43bdb2 100644
--- a/firmware/lib/builtins/Android.mk
+++ b/firmware/lib/builtins/Android.mk
@@ -31,6 +31,10 @@
     umoddi3.c               \
     aeabi_f2d.c             \
     aeabi_llsl.c            \
+    aeabi_llsr.c            \
+    aeabi_ul2f.c            \
+    aeabi_l2f.c             \
+    aeabi_f2ulz.c           \
 
 LOCAL_C_INCLUDES = $(LOCAL_PATH)
 LOCAL_EXPORT_C_INCLUDE_DIRS := \
diff --git a/firmware/lib/builtins/aeabi_f2ulz.c b/firmware/lib/builtins/aeabi_f2ulz.c
new file mode 100644
index 0000000..a6413a7
--- /dev/null
+++ b/firmware/lib/builtins/aeabi_f2ulz.c
@@ -0,0 +1,27 @@
+/* ===-- fixunssfdi.c - Implement __fixunssfdi -----------------------------===
+ *
+ *                     The LLVM Compiler Infrastructure
+ *
+ * This file is dual licensed under the MIT and the University of Illinois Open
+ * Source Licenses. See LICENSE.TXT for details.
+ *
+ * ===----------------------------------------------------------------------===
+ */
+
+#include "int_lib.h"
+
+du_int __aeabi_f2ulz(float a);
+
+/* Support for systems that have hardware floating-point; can set the invalid
+ * flag as a side-effect of computation.
+ */
+
+du_int
+__aeabi_f2ulz(float a)
+{
+    if (a <= 0.0f) return 0;
+    float da = a;
+    su_int high = da / 4294967296.f;               /* da / 0x1p32f; */
+    su_int low = da - (float)high * 4294967296.f; /* high * 0x1p32f; */
+    return ((du_int)high << 32) | low;
+}
diff --git a/firmware/lib/builtins/aeabi_l2f.c b/firmware/lib/builtins/aeabi_l2f.c
new file mode 100644
index 0000000..63d832f
--- /dev/null
+++ b/firmware/lib/builtins/aeabi_l2f.c
@@ -0,0 +1,80 @@
+/*===-- floatdisf.c - Implement __floatdisf -------------------------------===
+ *
+ *                     The LLVM Compiler Infrastructure
+ *
+ * This file is dual licensed under the MIT and the University of Illinois Open
+ * Source Licenses. See LICENSE.TXT for details.
+ *
+ *===----------------------------------------------------------------------===
+ *
+ * This file implements __floatdisf for the compiler_rt library.
+ *
+ *===----------------------------------------------------------------------===
+ */
+
+/* Returns: convert a to a float, rounding toward even.*/
+
+/* Assumption: float is a IEEE 32 bit floating point type
+ *             di_int is a 64 bit integral type
+ */
+
+/* seee eeee emmm mmmm mmmm mmmm mmmm mmmm */
+
+#include "int_lib.h"
+
+float __aeabi_l2f(di_int a);
+
+float
+__aeabi_l2f(di_int a)
+{
+    if (a == 0)
+        return 0.0F;
+    const unsigned N = sizeof(di_int) * CHAR_BIT;
+    const di_int s = a >> (N-1);
+    a = (a ^ s) - s;
+    int sd = N - __builtin_clzll(a);  /* number of significant digits */
+    int e = sd - 1;             /* exponent */
+    if (sd > FLT_MANT_DIG)
+    {
+        /*  start:  0000000000000000000001xxxxxxxxxxxxxxxxxxxxxxPQxxxxxxxxxxxxxxxxxx
+         *  finish: 000000000000000000000000000000000000001xxxxxxxxxxxxxxxxxxxxxxPQR
+         *                                                12345678901234567890123456
+         *  1 = msb 1 bit
+         *  P = bit FLT_MANT_DIG-1 bits to the right of 1
+         *  Q = bit FLT_MANT_DIG bits to the right of 1
+         *  R = "or" of all bits to the right of Q
+         */
+        switch (sd)
+        {
+        case FLT_MANT_DIG + 1:
+            a <<= 1;
+            break;
+        case FLT_MANT_DIG + 2:
+            break;
+        default:
+            a = ((du_int)a >> (sd - (FLT_MANT_DIG+2))) |
+                ((a & ((du_int)(-1) >> ((N + FLT_MANT_DIG+2) - sd))) != 0);
+        };
+        /* finish: */
+        a |= (a & 4) != 0;  /* Or P into R */
+        ++a;  /* round - this step may add a significant bit */
+        a >>= 2;  /* dump Q and R */
+        /* a is now rounded to FLT_MANT_DIG or FLT_MANT_DIG+1 bits */
+        if (a & ((du_int)1 << FLT_MANT_DIG))
+        {
+            a >>= 1;
+            ++e;
+        }
+        /* a is now rounded to FLT_MANT_DIG bits */
+    }
+    else
+    {
+        a <<= (FLT_MANT_DIG - sd);
+        /* a is now rounded to FLT_MANT_DIG bits */
+    }
+    float_bits fb;
+    fb.u = ((su_int)s & 0x80000000) |  /* sign */
+           ((e + 127) << 23)       |  /* exponent */
+           ((su_int)a & 0x007FFFFF);   /* mantissa */
+    return fb.f;
+}
diff --git a/firmware/lib/builtins/aeabi_llsr.c b/firmware/lib/builtins/aeabi_llsr.c
new file mode 100644
index 0000000..2f0ba6b
--- /dev/null
+++ b/firmware/lib/builtins/aeabi_llsr.c
@@ -0,0 +1,43 @@
+/* ===-- lshrdi3.c - Implement __lshrdi3 -----------------------------------===
+ *
+ *                     The LLVM Compiler Infrastructure
+ *
+ * This file is dual licensed under the MIT and the University of Illinois Open
+ * Source Licenses. See LICENSE.TXT for details.
+ *
+ * ===----------------------------------------------------------------------===
+ *
+ * This file implements __lshrdi3 for the compiler_rt library.
+ *
+ * ===----------------------------------------------------------------------===
+ */
+
+#include "int_lib.h"
+
+/* Returns: logical a >> b */
+
+/* Precondition:  0 <= b < bits_in_dword */
+
+di_int __aeabi_llsr(di_int a, si_int b);
+
+di_int
+__aeabi_llsr(di_int a, si_int b)
+{
+    const int bits_in_word = (int)(sizeof(si_int) * CHAR_BIT);
+    udwords input;
+    udwords result;
+    input.all = a;
+    if (b & bits_in_word)  /* bits_in_word <= b < bits_in_dword */
+    {
+        result.s.high = 0;
+        result.s.low = input.s.high >> (b - bits_in_word);
+    }
+    else  /* 0 <= b < bits_in_word */
+    {
+        if (b == 0)
+            return a;
+        result.s.high  = input.s.high >> b;
+        result.s.low = (input.s.high << (bits_in_word - b)) | (input.s.low >> b);
+    }
+    return result.all;
+}
diff --git a/firmware/lib/builtins/aeabi_ul2f.c b/firmware/lib/builtins/aeabi_ul2f.c
new file mode 100644
index 0000000..80734db
--- /dev/null
+++ b/firmware/lib/builtins/aeabi_ul2f.c
@@ -0,0 +1,77 @@
+/*===-- floatundisf.c - Implement __floatundisf ---------------------------===
+ *
+ *                     The LLVM Compiler Infrastructure
+ *
+ * This file is dual licensed under the MIT and the University of Illinois Open
+ * Source Licenses. See LICENSE.TXT for details.
+ *
+ * ===----------------------------------------------------------------------===
+ *
+ * This file implements __floatundisf for the compiler_rt library.
+ *
+ *===----------------------------------------------------------------------===
+ */
+
+/* Returns: convert a to a float, rounding toward even. */
+
+/* Assumption: float is a IEEE 32 bit floating point type
+ *            du_int is a 64 bit integral type
+ */
+
+/* seee eeee emmm mmmm mmmm mmmm mmmm mmmm */
+
+#include "int_lib.h"
+
+float __aeabi_ul2f(du_int a);
+
+float
+__aeabi_ul2f(du_int a)
+{
+    if (a == 0)
+        return 0.0F;
+    const unsigned N = sizeof(du_int) * CHAR_BIT;
+    int sd = N - __builtin_clzll(a);  /* number of significant digits */
+    int e = sd - 1;             /* 8 exponent */
+    if (sd > FLT_MANT_DIG)
+    {
+        /*  start:  0000000000000000000001xxxxxxxxxxxxxxxxxxxxxxPQxxxxxxxxxxxxxxxxxx
+         *  finish: 000000000000000000000000000000000000001xxxxxxxxxxxxxxxxxxxxxxPQR
+         *                                                12345678901234567890123456
+         *  1 = msb 1 bit
+         *  P = bit FLT_MANT_DIG-1 bits to the right of 1
+         *  Q = bit FLT_MANT_DIG bits to the right of 1
+         *  R = "or" of all bits to the right of Q
+         */
+        switch (sd)
+        {
+        case FLT_MANT_DIG + 1:
+            a <<= 1;
+            break;
+        case FLT_MANT_DIG + 2:
+            break;
+        default:
+            a = (a >> (sd - (FLT_MANT_DIG+2))) |
+                ((a & ((du_int)(-1) >> ((N + FLT_MANT_DIG+2) - sd))) != 0);
+        };
+        /* finish: */
+        a |= (a & 4) != 0;  /* Or P into R */
+        ++a;  /* round - this step may add a significant bit */
+        a >>= 2;  /* dump Q and R */
+        /* a is now rounded to FLT_MANT_DIG or FLT_MANT_DIG+1 bits */
+        if (a & ((du_int)1 << FLT_MANT_DIG))
+        {
+            a >>= 1;
+            ++e;
+        }
+        /* a is now rounded to FLT_MANT_DIG bits */
+    }
+    else
+    {
+        a <<= (FLT_MANT_DIG - sd);
+        /* a is now rounded to FLT_MANT_DIG bits */
+    }
+    float_bits fb;
+    fb.u = ((e + 127) << 23)       |  /* exponent */
+           ((su_int)a & 0x007FFFFF);  /* mantissa */
+    return fb.f;
+}
diff --git a/firmware/lib/builtins/int_lib.h b/firmware/lib/builtins/int_lib.h
index 3d968a8..fddef7f 100644
--- a/firmware/lib/builtins/int_lib.h
+++ b/firmware/lib/builtins/int_lib.h
@@ -16,7 +16,8 @@
 #ifndef INT_LIB_H
 #define INT_LIB_H
 
-#define CHAR_BIT 8
+#define FLT_MANT_DIG    __FLT_MANT_DIG__
+#define CHAR_BIT        8
 
 typedef unsigned su_int;
 typedef int si_int;
@@ -44,6 +45,12 @@
     } s;
 } udwords;
 
+typedef union
+{
+    su_int u;
+    float f;
+} float_bits;
+
 /* Assumption: Signed integral is 2's complement. */
 /* Assumption: Right shift of signed negative is arithmetic shift. */
 
diff --git a/firmware/lib/lib.mk b/firmware/lib/lib.mk
index 41d84ee..b26fc9f 100644
--- a/firmware/lib/lib.mk
+++ b/firmware/lib/lib.mk
@@ -84,4 +84,8 @@
 SRCS += $(BUILTINS_PATH)/umoddi3.c
 SRCS += $(BUILTINS_PATH)/aeabi_f2d.c
 SRCS += $(BUILTINS_PATH)/aeabi_llsl.c
+SRCS += $(BUILTINS_PATH)/aeabi_llsr.c
+SRCS += $(BUILTINS_PATH)/aeabi_ul2f.c
+SRCS += $(BUILTINS_PATH)/aeabi_l2f.c
+SRCS += $(BUILTINS_PATH)/aeabi_f2ulz.c
 CFLAGS += -I$(BUILTINS_PATH)
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index 0a6d96d..c700253 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -180,6 +180,7 @@
 
   acc->x_bias = acc->y_bias = acc->z_bias = 0;
   acc->x_bias_new = acc->y_bias_new = acc->z_bias_new = 0;
+  acc->average_temperature_celsius = 0;
 
 #ifdef IMU_TEMP_DBG_ENABLED
   acc->temp_time_nanos = 0;
@@ -422,7 +423,11 @@
   float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
   evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
 
-  float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z);
+  float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
+
+  // Testing for negative number.
+  float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
+
   // Passing when evmin/evmax> EIGEN_RATIO.
   int eigen_pass = (evmin > evmax * EIGEN_RATIO) && (evmag > EIGEN_MAG);
 
@@ -526,10 +531,11 @@
           // Eigen Ratio Test.
           if (accEigenTest(&acc->ac1[temp_gate].akf,
                            &acc->ac1[temp_gate].agd)) {
-            // Storing the new offsets.
+            // Storing the new offsets and average temperature.
             acc->x_bias_new = bias.x * KSCALE2;
             acc->y_bias_new = bias.y * KSCALE2;
             acc->z_bias_new = bias.z * KSCALE2;
+            acc->average_temperature_celsius = acc->ac1[temp_gate].agd.mean_t;
           }
 #ifdef ACCEL_CAL_DBG_ENABLED
           //// Debug ///////
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index e76abcd..1cfef61 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -139,6 +139,9 @@
   // to store a new offset, which gets updated during a power down event.
   float x_bias_new, y_bias_new, z_bias_new;
 
+  // Average temperature of the bias update.
+  float average_temperature_celsius;
+
   // Offset values that get subtracted from live data
   float x_bias, y_bias, z_bias;
 
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index d6a69f3..3179b0e 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -21,7 +21,7 @@
 #include <string.h>
 
 #include "calibration/util/cal_log.h"
-#include "common/math/vec.h"
+#include "common/math/macros.h"
 
 /////// DEFINITIONS AND MACROS ///////////////////////////////////////
 
@@ -29,28 +29,19 @@
 // of the given sensor).
 #define MAX_GYRO_BIAS (0.2f)  // [rad/sec]
 
-// Converts units of radians to milli-degrees.
-#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI)
-
 // Watchdog timeout value (5 seconds). Monitors dropouts in sensor data and
 // resets when exceeded.
-#define GYRO_WATCHDOG_TIMEOUT_NANOS (5000000000)
+#define GYRO_WATCHDOG_TIMEOUT_NANOS (SEC_TO_NANOS(5))
 
 #ifdef GYRO_CAL_DBG_ENABLED
 // The time value used to throttle debug messaging.
-#define GYROCAL_WAIT_TIME_NANOS (100000000)
-
-// Unit conversion: nanoseconds to seconds.
-#define NANOS_TO_SEC (1.0e-9f)
+#define GYROCAL_WAIT_TIME_NANOS (MSEC_TO_NANOS(100))
 
 // A debug version label to help with tracking results.
 #define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]"
 
 // Debug log tag string used to identify debug report output data.
 #define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]"
-
-// Debug log tag string used to identify debug tuning output data.
-#define GYROCAL_TUNE_TAG "[GYRO_CAL:TUNE]"
 #endif  // GYRO_CAL_DBG_ENABLED
 
 /////// FORWARD DECLARATIONS /////////////////////////////////////////
@@ -113,7 +104,7 @@
 };
 
 /*
- * Updates running calculation of the gyro's mean sampling rate.
+ * Updates the running calculation of the gyro's mean sampling rate.
  *
  * Behavior:
  *   1)  If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local
@@ -123,11 +114,13 @@
  *   3)  Otherwise, the local estimate of the mean sampling rates is updated.
  *
  * INPUTS:
- *   debug_mean_sampling_rate_hz:   Pointer to the mean sampling rate to update.
+ *   sample_rate_estimator:  Pointer to the estimator data structure.
+ *   debug_mean_sampling_rate_hz:  Pointer to the mean sampling rate to update.
  *   timestamp_nanos:  Time stamp (nanoseconds).
  *   reset_stats:  Flag that signals a reset of the sampling rate estimate.
  */
-static void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
+static void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
+                                   float* debug_mean_sampling_rate_hz,
                                    uint64_t timestamp_nanos, bool reset_stats);
 
 // Updates the information used for debug printouts.
@@ -146,17 +139,11 @@
     uint32_t hi = v >> 32, lo = v;
 
     if (!hi) //this is very fast for cases where we fit into a uint32_t
-        return(float)lo;
+        return (float)lo;
     else {
         return ((float)hi) * 4294967296.0f + (float)lo;
     }
 }
-
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-// Prints debug information useful for tuning the GyroCal parameters.
-static void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
-                                  uint64_t timestamp_nanos);
-#endif  // GYRO_CAL_DBG_TUNE_ENABLED
 #endif  // GYRO_CAL_DBG_ENABLED
 
 /////// FUNCTION DEFINITIONS /////////////////////////////////////////
@@ -235,7 +222,8 @@
   }
 
   // Ensures that the gyro sampling rate estimate is reset.
-  gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true);
+  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 0,
+                         /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
@@ -264,10 +252,10 @@
 
 #ifdef GYRO_CAL_DBG_ENABLED
   CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]",
-                "Gyro Bias Calibration [mDPS]: %s%d.%03d, %s%d.%03d, %s%d.%03d",
-                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 3),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 3),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 3));
+                "Gyro Bias Calibration [mDPS]: " CAL_FORMAT_3DIGITS_TRIPLET,
+                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 3),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 3),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 3));
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
@@ -310,7 +298,8 @@
 
 #ifdef GYRO_CAL_DBG_ENABLED
   // Update the gyro sampling rate estimate.
-  gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/false);
+  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+                         sample_time_nanos, /*reset_stats=*/false);
 #endif  // GYRO_CAL_DBG_ENABLED
 
   // Pass gyro data to stillness detector
@@ -453,7 +442,8 @@
 
 #ifdef GYRO_CAL_DBG_ENABLED
       // Resets the sampling rate estimate.
-      gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+      gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+                             sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
       // Update stillness flag. Force the start of a new stillness period.
@@ -496,7 +486,8 @@
 
 #ifdef GYRO_CAL_DBG_ENABLED
     // Resets the sampling rate estimate.
-    gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+    gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+                           sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
     // Update stillness flag.
@@ -517,20 +508,18 @@
         gyro_cal->gyro_stillness_detect.prev_mean_z <  MAX_GYRO_BIAS &&
         gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) {
 #ifdef GYRO_CAL_DBG_ENABLED
-    CAL_DEBUG_LOG("[GYRO_CAL:REJECT]",
-                  "Offset|Temp|Time [mDPS|C|nsec]: %s%d.%03d, %s%d.%03d, "
-                  "%s%d.%03d, %s%d.%03d, %llu",
-                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
-                                       RAD_TO_MILLI_DEGREES,
-                                   3),
-                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
-                                       RAD_TO_MILLI_DEGREES,
-                                   3),
-                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
-                                       RAD_TO_MILLI_DEGREES,
-                                   3),
-                  CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
-                  (unsigned long long int)calibration_time_nanos);
+    CAL_DEBUG_LOG(
+        "[GYRO_CAL:REJECT]",
+        "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+        ", " CAL_FORMAT_3DIGITS ", %llu",
+        CAL_ENCODE_FLOAT(
+            gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3),
+        CAL_ENCODE_FLOAT(
+            gyro_cal->gyro_stillness_detect.prev_mean_y * RAD_TO_MDEG, 3),
+        CAL_ENCODE_FLOAT(
+            gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3),
+        CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
+        (unsigned long long int)calibration_time_nanos);
 #endif  // GYRO_CAL_DBG_ENABLED
 
     // Outside of range. Ignore, reset, and continue.
@@ -620,7 +609,8 @@
 
 #ifdef GYRO_CAL_DBG_ENABLED
     // Resets the sampling rate estimate.
-    gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+    gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+                           sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
     // Resets the stillness window end-time.
@@ -661,10 +651,8 @@
       gyro_cal->temperature_mean_tracker.mean_accumulator = 0.0f;
 
       // Initializes the min/max temperatures values.
-      gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
-          FLT_MAX;
-      gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
-          -FLT_MAX;
+      gyro_cal->temperature_mean_tracker.temperature_min_celsius = FLT_MAX;
+      gyro_cal->temperature_mean_tracker.temperature_max_celsius = -FLT_MAX;
       break;
 
     case DO_UPDATE_DATA:
@@ -676,14 +664,14 @@
       // Tracks the min, max, and latest temperature values.
       gyro_cal->temperature_mean_tracker.latest_temperature_celsius =
           temperature_celsius;
-      if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] >
+      if (gyro_cal->temperature_mean_tracker.temperature_min_celsius >
           temperature_celsius) {
-        gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
+        gyro_cal->temperature_mean_tracker.temperature_min_celsius =
             temperature_celsius;
       }
-      if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] <
+      if (gyro_cal->temperature_mean_tracker.temperature_max_celsius <
           temperature_celsius) {
-        gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
+        gyro_cal->temperature_mean_tracker.temperature_max_celsius =
             temperature_celsius;
       }
       break;
@@ -709,9 +697,10 @@
       // Records the min/max and mean temperature values for debug purposes.
       gyro_cal->debug_gyro_cal.temperature_mean_celsius =
           gyro_cal->temperature_mean_celsius;
-      memcpy(gyro_cal->debug_gyro_cal.temperature_min_max_celsius,
-             gyro_cal->temperature_mean_tracker.temperature_min_max_celsius,
-             2 * sizeof(float));
+      gyro_cal->debug_gyro_cal.temperature_min_celsius =
+          gyro_cal->temperature_mean_tracker.temperature_min_celsius;
+      gyro_cal->debug_gyro_cal.temperature_max_celsius =
+          gyro_cal->temperature_mean_tracker.temperature_max_celsius;
 #endif
       break;
 
@@ -719,9 +708,8 @@
       // Determines if the min/max delta exceeded the set limit.
       if (gyro_cal->temperature_mean_tracker.num_points > 0) {
         min_max_temp_exceeded =
-            (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] -
-             gyro_cal->temperature_mean_tracker
-                 .temperature_min_max_celsius[0]) >
+            (gyro_cal->temperature_mean_tracker.temperature_max_celsius -
+             gyro_cal->temperature_mean_tracker.temperature_min_celsius) >
             gyro_cal->temperature_delta_limit_celsius;
 
 #ifdef GYRO_CAL_DBG_ENABLED
@@ -743,41 +731,50 @@
 
 bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
                           enum GyroCalTrackerCommand do_this) {
-  static float gyro_winmean_min[3] = {0.0f, 0.0f, 0.0f};
-  static float gyro_winmean_max[3] = {0.0f, 0.0f, 0.0f};
   bool mean_not_stable = false;
-  size_t i;
 
   switch (do_this) {
     case DO_RESET:
       // Resets the min/max window mean values to a default value.
-      for (i = 0; i < 3; i++) {
-        gyro_winmean_min[i] = FLT_MAX;
-        gyro_winmean_max[i] = -FLT_MAX;
+      for (size_t i = 0; i < 3; i++) {
+        gyro_cal->window_mean_tracker.gyro_winmean_min[i] = FLT_MAX;
+        gyro_cal->window_mean_tracker.gyro_winmean_max[i] = -FLT_MAX;
       }
       break;
 
     case DO_UPDATE_DATA:
       // Computes the min/max window mean values.
-      if (gyro_winmean_min[0] > gyro_cal->gyro_stillness_detect.win_mean_x) {
-        gyro_winmean_min[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_min[0] >
+          gyro_cal->gyro_stillness_detect.win_mean_x) {
+        gyro_cal->window_mean_tracker.gyro_winmean_min[0] =
+            gyro_cal->gyro_stillness_detect.win_mean_x;
       }
-      if (gyro_winmean_max[0] < gyro_cal->gyro_stillness_detect.win_mean_x) {
-        gyro_winmean_max[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_max[0] <
+          gyro_cal->gyro_stillness_detect.win_mean_x) {
+        gyro_cal->window_mean_tracker.gyro_winmean_max[0] =
+            gyro_cal->gyro_stillness_detect.win_mean_x;
       }
 
-      if (gyro_winmean_min[1] > gyro_cal->gyro_stillness_detect.win_mean_y) {
-        gyro_winmean_min[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_min[1] >
+          gyro_cal->gyro_stillness_detect.win_mean_y) {
+        gyro_cal->window_mean_tracker.gyro_winmean_min[1] =
+            gyro_cal->gyro_stillness_detect.win_mean_y;
       }
-      if (gyro_winmean_max[1] < gyro_cal->gyro_stillness_detect.win_mean_y) {
-        gyro_winmean_max[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_max[1] <
+          gyro_cal->gyro_stillness_detect.win_mean_y) {
+        gyro_cal->window_mean_tracker.gyro_winmean_max[1] =
+            gyro_cal->gyro_stillness_detect.win_mean_y;
       }
 
-      if (gyro_winmean_min[2] > gyro_cal->gyro_stillness_detect.win_mean_z) {
-        gyro_winmean_min[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_min[2] >
+          gyro_cal->gyro_stillness_detect.win_mean_z) {
+        gyro_cal->window_mean_tracker.gyro_winmean_min[2] =
+            gyro_cal->gyro_stillness_detect.win_mean_z;
       }
-      if (gyro_winmean_max[2] < gyro_cal->gyro_stillness_detect.win_mean_z) {
-        gyro_winmean_max[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_max[2] <
+          gyro_cal->gyro_stillness_detect.win_mean_z) {
+        gyro_cal->window_mean_tracker.gyro_winmean_max[2] =
+            gyro_cal->gyro_stillness_detect.win_mean_z;
       }
       break;
 
@@ -785,34 +782,45 @@
       // Store the most recent "stillness" mean data to the GyroCal data
       // structure. This functionality allows previous results to be recalled
       // when the device suddenly becomes "not still".
-      memcpy(gyro_cal->gyro_winmean_min, gyro_winmean_min, 3 * sizeof(float));
-      memcpy(gyro_cal->gyro_winmean_max, gyro_winmean_max, 3 * sizeof(float));
-    break;
+      memcpy(gyro_cal->gyro_winmean_min,
+             gyro_cal->window_mean_tracker.gyro_winmean_min,
+             sizeof(gyro_cal->window_mean_tracker.gyro_winmean_min));
+      memcpy(gyro_cal->gyro_winmean_max,
+             gyro_cal->window_mean_tracker.gyro_winmean_max,
+             sizeof(gyro_cal->window_mean_tracker.gyro_winmean_max));
+      break;
 
     case DO_EVALUATE:
       // Performs the stability check and returns the 'true' if the difference
       // between min/max window mean value is outside the stable range.
-      for (i = 0; i < 3; i++) {
-        mean_not_stable |= (gyro_winmean_max[i] - gyro_winmean_min[i]) >
+      for (size_t i = 0; i < 3; i++) {
+        mean_not_stable |= (gyro_cal->window_mean_tracker.gyro_winmean_max[i] -
+                            gyro_cal->window_mean_tracker.gyro_winmean_min[i]) >
                            gyro_cal->stillness_mean_delta_limit;
       }
 #ifdef GYRO_CAL_DBG_ENABLED
       if (mean_not_stable) {
         CAL_DEBUG_LOG(
             "[GYRO_CAL:MEAN_STABILITY_GATE]",
-            "Variation Limit|Delta [mDPS]: %s%d.%03d | %s%d.%03d, %s%d.%03d, "
-            "%s%d.%03d",
+            "Variation Limit|Delta [mDPS]: " CAL_FORMAT_3DIGITS
+            " | " CAL_FORMAT_3DIGITS_TRIPLET,
+            CAL_ENCODE_FLOAT(gyro_cal->stillness_mean_delta_limit * RAD_TO_MDEG,
+                             3),
             CAL_ENCODE_FLOAT(
-                gyro_cal->stillness_mean_delta_limit * RAD_TO_MILLI_DEGREES, 3),
-            CAL_ENCODE_FLOAT((gyro_winmean_max[0] - gyro_winmean_min[0]) *
-                                 RAD_TO_MILLI_DEGREES,
-                             3),
-            CAL_ENCODE_FLOAT((gyro_winmean_max[1] - gyro_winmean_min[1]) *
-                                 RAD_TO_MILLI_DEGREES,
-                             3),
-            CAL_ENCODE_FLOAT((gyro_winmean_max[2] - gyro_winmean_min[2]) *
-                                 RAD_TO_MILLI_DEGREES,
-                             3));
+                (gyro_cal->window_mean_tracker.gyro_winmean_max[0] -
+                 gyro_cal->window_mean_tracker.gyro_winmean_min[0]) *
+                    RAD_TO_MDEG,
+                3),
+            CAL_ENCODE_FLOAT(
+                (gyro_cal->window_mean_tracker.gyro_winmean_max[1] -
+                 gyro_cal->window_mean_tracker.gyro_winmean_min[1]) *
+                    RAD_TO_MDEG,
+                3),
+            CAL_ENCODE_FLOAT(
+                (gyro_cal->window_mean_tracker.gyro_winmean_max[2] -
+                 gyro_cal->window_mean_tracker.gyro_winmean_min[2]) *
+                    RAD_TO_MDEG,
+                3));
       }
 #endif  // GYRO_CAL_DBG_ENABLED
       break;
@@ -825,21 +833,19 @@
 }
 
 #ifdef GYRO_CAL_DBG_ENABLED
-void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
+void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
+                            float* debug_mean_sampling_rate_hz,
                             uint64_t timestamp_nanos, bool reset_stats) {
-  // This is used for local calculations of average sampling rate.
-  static uint64_t last_timestamp_nanos = 0;
-  static uint64_t time_delta_accumulator = 0;
-  static size_t num_samples = 0;
-
   // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads
   // out the estimate of the sampling rate.
   if (debug_mean_sampling_rate_hz) {
-    if (num_samples > 1 && time_delta_accumulator > 0) {
+    if (sample_rate_estimator->num_samples > 1 &&
+        sample_rate_estimator->time_delta_accumulator > 0) {
       // Computes the final mean calculation.
       *debug_mean_sampling_rate_hz =
-          num_samples /
-          (floatFromUint64(time_delta_accumulator) * NANOS_TO_SEC);
+          sample_rate_estimator->num_samples /
+          (floatFromUint64(sample_rate_estimator->time_delta_accumulator) *
+           NANOS_TO_SEC);
     } else {
       // Not enough samples to compute a valid sample rate estimate. Indicate
       // this with a -1 value.
@@ -850,26 +856,28 @@
 
   // Resets the sampling rate mean estimator data.
   if (reset_stats) {
-    last_timestamp_nanos = 0;
-    time_delta_accumulator = 0;
-    num_samples = 0;
+    sample_rate_estimator->last_timestamp_nanos = 0;
+    sample_rate_estimator->time_delta_accumulator = 0;
+    sample_rate_estimator->num_samples = 0;
     return;
   }
 
   // Skip adding this data to the accumulator if:
   //   1. A bad timestamp was received (i.e., time not monotonic).
   //   2. 'last_timestamp_nanos' is zero.
-  if (timestamp_nanos <= last_timestamp_nanos || last_timestamp_nanos == 0) {
-    last_timestamp_nanos = timestamp_nanos;
+  if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
+      sample_rate_estimator->last_timestamp_nanos == 0) {
+    sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
     return;
   }
 
   // Increments the number of samples.
-  num_samples++;
+  sample_rate_estimator->num_samples++;
 
   // Accumulate the time steps.
-  time_delta_accumulator += timestamp_nanos - last_timestamp_nanos;
-  last_timestamp_nanos = timestamp_nanos;
+  sample_rate_estimator->time_delta_accumulator +=
+      timestamp_nanos - sample_rate_estimator->last_timestamp_nanos;
+  sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
 }
 
 void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
@@ -905,14 +913,15 @@
   gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z;
 
   // Records the mean gyroscope sampling rate.
-  gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
+  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator,
+                         &gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
                          /*reset_stats=*/true);
 
   // Records the min/max gyroscope window stillness mean values.
   memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min,
-         3 * sizeof(float));
+         sizeof(gyro_cal->gyro_winmean_min));
   memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max,
-         3 * sizeof(float));
+         sizeof(gyro_cal->gyro_winmean_max));
 
   // Records the previous stillness window means.
   gyro_cal->debug_gyro_cal.accel_mean[0] =
@@ -971,23 +980,21 @@
   float mag_data;
   switch (print_data) {
     case OFFSET:
-      CAL_DEBUG_LOG(debug_tag,
-                    "Cal#|Offset|Temp|Time [mDPS|C|nsec]: %lu, %s%d.%03d, "
-                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
-                    (unsigned long int)gyro_cal->debug_calibration_count,
-                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[0] *
-                                         RAD_TO_MILLI_DEGREES,
-                                     3),
-                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[1] *
-                                         RAD_TO_MILLI_DEGREES,
-                                     3),
-                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[2] *
-                                         RAD_TO_MILLI_DEGREES,
-                                     3),
-                    CAL_ENCODE_FLOAT(
-                        gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3),
-                    (unsigned long long int)
-                        gyro_cal->debug_gyro_cal.end_still_time_nanos);
+      CAL_DEBUG_LOG(
+          debug_tag,
+          "Cal#|Offset|Temp|Time [mDPS|C|nsec]: "
+          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
+          (unsigned long int)gyro_cal->debug_calibration_count,
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.calibration[1] * RAD_TO_MDEG, 3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
+                           3),
+          (unsigned long long int)
+              gyro_cal->debug_gyro_cal.end_still_time_nanos);
       break;
 
     case STILLNESS_DATA:
@@ -996,8 +1003,8 @@
                      : -1.0f;  // Signals that magnetometer was not used.
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Stillness|Confidence [nsec]: %lu, %llu, %s%d.%03d, %s%d.%03d, "
-          "%s%d.%03d",
+          "Cal#|Stillness|Confidence [nsec]: %lu, "
+          "%llu, " CAL_FORMAT_3DIGITS_TRIPLET,
           (unsigned long int)gyro_cal->debug_calibration_count,
           (unsigned long long int)(gyro_cal->debug_gyro_cal
                                        .end_still_time_nanos -
@@ -1011,91 +1018,90 @@
     case SAMPLE_RATE_AND_TEMPERATURE:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: %lu, %s%d.%03d, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%04d, %s%d.%03d",
+          "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: "
+          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+          ", " CAL_FORMAT_3DIGITS,
           (unsigned long int)gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
                            3),
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0], 3),
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1], 3),
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1] -
-                  gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0],
-              4),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_min_celsius, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius -
+                               gyro_cal->debug_gyro_cal.temperature_min_celsius,
+                           3),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3));
       break;
 
     case GYRO_MINMAX_STILLNESS_MEAN:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Gyro Peak Stillness Variation [mDPS]: %lu, %s%d.%03d, "
-          "%s%d.%03d, %s%d.%03d",
+          "Cal#|Gyro Peak Stillness Variation [mDPS]: "
+          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET,
           (unsigned long int)gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] -
                             gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) *
-                               RAD_TO_MILLI_DEGREES,
+                               RAD_TO_MDEG,
                            3),
           CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] -
                             gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) *
-                               RAD_TO_MILLI_DEGREES,
+                               RAD_TO_MDEG,
                            3),
           CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] -
                             gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) *
-                               RAD_TO_MILLI_DEGREES,
+                               RAD_TO_MDEG,
                            3));
       break;
 
     case ACCEL_STATS:
-      CAL_DEBUG_LOG(
-          debug_tag,
-          "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %lu, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
-          (unsigned long int)gyro_cal->debug_calibration_count,
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 6),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6));
+      CAL_DEBUG_LOG(debug_tag,
+                    "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: "
+                    "%lu, " CAL_FORMAT_3DIGITS_TRIPLET
+                    ", " CAL_FORMAT_6DIGITS_TRIPLET,
+                    (unsigned long int)gyro_cal->debug_calibration_count,
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 6),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6));
       break;
 
     case GYRO_STATS:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, %s%d.%03d, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+          "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+          ", " CAL_FORMAT_3DIGITS_TRIPLET,
           (unsigned long int)gyro_cal->debug_calibration_count,
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 3),
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 3),
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MILLI_DEGREES, 3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0] *
-                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG,
                            3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1] *
-                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG,
                            3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2] *
-                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
-                           3));
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MDEG,
+                           3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.gyro_var[0] * RAD_TO_MDEG * RAD_TO_MDEG,
+              3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.gyro_var[1] * RAD_TO_MDEG * RAD_TO_MDEG,
+              3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.gyro_var[2] * RAD_TO_MDEG * RAD_TO_MDEG,
+              3));
       break;
 
     case MAG_STATS:
       if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
-        CAL_DEBUG_LOG(debug_tag,
-                      "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%03d, "
-                      "%s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
-                      (unsigned long int)gyro_cal->debug_calibration_count,
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 6),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 6),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
+        CAL_DEBUG_LOG(
+            debug_tag,
+            "Cal#|Mag Mean|Var [uT|uT^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+            ", " CAL_FORMAT_6DIGITS_TRIPLET,
+            (unsigned long int)gyro_cal->debug_calibration_count,
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 6),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 6),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
       } else {
         CAL_DEBUG_LOG(debug_tag,
                       "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
@@ -1104,73 +1110,12 @@
       }
       break;
 
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-    case ACCEL_STATS_TUNING:
-      CAL_DEBUG_LOG(
-          debug_tag,
-          "Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %s%d.%03d, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_x, 3),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_y, 3),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_z, 3),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 6));
-      break;
-
-    case GYRO_STATS_TUNING:
-      CAL_DEBUG_LOG(
-          debug_tag,
-          "Gyro Mean|Var [mDPS|mDPS^2]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-          "%s%d.%06d, %s%d.%06d, %s%d.%06d",
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
-                               RAD_TO_MILLI_DEGREES,
-                           3),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
-                               RAD_TO_MILLI_DEGREES,
-                           3),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
-                               RAD_TO_MILLI_DEGREES,
-                           3),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x *
-                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
-                           6),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y *
-                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
-                           6),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z *
-                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
-                           6));
-      break;
-
-    case MAG_STATS_TUNING:
-      if (gyro_cal->using_mag_sensor) {
-        CAL_DEBUG_LOG(
-            debug_tag,
-            "Mag Mean|Var [uT|uT^2]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-            "%s%d.%06d, %s%d.%06d, %s%d.%06d",
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_x, 3),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_y, 3),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_z, 3),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 6),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 6),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 6));
-      } else {
-        CAL_DEBUG_LOG(GYROCAL_TUNE_TAG,
-                      "Mag Mean|Var [uT|uT^2]: 0, 0, 0, -1.0, -1.0, -1.0");
-      }
-      break;
-#endif  // GYRO_CAL_DBG_TUNE_ENABLED
-
     default:
       break;
   }
 }
 
 void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) {
-  static enum GyroCalDebugState next_state = GYRO_IDLE;
-  static uint64_t wait_timer_nanos = 0;
-
   // This is a state machine that controls the reporting out of debug data.
   switch (gyro_cal->debug_state) {
     case GYRO_IDLE:
@@ -1188,144 +1133,70 @@
 
     case GYRO_WAIT_STATE:
       // This helps throttle the print statements.
-      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-              timestamp_nanos, wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) {
-        gyro_cal->debug_state = next_state;
+      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos,
+                                                   gyro_cal->wait_timer_nanos,
+                                                   GYROCAL_WAIT_TIME_NANOS)) {
+        gyro_cal->debug_state = gyro_cal->next_state;
       }
       break;
 
     case GYRO_PRINT_OFFSET:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_STILLNESS_DATA;   // Sets the next state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state = GYRO_PRINT_STILLNESS_DATA;  // Sets the next state.
       gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       break;
 
     case GYRO_PRINT_STILLNESS_DATA:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, STILLNESS_DATA);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE;  // Sets next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state =
+          GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE;  // Sets next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;     // First, go to wait state.
       break;
 
     case GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
                             SAMPLE_RATE_AND_TEMPERATURE);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN;  // Sets next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state =
+          GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN;  // Sets next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;    // First, go to wait state.
       break;
 
     case GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
                             GYRO_MINMAX_STILLNESS_MEAN);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_ACCEL_STATS;      // Sets the next state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;   // Starts the wait timer.
+      gyro_cal->next_state = GYRO_PRINT_ACCEL_STATS;  // Sets the next state.
       gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       break;
 
     case GYRO_PRINT_ACCEL_STATS:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, ACCEL_STATS);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_GYRO_STATS;       // Sets the next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state = GYRO_PRINT_GYRO_STATS;  // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
       break;
 
     case GYRO_PRINT_GYRO_STATS:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_STATS);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_MAG_STATS;        // Sets the next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state = GYRO_PRINT_MAG_STATS;   // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
       break;
 
     case GYRO_PRINT_MAG_STATS:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, MAG_STATS);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_IDLE;                   // Sets the next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state = GYRO_IDLE;              // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
       break;
 
     default:
       // Sends this state machine to its idle state.
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
-  }
-
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-  if (gyro_cal->debug_state == GYRO_IDLE) {
-    // This check keeps the tuning printout from interleaving with the above
-    // debug print data.
-    gyroCalTuneDebugPrint(gyro_cal, timestamp_nanos);
-  }
-#endif  // GYRO_CAL_DBG_TUNE_ENABLED
-}
-
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
-                           uint64_t timestamp_nanos) {
-  static enum GyroCalDebugState debug_state = GYRO_IDLE;
-  static enum GyroCalDebugState next_state = GYRO_IDLE;
-  static uint64_t wait_timer_nanos = 0;
-
-  // Output sensor variance levels to assist with tuning thresholds.
-  //   i.  Within the first 300 seconds of boot: output interval = 5
-  //       seconds.
-  //   ii. Thereafter: output interval is 60 seconds.
-  bool condition_i = ((timestamp_nanos <= 300000000000) &&
-                      NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-                          timestamp_nanos, wait_timer_nanos, 5000000000));
-  bool condition_ii = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-      timestamp_nanos, wait_timer_nanos, 60000000000);
-
-  // This is a state machine that controls the reporting out of tuning data.
-  switch (debug_state) {
-    case GYRO_IDLE:
-      // Wait for a trigger and start the data tuning printout sequence.
-      if (condition_i || condition_ii) {
-        CAL_DEBUG_LOG(GYROCAL_TUNE_TAG, "Temp [C]: %s%d.%03d",
-                      CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3));
-        wait_timer_nanos = timestamp_nanos;   // Starts the wait timer.
-        next_state = GYRO_PRINT_ACCEL_STATS;  // Sets the next state.
-        debug_state = GYRO_WAIT_STATE;        // First, go to wait state.
-      } else {
-        debug_state = GYRO_IDLE;
-      }
-      break;
-
-    case GYRO_WAIT_STATE:
-      // This helps throttle the print statements.
-      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-              timestamp_nanos, wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) {
-        debug_state = next_state;
-      }
-      break;
-
-    case GYRO_PRINT_ACCEL_STATS:
-      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, ACCEL_STATS_TUNING);
-      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
-      next_state = GYRO_PRINT_GYRO_STATS;  // Sets the next state.
-      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
-      break;
-
-    case GYRO_PRINT_GYRO_STATS:
-      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, GYRO_STATS_TUNING);
-      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
-      next_state = GYRO_PRINT_MAG_STATS;   // Sets the next state.
-      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
-      break;
-
-    case GYRO_PRINT_MAG_STATS:
-      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, MAG_STATS_TUNING);
-      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
-      next_state = GYRO_IDLE;              // Sets the next state.
-      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
-      break;
-
-    default:
-      // Sends this state machine to its idle state.
-      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
-      debug_state = GYRO_IDLE;
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->debug_state = GYRO_IDLE;             // Go to idle state.
   }
 }
-#endif  // GYRO_CAL_DBG_TUNE_ENABLED
 #endif  // GYRO_CAL_DBG_ENABLED
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index cd96676..5e7d5ee 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -35,7 +35,6 @@
  *       - Temperature   [Celsius]
  *
  * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
- * #define GYRO_CAL_DBG_TUNE_ENABLED to periodically printout sensor variance
  * data to assist in tuning the GyroCal parameters.
  */
 
@@ -80,15 +79,30 @@
   float mag_var[3];
   float gyro_winmean_min[3];
   float gyro_winmean_max[3];
-  float temperature_min_max_celsius[2];  // 0=min; 1=max
+  float temperature_min_celsius;
+  float temperature_max_celsius;
   float temperature_mean_celsius;
   bool using_mag_sensor;
 };
+
+// Data structure for sample rate estimation.
+struct SampleRateData {
+  uint64_t last_timestamp_nanos;
+  uint64_t time_delta_accumulator;
+  size_t num_samples;
+};
 #endif  // GYRO_CAL_DBG_ENABLED
 
+// Data structure for tracking min/max window mean during device stillness.
+struct MinMaxWindowMeanData {
+  float gyro_winmean_min[3];
+  float gyro_winmean_max[3];
+};
+
 // Data structure for tracking temperature data during device stillness.
 struct TemperatureMeanData {
-  float temperature_min_max_celsius[2];
+  float temperature_min_celsius;
+  float temperature_max_celsius;
   float latest_temperature_celsius;
   float mean_accumulator;
   size_t num_points;
@@ -103,6 +117,9 @@
   // Data for tracking temperature mean during periods of device stillness.
   struct TemperatureMeanData temperature_mean_tracker;
 
+  // Data for tracking gyro mean during periods of device stillness.
+  struct MinMaxWindowMeanData window_mean_tracker;
+
   // Aggregated sensor stillness threshold required for gyro bias calibration.
   float stillness_threshold;
 
@@ -166,6 +183,11 @@
   // Debug info.
   struct DebugGyroCal debug_gyro_cal;  // Debug data structure.
   enum GyroCalDebugState debug_state;  // Debug printout state machine.
+  enum GyroCalDebugState next_state;   // Debug state machine next state.
+  uint64_t wait_timer_nanos;           // Debug message throttle timer.
+
+  struct SampleRateData sample_rate_estimator;  // Debug sample rate estimator.
+
   size_t debug_calibration_count;      // Total number of cals performed.
   size_t debug_watchdog_count;         // Total number of watchdog timeouts.
   bool debug_print_trigger;            // Flag used to trigger data printout.
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
index 1fb7630..7f8e563 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -71,7 +71,10 @@
   float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
   evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
 
-  float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z);
+  float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
+
+  // Testing for negative number.
+  float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
 
   int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) &&
                    (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG);
@@ -119,7 +122,8 @@
   initVec3(&v, out.x, out.y, out.z);
   vec3ScalarMul(&v, -0.5f);
 
-  float r = sqrtf(vec3Dot(&v, &v) - out.w);
+  float r_square = vec3Dot(&v, &v) - out.w;
+  float r = (r_square > 0) ? sqrtf(r_square) : 0;
 
   initVec3(bias, v.x, v.y, v.z);
   *radius = r;
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
index 254dd7b..d92f1da 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -22,7 +22,7 @@
 #include <string.h>
 
 #include "calibration/util/cal_log.h"
-#include "common/math/vec.h"
+#include "common/math/macros.h"
 #include "util/nano_assert.h"
 
 /////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
@@ -32,9 +32,17 @@
 
 // Defines the default weighting function for the linear model fit routine.
 // Weighting = 10.0; for offsets newer than 5 minutes.
-#define OTC_WEIGHT_DEFINITION_0  0, 300000000000, 10.0f
+static const struct OverTempCalWeightPt kOtcDefaultWeight0 = {
+    .offset_age_nanos = MIN_TO_NANOS(5),
+    .weight = 10.0f,
+};
+
 // Weighting = 0.1; for offsets newer than 15 minutes.
-#define OTC_WEIGHT_DEFINITION_1  1, 900000000000, 0.1f
+static const struct OverTempCalWeightPt kOtcDefaultWeight1 = {
+    .offset_age_nanos = MIN_TO_NANOS(15),
+    .weight = 0.1f,
+};
+
 // The default weighting used for all older offsets.
 #define OTC_MIN_WEIGHT_VALUE  (0.04f)
 
@@ -42,11 +50,11 @@
 // A debug version label to help with tracking results.
 #define OTC_DEBUG_VERSION_STRING "[July 05, 2017]"
 
-// The time value used to throttle debug messaging (100msec).
-#define OTC_WAIT_TIME_NANOS (100000000)
+// The time interval used to throttle debug messaging (100msec).
+#define OTC_WAIT_TIME_NANOS (SEC_TO_NANOS(0.1))
 
-// The time value used to throttle temperture print messaging (1 second).
-#define OTC_PRINT_TEMP_NANOS (1000000000)
+// The time interval used to throttle temperture print messaging (1 second).
+#define OTC_PRINT_TEMP_NANOS (SEC_TO_NANOS(1))
 
 // Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
 static const char  kDebugAxisLabel[3] = "XYZ";
@@ -166,80 +174,25 @@
                          size_t axis_index, float temperature_celsius);
 
 // Sets the OTC model parameters to an "initialized" state.
-static void resetOtcLinearModel(struct OverTempCal *over_temp_cal) {
-  ASSERT_NOT_NULL(over_temp_cal);
-
-  // Sets the temperature sensitivity model parameters to
-  // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial"
-  // state.
-  over_temp_cal->temp_sensitivity[0] = OTC_INITIAL_SENSITIVITY;
-  over_temp_cal->temp_sensitivity[1] = OTC_INITIAL_SENSITIVITY;
-  over_temp_cal->temp_sensitivity[2] = OTC_INITIAL_SENSITIVITY;
-  memset(over_temp_cal->sensor_intercept, 0, 3 * sizeof(float));
-}
+static void resetOtcLinearModel(struct OverTempCal *over_temp_cal);
 
 // Checks that the input temperature value is within the valid range. If outside
 // of range, then 'temperature_celsius' is coerced to within the limits.
-static bool checkAndEnforceTemperatureRange(float *temperature_celsius) {
-  if (*temperature_celsius > OTC_TEMP_MAX_CELSIUS) {
-    *temperature_celsius = OTC_TEMP_MAX_CELSIUS;
-    return false;
-  }
-  if (*temperature_celsius < OTC_TEMP_MIN_CELSIUS) {
-    *temperature_celsius = OTC_TEMP_MIN_CELSIUS;
-    return false;
-  }
-  return true;
-}
+static bool checkAndEnforceTemperatureRange(float *temperature_celsius);
 
 // Returns "true" if the candidate linear model parameters are within the valid
 // range, and not all zeros.
 static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
-                   float temp_sensitivity, float sensor_intercept) {
-  ASSERT_NOT_NULL(over_temp_cal);
-
-  return NANO_ABS(temp_sensitivity) < over_temp_cal->temp_sensitivity_limit &&
-         NANO_ABS(sensor_intercept) < over_temp_cal->sensor_intercept_limit &&
-         NANO_ABS(temp_sensitivity) > OTC_MODELDATA_NEAR_ZERO_TOL &&
-         NANO_ABS(sensor_intercept) > OTC_MODELDATA_NEAR_ZERO_TOL;
-}
+                   float temp_sensitivity, float sensor_intercept);
 
 // Returns "true" if 'offset' and 'offset_temp_celsius' is valid.
-static bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
-  ASSERT_NOT_NULL(offset);
-
-  // Simple check to ensure that:
-  //   1. All of the input data is non "zero".
-  //   2. The offset temperature is within the valid range.
-  if (NANO_ABS(offset[0]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
-      NANO_ABS(offset[1]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
-      NANO_ABS(offset[2]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
-      NANO_ABS(offset_temp_celsius) < OTC_MODELDATA_NEAR_ZERO_TOL) {
-    return false;
-  }
-
-  // Only returns the "check" result. Don't care about coercion.
-  return checkAndEnforceTemperatureRange(&offset_temp_celsius);
-}
+static bool isValidOtcOffset(const float *offset, float offset_temp_celsius);
 
 // Returns the least-squares weight based on the age of a particular offset
 // estimate.
 static float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
                                        uint64_t offset_timestamp_nanos,
-                                       uint64_t current_timestamp_nanos) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  size_t i;
-  for (i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
-    if (current_timestamp_nanos <=
-        offset_timestamp_nanos +
-            over_temp_cal->weighting_function[i].offset_age_nanos) {
-      return over_temp_cal->weighting_function[i].weight;
-    }
-  }
-
-  // Returning the default weight for all older offsets.
-  return OTC_MIN_WEIGHT_VALUE;
-}
+                                       uint64_t current_timestamp_nanos);
 
 // Updates 'compensated_offset' using the linear OTC model.
 static void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
@@ -292,14 +245,7 @@
 //     new_debug_tag = "INIT]"
 //   Output: "[OVER_TEMP_CAL:INIT]"
 static void createDebugTag(struct OverTempCal *over_temp_cal,
-                           const char *new_debug_tag) {
-  over_temp_cal->otc_debug_tag[0] = '[';
-  memcpy(over_temp_cal->otc_debug_tag + 1, over_temp_cal->otc_sensor_tag,
-         strlen(over_temp_cal->otc_sensor_tag));
-  memcpy(
-      over_temp_cal->otc_debug_tag + strlen(over_temp_cal->otc_sensor_tag) + 1,
-      new_debug_tag, strlen(new_debug_tag) + 1);
-}
+                           const char *new_debug_tag);
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
 /////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
@@ -342,13 +288,13 @@
       OTC_TEMP_INVALID_CELSIUS;
 
   // Defines the default weighting function for the linear model fit routine.
-  overTempSetWeightingFunction(over_temp_cal, OTC_WEIGHT_DEFINITION_0);
-  overTempSetWeightingFunction(over_temp_cal, OTC_WEIGHT_DEFINITION_1);
+  overTempSetWeightingFunction(over_temp_cal, 0, &kOtcDefaultWeight0);
+  overTempSetWeightingFunction(over_temp_cal, 1, &kOtcDefaultWeight1);
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
   // Sets the default sensor descriptors for debugging.
   overTempCalDebugDescriptors(over_temp_cal, "OVER_TEMP_CAL", "mDPS",
-                              1e3f * 180.0f / NANO_PI);
+                              RAD_TO_MDEG);
 
   createDebugTag(over_temp_cal, ":INIT]");
   if (over_temp_cal->over_temp_enable) {
@@ -376,8 +322,7 @@
   // Sets the model parameters if they are within the acceptable limits.
   // Includes a check to reject input model parameters that may have been passed
   // in as all zeros.
-  size_t i;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
                               sensor_intercept[i])) {
       over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
@@ -394,7 +339,8 @@
     // Checks that the new offset data is valid.
     if (isValidOtcOffset(offset, offset_temp_celsius)) {
       // Sets the initial over-temp calibration estimate.
-      memcpy(over_temp_cal->model_data[0].offset, offset, 3 * sizeof(float));
+      memcpy(over_temp_cal->model_data[0].offset, offset,
+             sizeof(over_temp_cal->model_data[0].offset));
       over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius;
       over_temp_cal->model_data[0].timestamp_nanos = timestamp_nanos;
       over_temp_cal->num_model_pts = 1;
@@ -412,7 +358,8 @@
   // If the new offset is valid, then it will be used as the current compensated
   // offset, otherwise the current value will be kept.
   if (isValidOtcOffset(offset, offset_temp_celsius)) {
-    memcpy(over_temp_cal->compensated_offset.offset, offset, 3 * sizeof(float));
+    memcpy(over_temp_cal->compensated_offset.offset, offset,
+           sizeof(over_temp_cal->compensated_offset.offset));
     over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius;
     over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
   }
@@ -430,7 +377,8 @@
   createDebugTag(over_temp_cal, ":SET MODEL]");
   CAL_DEBUG_LOG(
       over_temp_cal->otc_debug_tag,
-      "Offset|Temp [%s|C]: %s%d.%03d, %s%d.%03d, %s%d.%03d | %s%d.%03d",
+      "Offset|Temp [%s|C]: " CAL_FORMAT_3DIGITS_TRIPLET
+      " | " CAL_FORMAT_3DIGITS,
       over_temp_cal->otc_unit_tag,
       CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
       CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
@@ -439,8 +387,8 @@
 
   CAL_DEBUG_LOG(
       over_temp_cal->otc_debug_tag,
-      "Sensitivity|Intercept [%s/C|%s]: %s%d.%03d, %s%d.%03d, %s%d.%03d | "
-      "%s%d.%03d, %s%d.%03d, %s%d.%03d",
+      "Sensitivity|Intercept [%s/C|%s]: " CAL_FORMAT_3DIGITS_TRIPLET
+      " | " CAL_FORMAT_3DIGITS_TRIPLET,
       over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag,
       CAL_ENCODE_FLOAT(temp_sensitivity[0] * over_temp_cal->otc_unit_conversion,
                        3),
@@ -475,8 +423,10 @@
   ASSERT_NOT_NULL(sensor_intercept);
 
   // Gets the latest over-temp calibration model data.
-  memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 3 * sizeof(float));
-  memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 3 * sizeof(float));
+  memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity,
+         sizeof(over_temp_cal->temp_sensitivity));
+  memcpy(sensor_intercept, over_temp_cal->sensor_intercept,
+         sizeof(over_temp_cal->sensor_intercept));
   *timestamp_nanos = over_temp_cal->last_model_update_nanos;
 
   // Gets the latest temperature compensated offset estimate.
@@ -491,9 +441,8 @@
 
   // Load only "good" data from the input 'model_data'.
   over_temp_cal->num_model_pts = NANO_MIN(data_length, OTC_MODEL_SIZE);
-  size_t i;
   size_t valid_data_count = 0;
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
     if (isValidOtcOffset(model_data[i].offset,
                          model_data[i].offset_temp_celsius)) {
       memcpy(&over_temp_cal->model_data[i], &model_data[i],
@@ -553,7 +502,7 @@
                           float *compensated_offset_temperature_celsius,
                           float *compensated_offset) {
   memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
-         3 * sizeof(float));
+         sizeof(over_temp_cal->compensated_offset.offset));
   *compensated_offset_temperature_celsius =
       over_temp_cal->compensated_offset.offset_temp_celsius;
 }
@@ -638,7 +587,7 @@
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
           "Offset|Temperature|Time [%s|C|nsec]: "
-          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
+          CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
           over_temp_cal->otc_unit_tag,
           CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
           CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
@@ -674,8 +623,7 @@
   //          Check condition:
   //          temp_lo_check <= model_data[i].offset_temp_celsius < temp_hi_check
   bool replaced_one = false;
-  size_t i = 0;
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
     if (over_temp_cal->model_data[i].offset_temp_celsius < temp_hi_check &&
         over_temp_cal->model_data[i].offset_temp_celsius >= temp_lo_check) {
       // NOTE - The pointer to the new model data point is set here; the offset
@@ -688,7 +636,7 @@
 
   // NOTE - The pointer to the new model data point is set here; the offset
   // data is set below in the call to 'setLatestEstimate'.
-  if (!replaced_one && over_temp_cal->num_model_pts < OTC_MODEL_SIZE) {
+  if (!replaced_one) {
     if (over_temp_cal->num_model_pts < OTC_MODEL_SIZE) {
       // 3) If nothing was replaced, and the 'model_data' buffer is not full
       //    then add the estimate data to the array.
@@ -699,7 +647,7 @@
       // 4) Otherwise (nothing was replaced and buffer is full), replace the
       //    oldest data with the incoming one.
       over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
-      for (i = 1; i < over_temp_cal->num_model_pts; i++) {
+      for (size_t i = 1; i < over_temp_cal->num_model_pts; i++) {
         if (over_temp_cal->latest_offset->timestamp_nanos <
             over_temp_cal->model_data[i].timestamp_nanos) {
           over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
@@ -757,7 +705,7 @@
     // Prints out temperature and the current timestamp.
     createDebugTag(over_temp_cal, ":TEMP]");
     CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                  "Temperature|Time [C|nsec] = %s%d.%03d, %llu",
+                  "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS ", %llu",
                   CAL_ENCODE_FLOAT(temperature_celsius, 3),
                   (unsigned long long int)timestamp_nanos);
   }
@@ -805,13 +753,11 @@
   ASSERT_NOT_NULL(sensor_intercept);
   ASSERT_NOT_NULL(max_error);
 
-  size_t i;
-  size_t j;
   float max_error_test;
   memset(max_error, 0, 3 * sizeof(float));
 
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
-    for (j = 0; j < 3; j++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
+    for (size_t j = 0; j < 3; j++) {
       max_error_test =
           NANO_ABS(over_temp_cal->model_data[i].offset[j] -
                    (temp_sensitivity[j] *
@@ -824,16 +770,13 @@
   }
 }
 
-// TODO: Refactor to implement a compliance check on the storage of
+// TODO(davejacobs): Refactor to implement a compliance check on the storage of
 // 'offset_age_nanos' to ensure a monotonically increasing order with index.
-void overTempSetWeightingFunction(struct OverTempCal *over_temp_cal,
-                                  size_t index,
-                                  uint64_t offset_age_nanos,
-                                  float weight) {
+void overTempSetWeightingFunction(
+    struct OverTempCal *over_temp_cal, size_t index,
+    const struct OverTempCalWeightPt *new_otc_weight) {
   if (index < OTC_NUM_WEIGHT_LEVELS) {
-    over_temp_cal->weighting_function[index].offset_age_nanos =
-        offset_age_nanos;
-    over_temp_cal->weighting_function[index].weight = weight;
+    over_temp_cal->weighting_function[index] = *new_otc_weight;
   }
 }
 
@@ -847,10 +790,9 @@
   // Defaults to using the current compensated offset value.
   float compensated_offset[3];
   memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
-         3 * sizeof(float));
+         sizeof(over_temp_cal->compensated_offset.offset));
 
-  size_t index;
-  for (index = 0; index < 3; index++) {
+  for (size_t index = 0; index < 3; index++) {
     if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
       // If a valid axis model is defined then the default compensation will
       // use the linear model:
@@ -875,8 +817,7 @@
 
   // Adds a delta term to the 'compensated_offset' using the temperature
   // difference defined by 'delta_temp_celsius'.
-  size_t index;
-  for (index = 0; index < 3; index++) {
+  for (size_t index = 0; index < 3; index++) {
     if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
       // If a valid axis model is defined, then use the linear model to assist
       // with computing an extrapolated compensation term.
@@ -894,7 +835,7 @@
 
   // Uses the most recent offset estimate for offset compensation.
   float compensated_offset[3];
-  memcpy(compensated_offset, estimate->offset, 3 * sizeof(float));
+  memcpy(compensated_offset, estimate->offset, sizeof(compensated_offset));
 
   // Checks that the offset temperature is valid.
   if (estimate->offset_temp_celsius > OTC_TEMP_INVALID_CELSIUS) {
@@ -922,12 +863,11 @@
   // The default compensated offset is the nearest-temperature offset vector.
   float compensated_offset[3];
   memcpy(compensated_offset, over_temp_cal->nearest_offset->offset,
-         3 * sizeof(float));
+         sizeof(compensated_offset));
   const float compensated_offset_temperature_celsius =
       over_temp_cal->nearest_offset->offset_temp_celsius;
 
-  size_t index;
-  for (index = 0; index < 3; index++) {
+  for (size_t index = 0; index < 3; index++) {
     if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
       // If a valid axis model is defined, then use the linear model to assist
       // with computing an extrapolated compensation term.
@@ -1099,9 +1039,8 @@
 
   // If the 'compensated_offset' value has changed significantly, then set
   // 'new_overtemp_offset_available' true.
-  size_t i;
   bool new_overtemp_offset_available = false;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (NANO_ABS(over_temp_cal->compensated_offset.offset[i] -
                  compensated_offset[i]) >=
         over_temp_cal->significant_offset_change) {
@@ -1115,7 +1054,7 @@
   // vector and timestamp are updated.
   if (new_overtemp_offset_available) {
     memcpy(over_temp_cal->compensated_offset.offset, compensated_offset,
-           3 * sizeof(float));
+           sizeof(over_temp_cal->compensated_offset.offset));
     over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
     over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;
   }
@@ -1128,7 +1067,8 @@
 
   if (over_temp_cal->latest_offset) {
     // Sets the latest over-temp calibration estimate.
-    memcpy(over_temp_cal->latest_offset->offset, offset, 3 * sizeof(float));
+    memcpy(over_temp_cal->latest_offset->offset, offset,
+           sizeof(over_temp_cal->latest_offset->offset));
     over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius;
     over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
   }
@@ -1178,9 +1118,8 @@
   // set. Otherwise, a lockout condition could occur where the entire model
   // data set would need to be replaced in order to bring the model fit error
   // below the error limit and allow a successful model update.
-  size_t i;
   bool updated_one = false;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
                               sensor_intercept[i])) {
       over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
@@ -1191,7 +1130,8 @@
       createDebugTag(over_temp_cal, ":REJECT]");
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "%c-Axis Parameters|Time [%s/C|%s|nsec]: %s%d.%03d, %s%d.%03d, %llu",
+          "%c-Axis Parameters|Time [%s/C|%s|nsec]: " CAL_FORMAT_3DIGITS
+          ", " CAL_FORMAT_3DIGITS ", %llu",
           kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
           over_temp_cal->otc_unit_tag,
           CAL_ENCODE_FLOAT(
@@ -1227,11 +1167,10 @@
 
   // Performs a brute force search for the estimate nearest
   // 'temperature_celsius'.
-  size_t i = 0;
   float dtemp_new = 0.0f;
   float dtemp_old = FLT_MAX;
   over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
     dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp_celsius -
                          temperature_celsius);
     if (dtemp_new < dtemp_old) {
@@ -1245,9 +1184,8 @@
                           uint64_t timestamp_nanos) {
   ASSERT_NOT_NULL(over_temp_cal);
 
-  size_t i;
   bool removed_one = false;
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
     if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
         timestamp_nanos > over_temp_cal->age_limit_nanos +
                               over_temp_cal->model_data[i].timestamp_nanos) {
@@ -1285,8 +1223,8 @@
   createDebugTag(over_temp_cal, ":REMOVE]");
   CAL_DEBUG_LOG(
       over_temp_cal->otc_debug_tag,
-      "Offset|Temp|Time [%s|C|nsec]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-      "%s%d.%03d, %llu",
+      "Offset|Temp|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+      ", " CAL_FORMAT_3DIGITS ", %llu",
       over_temp_cal->otc_unit_tag,
       CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
                            over_temp_cal->otc_unit_conversion,
@@ -1304,8 +1242,7 @@
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
   // Remove the model data at 'model_index'.
-  size_t i;
-  for (i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
+  for (size_t i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
     memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1],
            sizeof(struct OverTempCalDataPt));
   }
@@ -1328,8 +1265,7 @@
   // complete (i.e., x, y, z values are all provided). Therefore, the jumpstart
   // data produced here requires that the model parameters have all been fully
   // defined and are all within the valid range.
-  size_t i;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (!isValidOtcLinearModel(over_temp_cal,
                                over_temp_cal->temp_sensitivity[i],
                                over_temp_cal->sensor_intercept[i])) {
@@ -1348,9 +1284,8 @@
   float offset_temp_celsius =
       (start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;
 
-  size_t j;
-  for (i = 0; i < over_temp_cal->min_num_model_pts; i++) {
-    for (j = 0; j < 3; j++) {
+  for (size_t i = 0; i < over_temp_cal->min_num_model_pts; i++) {
+    for (size_t j = 0; j < 3; j++) {
       over_temp_cal->model_data[i].offset[j] =
           over_temp_cal->temp_sensitivity[j] * offset_temp_celsius +
           over_temp_cal->sensor_intercept[j];
@@ -1391,8 +1326,7 @@
 
   // First pass computes the weighted mean values.
   const size_t n = over_temp_cal->num_model_pts;
-  size_t i = 0;
-  for (i = 0; i < n; ++i) {
+  for (size_t i = 0; i < n; ++i) {
     weight = evaluateWeightingFunction(
         over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
         timestamp_nanos);
@@ -1407,7 +1341,7 @@
   // Second pass computes the mean corrected second moment values.
   ASSERT(sw > 0.0f);
   const float inv_sw = 1.0f / sw;
-  for (i = 0; i < n; ++i) {
+  for (size_t i = 0; i < n; ++i) {
     weight = evaluateWeightingFunction(
         over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
         timestamp_nanos);
@@ -1453,9 +1387,87 @@
   return false;
 }
 
+void resetOtcLinearModel(struct OverTempCal *over_temp_cal) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  // Sets the temperature sensitivity model parameters to
+  // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial"
+  // state.
+  over_temp_cal->temp_sensitivity[0] = OTC_INITIAL_SENSITIVITY;
+  over_temp_cal->temp_sensitivity[1] = OTC_INITIAL_SENSITIVITY;
+  over_temp_cal->temp_sensitivity[2] = OTC_INITIAL_SENSITIVITY;
+  memset(over_temp_cal->sensor_intercept, 0,
+         sizeof(over_temp_cal->sensor_intercept));
+}
+
+bool checkAndEnforceTemperatureRange(float *temperature_celsius) {
+  if (*temperature_celsius > OTC_TEMP_MAX_CELSIUS) {
+    *temperature_celsius = OTC_TEMP_MAX_CELSIUS;
+    return false;
+  }
+  if (*temperature_celsius < OTC_TEMP_MIN_CELSIUS) {
+    *temperature_celsius = OTC_TEMP_MIN_CELSIUS;
+    return false;
+  }
+  return true;
+}
+
+bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
+                           float temp_sensitivity, float sensor_intercept) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  return NANO_ABS(temp_sensitivity) < over_temp_cal->temp_sensitivity_limit &&
+         NANO_ABS(sensor_intercept) < over_temp_cal->sensor_intercept_limit &&
+         NANO_ABS(temp_sensitivity) > OTC_MODELDATA_NEAR_ZERO_TOL &&
+         NANO_ABS(sensor_intercept) > OTC_MODELDATA_NEAR_ZERO_TOL;
+}
+
+bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
+  ASSERT_NOT_NULL(offset);
+
+  // Simple check to ensure that:
+  //   1. All of the input data is non "zero".
+  //   2. The offset temperature is within the valid range.
+  if (NANO_ABS(offset[0]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+      NANO_ABS(offset[1]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+      NANO_ABS(offset[2]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+      NANO_ABS(offset_temp_celsius) < OTC_MODELDATA_NEAR_ZERO_TOL) {
+    return false;
+  }
+
+  // Only returns the "check" result. Don't care about coercion.
+  return checkAndEnforceTemperatureRange(&offset_temp_celsius);
+}
+
+float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
+                                uint64_t offset_timestamp_nanos,
+                                uint64_t current_timestamp_nanos) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
+    if (current_timestamp_nanos <=
+        offset_timestamp_nanos +
+            over_temp_cal->weighting_function[i].offset_age_nanos) {
+      return over_temp_cal->weighting_function[i].weight;
+    }
+  }
+
+  // Returning the default weight for all older offsets.
+  return OTC_MIN_WEIGHT_VALUE;
+}
+
 /////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
+void createDebugTag(struct OverTempCal *over_temp_cal,
+                    const char *new_debug_tag) {
+  over_temp_cal->otc_debug_tag[0] = '[';
+  memcpy(over_temp_cal->otc_debug_tag + 1, over_temp_cal->otc_sensor_tag,
+         strlen(over_temp_cal->otc_sensor_tag));
+  memcpy(
+      over_temp_cal->otc_debug_tag + strlen(over_temp_cal->otc_sensor_tag) + 1,
+      new_debug_tag, strlen(new_debug_tag) + 1);
+}
+
 void updateDebugData(struct OverTempCal* over_temp_cal) {
   ASSERT_NOT_NULL(over_temp_cal);
 
@@ -1473,8 +1485,7 @@
   memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal));
 
   // Copies over the relevant data.
-  size_t i;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (isValidOtcLinearModel(over_temp_cal, over_temp_cal->temp_sensitivity[i],
                               over_temp_cal->sensor_intercept[i])) {
       over_temp_cal->debug_overtempcal.temp_sensitivity[i] =
@@ -1543,8 +1554,8 @@
       // Prints out the latest offset estimate (input data).
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, %s%d.%03d, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
+          "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+          ", " CAL_FORMAT_3DIGITS ", %llu",
           over_temp_cal->otc_unit_tag,
           (unsigned long int)over_temp_cal->debug_num_estimates,
           CAL_ENCODE_FLOAT(
@@ -1576,7 +1587,7 @@
       // Prints out the model parameters.
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "Cal#|Sensitivity [%s/C]: %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+          "Cal#|Sensitivity [%s/C]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
           over_temp_cal->otc_unit_tag,
           (unsigned long int)over_temp_cal->debug_num_estimates,
           CAL_ENCODE_FLOAT(
@@ -1593,7 +1604,7 @@
               3));
 
       CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                    "Cal#|Intercept [%s]: %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+                    "Cal#|Intercept [%s]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
                     over_temp_cal->otc_unit_tag,
                     (unsigned long int)over_temp_cal->debug_num_estimates,
                     CAL_ENCODE_FLOAT(
@@ -1620,7 +1631,7 @@
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
           "Cal#|#Updates|#ModelPts|Model Error [%s]: %lu, "
-          "%lu, %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+          "%lu, %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
           over_temp_cal->otc_unit_tag,
           (unsigned long int)over_temp_cal->debug_num_estimates,
           (unsigned long int)over_temp_cal->debug_num_model_updates,
@@ -1647,8 +1658,8 @@
       if (over_temp_cal->model_counter < over_temp_cal->num_model_pts) {
         CAL_DEBUG_LOG(
             over_temp_cal->otc_debug_tag,
-            "  Model[%lu] [%s|C|nsec] = %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-            "%s%d.%03d, %llu",
+            "  Model[%lu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
+            ", " CAL_FORMAT_3DIGITS ", %llu",
             (unsigned long int)over_temp_cal->model_counter,
             over_temp_cal->otc_unit_tag,
             CAL_ENCODE_FLOAT(
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
index 81b2173..8a404d3 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -130,17 +130,26 @@
 #define OTC_NUM_WEIGHT_LEVELS (2)
 
 // Rate-limits the check of old data to every 2 hours.
-#define OTC_STALE_CHECK_TIME_NANOS (7200000000000)
+#define OTC_STALE_CHECK_TIME_NANOS (HRS_TO_NANOS(2))
 
 // Time duration in which to enforce using the last offset estimate for
 // compensation (30 seconds).
-#define OTC_USE_RECENT_OFFSET_TIME_NANOS (30000000000)
+#define OTC_USE_RECENT_OFFSET_TIME_NANOS (SEC_TO_NANOS(30))
 
 // The age at which an offset estimate is considered stale (30 minutes).
-#define OTC_OFFSET_IS_STALE_NANOS (1800000000000)
+#define OTC_OFFSET_IS_STALE_NANOS (MIN_TO_NANOS(30))
 
 // The refresh interval for the OTC model (30 seconds).
-#define OTC_REFRESH_MODEL_NANOS (30000000000)
+#define OTC_REFRESH_MODEL_NANOS (SEC_TO_NANOS(30))
+
+// Defines a weighting function value for the linear model fit routine.
+struct OverTempCalWeightPt {
+  // The age limit below which an offset will use this weight value.
+  uint64_t offset_age_nanos;
+
+  // The weighting applied (>0).
+  float weight;
+};
 
 // Over-temperature sensor offset estimate structure.
 struct OverTempCalDataPt {
@@ -150,15 +159,6 @@
   float offset[3];
 };
 
-// Weighting data used to improve the quality of the linear model fit.
-struct OverTempCalWeightPt {
-  // Offset age below which this weight applies.
-  uint64_t offset_age_nanos;
-
-  // Weighting value for offset estimates more recent than 'offset_age_nanos'.
-  float weight;
-};
-
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // Debug printout state enumeration.
 enum OverTempCalDebugState {
@@ -532,14 +532,13 @@
  * INPUTS:
  *   over_temp_cal:    Over-temp data structure.
  *   index:            Weighting function index.
- *   offset_age_nanos: The age limit below which an offset will use this weight
- *                     value.
- *   weight:           The weighting applied (>0).
+ *   new_otc_weight:   Pointer to the settings for the new non-zero weighting
+ *                     value and corresponding age limit below which an offset
+ *                     will use the weight.
  */
-void overTempSetWeightingFunction(struct OverTempCal *over_temp_cal,
-                                  size_t index,
-                                  uint64_t offset_age_nanos,
-                                  float weight);
+void overTempSetWeightingFunction(
+    struct OverTempCal *over_temp_cal, size_t index,
+    const struct OverTempCalWeightPt *new_otc_weight);
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // This debug printout function assumes the input sensor data is a gyroscope
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index f2e711f..1bd80fd 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -54,6 +54,13 @@
   ((x < 0) ? "-" : ""),                 \
   (int)CAL_FLOOR(fabsf(x)), (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * powf(10, num_digits))  // NOLINT
 
+// Helper definitions for CAL_ENCODE_FLOAT to specify the print format with
+// desired significant digits.
+#define CAL_FORMAT_3DIGITS "%s%d.%03d"
+#define CAL_FORMAT_6DIGITS "%s%d.%06d"
+#define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
+#define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c
index 9c179ac..66e423f 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.c
+++ b/firmware/os/algos/common/math/levenberg_marquardt.c
@@ -4,6 +4,7 @@
 #include <stdio.h>
 #include <string.h>
 
+#include "common/math/macros.h"
 #include "common/math/mat.h"
 #include "common/math/vec.h"
 
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
new file mode 100644
index 0000000..5c06e24
--- /dev/null
+++ b/firmware/os/algos/common/math/macros.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This file contains helper macros and definitions.
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
+
+// Mathematical constants.
+#define NANO_PI (3.14159265359f)
+
+// Common math operations.
+#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
+#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
+#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
+
+// Timestamp conversion macros.
+#ifdef __cplusplus
+#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x) * 1000000)
+#else
+#define MSEC_TO_NANOS(x) ((uint64_t)(x) * 1000000)  // NOLINT
+#endif
+
+#define SEC_TO_NANOS(x)  MSEC_TO_NANOS(x * 1000)
+#define MIN_TO_NANOS(x)  SEC_TO_NANOS(x * 60)
+#define HRS_TO_NANOS(x)  MIN_TO_NANOS(x * 60)
+#define DAYS_TO_NANOS(x) HRS_TO_NANOS(x * 24)
+
+// Unit conversion: nanoseconds to seconds.
+#define NANOS_TO_SEC (1.0e-9f)
+
+// Unit conversion: milli-degrees to radians.
+#define MDEG_TO_RAD (NANO_PI / 180.0e3f)
+
+// Unit conversion: radians to milli-degrees.
+#define RAD_TO_MDEG (180.0e3f / NANO_PI)
+
+// Time check helper macro that returns true if:
+//    i.  't1' is equal to or exceeds 't2' plus 't_delta'.
+//    ii. Or, a negative timestamp delta occurred since,
+//        't1' should always >= 't2'. This prevents potential lockout conditions
+//        if the timer count 't1' rolls over or an erroneously large
+//        timestamp is passed through.
+#define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
+  (((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c
index b55ea8e..bb9f929 100644
--- a/firmware/os/algos/common/math/vec.c
+++ b/firmware/os/algos/common/math/vec.c
@@ -15,6 +15,7 @@
  */
 
 #include "common/math/vec.h"
+#include "common/math/macros.h"
 
 void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
                           float *outY, float *outZ) {
diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h
index 49f197c..0a4c8b3 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -53,23 +53,6 @@
   float x, y, z, w;
 };
 
-#define NANO_PI (3.14159265359f)
-
-#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
-
-#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
-
-#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
-
-// Time check helper macro that returns true if:
-//    i.  't1' is equal to or exceeds 't2' plus 't_delta'.
-//    ii. Or, a negative timestamp delta occurred since,
-//        't1' should always >= 't2'. This prevents potential lockout conditions
-//        if the timer count 't1' rolls over or an erroneously large
-//        timestamp is passed through.
-#define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
-  (((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
-
 // 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
 static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
   ASSERT_NOT_NULL(v);
diff --git a/firmware/os/core/nanohubCommand.c b/firmware/os/core/nanohubCommand.c
index 439f3dc..79d8804 100644
--- a/firmware/os/core/nanohubCommand.c
+++ b/firmware/os/core/nanohubCommand.c
@@ -1078,7 +1078,7 @@
     uint64_t appId;
     uint32_t appVer, appSize;
 
-    if (osAppInfoByIndex(le32toh(req->idx), &appId, &appVer, &appSize)) {
+    if (osExtAppInfoByIndex(le32toh(req->idx), &appId, &appVer, &appSize)) {
         resp = heapAlloc(sizeof(*resp));
         if (resp) {
             resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
diff --git a/firmware/os/core/nanohub_chre.c b/firmware/os/core/nanohub_chre.c
index 6dd32d6..b8e6825 100644
--- a/firmware/os/core/nanohub_chre.c
+++ b/firmware/os/core/nanohub_chre.c
@@ -102,7 +102,7 @@
 {
     uint64_t *timeNanos = va_arg(args, uint64_t *);
     if (timeNanos)
-        *timeNanos = timGetTime();
+        *timeNanos = sensorGetTime();
 }
 
 static inline uint32_t osChreTimerSet(uint64_t duration, const void* cookie, bool oneShot)
diff --git a/firmware/os/core/osApi.c b/firmware/os/core/osApi.c
index 1716f09..8361d9a 100644
--- a/firmware/os/core/osApi.c
+++ b/firmware/os/core/osApi.c
@@ -178,7 +178,7 @@
     *retValP = sensorTriggerOndemand(0, sensorHandle);
 }
 
-static void osExpApiSensorGetRate(uintptr_t *retValP, va_list args)
+static void osExpApiSensorGetCurRate(uintptr_t *retValP, va_list args)
 {
     uint32_t sensorHandle = va_arg(args, uint32_t);
 
@@ -191,6 +191,13 @@
     *timeNanos = sensorGetTime();
 }
 
+static void osExpApiSensorGetReqRate(uintptr_t *retValP, va_list args)
+{
+    uint32_t sensorHandle = va_arg(args, uint32_t);
+
+    *retValP = sensorGetReqRate(sensorHandle);
+}
+
 static void osExpApiTimGetTime(uintptr_t *retValP, va_list args)
 {
     uint64_t *timeNanos = va_arg(args, uint64_t *);
@@ -482,9 +489,9 @@
     static const struct SyscallTable osMainEvtqTable = {
         .numEntries = SYSCALL_OS_MAIN_EVTQ_LAST,
         .entry = {
-            [SYSCALL_OS_MAIN_EVTQ_SUBCRIBE]        = { .func = osExpApiEvtqSubscribe,   },
-            [SYSCALL_OS_MAIN_EVTQ_UNSUBCRIBE]      = { .func = osExpApiEvtqUnsubscribe, },
-            [SYSCALL_OS_MAIN_EVTQ_ENQUEUE]         = { .func = osExpApiEvtqEnqueue,     },
+            [SYSCALL_OS_MAIN_EVTQ_SUBCRIBE]        = { .func = osExpApiEvtqSubscribe,      },
+            [SYSCALL_OS_MAIN_EVTQ_UNSUBCRIBE]      = { .func = osExpApiEvtqUnsubscribe,    },
+            [SYSCALL_OS_MAIN_EVTQ_ENQUEUE]         = { .func = osExpApiEvtqEnqueue,        },
             [SYSCALL_OS_MAIN_EVTQ_ENQUEUE_PRIVATE] = { .func = osExpApiEvtqEnqueuePrivate, },
             [SYSCALL_OS_MAIN_EVTQ_RETAIN_EVT]      = { .func = osExpApiEvtqRetainEvt,      },
             [SYSCALL_OS_MAIN_EVTQ_FREE_RETAINED]   = { .func = osExpApiEvtqFreeRetained,   },
@@ -501,17 +508,18 @@
     static const struct SyscallTable osMainSensorsTable = {
         .numEntries = SYSCALL_OS_MAIN_SENSOR_LAST,
         .entry = {
-            [SYSCALL_OS_MAIN_SENSOR_SIGNAL]        = { .func = osExpApiSensorSignal,  },
-            [SYSCALL_OS_MAIN_SENSOR_REG]           = { .func = osExpApiSensorReg,     },
-            [SYSCALL_OS_MAIN_SENSOR_UNREG]         = { .func = osExpApiSensorUnreg,   },
+            [SYSCALL_OS_MAIN_SENSOR_SIGNAL]        = { .func = osExpApiSensorSignal,     },
+            [SYSCALL_OS_MAIN_SENSOR_REG]           = { .func = osExpApiSensorReg,        },
+            [SYSCALL_OS_MAIN_SENSOR_UNREG]         = { .func = osExpApiSensorUnreg,      },
             [SYSCALL_OS_MAIN_SENSOR_REG_INIT_COMP] = { .func = osExpApiSensorRegInitComp },
-            [SYSCALL_OS_MAIN_SENSOR_FIND]          = { .func = osExpApiSensorFind,    },
-            [SYSCALL_OS_MAIN_SENSOR_REQUEST]       = { .func = osExpApiSensorReq,     },
-            [SYSCALL_OS_MAIN_SENSOR_RATE_CHG]      = { .func = osExpApiSensorRateChg, },
-            [SYSCALL_OS_MAIN_SENSOR_RELEASE]       = { .func = osExpApiSensorRel,     },
-            [SYSCALL_OS_MAIN_SENSOR_TRIGGER]       = { .func = osExpApiSensorTrigger, },
-            [SYSCALL_OS_MAIN_SENSOR_GET_RATE]      = { .func = osExpApiSensorGetRate, },
-            [SYSCALL_OS_MAIN_SENSOR_GET_TIME]      = { .func = osExpApiSensorGetTime, },
+            [SYSCALL_OS_MAIN_SENSOR_FIND]          = { .func = osExpApiSensorFind,       },
+            [SYSCALL_OS_MAIN_SENSOR_REQUEST]       = { .func = osExpApiSensorReq,        },
+            [SYSCALL_OS_MAIN_SENSOR_RATE_CHG]      = { .func = osExpApiSensorRateChg,    },
+            [SYSCALL_OS_MAIN_SENSOR_RELEASE]       = { .func = osExpApiSensorRel,        },
+            [SYSCALL_OS_MAIN_SENSOR_TRIGGER]       = { .func = osExpApiSensorTrigger,    },
+            [SYSCALL_OS_MAIN_SENSOR_GET_CUR_RATE]  = { .func = osExpApiSensorGetCurRate, },
+            [SYSCALL_OS_MAIN_SENSOR_GET_TIME]      = { .func = osExpApiSensorGetTime,    },
+            [SYSCALL_OS_MAIN_SENSOR_GET_REQ_RATE]  = { .func = osExpApiSensorGetReqRate, },
 
         },
     };
@@ -519,9 +527,9 @@
     static const struct SyscallTable osMainTimerTable = {
         .numEntries = SYSCALL_OS_MAIN_TIME_LAST,
         .entry = {
-            [SYSCALL_OS_MAIN_TIME_GET_TIME]     = { .func = osExpApiTimGetTime,  },
-            [SYSCALL_OS_MAIN_TIME_SET_TIMER]    = { .func = osExpApiTimSetTimer,     },
-            [SYSCALL_OS_MAIN_TIME_CANCEL_TIMER] = { .func = osExpApiTimCancelTimer,   },
+            [SYSCALL_OS_MAIN_TIME_GET_TIME]     = { .func = osExpApiTimGetTime,     },
+            [SYSCALL_OS_MAIN_TIME_SET_TIMER]    = { .func = osExpApiTimSetTimer,    },
+            [SYSCALL_OS_MAIN_TIME_CANCEL_TIMER] = { .func = osExpApiTimCancelTimer, },
         },
     };
 
@@ -529,17 +537,17 @@
         .numEntries = SYSCALL_OS_MAIN_HEAP_LAST,
         .entry = {
             [SYSCALL_OS_MAIN_HEAP_ALLOC] = { .func = osExpApiHeapAlloc },
-            [SYSCALL_OS_MAIN_HEAP_FREE]  = { .func = osExpApiHeapFree },
+            [SYSCALL_OS_MAIN_HEAP_FREE]  = { .func = osExpApiHeapFree  },
         },
     };
 
     static const struct SyscallTable osMainSlabTable = {
         .numEntries = SYSCALL_OS_MAIN_SLAB_LAST,
         .entry = {
-            [SYSCALL_OS_MAIN_SLAB_NEW]     = { .func = osExpApiSlabNew },
+            [SYSCALL_OS_MAIN_SLAB_NEW]     = { .func = osExpApiSlabNew     },
             [SYSCALL_OS_MAIN_SLAB_DESTROY] = { .func = osExpApiSlabDestroy },
-            [SYSCALL_OS_MAIN_SLAB_ALLOC]   = { .func = osExpApiSlabAlloc },
-            [SYSCALL_OS_MAIN_SLAB_FREE]    = { .func = osExpApiSlabFree },
+            [SYSCALL_OS_MAIN_SLAB_ALLOC]   = { .func = osExpApiSlabAlloc   },
+            [SYSCALL_OS_MAIN_SLAB_FREE]    = { .func = osExpApiSlabFree    },
         },
     };
 
diff --git a/firmware/os/core/sensors.c b/firmware/os/core/sensors.c
index 877e0a2..b8caec5 100644
--- a/firmware/os/core/sensors.c
+++ b/firmware/os/core/sensors.c
@@ -526,6 +526,7 @@
     if (s && s->currentRate != SENSOR_RATE_OFF && s->currentRate < SENSOR_RATE_POWERING_ON) {
         s->currentRate = evt->value1;
         s->currentLatency = evt->value2;
+        sensorReconfig(s, sensorCalcHwRate(s, 0, 0), sensorCalcHwLatency(s));
         osEnqueueEvtOrFree(sensorGetMyCfgEventType(s->si->sensorType), evt, sensorInternalEvtFreeF);
     } else {
         slabAllocatorFree(mInternalEvents, evt);
diff --git a/firmware/os/core/seos.c b/firmware/os/core/seos.c
index f220dc3..52ccf7a 100644
--- a/firmware/os/core/seos.c
+++ b/firmware/os/core/seos.c
@@ -1360,6 +1360,28 @@
     return false;
 }
 
+bool osExtAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize)
+{
+    struct Task *task;
+    int i = 0;
+
+    for_each_task(&mTasks, task) {
+        const struct AppHdr *app = task->app;
+        if (!(app->hdr.fwFlags & FL_APP_HDR_INTERNAL)) {
+            if (i != appIdx) {
+                ++i;
+            } else {
+                *appId = app->hdr.appId;
+                *appVer = app->hdr.appVer;
+                *appSize = app->sect.rel_end;
+                return true;
+            }
+        }
+    }
+
+    return false;
+}
+
 void osLogv(char clevel, uint32_t flags, const char *str, va_list vl)
 {
     void *userData = platLogAllocUserData();
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index f27a9e9..513a9c8 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
@@ -61,6 +61,7 @@
 
 #ifdef GYRO_CAL_ENABLED
 #include <calibration/gyroscope/gyro_cal.h>
+#include <common/math/macros.h>
 #endif  // GYRO_CAL_ENABLED
 
 #if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
@@ -75,6 +76,10 @@
 #include <stdlib.h>
 #include <string.h>
 
+#define VERBOSE_PRINT(fmt, ...) do { \
+        osLog(LOG_VERBOSE, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \
+    } while (0);
+
 #define INFO_PRINT(fmt, ...) do { \
         osLog(LOG_INFO, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \
     } while (0);
@@ -85,13 +90,13 @@
 
 #define DEBUG_PRINT(fmt, ...) do { \
         if (DBG_ENABLE) {  \
-            INFO_PRINT(fmt,  ##__VA_ARGS__); \
+            osLog(LOG_DEBUG, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \
         } \
     } while (0);
 
 #define DEBUG_PRINT_IF(cond, fmt, ...) do { \
         if ((cond) && DBG_ENABLE) {  \
-            INFO_PRINT(fmt,  ##__VA_ARGS__); \
+            osLog(LOG_DEBUG, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \
         } \
     } while (0);
 
@@ -103,7 +108,7 @@
 #define DBG_WM_CALC               0
 #define TIMESTAMP_DBG             0
 
-#define BMI160_APP_VERSION 15
+#define BMI160_APP_VERSION 17
 
 // fixme: to list required definitions for a slave mag
 #ifdef USE_BMM150
@@ -1313,7 +1318,7 @@
 {
     TDECL();
 
-    INFO_PRINT("accPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
+    VERBOSE_PRINT("accPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
         if (on) {
             // set ACC power mode to NORMAL
@@ -1336,7 +1341,7 @@
 static bool gyrPower(bool on, void *cookie)
 {
     TDECL();
-    INFO_PRINT("gyrPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
+    VERBOSE_PRINT("gyrPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
 
     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
         if (on) {
@@ -1369,7 +1374,7 @@
 static bool magPower(bool on, void *cookie)
 {
     TDECL();
-    INFO_PRINT("magPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
+    VERBOSE_PRINT("magPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
         if (on) {
             // set MAG power mode to NORMAL
@@ -1665,7 +1670,7 @@
     TDECL();
     int odr, osr = 0;
     int osr_mode = 2; // normal
-    INFO_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n",
+    VERBOSE_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n",
                rate, latency, getStateName(GET_STATE()));
 
     if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
@@ -1732,7 +1737,7 @@
     if (rate == SENSOR_RATE_ONCHANGE)
         rate = SENSOR_HZ(100);
 
-    INFO_PRINT("magSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n",
+    VERBOSE_PRINT("magSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n",
                rate, latency, getStateName(GET_STATE()));
 
     if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
@@ -2203,25 +2208,16 @@
       }
 
 #ifdef OVERTEMPCAL_ENABLED
-      // OTC-Gyro Cal -- A timer is used to limit the frequency of the offset
-      // update checks.
-      static uint64_t imu_new_otc_offset_timer = 0;  // nanoseconds
-      bool new_otc_offset_update = false;
-      bool new_otc_model_update = false;
-      if ((rtc_time - imu_new_otc_offset_timer) >= 500000000) {
-        imu_new_otc_offset_timer = rtc_time;
+      // OTC-Gyro Cal --  Gets the latest OTC-Gyro temperature compensated
+      // offset estimate.
+      bool new_otc_offset_update =
+          overTempCalNewOffsetAvailable(&mTask.over_temp_gyro_cal);
+      overTempCalGetOffset(&mTask.over_temp_gyro_cal,
+                           &gyro_offset_temperature_celsius, gyro_offset);
 
-        // OTC-Gyro Cal --  Gets the latest OTC-Gyro temperature compensated
-        // offset estimate.
-        new_otc_offset_update =
-            overTempCalNewOffsetAvailable(&mTask.over_temp_gyro_cal);
-        overTempCalGetOffset(&mTask.over_temp_gyro_cal,
-                             &gyro_offset_temperature_celsius, gyro_offset);
-
-        // OTC-Gyro Cal --  Checks for a model update.
-        new_otc_model_update =
-            overTempCalNewModelUpdateAvailable(&mTask.over_temp_gyro_cal);
-      }
+      // OTC-Gyro Cal --  Checks for a model update.
+      bool new_otc_model_update =
+          overTempCalNewModelUpdateAvailable(&mTask.over_temp_gyro_cal);
 
       if (new_otc_offset_update) {
 #else   // OVERTEMPCAL_ENABLED
@@ -3853,38 +3849,40 @@
 
 #ifdef GYRO_CAL_ENABLED
     // Gyro Cal -- Initialization.
-    gyroCalInit(
-        &mTask.gyro_cal,
-        5e9,                       // min stillness period = 5 seconds
-        6e9,                       // max stillness period = 6 seconds
-        0, 0, 0,                   // initial bias offset calibration
-        0,                         // time stamp of initial bias calibration
-        1.5e9,                     // analysis window length = 1.5 seconds
-        7.5e-5f,                   // gyroscope variance threshold [rad/sec]^2
-        1.5e-5f,                   // gyroscope confidence delta [rad/sec]^2
-        4.5e-3f,                   // accelerometer variance threshold [m/sec^2]^2
-        9.0e-4f,                   // accelerometer confidence delta [m/sec^2]^2
-        5.0f,                      // magnetometer variance threshold [uT]^2
-        1.0f,                      // magnetometer confidence delta [uT]^2
-        0.95f,                     // stillness threshold [0,1]
-        40.0e-3f * M_PI / 180.0f,  // stillness mean variation limit [rad/sec]
-        1.5f,                      // maximum temperature deviation during stillness [C]
-        true);                     // gyro calibration enable
+    gyroCalInit(&mTask.gyro_cal,
+                SEC_TO_NANOS(5.0f),   // Min stillness period = 5.0 seconds
+                SEC_TO_NANOS(5.9f),   // Max stillness period = 6.0 seconds (NOTE 1)
+                0, 0, 0,              // Initial bias offset calibration
+                0,                    // Time stamp of initial bias calibration
+                SEC_TO_NANOS(1.5f),   // Analysis window length = 1.5 seconds
+                7.5e-5f,              // Gyroscope variance threshold [rad/sec]^2
+                1.5e-5f,              // Gyroscope confidence delta [rad/sec]^2
+                4.5e-3f,              // Accelerometer variance threshold [m/sec^2]^2
+                9.0e-4f,              // Accelerometer confidence delta [m/sec^2]^2
+                5.0f,                 // Magnetometer variance threshold [uT]^2
+                1.0f,                 // Magnetometer confidence delta [uT]^2
+                0.95f,                // Stillness threshold [0,1]
+                40.0f * MDEG_TO_RAD,  // Stillness mean variation limit [rad/sec]
+                1.5f,                 // Max temperature delta during stillness [C]
+                true);                // Gyro calibration enable
+    // NOTE 1: This parameter is set to 5.9 seconds to achieve a max stillness
+    // period of 6.0 seconds and avoid buffer boundary conditions that could push
+    // the max stillness to the next multiple of the analysis window length
+    // (i.e., 7.5 seconds).
 
 #ifdef OVERTEMPCAL_ENABLED
     // Initialize over-temp calibration.
-    overTempCalInit(
-        &mTask.over_temp_gyro_cal,
-        5,                         // Min num of points to enable model update
-        5000000000,                // Min model update interval [nsec]
-        0.75f,                     // Temperature span of bin method [C]
-        50.0e-3f * M_PI / 180.0f,  // Model fit tolerance [rad/sec]
-        50.0e-3f * M_PI / 180.0f,  // Outlier rejection tolerance [rad/sec]
-        172800000000000,           // Model data point age limit [nsec]
-        50.0e-3f * M_PI / 180.0f,  // Limit for temp. sensitivity [rad/sec/C]
-        3.0f * M_PI / 180.0f,      // Limit for model intercept [rad/sec]
-        3.0e-3f * M_PI / 180.0f,   // Significant offset change [rad/sec]
-        true);                     // Over-temp compensation enable
+    overTempCalInit(&mTask.over_temp_gyro_cal,
+                    5,                     // Min num of points to enable model update
+                    SEC_TO_NANOS(0.5f),    // Min temperature update interval [nsec]
+                    0.75f,                 // Temperature span of bin method [C]
+                    40.0f * MDEG_TO_RAD,   // Jump tolerance [rad/sec]
+                    50.0f * MDEG_TO_RAD,   // Outlier rejection tolerance [rad/sec]
+                    DAYS_TO_NANOS(2),      // Model data point age limit [nsec]
+                    80.0f * MDEG_TO_RAD,   // Limit for temp. sensitivity [rad/sec/C]
+                    3.0e3f * MDEG_TO_RAD,  // Limit for model intercept parameter [rad/sec]
+                    0.1f * MDEG_TO_RAD,    // Significant offset change [rad/sec]
+                    true);                 // Over-temp compensation enable
 #endif  // OVERTEMPCAL_ENABLED
 #endif  // GYRO_CAL_ENABLED
 
@@ -3922,7 +3920,7 @@
     // XXX: this consumes too much memeory, need to optimize
     T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20);
     if (!T(mDataSlab)) {
-        INFO_PRINT("slabAllocatorNew() failed\n");
+        ERROR_PRINT("slabAllocatorNew() failed\n");
         return false;
     }
     T(mWbufCnt) = 0;
@@ -4199,10 +4197,10 @@
     for (i = 0; i < n; i++) {
         if (iPeriod[i] > 0) {
             anyActive = true;
-            size_t t =  minLatency / iPeriod[i];
+            size_t t = minLatency / iPeriod[i];
             head = t > head ? t : head;
             s += t * factor[i];
-            DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: %d, s+= %d*%d, head = %d", i, t, factor[i], head);
+            DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo %d: s += %d * %d, head = %d", i, t, factor[i], head);
         }
     }
 
@@ -4212,7 +4210,7 @@
 /**
  * Calculate the watermark setting from sensor registration information
  *
- * It is assumed  that all sensor period share a common denominator (true for BMI160) and the
+ * It is assumed that all sensor periods share a common denominator (true for BMI160) and the
  * latency of sensor will be lower bounded by its sampling period.
  *
  * @return watermark register setting
@@ -4224,12 +4222,12 @@
     int i;
 
     for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
-        if (T(sensors[i]).configed) {
+        if (T(sensors[i]).configed && T(sensors[i]).latency != SENSOR_LATENCY_NODATA) {
             period[i - ACC] = SENSOR_HZ((float)WATERMARK_MAX_SENSOR_RATE) / T(sensors[i]).rate;
             latency[i - ACC] = U64_DIV_BY_U64_CONSTANT(
                     T(sensors[i]).latency + WATERMARK_TIME_UNIT_NS/2, WATERMARK_TIME_UNIT_NS);
-            DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: f %dHz, l %dus => T %d unit, L %d unit",
-                    (int) T(sensors[i]).rate/1024,
+            DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2 %d: f %dHz, l %dus => T %d unit, L %d unit",
+                    i, (int) T(sensors[i]).rate/1024,
                     (int) U64_DIV_BY_U64_CONSTANT(T(sensors[i]).latency, 1000),
                     period[i-ACC], latency[i-ACC]);
         }
diff --git a/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c b/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c
index 8f2f16a..edb8fb2 100644
--- a/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c
+++ b/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c
@@ -139,10 +139,18 @@
 #define ROHM_RPR0521_MAX_PENDING_I2C_REQUESTS   4
 #define ROHM_RPR0521_MAX_I2C_TRANSFER_SIZE      16
 
+#define VERBOSE_PRINT(fmt, ...) do { \
+        osLog(LOG_VERBOSE, "[Rohm RPR-0521] " fmt, ##__VA_ARGS__); \
+    } while (0);
+
 #define INFO_PRINT(fmt, ...) do { \
         osLog(LOG_INFO, "[Rohm RPR-0521] " fmt, ##__VA_ARGS__); \
     } while (0);
 
+#define ERROR_PRINT(fmt, ...) do { \
+        osLog(LOG_ERROR, "[Rohm RPR-0521] " fmt, ##__VA_ARGS__); \
+    } while (0);
+
 #define DEBUG_PRINT(fmt, ...) do { \
         if (enable_debug) {  \
             osLog(LOG_INFO, "[Rohm RPR-0521] " fmt, ##__VA_ARGS__); \
@@ -298,7 +306,7 @@
 
     osEnqueuePrivateEvt(EVT_SENSOR_I2C, cookie, NULL, mTask.tid);
     if (err != 0)
-        INFO_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err);
+        ERROR_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err);
 }
 
 static void alsTimerCallback(uint32_t timerId, void *cookie)
@@ -385,7 +393,7 @@
 
 static bool sensorPowerAls(bool on, void *cookie)
 {
-    DEBUG_PRINT("sensorPowerAls: %d\n", on);
+    VERBOSE_PRINT("sensorPowerAls: %d\n", on);
 
     if (on && !mTask.alsTimerHandle) {
         mTask.alsTimerHandle = timTimerSet(ROHM_RPR0521_ALS_TIMER_DELAY, 0, 50, alsTimerCallback, NULL, false);
@@ -411,7 +419,7 @@
     if (rate == SENSOR_RATE_ONCHANGE)
         rate = ROHM_RPR0521_DEFAULT_RATE;
 
-    DEBUG_PRINT("sensorRateAls: rate=%ld Hz latency=%lld ns\n", rate/1024, latency);
+    VERBOSE_PRINT("sensorRateAls: rate=%ld Hz latency=%lld ns\n", rate/1024, latency);
 
     return sensorSignalInternalEvt(mTask.alsHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
 }
@@ -434,7 +442,7 @@
 
 static bool sensorPowerProx(bool on, void *cookie)
 {
-    DEBUG_PRINT("sensorPowerProx: %d\n", on);
+    VERBOSE_PRINT("sensorPowerProx: %d\n", on);
 
     if (on) {
         extiClearPendingGpio(mTask.pin);
@@ -461,7 +469,7 @@
     if (rate == SENSOR_RATE_ONCHANGE)
         rate = ROHM_RPR0521_DEFAULT_RATE;
 
-    DEBUG_PRINT("sensorRateProx: rate=%ld Hz latency=%lld ns\n", rate/1024, latency);
+    VERBOSE_PRINT("sensorRateProx: rate=%ld Hz latency=%lld ns\n", rate/1024, latency);
 
     return sensorSignalInternalEvt(mTask.proxHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
 }
@@ -475,8 +483,6 @@
 {
     struct I2cTransfer *xfer;
 
-    DEBUG_PRINT("sensorCfgDataProx");
-
     int32_t offset = *(int32_t*)data;
 
     INFO_PRINT("Received cfg data: %d\n", (int)offset);
diff --git a/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c b/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c
index df1afce..62f364a 100644
--- a/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c
+++ b/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c
@@ -76,10 +76,11 @@
 
 #define ENABLE_DEBUG 0
 
+#define VERBOSE_PRINT(fmt, ...) osLog(LOG_VERBOSE, "[DoubleTouch] " fmt, ##__VA_ARGS__)
 #define INFO_PRINT(fmt, ...) osLog(LOG_INFO, "[DoubleTouch] " fmt, ##__VA_ARGS__)
 #define ERROR_PRINT(fmt, ...) osLog(LOG_ERROR, "[DoubleTouch] " fmt, ##__VA_ARGS__)
 #if ENABLE_DEBUG
-#define DEBUG_PRINT(fmt, ...) INFO_PRINT(fmt, ##__VA_ARGS__)
+#define DEBUG_PRINT(fmt, ...)  osLog(LOG_DEBUG, "[DoubleTouch] " fmt, ##__VA_ARGS__)
 #else
 #define DEBUG_PRINT(fmt, ...) ((void)0)
 #endif
@@ -303,7 +304,7 @@
     bool ret;
     size_t i;
 
-    INFO_PRINT("gesture: %d", enable);
+    VERBOSE_PRINT("gesture: %d", enable);
 
     // Cancel any pending I2C transactions by changing the callback state
     for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) {
@@ -360,7 +361,7 @@
 {
     uint32_t enabledSeconds, proxEnabledSeconds, proxFarSeconds;
 
-    INFO_PRINT("power: %d", on);
+    VERBOSE_PRINT("power: %d", on);
 
     if (on) {
         mTask.stats.enabledTimestamp = sensorGetTime();
@@ -371,7 +372,7 @@
     enabledSeconds = U64_DIV_BY_U64_CONSTANT(mTask.stats.totalEnabledTime, 1000000000);
     proxEnabledSeconds = U64_DIV_BY_U64_CONSTANT(mTask.stats.totalProxEnabledTime, 1000000000);
     proxFarSeconds = U64_DIV_BY_U64_CONSTANT(mTask.stats.totalProxFarTime, 1000000000);
-    INFO_PRINT("STATS: enabled %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32
+    VERBOSE_PRINT("STATS: enabled %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32
                ", prox enabled %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32
                ", prox far %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32
                ", prox *->f %" PRIu32
diff --git a/firmware/os/drivers/vsync/vsync.c b/firmware/os/drivers/vsync/vsync.c
index 68147b0..9d76759 100644
--- a/firmware/os/drivers/vsync/vsync.c
+++ b/firmware/os/drivers/vsync/vsync.c
@@ -49,6 +49,10 @@
 #error "VSYNC_IRQ is not defined; please define in variant.h"
 #endif
 
+#define VERBOSE_PRINT(fmt, ...) do { \
+        osLog(LOG_VERBOSE, "%s " fmt, "[VSYNC]", ##__VA_ARGS__); \
+    } while (0);
+
 #define INFO_PRINT(fmt, ...) do { \
         osLog(LOG_INFO, "%s " fmt, "[VSYNC]", ##__VA_ARGS__); \
     } while (0);
@@ -149,7 +153,7 @@
 
 static bool vsyncPower(bool on, void *cookie)
 {
-    INFO_PRINT("power %d\n", on);
+    VERBOSE_PRINT("power %d\n", on);
 
     if (on) {
         extiClearPendingGpio(mTask.pin);
@@ -171,13 +175,13 @@
 
 static bool vsyncSetRate(uint32_t rate, uint64_t latency, void *cookie)
 {
-    INFO_PRINT("setRate\n");
+    VERBOSE_PRINT("setRate\n");
     return sensorSignalInternalEvt(mTask.sensorHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
 }
 
 static bool vsyncFlush(void *cookie)
 {
-    INFO_PRINT("flush\n");
+    VERBOSE_PRINT("flush\n");
     return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_VSYNC), SENSOR_DATA_EVENT_FLUSH, NULL);
 }
 
diff --git a/firmware/os/drivers/window_orientation/window_orientation.c b/firmware/os/drivers/window_orientation/window_orientation.c
index 067649a..c81d8a6 100644
--- a/firmware/os/drivers/window_orientation/window_orientation.c
+++ b/firmware/os/drivers/window_orientation/window_orientation.c
@@ -34,6 +34,10 @@
 
 #define LOG_TAG "[WO]"
 
+#define LOGV(fmt, ...) do { \
+        osLog(LOG_VERBOSE, LOG_TAG " " fmt,  ##__VA_ARGS__);  \
+    } while (0);
+
 #define LOGW(fmt, ...) do { \
         osLog(LOG_WARN, LOG_TAG " " fmt,  ##__VA_ARGS__);  \
     } while (0);
@@ -618,7 +622,7 @@
         rotation_changed = add_samples(ev);
 
         if (rotation_changed) {
-            LOGI("rotation changed to: ******* %d *******\n",
+            LOGV("rotation changed to: ******* %d *******\n",
                  (int)mTask.proposed_rotation);
 
             // send a single int32 here so no memory alloc/free needed.
diff --git a/firmware/os/inc/osApi.h b/firmware/os/inc/osApi.h
index 287281c..77cff48 100644
--- a/firmware/os/inc/osApi.h
+++ b/firmware/os/inc/osApi.h
@@ -93,9 +93,10 @@
 #define SYSCALL_OS_MAIN_SENSOR_RATE_CHG      6 // (uint32_t clientId, uint32_t sensorHandle, uint32_t newRate) -> bool success
 #define SYSCALL_OS_MAIN_SENSOR_RELEASE       7 // (uint32_t clientId, uint32_t sensorHandle) -> bool success
 #define SYSCALL_OS_MAIN_SENSOR_TRIGGER       8 // (uint32_t clientId, uint32_t sensorHandle) -> bool success
-#define SYSCALL_OS_MAIN_SENSOR_GET_RATE      9 // (uint32_t sensorHandle) -> uint32_t rate
+#define SYSCALL_OS_MAIN_SENSOR_GET_CUR_RATE  9 // (uint32_t sensorHandle) -> uint32_t curRate
 #define SYSCALL_OS_MAIN_SENSOR_GET_TIME     10 // (uint64_t *timeNanos) -> void
-#define SYSCALL_OS_MAIN_SENSOR_LAST         11 // always last. holes are allowed, but not immediately before this
+#define SYSCALL_OS_MAIN_SENSOR_GET_REQ_RATE 11 // (uint32_t sensorHandle) -> uint32_t reqRate
+#define SYSCALL_OS_MAIN_SENSOR_LAST         12 // always last. holes are allowed, but not immediately before this
 
 //level 3 indices in the OS.main.timer table
 #define SYSCALL_OS_MAIN_TIME_GET_TIME     0 // (uint64_t *timeNanos) -> void
diff --git a/firmware/os/inc/seos.h b/firmware/os/inc/seos.h
index 7b17889..5f3e60f 100644
--- a/firmware/os/inc/seos.h
+++ b/firmware/os/inc/seos.h
@@ -174,6 +174,7 @@
 bool osTidById(uint64_t *appId, uint32_t *tid);
 bool osAppInfoById(uint64_t appId, uint32_t *appIdx, uint32_t *appVer, uint32_t *appSize);
 bool osAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize);
+bool osExtAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize);
 uint32_t osGetCurrentTid();
 uint32_t osSetCurrentTid(uint32_t);
 
@@ -270,10 +271,11 @@
 
 /* Logging */
 enum LogLevel {
-    LOG_ERROR = 'E',
-    LOG_WARN  = 'W',
-    LOG_INFO  = 'I',
-    LOG_DEBUG = 'D',
+    LOG_ERROR   = 'E',
+    LOG_WARN    = 'W',
+    LOG_INFO    = 'I',
+    LOG_DEBUG   = 'D',
+    LOG_VERBOSE = 'V',
 };
 
 void osLogv(char clevel, uint32_t flags, const char *str, va_list vl);
diff --git a/firmware/os/inc/syscallDo.h b/firmware/os/inc/syscallDo.h
index 0a96e36..0fd4325 100644
--- a/firmware/os/inc/syscallDo.h
+++ b/firmware/os/inc/syscallDo.h
@@ -152,7 +152,7 @@
 
 static inline uint32_t eOsSensorGetCurRate(uint32_t sensorHandle)
 {
-    return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_OS, SYSCALL_OS_MAIN, SYSCALL_OS_MAIN_SENSOR, SYSCALL_OS_MAIN_SENSOR_GET_RATE), sensorHandle);
+    return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_OS, SYSCALL_OS_MAIN, SYSCALL_OS_MAIN_SENSOR, SYSCALL_OS_MAIN_SENSOR_GET_CUR_RATE), sensorHandle);
 }
 
 static inline uint64_t eOsSensorGetTime(void)
@@ -162,6 +162,11 @@
     return timeNanos;
 }
 
+static inline uint32_t eOsSensorGetReqRate(uint32_t sensorHandle)
+{
+    return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_OS, SYSCALL_OS_MAIN, SYSCALL_OS_MAIN_SENSOR, SYSCALL_OS_MAIN_SENSOR_GET_REQ_RATE), sensorHandle);
+}
+
 static inline uint64_t eOsTimGetTime(void)
 {
     uint64_t timeNanos;
diff --git a/firmware/os/platform/stm32/platform.c b/firmware/os/platform/stm32/platform.c
index befafe9..313f1bd 100644
--- a/firmware/os/platform/stm32/platform.c
+++ b/firmware/os/platform/stm32/platform.c
@@ -110,7 +110,7 @@
 
 #ifdef DEBUG_LOG_EVT
 #ifndef EARLY_LOG_BUF_SIZE
-#define EARLY_LOG_BUF_SIZE      1024
+#define EARLY_LOG_BUF_SIZE      2048
 #endif
 #define HOSTINTF_HEADER_SIZE    4
 uint8_t *mEarlyLogBuffer;
diff --git a/lefty/Android.mk b/lefty/Android.mk
new file mode 100644
index 0000000..5e8edf7
--- /dev/null
+++ b/lefty/Android.mk
@@ -0,0 +1,95 @@
+# Copyright (C) 2017 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+LOCAL_PATH := $(call my-dir)
+
+ifeq ($(TARGET_USES_NANOHUB_SENSORHAL), true)
+
+COMMON_CFLAGS := -Wall -Werror -Wextra
+
+################################################################################
+ifeq ($(NANOHUB_SENSORHAL_LEFTY_IMPL_ENABLED), true)
+
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := vendor.google_clockwork.lefty@1.0-impl.nanohub
+
+LOCAL_MODULE_RELATIVE_PATH := hw
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := google
+LOCAL_PROPRIETARY_MODULE := true
+
+LOCAL_CFLAGS += $(COMMON_CFLAGS)
+
+LOCAL_C_INCLUDES += \
+    device/google/contexthub/firmware/os/inc \
+    device/google/contexthub/sensorhal \
+    device/google/contexthub/util/common \
+
+LOCAL_SRC_FILES := \
+    Lefty.cpp \
+
+LOCAL_SHARED_LIBRARIES := \
+    vendor.google_clockwork.lefty@1.0 \
+    libbase \
+    libcutils \
+    libhidlbase \
+    libhidltransport \
+    libhubconnection \
+    liblog \
+    libutils \
+
+include $(BUILD_SHARED_LIBRARY)
+
+endif
+################################################################################
+ifeq ($(NANOHUB_SENSORHAL_LEFTY_SERVICE_ENABLED), true)
+
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := liblefty_service_nanohub
+
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := google
+LOCAL_PROPRIETARY_MODULE := true
+
+LOCAL_CFLAGS += $(COMMON_CFLAGS)
+
+LOCAL_C_INCLUDES += \
+    device/google/contexthub/firmware/os/inc \
+    device/google/contexthub/sensorhal \
+    device/google/contexthub/util/common \
+
+LOCAL_EXPORT_C_INCLUDE_DIRS := $(LOCAL_PATH)
+
+LOCAL_SRC_FILES := \
+    Lefty.cpp \
+    lefty_service.cpp \
+
+LOCAL_SHARED_LIBRARIES := \
+    vendor.google_clockwork.lefty@1.0 \
+    libbase \
+    libcutils \
+    libhidlbase \
+    libhidltransport \
+    libhubconnection \
+    liblog \
+    libutils \
+
+include $(BUILD_SHARED_LIBRARY)
+
+endif
+################################################################################
+
+endif
diff --git a/lefty/Lefty.cpp b/lefty/Lefty.cpp
new file mode 100644
index 0000000..b04ac4f
--- /dev/null
+++ b/lefty/Lefty.cpp
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#define LOG_TAG "Lefty"
+#include <utils/Log.h>
+
+#include "Lefty.h"
+
+namespace vendor {
+namespace google_clockwork {
+namespace lefty {
+namespace V1_0 {
+namespace implementation {
+
+Lefty::Lefty() : mHubConnection(HubConnection::getInstance()) {
+    ALOGI("Created a Lefty interface instance");
+}
+
+// Methods from ::vendor::google_clockwork::lefty::V1_0::ILefty follow.
+Return<void> Lefty::setLeftyMode(bool enabled) {
+    if (mHubConnection->initCheck() == ::android::OK
+            && mHubConnection->getAliveCheck() == ::android::OK) {
+        mHubConnection->setLeftyMode(enabled);
+    }
+    return Void();
+}
+
+
+ILefty* HIDL_FETCH_ILefty(const char* /* name */) {
+    return new Lefty();
+}
+
+}  // namespace implementation
+}  // namespace V1_0
+}  // namespace lefty
+}  // namespace google_clockwork
+}  // namespace vendor
diff --git a/lefty/Lefty.h b/lefty/Lefty.h
new file mode 100644
index 0000000..397d215
--- /dev/null
+++ b/lefty/Lefty.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef VENDOR_GOOGLE_CLOCKWORK_LEFTY_V1_0_LEFTY_H
+#define VENDOR_GOOGLE_CLOCKWORK_LEFTY_V1_0_LEFTY_H
+
+#include "hubconnection.h"
+#undef LIKELY
+#undef UNLIKELY
+#include <vendor/google_clockwork/lefty/1.0/ILefty.h>
+
+namespace vendor {
+namespace google_clockwork {
+namespace lefty {
+namespace V1_0 {
+namespace implementation {
+
+using ::vendor::google_clockwork::lefty::V1_0::ILefty;
+using ::android::hardware::Return;
+using ::android::hardware::Void;
+using ::android::sp;
+using ::android::HubConnection;
+
+struct Lefty : public ILefty {
+    Lefty();
+
+    // Methods from ::vendor::google_clockwork::lefty::V1_0::ILefty follow.
+    Return<void> setLeftyMode(bool enabled) override;
+
+private:
+    sp<HubConnection> mHubConnection;
+};
+
+extern "C" ILefty* HIDL_FETCH_ILefty(const char* name);
+
+}  // namespace implementation
+}  // namespace V1_0
+}  // namespace lefty
+}  // namespace google_clockwork
+}  // namespace vendor
+
+#endif  // VENDOR_GOOGLE_CLOCKWORK_LEFTY_V1_0_LEFTY_H
diff --git a/lefty/lefty_service.cpp b/lefty/lefty_service.cpp
new file mode 100644
index 0000000..f9e3f94
--- /dev/null
+++ b/lefty/lefty_service.cpp
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "Lefty.h"
+#include "lefty_service.h"
+
+#include <android-base/logging.h>
+#include <hidl/LegacySupport.h>
+#include <utils/StrongPointer.h>
+#include <vendor/google_clockwork/lefty/1.0/ILefty.h>
+
+using ::android::sp;
+using ::vendor::google_clockwork::lefty::V1_0::ILefty;
+using ::vendor::google_clockwork::lefty::V1_0::implementation::Lefty;
+
+void register_lefty_service() {
+    // Kids, don't do this at home. Here, registerAsService is called without
+    // configureRpcThreadpool/joinRpcThreadpool, because it is called from
+    // open_sensors() function, called from HIDL_FETCH_ISensors, called from
+    // ISensor::getService, called from registerPassthroughServiceImplementation
+    // which is surrounded with configureRpcThreadpool/joinRpcThreadpool.
+    sp<ILefty> lefty = new Lefty();
+    CHECK_EQ(lefty->registerAsService(), android::NO_ERROR)
+            << "Failed to register Lefty HAL";
+}
diff --git a/lefty/lefty_service.h b/lefty/lefty_service.h
new file mode 100644
index 0000000..f6ffb74
--- /dev/null
+++ b/lefty/lefty_service.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+extern "C" void register_lefty_service();
diff --git a/lib/Android.bp b/lib/Android.bp
new file mode 100644
index 0000000..828d29e
--- /dev/null
+++ b/lib/Android.bp
@@ -0,0 +1,5 @@
+cc_library_headers {
+    name: "libnanohub_common_headers",
+    vendor_available: true,
+    export_include_dirs: ["include"],
+}
diff --git a/lib/Android.mk b/lib/Android.mk
index e88b975..0caa123 100644
--- a/lib/Android.mk
+++ b/lib/Android.mk
@@ -58,20 +58,6 @@
 
 include $(BUILD_HOST_STATIC_LIBRARY)
 
-include $(CLEAR_VARS)
-
-# now-empty static library to export include files for other projects
-LOCAL_MODULE := libnanohub_common
-LOCAL_MODULE_TAGS := optional
-
-LOCAL_C_INCLUDES := \
-    $(src_includes)
-
-LOCAL_EXPORT_C_INCLUDE_DIRS := \
-    $(src_includes)
-
-include $(BUILD_STATIC_LIBRARY)
-
 include $(call first-makefiles-under, $(LOCAL_PATH))
 
 src_files :=
diff --git a/sensorhal/Android.mk b/sensorhal/Android.mk
index 5473825..385a762 100644
--- a/sensorhal/Android.mk
+++ b/sensorhal/Android.mk
@@ -88,6 +88,11 @@
 LOCAL_SHARED_LIBRARIES += libdynamic_sensor_ext
 endif
 
+ifeq ($(NANOHUB_SENSORHAL_LEFTY_SERVICE_ENABLED), true)
+LOCAL_CFLAGS += -DLEFTY_SERVICE_ENABLED
+LOCAL_SHARED_LIBRARIES += liblefty_service_nanohub
+endif
+
 include $(BUILD_SHARED_LIBRARY)
 
 ################################################################################
diff --git a/sensorhal/directchannel.cpp b/sensorhal/directchannel.cpp
index 8d87782..b0df24c 100644
--- a/sensorhal/directchannel.cpp
+++ b/sensorhal/directchannel.cpp
@@ -74,6 +74,10 @@
     ::close(mAshmemFd);
 }
 
+bool AshmemDirectChannel::memoryMatches(const struct sensors_direct_mem_t * /*mem*/) const {
+    return false;
+}
+
 ANDROID_SINGLETON_STATIC_INSTANCE(GrallocHalWrapper);
 
 GrallocHalWrapper::GrallocHalWrapper()
@@ -128,10 +132,15 @@
                                                     GRALLOC1_FUNCTION_LOCK));
             mPfnUnlock = (GRALLOC1_PFN_UNLOCK)(mGralloc1Device->getFunction(mGralloc1Device,
                                                       GRALLOC1_FUNCTION_UNLOCK));
+            mPfnGetBackingStore = (GRALLOC1_PFN_GET_BACKING_STORE)
+                    (mGralloc1Device->getFunction(mGralloc1Device,
+                                                  GRALLOC1_FUNCTION_GET_BACKING_STORE));
             if (mPfnRetain == nullptr || mPfnRelease == nullptr
-                    || mPfnLock == nullptr || mPfnUnlock == nullptr) {
-                ALOGE("Function pointer for retain, release, lock and unlock are %p, %p, %p, %p",
-                      mPfnRetain, mPfnRelease, mPfnLock, mPfnUnlock);
+                    || mPfnLock == nullptr || mPfnUnlock == nullptr
+                    || mPfnGetBackingStore == nullptr) {
+                ALOGE("Function pointer for retain, release, lock, unlock and getBackingStore are "
+                      "%p, %p, %p, %p, %p",
+                      mPfnRetain, mPfnRelease, mPfnLock, mPfnUnlock, mPfnGetBackingStore);
                 err = BAD_VALUE;
                 break;
             }
@@ -223,6 +232,21 @@
     }
 }
 
+bool GrallocHalWrapper::isSameMemory(const native_handle_t *h1, const native_handle_t *h2) {
+    switch (mVersion) {
+        case 0:
+            return false; // version 1.0 cannot compare two memory
+        case 1: {
+            gralloc1_backing_store_t s1, s2;
+
+            return mPfnGetBackingStore(mGralloc1Device, h1, &s1) == GRALLOC1_ERROR_NONE
+                    && mPfnGetBackingStore(mGralloc1Device, h2, &s2) == GRALLOC1_ERROR_NONE
+                    && s1 == s2;
+        }
+    }
+    return false;
+}
+
 int GrallocHalWrapper::mapGralloc1Error(int grallocError) {
     switch (grallocError) {
         case GRALLOC1_ERROR_NONE:
@@ -302,4 +326,9 @@
     }
 }
 
+bool GrallocDirectChannel::memoryMatches(const struct sensors_direct_mem_t *mem) const {
+    return mem->type == SENSOR_DIRECT_MEM_TYPE_GRALLOC &&
+            GrallocHalWrapper::getInstance().isSameMemory(mem->handle, mNativeHandle);
+}
+
 } // namespace android
diff --git a/sensorhal/directchannel.h b/sensorhal/directchannel.h
index 4842ffc..70d8645 100644
--- a/sensorhal/directchannel.h
+++ b/sensorhal/directchannel.h
@@ -31,6 +31,7 @@
 public:
     DirectChannelBase() : mError(NO_INIT), mSize(0), mBase(nullptr) { }
     virtual ~DirectChannelBase() {}
+    virtual bool memoryMatches(const struct sensors_direct_mem_t *mem) const = 0;
 
     bool isValid();
     int getError();
@@ -47,7 +48,8 @@
 class AshmemDirectChannel : public DirectChannelBase {
 public:
     AshmemDirectChannel(const struct sensors_direct_mem_t *mem);
-    virtual ~AshmemDirectChannel();
+    ~AshmemDirectChannel() override;
+    bool memoryMatches(const struct sensors_direct_mem_t *mem) const override;
 private:
     int mAshmemFd;
 };
@@ -58,6 +60,7 @@
     int unregisterBuffer(const native_handle_t *handle);
     int lock(const native_handle_t *handle, int usage, int l, int t, int w, int h, void **vaddr);
     int unlock(const native_handle_t *handle);
+    bool isSameMemory(const native_handle_t *h1, const native_handle_t *h2);
     bool unregisterImplyDelete() { return mUnregisterImplyDelete; }
 private:
     friend class Singleton<GrallocHalWrapper>;
@@ -77,13 +80,15 @@
     GRALLOC1_PFN_RELEASE mPfnRelease;
     GRALLOC1_PFN_LOCK mPfnLock;
     GRALLOC1_PFN_UNLOCK mPfnUnlock;
+    GRALLOC1_PFN_GET_BACKING_STORE mPfnGetBackingStore;
     bool mUnregisterImplyDelete;
 };
 
 class GrallocDirectChannel : public DirectChannelBase {
 public:
     GrallocDirectChannel(const struct sensors_direct_mem_t *mem);
-    virtual ~GrallocDirectChannel();
+    ~GrallocDirectChannel() override;
+    bool memoryMatches(const struct sensors_direct_mem_t *mem) const override;
 private:
     native_handle_t *mNativeHandle;
 };
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index 8d27706..c283a81 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -45,6 +45,9 @@
 #define APP_ID_MAKE(vendor, app)       ((((uint64_t)(vendor)) << 24) | ((app) & 0x00FFFFFF))
 #define APP_ID_VENDOR_GOOGLE           0x476f6f676cULL // "Googl"
 #define APP_ID_APP_BMI160              2
+#define APP_ID_APP_WRIST_TILT_DETECT   0x1005
+#define APP_ID_APP_GAZE_DETECT         0x1009
+#define APP_ID_APP_UNGAZE_DETECT       0x100a
 
 #define SENS_TYPE_TO_EVENT(_sensorType) (EVT_NO_FIRST_SENSOR_EVENT + (_sensorType))
 
@@ -64,6 +67,8 @@
 
 #define OS_LOG_EVENT            0x474F4C41  // ascii: ALOG
 
+#define MAX_RETRY_CNT           5
+
 #ifdef LID_STATE_REPORTING_ENABLED
 const char LID_STATE_PROPERTY[] = "sensors.contexthub.lid_state";
 const char LID_STATE_UNKNOWN[]  = "unknown";
@@ -98,6 +103,21 @@
         && sensorIndex <= COMMS_SENSOR_ACTIVITY_LAST;
 }
 
+static bool isWakeEvent(int32_t sensor)
+{
+    switch (sensor) {
+    case COMMS_SENSOR_DOUBLE_TOUCH:
+    case COMMS_SENSOR_DOUBLE_TWIST:
+    case COMMS_SENSOR_GESTURE:
+    case COMMS_SENSOR_PROXIMITY:
+    case COMMS_SENSOR_SIGNIFICANT_MOTION:
+    case COMMS_SENSOR_TILT:
+        return true;
+    default:
+        return false;
+    }
+}
+
 HubConnection::HubConnection()
     : Thread(false /* canCallJava */),
       mRing(10 *1024),
@@ -114,6 +134,10 @@
     mAccelBias[0] = mAccelBias[1] = mAccelBias[2] = 0.0f;
     memset(&mGyroOtcData, 0, sizeof(mGyroOtcData));
 
+    mLefty.accel = false;
+    mLefty.gyro = false;
+    mLefty.hub = false;
+
     memset(&mSensorState, 0x00, sizeof(mSensorState));
     mFd = open(NANOHUB_FILE_PATH, O_RDWR);
     mPollFds[0].fd = mFd;
@@ -123,6 +147,7 @@
 
     mWakelockHeld = false;
     mWakeEventCount = 0;
+    mWriteFailures = 0;
 
     initNanohubLock();
 
@@ -156,20 +181,32 @@
 #endif  // DOUBLE_TOUCH_ENABLED
 
     mSensorState[COMMS_SENSOR_ACCEL].sensorType = SENS_TYPE_ACCEL;
-    mSensorState[COMMS_SENSOR_ACCEL].alt = COMMS_SENSOR_ACCEL_UNCALIBRATED;
+    mSensorState[COMMS_SENSOR_ACCEL].alt[0] = COMMS_SENSOR_ACCEL_UNCALIBRATED;
+    mSensorState[COMMS_SENSOR_ACCEL].alt[1] = COMMS_SENSOR_ACCEL_WRIST_AWARE;
     mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].sensorType = SENS_TYPE_ACCEL;
-    mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].primary = SENS_TYPE_ACCEL;
-    mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt = COMMS_SENSOR_ACCEL;
+    mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].primary = COMMS_SENSOR_ACCEL;
+    mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt[0] = COMMS_SENSOR_ACCEL;
+    mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt[1] = COMMS_SENSOR_ACCEL_WRIST_AWARE;
+    mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].sensorType = SENS_TYPE_ACCEL;
+    mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].primary = COMMS_SENSOR_ACCEL;
+    mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].alt[0] = COMMS_SENSOR_ACCEL;
+    mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].alt[1] = COMMS_SENSOR_ACCEL_UNCALIBRATED;
     mSensorState[COMMS_SENSOR_GYRO].sensorType = SENS_TYPE_GYRO;
-    mSensorState[COMMS_SENSOR_GYRO].alt = COMMS_SENSOR_GYRO_UNCALIBRATED;
+    mSensorState[COMMS_SENSOR_GYRO].alt[0] = COMMS_SENSOR_GYRO_UNCALIBRATED;
+    mSensorState[COMMS_SENSOR_GYRO].alt[1] = COMMS_SENSOR_GYRO_WRIST_AWARE;
     mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].sensorType = SENS_TYPE_GYRO;
     mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].primary = COMMS_SENSOR_GYRO;
-    mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt = COMMS_SENSOR_GYRO;
+    mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt[0] = COMMS_SENSOR_GYRO;
+    mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt[1] = COMMS_SENSOR_GYRO_WRIST_AWARE;
+    mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].sensorType = SENS_TYPE_GYRO;
+    mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].primary = COMMS_SENSOR_GYRO;
+    mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].alt[0] = COMMS_SENSOR_GYRO;
+    mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].alt[1] = COMMS_SENSOR_GYRO_UNCALIBRATED;
     mSensorState[COMMS_SENSOR_MAG].sensorType = SENS_TYPE_MAG;
-    mSensorState[COMMS_SENSOR_MAG].alt = COMMS_SENSOR_MAG_UNCALIBRATED;
+    mSensorState[COMMS_SENSOR_MAG].alt[0] = COMMS_SENSOR_MAG_UNCALIBRATED;
     mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].sensorType = SENS_TYPE_MAG;
     mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].primary = COMMS_SENSOR_MAG;
-    mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].alt = COMMS_SENSOR_MAG;
+    mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].alt[0] = COMMS_SENSOR_MAG;
     mSensorState[COMMS_SENSOR_LIGHT].sensorType = SENS_TYPE_ALS;
     mSensorState[COMMS_SENSOR_PROXIMITY].sensorType = SENS_TYPE_PROX;
     mSensorState[COMMS_SENSOR_PRESSURE].sensorType = SENS_TYPE_BARO;
@@ -248,9 +285,18 @@
 
 #ifdef DIRECT_REPORT_ENABLED
     mDirectChannelHandle = 1;
-    mSensorToChannel.emplace(COMMS_SENSOR_ACCEL, std::unordered_map<int32_t, int32_t>());
-    mSensorToChannel.emplace(COMMS_SENSOR_GYRO, std::unordered_map<int32_t, int32_t>());
-    mSensorToChannel.emplace(COMMS_SENSOR_MAG, std::unordered_map<int32_t, int32_t>());
+    mSensorToChannel.emplace(COMMS_SENSOR_ACCEL,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_GYRO,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_MAG,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_ACCEL_UNCALIBRATED,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_GYRO_UNCALIBRATED,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_MAG_UNCALIBRATED,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
 #endif // DIRECT_REPORT_ENABLED
 }
 
@@ -457,6 +503,81 @@
     }
 }
 
+ssize_t HubConnection::sendCmd(const void *buf, size_t count)
+{
+    ssize_t ret;
+    int retryCnt = 0;
+
+    do {
+        ret = TEMP_FAILURE_RETRY(::write(mFd, buf, count));
+    } while (ret == 0 && retryCnt++ < MAX_RETRY_CNT);
+
+    if (retryCnt > 0)
+        ALOGW("sendCmd: retry: count=%zu, ret=%zd, retryCnt=%d",
+              count, ret, retryCnt);
+    else if (ret < 0 || static_cast<size_t>(ret) != count)
+        ALOGW("sendCmd: failed: count=%zu, ret=%zd, errno=%d",
+              count, ret, errno);
+
+    return ret;
+}
+
+void HubConnection::setLeftyMode(bool enable) {
+    struct MsgCmd *cmd;
+    size_t ret;
+
+    Mutex::Autolock autoLock(mLock);
+
+    if (enable == mLefty.hub) return;
+
+    cmd = (struct MsgCmd *)malloc(sizeof(struct MsgCmd) + sizeof(bool));
+
+    if (cmd) {
+        cmd->evtType = EVT_APP_FROM_HOST;
+        cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_GAZE_DETECT);
+        cmd->msg.dataLen = sizeof(bool);
+        memcpy((bool *)(cmd+1), &enable, sizeof(bool));
+
+        ret = sendCmd(cmd, sizeof(*cmd) + sizeof(bool));
+        if (ret == sizeof(*cmd) + sizeof(bool))
+            ALOGV("setLeftyMode: lefty (gaze) = %s\n",
+                  (enable ? "true" : "false"));
+        else
+            ALOGE("setLeftyMode: failed to send command lefty (gaze) = %s\n",
+                  (enable ? "true" : "false"));
+
+        cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_UNGAZE_DETECT);
+
+        ret = sendCmd(cmd, sizeof(*cmd) + sizeof(bool));
+        if (ret == sizeof(*cmd) + sizeof(bool))
+            ALOGV("setLeftyMode: lefty (ungaze) = %s\n",
+                  (enable ? "true" : "false"));
+        else
+            ALOGE("setLeftyMode: failed to send command lefty (ungaze) = %s\n",
+                  (enable ? "true" : "false"));
+
+        cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_WRIST_TILT_DETECT);
+
+        ret = sendCmd(cmd, sizeof(*cmd) + sizeof(bool));
+        if (ret == sizeof(*cmd) + sizeof(bool))
+            ALOGV("setLeftyMode: lefty (tilt) = %s\n",
+                  (enable ? "true" : "false"));
+        else
+            ALOGE("setLeftyMode: failed to send command lefty (tilt) = %s\n",
+                  (enable ? "true" : "false"));
+
+        free(cmd);
+    } else {
+        ALOGE("setLeftyMode: failed to allocate command\n");
+        return;
+    }
+
+    queueFlushInternal(COMMS_SENSOR_ACCEL_WRIST_AWARE, true);
+    queueFlushInternal(COMMS_SENSOR_GYRO_WRIST_AWARE, true);
+
+    mLefty.hub = enable;
+}
+
 sensors_event_t *HubConnection::initEv(sensors_event_t *ev, uint64_t timestamp, uint32_t type, uint32_t sensor)
 {
     memset(ev, 0x00, sizeof(sensors_event_t));
@@ -468,31 +589,20 @@
     return ev;
 }
 
-ssize_t HubConnection::getWakeEventCount()
+ssize_t HubConnection::decrementIfWakeEventLocked(int32_t sensor)
 {
+    if (isWakeEvent(sensor)) {
+        if (mWakeEventCount > 0)
+            mWakeEventCount--;
+        else
+            ALOGW("%s: sensor=%d, unexpected count=%d, no-op",
+                  __FUNCTION__, sensor, mWakeEventCount);
+    }
+
     return mWakeEventCount;
 }
 
-ssize_t HubConnection::decrementWakeEventCount()
-{
-    return --mWakeEventCount;
-}
-
-bool HubConnection::isWakeEvent(int32_t sensor)
-{
-    switch (sensor) {
-    case COMMS_SENSOR_PROXIMITY:
-    case COMMS_SENSOR_SIGNIFICANT_MOTION:
-    case COMMS_SENSOR_TILT:
-    case COMMS_SENSOR_DOUBLE_TWIST:
-    case COMMS_SENSOR_GESTURE:
-        return true;
-    default:
-        return false;
-    }
-}
-
-void HubConnection::protectIfWakeEvent(int32_t sensor)
+void HubConnection::protectIfWakeEventLocked(int32_t sensor)
 {
     if (isWakeEvent(sensor)) {
         if (mWakelockHeld == false) {
@@ -505,6 +615,8 @@
 
 void HubConnection::releaseWakeLockIfAppropriate()
 {
+    Mutex::Autolock autoLock(mLock);
+
     if (mWakelockHeld && (mWakeEventCount == 0)) {
         mWakelockHeld = false;
         release_wake_lock(WAKELOCK_NAME);
@@ -583,11 +695,8 @@
         break;
     }
 
-    if (cnt > 0) {
-        // If event is a wake event, protect it with a wakelock
-        protectIfWakeEvent(sensor);
+    if (cnt > 0)
         write(nev, cnt);
-    }
 }
 
 uint8_t HubConnection::magAccuracyUpdate(sensors_vec_t *sv)
@@ -611,7 +720,7 @@
 {
     sensors_vec_t *sv;
     uncalibrated_event_t *ue;
-    sensors_event_t nev[2];
+    sensors_event_t nev[3];
     int cnt = 0;
 
     switch (sensor) {
@@ -621,22 +730,37 @@
         sv->y = sample->iy * mScaleAccel;
         sv->z = sample->iz * mScaleAccel;
         sv->status = SENSOR_STATUS_ACCURACY_HIGH;
-        sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
-                COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer;
-            ue->x_uncalib = sample->ix * mScaleAccel + mAccelBias[0];
-            ue->y_uncalib = sample->iy * mScaleAccel + mAccelBias[1];
-            ue->z_uncalib = sample->iz * mScaleAccel + mAccelBias[2];
-            ue->x_bias = mAccelBias[0];
-            ue->y_bias = mAccelBias[1];
-            ue->z_bias = mAccelBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
+            COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer;
+        ue->x_uncalib = sample->ix * mScaleAccel + mAccelBias[0];
+        ue->y_uncalib = sample->iy * mScaleAccel + mAccelBias[1];
+        ue->z_uncalib = sample->iz * mScaleAccel + mAccelBias[2];
+        ue->x_bias = mAccelBias[0];
+        ue->y_bias = mAccelBias[1];
+        ue->z_bias = mAccelBias[2];
+
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_UNCALIBRATED, timestamp)) {
+            ++cnt;
+        }
+
+        if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_WRIST_AWARE, timestamp)) {
+            sv = &initEv(&nev[cnt++], timestamp,
+                SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE,
+                COMMS_SENSOR_ACCEL_WRIST_AWARE)->acceleration;
+            sv->x = sample->ix * mScaleAccel;
+            sv->y = (mLefty.accel ? -sample->iy : sample->iy) * mScaleAccel;
+            sv->z = sample->iz * mScaleAccel;
+            sv->status = SENSOR_STATUS_ACCURACY_HIGH;
         }
         break;
     case COMMS_SENSOR_MAG:
@@ -645,32 +769,33 @@
         sv->y = sample->iy * mScaleMag;
         sv->z = sample->iz * mScaleMag;
         sv->status = magAccuracyUpdate(sv);
-        sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
-                COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
-            ue->x_uncalib = sample->ix * mScaleMag + mMagBias[0];
-            ue->y_uncalib = sample->iy * mScaleMag + mMagBias[1];
-            ue->z_uncalib = sample->iz * mScaleMag + mMagBias[2];
-            ue->x_bias = mMagBias[0];
-            ue->y_bias = mMagBias[1];
-            ue->z_bias = mMagBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+            COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
+        ue->x_uncalib = sample->ix * mScaleMag + mMagBias[0];
+        ue->y_uncalib = sample->iy * mScaleMag + mMagBias[1];
+        ue->z_uncalib = sample->iz * mScaleMag + mMagBias[2];
+        ue->x_bias = mMagBias[0];
+        ue->y_bias = mMagBias[1];
+        ue->z_bias = mMagBias[2];
+
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_MAG_UNCALIBRATED, timestamp)) {
+            ++cnt;
         }
     default:
         break;
     }
 
-    if (cnt > 0) {
-        // If event is a wake event, protect it with a wakelock
-        protectIfWakeEvent(sensor);
+    if (cnt > 0)
         write(nev, cnt);
-    }
 }
 
 void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct ThreeAxisSample *sample, bool highAccuracy)
@@ -678,7 +803,7 @@
     sensors_vec_t *sv;
     uncalibrated_event_t *ue;
     sensors_event_t *ev;
-    sensors_event_t nev[2];
+    sensors_event_t nev[3];
     static const float heading_accuracy = M_PI / 6.0f;
     float w;
     int cnt = 0;
@@ -690,22 +815,38 @@
         sv->y = sample->y;
         sv->z = sample->z;
         sv->status = SENSOR_STATUS_ACCURACY_HIGH;
-        sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
-                COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer;
-            ue->x_uncalib = sample->x + mAccelBias[0];
-            ue->y_uncalib = sample->y + mAccelBias[1];
-            ue->z_uncalib = sample->z + mAccelBias[2];
-            ue->x_bias = mAccelBias[0];
-            ue->y_bias = mAccelBias[1];
-            ue->z_bias = mAccelBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
+            COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer;
+        ue->x_uncalib = sample->x + mAccelBias[0];
+        ue->y_uncalib = sample->y + mAccelBias[1];
+        ue->z_uncalib = sample->z + mAccelBias[2];
+        ue->x_bias = mAccelBias[0];
+        ue->y_bias = mAccelBias[1];
+        ue->z_bias = mAccelBias[2];
+
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_UNCALIBRATED, timestamp)) {
+            ++cnt;
+        }
+
+        if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_WRIST_AWARE, timestamp)) {
+            sv = &initEv(&nev[cnt], timestamp,
+                SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE,
+                COMMS_SENSOR_ACCEL_WRIST_AWARE)->acceleration;
+            sv->x = sample->x;
+            sv->y = (mLefty.accel ? -sample->y : sample->y);
+            sv->z = sample->z;
+            sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+            ++cnt;
         }
         break;
     case COMMS_SENSOR_GYRO:
@@ -714,22 +855,38 @@
         sv->y = sample->y;
         sv->z = sample->z;
         sv->status = SENSOR_STATUS_ACCURACY_HIGH;
-        sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
-                COMMS_SENSOR_GYRO_UNCALIBRATED)->uncalibrated_gyro;
-            ue->x_uncalib = sample->x + mGyroBias[0];
-            ue->y_uncalib = sample->y + mGyroBias[1];
-            ue->z_uncalib = sample->z + mGyroBias[2];
-            ue->x_bias = mGyroBias[0];
-            ue->y_bias = mGyroBias[1];
-            ue->z_bias = mGyroBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
+            COMMS_SENSOR_GYRO_UNCALIBRATED)->uncalibrated_gyro;
+        ue->x_uncalib = sample->x + mGyroBias[0];
+        ue->y_uncalib = sample->y + mGyroBias[1];
+        ue->z_uncalib = sample->z + mGyroBias[2];
+        ue->x_bias = mGyroBias[0];
+        ue->y_bias = mGyroBias[1];
+        ue->z_bias = mGyroBias[2];
+        sendDirectReportEvent(&nev[cnt], 1);
+
+        if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_GYRO_UNCALIBRATED, timestamp)) {
+            ++cnt;
+        }
+
+        if (mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_GYRO_WRIST_AWARE, timestamp)) {
+            sv = &initEv(&nev[cnt], timestamp,
+                SENSOR_TYPE_GYROSCOPE_WRIST_AWARE,
+                COMMS_SENSOR_GYRO_WRIST_AWARE)->gyro;
+            sv->x = (mLefty.gyro ? -sample->x : sample->x);
+            sv->y = sample->y;
+            sv->z = (mLefty.gyro ? -sample->z : sample->z);
+            sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+            ++cnt;
         }
         break;
     case COMMS_SENSOR_ACCEL_BIAS:
@@ -752,20 +909,24 @@
         sv->status = magAccuracyUpdate(sv);
         sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
-                COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
-            ue->x_uncalib = sample->x + mMagBias[0];
-            ue->y_uncalib = sample->y + mMagBias[1];
-            ue->z_uncalib = sample->z + mMagBias[2];
-            ue->x_bias = mMagBias[0];
-            ue->y_bias = mMagBias[1];
-            ue->z_bias = mMagBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+            COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
+        ue->x_uncalib = sample->x + mMagBias[0];
+        ue->y_uncalib = sample->y + mMagBias[1];
+        ue->z_uncalib = sample->z + mMagBias[2];
+        ue->x_bias = mMagBias[0];
+        ue->y_bias = mMagBias[1];
+        ue->z_bias = mMagBias[2];
+        sendDirectReportEvent(&nev[cnt], 1);
+
+        if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_MAG_UNCALIBRATED, timestamp)) {
+            ++cnt;
         }
         break;
     case COMMS_SENSOR_MAG_BIAS:
@@ -821,11 +982,8 @@
         break;
     }
 
-    if (cnt > 0) {
-        // If event is a wake event, protect it with a wakelock
-        protectIfWakeEvent(sensor);
+    if (cnt > 0)
         write(nev, cnt);
-    }
 }
 
 void HubConnection::discardInotifyEvent() {
@@ -870,7 +1028,7 @@
                   cmd.sensorType, i, mSensorState[i].enable, frequency_q10_to_period_ns(mSensorState[i].rate),
                   mSensorState[i].latency);
 
-            int ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
+            int ret = sendCmd(&cmd, sizeof(cmd));
             if (ret != sizeof(cmd)) {
                 ALOGW("failed to send config command to restore sensor %d\n", cmd.sensorType);
             }
@@ -879,7 +1037,7 @@
 
             for (auto iter = mFlushesPending[i].cbegin(); iter != mFlushesPending[i].cend(); ++iter) {
                 for (int j = 0; j < iter->count; j++) {
-                    int ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
+                    int ret = sendCmd(&cmd, sizeof(cmd));
                     if (ret != sizeof(cmd)) {
                         ALOGW("failed to send flush command to sensor %d\n", cmd.sensorType);
                     }
@@ -915,6 +1073,9 @@
     case 'D':
         ALOGD("osLog: %s", &buf[5]);
         break;
+    case 'V':
+        ALOGV("osLog: %s", &buf[5]);
+        break;
     default:
         break;
     }
@@ -1261,13 +1422,18 @@
                 ev.sensor = 0;
                 ev.meta_data.what = META_DATA_FLUSH_COMPLETE;
                 ev.meta_data.sensor = flush.handle;
-                --flush.count;
 
-                if (flush.count == 0) {
+                if (flush.internal) {
+                    if (flush.handle == COMMS_SENSOR_ACCEL_WRIST_AWARE)
+                        mLefty.accel = !mLefty.accel;
+                    else if (flush.handle == COMMS_SENSOR_GYRO_WRIST_AWARE)
+                        mLefty.gyro = !mLefty.gyro;
+                } else
+                    write(&ev, 1);
+
+                if (--flush.count == 0)
                     mFlushesPending[primary].pop_front();
-                }
 
-                write(&ev, 1);
                 ALOGV("flushing %d", ev.meta_data.sensor);
             }
         }
@@ -1456,10 +1622,6 @@
     return false;
 }
 
-ssize_t HubConnection::read(sensors_event_t *ev, size_t size) {
-    return mRing.read(ev, size);
-}
-
 void HubConnection::setActivityCallback(ActivityEventHandler *eventHandler)
 {
     Mutex::Autolock autoLock(mLock);
@@ -1468,31 +1630,28 @@
 
 void HubConnection::initConfigCmd(struct ConfigCmd *cmd, int handle)
 {
-    uint8_t alt = mSensorState[handle].alt;
-
     memset(cmd, 0x00, sizeof(*cmd));
 
     cmd->evtType = EVT_NO_SENSOR_CONFIG_EVENT;
     cmd->sensorType = mSensorState[handle].sensorType;
+    cmd->cmd = mSensorState[handle].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
+    cmd->rate = mSensorState[handle].rate;
+    cmd->latency = mSensorState[handle].latency;
 
-    if (alt && mSensorState[alt].enable && mSensorState[handle].enable) {
+    for (int i=0; i<MAX_ALTERNATES; ++i) {
+        uint8_t alt = mSensorState[handle].alt[i];
+
+        if (alt == COMMS_SENSOR_INVALID) continue;
+        if (!mSensorState[alt].enable) continue;
+
         cmd->cmd = CONFIG_CMD_ENABLE;
-        if (mSensorState[alt].rate > mSensorState[handle].rate)
+
+        if (mSensorState[alt].rate > cmd->rate) {
             cmd->rate = mSensorState[alt].rate;
-        else
-            cmd->rate = mSensorState[handle].rate;
-        if (mSensorState[alt].latency < mSensorState[handle].latency)
+        }
+        if (mSensorState[alt].latency < cmd->latency) {
             cmd->latency = mSensorState[alt].latency;
-        else
-            cmd->latency = mSensorState[handle].latency;
-    } else if (alt && mSensorState[alt].enable) {
-        cmd->cmd = mSensorState[alt].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
-        cmd->rate = mSensorState[alt].rate;
-        cmd->latency = mSensorState[alt].latency;
-    } else { /* !alt || !mSensorState[alt].enable */
-        cmd->cmd = mSensorState[handle].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
-        cmd->rate = mSensorState[handle].rate;
-        cmd->latency = mSensorState[handle].latency;
+        }
     }
 
     // will be a nop if direct report mode is not enabled
@@ -1511,10 +1670,12 @@
 
         initConfigCmd(&cmd, handle);
 
-        ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
-        if (ret == sizeof(cmd))
+        ret = sendCmd(&cmd, sizeof(cmd));
+        if (ret == sizeof(cmd)) {
+            updateSampleRate(handle, enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE);
             ALOGV("queueActivate: sensor=%d, handle=%d, enable=%d",
                     cmd.sensorType, handle, enable);
+        }
         else
             ALOGW("queueActivate: failed to send command: sensor=%d, handle=%d, enable=%d",
                     cmd.sensorType, handle, enable);
@@ -1539,7 +1700,7 @@
 
         initConfigCmd(&cmd, handle);
 
-        ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
+        ret = sendCmd(&cmd, sizeof(cmd));
         if (ret == sizeof(cmd))
             ALOGV("queueSetDelay: sensor=%d, handle=%d, period=%" PRId64,
                     cmd.sensorType, handle, sampling_period_ns);
@@ -1571,13 +1732,15 @@
 
         initConfigCmd(&cmd, handle);
 
-        ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
-        if (ret == sizeof(cmd))
+        ret = sendCmd(&cmd, sizeof(cmd));
+        if (ret == sizeof(cmd)) {
+            updateSampleRate(handle, CONFIG_CMD_ENABLE); // batch uses CONFIG_CMD_ENABLE command
             ALOGV("queueBatch: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64,
                     cmd.sensorType, handle, sampling_period_ns, max_report_latency_ns);
-        else
+        } else {
             ALOGW("queueBatch: failed to send command: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64,
                     cmd.sensorType, handle, sampling_period_ns, max_report_latency_ns);
+        }
     } else {
         ALOGV("queueBatch: unhandled handle=%d, period=%" PRId64 ", latency=%" PRId64,
                 handle, sampling_period_ns, max_report_latency_ns);
@@ -1586,12 +1749,16 @@
 
 void HubConnection::queueFlush(int handle)
 {
+    Mutex::Autolock autoLock(mLock);
+    queueFlushInternal(handle, false);
+}
+
+void HubConnection::queueFlushInternal(int handle, bool internal)
+{
     struct ConfigCmd cmd;
     uint32_t primary;
     int ret;
 
-    Mutex::Autolock autoLock(mLock);
-
     if (isValidHandle(handle)) {
         // If no primary sensor type is specified,
         // then 'handle' is the primary sensor type.
@@ -1600,16 +1767,18 @@
 
         std::list<Flush>& flushList = mFlushesPending[primary];
 
-        if (!flushList.empty() && flushList.back().handle == handle) {
+        if (!flushList.empty() &&
+            flushList.back().internal == internal &&
+            flushList.back().handle == handle) {
             ++flushList.back().count;
         } else {
-            flushList.push_back((struct Flush){handle, 1});
+            flushList.push_back((struct Flush){handle, 1, internal});
         }
 
         initConfigCmd(&cmd, handle);
         cmd.cmd = CONFIG_CMD_FLUSH;
 
-        ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
+        ret = sendCmd(&cmd, sizeof(cmd));
         if (ret == sizeof(cmd)) {
             ALOGV("queueFlush: sensor=%d, handle=%d",
                     cmd.sensorType, handle);
@@ -1632,7 +1801,7 @@
         memcpy(cmd->data, data, length);
         cmd->cmd = CONFIG_CMD_CFG_DATA;
 
-        ret = TEMP_FAILURE_RETRY(::write(mFd, cmd, sizeof(*cmd) + length));
+        ret = sendCmd(cmd, sizeof(*cmd) + length);
         if (ret == sizeof(*cmd) + length)
             ALOGV("queueData: sensor=%d, length=%zu",
                     cmd->sensorType, length);
@@ -1702,8 +1871,44 @@
     }
 }
 
+ssize_t HubConnection::read(sensors_event_t *ev, size_t size) {
+    ssize_t n = mRing.read(ev, size);
+
+    Mutex::Autolock autoLock(mLock);
+
+    // We log the first failure in write, so only log 2+ errors
+    if (mWriteFailures > 1) {
+        ALOGW("%s: mRing.write failed %d times",
+              __FUNCTION__, mWriteFailures);
+        mWriteFailures = 0;
+    }
+
+    for (ssize_t i = 0; i < n; i++)
+        decrementIfWakeEventLocked(ev[i].sensor);
+
+    return n;
+}
+
+
 ssize_t HubConnection::write(const sensors_event_t *ev, size_t n) {
-    return mRing.write(ev, n);
+    ssize_t ret = 0;
+
+    Mutex::Autolock autoLock(mLock);
+
+    for (size_t i=0; i<n; i++) {
+        if (mRing.write(&ev[i], 1) == 1) {
+            ret++;
+            // If event is a wake event, protect it with a wakelock
+            protectIfWakeEventLocked(ev[i].sensor);
+        } else {
+            if (mWriteFailures++ == 0)
+                ALOGW("%s: mRing.write failed @ %zu/%zu",
+                      __FUNCTION__, i, n);
+            break;
+        }
+    }
+
+    return ret;
 }
 
 #ifdef USB_MAG_BIAS_REPORTING_ENABLED
@@ -1718,7 +1923,7 @@
         cmd->msg.dataLen = sizeof(float);
         memcpy((float *)(cmd+1), &mUsbMagBias, sizeof(float));
 
-        ret = TEMP_FAILURE_RETRY(::write(mFd, cmd, sizeof(*cmd) + sizeof(float)));
+        ret = sendCmd(cmd, sizeof(*cmd) + sizeof(float));
         if (ret == sizeof(*cmd) + sizeof(float))
             ALOGV("queueUsbMagBias: bias=%f\n", mUsbMagBias);
         else
@@ -1820,7 +2025,14 @@
             auto i = mSensorToChannel.find(nev->sensor);
             if (i != mSensorToChannel.end()) {
                 for (auto &j : i->second) {
-                    mDirectChannel[j.first]->write(nev);
+                    if ((uint64_t)nev->timestamp > j.second.lastTimestamp
+                            && intervalLargeEnough(
+                                nev->timestamp - j.second.lastTimestamp,
+                                rateLevelToDeviceSamplingPeriodNs(
+                                        nev->sensor, j.second.rateLevel))) {
+                        mDirectChannel[j.first]->write(nev);
+                        j.second.lastTimestamp = nev->timestamp;
+                    }
                 }
             }
             ++nev;
@@ -1830,35 +2042,31 @@
 }
 
 void HubConnection::mergeDirectReportRequest(struct ConfigCmd *cmd, int handle) {
+    int maxRateLevel = SENSOR_DIRECT_RATE_STOP;
+
     auto j = mSensorToChannel.find(handle);
     if (j != mSensorToChannel.end()) {
-        bool enable = false;
-        rate_q10_t rate;
-
-        if (!j->second.empty()) {
-            int maxRateLevel = SENSOR_DIRECT_RATE_STOP;
+        for (auto &i : j->second) {
+            maxRateLevel = std::max(i.second.rateLevel, maxRateLevel);
+        }
+    }
+    for (auto handle : mSensorState[handle].alt) {
+        auto j = mSensorToChannel.find(handle);
+        if (j != mSensorToChannel.end()) {
             for (auto &i : j->second) {
-                maxRateLevel = (i.second) > maxRateLevel ? i.second : maxRateLevel;
-            }
-            switch(maxRateLevel) {
-                case SENSOR_DIRECT_RATE_NORMAL:
-                    enable = true;
-                    rate = period_ns_to_frequency_q10(20000000ull); // NORMAL = 50Hz
-                    break;
-                case SENSOR_DIRECT_RATE_FAST:
-                    enable = true;
-                    rate = period_ns_to_frequency_q10(5000000ull);  // FAST = 200Hz
-                    break;
-                default:
-                    break;
+                maxRateLevel = std::max(i.second.rateLevel, maxRateLevel);
             }
         }
+    }
 
-        if (enable) {
-            cmd->rate = (rate > cmd->rate || cmd->cmd == CONFIG_CMD_DISABLE) ? rate : cmd->rate;
-            cmd->latency = 0;
-            cmd->cmd = CONFIG_CMD_ENABLE;
-        }
+    uint64_t period = rateLevelToDeviceSamplingPeriodNs(handle, maxRateLevel);
+    if (period != INT64_MAX) {
+        rate_q10_t rate;
+        rate = period_ns_to_frequency_q10(period);
+
+        cmd->rate = (rate > cmd->rate || cmd->cmd == CONFIG_CMD_DISABLE) ? rate : cmd->rate;
+        cmd->latency = 0;
+        cmd->cmd = CONFIG_CMD_ENABLE;
     }
 }
 
@@ -1866,6 +2074,13 @@
     std::unique_ptr<DirectChannelBase> ch;
     int ret = NO_MEMORY;
 
+    Mutex::Autolock autoLock(mDirectChannelLock);
+    for (const auto& c : mDirectChannel) {
+        if (c.second->memoryMatches(mem)) {
+            // cannot reusing same memory
+            return BAD_VALUE;
+        }
+    }
     switch(mem->type) {
         case SENSOR_DIRECT_MEM_TYPE_ASHMEM:
             ch = std::make_unique<AshmemDirectChannel>(mem);
@@ -1879,7 +2094,6 @@
 
     if (ch) {
         if (ch->isValid()) {
-            Mutex::Autolock autoLock(mDirectChannelLock);
             ret = mDirectChannelHandle++;
             mDirectChannel.insert(std::make_pair(ret, std::move(ch)));
         } else {
@@ -1941,7 +2155,7 @@
         struct ConfigCmd cmd;
         initConfigCmd(&cmd, sensor_handle);
 
-        int result = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
+        int result = sendCmd(&cmd, sizeof(cmd));
         ret = ret && (result == sizeof(cmd));
     }
     return ret ? NO_ERROR : BAD_VALUE;
@@ -1975,14 +2189,14 @@
 
     j->second.erase(channel_handle);
     if (rate_level != SENSOR_DIRECT_RATE_STOP) {
-        j->second.insert(std::make_pair(channel_handle, rate_level));
+        j->second.insert(std::make_pair(channel_handle, (DirectChannelTimingInfo){0, rate_level}));
     }
 
     Mutex::Autolock autoLock2(mLock);
     struct ConfigCmd cmd;
     initConfigCmd(&cmd, sensor_handle);
 
-    int ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
+    int ret = sendCmd(&cmd, sizeof(cmd));
 
     if (rate_level == SENSOR_DIRECT_RATE_STOP) {
         ret = NO_ERROR;
@@ -1995,6 +2209,80 @@
 bool HubConnection::isDirectReportSupported() const {
     return true;
 }
+
+void HubConnection::updateSampleRate(int handle, int reason) {
+    bool affected = mSensorToChannel.find(handle) != mSensorToChannel.end();
+    for (size_t i = 0; i < MAX_ALTERNATES && !affected; ++i) {
+        if (mSensorState[handle].alt[i] != COMMS_SENSOR_INVALID) {
+            affected |=
+                    mSensorToChannel.find(mSensorState[handle].alt[i]) != mSensorToChannel.end();
+        }
+    }
+    if (!affected) {
+        return;
+    }
+
+    switch (reason) {
+        case CONFIG_CMD_ENABLE: {
+            constexpr uint64_t PERIOD_800HZ = 1250000;
+            uint64_t period_multiplier =
+                    (frequency_q10_to_period_ns(mSensorState[handle].rate) + PERIOD_800HZ / 2)
+                        / PERIOD_800HZ;
+            uint64_t desiredTSample = PERIOD_800HZ;
+            while (period_multiplier /= 2) {
+                desiredTSample *= 2;
+            }
+            mSensorState[handle].desiredTSample = desiredTSample;
+            ALOGV("DesiredTSample for handle 0x%x set to %" PRIu64, handle, desiredTSample);
+            break;
+        }
+        case CONFIG_CMD_DISABLE:
+            mSensorState[handle].desiredTSample = INT64_MAX;
+            ALOGV("DesiredTSample 0x%x set to disable", handle);
+            break;
+        default:
+            ALOGW("%s: unexpected reason = %d, no-op", __FUNCTION__, reason);
+            break;
+    }
+}
+
+bool HubConnection::isSampleIntervalSatisfied(int handle, uint64_t timestamp) {
+    if (mSensorToChannel.find(handle) == mSensorToChannel.end()) {
+        return true;
+    }
+
+    if (mSensorState[handle].lastTimestamp >= timestamp
+            || mSensorState[handle].desiredTSample == INT64_MAX) {
+        return false;
+    } else if (intervalLargeEnough(timestamp - mSensorState[handle].lastTimestamp,
+                                   mSensorState[handle].desiredTSample)) {
+        mSensorState[handle].lastTimestamp = timestamp;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+uint64_t HubConnection::rateLevelToDeviceSamplingPeriodNs(int handle, int rateLevel) const {
+    if (mSensorToChannel.find(handle) == mSensorToChannel.end()) {
+        return INT64_MAX;
+    }
+
+    switch (rateLevel) {
+        case SENSOR_DIRECT_RATE_VERY_FAST:
+            // No sensor support VERY_FAST, fall through
+        case SENSOR_DIRECT_RATE_FAST:
+            if (handle != COMMS_SENSOR_MAG && handle != COMMS_SENSOR_MAG_UNCALIBRATED) {
+                return 2500*1000; // 400Hz
+            }
+            // fall through
+        case SENSOR_DIRECT_RATE_NORMAL:
+            return 20*1000*1000; // 50 Hz
+            // fall through
+        default:
+            return INT64_MAX;
+    }
+}
 #else // DIRECT_REPORT_ENABLED
 // nop functions if feature is turned off
 int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *) {
@@ -2018,6 +2306,13 @@
 bool HubConnection::isDirectReportSupported() const {
     return false;
 }
+
+void HubConnection::updateSampleRate(int, int) {
+}
+
+bool HubConnection::isSampleIntervalSatisfied(int, uint64_t) {
+    return true;
+}
 #endif // DIRECT_REPORT_ENABLED
 
 } // namespace android
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
index 4ad4b40..558ece3 100644
--- a/sensorhal/hubconnection.h
+++ b/sensorhal/hubconnection.h
@@ -47,6 +47,8 @@
 #define GYRO_SW_BIAS_TAG   "gyro_sw"
 #define MAG_BIAS_TAG       "mag"
 
+#define MAX_ALTERNATES     2
+
 namespace android {
 
 struct HubConnection : public Thread {
@@ -74,10 +76,7 @@
 
     void setOperationParameter(const additional_info_event_t &info);
 
-    bool isWakeEvent(int32_t sensor);
     void releaseWakeLockIfAppropriate();
-    ssize_t getWakeEventCount();
-    ssize_t decrementWakeEventCount();
 
     //TODO: factor out event ring buffer functionality into a separate class
     ssize_t read(sensors_event_t *ev, size_t size);
@@ -92,6 +91,8 @@
         mScaleMag = scaleMag;
     }
 
+    void setLeftyMode(bool enable);
+
 protected:
     HubConnection();
     virtual ~HubConnection();
@@ -104,7 +105,8 @@
     bool mWakelockHeld;
     int32_t mWakeEventCount;
 
-    void protectIfWakeEvent(int32_t sensor);
+    void protectIfWakeEventLocked(int32_t sensor);
+    ssize_t decrementIfWakeEventLocked(int32_t sensor);
 
     static inline uint64_t period_ns_to_frequency_q10(nsecs_t period_ns) {
         return 1024000000000ULL / period_ns;
@@ -147,18 +149,31 @@
         struct HostHubRawPacket msg;
     } __attribute__((packed));
 
+    struct LeftyState
+    {
+        bool accel; // Process wrist-aware accel samples as lefty mode
+        bool gyro; // Process wrist-aware gyro samples as lefty mode
+        bool hub; // Sensor hub is currently operating in lefty mode
+    };
+
     struct Flush
     {
         int handle;
         uint8_t count;
+
+        // Used to synchronize the transition in and out of
+        // lefty mode between nanohub and the AP.
+        bool internal;
     };
 
     struct SensorState {
         uint64_t latency;
+        uint64_t lastTimestamp;
+        uint64_t desiredTSample;
         rate_q10_t rate;
         uint8_t sensorType;
         uint8_t primary;
-        uint8_t alt;
+        uint8_t alt[MAX_ALTERNATES];
         bool enable;
     };
 
@@ -224,6 +239,7 @@
     Mutex mLock;
 
     RingBuffer mRing;
+    int32_t mWriteFailures;
 
     ActivityEventHandler *mActivityEventHandler;
 
@@ -236,6 +252,8 @@
 
     float mScaleAccel, mScaleMag;
 
+    LeftyState mLefty;
+
     SensorState mSensorState[NUM_COMMS_SENSORS_PLUS_1];
     std::list<struct Flush> mFlushesPending[NUM_COMMS_SENSORS_PLUS_1];
 
@@ -262,8 +280,11 @@
             && mSensorState[handle].sensorType;
     }
 
+    ssize_t sendCmd(const void *buf, size_t count);
     void initConfigCmd(struct ConfigCmd *cmd, int handle);
 
+    void queueFlushInternal(int handle, bool internal);
+
     void queueDataInternal(int handle, void *data, size_t length);
 
     void discardInotifyEvent();
@@ -304,12 +325,24 @@
 private:
     void sendDirectReportEvent(const sensors_event_t *nev, size_t n);
     void mergeDirectReportRequest(struct ConfigCmd *cmd, int handle);
+    bool isSampleIntervalSatisfied(int handle, uint64_t timestamp);
+    void updateSampleRate(int handle, int reason);
 #ifdef DIRECT_REPORT_ENABLED
     int stopAllDirectReportOnChannel(
             int channel_handle, std::vector<int32_t> *unstoppedSensors);
+    uint64_t rateLevelToDeviceSamplingPeriodNs(int handle, int rateLevel) const;
+    inline static bool intervalLargeEnough(uint64_t actual, uint64_t desired) {
+        return (actual + (actual >> 4)) >= desired; // >= 94.11% of desired
+    }
+
+    struct DirectChannelTimingInfo{
+        uint64_t lastTimestamp;
+        int rateLevel;
+    };
     Mutex mDirectChannelLock;
-    //sensor_handle=>(channel_handle, rate_level)
-    std::unordered_map<int32_t, std::unordered_map<int32_t, int32_t> > mSensorToChannel;
+    //sensor_handle=>(channel_handle => DirectChannelTimingInfo)
+    std::unordered_map<int32_t,
+            std::unordered_map<int32_t, DirectChannelTimingInfo> > mSensorToChannel;
     //channel_handle=>ptr of Channel obj
     std::unordered_map<int32_t, std::unique_ptr<DirectChannelBase>> mDirectChannel;
     int32_t mDirectChannelHandle;
diff --git a/sensorhal/hubdefs.h b/sensorhal/hubdefs.h
index 9664e2b..f63cc42 100644
--- a/sensorhal/hubdefs.h
+++ b/sensorhal/hubdefs.h
@@ -84,6 +84,8 @@
     COMMS_SENSOR_UNGAZE                      = 46,
     COMMS_SENSOR_ACCEL_UNCALIBRATED          = 47,
     COMMS_SENSOR_HUMIDITY                    = 48,
+    COMMS_SENSOR_ACCEL_WRIST_AWARE           = 49,
+    COMMS_SENSOR_GYRO_WRIST_AWARE            = 50,
     COMMS_SENSOR_AMBIENT_TEMPERATURE         = 51,
 
     NUM_COMMS_SENSORS_PLUS_1,
@@ -118,6 +120,8 @@
     SENSOR_TYPE_DOUBLE_TOUCH            = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 4,
     SENSOR_TYPE_GAZE                    = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 5,
     SENSOR_TYPE_UNGAZE                  = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 6,
+    SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE=SENSOR_TYPE_DEVICE_PRIVATE_BASE + 7,
+    SENSOR_TYPE_GYROSCOPE_WRIST_AWARE   = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 8,
 };
 
 }  // namespace android
diff --git a/sensorhal/sensors.cpp b/sensorhal/sensors.cpp
index 65b881c..234f7da 100644
--- a/sensorhal/sensors.cpp
+++ b/sensorhal/sensors.cpp
@@ -35,6 +35,10 @@
 #include <SensorEventCallback.h>
 #endif
 
+#ifdef LEFTY_SERVICE_ENABLED
+#include "lefty_service.h"
+#endif
+
 using namespace android;
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -101,28 +105,7 @@
     // Release wakelock if held and no more events in ring buffer
     mHubConnection->releaseWakeLockIfAppropriate();
 
-    ssize_t n = mHubConnection->read(data, count);
-
-    if (n < 0) {
-        return -1;
-    }
-
-    // If we have wake events in the queue, determine how many we're sending
-    // up this round and decrement that count now so that when we get called back,
-    // we'll have an accurate count of how many wake events are STILL in the HAL queue
-    // to be able to determine whether we can release our wakelock if held.
-    if (mHubConnection->getWakeEventCount() != 0) {
-        for (ssize_t i = 0; i < n; i++) {
-            if (mHubConnection->isWakeEvent(data[i].sensor)) {
-                ssize_t count = mHubConnection->decrementWakeEventCount();
-                if (count == 0) {
-                    break;
-                }
-            }
-        }
-    }
-
-    return n;
+    return mHubConnection->read(data, count);
 }
 
 int SensorContext::batch(
@@ -407,6 +390,9 @@
     gHubAlive = ctx->getHubAlive();
     *dev = &ctx->device.common;
 
+#ifdef LEFTY_SERVICE_ENABLED
+    register_lefty_service();
+#endif
     return 0;
 }
 
diff --git a/util/common/ring.cpp b/util/common/ring.cpp
index 26d44d6..245f3d0 100644
--- a/util/common/ring.cpp
+++ b/util/common/ring.cpp
@@ -22,6 +22,7 @@
 
 #include <stdlib.h>
 #include <string.h>
+#include <atomic>
 
 namespace android {
 
@@ -120,8 +121,18 @@
     }
 
     while(size--) {
-        mData[mWritePos] = *(ev++);
+        // part before reserved0 field
+        memcpy(&mData[mWritePos], ev, offsetof(sensors_event_t, reserved0));
+        // part after reserved0 field
+        memcpy(reinterpret_cast<char *>(&mData[mWritePos]) + offsetof(sensors_event_t, timestamp),
+               reinterpret_cast<const char *>(ev) + offsetof(sensors_event_t, timestamp),
+               sizeof(sensors_event_t) - offsetof(sensors_event_t, timestamp));
+        // barrier before writing the atomic counter
+        std::atomic_thread_fence(std::memory_order_release);
         mData[mWritePos].reserved0 = mCounter++;
+        // barrier after writing the atomic counter
+        std::atomic_thread_fence(std::memory_order_release);
+        ++ev;
 
         if (++mWritePos >= mSize) {
             mWritePos = 0;
diff --git a/util/nanoapp_cmd/nanoapp_cmd.c b/util/nanoapp_cmd/nanoapp_cmd.c
index 28456d6..5692181 100644
--- a/util/nanoapp_cmd/nanoapp_cmd.c
+++ b/util/nanoapp_cmd/nanoapp_cmd.c
@@ -484,7 +484,7 @@
                     LOGE("Download failed after %d retries; erasing all apps "
                          "before final attempt", i);
                     eraseSharedArea();
-                    uninstallCnt = 0;
+                    parseConfigAppInfo(&installCnt, &uninstallCnt);
                 }
                 removeApps(uninstallCnt);
                 downloadApps(installCnt);