Snap for 4448085 from b1a8e55ffcff1b5cc8f3aba33d24fa05f45dab5b to oc-m3-release
Change-Id: Ie090ec34edcf4e465d68a18d940a63334ff14b3c
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index 0a6d96d..c700253 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -180,6 +180,7 @@
acc->x_bias = acc->y_bias = acc->z_bias = 0;
acc->x_bias_new = acc->y_bias_new = acc->z_bias_new = 0;
+ acc->average_temperature_celsius = 0;
#ifdef IMU_TEMP_DBG_ENABLED
acc->temp_time_nanos = 0;
@@ -422,7 +423,11 @@
float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
- float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z);
+ float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
+
+ // Testing for negative number.
+ float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
+
// Passing when evmin/evmax> EIGEN_RATIO.
int eigen_pass = (evmin > evmax * EIGEN_RATIO) && (evmag > EIGEN_MAG);
@@ -526,10 +531,11 @@
// Eigen Ratio Test.
if (accEigenTest(&acc->ac1[temp_gate].akf,
&acc->ac1[temp_gate].agd)) {
- // Storing the new offsets.
+ // Storing the new offsets and average temperature.
acc->x_bias_new = bias.x * KSCALE2;
acc->y_bias_new = bias.y * KSCALE2;
acc->z_bias_new = bias.z * KSCALE2;
+ acc->average_temperature_celsius = acc->ac1[temp_gate].agd.mean_t;
}
#ifdef ACCEL_CAL_DBG_ENABLED
//// Debug ///////
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index e76abcd..1cfef61 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -139,6 +139,9 @@
// to store a new offset, which gets updated during a power down event.
float x_bias_new, y_bias_new, z_bias_new;
+ // Average temperature of the bias update.
+ float average_temperature_celsius;
+
// Offset values that get subtracted from live data
float x_bias, y_bias, z_bias;
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
index 1fb7630..7f8e563 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -71,7 +71,10 @@
float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
- float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z);
+ float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
+
+ // Testing for negative number.
+ float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) &&
(evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG);
@@ -119,7 +122,8 @@
initVec3(&v, out.x, out.y, out.z);
vec3ScalarMul(&v, -0.5f);
- float r = sqrtf(vec3Dot(&v, &v) - out.w);
+ float r_square = vec3Dot(&v, &v) - out.w;
+ float r = (r_square > 0) ? sqrtf(r_square) : 0;
initVec3(bias, v.x, v.y, v.z);
*radius = r;
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index 251dcb9..ba87c15 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -114,6 +114,21 @@
&& sensorIndex <= COMMS_SENSOR_ACTIVITY_LAST;
}
+static bool isWakeEvent(int32_t sensor)
+{
+ switch (sensor) {
+ case COMMS_SENSOR_DOUBLE_TOUCH:
+ case COMMS_SENSOR_DOUBLE_TWIST:
+ case COMMS_SENSOR_GESTURE:
+ case COMMS_SENSOR_PROXIMITY:
+ case COMMS_SENSOR_SIGNIFICANT_MOTION:
+ case COMMS_SENSOR_TILT:
+ return true;
+ default:
+ return false;
+ }
+}
+
HubConnection::HubConnection()
: Thread(false /* canCallJava */),
mRing(10 *1024),
@@ -143,6 +158,7 @@
mWakelockHeld = false;
mWakeEventCount = 0;
+ mWriteFailures = 0;
initNanohubLock();
@@ -616,31 +632,20 @@
return ev;
}
-ssize_t HubConnection::getWakeEventCount()
+ssize_t HubConnection::decrementIfWakeEventLocked(int32_t sensor)
{
+ if (isWakeEvent(sensor)) {
+ if (mWakeEventCount > 0)
+ mWakeEventCount--;
+ else
+ ALOGW("%s: sensor=%d, unexpected count=%d, no-op",
+ __FUNCTION__, sensor, mWakeEventCount);
+ }
+
return mWakeEventCount;
}
-ssize_t HubConnection::decrementWakeEventCount()
-{
- return --mWakeEventCount;
-}
-
-bool HubConnection::isWakeEvent(int32_t sensor)
-{
- switch (sensor) {
- case COMMS_SENSOR_PROXIMITY:
- case COMMS_SENSOR_SIGNIFICANT_MOTION:
- case COMMS_SENSOR_TILT:
- case COMMS_SENSOR_DOUBLE_TWIST:
- case COMMS_SENSOR_GESTURE:
- return true;
- default:
- return false;
- }
-}
-
-void HubConnection::protectIfWakeEvent(int32_t sensor)
+void HubConnection::protectIfWakeEventLocked(int32_t sensor)
{
if (isWakeEvent(sensor)) {
if (mWakelockHeld == false) {
@@ -653,6 +658,8 @@
void HubConnection::releaseWakeLockIfAppropriate()
{
+ Mutex::Autolock autoLock(mLock);
+
if (mWakelockHeld && (mWakeEventCount == 0)) {
mWakelockHeld = false;
release_wake_lock(WAKELOCK_NAME);
@@ -728,11 +735,8 @@
break;
}
- if (cnt > 0) {
- // If event is a wake event, protect it with a wakelock
- protectIfWakeEvent(sensor);
+ if (cnt > 0)
write(nev, cnt);
- }
}
uint8_t HubConnection::magAccuracyUpdate(sensors_vec_t *sv)
@@ -830,11 +834,8 @@
break;
}
- if (cnt > 0) {
- // If event is a wake event, protect it with a wakelock
- protectIfWakeEvent(sensor);
+ if (cnt > 0)
write(nev, cnt);
- }
}
void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct ThreeAxisSample *sample, bool highAccuracy)
@@ -1021,11 +1022,8 @@
break;
}
- if (cnt > 0) {
- // If event is a wake event, protect it with a wakelock
- protectIfWakeEvent(sensor);
+ if (cnt > 0)
write(nev, cnt);
- }
}
void HubConnection::discardInotifyEvent() {
@@ -1659,10 +1657,6 @@
return false;
}
-ssize_t HubConnection::read(sensors_event_t *ev, size_t size) {
- return mRing.read(ev, size);
-}
-
void HubConnection::setActivityCallback(ActivityEventHandler *eventHandler)
{
Mutex::Autolock autoLock(mLock);
@@ -1912,8 +1906,44 @@
}
}
+ssize_t HubConnection::read(sensors_event_t *ev, size_t size) {
+ ssize_t n = mRing.read(ev, size);
+
+ Mutex::Autolock autoLock(mLock);
+
+ // We log the first failure in write, so only log 2+ errors
+ if (mWriteFailures > 1) {
+ ALOGW("%s: mRing.write failed %d times",
+ __FUNCTION__, mWriteFailures);
+ mWriteFailures = 0;
+ }
+
+ for (ssize_t i = 0; i < n; i++)
+ decrementIfWakeEventLocked(ev[i].sensor);
+
+ return n;
+}
+
+
ssize_t HubConnection::write(const sensors_event_t *ev, size_t n) {
- return mRing.write(ev, n);
+ ssize_t ret = 0;
+
+ Mutex::Autolock autoLock(mLock);
+
+ for (size_t i=0; i<n; i++) {
+ if (mRing.write(&ev[i], 1) == 1) {
+ ret++;
+ // If event is a wake event, protect it with a wakelock
+ protectIfWakeEventLocked(ev[i].sensor);
+ } else {
+ if (mWriteFailures++ == 0)
+ ALOGW("%s: mRing.write failed @ %zu/%zu",
+ __FUNCTION__, i, n);
+ break;
+ }
+ }
+
+ return ret;
}
#ifdef USB_MAG_BIAS_REPORTING_ENABLED
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
index 4dbd64e..388660c 100644
--- a/sensorhal/hubconnection.h
+++ b/sensorhal/hubconnection.h
@@ -79,10 +79,7 @@
void setOperationParameter(const additional_info_event_t &info);
- bool isWakeEvent(int32_t sensor);
void releaseWakeLockIfAppropriate();
- ssize_t getWakeEventCount();
- ssize_t decrementWakeEventCount();
//TODO: factor out event ring buffer functionality into a separate class
ssize_t read(sensors_event_t *ev, size_t size);
@@ -111,7 +108,8 @@
bool mWakelockHeld;
int32_t mWakeEventCount;
- void protectIfWakeEvent(int32_t sensor);
+ void protectIfWakeEventLocked(int32_t sensor);
+ ssize_t decrementIfWakeEventLocked(int32_t sensor);
static inline uint64_t period_ns_to_frequency_q10(nsecs_t period_ns) {
return 1024000000000ULL / period_ns;
@@ -244,6 +242,7 @@
Mutex mLock;
RingBuffer mRing;
+ int32_t mWriteFailures;
ActivityEventHandler *mActivityEventHandler;
diff --git a/sensorhal/sensors.cpp b/sensorhal/sensors.cpp
index 0ddb263..234f7da 100644
--- a/sensorhal/sensors.cpp
+++ b/sensorhal/sensors.cpp
@@ -105,28 +105,7 @@
// Release wakelock if held and no more events in ring buffer
mHubConnection->releaseWakeLockIfAppropriate();
- ssize_t n = mHubConnection->read(data, count);
-
- if (n < 0) {
- return -1;
- }
-
- // If we have wake events in the queue, determine how many we're sending
- // up this round and decrement that count now so that when we get called back,
- // we'll have an accurate count of how many wake events are STILL in the HAL queue
- // to be able to determine whether we can release our wakelock if held.
- if (mHubConnection->getWakeEventCount() != 0) {
- for (ssize_t i = 0; i < n; i++) {
- if (mHubConnection->isWakeEvent(data[i].sensor)) {
- ssize_t count = mHubConnection->decrementWakeEventCount();
- if (count == 0) {
- break;
- }
- }
- }
- }
-
- return n;
+ return mHubConnection->read(data, count);
}
int SensorContext::batch(