nanohub: isl29034: fix build error when DEBUG not defined
am: 3fac23e689
Change-Id: Id4dc90f12aa03176fc411964054f7cfe7737354a
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);
