firmware: Add neonkey Android build config
am: a9c3479b81
Change-Id: I0ba262f10013a8ed7d18ca1abccb3ffc4289fe4d
diff --git a/CleanSpec.mk b/CleanSpec.mk
new file mode 100644
index 0000000..be0d4a0
--- /dev/null
+++ b/CleanSpec.mk
@@ -0,0 +1,54 @@
+# Copyright 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.
+#
+
+# If you don't need to do a full clean build but would like to touch
+# a file or delete some intermediate files, add a clean step to the end
+# of the list. These steps will only be run once, if they haven't been
+# run before.
+#
+# E.g.:
+# $(call add-clean-step, touch -c external/sqlite/sqlite3.h)
+# $(call add-clean-step, rm -rf $(PRODUCT_OUT)/obj/STATIC_LIBRARIES/libz_intermediates)
+#
+# Always use "touch -c" and "rm -f" or "rm -rf" to gracefully deal with
+# files that are missing or have been moved.
+#
+# Use $(PRODUCT_OUT) to get to the "out/target/product/blah/" directory.
+# Use $(OUT_DIR) to refer to the "out" directory.
+#
+# If you need to re-do something that's already mentioned, just copy
+# the command and add it to the bottom of the list. E.g., if a change
+# that you made last week required touching a file and a change you
+# made today requires touching the same file, just copy the old
+# touch step and add it to the end of the list.
+#
+# ************************************************
+# NEWER CLEAN STEPS MUST BE AT THE END OF THE LIST
+# ************************************************
+
+# For example:
+#$(call add-clean-step, rm -rf $(OUT_DIR)/target/common/obj/APPS/AndroidTests_intermediates)
+#$(call add-clean-step, rm -rf $(OUT_DIR)/target/common/obj/JAVA_LIBRARIES/core_intermediates)
+#$(call add-clean-step, find $(OUT_DIR) -type f -name "IGTalkSession*" -print0 | xargs -0 rm -f)
+#$(call add-clean-step, rm -rf $(PRODUCT_OUT)/data/*)
+
+$(call add-clean-step, rm -f $(PRODUCT_OUT)/system/lib/hw/context_hub.default.so)
+$(call add-clean-step, rm -f $(PRODUCT_OUT)/system/lib64/hw/context_hub.default.so)
+$(call add-clean-step, rm -f $(PRODUCT_OUT)/system/lib/hw/sensors.*.so)
+$(call add-clean-step, rm -f $(PRODUCT_OUT)/system/lib64/hw/sensors.*.so)
+$(call add-clean-step, rm -f $(PRODUCT_OUT)/system/lib/hw/activity_recognition.*.so)
+$(call add-clean-step, rm -f $(PRODUCT_OUT)/system/lib64/hw/activity_recognition.*.so)
+$(call add-clean-step, rm -f $(PRODUCT_OUT)/system/lib/libhubconnection.so)
+$(call add-clean-step, rm -f $(PRODUCT_OUT)/system/lib64/libhubconnection.so)
diff --git a/contexthubhal/Android.mk b/contexthubhal/Android.mk
index 2b9f0e4..1600e1b 100644
--- a/contexthubhal/Android.mk
+++ b/contexthubhal/Android.mk
@@ -18,4 +18,6 @@
LOCAL_MODULE := context_hub.default
LOCAL_MODULE_TAGS := optional
+LOCAL_PROPRIETARY_MODULE := true
+
include $(BUILD_SHARED_LIBRARY)
diff --git a/contexthubhal/nanohubhal.cpp b/contexthubhal/nanohubhal.cpp
index d54900a..fa3c5f8 100644
--- a/contexthubhal/nanohubhal.cpp
+++ b/contexthubhal/nanohubhal.cpp
@@ -39,7 +39,7 @@
#include "system_comms.h"
#include "nanohubhal.h"
-#define NANOHUB_LOCK_DIR "/data/system/nanohub_lock"
+#define NANOHUB_LOCK_DIR "/data/vendor/sensor/nanohub_lock"
#define NANOHUB_LOCK_FILE NANOHUB_LOCK_DIR "/lock"
#define NANOHUB_LOCK_DIR_PERMS (S_IRUSR | S_IWUSR | S_IXUSR)
diff --git a/firmware/app/chre/common/chre_app.c b/firmware/app/chre/common/chre_app.c
index cd3c7e7..c828314 100644
--- a/firmware/app/chre/common/chre_app.c
+++ b/firmware/app/chre/common/chre_app.c
@@ -257,6 +257,18 @@
u.msg.messageSize = hdr->size;
break;
}
+ case EVT_APP_SENSOR_SELF_TEST:
+ case EVT_APP_SENSOR_MARSHALL:
+ case EVT_APP_SENSOR_SEND_ONE_DIR_EVT:
+ case EVT_APP_SENSOR_CFG_DATA:
+ case EVT_APP_SENSOR_CALIBRATE:
+ case EVT_APP_SENSOR_TRIGGER:
+ case EVT_APP_SENSOR_FLUSH:
+ case EVT_APP_SENSOR_SET_RATE:
+ case EVT_APP_SENSOR_FW_UPLD:
+ case EVT_APP_SENSOR_POWER:
+ // sensor events; pass through
+ break;
default:
// ignore any other system events; OS may send them to any app
if (evt < EVT_NO_FIRST_USER_EVENT)
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index a789ce9..0a6d96d 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -17,7 +17,6 @@
#include "calibration/accelerometer/accel_cal.h"
#include <errno.h>
#include <math.h>
-#include <seos.h>
#include <stdio.h>
#include <string.h>
#include "calibration/magnetometer/mag_cal.h"
@@ -46,6 +45,10 @@
62 // Putting all Temp counts in last bucket for temp > 62 degree C.
#define HIST_COUNT 9
#endif
+#ifdef IMU_TEMP_DBG_ENABLED
+#define IMU_TEMP_DELTA_TIME_NANOS \
+ 5000000000 // Printing every 5 seconds IMU temp.
+#endif
/////////// Start Debug //////////////////////
@@ -90,7 +93,7 @@
int index = 0;
// Take temp at every stillness detection.
- adf->start_time = 0;
+ adf->start_time_nanos = 0;
if (temp <= TEMP_HIST_LOW) {
adf->t_hist[0] += 1;
return;
@@ -177,24 +180,28 @@
acc->x_bias = acc->y_bias = acc->z_bias = 0;
acc->x_bias_new = acc->y_bias_new = acc->z_bias_new = 0;
+
+#ifdef IMU_TEMP_DBG_ENABLED
+ acc->temp_time_nanos = 0;
+#endif
}
// Stillness time check.
static int stillnessBatchComplete(struct AccelStillDet *asd,
- uint64_t sample_time_nsec) {
+ uint64_t sample_time_nanos) {
int complete = 0;
// Checking if enough data is accumulated to calc Mean and Var.
- if ((sample_time_nsec - asd->start_time > asd->min_batch_window) &&
+ if ((sample_time_nanos - asd->start_time > asd->min_batch_window) &&
(asd->nsamples > asd->min_batch_size)) {
- if (sample_time_nsec - asd->start_time < asd->max_batch_window) {
+ if (sample_time_nanos - asd->start_time < asd->max_batch_window) {
complete = 1;
} else {
// Checking for too long batch window, if yes reset and start over.
asdReset(asd);
return complete;
}
- } else if (sample_time_nsec - asd->start_time > asd->min_batch_window &&
+ } else if (sample_time_nanos - asd->start_time > asd->min_batch_window &&
(asd->nsamples < asd->min_batch_size)) {
// Not enough samples collected in max_batch_window during sample window.
asdReset(asd);
@@ -207,7 +214,7 @@
// Stillness Detection.
static int accelStillnessDetection(struct AccelStillDet *asd,
- uint64_t sample_time_nsec, float x, float y,
+ uint64_t sample_time_nanos, float x, float y,
float z) {
float inv = 0.0f;
int complete = 0.0f;
@@ -223,9 +230,9 @@
// Setting a new start time and wait until T0 is reached.
if (++asd->nsamples == 1) {
- asd->start_time = sample_time_nsec;
+ asd->start_time = sample_time_nanos;
}
- if (stillnessBatchComplete(asd, sample_time_nsec)) {
+ if (stillnessBatchComplete(asd, sample_time_nanos)) {
// Getting 1/#samples and checking asd->nsamples != 0.
if (0 < asd->nsamples) {
inv = 1.0f / asd->nsamples;
@@ -462,19 +469,33 @@
}
// Accel Cal Runner.
-void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x,
+void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
float y, float z, float temp) {
// Scaling to 1g, better for the algorithm.
x *= KSCALE;
y *= KSCALE;
z *= KSCALE;
+ // DBG: IMU temp messages every 5s.
+#ifdef IMU_TEMP_DBG_ENABLED
+ if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) {
+ CAL_DEBUG_LOG("IMU Temp Data: ",
+ ", %s%d.%02d, %llu, %s%d.%05d, %s%d.%05d, %s%d.%05d \n",
+ CAL_ENCODE_FLOAT(temp, 2),
+ (unsigned long long int)sample_time_nanos,
+ CAL_ENCODE_FLOAT(acc->x_bias_new,5),
+ CAL_ENCODE_FLOAT(acc->y_bias_new,5),
+ CAL_ENCODE_FLOAT(acc->z_bias_new,5));
+ acc->temp_time_nanos = sample_time_nanos;
+ }
+#endif
+
int temp_gate = 0;
// Temp GATE.
if (temp < MAX_TEMP && temp > MIN_TEMP) {
// Checking if accel is still.
- if (accelStillnessDetection(&acc->asd, sample_time_nsec, x, y, z)) {
+ if (accelStillnessDetection(&acc->asd, sample_time_nanos, x, y, z)) {
#ifdef ACCEL_CAL_DBG_ENABLED
// Creating temp hist data.
accelTempHisto(&acc->adf, temp);
@@ -527,7 +548,7 @@
acc->adf.e_z[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_z;
acc->adf.var_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.var_t;
acc->adf.mean_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.mean_t;
- acc->adf.cal_time[acc->adf.n_o] = sample_time_nsec;
+ acc->adf.cal_time[acc->adf.n_o] = sample_time_nanos;
acc->adf.rad[acc->adf.n_o] = radius;
acc->adf.n_o += 1;
#endif
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index 03c65e9..5c63d78 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -95,7 +95,7 @@
struct AccelStatsMem {
// Temp (in degree C).
uint32_t t_hist[TEMP_HISTOGRAM];
- uint64_t start_time;
+ uint64_t start_time_nanos;
// Offset update counter.
uint32_t noff;
@@ -136,22 +136,27 @@
#endif
// Offsets are only updated while the accelerometer is not running. Hence need
- // to store a new offset,
- // which gets updated during a power down event.
+ // to store a new offset, which gets updated during a power down event.
float x_bias_new, y_bias_new, z_bias_new;
// Offset values that get subtracted from live data
float x_bias, y_bias, z_bias;
+
+#ifdef IMU_TEMP_DBG_ENABLED
+ // Temporary time variable used to to print an IMU temperature value with a
+ // lower custom sample rate.
+ uint64_t temp_time_nanos;
+#endif
};
/* This function runs the accel calibration algorithm.
- * sample_time_nsec -> is the sensor timestamp in ns and
+ * sample_time_nanos -> is the sensor timestamp in ns and
* is used to check the stillness time.
* x,y,z -> is the sensor data (m/s^2) for the three axes.
* Data is converted to g’s inside the function.
* temp -> is the temperature of the IMU (degree C).
*/
-void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x,
+void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
float y, float z, float temp);
/* This function initializes the accCalRun data struct.
diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/common/diversity_checker.c
index 312be69..198a08d 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.c
+++ b/firmware/os/algos/calibration/common/diversity_checker.c
@@ -22,6 +22,9 @@
#include <string.h>
#include "common/math/vec.h"
+#define MAX_FIT_MAG 70.0f
+#define MIN_FIT_MAG 20.0f
+
// Struct initialization.
void diversityCheckerInit(
struct DiversityChecker* diverse_data,
@@ -188,7 +191,7 @@
void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
float local_field) {
- if ( local_field > 0 ) {
+ if ((local_field < MAX_FIT_MAG) && (local_field > MIN_FIT_MAG)) {
// Updating threshold based on the local field information.
diverse_data->threshold = diverse_data->threshold_tuning_param_sq *
(local_field * local_field);
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index e6bc275..34bf153 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -27,10 +27,10 @@
// Maximum gyro bias correction (should be set based on expected max bias
// of the given sensor).
-#define MAX_GYRO_BIAS (0.096f) // [rad/sec]
+#define MAX_GYRO_BIAS (0.1f) // [rad/sec]
// Converts units of radians to milli-degrees.
-#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI)
+#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI)
#ifdef GYRO_CAL_DBG_ENABLED
// The time value used to throttle debug messaging.
@@ -102,7 +102,10 @@
GYRO_MINMAX_STILLNESS_MEAN,
ACCEL_STATS,
GYRO_STATS,
- MAG_STATS
+ MAG_STATS,
+ ACCEL_STATS_TUNING,
+ GYRO_STATS_TUNING,
+ MAG_STATS_TUNING
};
/*
@@ -579,10 +582,11 @@
return;
}
- // Check for timeout condition of watchdog.
+ // Check for the watchdog timeout condition (i.e., the time elapsed since the
+ // last received sample has exceeded the allowed watchdog duration).
watchdog_timeout =
- ((sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos) >
- gyro_cal->gyro_watchdog_timeout_duration_nanos);
+ (sample_time_nanos > gyro_cal->gyro_watchdog_timeout_duration_nanos +
+ gyro_cal->gyro_watchdog_start_nanos);
// If a timeout occurred then reset to known good state.
if (watchdog_timeout) {
@@ -759,9 +763,9 @@
}
#ifdef GYRO_CAL_DBG_ENABLED
if (mean_not_stable) {
- CAL_DEBUG_LOG(
- "[GYRO_CAL:MEAN_STABILITY_GATE]",
- "Exceeded the max variation in the stillness window mean values.");
+ CAL_DEBUG_LOG("[GYRO_CAL:MEAN_STABILITY_GATE]",
+ "Exceeded the max variation in the gyro's stillness "
+ "window mean values.");
}
#endif // GYRO_CAL_DBG_ENABLED
break;
@@ -1034,26 +1038,76 @@
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.%06d, "
- "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
- (unsigned long int)gyro_cal->debug_calibration_count,
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 6),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 6),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 6),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0], 8),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1], 8),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2], 8));
+ CAL_DEBUG_LOG(debug_tag,
+ "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 8),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 8),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 8));
} else {
CAL_DEBUG_LOG(debug_tag,
"Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
"-1.0",
(unsigned long int)gyro_cal->debug_calibration_count);
}
-
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.%06d, "
+ "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+ CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_x, 6),
+ CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_y, 6),
+ CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_z, 6),
+ CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 8),
+ CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 8),
+ CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 8));
+ break;
+
+ case GYRO_STATS_TUNING:
+ CAL_DEBUG_LOG(
+ debug_tag,
+ "Gyro Mean|Var [mdps|(rad/sec)^2]: %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+ "%s%d.%08d, %s%d.%08d, %s%d.%08d",
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x, 8),
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y, 8),
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z, 8));
+ break;
+
+ case MAG_STATS_TUNING:
+ if (gyro_cal->using_mag_sensor) {
+ CAL_DEBUG_LOG(
+ debug_tag,
+ "Mag Mean|Var [uT|uT^2]: %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+ "%s%d.%08d, %s%d.%08d, %s%d.%08d",
+ CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_x, 6),
+ CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_y, 6),
+ CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_z, 6),
+ CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 8),
+ CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 8),
+ CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 8));
+ } 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;
}
@@ -1080,7 +1134,7 @@
case GYRO_WAIT_STATE:
// This helps throttle the print statements.
- if ((timestamp_nanos - wait_timer_nanos) >= GYROCAL_WAIT_TIME_NANOS) {
+ if (timestamp_nanos >= GYROCAL_WAIT_TIME_NANOS + wait_timer_nanos) {
gyro_cal->debug_state = next_state;
}
break;
@@ -1159,22 +1213,24 @@
static uint64_t wait_timer_nanos = 0;
// Output sensor variance levels to assist with tuning thresholds.
- // i. Within the first 180 seconds of boot: output interval = 5
+ // i. Within the first 300 seconds of boot: output interval = 5
// seconds.
// ii. Thereafter: output interval is 60 seconds.
bool condition_i =
- ((timestamp_nanos <= 180000000000) &&
- ((timestamp_nanos - wait_timer_nanos) > 5000000000)); // nsec
- bool condition_ii = ((timestamp_nanos > 60000000000) &&
- ((timestamp_nanos - wait_timer_nanos) > 60000000000));
+ ((timestamp_nanos <= 300000000000) &&
+ (timestamp_nanos > 5000000000 + wait_timer_nanos)); // nsec
+ bool condition_ii = (timestamp_nanos > 60000000000 + wait_timer_nanos);
// 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, "");
- debug_state = GYRO_PRINT_OFFSET;
+ 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;
}
@@ -1182,34 +1238,27 @@
case GYRO_WAIT_STATE:
// This helps throttle the print statements.
- if ((timestamp_nanos - wait_timer_nanos) >= GYROCAL_WAIT_TIME_NANOS) {
+ if (timestamp_nanos >= GYROCAL_WAIT_TIME_NANOS + wait_timer_nanos) {
debug_state = next_state;
}
break;
- case GYRO_PRINT_OFFSET:
- gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, OFFSET);
- 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.
- break;
-
case GYRO_PRINT_ACCEL_STATS:
- gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, 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);
+ 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);
+ 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.
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
index 0b38e5b..80f2fa2 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
@@ -193,11 +193,11 @@
tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh);
gyro_still_det->stillness_confidence =
gyroStillDetLimit(
- 0.5 - (gyro_still_det->win_var_x - var_thresh) * tmp_denom) *
+ 0.5f - (gyro_still_det->win_var_x - var_thresh) * tmp_denom) *
gyroStillDetLimit(
- 0.5 - (gyro_still_det->win_var_y - var_thresh) * tmp_denom) *
+ 0.5f - (gyro_still_det->win_var_y - var_thresh) * tmp_denom) *
gyroStillDetLimit(
- 0.5 - (gyro_still_det->win_var_z - var_thresh) * tmp_denom);
+ 0.5f - (gyro_still_det->win_var_z - var_thresh) * tmp_denom);
}
}
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 f1d02b5..ebc8cf4 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -26,23 +26,18 @@
/////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
-// The 'temp_sensitivity' parameters are set to this value to indicate that the
-// model is in its initial state.
-#define MODEL_INITIAL_STATE (1e6f)
-
// Rate-limits the search for the nearest offset estimate to every 2 seconds.
#define OVERTEMPCAL_NEAREST_NANOS (2000000000)
// Rate-limits the check of old data to every 2 hours.
#define OVERTEMPCAL_STALE_CHECK_TIME_NANOS (7200000000000)
-// A common sensor operating temperature at which to start producing the model
-// jump-start data.
-#define JUMPSTART_START_TEMP_CELSIUS (30.0f)
+// Value used to check whether OTC model parameters are near zero.
+#define OTC_MODELDATA_NEAR_ZERO_TOL (1e-7f) // [rad/sec]
#ifdef OVERTEMPCAL_DBG_ENABLED
// A debug version label to help with tracking results.
-#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Jan 20, 2017]"
+#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Apr 05, 2017]"
// The time value used to throttle debug messaging.
#define OVERTEMPCAL_WAIT_TIME_NANOS (300000000)
@@ -51,7 +46,7 @@
#define OVERTEMPCAL_REPORT_TAG "[OVER_TEMP_CAL:REPORT]"
// Converts units of radians to milli-degrees.
-#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI)
+#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI)
// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
static const char kDebugAxisLabel[3] = "XYZ";
@@ -59,7 +54,7 @@
/////// FORWARD DECLARATIONS //////////////////////////////////////////////////
-// Updates the most recent model estimate data.
+// Updates the model estimate data nearest to the sensor's temperature.
static void setNearestEstimate(struct OverTempCal *over_temp_cal,
const float *offset, float offset_temp_celsius,
uint64_t timestamp_nanos);
@@ -82,6 +77,23 @@
static void findNearestEstimate(struct OverTempCal *over_temp_cal);
/*
+ * Provides the current over-temperature compensated offset vector.
+ *
+ * INPUTS:
+ * over_temp_cal: Over-temp data structure.
+ * timestamp_nanos: The current system timestamp.
+ * OUTPUTS:
+ * compensated_offset: Temperature compensated offset estimate array.
+ * compensated_offset_temperature_celsius: Compensated offset temperature.
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ */
+static void getCalOffset(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ float *compensated_offset_temperature_celsius,
+ float *compensated_offset);
+
+/*
* Removes the "old" offset estimates from 'model_data' (i.e., eliminates the
* drift-compromised data). Returns 'true' if any data was removed.
*/
@@ -121,19 +133,6 @@
float *temp_sensitivity, float *sensor_intercept);
/*
- * Removes the over-temp compensated offset from the input sensor data.
- *
- * INPUTS:
- * over_temp_cal: Over-temp data structure.
- * axis_in: Single axis sensor data to be compensated.
- * index: Index for model parameter compensation (0=x, 1=y, 2=z).
- * OUTPUTS:
- * axis_out: Single axis sensor data that has been compensated.
- */
-static void removeSensorOffset(const struct OverTempCal *over_temp_cal,
- float axis_in, size_t index, float *axis_out);
-
-/*
* Checks new offset estimates to determine if they could be an outlier that
* should be rejected. Operates on a per-axis basis determined by 'axis_index'.
*
@@ -148,6 +147,63 @@
static bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
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));
+}
+
+// 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 > OVERTEMPCAL_TEMP_MAX_CELSIUS) {
+ *temperature_celsius = OVERTEMPCAL_TEMP_MAX_CELSIUS;
+ return false;
+ }
+ if (*temperature_celsius < OVERTEMPCAL_TEMP_MIN_CELSIUS) {
+ *temperature_celsius = OVERTEMPCAL_TEMP_MIN_CELSIUS;
+ return false;
+ }
+ return true;
+}
+
+// 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;
+}
+
+// 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);
+}
+
#ifdef OVERTEMPCAL_DBG_ENABLED
// This helper function stores all of the debug tracking information necessary
// for printing log messages.
@@ -171,11 +227,8 @@
// as the first element in 'model_data'.
over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
- // Sets the temperature sensitivity model parameters to MODEL_INITIAL_STATE to
- // indicate that the model is in an "initial" state.
- over_temp_cal->temp_sensitivity[0] = MODEL_INITIAL_STATE;
- over_temp_cal->temp_sensitivity[1] = MODEL_INITIAL_STATE;
- over_temp_cal->temp_sensitivity[2] = MODEL_INITIAL_STATE;
+ // Initializes the OTC linear model parameters.
+ resetOtcLinearModel(over_temp_cal);
// Initializes the model identification parameters.
over_temp_cal->new_overtemp_model_available = false;
@@ -188,6 +241,9 @@
over_temp_cal->sensor_intercept_limit = sensor_intercept_limit;
over_temp_cal->over_temp_enable = over_temp_enable;
+ // Initialize the sensor's temperature with a good initial operating point.
+ over_temp_cal->temperature_celsius = JUMPSTART_START_TEMP_CELSIUS;
+
#ifdef OVERTEMPCAL_DBG_ENABLED
CAL_DEBUG_LOG("[OVER_TEMP_CAL:MEMORY]", "sizeof(struct OverTempCal): %lu",
(unsigned long int)sizeof(struct OverTempCal));
@@ -211,11 +267,16 @@
ASSERT_NOT_NULL(temp_sensitivity);
ASSERT_NOT_NULL(sensor_intercept);
+ // Initializes the OTC linear model parameters.
+ resetOtcLinearModel(over_temp_cal);
+
// 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++) {
- if (NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit &&
- NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) {
+ if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
+ sensor_intercept[i])) {
over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
}
@@ -229,18 +290,55 @@
(jump_start_model) ? jumpStartModelData(over_temp_cal) : false;
if (!model_jump_started) {
- // Sets the initial over-temp calibration estimate and model data.
- setNearestEstimate(over_temp_cal, offset, offset_temp_celsius,
- timestamp_nanos);
-
- // Now there is one offset estimate in the model.
- over_temp_cal->num_model_pts = 1;
+ // Checks that the new offset data is valid.
+ if (isValidOtcOffset(offset, offset_temp_celsius)) {
+ // Sets the initial over-temp calibration estimate and model data.
+ over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
+ setNearestEstimate(over_temp_cal, offset, offset_temp_celsius,
+ timestamp_nanos);
+ over_temp_cal->num_model_pts = 1;
+ } else {
+ // No valid offset data to load.
+ over_temp_cal->num_model_pts = 0;
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
+ "No valid sensor offset vector to load.");
+#endif // OVERTEMPCAL_DBG_ENABLED
+ }
+ } else {
+ // Finds the offset nearest the sensor's current temperature.
+ findNearestEstimate(over_temp_cal);
}
+ // Updates the 'compensated_offset_previous' vector to prevent from
+ // immediately triggering a new calibration update.
+ float compensated_offset_temperature_celsius = 0.0f;
+ getCalOffset(over_temp_cal, timestamp_nanos,
+ &compensated_offset_temperature_celsius,
+ over_temp_cal->compensated_offset_previous);
+
#ifdef OVERTEMPCAL_DBG_ENABLED
// Prints the updated model data.
- CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
- "Over-temperature model parameters recalled.");
+ CAL_DEBUG_LOG(
+ "[OVER_TEMP_CAL:RECALL]",
+ "Temperature|Offset|Sensitivity|Intercept [rps]: %s%d.%06d, | %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d | %s%d.%06d, %s%d.%06d, %s%d.%06d | "
+ "%s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d",
+ CAL_ENCODE_FLOAT(offset_temp_celsius, 6),
+ CAL_ENCODE_FLOAT(offset[0], 6),
+ CAL_ENCODE_FLOAT(offset[1], 6),
+ CAL_ENCODE_FLOAT(offset[2], 6),
+ CAL_ENCODE_FLOAT(temp_sensitivity[0], 6),
+ CAL_ENCODE_FLOAT(temp_sensitivity[1], 6),
+ CAL_ENCODE_FLOAT(temp_sensitivity[2], 6),
+ CAL_ENCODE_FLOAT(sensor_intercept[0], 6),
+ CAL_ENCODE_FLOAT(sensor_intercept[1], 6),
+ CAL_ENCODE_FLOAT(sensor_intercept[2], 6));
+
+ // Resets the debug print machine to ensure that updateDebugData() can
+ // produce a debug report and interupt any ongoing report.
+ over_temp_cal->debug_state = OTC_IDLE;
// Triggers a debug print out to view the new model parameters.
updateDebugData(over_temp_cal);
@@ -258,54 +356,149 @@
ASSERT_NOT_NULL(temp_sensitivity);
ASSERT_NOT_NULL(sensor_intercept);
- // Gets the over-temp calibration estimate and model data.
- memcpy(offset, over_temp_cal->nearest_offset->offset, 3 * sizeof(float));
+ // 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));
- *offset_temp_celsius = over_temp_cal->nearest_offset->offset_temp_celsius;
- *timestamp_nanos = over_temp_cal->nearest_offset->timestamp_nanos;
+ *timestamp_nanos = over_temp_cal->modelupdate_timestamp_nanos;
+
+ // Gets the latest temperature compensated offset estimate.
+ getCalOffset(over_temp_cal, *timestamp_nanos, offset_temp_celsius, offset);
#ifdef OVERTEMPCAL_DBG_ENABLED
- CAL_DEBUG_LOG("[OVER_TEMP_CAL:STORED]",
- "Over-temperature model parameters stored.");
+ // Prints the updated model data.
+ CAL_DEBUG_LOG(
+ "[OVER_TEMP_CAL:STORED]",
+ "Temperature|Offset|Sensitivity|Intercept [rps]: %s%d.%06d, | %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d | %s%d.%06d, %s%d.%06d, %s%d.%06d | "
+ "%s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d",
+ CAL_ENCODE_FLOAT(*offset_temp_celsius, 6),
+ CAL_ENCODE_FLOAT(offset[0], 6),
+ CAL_ENCODE_FLOAT(offset[1], 6),
+ CAL_ENCODE_FLOAT(offset[2], 6),
+ CAL_ENCODE_FLOAT(temp_sensitivity[0], 6),
+ CAL_ENCODE_FLOAT(temp_sensitivity[1], 6),
+ CAL_ENCODE_FLOAT(temp_sensitivity[2], 6),
+ CAL_ENCODE_FLOAT(sensor_intercept[0], 6),
+ CAL_ENCODE_FLOAT(sensor_intercept[1], 6),
+ CAL_ENCODE_FLOAT(sensor_intercept[2], 6));
#endif // OVERTEMPCAL_DBG_ENABLED
}
+void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
+ size_t data_length,
+ const struct OverTempCalDataPt *model_data) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(model_data);
+
+ // Load only "good" data from the input 'model_data'.
+ over_temp_cal->num_model_pts = NANO_MIN(data_length, OVERTEMPCAL_MODEL_SIZE);
+ size_t i;
+ size_t valid_data_count = 0;
+ for (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],
+ sizeof(struct OverTempCalDataPt));
+ valid_data_count++;
+ }
+ }
+ over_temp_cal->num_model_pts = valid_data_count;
+
+ // Initializes the OTC linear model parameters.
+ resetOtcLinearModel(over_temp_cal);
+
+ // Finds the offset nearest the sensor's current temperature.
+ findNearestEstimate(over_temp_cal);
+
+ // Updates the 'compensated_offset_previous' vector to prevent from
+ // immediately triggering a new calibration update.
+ float compensated_offset_temperature_celsius = 0.0f;
+ getCalOffset(over_temp_cal, /*timestamp_nanos=*/0,
+ &compensated_offset_temperature_celsius,
+ over_temp_cal->compensated_offset_previous);
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ // Prints the updated model data.
+ CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
+ "Over-temperature full model data set recalled.");
+ // Resets the debug print machine to ensure that computeModelUpdate() can
+ // produce a debug report and interupt any ongoing report.
+ over_temp_cal->debug_state = OTC_IDLE;
+#endif // OVERTEMPCAL_DBG_ENABLED
+
+ // Ensures that minimum number of points required for a model fit has been
+ // satisfied and recomputes the OTC model parameters.
+ if (over_temp_cal->num_model_pts > over_temp_cal->min_num_model_pts) {
+ // Computes and replaces the model parameters. If successful, this will
+ // trigger a "new calibration" update.
+ computeModelUpdate(over_temp_cal,
+ over_temp_cal->modelupdate_timestamp_nanos);
+ }
+}
+
+void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
+ size_t *data_length,
+ struct OverTempCalDataPt *model_data) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ *data_length = over_temp_cal->num_model_pts;
+ memcpy(model_data, over_temp_cal->model_data,
+ over_temp_cal->num_model_pts * sizeof(struct OverTempCalDataPt));
+}
+
+bool overTempCalGetOffset(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ float *compensated_offset_temperature_celsius,
+ float *compensated_offset) {
+ // Gets the temperature compensated sensor offset estimate.
+ getCalOffset(over_temp_cal, timestamp_nanos,
+ compensated_offset_temperature_celsius, compensated_offset);
+
+ // If the compensated_offset value has changed significantly then return
+ // 'true' status.
+ bool offset_has_changed = false;
+ int i;
+ for (i = 0; i < 3; i++) {
+ if (NANO_ABS(over_temp_cal->compensated_offset_previous[i] -
+ compensated_offset[i]) >= SIGNIFICANT_OFFSET_CHANGE_RPS) {
+ offset_has_changed = true;
+
+ // Update the 'compensated_offset_previous' vector.
+ memcpy(over_temp_cal->compensated_offset_previous, compensated_offset,
+ 3 * sizeof(float));
+ break;
+ }
+ }
+
+ return offset_has_changed;
+}
+
void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal,
uint64_t timestamp_nanos, float xi, float yi,
float zi, float *xo, float *yo, float *zo) {
ASSERT_NOT_NULL(over_temp_cal);
- ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
ASSERT_NOT_NULL(xo);
ASSERT_NOT_NULL(yo);
ASSERT_NOT_NULL(zo);
- // Removes very old data from the collected model estimates (eliminates
- // drift-compromised data). Only does this when there is more than one
- // estimate in the model (i.e., don't want to remove all data, even if it is
- // very old [something is likely better than nothing]).
- if ((timestamp_nanos - over_temp_cal->stale_data_timer) >=
- OVERTEMPCAL_STALE_CHECK_TIME_NANOS &&
- over_temp_cal->num_model_pts > 1) {
- over_temp_cal->stale_data_timer = timestamp_nanos; // Resets timer.
-
- if (removeStaleModelData(over_temp_cal, timestamp_nanos)) {
- // If anything was removed, then this attempts to recompute the model.
- if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts) {
- computeModelUpdate(over_temp_cal, timestamp_nanos);
- }
- }
- }
-
// Determines whether over-temp compensation will be applied.
- if (!over_temp_cal->over_temp_enable) {
- return;
- }
+ if (over_temp_cal->over_temp_enable) {
+ // Gets the temperature compensated sensor offset estimate.
+ float compensated_offset[3] = {0.0f, 0.0f, 0.0f};
+ float compensated_offset_temperature_celsius = 0.0f;
+ getCalOffset(over_temp_cal, timestamp_nanos,
+ &compensated_offset_temperature_celsius, compensated_offset);
- // Removes the over-temperature compensated offset from the input sensor data.
- removeSensorOffset(over_temp_cal, xi, 0, xo);
- removeSensorOffset(over_temp_cal, yi, 1, yo);
- removeSensorOffset(over_temp_cal, zi, 2, zo);
+ // Removes the over-temperature compensated offset from the input sensor
+ // data.
+ *xo = xi - compensated_offset[0];
+ *yo = yi - compensated_offset[1];
+ *zo = zi - compensated_offset[2];
+ } else {
+ *xo = xi;
+ *yo = yi;
+ *zo = zi;
+ }
}
bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) {
@@ -328,6 +521,11 @@
ASSERT_NOT_NULL(offset);
ASSERT(over_temp_cal->delta_temp_per_bin > 0);
+ // Checks that the new offset data is valid, returns if bad.
+ if (!isValidOtcOffset(offset, temperature_celsius)) {
+ return;
+ }
+
// Prevent a divide by zero below.
if (over_temp_cal->delta_temp_per_bin <= 0) {
return;
@@ -354,7 +552,7 @@
(unsigned long long int)timestamp_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
- return; // Skips the process of adding this offset to the model.
+ return; // Outlier detected: skips adding this offset to the model.
} else {
// Resets the count of rejected outliers.
over_temp_cal->num_outliers = 0;
@@ -420,11 +618,6 @@
timestamp_nanos);
#ifdef OVERTEMPCAL_DBG_ENABLED
- // Updates the latest sensor offset estimate so this can be tracked for debug
- // printout later.
- memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
- over_temp_cal->nearest_offset, sizeof(struct OverTempCalDataPt));
-
// Updates the total number of received sensor offset estimates.
over_temp_cal->debug_num_estimates++;
#endif // OVERTEMPCAL_DBG_ENABLED
@@ -440,8 +633,8 @@
// (current_timestamp_nanos - modelupdate_timestamp_nanos) <
// min_update_interval_nanos
if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts ||
- (timestamp_nanos - over_temp_cal->modelupdate_timestamp_nanos) <
- over_temp_cal->min_update_interval_nanos) {
+ timestamp_nanos < over_temp_cal->min_update_interval_nanos +
+ over_temp_cal->modelupdate_timestamp_nanos) {
#ifdef OVERTEMPCAL_DBG_ENABLED
// Triggers a log printout to show the updated sensor offset estimate.
updateDebugData(over_temp_cal);
@@ -462,7 +655,7 @@
static uint64_t wait_timer = 0;
// Prints the sensor temperature trajectory for debugging purposes.
// This throttles the print statements.
- if ((timestamp_nanos - wait_timer) >= 1000000000) {
+ if (timestamp_nanos >= 1000000000 + wait_timer) {
wait_timer = timestamp_nanos; // Starts the wait timer.
// Prints out temperature and the current timestamp.
@@ -474,21 +667,25 @@
#endif // OVERTEMPCAL_DBG_LOG_TEMP
#endif // OVERTEMPCAL_DBG_ENABLED
+ // Checks that the offset temperature is within a valid range, saturates if
+ // outside.
+ checkAndEnforceTemperatureRange(&temperature_celsius);
+
// Updates the sensor temperature.
over_temp_cal->temperature_celsius = temperature_celsius;
- // This searches for the sensor offset estimate closest to the current
+ // Searches for the sensor offset estimate closest to the current
// temperature. A timer is used to limit the rate at which this search is
// performed.
if (over_temp_cal->num_model_pts > 0 &&
- (timestamp_nanos - over_temp_cal->nearest_search_timer) >=
- OVERTEMPCAL_NEAREST_NANOS) {
+ timestamp_nanos >=
+ OVERTEMPCAL_NEAREST_NANOS + over_temp_cal->nearest_search_timer) {
findNearestEstimate(over_temp_cal);
over_temp_cal->nearest_search_timer = timestamp_nanos; // Reset timer.
}
}
-void getModelError(const struct OverTempCal *over_temp_cal,
+void overTempGetModelError(const struct OverTempCal *over_temp_cal,
const float *temp_sensitivity, const float *sensor_intercept,
float *max_error) {
ASSERT_NOT_NULL(over_temp_cal);
@@ -517,6 +714,64 @@
/////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
+void getCalOffset(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
+ float *compensated_offset_temperature_celsius,
+ float *compensated_offset) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
+ ASSERT_NOT_NULL(compensated_offset);
+ ASSERT_NOT_NULL(compensated_offset_temperature_celsius);
+
+ // Sets the sensor temperature associated with the compensated offset.
+ *compensated_offset_temperature_celsius = over_temp_cal->temperature_celsius;
+
+ // Removes very old data from the collected model estimates (eliminates
+ // drift-compromised data). Only does this when there is more than one
+ // estimate in the model (i.e., don't want to remove all data, even if it is
+ // very old [something is likely better than nothing]).
+ if ((timestamp_nanos >=
+ OVERTEMPCAL_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer) &&
+ over_temp_cal->num_model_pts > 1) {
+ over_temp_cal->stale_data_timer = timestamp_nanos; // Resets timer.
+
+ if (removeStaleModelData(over_temp_cal, timestamp_nanos)) {
+ // If anything was removed, then this attempts to recompute the model.
+ if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts) {
+ computeModelUpdate(over_temp_cal, timestamp_nanos);
+ }
+ }
+ }
+
+ size_t index;
+ for (index = 0; index < 3; index++) {
+ if (over_temp_cal->temp_sensitivity[index] >= OTC_INITIAL_SENSITIVITY ||
+ NANO_ABS(over_temp_cal->temperature_celsius -
+ over_temp_cal->nearest_offset->offset_temp_celsius) <
+ over_temp_cal->delta_temp_per_bin) {
+ // Use the nearest estimate to perform the compensation if either of the
+ // following is true:
+ // 1) This axis model is in its initial state.
+ // 2) The sensor's temperature is within a small neighborhood of the
+ // 'nearest_offset'.
+ // compensated_offset = nearest_offset
+ //
+ // If either of the above conditions applies and 'nearest_offset' is not
+ // defined, then the offset returned is zero.
+ compensated_offset[index] =
+ (over_temp_cal->num_model_pts > 0)
+ ? over_temp_cal->nearest_offset->offset[index]
+ : 0.0f;
+ } else {
+ // Offset computed from the linear model:
+ // compensated_offset = (temp_sensitivity * temperature +
+ // sensor_intercept)
+ compensated_offset[index] = (over_temp_cal->temp_sensitivity[index] *
+ over_temp_cal->temperature_celsius +
+ over_temp_cal->sensor_intercept[index]);
+ }
+ }
+}
+
void setNearestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
float offset_temp_celsius, uint64_t timestamp_nanos) {
ASSERT_NOT_NULL(over_temp_cal);
@@ -540,11 +795,12 @@
// Computes the maximum error over all of the model data.
float max_error[3];
- getModelError(over_temp_cal, temp_sensitivity, sensor_intercept, max_error);
+ overTempGetModelError(over_temp_cal, temp_sensitivity, sensor_intercept,
+ max_error);
// 3) A new set of model parameters are accepted if:
// i. The model fit error is less than, 'max_error_limit'. See
- // getModelError() for error metric description.
+ // overTempGetModelError() for error metric description.
// ii. The model fit parameters must be within certain absolute
// bounds:
// a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
@@ -553,8 +809,8 @@
bool updated_one = false;
for (i = 0; i < 3; i++) {
if (max_error[i] < over_temp_cal->max_error_limit &&
- NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit &&
- NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) {
+ isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
+ sensor_intercept[i])) {
over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
updated_one = true;
@@ -616,8 +872,8 @@
bool removed_one = false;
for (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->model_data[i].timestamp_nanos) >
- over_temp_cal->age_limit_nanos) {
+ timestamp_nanos > over_temp_cal->age_limit_nanos +
+ over_temp_cal->model_data[i].timestamp_nanos) {
removed_one |= removeModelDataByIndex(over_temp_cal, i);
}
}
@@ -682,17 +938,19 @@
// In normal operation the offset estimates enter into the 'model_data' array
// 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 (i.e., no models in an initial state) and are all within the valid
- // range (this is assumed to have been checked prior to this function). There
- // must also be no preexisting model data; that is, this function will not
- // replace any actual offset estimates already buffered.
- if (over_temp_cal->num_model_pts > 0 ||
- over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE ||
- over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE ||
- over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) {
- return false;
+ // defined and are all within the valid range.
+ size_t i;
+ for (i = 0; i < 3; i++) {
+ if (!isValidOtcLinearModel(over_temp_cal,
+ over_temp_cal->temp_sensitivity[i],
+ over_temp_cal->sensor_intercept[i])) {
+ return false;
+ }
}
+ // Any pre-existing model data points will be overwritten.
+ over_temp_cal->num_model_pts = 0;
+
// This defines the minimum contiguous set of points to allow a model update
// when the next offset estimate is received. They are placed at a common
// temperature range that is likely to get replaced with actual data soon.
@@ -701,7 +959,6 @@
float offset_temp_celsius =
(start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;
- size_t i;
size_t j;
for (i = 0; i < over_temp_cal->min_num_model_pts; i++) {
float offset[3];
@@ -718,13 +975,13 @@
}
#ifdef OVERTEMPCAL_DBG_ENABLED
- if (over_temp_cal->min_num_model_pts > 0) {
+ if (over_temp_cal->num_model_pts > 0) {
CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]", "Model Jump-Start: #Points = %lu.",
- (unsigned long int)over_temp_cal->min_num_model_pts);
+ (unsigned long int)over_temp_cal->num_model_pts);
}
#endif // OVERTEMPCAL_DBG_ENABLED
- return (over_temp_cal->min_num_model_pts > 0);
+ return (over_temp_cal->num_model_pts > 0);
}
void updateModel(const struct OverTempCal *over_temp_cal,
@@ -771,34 +1028,6 @@
sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_n;
}
-void removeSensorOffset(const struct OverTempCal *over_temp_cal, float axis_in,
- size_t index, float *axis_out) {
- ASSERT_NOT_NULL(over_temp_cal);
- ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
- ASSERT_NOT_NULL(axis_out);
-
- // Removes the over-temperature compensated offset from the input sensor data.
- if (over_temp_cal->temp_sensitivity[index] >= MODEL_INITIAL_STATE ||
- NANO_ABS(over_temp_cal->temperature_celsius -
- over_temp_cal->nearest_offset->offset_temp_celsius) <
- over_temp_cal->delta_temp_per_bin) {
- // Use the nearest estimate to perform the compensation if either of the
- // following is true:
- // 1) This axis model is in its initial state.
- // 2) The current temperature is within a small neighborhood of the
- // 'nearest_offset'.
- // axis_out = axis_in - nearest_offset
- *axis_out = axis_in - over_temp_cal->nearest_offset->offset[index];
- } else {
- // axis_out = axis_in - compensated_offset
- // Where,
- // compensated_offset = (temp_sensitivity * temperature + sensor_intercept)
- *axis_out = axis_in - (over_temp_cal->temp_sensitivity[index] *
- over_temp_cal->temperature_celsius +
- over_temp_cal->sensor_intercept[index]);
- }
-}
-
bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
size_t axis_index, float temperature_celsius) {
ASSERT_NOT_NULL(over_temp_cal);
@@ -806,7 +1035,7 @@
// If a model has been defined, then check to see if this offset could be a
// potential outlier:
- if (over_temp_cal->temp_sensitivity[axis_index] < MODEL_INITIAL_STATE) {
+ if (over_temp_cal->temp_sensitivity[axis_index] < OTC_INITIAL_SENSITIVITY) {
const float max_error_test = NANO_ABS(
offset[axis_index] -
(over_temp_cal->temp_sensitivity[axis_index] * temperature_celsius +
@@ -841,6 +1070,8 @@
memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal));
// Copies over the relevant data.
+ memcpy(over_temp_cal->debug_overtempcal.temp_sensitivity,
+ over_temp_cal->temp_sensitivity, 3 * sizeof(float));
memcpy(over_temp_cal->debug_overtempcal.sensor_intercept,
over_temp_cal->sensor_intercept, 3 * sizeof(float));
memcpy(&over_temp_cal->debug_overtempcal.nearest_offset,
@@ -852,16 +1083,8 @@
over_temp_cal->debug_overtempcal.temperature_celsius =
over_temp_cal->temperature_celsius;
- size_t j;
- for (j = 0; j < 3; j++) {
- over_temp_cal->debug_overtempcal.temp_sensitivity[j] =
- (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE)
- ? 0.0f
- : over_temp_cal->temp_sensitivity[j];
- }
-
// Computes the maximum error over all of the model data.
- getModelError(over_temp_cal,
+ overTempGetModelError(over_temp_cal,
over_temp_cal->debug_overtempcal.temp_sensitivity,
over_temp_cal->debug_overtempcal.sensor_intercept,
over_temp_cal->debug_overtempcal.max_error);
@@ -892,7 +1115,7 @@
case OTC_WAIT_STATE:
// This helps throttle the print statements.
- if ((timestamp_nanos - wait_timer) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
+ if (timestamp_nanos >= OVERTEMPCAL_WAIT_TIME_NANOS + wait_timer) {
over_temp_cal->debug_state = next_state;
}
break;
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 4ae8952..3aa3d39 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -68,9 +68,24 @@
// Defines the maximum size of the 'model_data' array.
#define OVERTEMPCAL_MODEL_SIZE (40)
+// A common sensor operating temperature at which to start producing the model
+// jump-start data.
+#define JUMPSTART_START_TEMP_CELSIUS (30.0f)
+
// The maximum number of successive outliers that may be rejected.
#define OVERTEMPCAL_MAX_OUTLIER_COUNT (3)
+// The 'temp_sensitivity' parameters are set to this value to indicate that the
+// model is in its initial state.
+#define OTC_INITIAL_SENSITIVITY (1e6f)
+
+// Minimum "significant" change of offset value.
+#define SIGNIFICANT_OFFSET_CHANGE_RPS (5.23e-5f) // 3mDPS
+
+// Valid sensor temperature operating range.
+#define OVERTEMPCAL_TEMP_MIN_CELSIUS (-40.0f)
+#define OVERTEMPCAL_TEMP_MAX_CELSIUS (85.0f)
+
// Over-temperature sensor offset estimate structure.
struct OverTempCalDataPt {
// Sensor offset estimate, temperature, and timestamp.
@@ -94,9 +109,6 @@
struct DebugOverTempCal {
uint64_t modelupdate_timestamp_nanos;
- // The most recent offset estimate received.
- struct OverTempCalDataPt latest_offset;
-
// The offset estimate nearest the current sensor temperature.
struct OverTempCalDataPt nearest_offset;
@@ -132,6 +144,9 @@
// The temperature at which the offset compensation is performed.
float temperature_celsius;
+ // The stored value of the temperature compensated sensor offset.
+ float compensated_offset_previous[3];
+
// Pointer to the offset estimate closest to the current sensor temperature.
struct OverTempCalDataPt *nearest_offset;
@@ -150,7 +165,7 @@
// min_update_interval_nanos
// 3) A new set of model parameters are accepted if:
// i. The model fit error is less than, 'max_error_limit'. See
- // getModelError() for error metric description.
+ // overTempGetModelError() for error metric description.
// ii. The model fit parameters must be within certain absolute
// bounds:
// a. ABS(temp_sensitivity) < temp_sensitivity_limit
@@ -287,6 +302,56 @@
float *temp_sensitivity, float *sensor_intercept);
/*
+ * Sets the over-temp compensation model data set, and computes new model
+ * parameters provided that 'min_num_model_pts' is satisfied.
+ *
+ * INPUTS:
+ * over_temp_cal: Over-temp main data structure.
+ * model_data: Array of the new model data set.
+ * data_length: Number of model data entries in 'model_data'.
+ *
+ * NOTE: Max array length for 'model_data' is OVERTEMPCAL_MODEL_SIZE.
+ */
+void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
+ size_t data_length,
+ const struct OverTempCalDataPt *model_data);
+
+/*
+ * Gets the over-temp compensation model data set.
+ *
+ * INPUTS:
+ * over_temp_cal: Over-temp main data structure.
+ * OUTPUTS:
+ * model_data: Array containing the model data set.
+ * data_length: Number of model data entries in 'model_data'.
+ *
+ * NOTE: Max array length for 'model_data' is OVERTEMPCAL_MODEL_SIZE.
+ */
+void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
+ size_t *data_length,
+ struct OverTempCalDataPt *model_data);
+
+/*
+ * Returns 'true' if the estimated offset has changed by
+ * 'SIGNIFICANT_OFFSET_CHANGE_RPS' and provides the current over-temperature
+ * compensated offset vector. This function is useful for detecting changes in
+ * the offset vector.
+ *
+ * INPUTS:
+ * over_temp_cal: Over-temp data structure.
+ * timestamp_nanos: The current system timestamp.
+ * OUTPUTS:
+ * compensated_offset: Temperature compensated offset estimate array.
+ * compensated_offset_temperature_celsius: Compensated offset temperature.
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ */
+bool overTempCalGetOffset(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ float *compensated_offset_temperature_celsius,
+ float *compensated_offset);
+
+/*
* Removes the over-temp compensated offset from the input sensor data.
*
* INPUTS:
@@ -347,9 +412,9 @@
* NOTE 1: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
* NOTE 2: This function is provided for testing purposes.
*/
-void getModelError(const struct OverTempCal *over_temp_cal,
- const float *temp_sensitivity, const float *sensor_intercept,
- float *max_error);
+void overTempGetModelError(const struct OverTempCal *over_temp_cal,
+ const float *temp_sensitivity,
+ const float *sensor_intercept, float *max_error);
#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 64f70f8..f2e711f 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -25,16 +25,20 @@
#ifdef GCC_DEBUG_LOG
# include <stdio.h>
# define CAL_DEBUG_LOG(tag, fmt, ...) \
- printf("%s " fmt "\n", tag, ##__VA_ARGS__);
-#else // GCC_DEBUG_LOG
+ printf("%s " fmt "\n", tag, ##__VA_ARGS__);
+#elif _OS_BUILD_
# include <seos.h>
# ifndef LOG_FUNC
# define LOG_FUNC(level, fmt, ...) osLog(level, fmt, ##__VA_ARGS__)
# endif // LOG_FUNC
# define LOGD_TAG(tag, fmt, ...) \
- LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
+ LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
# define CAL_DEBUG_LOG(tag, fmt, ...) \
- osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__);
+ osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__);
+#else // _OS_BUILD_
+# include <chre.h>
+# define CAL_DEBUG_LOG(tag, fmt, ...) \
+ chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__)
#endif // GCC_DEBUG_LOG
#ifdef __cplusplus
diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c
index 5ab6625..e137435 100644
--- a/firmware/os/algos/common/math/mat.c
+++ b/firmware/os/algos/common/math/mat.c
@@ -18,8 +18,17 @@
#include <assert.h>
#include <float.h>
+
+#ifdef _OS_BUILD_
#include <nanohub_math.h>
#include <seos.h>
+#else
+#include <math.h>
+#ifndef UNROLLED
+#define UNROLLED
+#endif
+#endif // _OS_BUILD_
+
#include <stddef.h>
#include <string.h>
@@ -595,7 +604,7 @@
size_t i;
for (i = 0; i < nrows; ++i) {
const float *row = &A[i * ncols];
- out[i] = vecDot(row, v, ncols);
+ out[i] = vecDot(row, v, (int)ncols);
}
}
@@ -617,29 +626,31 @@
ASSERT_NOT_NULL(x);
ASSERT_NOT_NULL(L);
ASSERT_NOT_NULL(b);
+ ASSERT(n <= INT32_MAX);
int32_t i, j; // Loops below require signed integers.
+ int32_t s_n = (int32_t)n; // Signed n.
float sum = 0.0f;
// 1. Solve Ly = b through forward substitution. Use x[] to store y.
- for (i = 0; i < n; ++i) {
+ for (i = 0; i < s_n; ++i) {
sum = 0.0f;
for (j = 0; j < i; ++j) {
- sum += L[i * n + j] * x[j];
+ sum += L[i * s_n + j] * x[j];
}
// Check for non-zero diagonals (don't divide by zero).
- if (L[i * n + i] < EPSILON) {
+ if (L[i * s_n + i] < EPSILON) {
return false;
}
- x[i] = (b[i] - sum) / L[i * n + i];
+ x[i] = (b[i] - sum) / L[i * s_n + i];
}
// 2. Solve L'x = y through backwards substitution. Use x[] to store both
// y and x.
- for (i = n - 1; i >= 0; --i) {
+ for (i = s_n - 1; i >= 0; --i) {
sum = 0.0f;
- for (j = i + 1; j < n; ++j) {
- sum += L[j * n + i] * x[j];
+ for (j = i + 1; j < s_n; ++j) {
+ sum += L[j * s_n + i] * x[j];
}
- x[i] = (x[i] - sum) / L[i * n + i];
+ x[i] = (x[i] - sum) / L[i * s_n + i];
}
return true;
diff --git a/firmware/os/algos/common/math/quat.c b/firmware/os/algos/common/math/quat.c
index 89727ff..f7fb3c7 100644
--- a/firmware/os/algos/common/math/quat.c
+++ b/firmware/os/algos/common/math/quat.c
@@ -15,7 +15,6 @@
*/
#include "common/math/quat.h"
-#include <nanohub_math.h>
static float clamp(float x) { return x < 0.0f ? 0.0f : x; }
diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c
index 97b2b8c..62039bb 100644
--- a/firmware/os/algos/common/math/vec.c
+++ b/firmware/os/algos/common/math/vec.c
@@ -15,7 +15,6 @@
*/
#include "common/math/vec.h"
-#include <nanohub_math.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 77ab492..098c6d1 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -32,7 +32,12 @@
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
+#ifdef NANOHUB_NON_CHRE_API
#include <nanohub_math.h>
+#else
+#include <math.h>
+#endif // NANOHUB_NON_CHRE_API
+
#include <stddef.h>
#include "util/nano_assert.h"
@@ -48,13 +53,13 @@
float x, y, z, w;
};
-#ifndef NANO_ABS
-#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
-#endif
+#define NANO_PI (3.14159265359f)
-#ifndef NANO_MAX
+#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
+
#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
-#endif
+
+#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
// 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
diff --git a/firmware/os/algos/util/nano_assert.h b/firmware/os/algos/util/nano_assert.h
index c6389c3..e4f1467 100644
--- a/firmware/os/algos/util/nano_assert.h
+++ b/firmware/os/algos/util/nano_assert.h
@@ -34,11 +34,13 @@
#endif
+#ifndef ASSERT
#ifdef NANO_ASSERT_ENABLED
#define ASSERT(x) ASSERT_IMPL(x)
#else
#define ASSERT(x) ((void)(x))
-#endif
+#endif // NANO_ASSERT_ENABLED
+#endif // ASSERT
// Use NULL when compiling for C and nullptr for C++.
#ifdef __cplusplus
diff --git a/firmware/os/core/hostIntf.c b/firmware/os/core/hostIntf.c
index 4aa9a1e..084e508 100644
--- a/firmware/os/core/hostIntf.c
+++ b/firmware/os/core/hostIntf.c
@@ -29,6 +29,7 @@
#include <platform.h>
#include <cpu.h>
+#include <halIntf.h>
#include <hostIntf.h>
#include <hostIntf_priv.h>
#include <nanohubCommand.h>
@@ -1082,7 +1083,7 @@
}
}
-static void hostIntfAddBlock(struct HostIntfDataBuffer *data, bool discardable)
+static void hostIntfAddBlock(struct HostIntfDataBuffer *data, bool discardable, bool interrupt)
{
if (!simpleQueueEnqueue(mOutputQ, data, sizeof(uint32_t) + data->length, discardable))
return;
@@ -1091,7 +1092,7 @@
mWakeupBlocks++;
else if (data->interrupt == NANOHUB_INT_NONWAKEUP)
mNonWakeupBlocks++;
- nanohubPrefetchTx(data->interrupt, mWakeupBlocks, mNonWakeupBlocks);
+ nanohubPrefetchTx(interrupt ? data->interrupt : HOSTINTF_MAX_INTERRUPTS, mWakeupBlocks, mNonWakeupBlocks);
}
static void hostIntfNotifyReboot(uint32_t reason)
@@ -1151,6 +1152,7 @@
osEventUnsubscribe(mHostIntfTid, EVT_APP_START);
osEventSubscribe(mHostIntfTid, EVT_NO_SENSOR_CONFIG_EVENT);
+ osEventSubscribe(mHostIntfTid, EVT_APP_TO_SENSOR_HAL_DATA);
osEventSubscribe(mHostIntfTid, EVT_APP_TO_HOST);
#ifdef DEBUG_LOG_EVT
osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG);
@@ -1163,7 +1165,7 @@
data->dataType = HOSTINTF_DATA_TYPE_RESET_REASON;
data->interrupt = NANOHUB_INT_WAKEUP;
memcpy(data->buffer, &reason, sizeof(reason));
- hostIntfAddBlock(data, false);
+ hostIntfAddBlock(data, false, true);
hostIntfNotifyReboot(reason);
}
}
@@ -1180,7 +1182,7 @@
data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
data->interrupt = NANOHUB_INT_WAKEUP;
memcpy(data->buffer, evtData, data->length);
- hostIntfAddBlock(data, false);
+ hostIntfAddBlock(data, false, true);
}
}
@@ -1198,7 +1200,7 @@
struct HostIntfDataBuffer *data = (struct HostIntfDataBuffer *)evtData;
if (data->sensType == SENS_TYPE_INVALID && data->dataType == HOSTINTF_DATA_TYPE_LOG)
- hostIntfAddBlock(data, true);
+ hostIntfAddBlock(data, true, true);
}
#endif
@@ -1324,6 +1326,9 @@
case CONFIG_CMD_DISABLE:
onConfigCmdDisableOne(sensor, cmd);
break;
+ case CONFIG_CMD_CFG_DATA:
+ onConfigCmdCfgDataAll(sensor, cmd);
+ break;
}
} else {
switch (cmd->cmd) {
@@ -1351,6 +1356,16 @@
}
}
+static void onEvtAppToSensorHalData(const void *evtData)
+{
+ struct HostIntfDataBuffer *data = (struct HostIntfDataBuffer *)evtData;
+ if (data->sensType == SENS_TYPE_INVALID
+ && data->dataType == HOSTINTF_DATA_TYPE_APP_TO_SENSOR_HAL) {
+ struct AppToSensorHalDataBuffer *buffer = (struct AppToSensorHalDataBuffer *)data;
+ hostIntfAddBlock(data, (buffer->payload.type & EVENT_TYPE_BIT_DISCARDABLE) != 0, false);
+ }
+}
+
static void copyEmbeddedSamples(struct ActiveSensor *sensor, const void* evtData)
{
uint64_t sensorTime = sensorGetTime();
@@ -1491,6 +1506,9 @@
case EVT_NO_SENSOR_CONFIG_EVENT:
onEvtNoSensorConfigEvent(evtData);
break;
+ case EVT_APP_TO_SENSOR_HAL_DATA:
+ onEvtAppToSensorHalData(evtData);
+ break;
default:
onEvtSensorData(evtType, evtData);
break;
diff --git a/firmware/os/core/nanohubCommand.c b/firmware/os/core/nanohubCommand.c
index e128314..1719cef 100644
--- a/firmware/os/core/nanohubCommand.c
+++ b/firmware/os/core/nanohubCommand.c
@@ -698,6 +698,9 @@
case HOSTINTF_DATA_TYPE_RESET_REASON:
packet->evtType = htole32(EVT_RESET_REASON);
break;
+ case HOSTINTF_DATA_TYPE_APP_TO_SENSOR_HAL:
+ packet->evtType = htole32(EVT_APP_TO_SENSOR_HAL_DATA);
+ break;
#ifdef DEBUG_LOG_EVT
case HOSTINTF_DATA_TYPE_LOG:
packet->evtType = htole32(HOST_EVT_DEBUG_LOG);
diff --git a/firmware/os/drivers/bosch_bmi160/akm_ak09915_slave.c b/firmware/os/drivers/bosch_bmi160/akm_ak09915_slave.c
index 371f997..286ff21 100644
--- a/firmware/os/drivers/bosch_bmi160/akm_ak09915_slave.c
+++ b/firmware/os/drivers/bosch_bmi160/akm_ak09915_slave.c
@@ -17,8 +17,6 @@
#include <string.h>
#include "akm_ak09915_slave.h"
-#define kScale_mag 0.15f
-
void parseMagData(struct MagTask *magTask, uint8_t *buf, float *x, float *y, float *z) {
int32_t raw_x = (*(int16_t *)&buf[0]);
int32_t raw_y = (*(int16_t *)&buf[2]);
diff --git a/firmware/os/drivers/bosch_bmi160/akm_ak09915_slave.h b/firmware/os/drivers/bosch_bmi160/akm_ak09915_slave.h
index d7bb12e..af9d520 100644
--- a/firmware/os/drivers/bosch_bmi160/akm_ak09915_slave.h
+++ b/firmware/os/drivers/bosch_bmi160/akm_ak09915_slave.h
@@ -25,6 +25,8 @@
extern "C" {
#endif
+#define kScale_mag 0.15f
+
#define AKM_AK09915_DEVICE_ID 0x1048
#define AKM_AK09915_REG_WIA1 0x00
#define AKM_AK09915_REG_DATA 0x11
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index 9e10ec4..fec027c 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
@@ -1,3 +1,4 @@
+
/*
* Copyright (C) 2016 The Android Open Source Project
*
@@ -20,6 +21,7 @@
#include <errno.h>
#include <gpio.h>
#include <heap.h>
+#include <halIntf.h>
#include <hostIntf.h>
#include <i2c.h>
#include <isr.h>
@@ -46,12 +48,29 @@
#include <calibration/accelerometer/accel_cal.h>
#endif
+#if defined(OVERTEMPCAL_ENABLED) && !defined(GYRO_CAL_ENABLED)
+#undef OVERTEMPCAL_ENABLED
+#endif
+
+#if defined(GYRO_CAL_DBG_ENABLED) && !defined(GYRO_CAL_ENABLED)
+#undef GYRO_CAL_DBG_ENABLED
+#endif
+
+#if defined(OVERTEMPCAL_DBG_ENABLED) && !defined(OVERTEMPCAL_ENABLED)
+#undef OVERTEMPCAL_DBG_ENABLED
+#endif
+
#ifdef GYRO_CAL_ENABLED
#include <calibration/gyroscope/gyro_cal.h>
+#endif // GYRO_CAL_ENABLED
+
+#ifdef GYRO_CAL_DBG_ENABLED
+#include <calibration/util/cal_log.h>
+#endif // GYRO_CAL_DBG_ENABLED
+
#ifdef OVERTEMPCAL_ENABLED
#include <calibration/over_temp/over_temp_cal.h>
#endif // OVERTEMPCAL_ENABLED
-#endif // GYRO_CAL_ENABLED
#include <limits.h>
#include <stdlib.h>
@@ -85,7 +104,7 @@
#define DBG_WM_CALC 0
#define TIMESTAMP_DBG 0
-#define BMI160_APP_VERSION 12
+#define BMI160_APP_VERSION 14
// fixme: to list required definitions for a slave mag
#ifdef USE_BMM150
@@ -421,6 +440,12 @@
enum SensorIndex idx;
};
+struct OtcGyroUpdateBuffer {
+ struct AppToSensorHalDataBuffer head;
+ struct GyroOtcData data;
+ volatile uint8_t lock; // lock for static object
+} __attribute__((packed));
+
struct BMI160Task {
uint32_t tid;
struct BMI160Sensor sensors[NUM_OF_SENSOR];
@@ -428,12 +453,13 @@
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Declaration.
struct GyroCal gyro_cal;
+#endif // GYRO_CAL_ENABLED
#ifdef OVERTEMPCAL_ENABLED
// Over-temp gyro calibration object.
struct OverTempCal over_temp_gyro_cal;
+ struct OtcGyroUpdateBuffer otcGyroUpdateBuffer;
#endif // OVERTEMPCAL_ENABLED
-#endif // GYRO_CAL_ENABLED
// time keeping.
uint64_t last_sensortime;
@@ -684,6 +710,15 @@
#define initiateFifoRead(a) initiateFifoRead_(_task, (a))
static uint8_t* shallowParseFrame(uint8_t * buf, int size);
+#ifdef OVERTEMPCAL_ENABLED
+// otc gyro cal save restore functions
+static void handleOtcGyroConfig_(TASK, const struct AppToSensorHalDataPayload *data);
+#define handleOtcGyroConfig(a) handleOtcGyroConfig_(_task, (a))
+static bool sendOtcGyroUpdate_();
+#define sendOtcGyroUpdate() sendOtcGyroUpdate_(_task)
+static void unlockOtcGyroUpdateBuffer();
+#endif // OVERTEMPCAL_ENABLED
+
// Binary dump to osLog
static void dumpBinary(void* buf, unsigned int address, size_t size);
@@ -704,8 +739,9 @@
{ DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) },
#ifdef MAG_SLAVE_PRESENT
- { DEC_INFO_RATE_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE,
- NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS) },
+ { DEC_INFO_RATE_RAW_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE,
+ NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_RAW, 1.0/kScale_mag,
+ SENS_TYPE_MAG_BIAS) },
#endif
{ DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED,
NANOHUB_INT_NONWAKEUP, 100) },
@@ -1005,8 +1041,8 @@
// set mag to SLEEP mode
MAG_WRITE(BMM150_REG_CTRL_1, 0x01);
#elif USE_AK09915
- // set "low" Noise Suppression Filter (NSF) settings
- MAG_WRITE(AKM_AK09915_REG_CNTL1, 0x20);
+ // Disable Noise Suppression Filter (NSF) settings
+ MAG_WRITE(AKM_AK09915_REG_CNTL1, 0x00);
#endif
}
@@ -1944,6 +1980,7 @@
static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScale, uint64_t sensorTime)
{
+ TDECL();
struct TripleAxisDataPoint *sample;
uint64_t rtc_time, cur_time;
uint32_t delta_time;
@@ -1995,9 +2032,9 @@
gyroCalUpdateMag(&mTask.gyro_cal,
rtc_time, // nsec
x, y, z);
-#endif
+#endif // GYRO_CAL_ENABLED
} else
-#endif
+#endif // MAG_SLAVE_PRESENT
{
raw_x = (buf[0] | buf[1] << 8);
raw_y = (buf[2] | buf[3] << 8);
@@ -2031,36 +2068,47 @@
rtc_time, // nsec
x, y, z, mTask.tempCelsius);
-#ifdef GYRO_CAL_DBG_ENABLED
- // Gyro Cal -- Read out Debug data.
- gyroCalDebugPrint(&mTask.gyro_cal, rtc_time);
-#endif // GYRO_CAL_DBG_ENABLED
-
-#ifdef OVERTEMPCAL_ENABLED
-#ifdef OVERTEMPCAL_DBG_ENABLED
- overTempCalDebugPrint(&mTask.over_temp_gyro_cal, rtc_time);
-#endif // OVERTEMPCAL_DBG_ENABLED
-#endif // OVERTEMPCAL_ENABLED
-
#ifdef OVERTEMPCAL_ENABLED
// Over-Temp Gyro Cal -- Update measured temperature.
overTempCalSetTemperature(&mTask.over_temp_gyro_cal, rtc_time,
mTask.tempCelsius);
// Over-Temp Gyro Cal -- Apply over-temp calibration correction.
- overTempCalRemoveOffset(&mTask.over_temp_gyro_cal, rtc_time, x, y,
- z, /* input values */
- &x, &y, &z /* calibrated output */);
-#else
+ overTempCalRemoveOffset(&mTask.over_temp_gyro_cal, rtc_time,
+ x, y, z, /* input values */
+ &x, &y, &z /* calibrated output */);
+#else // OVERTEMPCAL_ENABLED
// Gyro Cal -- Apply calibration correction.
- gyroCalRemoveBias(&mTask.gyro_cal, x, y, z, /* input values */
- &x, &y, &z /* calibrated output */);
+ gyroCalRemoveBias(&mTask.gyro_cal,
+ x, y, z, /* input values */
+ &x, &y, &z /* calibrated output */);
#endif // OVERTEMPCAL_ENABLED
+
+#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
+ // This flag keeps GyroCal and OverTempCal from printing back-to-back.
+ // If they do, then sometimes important print log data gets dropped.
+ static size_t print_flag = 0;
+
+ if (print_flag > 0) {
+#ifdef GYRO_CAL_DBG_ENABLED
+ // Gyro Cal -- Read out Debug data.
+ gyroCalDebugPrint(&mTask.gyro_cal, rtc_time);
+#endif // GYRO_CAL_DBG_ENABLED
+ print_flag = 0;
+ } else {
+#ifdef OVERTEMPCAL_ENABLED
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ // Over-Temp Gyro Cal -- Read out Debug data.
+ overTempCalDebugPrint(&mTask.over_temp_gyro_cal, rtc_time);
+#endif // OVERTEMPCAL_DBG_ENABLED
+#endif // OVERTEMPCAL_ENABLED
+ print_flag = 1;
+ }
+#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED
#endif // GYRO_CAL_ENABLED
}
}
-
if (mSensor->data_evt == NULL) {
if (!allocateDataEvt(mSensor, rtc_time)) {
return;
@@ -2102,54 +2150,87 @@
}
#endif
#ifdef GYRO_CAL_ENABLED
- // Gyro Cal -- Notify HAL about new gyro bias calibration
- bool new_gyro_cal_update = gyroCalNewBiasAvailable(&mTask.gyro_cal);
- if (mSensor->idx == GYR && new_gyro_cal_update) {
+ if (mSensor->idx == GYR) {
+ // GyroCal -- Checks for a new offset estimate update.
+ float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
+ float gyro_offset_temperature_celsius = 0.0f;
+ bool new_gyrocal_offset_update = gyroCalNewBiasAvailable(&mTask.gyro_cal);
+ if (new_gyrocal_offset_update) {
+ // GyroCal -- Gets the GyroCal offset estimate.
+ gyroCalGetBias(&mTask.gyro_cal, &gyro_offset[0], &gyro_offset[1],
+ &gyro_offset[2], &gyro_offset_temperature_celsius);
+
+#ifdef OVERTEMPCAL_ENABLED
+ // OTC-Gyro Cal -- Sends a new GyroCal estimate to the OTC-Gyro.
+ overTempCalUpdateSensorEstimate(&mTask.over_temp_gyro_cal, rtc_time,
+ gyro_offset,
+ gyro_offset_temperature_celsius);
+#endif // OVERTEMPCAL_ENABLED
+ }
+
+#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;
+ 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.
+ new_otc_offset_update =
+ overTempCalGetOffset(&mTask.over_temp_gyro_cal, rtc_time,
+ &gyro_offset_temperature_celsius, gyro_offset);
+ }
+
+ if (new_otc_offset_update) {
+#else // OVERTEMPCAL_ENABLED
+ if (new_gyrocal_offset_update) {
+#endif // OVERTEMPCAL_ENABLED
if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
- // flush existing samples so the bias appears after them
- flushData(mSensor,
- EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[GYR].sensorType));
- if (!allocateDataEvt(mSensor, rtc_time)) {
- return;
- }
+ // flush existing samples so the bias appears after them.
+ flushData(mSensor,
+ EVENT_TYPE_BIT_DISCARDABLE |
+ sensorGetMyEventType(mSensorInfo[GYR].sensorType));
+ if (!allocateDataEvt(mSensor, rtc_time)) {
+ return;
+ }
}
mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
mSensor->data_evt->samples[0].firstSample.biasSample =
- mSensor->data_evt->samples[0].firstSample.numSamples;
- sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
- gyroCalGetBias(&mTask.gyro_cal, &sample->x, &sample->y, &sample->z);
+ mSensor->data_evt->samples[0].firstSample.numSamples;
+ sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0]
+ .firstSample.numSamples++];
+ // Updates the gyro offset in HAL.
+ sample->x = gyro_offset[0];
+ sample->y = gyro_offset[1];
+ sample->z = gyro_offset[2];
+
+#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
+ CAL_DEBUG_LOG("[GYRO_OFFSET:STORED]",
+ "Offset|Temp|Time: %s%d.%06d, %s%d.%06d, %s%d.%06d | "
+ "%s%d.%06d | %llu",
+ CAL_ENCODE_FLOAT(sample->x, 6),
+ CAL_ENCODE_FLOAT(sample->y, 6),
+ CAL_ENCODE_FLOAT(sample->z, 6),
+ CAL_ENCODE_FLOAT(gyro_offset_temperature_celsius, 6),
+ (unsigned long long int)rtc_time);
+#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED
+
flushData(mSensor, sensorGetMyEventType(mSensorInfo[GYR].biasType));
-
if (!allocateDataEvt(mSensor, rtc_time)) {
- return;
+ return;
}
- }
+ }
#ifdef OVERTEMPCAL_ENABLED
- // Over-Temp Gyro Cal -- Send new gyro cal result to the over-temp cal.
- float offset[3] = {0.0f, 0.0f, 0.0f};
- if (new_gyro_cal_update) {
- // Gets the gyro cal offset value.
- gyroCalGetBias(&mTask.gyro_cal, &offset[0], &offset[1], &offset[2]);
-
- // Sends it to the over-temp cal along with the current temperature and
- // time stamp.
- overTempCalUpdateSensorEstimate(&mTask.over_temp_gyro_cal, rtc_time,
- offset, mTask.tempCelsius);
- }
-
- // Over-Temp Gyro Cal -- Notify HAL about new over-temp bias model data.
- if (mSensor->idx == GYR &&
- overTempCalNewModelUpdateAvailable(&mTask.over_temp_gyro_cal)) {
- float offset_temp = 0.0f;
- float temp_sensitivity[3] = {0.0f, 0.0f, 0.0f};
- float sensor_intercept[3] = {0.0f, 0.0f, 0.0f};
- overTempCalGetModel(&mTask.over_temp_gyro_cal,
- offset, &offset_temp, &rtc_time,
- temp_sensitivity, sensor_intercept);
- // TODO(davejacobs, pengxu) -- Send data to HAL.
- }
+ if (overTempCalNewModelUpdateAvailable(&mTask.over_temp_gyro_cal)
+ || new_otc_offset_update) {
+ // Notify HAL to store new gyro OTC-Gyro data.
+ sendOtcGyroUpdate();
+ }
#endif // OVERTEMPCAL_ENABLED
+ }
#endif // GYRO_CAL_ENABLED
sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
@@ -2986,28 +3067,33 @@
static bool gyrCfgData(void *data, void *cookie)
{
- struct CfgData {
- int32_t hw[3];
- float sw[3];
- };
- struct CfgData *values = data;
-
- mTask.sensors[GYR].offset[0] = values->hw[0];
- mTask.sensors[GYR].offset[1] = values->hw[1];
- mTask.sensors[GYR].offset[2] = values->hw[2];
- mTask.sensors[GYR].offset_enable = true;
+ TDECL();
+ const struct AppToSensorHalDataPayload *p = data;
+ if (p->type == HALINTF_TYPE_GYRO_CAL_BIAS && p->size == sizeof(struct GyroCalBias)) {
+ const struct GyroCalBias *bias = p->gyroCalBias;
+ mTask.sensors[GYR].offset[0] = bias->hardwareBias[0];
+ mTask.sensors[GYR].offset[1] = bias->hardwareBias[1];
+ mTask.sensors[GYR].offset[2] = bias->hardwareBias[2];
+ mTask.sensors[GYR].offset_enable = true;
+ INFO_PRINT("gyrCfgData hw bias: data=%02lx, %02lx, %02lx\n",
+ bias->hardwareBias[0] & 0xFF,
+ bias->hardwareBias[1] & 0xFF,
+ bias->hardwareBias[2] & 0xFF);
#ifdef GYRO_CAL_ENABLED
- gyroCalSetBias(&mTask.gyro_cal, values->sw[0], values->sw[1], values->sw[2], sensorGetTime());
+ gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], bias->softwareBias[1],
+ bias->softwareBias[2], sensorGetTime());
#endif // GYRO_CAL_ENABLED
-
- INFO_PRINT("gyrCfgData: data=%02lx, %02lx, %02lx\n",
- values->hw[0] & 0xFF, values->hw[1] & 0xFF, values->hw[2] & 0xFF);
-
- if (!saveCalibration()) {
- mTask.pending_calibration_save = true;
+ if (!saveCalibration()) {
+ T(pending_calibration_save) = true;
+ }
+#if OVERTEMPCAL_ENABLED
+ } else if (p->type == HALINTF_TYPE_GYRO_OTC_DATA && p->size == sizeof(struct GyroOtcData)) {
+ handleOtcGyroConfig(data);
+#endif // OVERTEMPCAL_ENABLED
+ } else {
+ ERROR_PRINT("Unknown gyro config data type 0x%04x, size %d\n", p->type, p->size);
}
-
return true;
}
@@ -3076,17 +3162,32 @@
#ifdef MAG_SLAVE_PRESENT
static bool magCfgData(void *data, void *cookie)
{
- float *values = data;
+ const struct AppToSensorHalDataPayload *p = data;
+ if (p->type == HALINTF_TYPE_MAG_CAL_BIAS && p->size == sizeof(struct MagCalBias)) {
+ const struct MagCalBias *d = p->magCalBias;
+ INFO_PRINT("magCfgData: calibration %ldnT, %ldnT, %ldnT\n",
+ (int32_t)(d->bias[0] * 1000),
+ (int32_t)(d->bias[1] * 1000),
+ (int32_t)(d->bias[2] * 1000));
- INFO_PRINT("magCfgData: %ld, %ld, %ld\n",
- (int32_t)(values[0] * 1000), (int32_t)(values[1] * 1000), (int32_t)(values[2] * 1000));
+ mTask.moc.x_bias = d->bias[0];
+ mTask.moc.y_bias = d->bias[1];
+ mTask.moc.z_bias = d->bias[2];
+ mTask.magBiasPosted = false;
+ } else if (p->type == HALINTF_TYPE_MAG_LOCAL_FIELD && p->size == sizeof(struct MagLocalField)) {
+ const struct MagLocalField *d = p->magLocalField;
+ INFO_PRINT("magCfgData: local field strength %dnT, dec %ddeg, inc %ddeg\n",
+ (int)(d->strength * 1000),
+ (int)(d->declination * 180 / M_PI + 0.5f),
+ (int)(d->inclination * 180 / M_PI + 0.5f));
- mTask.moc.x_bias = values[0];
- mTask.moc.y_bias = values[1];
- mTask.moc.z_bias = values[2];
+ // Passing local field information to mag calibration routine
+ diversityCheckerLocalFieldUpdate(&mTask.moc.diversity_checker, d->strength);
- mTask.magBiasPosted = false;
-
+ // TODO: pass local field information to rotation vector sensor.
+ } else {
+ ERROR_PRINT("magCfgData: unknown type 0x%04x, size %d", p->type, p->size);
+ }
return true;
}
#endif
@@ -3711,14 +3812,16 @@
0, 0, 0, // initial bias offset calibration
0, // time stamp of initial bias calibration
1.5e9, // analysis window length = 1.5 seconds
- 5e-5f, // gyroscope variance threshold [rad/sec]^2
+ 7.5e-5f, // gyroscope variance threshold [rad/sec]^2
1e-5f, // gyroscope confidence delta [rad/sec]^2
8e-3f, // accelerometer variance threshold [m/sec^2]^2
1.6e-3f, // accelerometer confidence delta [m/sec^2]^2
- 1.4f, // magnetometer variance threshold [uT]^2
+ 5.0f, // magnetometer variance threshold [uT]^2
0.25, // magnetometer confidence delta [uT]^2
0.95f, // stillness threshold [0,1]
- 1); // 1=gyro calibrations will be applied
+ 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
#ifdef OVERTEMPCAL_ENABLED
// Initialize over-temp calibration.
@@ -3726,27 +3829,28 @@
&mTask.over_temp_gyro_cal,
5, // Min num of points to enable model update.
5000000000, // Min model update interval [nsec].
- 2.0f, // Temperature span of bin method [C].
+ 0.75f, // Temperature span of bin method [C].
50.0e-3f * M_PI / 180.0f, // Model fit 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 parameter [C].
+ 3.0f * M_PI / 180.0f, // Limit for model intercept parameter [rad/sec].
true); // Over-temp compensation enable.
#endif // OVERTEMPCAL_ENABLED
#endif // GYRO_CAL_ENABLED
#ifdef MAG_SLAVE_PRESENT
#ifdef DIVERSITY_CHECK_ENABLED
- initMagCal(&mTask.moc, 0.0f, 0.0f, 0.0f, // bias x, y, z
- 1.0f, 0.0f, 0.0f, // c00, c01, c02
- 0.0f, 1.0f, 0.0f, // c10, c11, c12
- 0.0f, 0.0f, 1.0f, // c20, c21, c22
- 500.0f, // threshold
- 15000.0f, // max_distance
- 7, // min_num_diverse_vectors
- 1, // max_num_max_distance
- 6.0f, // var_threshold
- 10.0f); // max_min_threshold
+ initMagCal(&mTask.moc, 0.0f, 0.0f, 0.0f, // bias x, y, z
+ 1.0f, 0.0f, 0.0f, // c00, c01, c02
+ 0.0f, 1.0f, 0.0f, // c10, c11, c12
+ 0.0f, 0.0f, 1.0f, // c20, c21, c22
+ 8, // min_num_diverse_vectors
+ 1, // max_num_max_distance
+ 6.0f, // var_threshold
+ 10.0f, // max_min_threshold
+ 48.f, // local_field
+ 0.5f, // threshold_tuning_param
+ 2.552); // max_distance_tuning_param
#else
initMagCal(&mTask.moc, 0.0f, 0.0f, 0.0f, // bias x, y, z
1.0f, 0.0f, 0.0f, // c00, c01, c02
@@ -4123,4 +4227,55 @@
}
}
+#ifdef OVERTEMPCAL_ENABLED
+static void handleOtcGyroConfig_(TASK, const struct AppToSensorHalDataPayload *data) {
+ const struct GyroOtcData *d = data->gyroOtcData;
+
+ INFO_PRINT("gyrCfgData otc-data: off %d %d %d, t %d, s %d %d %d, i %d %d %d",
+ (int)(d->lastOffset[0]), (int)(d->lastOffset[1]), (int)(d->lastOffset[2]),
+ (int)(d->lastTemperature),
+ (int)(d->sensitivity[0]), (int)(d->sensitivity[1]), (int)(d->sensitivity[2]),
+ (int)(d->intercept[0]), (int)(d->intercept[1]), (int)(d->intercept[2]));
+
+ overTempCalSetModel(&T(over_temp_gyro_cal), d->lastOffset, d->lastTemperature,
+ sensorGetTime(), d->sensitivity, d->intercept, true /*jumpstart*/);
+}
+
+static bool sendOtcGyroUpdate_(TASK) {
+ int step = 0;
+ if (atomicCmpXchgByte(&T(otcGyroUpdateBuffer).lock, false, true)) {
+ ++step;
+ //fill HostIntfDataBuffer header
+ struct HostIntfDataBuffer *p = (struct HostIntfDataBuffer *)(&T(otcGyroUpdateBuffer));
+ p->sensType = SENS_TYPE_INVALID;
+ p->length = sizeof(struct AppToSensorHalDataPayload) + sizeof(struct GyroOtcData);
+ p->dataType = HOSTINTF_DATA_TYPE_APP_TO_SENSOR_HAL;
+ p->interrupt = NANOHUB_INT_NONWAKEUP;
+
+ //fill AppToSensorHalDataPayload header
+ struct AppToSensorHalDataBuffer *q = (struct AppToSensorHalDataBuffer *)p;
+ q->payload.size = sizeof(struct GyroOtcData);
+ q->payload.type = HALINTF_TYPE_GYRO_OTC_DATA; // bit-or EVENT_TYPE_BIT_DISCARDABLE
+ // to make it discardable
+
+ // fill payload data
+ struct GyroOtcData *data = q->payload.gyroOtcData;
+ uint64_t timestamp;
+ overTempCalGetModel(&T(over_temp_gyro_cal), data->lastOffset, &data->lastTemperature,
+ ×tamp, data->sensitivity, data->intercept);
+ if (osEnqueueEvtOrFree(EVT_APP_TO_SENSOR_HAL_DATA, // bit-or EVENT_TYPE_BIT_DISCARDABLE
+ // to make event discardable
+ p, unlockOtcGyroUpdateBuffer)) {
+ ++step;
+ }
+ }
+ DEBUG_PRINT("otc gyro update, finished at step %d", step);
+ return step == 2;
+}
+
+static void unlockOtcGyroUpdateBuffer(void *event) {
+ atomicXchgByte(&(((struct OtcGyroUpdateBuffer*)(event))->lock), false);
+}
+#endif // OVERTEMPCAL_ENABLED
+
INTERNAL_APP_INIT(BMI160_APP_ID, BMI160_APP_VERSION, startTask, endTask, handleEvent);
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmm150_slave.c b/firmware/os/drivers/bosch_bmi160/bosch_bmm150_slave.c
index 4a223fb..3f21dc7 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmm150_slave.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmm150_slave.c
@@ -17,8 +17,6 @@
#include <string.h>
#include "bosch_bmm150_slave.h"
-#define kScale_mag 0.0625f // 1.0f / 16.0f;
-
void bmm150SaveDigData(struct MagTask *magTask, uint8_t *data, size_t offset)
{
// magnetometer temperature calibration data is read in 3 bursts of 8 byte
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmm150_slave.h b/firmware/os/drivers/bosch_bmi160/bosch_bmm150_slave.h
index 59e53fb..abc82a9 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmm150_slave.h
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmm150_slave.h
@@ -25,6 +25,8 @@
extern "C" {
#endif
+#define kScale_mag 0.0625f // 1.0f / 16.0f;
+
#define BMM150_REG_DATA 0x42
#define BMM150_REG_CTRL_1 0x4b
#define BMM150_REG_CTRL_2 0x4c
diff --git a/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c b/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c
index be5d3b1..df1afce 100644
--- a/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c
+++ b/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c
@@ -129,6 +129,8 @@
uint64_t totalEnabledTime;
uint64_t totalProxEnabledTime;
uint64_t totalProxFarTime;
+ uint32_t totalProxBecomesFar;
+ uint32_t totalProxBecomesNear;
};
enum ProxState {
@@ -371,10 +373,14 @@
proxFarSeconds = U64_DIV_BY_U64_CONSTANT(mTask.stats.totalProxFarTime, 1000000000);
INFO_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 far %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32
+ ", prox *->f %" PRIu32
+ ", prox *->n %" PRIu32,
enabledSeconds / 3600, (enabledSeconds % 3600) / 60, enabledSeconds % 60,
proxEnabledSeconds / 3600, (proxEnabledSeconds % 3600) / 60, proxEnabledSeconds % 60,
- proxFarSeconds / 3600, (proxFarSeconds % 3600) / 60, proxFarSeconds % 60);
+ proxFarSeconds / 3600, (proxFarSeconds % 3600) / 60, proxFarSeconds % 60,
+ mTask.stats.totalProxBecomesFar,
+ mTask.stats.totalProxBecomesNear);
// If the task is disabled, that means the AP is on and has switched the I2C
// mux. Therefore, no I2C transactions will succeed so skip them.
@@ -539,11 +545,15 @@
mTask.proxState = (embeddedSample.fdata < PROXIMITY_THRESH_NEAR) ? PROX_STATE_NEAR : PROX_STATE_FAR;
if ((lastProxState != PROX_STATE_FAR) && (mTask.proxState == PROX_STATE_FAR)) {
+ ++mTask.stats.totalProxBecomesFar;
mTask.stats.lastProxFarTimestamp = sensorGetTime();
setGesturePower(true, false);
- } else if ((lastProxState == PROX_STATE_FAR) && (mTask.proxState == PROX_STATE_NEAR)) {
- mTask.stats.totalProxFarTime += sensorGetTime() - mTask.stats.lastProxFarTimestamp;
- setGesturePower(false, false);
+ } else if ((lastProxState != PROX_STATE_NEAR) && (mTask.proxState == PROX_STATE_NEAR)) {
+ ++mTask.stats.totalProxBecomesNear;
+ if (lastProxState == PROX_STATE_FAR) {
+ mTask.stats.totalProxFarTime += sensorGetTime() - mTask.stats.lastProxFarTimestamp;
+ setGesturePower(false, false);
+ }
}
}
break;
@@ -567,6 +577,9 @@
syscfgSetExtiPort(mTask.pin);
mTask.isr.func = touchIsr;
+ mTask.stats.totalProxBecomesFar = 0;
+ mTask.stats.totalProxBecomesNear = 0;
+
osEventSubscribe(taskId, EVT_APP_START);
return true;
}
diff --git a/firmware/os/inc/eventnums.h b/firmware/os/inc/eventnums.h
index e22fa04..246587a 100644
--- a/firmware/os/inc/eventnums.h
+++ b/firmware/os/inc/eventnums.h
@@ -29,6 +29,7 @@
#define EVT_APP_TO_HOST 0x00000401 //app data to host. Type is struct HostHubRawPacket
#define EVT_MARSHALLED_SENSOR_DATA 0x00000402 //marshalled event data. Type is MarshalledUserEventData
#define EVT_RESET_REASON 0x00000403 //reset reason to host.
+#define EVT_APP_TO_SENSOR_HAL_DATA 0x00000404 // sensor driver out of band data update to sensor hal
#define EVT_DEBUG_LOG 0x00007F01 // send message payload to Linux kernel log
#define EVT_MASK 0x0000FFFF
diff --git a/firmware/os/inc/halIntf.h b/firmware/os/inc/halIntf.h
new file mode 100644
index 0000000..1fc727a
--- /dev/null
+++ b/firmware/os/inc/halIntf.h
@@ -0,0 +1,117 @@
+/*
+ * 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 __HALINTF_H
+#define __HALINTF_H
+#include <stdint.h>
+#include "toolchain.h"
+#include "sensType.h"
+
+/*
+ * This files contains data structure for HAL and driver / algo to exchange information.
+ */
+
+#define APP_TO_SENSOR_HAL_SIZE_MAX HOSTINTF_SENSOR_DATA_MAX
+#define APP_TO_SENSOR_HAL_PAYLOAD_MAX \
+ (APP_TO_SENSOR_HAL_DATA_MAX - sizeof(struct AppToSensorHalDataBuffer))
+#define APP_TO_SENSOR_HAL_TYPE(sensor, subtype) (((sensor) && 0xFF) | (((subtype) & 0x7F) << 8))
+#define APP_TO_SENSOR_HAL_TYPE_MASK 0x7FFF
+
+enum {
+ // gyro sensor calibration: GyroCalBias
+ HALINTF_TYPE_GYRO_CAL_BIAS = APP_TO_SENSOR_HAL_TYPE(SENS_TYPE_GYRO, 1),
+
+ // gyro sensor over temperature calibration: GyroOtcData
+ HALINTF_TYPE_GYRO_OTC_DATA = APP_TO_SENSOR_HAL_TYPE(SENS_TYPE_GYRO, 2),
+
+ // mag sensor calibration: MagCalBias
+ HALINTF_TYPE_MAG_CAL_BIAS = APP_TO_SENSOR_HAL_TYPE(SENS_TYPE_MAG, 1),
+
+ // mag local field information: MagLocalField
+ HALINTF_TYPE_MAG_LOCAL_FIELD = APP_TO_SENSOR_HAL_TYPE(SENS_TYPE_MAG, 2),
+
+ // mag sensor spherical calibration: MagSphericalData
+ HALINTF_TYPE_MAG_SPERICAL_DATA = APP_TO_SENSOR_HAL_TYPE(SENS_TYPE_MAG, 3),
+};
+
+SET_PACKED_STRUCT_MODE_ON
+struct GyroCalBias {
+ int32_t hardwareBias[3]; // unit depending on hardware implementation
+ float softwareBias[3]; // rad/s
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct GyroOtcData {
+ float lastOffset[3]; // rad/s
+ float lastTemperature; // Celsius
+ float sensitivity[3]; // rad/s/K
+ float intercept[3]; // rad/s
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct MagCalBias {
+ float bias[3]; // in uT
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct MagLocalField {
+ float strength; // in uT
+ float declination; // in rad
+ float inclination; // in rad
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct MagSphericalData {
+ float bias[3];
+ float scale[3];
+ float skew[3];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+// data structure for upload bulk data to sensor hal
+SET_PACKED_STRUCT_MODE_ON
+struct AppToSensorHalDataPayload {
+ uint8_t size; // number of bytes in payload data
+ uint8_t reserved;
+ uint16_t type; // use EVENT_TYPE_BIT_DISCARDABLE to mark discardable update
+ union
+ {
+ uint32_t u[0];
+ int32_t i[0];
+ float f[0];
+ struct GyroCalBias gyroCalBias[0];
+ struct GyroOtcData gyroOtcData[0];
+ struct MagCalBias magCalBias[0];
+ struct MagLocalField magLocalField[0];
+ struct MagSphericalData magSphericalData[0];
+ };
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+// buffer data structure with header, header compatible with HostIntfDataBuffer
+SET_PACKED_STRUCT_MODE_ON
+struct AppToSensorHalDataBuffer {
+ uint32_t eventType; // placeholder for HostIntfDataBuffer event type field
+ struct AppToSensorHalDataPayload payload;
+};
+SET_PACKED_STRUCT_MODE_OFF
+
+#endif //__HALINTF_H
+
diff --git a/firmware/os/inc/hostIntf.h b/firmware/os/inc/hostIntf.h
index 66ed7e4..0d83841 100644
--- a/firmware/os/inc/hostIntf.h
+++ b/firmware/os/inc/hostIntf.h
@@ -34,6 +34,7 @@
HOSTINTF_DATA_TYPE_LOG,
HOSTINTF_DATA_TYPE_APP_TO_HOST,
HOSTINTF_DATA_TYPE_RESET_REASON,
+ HOSTINTF_DATA_TYPE_APP_TO_SENSOR_HAL, // for config data upload
};
SET_PACKED_STRUCT_MODE_ON
diff --git a/firmware/os/inc/seos.h b/firmware/os/inc/seos.h
index f10108a..7b17889 100644
--- a/firmware/os/inc/seos.h
+++ b/firmware/os/inc/seos.h
@@ -79,7 +79,7 @@
#define ENCR_KEY_GOOGLE_PREPOPULATED 1 // our key ID is 1
#define APP_HDR_MAGIC NANOAPP_FW_MAGIC
-#define APP_HDR_VER_CUR 0
+#define APP_HDR_VER_CUR 1
#define FL_APP_HDR_INTERNAL 0x0001 // to be able to fork behavior at run time for internal apps
#define FL_APP_HDR_APPLICATION 0x0002 // image has AppHdr; otherwise is has AppInfo header
diff --git a/inc/chre/sensor.h b/inc/chre/sensor.h
index 1b350c5..9e74336 100644
--- a/inc/chre/sensor.h
+++ b/inc/chre/sensor.h
@@ -444,7 +444,7 @@
*
* struct chreSensorTypeData {
* struct chreSensorDataHeader header;
- * struct {
+ * struct chreSensorTypeSampleData {
* uint32_t timestampDelta;
* union {
* <type> value;
@@ -512,7 +512,8 @@
/**
* Data for a sensor which reports on three axes.
*
- * This is used by CHRE_EVENT_SENSOR_DATA, CHRE_EVENT_SENSOR_GYROSCOPE_DATA,
+ * This is used by CHRE_EVENT_SENSOR_ACCELEROMETER_DATA,
+ * CHRE_EVENT_SENSOR_GYROSCOPE_DATA,
* CHRE_EVENT_SENSOR_GYROSCOPE_BIAS_INFO,
* CHRE_EVENT_SENSOR_GEOMAGNETIC_FIELD_DATA, and
* CHRE_EVENT_SENSOR_GEOMAGNETIC_FIELD_BIAS_INFO.
@@ -522,7 +523,7 @@
* @see chreSensorDataHeader
*/
struct chreSensorDataHeader header;
- struct {
+ struct chreSensorThreeAxisSampleData {
/**
* @see chreSensorDataHeader
*/
@@ -557,7 +558,7 @@
*/
struct chreSensorOccurrenceData {
struct chreSensorDataHeader header;
- struct {
+ struct chreSensorOccurenceSampleData {
uint32_t timestampDelta;
// This space intentionally left blank.
// Only the timestamp is meaningful here, there
@@ -570,7 +571,7 @@
*/
struct chreSensorFloatData {
struct chreSensorDataHeader header;
- struct {
+ struct chreSensorFloatSampleData {
uint32_t timestampDelta;
union {
float value;
@@ -585,7 +586,7 @@
*/
struct chreSensorByteData {
struct chreSensorDataHeader header;
- struct {
+ struct chreSensorByteSampleData {
uint32_t timestampDelta;
union {
uint8_t value;
@@ -799,8 +800,8 @@
*
* @see chreSensorConfigure
*/
-inline bool chreSensorConfigureModeOnly(uint32_t sensorHandle,
- enum chreSensorConfigureMode mode) {
+static inline bool chreSensorConfigureModeOnly(
+ uint32_t sensorHandle, enum chreSensorConfigureMode mode) {
return chreSensorConfigure(sensorHandle,
mode,
CHRE_SENSOR_INTERVAL_DEFAULT,
diff --git a/sensorhal/Android.mk b/sensorhal/Android.mk
index 9c00452..1f35410 100644
--- a/sensorhal/Android.mk
+++ b/sensorhal/Android.mk
@@ -60,6 +60,7 @@
LOCAL_MODULE_RELATIVE_PATH := hw
LOCAL_MODULE_TAGS := optional
LOCAL_MODULE_OWNER := google
+LOCAL_PROPRIETARY_MODULE := true
LOCAL_CFLAGS += $(COMMON_CFLAGS)
@@ -78,6 +79,15 @@
libstagefright_foundation \
libutils
+ifeq ($(NANOHUB_SENSORHAL_DIRECT_REPORT_ENABLED), true)
+LOCAL_CFLAGS += -DDIRECT_REPORT_ENABLED
+endif
+
+ifeq ($(NANOHUB_SENSORHAL_DYNAMIC_SENSOR_EXT_ENABLED), true)
+LOCAL_CFLAGS += -DDYNAMIC_SENSOR_EXT_ENABLED
+LOCAL_SHARED_LIBRARIES += libdynamic_sensor_ext
+endif
+
include $(BUILD_SHARED_LIBRARY)
################################################################################
@@ -88,6 +98,7 @@
LOCAL_MODULE_RELATIVE_PATH := hw
LOCAL_MODULE_TAGS := optional
LOCAL_MODULE_OWNER := google
+LOCAL_PROPRIETARY_MODULE := true
LOCAL_CFLAGS += $(COMMON_CFLAGS)
@@ -114,8 +125,12 @@
LOCAL_MODULE := libhubconnection
LOCAL_MODULE_TAGS := optional
LOCAL_MODULE_OWNER := google
+LOCAL_PROPRIETARY_MODULE := true
LOCAL_CFLAGS += $(COMMON_CFLAGS)
+ifeq ($(PRODUCT_FULL_TREBLE),true)
+LOCAL_CFLAGS += -DUSE_SENSORSERVICE_TO_GET_FIFO
+endif
ifeq ($(NANOHUB_SENSORHAL_LID_STATE_ENABLED), true)
LOCAL_CFLAGS += -DLID_STATE_REPORTING_ENABLED
@@ -129,21 +144,30 @@
LOCAL_CFLAGS += -DDOUBLE_TOUCH_ENABLED
endif
+ifeq ($(NANOHUB_SENSORHAL_DIRECT_REPORT_ENABLED), true)
+LOCAL_CFLAGS += -DDIRECT_REPORT_ENABLED
+endif
+
LOCAL_C_INCLUDES += \
device/google/contexthub/firmware/os/inc
LOCAL_SRC_FILES := \
- hubconnection.cpp
+ hubconnection.cpp \
+ directchannel.cpp
LOCAL_STATIC_LIBRARIES := \
libhubutilcommon
LOCAL_SHARED_LIBRARIES := \
+ android.frameworks.schedulerservice@1.0 \
libcutils \
+ libhardware \
+ libhardware_legacy \
+ libhidlbase \
+ libhidltransport \
liblog \
libstagefright_foundation \
- libhardware_legacy \
- libutils
+ libutils \
include $(BUILD_SHARED_LIBRARY)
diff --git a/sensorhal/directchannel.cpp b/sensorhal/directchannel.cpp
new file mode 100644
index 0000000..d0e719d
--- /dev/null
+++ b/sensorhal/directchannel.cpp
@@ -0,0 +1,289 @@
+/*
+ * 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 "directchannel"
+#include "directchannel.h"
+
+#include <cutils/ashmem.h>
+#include <hardware/sensors.h>
+#include <utils/Log.h>
+
+#include <sys/mman.h>
+
+namespace android {
+
+bool DirectChannelBase::isValid() {
+ return mBuffer != nullptr;
+}
+
+int DirectChannelBase::getError() {
+ return mError;
+}
+
+void DirectChannelBase::write(const sensors_event_t * ev) {
+ if (isValid()) {
+ mBuffer->write(ev, 1);
+ }
+}
+
+AshmemDirectChannel::AshmemDirectChannel(const struct sensors_direct_mem_t *mem) : mAshmemFd(0) {
+ mAshmemFd = mem->handle->data[0];
+
+ if (!::ashmem_valid(mAshmemFd)) {
+ mError = BAD_VALUE;
+ return;
+ }
+
+ if ((size_t)::ashmem_get_size_region(mAshmemFd) != mem->size) {
+ mError = BAD_VALUE;
+ return;
+ }
+
+ mSize = mem->size;
+
+ mBase = ::mmap(NULL, mem->size, PROT_WRITE, MAP_SHARED, mAshmemFd, 0);
+ if (mBase == nullptr) {
+ mError = NO_MEMORY;
+ return;
+ }
+
+ mBuffer = std::unique_ptr<LockfreeBuffer>(new LockfreeBuffer(mBase, mSize));
+ if (!mBuffer) {
+ mError = NO_MEMORY;
+ }
+}
+
+AshmemDirectChannel::~AshmemDirectChannel() {
+ if (mBase) {
+ mBuffer = nullptr;
+ ::munmap(mBase, mSize);
+ mBase = nullptr;
+ }
+ ::close(mAshmemFd);
+}
+
+ANDROID_SINGLETON_STATIC_INSTANCE(GrallocHalWrapper);
+
+GrallocHalWrapper::GrallocHalWrapper()
+ : mError(NO_INIT), mVersion(-1),
+ mGrallocModule(nullptr), mAllocDevice(nullptr), mGralloc1Device(nullptr),
+ mPfnRetain(nullptr), mPfnRelease(nullptr), mPfnLock(nullptr), mPfnUnlock(nullptr) {
+ const hw_module_t *module;
+ status_t err = ::hw_get_module(GRALLOC_HARDWARE_MODULE_ID, &module);
+ ALOGE_IF(err, "couldn't load %s module (%s)", GRALLOC_HARDWARE_MODULE_ID, strerror(-err));
+
+ if (module == nullptr) {
+ mError = (err < 0) ? err : NO_INIT;
+ }
+
+ switch ((module->module_api_version >> 8) & 0xFF) {
+ case 0:
+ err = ::gralloc_open(module, &mAllocDevice);
+ if (err != NO_ERROR) {
+ ALOGE("cannot open alloc device (%s)", strerror(-err));
+ break;
+ }
+
+ if (mAllocDevice == nullptr) {
+ ALOGE("gralloc_open returns no error, but result is nullptr");
+ err = INVALID_OPERATION;
+ break;
+ }
+
+ // successfully initialized gralloc
+ mGrallocModule = (gralloc_module_t *)module;
+ mVersion = 0;
+ break;
+ case 1:
+ err = ::gralloc1_open(module, &mGralloc1Device);
+ if (err != NO_ERROR) {
+ ALOGE("cannot open gralloc1 device (%s)", strerror(-err));
+ break;
+ }
+
+ if (mGralloc1Device == nullptr || mGralloc1Device->getFunction == nullptr) {
+ ALOGE("gralloc1_open returns no error, but result is nullptr");
+ err = INVALID_OPERATION;
+ break;
+ }
+
+ mPfnRetain = (GRALLOC1_PFN_RETAIN)(mGralloc1Device->getFunction(mGralloc1Device,
+ GRALLOC1_FUNCTION_RETAIN));
+ mPfnRelease = (GRALLOC1_PFN_RELEASE)(mGralloc1Device->getFunction(mGralloc1Device,
+ GRALLOC1_FUNCTION_RELEASE));
+ mPfnLock = (GRALLOC1_PFN_LOCK)(mGralloc1Device->getFunction(mGralloc1Device,
+ GRALLOC1_FUNCTION_LOCK));
+ mPfnUnlock = (GRALLOC1_PFN_UNLOCK)(mGralloc1Device->getFunction(mGralloc1Device,
+ GRALLOC1_FUNCTION_UNLOCK));
+ 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);
+ err = BAD_VALUE;
+ break;
+ }
+
+ // successfully initialized gralloc1
+ mGrallocModule = (gralloc_module_t *)module;
+ mVersion = 1;
+ break;
+ default:
+ ALOGE("Unknown version, not supported");
+ break;
+ }
+ mError = err;
+}
+
+GrallocHalWrapper::~GrallocHalWrapper() {
+ if (mAllocDevice != nullptr) {
+ ::gralloc_close(mAllocDevice);
+ }
+}
+
+int GrallocHalWrapper::registerBuffer(const native_handle_t *handle) {
+ switch (mVersion) {
+ case 0:
+ return mGrallocModule->registerBuffer(mGrallocModule, handle);
+ case 1:
+ return mapGralloc1Error(mPfnRetain(mGralloc1Device, handle));
+ default:
+ return NO_INIT;
+ }
+}
+
+int GrallocHalWrapper::unregisterBuffer(const native_handle_t *handle) {
+ switch (mVersion) {
+ case 0:
+ return mGrallocModule->unregisterBuffer(mGrallocModule, handle);
+ case 1:
+ return mapGralloc1Error(mPfnRelease(mGralloc1Device, handle));
+ default:
+ return NO_INIT;
+ }
+}
+
+int GrallocHalWrapper::lock(const native_handle_t *handle,
+ int usage, int l, int t, int w, int h, void **vaddr) {
+ switch (mVersion) {
+ case 0:
+ return mGrallocModule->lock(mGrallocModule, handle, usage, l, t, w, h, vaddr);
+ case 1: {
+ const gralloc1_rect_t rect = {
+ .left = l,
+ .top = t,
+ .width = w,
+ .height = h
+ };
+ return mapGralloc1Error(mPfnLock(mGralloc1Device, handle,
+ GRALLOC1_PRODUCER_USAGE_CPU_WRITE_OFTEN,
+ GRALLOC1_CONSUMER_USAGE_NONE,
+ &rect, vaddr, -1));
+ }
+ default:
+ return NO_INIT;
+ }
+}
+
+int GrallocHalWrapper::unlock(const native_handle_t *handle) {
+ switch (mVersion) {
+ case 0:
+ return mGrallocModule->unlock(mGrallocModule, handle);
+ case 1: {
+ int32_t dummy;
+ return mapGralloc1Error(mPfnUnlock(mGralloc1Device, handle, &dummy));
+ }
+ default:
+ return NO_INIT;
+ }
+}
+
+int GrallocHalWrapper::mapGralloc1Error(int grallocError) {
+ switch (grallocError) {
+ case GRALLOC1_ERROR_NONE:
+ return NO_ERROR;
+ case GRALLOC1_ERROR_BAD_DESCRIPTOR:
+ case GRALLOC1_ERROR_BAD_HANDLE:
+ case GRALLOC1_ERROR_BAD_VALUE:
+ return BAD_VALUE;
+ case GRALLOC1_ERROR_NOT_SHARED:
+ case GRALLOC1_ERROR_NO_RESOURCES:
+ return NO_MEMORY;
+ case GRALLOC1_ERROR_UNDEFINED:
+ case GRALLOC1_ERROR_UNSUPPORTED:
+ return INVALID_OPERATION;
+ default:
+ return UNKNOWN_ERROR;
+ }
+}
+
+GrallocDirectChannel::GrallocDirectChannel(const struct sensors_direct_mem_t *mem)
+ : mNativeHandle(nullptr) {
+ if (mem->handle == nullptr) {
+ ALOGE("mem->handle == nullptr");
+ mError = BAD_VALUE;
+ return;
+ }
+
+ mNativeHandle = ::native_handle_clone(mem->handle);
+ if (mNativeHandle == nullptr) {
+ ALOGE("clone mem->handle failed...");
+ mError = NO_MEMORY;
+ return;
+ }
+
+ mError = GrallocHalWrapper::getInstance().registerBuffer(mNativeHandle);
+ if (mError != NO_ERROR) {
+ ALOGE("registerBuffer failed");
+ return;
+ }
+
+ mError = GrallocHalWrapper::getInstance().lock(mNativeHandle,
+ GRALLOC_USAGE_SW_WRITE_OFTEN, 0, 0, mem->size, 1, &mBase);
+ if (mError != NO_ERROR) {
+ ALOGE("lock buffer failed");
+ return;
+ }
+
+ if (mBase == nullptr) {
+ ALOGE("lock buffer => nullptr");
+ mError = NO_MEMORY;
+ return;
+ }
+
+ mSize = mem->size;
+ mBuffer = std::make_unique<LockfreeBuffer>(mBase, mSize);
+ if (!mBuffer) {
+ mError = NO_MEMORY;
+ return;
+ }
+
+ mError = NO_ERROR;
+}
+
+GrallocDirectChannel::~GrallocDirectChannel() {
+ if (mNativeHandle != nullptr) {
+ if (mBase) {
+ mBuffer = nullptr;
+ GrallocHalWrapper::getInstance().unlock(mNativeHandle);
+ mBase = nullptr;
+ }
+ GrallocHalWrapper::getInstance().unregisterBuffer(mNativeHandle);
+ ::native_handle_close(mNativeHandle);
+ ::native_handle_delete(mNativeHandle);
+ mNativeHandle = nullptr;
+ }
+}
+
+} // namespace android
diff --git a/sensorhal/directchannel.h b/sensorhal/directchannel.h
new file mode 100644
index 0000000..192e35d
--- /dev/null
+++ b/sensorhal/directchannel.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2016 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 DIRECTCHANNEL_H_
+#define DIRECTCHANNEL_H_
+
+#include "ring.h"
+#include <cutils/native_handle.h>
+#include <hardware/gralloc.h>
+#include <hardware/gralloc1.h>
+#include <hardware/sensors.h>
+#include <utils/Singleton.h>
+#include <memory>
+
+namespace android {
+
+class DirectChannelBase {
+public:
+ DirectChannelBase() : mError(NO_INIT), mSize(0), mBase(nullptr) { }
+ virtual ~DirectChannelBase() {}
+
+ bool isValid();
+ int getError();
+ void write(const sensors_event_t * ev);
+
+protected:
+ int mError;
+ std::unique_ptr<LockfreeBuffer> mBuffer;
+
+ size_t mSize;
+ void* mBase;
+};
+
+class AshmemDirectChannel : public DirectChannelBase {
+public:
+ AshmemDirectChannel(const struct sensors_direct_mem_t *mem);
+ virtual ~AshmemDirectChannel();
+private:
+ int mAshmemFd;
+};
+
+class GrallocHalWrapper : public Singleton<GrallocHalWrapper> {
+public:
+ int registerBuffer(const native_handle_t *handle);
+ 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);
+private:
+ friend class Singleton<GrallocHalWrapper>;
+ GrallocHalWrapper();
+ ~GrallocHalWrapper();
+ static int mapGralloc1Error(int grallocError);
+
+ int mError;
+ int mVersion;
+ gralloc_module_t *mGrallocModule;
+ // gralloc
+ alloc_device_t *mAllocDevice;
+
+ // gralloc1
+ gralloc1_device_t *mGralloc1Device;
+ GRALLOC1_PFN_RETAIN mPfnRetain;
+ GRALLOC1_PFN_RELEASE mPfnRelease;
+ GRALLOC1_PFN_LOCK mPfnLock;
+ GRALLOC1_PFN_UNLOCK mPfnUnlock;
+};
+
+class GrallocDirectChannel : public DirectChannelBase {
+public:
+ GrallocDirectChannel(const struct sensors_direct_mem_t *mem);
+ virtual ~GrallocDirectChannel();
+private:
+ native_handle_t *mNativeHandle;
+};
+
+} // namespace android
+
+#endif // DIRECTCHANNEL_H_
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index 5c5db92..b57a334 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -14,13 +14,14 @@
* limitations under the License.
*/
-#include "hubconnection.h"
-#include "eventnums.h"
-#include "sensType.h"
-
#define LOG_TAG "nanohub"
-#include <utils/Log.h>
-#include <utils/SystemClock.h>
+#define LOG_NDEBUG 1
+
+#include "hubconnection.h"
+
+// TODO: remove the includes that introduce LIKELY and UNLIKELY (firmware/os/inc/toolchain.h)
+#undef LIKELY
+#undef UNLIKELY
#include "file.h"
#include "JSONObject.h"
@@ -35,9 +36,18 @@
#include <linux/input.h>
#include <linux/uinput.h>
+#include <android/frameworks/schedulerservice/1.0/ISchedulingPolicyService.h>
+#include <cutils/ashmem.h>
#include <cutils/properties.h>
#include <hardware_legacy/power.h>
#include <media/stagefright/foundation/ADebug.h>
+#include <utils/Log.h>
+#include <utils/SystemClock.h>
+
+#include <algorithm>
+#include <cmath>
+#include <sstream>
+#include <vector>
#define APP_ID_GET_VENDOR(appid) ((appid) >> 24)
#define APP_ID_MAKE(vendor, app) ((((uint64_t)(vendor)) << 24) | ((app) & 0x00FFFFFF))
@@ -47,7 +57,7 @@
#define SENS_TYPE_TO_EVENT(_sensorType) (EVT_NO_FIRST_SENSOR_EVENT + (_sensorType))
#define NANOHUB_FILE_PATH "/dev/nanohub"
-#define NANOHUB_LOCK_DIR "/data/system/nanohub_lock"
+#define NANOHUB_LOCK_DIR "/data/vendor/sensor/nanohub_lock"
#define NANOHUB_LOCK_FILE NANOHUB_LOCK_DIR "/lock"
#define MAG_BIAS_FILE_PATH "/sys/class/power_supply/battery/compass_compensation"
#define DOUBLE_TOUCH_FILE_PATH "/sys/android_touch/synaptics_rmi4_dsx/wake_event"
@@ -60,12 +70,8 @@
#define MIN_MAG_SQ (10.0f * 10.0f)
#define MAX_MAG_SQ (80.0f * 80.0f)
-#define ACCEL_RAW_KSCALE (8.0f * 9.81f / 32768.0f)
-
#define OS_LOG_EVENT 0x474F4C41 // ascii: ALOG
-#define HUBCONNECTION_SCHED_FIFO_PRIORITY 10
-
#ifdef LID_STATE_REPORTING_ENABLED
const char LID_STATE_PROPERTY[] = "sensors.contexthub.lid_state";
const char LID_STATE_UNKNOWN[] = "unknown";
@@ -76,6 +82,12 @@
static const uint32_t delta_time_encoded = 1;
static const uint32_t delta_time_shift_table[2] = {9, 0};
+#ifdef USE_SENSORSERVICE_TO_GET_FIFO
+// TODO(b/35219747): retain sched_fifo before eval is done to avoid
+// performance regression.
+const char SCHED_FIFO_PRIOIRTY[] = "sensor.hubconnection.sched_fifo";
+#endif
+
namespace android {
// static
@@ -102,6 +114,8 @@
: Thread(false /* canCallJava */),
mRing(10 *1024),
mActivityEventHandler(NULL),
+ mScaleAccel(1.0f),
+ mScaleMag(1.0f),
mStepCounterOffset(0ull),
mLastStepCount(0ull)
{
@@ -110,6 +124,7 @@
mMagAccuracyRestore = SENSOR_STATUS_UNRELIABLE;
mGyroBias[0] = mGyroBias[1] = mGyroBias[2] = 0.0f;
mAccelBias[0] = mAccelBias[1] = mAccelBias[2] = 0.0f;
+ memset(&mGyroOtcData, 0, sizeof(mGyroOtcData));
memset(&mSensorState, 0x00, sizeof(mSensorState));
mFd = open(NANOHUB_FILE_PATH, O_RDWR);
@@ -153,6 +168,9 @@
#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_UNCALIBRATED].sensorType = SENS_TYPE_ACCEL;
+ mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt = COMMS_SENSOR_ACCEL;
mSensorState[COMMS_SENSOR_GYRO].sensorType = SENS_TYPE_GYRO;
mSensorState[COMMS_SENSOR_GYRO].alt = COMMS_SENSOR_GYRO_UNCALIBRATED;
mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].sensorType = SENS_TYPE_GYRO;
@@ -227,7 +245,7 @@
// set initial lid state
if (property_set(LID_STATE_PROPERTY, LID_STATE_UNKNOWN) < 0) {
- ALOGE("could not set lid_state property");
+ ALOGW("could not set lid_state property");
}
// enable hall sensor for folio
@@ -235,6 +253,13 @@
queueActivate(COMMS_SENSOR_HALL, true /* enable */);
}
#endif // LID_STATE_REPORTING_ENABLED
+
+#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>());
+#endif // DIRECT_REPORT_ENABLED
}
HubConnection::~HubConnection()
@@ -245,16 +270,49 @@
void HubConnection::onFirstRef()
{
run("HubConnection", PRIORITY_URGENT_DISPLAY);
- enableSchedFifoMode();
+#ifdef USE_SENSORSERVICE_TO_GET_FIFO
+ if (property_get_bool(SCHED_FIFO_PRIOIRTY, true)) {
+ ALOGV("Try activate sched-fifo priority for HubConnection thread");
+ mEnableSchedFifoThread = std::thread(enableSchedFifoMode, this);
+ }
+#else
+ enableSchedFifoMode(this);
+#endif
}
// Set main thread to SCHED_FIFO to lower sensor event latency when system is under load
-void HubConnection::enableSchedFifoMode() {
+void HubConnection::enableSchedFifoMode(sp<HubConnection> hub) {
+#ifdef USE_SENSORSERVICE_TO_GET_FIFO
+ using ::android::frameworks::schedulerservice::V1_0::ISchedulingPolicyService;
+ using ::android::hardware::Return;
+
+ // SchedulingPolicyService will not start until system server start.
+ // Thus, cannot block on this.
+ sp<ISchedulingPolicyService> scheduler = ISchedulingPolicyService::getService();
+
+ if (scheduler == nullptr) {
+ ALOGW("Couldn't get scheduler scheduler to set SCHED_FIFO.");
+ } else {
+ Return<int32_t> max = scheduler->getMaxAllowedPriority();
+ if (!max.isOk()) {
+ ALOGW("Failed to retrieve maximum allowed priority for HubConnection.");
+ return;
+ }
+ Return<bool> ret = scheduler->requestPriority(::getpid(), hub->getTid(), max);
+ if (!ret.isOk() || !ret) {
+ ALOGW("Failed to set SCHED_FIFO for HubConnection.");
+ } else {
+ ALOGV("Enabled sched fifo thread mode (prio %d)", static_cast<int32_t>(max));
+ }
+ }
+#else
+#define HUBCONNECTION_SCHED_FIFO_PRIORITY 10
struct sched_param param = {0};
param.sched_priority = HUBCONNECTION_SCHED_FIFO_PRIORITY;
- if (sched_setscheduler(getTid(), SCHED_FIFO | SCHED_RESET_ON_FORK, ¶m) != 0) {
- ALOGE("Couldn't set SCHED_FIFO for HubConnection thread");
+ if (sched_setscheduler(hub->getTid(), SCHED_FIFO | SCHED_RESET_ON_FORK, ¶m) != 0) {
+ ALOGW("Couldn't set SCHED_FIFO for HubConnection thread");
}
+#endif
}
status_t HubConnection::initCheck() const
@@ -331,6 +389,32 @@
return true;
}
+static std::vector<int32_t> getInt32Setting(const sp<JSONObject> &settings, const char *key) {
+ std::vector<int32_t> ret;
+
+ sp<JSONArray> array;
+ if (settings->getArray(key, &array)) {
+ ret.resize(array->size());
+ for (size_t i = 0; i < array->size(); ++i) {
+ array->getInt32(i, &ret[i]);
+ }
+ }
+ return ret;
+}
+
+static std::vector<float> getFloatSetting(const sp<JSONObject> &settings, const char *key) {
+ std::vector<float> ret;
+
+ sp<JSONArray> array;
+ if (settings->getArray(key, &array)) {
+ ret.resize(array->size());
+ for (size_t i = 0; i < array->size(); ++i) {
+ array->getFloat(i, &ret[i]);
+ }
+ }
+ return ret;
+}
+
static void loadSensorSettings(sp<JSONObject>* settings,
sp<JSONObject>* saved_settings) {
File settings_file(CONTEXTHUB_SETTINGS_PATH, "r");
@@ -338,7 +422,7 @@
status_t err;
if ((err = settings_file.initCheck()) != OK) {
- ALOGE("settings file open failed: %d (%s)",
+ ALOGW("settings file open failed: %d (%s)",
err,
strerror(-err));
@@ -348,7 +432,7 @@
}
if ((err = saved_settings_file.initCheck()) != OK) {
- ALOGE("saved settings file open failed: %d (%s)",
+ ALOGW("saved settings file open failed: %d (%s)",
err,
strerror(-err));
*saved_settings = new JSONObject;
@@ -363,7 +447,7 @@
status_t err;
if ((err = saved_settings_file.initCheck()) != OK) {
- ALOGE("saved settings file open failed %d (%s)",
+ ALOGW("saved settings file open failed %d (%s)",
err,
strerror(-err));
return;
@@ -378,27 +462,37 @@
#endif // USB_MAG_BIAS_REPORTING_ENABLED
magArray->addFloat(mMagBias[1]);
magArray->addFloat(mMagBias[2]);
- settingsObject->setArray("mag", magArray);
+ settingsObject->setArray(MAG_BIAS_TAG, magArray);
// Add gyro settings
sp<JSONArray> gyroArray = new JSONArray;
gyroArray->addFloat(mGyroBias[0]);
gyroArray->addFloat(mGyroBias[1]);
gyroArray->addFloat(mGyroBias[2]);
- settingsObject->setArray("gyro_sw", gyroArray);
+ settingsObject->setArray(GYRO_SW_BIAS_TAG, gyroArray);
// Add accel settings
sp<JSONArray> accelArray = new JSONArray;
accelArray->addFloat(mAccelBias[0]);
accelArray->addFloat(mAccelBias[1]);
accelArray->addFloat(mAccelBias[2]);
- settingsObject->setArray("accel_sw", accelArray);
+ settingsObject->setArray(ACCEL_SW_BIAS_TAG, accelArray);
+
+ // Add overtemp calibration values for gyro
+ sp<JSONArray> gyroOtcDataArray = new JSONArray;
+ const float *f;
+ size_t i;
+ for (f = reinterpret_cast<const float *>(&mGyroOtcData), i = 0;
+ i < sizeof(mGyroOtcData)/sizeof(float); ++i, ++f) {
+ gyroOtcDataArray->addFloat(*f);
+ }
+ settingsObject->setArray(GYRO_OTC_DATA_TAG, gyroOtcDataArray);
// Write the JSON string to disk.
AString serializedSettings = settingsObject->toString();
size_t size = serializedSettings.size();
if ((err = saved_settings_file.write(serializedSettings.c_str(), size)) != (ssize_t)size) {
- ALOGE("saved settings file write failed %d (%s)",
+ ALOGW("saved settings file write failed %d (%s)",
err,
strerror(-err));
}
@@ -530,13 +624,13 @@
if (cnt > 0) {
// If event is a wake event, protect it with a wakelock
protectIfWakeEvent(sensor);
- mRing.write(nev, cnt);
+ write(nev, cnt);
}
}
-void HubConnection::magAccuracyUpdate(float x, float y, float z)
+uint8_t HubConnection::magAccuracyUpdate(sensors_vec_t *sv)
{
- float magSq = x * x + y * y + z * z;
+ float magSq = sv->x * sv->x + sv->y * sv->y + sv->z * sv->z;
if (magSq < MIN_MAG_SQ || magSq > MAX_MAG_SQ) {
// save last good accuracy (either MEDIUM or HIGH)
@@ -547,22 +641,65 @@
// restore
mMagAccuracy = mMagAccuracyRestore;
}
+
+ return mMagAccuracy;
}
void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct RawThreeAxisSample *sample, __attribute__((unused)) bool highAccuracy)
{
sensors_vec_t *sv;
+ uncalibrated_event_t *ue;
sensors_event_t nev[2];
int cnt = 0;
switch (sensor) {
case COMMS_SENSOR_ACCEL:
- sv = &initEv(&nev[cnt++], timestamp, type, sensor)->acceleration;
- sv->x = sample->ix * ACCEL_RAW_KSCALE;
- sv->y = sample->iy * ACCEL_RAW_KSCALE;
- sv->z = sample->iz * ACCEL_RAW_KSCALE;
+ sv = &initEv(&nev[cnt], timestamp, type, sensor)->acceleration;
+ sv->x = sample->ix * mScaleAccel;
+ sv->y = sample->iy * mScaleAccel;
+ sv->z = sample->iz * mScaleAccel;
sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+ sendDirectReportEvent(&nev[cnt], 1);
+
+ if (mSensorState[sensor].enable) {
+ ++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];
+ }
break;
+ case COMMS_SENSOR_MAG:
+ sv = &initEv(&nev[cnt], timestamp, type, sensor)->magnetic;
+ sv->x = sample->ix * mScaleMag;
+ sv->y = sample->iy * mScaleMag;
+ sv->z = sample->iz * mScaleMag;
+ sv->status = magAccuracyUpdate(sv);
+ sendDirectReportEvent(&nev[cnt], 1);
+
+ if (mSensorState[sensor].enable) {
+ ++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];
+ }
default:
break;
}
@@ -570,7 +707,7 @@
if (cnt > 0) {
// If event is a wake event, protect it with a wakelock
protectIfWakeEvent(sensor);
- mRing.write(nev, cnt);
+ write(nev, cnt);
}
}
@@ -586,19 +723,39 @@
switch (sensor) {
case COMMS_SENSOR_ACCEL:
- sv = &initEv(&nev[cnt++], timestamp, type, sensor)->acceleration;
+ sv = &initEv(&nev[cnt], timestamp, type, sensor)->acceleration;
sv->x = sample->x;
sv->y = sample->y;
sv->z = sample->z;
sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+ sendDirectReportEvent(&nev[cnt], 1);
+
+ if (mSensorState[sensor].enable) {
+ ++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];
+ }
break;
case COMMS_SENSOR_GYRO:
+ sv = &initEv(&nev[cnt], timestamp, type, sensor)->gyro;
+ sv->x = sample->x;
+ sv->y = sample->y;
+ sv->z = sample->z;
+ sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+ sendDirectReportEvent(&nev[cnt], 1);
+
if (mSensorState[sensor].enable) {
- sv = &initEv(&nev[cnt++], timestamp, type, sensor)->gyro;
- sv->x = sample->x;
- sv->y = sample->y;
- sv->z = sample->z;
- sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+ ++cnt;
}
if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable) {
@@ -626,14 +783,15 @@
saveSensorSettings();
break;
case COMMS_SENSOR_MAG:
- magAccuracyUpdate(sample->x, sample->y, sample->z);
+ sv = &initEv(&nev[cnt], timestamp, type, sensor)->magnetic;
+ sv->x = sample->x;
+ sv->y = sample->y;
+ sv->z = sample->z;
+ sv->status = magAccuracyUpdate(sv);
+ sendDirectReportEvent(&nev[cnt], 1);
if (mSensorState[sensor].enable) {
- sv = &initEv(&nev[cnt++], timestamp, type, sensor)->magnetic;
- sv->x = sample->x;
- sv->y = sample->y;
- sv->z = sample->z;
- sv->status = mMagAccuracy;
+ ++cnt;
}
if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable) {
@@ -704,7 +862,7 @@
if (cnt > 0) {
// If event is a wake event, protect it with a wakelock
protectIfWakeEvent(sensor);
- mRing.write(nev, cnt);
+ write(nev, cnt);
}
}
@@ -714,7 +872,7 @@
if (mInotifyPollIndex >= 0) {
char buf[sizeof(struct inotify_event) + NAME_MAX + 1];
int ret = ::read(mPollFds[mInotifyPollIndex].fd, buf, sizeof(buf));
- ALOGD("Discarded %d bytes of inotify data", ret);
+ ALOGV("Discarded %d bytes of inotify data", ret);
}
}
@@ -746,21 +904,21 @@
initConfigCmd(&cmd, i);
- ALOGI("restoring: sensor=%d, handle=%d, enable=%d, period=%" PRId64 ", latency=%" PRId64,
+ ALOGV("restoring: sensor=%d, handle=%d, enable=%d, period=%" PRId64 ", latency=%" PRId64,
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 = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
if (ret != sizeof(cmd)) {
- ALOGE("failed to send config command to restore sensor %d\n", cmd.sensorType);
+ ALOGW("failed to send config command to restore sensor %d\n", cmd.sensorType);
}
cmd.cmd = CONFIG_CMD_FLUSH;
for (int j = 0; j < mSensorState[i].flushCnt; j++) {
- int ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
+ int ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
if (ret != sizeof(cmd)) {
- ALOGE("failed to send flush command to sensor %d\n", cmd.sensorType);
+ ALOGW("failed to send flush command to sensor %d\n", cmd.sensorType);
}
}
}
@@ -788,16 +946,44 @@
ALOGW("osLog: %s", &buf[5]);
break;
case 'I':
- ALOGI("osLog: %s", &buf[5]);
+ // The other side of this is too chatty, reducing the priority to VERBOSE
+ ALOGV("osLog: %s", &buf[5]);
break;
case 'D':
- ALOGD("osLog: %s", &buf[5]);
+ // The other side of this is too chatty, reducing the priority to VERBOSE
+ ALOGV("osLog: %s", &buf[5]);
break;
default:
break;
}
}
+void HubConnection::processAppData(uint8_t *buf, ssize_t len) {
+ if (len < static_cast<ssize_t>(sizeof(AppToSensorHalDataBuffer)))
+ return;
+
+ AppToSensorHalDataPayload *data =
+ &(reinterpret_cast<AppToSensorHalDataBuffer *>(buf)->payload);
+ if (data->size + sizeof(AppToSensorHalDataBuffer) != len) {
+ ALOGW("Received corrupted data update packet, len %zd, size %u", len, data->size);
+ return;
+ }
+
+ switch (data->type & APP_TO_SENSOR_HAL_TYPE_MASK) {
+ case HALINTF_TYPE_GYRO_OTC_DATA:
+ if (data->size != sizeof(GyroOtcData)) {
+ ALOGW("Corrupted HALINTF_TYPE_GYRO_OTC_DATA with size %u", data->size);
+ return;
+ }
+ mGyroOtcData = data->gyroOtcData[0];
+ saveSensorSettings();
+ break;
+ default:
+ ALOGW("Unknown app to hal data type 0x%04x", data->type);
+ break;
+ }
+}
+
ssize_t HubConnection::processBuf(uint8_t *buf, size_t len)
{
struct nAxisEvent *data = (struct nAxisEvent *)buf;
@@ -816,6 +1002,9 @@
case OS_LOG_EVENT:
postOsLog(buf, len);
return 0;
+ case EVT_APP_TO_SENSOR_HAL_DATA:
+ processAppData(buf, len);
+ return 0;
case SENS_TYPE_TO_EVENT(SENS_TYPE_ACCEL):
type = SENSOR_TYPE_ACCELEROMETER;
sensor = COMMS_SENSOR_ACCEL;
@@ -839,6 +1028,11 @@
bias = COMMS_SENSOR_MAG_BIAS;
three = true;
break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_MAG_RAW):
+ type = SENSOR_TYPE_MAGNETIC_FIELD;
+ sensor = COMMS_SENSOR_MAG;
+ rawThree = true;
+ break;
case SENS_TYPE_TO_EVENT(SENS_TYPE_ALS):
type = SENSOR_TYPE_LIGHT;
sensor = COMMS_SENSOR_LIGHT;
@@ -1028,11 +1222,11 @@
restoreSensorState();
return 0;
default:
- ALOGE("unknown evtType: 0x%08x\n", data->evtType);
+ ALOGW("unknown evtType: 0x%08x len: %zu\n", data->evtType, len);
return -1;
}
} else {
- ALOGE("too little data: len=%zu\n", len);
+ ALOGW("too little data: len=%zu\n", len);
return -1;
}
@@ -1048,7 +1242,7 @@
if (one) {
if (ret + sizeof(data->oneSamples[i]) > len) {
- ALOGE("sensor %d (one): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
+ ALOGW("sensor %d (one): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
return -1;
}
if (i > 0)
@@ -1057,7 +1251,7 @@
ret += sizeof(data->oneSamples[i]);
} else if (rawThree) {
if (ret + sizeof(data->rawThreeSamples[i]) > len) {
- ALOGE("sensor %d (rawThree): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
+ ALOGW("sensor %d (rawThree): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
return -1;
}
if (i > 0)
@@ -1066,7 +1260,7 @@
ret += sizeof(data->rawThreeSamples[i]);
} else if (three) {
if (ret + sizeof(data->threeSamples[i]) > len) {
- ALOGE("sensor %d (three): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
+ ALOGW("sensor %d (three): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
return -1;
}
if (i > 0)
@@ -1074,7 +1268,7 @@
processSample(timestamp, type, currSensor, &data->threeSamples[i], data->firstSample.highAccuracy);
ret += sizeof(data->threeSamples[i]);
} else {
- ALOGE("sensor %d (unknown): cannot processSample\n", currSensor);
+ ALOGW("sensor %d (unknown): cannot processSample\n", currSensor);
return -1;
}
}
@@ -1100,12 +1294,12 @@
ev.meta_data.sensor = sensor;
}
- mRing.write(&ev, 1);
- ALOGI("flushing %d", ev.meta_data.sensor);
+ write(&ev, 1);
+ ALOGV("flushing %d", ev.meta_data.sensor);
}
}
} else {
- ALOGE("too little data for sensor %d: len=%zu\n", sensor, len);
+ ALOGW("too little data for sensor %d: len=%zu\n", sensor, len);
return -1;
}
@@ -1119,16 +1313,16 @@
struct {
int32_t hw[3];
float sw[3];
- } gyro, accel;
+ } accel;
+
int32_t proximity, proximity_array[4];
- float barometer, humidity, mag[3], light;
- bool gyro_hw_cal_exists, gyro_sw_cal_exists;
+ float barometer, humidity, light;
bool accel_hw_cal_exists, accel_sw_cal_exists;
loadSensorSettings(&settings, &saved_settings);
- accel_hw_cal_exists = getCalibrationInt32(settings, "accel", accel.hw, 3);
- accel_sw_cal_exists = getCalibrationFloat(saved_settings, "accel_sw", accel.sw);
+ accel_hw_cal_exists = getCalibrationInt32(settings, ACCEL_BIAS_TAG, accel.hw, 3);
+ accel_sw_cal_exists = getCalibrationFloat(saved_settings, ACCEL_SW_BIAS_TAG, accel.sw);
if (accel_hw_cal_exists || accel_sw_cal_exists) {
// Store SW bias so we can remove bias for uncal data
mAccelBias[0] = accel.sw[0];
@@ -1138,15 +1332,70 @@
queueDataInternal(COMMS_SENSOR_ACCEL, &accel, sizeof(accel));
}
- gyro_hw_cal_exists = getCalibrationInt32(settings, "gyro", gyro.hw, 3);
- gyro_sw_cal_exists = getCalibrationFloat(saved_settings, "gyro_sw", gyro.sw);
- if (gyro_hw_cal_exists || gyro_sw_cal_exists) {
- // Store SW bias so we can remove bias for uncal data
- mGyroBias[0] = gyro.sw[0];
- mGyroBias[1] = gyro.sw[1];
- mGyroBias[2] = gyro.sw[2];
+ ALOGV("Use new configuration format");
+ std::vector<int32_t> hardwareGyroBias = getInt32Setting(settings, GYRO_BIAS_TAG);
+ std::vector<float> softwareGyroBias = getFloatSetting(saved_settings, GYRO_SW_BIAS_TAG);
+ if (hardwareGyroBias.size() == 3 || softwareGyroBias.size() == 3) {
+ struct {
+ AppToSensorHalDataPayload header;
+ GyroCalBias data;
+ } packet = {
+ .header = {
+ .size = sizeof(GyroCalBias),
+ .type = HALINTF_TYPE_GYRO_CAL_BIAS }
+ };
+ if (hardwareGyroBias.size() == 3) {
+ std::copy(hardwareGyroBias.begin(), hardwareGyroBias.end(),
+ packet.data.hardwareBias);
+ }
+ if (softwareGyroBias.size() == 3) {
+ // Store SW bias so we can remove bias for uncal data
+ std::copy(softwareGyroBias.begin(), softwareGyroBias.end(),
+ mGyroBias);
- queueDataInternal(COMMS_SENSOR_GYRO, &gyro, sizeof(gyro));
+ std::copy(softwareGyroBias.begin(), softwareGyroBias.end(),
+ packet.data.softwareBias);
+ }
+ // send packet to hub
+ queueDataInternal(COMMS_SENSOR_GYRO, &packet, sizeof(packet));
+ }
+
+ // over temp cal
+ std::vector<float> gyroOtcData = getFloatSetting(saved_settings, GYRO_OTC_DATA_TAG);
+ if (gyroOtcData.size() == sizeof(GyroOtcData) / sizeof(float)) {
+ std::copy(gyroOtcData.begin(), gyroOtcData.end(),
+ reinterpret_cast<float*>(&mGyroOtcData));
+ struct {
+ AppToSensorHalDataPayload header;
+ GyroOtcData data;
+ } packet = {
+ .header = {
+ .size = sizeof(GyroOtcData),
+ .type = HALINTF_TYPE_GYRO_OTC_DATA },
+ .data = mGyroOtcData
+ };
+
+ // send it to hub
+ queueDataInternal(COMMS_SENSOR_GYRO, &packet, sizeof(packet));
+ } else {
+ ALOGW("Illegal otc_gyro data size = %zu", gyroOtcData.size());
+ }
+
+ std::vector<float> magBiasData = getFloatSetting(saved_settings, MAG_BIAS_TAG);
+ if (magBiasData.size() == 3) {
+ // Store SW bias so we can remove bias for uncal data
+ std::copy(magBiasData.begin(), magBiasData.end(), mMagBias);
+
+ struct {
+ AppToSensorHalDataPayload header;
+ MagCalBias mag;
+ } packet = {
+ .header = {
+ .size = sizeof(MagCalBias),
+ .type = HALINTF_TYPE_MAG_CAL_BIAS }
+ };
+ std::copy(magBiasData.begin(), magBiasData.end(), packet.mag.bias);
+ queueDataInternal(COMMS_SENSOR_MAG, &packet, sizeof(packet));
}
if (settings->getFloat("barometer", &barometer))
@@ -1163,22 +1412,13 @@
if (settings->getFloat("light", &light))
queueDataInternal(COMMS_SENSOR_LIGHT, &light, sizeof(light));
-
- if (getCalibrationFloat(saved_settings, "mag", mag)) {
- // Store SW bias so we can remove bias for uncal data
- mMagBias[0] = mag[0];
- mMagBias[1] = mag[1];
- mMagBias[2] = mag[2];
-
- queueDataInternal(COMMS_SENSOR_MAG, mag, sizeof(mag));
- }
}
bool HubConnection::threadLoop() {
- ALOGI("threadLoop: starting");
+ ALOGV("threadLoop: starting");
if (mFd < 0) {
- ALOGE("threadLoop: exiting prematurely: nanohub is unavailable");
+ ALOGW("threadLoop: exiting prematurely: nanohub is unavailable");
return false;
}
waitOnNanohubLock();
@@ -1217,7 +1457,7 @@
::read(mPollFds[mDoubleTouchPollIndex].fd, buf, 16);
sensors_event_t gestureEvent;
initEv(&gestureEvent, elapsedRealtimeNano(), SENSOR_TYPE_PICK_UP_GESTURE, COMMS_SENSOR_GESTURE)->data[0] = 8;
- mRing.write(&gestureEvent, 1);
+ write(&gestureEvent, 1);
}
#endif // DOUBLE_TOUCH_ENABLED
@@ -1235,7 +1475,7 @@
break;
}
} else {
- ALOGE("read -1: errno=%d\n", errno);
+ ALOGW("read -1: errno=%d\n", errno);
}
}
}
@@ -1281,6 +1521,9 @@
cmd->rate = mSensorState[handle].rate;
cmd->latency = mSensorState[handle].latency;
}
+
+ // will be a nop if direct report mode is not enabled
+ mergeDirectReportRequest(cmd, handle);
}
void HubConnection::queueActivate(int handle, bool enable)
@@ -1295,15 +1538,15 @@
initConfigCmd(&cmd, handle);
- ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
+ ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
if (ret == sizeof(cmd))
- ALOGI("queueActivate: sensor=%d, handle=%d, enable=%d",
+ ALOGV("queueActivate: sensor=%d, handle=%d, enable=%d",
cmd.sensorType, handle, enable);
else
- ALOGE("queueActivate: failed to send command: sensor=%d, handle=%d, enable=%d",
+ ALOGW("queueActivate: failed to send command: sensor=%d, handle=%d, enable=%d",
cmd.sensorType, handle, enable);
} else {
- ALOGI("queueActivate: unhandled handle=%d, enable=%d", handle, enable);
+ ALOGV("queueActivate: unhandled handle=%d, enable=%d", handle, enable);
}
}
@@ -1323,15 +1566,15 @@
initConfigCmd(&cmd, handle);
- ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
+ ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
if (ret == sizeof(cmd))
- ALOGI("queueSetDelay: sensor=%d, handle=%d, period=%" PRId64,
+ ALOGV("queueSetDelay: sensor=%d, handle=%d, period=%" PRId64,
cmd.sensorType, handle, sampling_period_ns);
else
- ALOGE("queueSetDelay: failed to send command: sensor=%d, handle=%d, period=%" PRId64,
+ ALOGW("queueSetDelay: failed to send command: sensor=%d, handle=%d, period=%" PRId64,
cmd.sensorType, handle, sampling_period_ns);
} else {
- ALOGI("queueSetDelay: unhandled handle=%d, period=%" PRId64, handle, sampling_period_ns);
+ ALOGV("queueSetDelay: unhandled handle=%d, period=%" PRId64, handle, sampling_period_ns);
}
}
@@ -1355,15 +1598,15 @@
initConfigCmd(&cmd, handle);
- ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
+ ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
if (ret == sizeof(cmd))
- ALOGI("queueBatch: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64,
+ ALOGV("queueBatch: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64,
cmd.sensorType, handle, sampling_period_ns, max_report_latency_ns);
else
- ALOGE("queueBatch: failed to send command: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64,
+ 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 {
- ALOGI("queueBatch: unhandled handle=%d, period=%" PRId64 ", latency=%" PRId64,
+ ALOGV("queueBatch: unhandled handle=%d, period=%" PRId64 ", latency=%" PRId64,
handle, sampling_period_ns, max_report_latency_ns);
}
}
@@ -1381,16 +1624,16 @@
initConfigCmd(&cmd, handle);
cmd.cmd = CONFIG_CMD_FLUSH;
- ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
+ ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
if (ret == sizeof(cmd)) {
- ALOGI("queueFlush: sensor=%d, handle=%d",
+ ALOGV("queueFlush: sensor=%d, handle=%d",
cmd.sensorType, handle);
} else {
- ALOGE("queueFlush: failed to send command: sensor=%d, handle=%d"
+ ALOGW("queueFlush: failed to send command: sensor=%d, handle=%d"
" with error %s", cmd.sensorType, handle, strerror(errno));
}
} else {
- ALOGI("queueFlush: unhandled handle=%d", handle);
+ ALOGV("queueFlush: unhandled handle=%d", handle);
}
}
@@ -1404,15 +1647,15 @@
memcpy(cmd->data, data, length);
cmd->cmd = CONFIG_CMD_CFG_DATA;
- ret = TEMP_FAILURE_RETRY(write(mFd, cmd, sizeof(*cmd) + length));
+ ret = TEMP_FAILURE_RETRY(::write(mFd, cmd, sizeof(*cmd) + length));
if (ret == sizeof(*cmd) + length)
- ALOGI("queueData: sensor=%d, length=%zu",
+ ALOGV("queueData: sensor=%d, length=%zu",
cmd->sensorType, length);
else
- ALOGE("queueData: failed to send command: sensor=%d, length=%zu",
+ ALOGW("queueData: failed to send command: sensor=%d, length=%zu",
cmd->sensorType, length);
} else {
- ALOGI("queueData: unhandled handle=%d", handle);
+ ALOGV("queueData: unhandled handle=%d", handle);
}
free(cmd);
}
@@ -1423,19 +1666,47 @@
queueDataInternal(handle, data, length);
}
+void HubConnection::setOperationParameter(const additional_info_event_t &info) {
+ switch (info.type) {
+ case AINFO_LOCAL_GEOMAGNETIC_FIELD: {
+ ALOGV("local geomag field update: strength %fuT, dec %fdeg, inc %fdeg",
+ static_cast<double>(info.data_float[0]),
+ info.data_float[1] * 180 / M_PI,
+ info.data_float[2] * 180 / M_PI);
+
+ struct {
+ AppToSensorHalDataPayload header;
+ MagLocalField magLocalField;
+ } packet = {
+ .header = {
+ .size = sizeof(MagLocalField),
+ .type = HALINTF_TYPE_MAG_LOCAL_FIELD },
+ .magLocalField = {
+ .strength = info.data_float[0],
+ .declination = info.data_float[1],
+ .inclination = info.data_float[2]}
+ };
+ queueDataInternal(COMMS_SENSOR_MAG, &packet, sizeof(packet));
+ break;
+ }
+ default:
+ break;
+ }
+}
+
void HubConnection::initNanohubLock() {
// Create the lock directory (if it doesn't already exist)
if (mkdir(NANOHUB_LOCK_DIR, NANOHUB_LOCK_DIR_PERMS) < 0 && errno != EEXIST) {
- ALOGE("Couldn't create Nanohub lock directory: %s", strerror(errno));
+ ALOGW("Couldn't create Nanohub lock directory: %s", strerror(errno));
return;
}
mInotifyPollIndex = -1;
int inotifyFd = inotify_init1(IN_NONBLOCK);
if (inotifyFd < 0) {
- ALOGE("Couldn't initialize inotify: %s", strerror(errno));
+ ALOGW("Couldn't initialize inotify: %s", strerror(errno));
} else if (inotify_add_watch(inotifyFd, NANOHUB_LOCK_DIR, IN_CREATE | IN_DELETE) < 0) {
- ALOGE("Couldn't add inotify watch: %s", strerror(errno));
+ ALOGW("Couldn't add inotify watch: %s", strerror(errno));
close(inotifyFd);
} else {
mPollFds[mNumPollFds].fd = inotifyFd;
@@ -1446,6 +1717,10 @@
}
}
+ssize_t HubConnection::write(const sensors_event_t *ev, size_t n) {
+ return mRing.write(ev, n);
+}
+
#ifdef USB_MAG_BIAS_REPORTING_ENABLED
void HubConnection::queueUsbMagBias()
{
@@ -1458,11 +1733,11 @@
cmd->msg.dataLen = sizeof(float);
memcpy((float *)(cmd+1), &mUsbMagBias, sizeof(float));
- ret = TEMP_FAILURE_RETRY(write(mFd, cmd, sizeof(*cmd) + sizeof(float)));
+ ret = TEMP_FAILURE_RETRY(::write(mFd, cmd, sizeof(*cmd) + sizeof(float)));
if (ret == sizeof(*cmd) + sizeof(float))
- ALOGI("queueUsbMagBias: bias=%f\n", mUsbMagBias);
+ ALOGV("queueUsbMagBias: bias=%f\n", mUsbMagBias);
else
- ALOGE("queueUsbMagBias: failed to send command: bias=%f\n", mUsbMagBias);
+ ALOGW("queueUsbMagBias: failed to send command: bias=%f\n", mUsbMagBias);
free(cmd);
}
}
@@ -1476,7 +1751,7 @@
// Open uinput dev node
mUinputFd = TEMP_FAILURE_RETRY(open("/dev/uinput", O_WRONLY | O_NONBLOCK));
if (mUinputFd < 0) {
- ALOGE("could not open uinput node: %s", strerror(errno));
+ ALOGW("could not open uinput node: %s", strerror(errno));
return UNKNOWN_ERROR;
}
@@ -1485,7 +1760,7 @@
ret |= TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_EVBIT, EV_SYN));
ret |= TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_SWBIT, SW_LID));
if (ret < 0) {
- ALOGE("could not send ioctl to uinput node: %s", strerror(errno));
+ ALOGW("could not send ioctl to uinput node: %s", strerror(errno));
return UNKNOWN_ERROR;
}
@@ -1498,15 +1773,15 @@
uidev.id.product = 0;
uidev.id.version = 0;
- ret = TEMP_FAILURE_RETRY(write(mUinputFd, &uidev, sizeof(uidev)));
+ ret = TEMP_FAILURE_RETRY(::write(mUinputFd, &uidev, sizeof(uidev)));
if (ret < 0) {
- ALOGE("write to uinput node failed: %s", strerror(errno));
+ ALOGW("write to uinput node failed: %s", strerror(errno));
return UNKNOWN_ERROR;
}
ret = TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_DEV_CREATE));
if (ret < 0) {
- ALOGE("could not send ioctl to uinput node: %s", strerror(errno));
+ ALOGW("could not send ioctl to uinput node: %s", strerror(errno));
return UNKNOWN_ERROR;
}
@@ -1522,9 +1797,9 @@
ev.type = EV_SW;
ev.code = SW_LID;
ev.value = data;
- ret = TEMP_FAILURE_RETRY(write(mUinputFd, &ev, sizeof(ev)));
+ ret = TEMP_FAILURE_RETRY(::write(mUinputFd, &ev, sizeof(ev)));
if (ret < 0) {
- ALOGE("write to uinput node failed: %s", strerror(errno));
+ ALOGW("write to uinput node failed: %s", strerror(errno));
return;
}
@@ -1532,18 +1807,232 @@
ev.type = EV_SYN;
ev.code = SYN_REPORT;
ev.value = 0;
- ret = TEMP_FAILURE_RETRY(write(mUinputFd, &ev, sizeof(ev)));
+ ret = TEMP_FAILURE_RETRY(::write(mUinputFd, &ev, sizeof(ev)));
if (ret < 0) {
- ALOGE("write to uinput node failed: %s", strerror(errno));
+ ALOGW("write to uinput node failed: %s", strerror(errno));
return;
}
// Set lid state property
if (property_set(LID_STATE_PROPERTY,
(data ? LID_STATE_CLOSED : LID_STATE_OPEN)) < 0) {
- ALOGE("could not set lid_state property");
+ ALOGW("could not set lid_state property");
}
}
#endif // LID_STATE_REPORTING_ENABLED
+#ifdef DIRECT_REPORT_ENABLED
+void HubConnection::sendDirectReportEvent(const sensors_event_t *nev, size_t n) {
+ // short circuit to avoid lock operation
+ if (n == 0) {
+ return;
+ }
+
+ // no intention to block sensor delivery thread. when lock is needed ignore
+ // the event (this only happens when the channel is reconfiured, so it's ok
+ if (mDirectChannelLock.tryLock() == NO_ERROR) {
+ while (n--) {
+ auto i = mSensorToChannel.find(nev->sensor);
+ if (i != mSensorToChannel.end()) {
+ for (auto &j : i->second) {
+ mDirectChannel[j.first]->write(nev);
+ }
+ }
+ ++nev;
+ }
+ mDirectChannelLock.unlock();
+ }
+}
+
+void HubConnection::mergeDirectReportRequest(struct ConfigCmd *cmd, int handle) {
+ 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 = (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;
+ }
+ }
+
+ if (enable) {
+ cmd->rate = (rate > cmd->rate || cmd->cmd == CONFIG_CMD_DISABLE) ? rate : cmd->rate;
+ cmd->latency = 0;
+ cmd->cmd = CONFIG_CMD_ENABLE;
+ }
+ }
+}
+
+int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *mem) {
+ std::unique_ptr<DirectChannelBase> ch;
+ int ret = NO_MEMORY;
+
+ switch(mem->type) {
+ case SENSOR_DIRECT_MEM_TYPE_ASHMEM:
+ ch = std::make_unique<AshmemDirectChannel>(mem);
+ break;
+ case SENSOR_DIRECT_MEM_TYPE_GRALLOC:
+ ch = std::make_unique<GrallocDirectChannel>(mem);
+ break;
+ default:
+ ret = INVALID_OPERATION;
+ }
+
+ if (ch) {
+ if (ch->isValid()) {
+ Mutex::Autolock autoLock(mDirectChannelLock);
+ ret = mDirectChannelHandle++;
+ mDirectChannel.insert(std::make_pair(ret, std::move(ch)));
+ } else {
+ ret = ch->getError();
+ ALOGW("Direct channel object(type:%d) has error %d upon init", mem->type, ret);
+ }
+ }
+
+ return ret;
+}
+
+int HubConnection::removeDirectChannel(int channel_handle) {
+ // make sure no active sensor in this channel
+ std::vector<int32_t> activeSensorList;
+ stopAllDirectReportOnChannel(channel_handle, &activeSensorList);
+
+ // sensor service is responsible for stop all sensors before remove direct
+ // channel. Thus, this is an error.
+ if (!activeSensorList.empty()) {
+ std::stringstream ss;
+ std::copy(activeSensorList.begin(), activeSensorList.end(),
+ std::ostream_iterator<int32_t>(ss, ","));
+ ALOGW("Removing channel %d when sensors (%s) are not stopped.",
+ channel_handle, ss.str().c_str());
+ }
+
+ // remove the channel record
+ Mutex::Autolock autoLock(mDirectChannelLock);
+ mDirectChannel.erase(channel_handle);
+ return NO_ERROR;
+}
+
+int HubConnection::stopAllDirectReportOnChannel(
+ int channel_handle, std::vector<int32_t> *activeSensorList) {
+ Mutex::Autolock autoLock(mDirectChannelLock);
+ if (mDirectChannel.find(channel_handle) == mDirectChannel.end()) {
+ return BAD_VALUE;
+ }
+
+ std::vector<int32_t> sensorToStop;
+ for (auto &it : mSensorToChannel) {
+ auto j = it.second.find(channel_handle);
+ if (j != it.second.end()) {
+ it.second.erase(j);
+ if (it.second.empty()) {
+ sensorToStop.push_back(it.first);
+ }
+ }
+ }
+
+ if (activeSensorList != nullptr) {
+ *activeSensorList = sensorToStop;
+ }
+
+ // re-evaluate and send config for all sensor that need to be stopped
+ bool ret = true;
+ for (auto sensor_handle : sensorToStop) {
+ Mutex::Autolock autoLock2(mLock);
+ struct ConfigCmd cmd;
+ initConfigCmd(&cmd, sensor_handle);
+
+ int result = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
+ ret = ret && (result == sizeof(cmd));
+ }
+ return ret ? NO_ERROR : BAD_VALUE;
+}
+
+int HubConnection::configDirectReport(int sensor_handle, int channel_handle, int rate_level) {
+ if (sensor_handle == -1 && rate_level == SENSOR_DIRECT_RATE_STOP) {
+ return stopAllDirectReportOnChannel(channel_handle, nullptr);
+ }
+
+ if (!isValidHandle(sensor_handle)) {
+ return BAD_VALUE;
+ }
+
+ // clamp to fast
+ if (rate_level > SENSOR_DIRECT_RATE_FAST) {
+ rate_level = SENSOR_DIRECT_RATE_FAST;
+ }
+
+ // manage direct channel data structure
+ Mutex::Autolock autoLock(mDirectChannelLock);
+ auto i = mDirectChannel.find(channel_handle);
+ if (i == mDirectChannel.end()) {
+ return BAD_VALUE;
+ }
+
+ auto j = mSensorToChannel.find(sensor_handle);
+ if (j == mSensorToChannel.end()) {
+ return BAD_VALUE;
+ }
+
+ j->second.erase(channel_handle);
+ if (rate_level != SENSOR_DIRECT_RATE_STOP) {
+ j->second.insert(std::make_pair(channel_handle, rate_level));
+ }
+
+ Mutex::Autolock autoLock2(mLock);
+ struct ConfigCmd cmd;
+ initConfigCmd(&cmd, sensor_handle);
+
+ int ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
+
+ if (rate_level == SENSOR_DIRECT_RATE_STOP) {
+ ret = NO_ERROR;
+ } else {
+ ret = (ret == sizeof(cmd)) ? sensor_handle : BAD_VALUE;
+ }
+ return ret;
+}
+
+bool HubConnection::isDirectReportSupported() const {
+ return true;
+}
+#else // DIRECT_REPORT_ENABLED
+// nop functions if feature is turned off
+int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *) {
+ return INVALID_OPERATION;
+}
+
+int HubConnection::removeDirectChannel(int) {
+ return INVALID_OPERATION;
+}
+
+int HubConnection::configDirectReport(int, int, int) {
+ return INVALID_OPERATION;
+}
+
+void HubConnection::sendDirectReportEvent(const sensors_event_t *, size_t) {
+}
+
+void HubConnection::mergeDirectReportRequest(struct ConfigCmd *, int) {
+}
+
+bool HubConnection::isDirectReportSupported() const {
+ return false;
+}
+#endif // DIRECT_REPORT_ENABLED
+
} // namespace android
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
index a891612..ce35877 100644
--- a/sensorhal/hubconnection.h
+++ b/sensorhal/hubconnection.h
@@ -28,12 +28,26 @@
#include <utils/Thread.h>
#include "activityeventhandler.h"
+#include "directchannel.h"
#include "eventnums.h"
+#include "halIntf.h"
#include "hubdefs.h"
#include "ring.h"
+#ifdef USE_SENSORSERVICE_TO_GET_FIFO
+#include <thread>
+#endif
+#include <unordered_map>
+
#define WAKELOCK_NAME "sensorHal"
+#define ACCEL_BIAS_TAG "accel"
+#define ACCEL_SW_BIAS_TAG "accel_sw"
+#define GYRO_BIAS_TAG "gyro"
+#define GYRO_OTC_DATA_TAG "gyro_otc"
+#define GYRO_SW_BIAS_TAG "gyro_sw"
+#define MAG_BIAS_TAG "mag"
+
namespace android {
struct HubConnection : public Thread {
@@ -59,17 +73,26 @@
void queueFlush(int handle);
void queueData(int handle, void *data, size_t length);
+ 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);
+ ssize_t write(const sensors_event_t *ev, size_t n);
void setActivityCallback(ActivityEventHandler *eventHandler);
void saveSensorSettings() const;
+ void setRawScale(float scaleAccel, float scaleMag) {
+ mScaleAccel = scaleAccel;
+ mScaleMag = scaleMag;
+ }
+
protected:
HubConnection();
virtual ~HubConnection();
@@ -95,6 +118,10 @@
return (nsecs_t)0;
}
+ static inline uint64_t frequency_to_frequency_q10(float frequency) {
+ return period_ns_to_frequency_q10(static_cast<nsecs_t>(1e9f/frequency));
+ }
+
enum
{
CONFIG_CMD_DISABLE = 0,
@@ -200,6 +227,9 @@
uint8_t mMagAccuracyRestore;
float mGyroBias[3], mAccelBias[3];
+ GyroOtcData mGyroOtcData;
+
+ float mScaleAccel, mScaleMag;
SensorState mSensorState[NUM_COMMS_SENSORS_PLUS_1];
@@ -212,11 +242,12 @@
int mNumPollFds;
sensors_event_t *initEv(sensors_event_t *ev, uint64_t timestamp, uint32_t type, uint32_t sensor);
- void magAccuracyUpdate(float x, float y, float z);
+ uint8_t magAccuracyUpdate(sensors_vec_t *sv);
void processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct OneAxisSample *sample, bool highAccuracy);
void processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct RawThreeAxisSample *sample, bool highAccuracy);
void processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct ThreeAxisSample *sample, bool highAccuracy);
void postOsLog(uint8_t *buf, ssize_t len);
+ void processAppData(uint8_t *buf, ssize_t len);
ssize_t processBuf(uint8_t *buf, size_t len);
inline bool isValidHandle(int handle) {
@@ -237,8 +268,11 @@
void restoreSensorState();
void sendCalibrationOffsets();
+#ifdef USE_SENSORSERVICE_TO_GET_FIFO
// Enable SCHED_FIFO priority for main thread
- void enableSchedFifoMode();
+ std::thread mEnableSchedFifoThread;
+#endif
+ static void enableSchedFifoMode(sp<HubConnection> hub);
#ifdef LID_STATE_REPORTING_ENABLED
int mUinputFd;
@@ -258,6 +292,26 @@
int mDoubleTouchPollIndex;
#endif // DOUBLE_TOUCH_ENABLED
+ // Direct report functions
+public:
+ int addDirectChannel(const struct sensors_direct_mem_t *mem);
+ int removeDirectChannel(int channel_handle);
+ int configDirectReport(int sensor_handle, int channel_handle, int rate_level);
+ bool isDirectReportSupported() const;
+private:
+ void sendDirectReportEvent(const sensors_event_t *nev, size_t n);
+ void mergeDirectReportRequest(struct ConfigCmd *cmd, int handle);
+#ifdef DIRECT_REPORT_ENABLED
+ int stopAllDirectReportOnChannel(
+ int channel_handle, std::vector<int32_t> *unstoppedSensors);
+ Mutex mDirectChannelLock;
+ //sensor_handle=>(channel_handle, rate_level)
+ std::unordered_map<int32_t, std::unordered_map<int32_t, int32_t> > mSensorToChannel;
+ //channel_handle=>ptr of Channel obj
+ std::unordered_map<int32_t, std::unique_ptr<DirectChannelBase>> mDirectChannel;
+ int32_t mDirectChannelHandle;
+#endif
+
DISALLOW_EVIL_CONSTRUCTORS(HubConnection);
};
diff --git a/sensorhal/hubdefs.h b/sensorhal/hubdefs.h
index ba1f6e9..99c8e26 100644
--- a/sensorhal/hubdefs.h
+++ b/sensorhal/hubdefs.h
@@ -24,7 +24,7 @@
namespace android {
#define CONTEXTHUB_SETTINGS_PATH "/persist/sensorcal.json"
-#define CONTEXTHUB_SAVED_SETTINGS_PATH "/data/misc/sensorcal_saved.json"
+#define CONTEXTHUB_SAVED_SETTINGS_PATH "/data/vendor/sensor/sensorcal_saved.json"
#define MAG_BIAS_FILE_PATH "/sys/class/power_supply/battery/compass_compensation"
static const uint32_t kMinClockRateHz = 960000;
diff --git a/sensorhal/sensorlist.h b/sensorhal/sensorlist.h
index b26110b..4d09133 100644
--- a/sensorhal/sensorlist.h
+++ b/sensorhal/sensorlist.h
@@ -20,6 +20,8 @@
#include <hardware/sensors.h>
+extern const float kScaleAccel;
+extern const float kScaleMag;
// A list of sensors provided by a device.
extern const sensor_t kSensorList[];
extern const size_t kSensorCount;
diff --git a/sensorhal/sensors.cpp b/sensorhal/sensors.cpp
index 77ab3ba..65b881c 100644
--- a/sensorhal/sensors.cpp
+++ b/sensorhal/sensors.cpp
@@ -15,28 +15,37 @@
*/
#define LOG_TAG "sensors"
-// #defined LOG_NDEBUG 1
+#define LOG_NDEBUG 1
#include <utils/Log.h>
#include "hubconnection.h"
#include "sensorlist.h"
#include "sensors.h"
+#include <cutils/ashmem.h>
#include <errno.h>
#include <math.h>
#include <media/stagefright/foundation/ADebug.h>
#include <string.h>
+#include <sys/mman.h>
+#include <stdlib.h>
+
+#ifdef DYNAMIC_SENSOR_EXT_ENABLED
+#include <DynamicSensorManager.h>
+#include <SensorEventCallback.h>
+#endif
using namespace android;
////////////////////////////////////////////////////////////////////////////////
SensorContext::SensorContext(const struct hw_module_t *module)
- : mHubConnection(HubConnection::getInstance()) {
+ : mSensorList(kSensorList, kSensorList + kSensorCount),
+ mHubConnection(HubConnection::getInstance()) {
memset(&device, 0, sizeof(device));
device.common.tag = HARDWARE_DEVICE_TAG;
- device.common.version = SENSORS_DEVICE_API_VERSION_1_3;
+ device.common.version = SENSORS_DEVICE_API_VERSION_1_4;
device.common.module = const_cast<hw_module_t *>(module);
device.common.close = CloseWrapper;
device.activate = ActivateWrapper;
@@ -44,13 +53,20 @@
device.poll = PollWrapper;
device.batch = BatchWrapper;
device.flush = FlushWrapper;
+ device.inject_sensor_data = InjectSensorDataWrapper;
+ mHubConnection->setRawScale(kScaleAccel, kScaleMag);
+ if (mHubConnection->isDirectReportSupported()) {
+ device.register_direct_channel = RegisterDirectChannelWrapper;
+ device.config_direct_report = ConfigDirectReportWrapper;
+ }
- mHubAlive = (mHubConnection->initCheck() == OK
- && mHubConnection->getAliveCheck() == OK);
+ mOperationHandler.emplace_back(new HubConnectionOperation(mHubConnection));
+
+ initializeHalExtension();
}
int SensorContext::close() {
- ALOGI("close");
+ ALOGV("close");
delete this;
@@ -58,38 +74,25 @@
}
int SensorContext::activate(int handle, int enabled) {
- ALOGI("activate");
+ ALOGV("activate");
- mHubConnection->queueActivate(handle, enabled);
-
- return 0;
+ for (auto &h : mOperationHandler) {
+ if (h->owns(handle)) {
+ return h->activate(handle, enabled);
+ }
+ }
+ return INVALID_OPERATION;
}
int SensorContext::setDelay(int handle, int64_t delayNs) {
- ALOGI("setDelay");
+ ALOGV("setDelay");
- // clamp sample rate based on minDelay and maxDelay defined in kSensorList
- int64_t delayNsClamped = delayNs;
- for (size_t i = 0; i < kSensorCount; i++) {
- sensor_t sensor = kSensorList[i];
- if (sensor.handle != handle) {
- continue;
+ for (auto &h: mOperationHandler) {
+ if (h->owns(handle)) {
+ return h->setDelay(handle, delayNs);
}
-
- if ((sensor.flags & REPORTING_MODE_MASK) == SENSOR_FLAG_CONTINUOUS_MODE) {
- if ((delayNs/1000) < sensor.minDelay) {
- delayNsClamped = sensor.minDelay * 1000;
- } else if ((delayNs/1000) > sensor.maxDelay) {
- delayNsClamped = sensor.maxDelay * 1000;
- }
- }
-
- break;
}
-
- mHubConnection->queueSetDelay(handle, delayNsClamped);
-
- return 0;
+ return INVALID_OPERATION;
}
int SensorContext::poll(sensors_event_t *data, int count) {
@@ -126,37 +129,43 @@
int handle,
int64_t sampling_period_ns,
int64_t max_report_latency_ns) {
- ALOGI("batch");
+ ALOGV("batch");
- // clamp sample rate based on minDelay and maxDelay defined in kSensorList
- int64_t sampling_period_ns_clamped = sampling_period_ns;
- for (size_t i = 0; i < kSensorCount; i++) {
- sensor_t sensor = kSensorList[i];
- if (sensor.handle != handle) {
- continue;
+ for (auto &h : mOperationHandler) {
+ if (h->owns(handle)) {
+ return h->batch(handle, sampling_period_ns, max_report_latency_ns);
}
-
- if ((sensor.flags & REPORTING_MODE_MASK) == SENSOR_FLAG_CONTINUOUS_MODE) {
- if ((sampling_period_ns/1000) < sensor.minDelay) {
- sampling_period_ns_clamped = sensor.minDelay * 1000;
- } else if ((sampling_period_ns/1000) > sensor.maxDelay) {
- sampling_period_ns_clamped = sensor.maxDelay * 1000;
- }
- }
-
- break;
}
-
- mHubConnection->queueBatch(handle, sampling_period_ns_clamped,
- max_report_latency_ns);
- return 0;
+ return INVALID_OPERATION;
}
int SensorContext::flush(int handle) {
- ALOGI("flush");
+ ALOGV("flush");
- mHubConnection->queueFlush(handle);
- return 0;
+ for (auto &h : mOperationHandler) {
+ if (h->owns(handle)) {
+ return h->flush(handle);
+ }
+ }
+ return INVALID_OPERATION;
+}
+
+int SensorContext::register_direct_channel(
+ const struct sensors_direct_mem_t *mem, int32_t channel_handle) {
+ if (mem) {
+ //add
+ return mHubConnection->addDirectChannel(mem);
+ } else {
+ //remove
+ mHubConnection->removeDirectChannel(channel_handle);
+ return NO_ERROR;
+ }
+}
+
+int SensorContext::config_direct_report(
+ int32_t sensor_handle, int32_t channel_handle, const struct sensors_direct_cfg_t * config) {
+ int rate_level = config->rate_level;
+ return mHubConnection->configDirectReport(sensor_handle, channel_handle, rate_level);
}
// static
@@ -199,22 +208,202 @@
return reinterpret_cast<SensorContext *>(dev)->flush(handle);
}
+// static
+int SensorContext::RegisterDirectChannelWrapper(struct sensors_poll_device_1 *dev,
+ const struct sensors_direct_mem_t* mem, int channel_handle) {
+ return reinterpret_cast<SensorContext *>(dev)->register_direct_channel(
+ mem, channel_handle);
+}
+
+// static
+int SensorContext::ConfigDirectReportWrapper(struct sensors_poll_device_1 *dev,
+ int sensor_handle, int channel_handle, const sensors_direct_cfg_t * config) {
+ return reinterpret_cast<SensorContext *>(dev)->config_direct_report(
+ sensor_handle, channel_handle, config);
+}
+
+int SensorContext::inject_sensor_data(const sensors_event_t *event) {
+ ALOGV("inject_sensor_data");
+
+ // only support set operation parameter, which will have handle == 0
+ if (event == nullptr || event->type != SENSOR_TYPE_ADDITIONAL_INFO) {
+ return -EINVAL;
+ }
+
+ if (event->sensor != SENSORS_HANDLE_BASE - 1) {
+ return -ENOSYS;
+ }
+
+ if (event->additional_info.type == AINFO_BEGIN
+ || event->additional_info.type == AINFO_END) {
+ return 0;
+ }
+
+ mHubConnection->setOperationParameter(event->additional_info);
+ return 0;
+}
+
+// static
+int SensorContext::InjectSensorDataWrapper(struct sensors_poll_device_1 *dev,
+ const struct sensors_event_t *event) {
+ return reinterpret_cast<SensorContext *>(dev)->inject_sensor_data(event);
+}
+
bool SensorContext::getHubAlive() {
- return mHubAlive;
+ return (mHubConnection->initCheck() == OK && mHubConnection->getAliveCheck() == OK);
+}
+
+size_t SensorContext::getSensorList(sensor_t const **list) {
+ ALOGE("sensor p = %p, n = %zu", mSensorList.data(), mSensorList.size());
+ *list = mSensorList.data();
+ return mSensorList.size();
+}
+
+// HubConnectionOperation functions
+SensorContext::HubConnectionOperation::HubConnectionOperation(sp<HubConnection> hubConnection)
+ : mHubConnection(hubConnection) {
+ for (size_t i = 0; i < kSensorCount; i++) {
+ mHandles.emplace(kSensorList[i].handle);
+ }
+}
+
+bool SensorContext::HubConnectionOperation::owns(int handle) {
+ return mHandles.find(handle) != mHandles.end();
+}
+
+int SensorContext::HubConnectionOperation::activate(int handle, int enabled) {
+ mHubConnection->queueActivate(handle, enabled);
+ return 0;
+}
+
+int SensorContext::HubConnectionOperation::setDelay(int handle, int64_t delayNs) {
+ // clamp sample rate based on minDelay and maxDelay defined in kSensorList
+ int64_t delayNsClamped = delayNs;
+ for (size_t i = 0; i < kSensorCount; i++) {
+ sensor_t sensor = kSensorList[i];
+ if (sensor.handle != handle) {
+ continue;
+ }
+
+ if ((sensor.flags & REPORTING_MODE_MASK) == SENSOR_FLAG_CONTINUOUS_MODE) {
+ if ((delayNs/1000) < sensor.minDelay) {
+ delayNsClamped = sensor.minDelay * 1000;
+ } else if ((delayNs/1000) > sensor.maxDelay) {
+ delayNsClamped = sensor.maxDelay * 1000;
+ }
+ }
+
+ break;
+ }
+
+ mHubConnection->queueSetDelay(handle, delayNsClamped);
+ return 0;
+}
+
+int SensorContext::HubConnectionOperation::batch(
+ int handle, int64_t sampling_period_ns,
+ int64_t max_report_latency_ns) {
+ // clamp sample rate based on minDelay and maxDelay defined in kSensorList
+ int64_t sampling_period_ns_clamped = sampling_period_ns;
+ for (size_t i = 0; i < kSensorCount; i++) {
+ sensor_t sensor = kSensorList[i];
+ if (sensor.handle != handle) {
+ continue;
+ }
+
+ if ((sensor.flags & REPORTING_MODE_MASK) == SENSOR_FLAG_CONTINUOUS_MODE) {
+ if ((sampling_period_ns/1000) < sensor.minDelay) {
+ sampling_period_ns_clamped = sensor.minDelay * 1000;
+ } else if ((sampling_period_ns/1000) > sensor.maxDelay) {
+ sampling_period_ns_clamped = sensor.maxDelay * 1000;
+ }
+ }
+
+ break;
+ }
+
+ mHubConnection->queueBatch(handle, sampling_period_ns_clamped,
+ max_report_latency_ns);
+ return 0;
+}
+
+int SensorContext::HubConnectionOperation::flush(int handle) {
+ mHubConnection->queueFlush(handle);
+ return 0;
+}
+
+#ifdef DYNAMIC_SENSOR_EXT_ENABLED
+namespace {
+// adaptor class
+class Callback : public SensorEventCallback {
+public:
+ Callback(sp<HubConnection> hubConnection) : mHubConnection(hubConnection) {}
+ virtual int submitEvent(sp<BaseSensorObject> source, const sensors_event_t &e) override;
+private:
+ sp<HubConnection> mHubConnection;
+};
+
+int Callback::submitEvent(sp<BaseSensorObject> source, const sensors_event_t &e) {
+ (void) source; // irrelavent in this context
+ return (mHubConnection->write(&e, 1) == 1) ? 0 : -ENOSPC;
+}
+} // anonymous namespace
+
+SensorContext::DynamicSensorManagerOperation::DynamicSensorManagerOperation(DynamicSensorManager* manager)
+ : mDynamicSensorManager(manager) {
+}
+
+bool SensorContext::DynamicSensorManagerOperation::owns(int handle) {
+ return mDynamicSensorManager->owns(handle);
+}
+
+int SensorContext::DynamicSensorManagerOperation::activate(int handle, int enabled) {
+ return mDynamicSensorManager->activate(handle, enabled);
+}
+
+int SensorContext::DynamicSensorManagerOperation::setDelay(int handle, int64_t delayNs) {
+ return mDynamicSensorManager->setDelay(handle, delayNs);
+}
+
+int SensorContext::DynamicSensorManagerOperation::batch(int handle, int64_t sampling_period_ns,
+ int64_t max_report_latency_ns) {
+ return mDynamicSensorManager->batch(handle, sampling_period_ns, max_report_latency_ns);
+}
+
+int SensorContext::DynamicSensorManagerOperation::flush(int handle) {
+ return mDynamicSensorManager->flush(handle);
+}
+#endif
+
+void SensorContext::initializeHalExtension() {
+#ifdef DYNAMIC_SENSOR_EXT_ENABLED
+ // initialize callback and dynamic sensor manager
+ mEventCallback.reset(new Callback(mHubConnection));
+ DynamicSensorManager* manager = DynamicSensorManager::createInstance(
+ kDynamicHandleBase, kMaxDynamicHandleCount, mEventCallback.get());
+
+ // add meta sensor to list
+ mSensorList.push_back(manager->getDynamicMetaSensor());
+
+ // register operation
+ mOperationHandler.emplace_back(new DynamicSensorManagerOperation(manager));
+#endif
}
////////////////////////////////////////////////////////////////////////////////
static bool gHubAlive;
+static sensor_t const *sensor_list;
+static int n_sensor;
static int open_sensors(
const struct hw_module_t *module,
const char *,
struct hw_device_t **dev) {
- ALOGI("open_sensors");
+ ALOGV("open_sensors");
SensorContext *ctx = new SensorContext(module);
-
+ n_sensor = ctx->getSensorList(&sensor_list);
gHubAlive = ctx->getHubAlive();
*dev = &ctx->device.common;
@@ -228,11 +417,10 @@
static int get_sensors_list(
struct sensors_module_t *,
struct sensor_t const **list) {
- ALOGI("get_sensors_list");
-
- if (gHubAlive) {
- *list = kSensorList;
- return kSensorCount;
+ ALOGV("get_sensors_list");
+ if (gHubAlive && sensor_list != nullptr) {
+ *list = sensor_list;
+ return n_sensor;
} else {
*list = {};
return 0;
@@ -240,8 +428,13 @@
}
static int set_operation_mode(unsigned int mode) {
- ALOGI("set_operation_mode");
- return (mode) ? -EINVAL : 0;
+ ALOGV("set_operation_mode");
+
+ // This is no-op because there is no sensor in the hal that system can
+ // inject events. Only operation parameter injection is implemented, which
+ // works in both data injection and normal mode.
+ (void) mode;
+ return 0;
}
struct sensors_module_t HAL_MODULE_INFO_SYM = {
diff --git a/sensorhal/sensors.h b/sensorhal/sensors.h
index 69ba2d6..a30a730 100644
--- a/sensorhal/sensors.h
+++ b/sensorhal/sensors.h
@@ -21,8 +21,30 @@
#include <hardware/hardware.h>
#include <hardware/sensors.h>
#include <media/stagefright/foundation/ABase.h>
+#include <utils/RefBase.h>
-#include "hubconnection.h"
+#include <memory>
+#include <unordered_set>
+#include <vector>
+
+using android::sp;
+
+namespace android {
+ struct HubConnection;
+} // namespace android
+using android::HubConnection;
+
+namespace android {
+ namespace SensorHalExt {
+ class BaseSensorObject;
+ class DynamicSensorManager;
+ class SensorEventCallback;
+ } // namespace BaseSensorObject
+} // namespace android
+
+using android::SensorHalExt::BaseSensorObject;
+using android::SensorHalExt::DynamicSensorManager;
+using android::SensorHalExt::SensorEventCallback;
struct SensorContext {
struct sensors_poll_device_1 device;
@@ -31,9 +53,9 @@
bool getHubAlive();
+ size_t getSensorList(sensor_t const **list);
+
private:
- android::sp<android::HubConnection> mHubConnection;
- bool mHubAlive;
int close();
int activate(int handle, int enabled);
@@ -45,6 +67,17 @@
int flush(int handle);
+ int register_direct_channel(
+ const struct sensors_direct_mem_t* mem, int channel_handle);
+
+ int config_direct_report(
+ int sensor_handle, int channel_handle, const struct sensors_direct_cfg_t * config);
+
+ int inject_sensor_data(const struct sensors_event_t *event);
+
+ void initializeHalExtension();
+
+ // static wrappers
static int CloseWrapper(struct hw_device_t *dev);
static int ActivateWrapper(
@@ -65,6 +98,68 @@
static int FlushWrapper(struct sensors_poll_device_1 *dev, int handle);
+ static int RegisterDirectChannelWrapper(struct sensors_poll_device_1 *dev,
+ const struct sensors_direct_mem_t* mem, int channel_handle);
+ static int ConfigDirectReportWrapper(struct sensors_poll_device_1 *dev,
+ int sensor_handle, int channel_handle, const struct sensors_direct_cfg_t * config);
+ static int InjectSensorDataWrapper(struct sensors_poll_device_1 *dev, const sensors_event_t *event);
+
+ class SensorOperation {
+ public:
+ virtual bool owns(int handle) = 0;
+ virtual int activate(int handle, int enabled) = 0;
+ virtual int setDelay(int handle, int64_t delayNs) = 0;
+ virtual int batch(
+ int handle, int64_t sampling_period_ns,
+ int64_t max_report_latency_ns) = 0;
+ virtual int flush(int handle) = 0;
+ virtual ~SensorOperation() {}
+ };
+
+ class HubConnectionOperation : public SensorOperation {
+ public:
+ HubConnectionOperation(sp<HubConnection> hubConnection);
+ virtual bool owns(int handle) override;
+ virtual int activate(int handle, int enabled) override;
+ virtual int setDelay(int handle, int64_t delayNs) override;
+ virtual int batch(
+ int handle, int64_t sampling_period_ns,
+ int64_t max_report_latency_ns) override;
+ virtual int flush(int handle) override;
+ virtual ~HubConnectionOperation() {}
+ private:
+ sp<HubConnection> mHubConnection;
+ std::unordered_set<int> mHandles;
+ };
+
+ std::vector<sensor_t> mSensorList;
+
+ sp<HubConnection> mHubConnection;
+ std::vector<std::unique_ptr<SensorOperation> > mOperationHandler;
+
+#ifdef DYNAMIC_SENSOR_EXT_ENABLED
+private:
+ class DynamicSensorManagerOperation : public SensorOperation {
+ public:
+ DynamicSensorManagerOperation(DynamicSensorManager* manager);
+ virtual bool owns(int handle) override;
+ virtual int activate(int handle, int enabled) override;
+ virtual int setDelay(int handle, int64_t delayNs) override;
+ virtual int batch(
+ int handle, int64_t sampling_period_ns,
+ int64_t max_report_latency_ns) override;
+ virtual int flush(int handle) override;
+ virtual ~DynamicSensorManagerOperation() {}
+ private:
+ std::unique_ptr<DynamicSensorManager> mDynamicSensorManager;
+ };
+
+ static constexpr int32_t kDynamicHandleBase = 0x10000;
+ static constexpr int32_t kMaxDynamicHandleCount = 0xF0000; // ~1M handles, enough before reboot
+
+ std::unique_ptr<SensorEventCallback> mEventCallback;
+#endif //DYNAMIC_SENSOR_EXT_ENABLED
+
DISALLOW_EVIL_CONSTRUCTORS(SensorContext);
};
diff --git a/util/common/ring.cpp b/util/common/ring.cpp
index 1d23b82..26d44d6 100644
--- a/util/common/ring.cpp
+++ b/util/common/ring.cpp
@@ -104,5 +104,30 @@
return size;
}
+LockfreeBuffer::LockfreeBuffer(void* buf, size_t size)
+ : mData((sensors_event_t *)buf), mSize(size/sizeof(sensors_event_t)),
+ mWritePos(0), mCounter(1) {
+ memset(mData, 0, size);
+}
+
+LockfreeBuffer::~LockfreeBuffer() {
+ memset(mData, 0, mSize*sizeof(sensors_event_t));
+}
+
+void LockfreeBuffer::write(const sensors_event_t *ev, size_t size) {
+ if (!mSize) {
+ return;
+ }
+
+ while(size--) {
+ mData[mWritePos] = *(ev++);
+ mData[mWritePos].reserved0 = mCounter++;
+
+ if (++mWritePos >= mSize) {
+ mWritePos = 0;
+ }
+ }
+}
+
} // namespace android
diff --git a/util/common/ring.h b/util/common/ring.h
index c77c3de..31bf848 100644
--- a/util/common/ring.h
+++ b/util/common/ring.h
@@ -43,6 +43,21 @@
DISALLOW_EVIL_CONSTRUCTORS(RingBuffer);
};
+struct LockfreeBuffer {
+ LockfreeBuffer(void* buf, size_t size);
+ ~LockfreeBuffer();
+
+ // support single writer
+ void write(const sensors_event_t *ev, size_t size);
+private:
+ sensors_event_t *mData;
+ size_t mSize;
+ size_t mWritePos;
+ int32_t mCounter;
+
+ DISALLOW_EVIL_CONSTRUCTORS(LockfreeBuffer);
+};
+
} // namespace android
#endif // RING_BUFFER_H_
diff --git a/util/nanoapp_cmd/Android.mk b/util/nanoapp_cmd/Android.mk
index 80af754..35320eb 100644
--- a/util/nanoapp_cmd/Android.mk
+++ b/util/nanoapp_cmd/Android.mk
@@ -19,13 +19,17 @@
LOCAL_SRC_FILES:= nanoapp_cmd.c
-LOCAL_C_INCLUDES := $(LOCAL_PATH)/../../firmware/os/inc
+LOCAL_C_INCLUDES += \
+ $(LOCAL_PATH)/../../firmware/os/inc \
+ $(LOCAL_PATH)/../../lib/include
+
LOCAL_CFLAGS := -Wall -Werror -Wextra
LOCAL_MODULE:= nanoapp_cmd
LOCAL_MODULE_TAGS:= optional
LOCAL_MODULE_OWNER := google
+LOCAL_PROPRIETARY_MODULE := true
LOCAL_LDLIBS := -llog
diff --git a/util/nanoapp_cmd/nanoapp_cmd.c b/util/nanoapp_cmd/nanoapp_cmd.c
index e0334ef..328ba0c 100644
--- a/util/nanoapp_cmd/nanoapp_cmd.c
+++ b/util/nanoapp_cmd/nanoapp_cmd.c
@@ -32,14 +32,20 @@
#include <android/log.h>
+#include <nanohub/nanohub.h>
#include <eventnums.h>
#include <sensType.h>
#define SENSOR_RATE_ONCHANGE 0xFFFFFF01UL
#define SENSOR_RATE_ONESHOT 0xFFFFFF02UL
#define SENSOR_HZ(_hz) ((uint32_t)((_hz) * 1024.0f))
+#define MAX_APP_NAME_LEN 32
#define MAX_INSTALL_CNT 8
+#define MAX_UNINSTALL_CNT 8
#define MAX_DOWNLOAD_RETRIES 4
+#define UNINSTALL_CMD "uninstall"
+
+#define NANOHUB_EXT_APP_DELETE 2
#define LOGE(fmt, ...) do { \
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, fmt, ##__VA_ARGS__); \
@@ -66,7 +72,7 @@
uint8_t data[];
} __attribute__((packed));
-struct AppInfo
+struct App
{
uint32_t num;
uint64_t id;
@@ -170,9 +176,10 @@
bool stop = false;
char *buf;
int nread, buf_size = 2048;
-struct AppInfo apps[32];
+struct App apps[32];
uint8_t appCount;
-char appsToInstall[MAX_INSTALL_CNT][32];
+char appsToInstall[MAX_INSTALL_CNT][MAX_APP_NAME_LEN+1];
+uint64_t appsToUninstall[MAX_UNINSTALL_CNT];
void sig_handle(__attribute__((unused)) int sig)
{
@@ -204,7 +211,7 @@
return;
while ((numRead = getline(&line, &len, fp)) != -1) {
- struct AppInfo *currApp = &apps[appCount++];
+ struct App *currApp = &apps[appCount++];
sscanf(line, "app: %d id: %" PRIx64 " ver: %" PRIx32 " size: %" PRIx32 "\n", &currApp->num, &currApp->id, &currApp->version, &currApp->size);
}
@@ -214,7 +221,7 @@
free(line);
}
-struct AppInfo *findApp(uint64_t appId)
+struct App *findApp(uint64_t appId)
{
uint8_t i;
@@ -227,13 +234,12 @@
return NULL;
}
-int parseConfigAppInfo()
+int parseConfigAppInfo(int *installCnt, int *uninstallCnt)
{
FILE *fp;
char *line = NULL;
size_t len;
ssize_t numRead;
- int installCnt;
fp = openFile("/vendor/firmware/napp_list.cfg", "r");
if (!fp)
@@ -241,17 +247,22 @@
parseInstalledAppInfo();
- installCnt = 0;
- while (((numRead = getline(&line, &len, fp)) != -1) && (installCnt < MAX_INSTALL_CNT)) {
+ *installCnt = *uninstallCnt = 0;
+ while (((numRead = getline(&line, &len, fp)) != -1) && (*installCnt < MAX_INSTALL_CNT) && (*uninstallCnt < MAX_UNINSTALL_CNT)) {
uint64_t appId;
uint32_t appVersion;
- struct AppInfo* installedApp;
+ struct App *installedApp;
- sscanf(line, "%32s %" PRIx64 " %" PRIx32 "\n", appsToInstall[installCnt], &appId, &appVersion);
+ sscanf(line, "%" STRINGIFY(MAX_APP_NAME_LEN) "s %" PRIx64 " %" PRIx32 "\n", appsToInstall[*installCnt], &appId, &appVersion);
installedApp = findApp(appId);
- if (!installedApp || (installedApp->version < appVersion)) {
- installCnt++;
+ if (strncmp(appsToInstall[*installCnt], UNINSTALL_CMD, MAX_APP_NAME_LEN) == 0) {
+ if (installedApp) {
+ appsToUninstall[*uninstallCnt] = appId;
+ (*uninstallCnt)++;
+ }
+ } else if (!installedApp || (installedApp->version < appVersion)) {
+ (*installCnt)++;
}
}
@@ -260,7 +271,7 @@
if (line)
free(line);
- return installCnt;
+ return *installCnt + *uninstallCnt;
}
bool fileWriteData(const char *fname, const void *data, size_t size)
@@ -294,6 +305,27 @@
printf("done\n");
}
+void removeApps(int updateCnt)
+{
+ uint8_t buffer[sizeof(struct HostMsgHdr) + 1 + sizeof(uint64_t)];
+ struct HostMsgHdr *mHostMsgHdr = (struct HostMsgHdr *)(&buffer[0]);
+ uint8_t *cmd = (uint8_t *)(&buffer[sizeof(struct HostMsgHdr)]);
+ uint64_t *appId = (uint64_t *)(&buffer[sizeof(struct HostMsgHdr) + 1]);
+ int i;
+
+ for (i = 0; i < updateCnt; i++) {
+ mHostMsgHdr->eventId = EVT_APP_FROM_HOST;
+ mHostMsgHdr->appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
+ mHostMsgHdr->len = 1 + sizeof(uint64_t);
+ *cmd = NANOHUB_EXT_APP_DELETE;
+ memcpy(appId, &appsToUninstall[i], sizeof(uint64_t));
+ printf("Deleting \"%016" PRIx64 "\"...", appsToUninstall[i]);
+ fflush(stdout);
+ if (fileWriteData("/dev/nanohub", buffer, sizeof(buffer)))
+ printf("done\n");
+ }
+}
+
void downloadApps(int updateCnt)
{
int i;
@@ -434,27 +466,31 @@
return 1;
}
} else if (strcmp(argv[1], "download") == 0) {
+ int installCnt, uninstallCnt;
+
if (argc != 2) {
printf("Wrong arg number\n");
return 1;
}
downloadNanohub();
for (i = 0; i < MAX_DOWNLOAD_RETRIES; i++) {
- int updateCnt = parseConfigAppInfo();
+ int updateCnt = parseConfigAppInfo(&installCnt, &uninstallCnt);
if (updateCnt > 0) {
if (i == MAX_DOWNLOAD_RETRIES - 1) {
LOGE("Download failed after %d retries; erasing all apps "
"before final attempt", i);
eraseSharedArea();
+ uninstallCnt = 0;
}
- downloadApps(updateCnt);
+ removeApps(uninstallCnt);
+ downloadApps(installCnt);
resetHub();
} else if (!updateCnt){
return 0;
}
}
- if (parseConfigAppInfo() != 0) {
+ if (parseConfigAppInfo(&installCnt, &uninstallCnt) != 0) {
LOGE("Failed to download all apps!");
}
return 1;
diff --git a/util/nanotool/androidcontexthub.cpp b/util/nanotool/androidcontexthub.cpp
index 8ffecc0..44af1b4 100644
--- a/util/nanotool/androidcontexthub.cpp
+++ b/util/nanotool/androidcontexthub.cpp
@@ -37,8 +37,8 @@
constexpr char kSensorDeviceFile[] = "/dev/nanohub";
constexpr char kCommsDeviceFile[] = "/dev/nanohub_comms";
-constexpr char kLockDirectory[] = "/data/system/nanohub_lock";
-constexpr char kLockFile[] = "/data/system/nanohub_lock/lock";
+constexpr char kLockDirectory[] = "/data/vendor/sensor/nanohub_lock";
+constexpr char kLockFile[] = "/data/vendor/sensor/nanohub_lock/lock";
constexpr mode_t kLockDirPermissions = (S_IRUSR | S_IWUSR | S_IXUSR);
diff --git a/util/sensortest/Android.mk b/util/sensortest/Android.mk
index cbf1b3e..0f385e0 100644
--- a/util/sensortest/Android.mk
+++ b/util/sensortest/Android.mk
@@ -33,5 +33,7 @@
LOCAL_MODULE:= sensortest
+LOCAL_SDK_VERSION := current
+
include $(BUILD_EXECUTABLE)
diff --git a/util/sensortest/sensortest.cpp b/util/sensortest/sensortest.cpp
index c032ca9..36d727a 100644
--- a/util/sensortest/sensortest.cpp
+++ b/util/sensortest/sensortest.cpp
@@ -26,7 +26,7 @@
int listIndex;
int type;
int32_t rate;
- int reportLatency;
+ int64_t reportLatency;
bool receivedEvent;
};
@@ -119,7 +119,7 @@
if (existingSensorConfigIndex >= 0) {
printf("Replacing previous config for sensor type %d\n", atoi(argv[currArgumentIndex+1]));
- mSensorConfigList[existingSensorConfigIndex] = {
+ mSensorConfigList[existingSensorConfigIndex] = (SensorConfig) {
.listIndex = sensorIndex,
.type = atoi(argv[currArgumentIndex+1]),
.rate = atoi(argv[currArgumentIndex+2]),
@@ -127,7 +127,7 @@
.receivedEvent = false
};
} else {
- mSensorConfigList[(mNumSensorConfigs)++] = {
+ mSensorConfigList[(mNumSensorConfigs)++] = (SensorConfig) {
.listIndex = sensorIndex,
.type = atoi(argv[currArgumentIndex+1]),
.rate = atoi(argv[currArgumentIndex+2]),
@@ -152,7 +152,7 @@
if (existingSensorConfigIndex >= 0) {
printf("Replacing previous config for sensor type %d\n", atoi(argv[currArgumentIndex+1]));
- mSensorConfigList[existingSensorConfigIndex] = {
+ mSensorConfigList[existingSensorConfigIndex] = (SensorConfig) {
.listIndex = sensorIndex,
.type = atoi(argv[currArgumentIndex+1]),
.rate = atoi(argv[currArgumentIndex+2]),
@@ -160,7 +160,7 @@
.receivedEvent = false
};
} else {
- mSensorConfigList[(mNumSensorConfigs)++] = {
+ mSensorConfigList[(mNumSensorConfigs)++] = (SensorConfig) {
.listIndex = sensorIndex,
.type = atoi(argv[currArgumentIndex+1]),
.rate = atoi(argv[currArgumentIndex+2]),
@@ -215,8 +215,9 @@
for (int i = 0; i < mNumSensorConfigs; i++) {
if (ASensorEventQueue_registerSensor(sensorEventQueue, mSensorList[mSensorConfigList[i].listIndex],
mSensorConfigList[i].rate, mSensorConfigList[i].reportLatency) < 0) {
- printf("Unable to register sensor %d with rate %d and report latency %d\n", mSensorConfigList[i].listIndex,
- mSensorConfigList[i].rate, mSensorConfigList[i].reportLatency);
+ printf("Unable to register sensor %d with rate %d and report latency %" PRId64 "\n",
+ mSensorConfigList[i].listIndex, mSensorConfigList[i].rate,
+ mSensorConfigList[i].reportLatency);
}
}
