dynamic_sensor: Support Android head tracker sensor type.

Bug: 189507742
Test: Verified head tracker sensor type and sampling.
Test: Verified custom Android sensor sampling.
Change-Id: Id403de916111ed737b9cebebe9850d5e5845cf56

2 files changed, 88 insertions, 12 deletions

diff --git a/modules/sensors/dynamic_sensor/HidRawSensor.cpp b/modules/sensors/dynamic_sensor/HidRawSensor.cpp
index 66542280..4520ddaa 100644
--- a/modules/sensors/dynamic_sensor/HidRawSensor.cpp
+++ b/modules/sensors/dynamic_sensor/HidRawSensor.cpp
@@ -635,8 +635,8 @@ bool HidRawSensor::detectAndroidHeadTrackerSensor(
         return false;
     }
 
-    mFeatureInfo.type = SENSOR_TYPE_DEVICE_PRIVATE_BASE;
-    mFeatureInfo.typeString = CUSTOM_TYPE_PREFIX + "headtracker";
+    mFeatureInfo.type = SENSOR_TYPE_HEAD_TRACKER;
+    mFeatureInfo.typeString = SENSOR_STRING_TYPE_HEAD_TRACKER;
     mFeatureInfo.reportModeFlag = SENSOR_FLAG_CONTINUOUS_MODE;
     mFeatureInfo.permission = "";
     mFeatureInfo.isWakeUp = false;
@@ -1011,6 +1011,50 @@ void HidRawSensor::handleInput(uint8_t id, const std::vector<uint8_t> &message)
         .type = mSensor.type
     };
     bool valid = true;
+
+    switch (mFeatureInfo.type) {
+        case SENSOR_TYPE_HEAD_TRACKER:
+            valid = getHeadTrackerEventData(message, &event);
+            break;
+        default:
+            valid = getSensorEventData(message, &event);
+            break;
+    }
+    if (!valid) {
+        LOG_E << "Invalid data observed in decoding, discard" << LOG_ENDL;
+        return;
+    }
+    event.timestamp = -1;
+    generateEvent(event);
+}
+
+bool HidRawSensor::getHeadTrackerEventData(const std::vector<uint8_t> &message,
+                                           sensors_event_t *event) {
+    head_tracker_event_t *head_tracker;
+
+    head_tracker = &(event->head_tracker);
+    if (!getReportFieldValue(message, &(mTranslateTable[0]),
+                             &(head_tracker->rx))
+            || !getReportFieldValue(message, &(mTranslateTable[1]),
+                                    &(head_tracker->ry))
+            || !getReportFieldValue(message, &(mTranslateTable[2]),
+                                    &(head_tracker->rz))
+            || !getReportFieldValue(message, &(mTranslateTable[3]),
+                                    &(head_tracker->vx))
+            || !getReportFieldValue(message, &(mTranslateTable[4]),
+                                    &(head_tracker->vy))
+            || !getReportFieldValue(message, &(mTranslateTable[5]),
+                                    &(head_tracker->vz))
+            || !getReportFieldValue(message, &(mTranslateTable[6]),
+                                    &(head_tracker->discontinuity_count))) {
+        return false;
+    }
+
+    return true;
+}
+
+bool HidRawSensor::getSensorEventData(const std::vector<uint8_t> &message,
+                                      sensors_event_t *event) {
     for (const auto &rec : mTranslateTable) {
         int64_t v = (message[rec.byteOffset + rec.byteSize - 1] & 0x80) ? -1 : 0;
         for (int i = static_cast<int>(rec.byteSize) - 1; i >= 0; --i) {
@@ -1020,26 +1064,23 @@ void HidRawSensor::handleInput(uint8_t id, const std::vector<uint8_t> &message)
         switch (rec.type) {
             case TYPE_FLOAT:
                 if (v > rec.maxValue || v < rec.minValue) {
-                    valid = false;
+                    return false;
                 }
-                event.data[rec.index] = rec.a * (v + rec.b);
+                event->data[rec.index] = rec.a * (v + rec.b);
                 break;
             case TYPE_INT64:
                 if (v > rec.maxValue || v < rec.minValue) {
-                    valid = false;
+                    return false;
                 }
-                event.u64.data[rec.index] = v + rec.b;
+                event->u64.data[rec.index] = v + rec.b;
                 break;
             case TYPE_ACCURACY:
-                event.magnetic.status = (v & 0xFF) + rec.b;
+                event->magnetic.status = (v & 0xFF) + rec.b;
                 break;
         }
     }
-    if (!valid) {
-        LOG_V << "Range error observed in decoding, discard" << LOG_ENDL;
-    }
-    event.timestamp = -1;
-    generateEvent(event);
+
+    return true;
 }
 
 std::string HidRawSensor::dump() const {
diff --git a/modules/sensors/dynamic_sensor/HidRawSensor.h b/modules/sensors/dynamic_sensor/HidRawSensor.h
index f6d13b59..66843fcd 100644
--- a/modules/sensors/dynamic_sensor/HidRawSensor.h
+++ b/modules/sensors/dynamic_sensor/HidRawSensor.h
@@ -46,6 +46,14 @@ public:
     // handle input report received
     void handleInput(uint8_t id, const std::vector<uint8_t> &message);
 
+    // get head tracker sensor event data
+    bool getHeadTrackerEventData(const std::vector<uint8_t> &message,
+                                 sensors_event_t *event);
+
+    // get generic sensor event data
+    bool getSensorEventData(const std::vector<uint8_t> &message,
+                            sensors_event_t *event);
+
     // indicate if the HidRawSensor is a valid one
     bool isValid() const { return mValid; };
 
@@ -141,6 +149,33 @@ private:
     // process HID snesor spec defined orientation(quaternion) sensor usages.
     bool processQuaternionUsage(const std::vector<HidParser::ReportPacket> &packets);
 
+    // get the value of a report field
+    template<typename ValueType>
+    bool getReportFieldValue(const std::vector<uint8_t> &message,
+                             ReportTranslateRecord* rec, ValueType* value) {
+        bool valid = true;
+        int64_t v;
+
+        v = (message[rec->byteOffset + rec->byteSize - 1] & 0x80) ? -1 : 0;
+        for (int i = static_cast<int>(rec->byteSize) - 1; i >= 0; --i) {
+            v = (v << 8) | message[rec->byteOffset + i]; // HID is little endian
+        }
+        if (v > rec->maxValue || v < rec->minValue) {
+            valid = false;
+        }
+
+        switch (rec->type) {
+            case TYPE_FLOAT:
+                *value = rec->a * (v + rec->b);
+                break;
+            case TYPE_INT64:
+                *value = v + rec->b;
+                break;
+        }
+
+        return valid;
+    }
+
     // dump data for test/debug purpose
     std::string dump() const;