Move sensord
Move service to more device-specific directory.
Bug: 36996994
Test: Build targets
Change-Id: Ie0befa422d6cc114826d6a7e145d7f8d4da61185
diff --git a/cmds/vr/pose/Android.mk b/cmds/vr/pose/Android.mk
deleted file mode 100644
index 8be3214..0000000
--- a/cmds/vr/pose/Android.mk
+++ /dev/null
@@ -1,35 +0,0 @@
-# Copyright (C) 2008 The Android Open Source Project
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-LOCAL_PATH := $(call my-dir)
-
-sourceFiles := \
- pose.cpp
-
-staticLibraries := \
- libdvrcommon \
- libvrsensor \
- libpdx_default_transport \
-
-sharedLibraries := \
- libcutils \
- liblog
-
-include $(CLEAR_VARS)
-LOCAL_SRC_FILES := $(sourceFiles)
-LOCAL_STATIC_LIBRARIES := $(staticLibraries)
-LOCAL_SHARED_LIBRARIES := $(sharedLibraries)
-LOCAL_MODULE := pose
-LOCAL_MODULE_TAGS := optional
-include $(BUILD_EXECUTABLE)
diff --git a/cmds/vr/pose/pose.cpp b/cmds/vr/pose/pose.cpp
deleted file mode 100644
index 2288a86..0000000
--- a/cmds/vr/pose/pose.cpp
+++ /dev/null
@@ -1,274 +0,0 @@
-// pose is a utility to query and manipulate the current pose via the pose
-// service.
-
-#include <cmath>
-#include <cstdio>
-#include <iomanip>
-#include <iostream>
-#include <regex>
-#include <vector>
-
-#include <private/dvr/types.h>
-#include <dvr/pose_client.h>
-
-using android::dvr::vec3;
-using android::dvr::quat;
-
-namespace {
-
-// Prints usage information to stderr.
-void PrintUsage(const char* executable_name) {
- std::cerr << "Usage: " << executable_name
- << " [--identity|--set=...|--unfreeze]\n"
- << "\n"
- << " no arguments: display the current pose.\n"
- << " --identity: freeze the pose to the identity pose.\n"
- << " --set=rx,ry,rz,rw[,px,py,pz]: freeze the pose to the given "
- "state. rx,ry,rz,rw are interpreted as rotation quaternion. "
- " px, py, pz as position (0,0,0 if omitted).\n"
- << " --mode=mode: sets mode to one of normal, head_turn:slow, "
- "head_turn:fast, rotate:slow, rotate:medium, rotate:fast, "
- "circle_strafe.\n"
- << " --unfreeze: sets the mode to normal.\n"
- << " --log_controller=[true|false]: starts and stops controller"
- " logs\n"
- << std::endl;
-}
-
-// If return_code is negative, print out its corresponding string description
-// and exit the program with a non-zero exit code.
-void ExitIfNegative(int return_code) {
- if (return_code < 0) {
- std::cerr << "Error: " << strerror(-return_code) << std::endl;
- std::exit(1);
- }
-}
-
-// Parses the following command line flags:
-// --identity
-// --set=rx,ry,rz,rw[,px,py,pz]
-// Returns false if parsing fails.
-bool ParseState(const std::string& arg, DvrPoseState* out_state) {
- if (arg == "--identity") {
- *out_state = {.head_from_start_rotation = {0.f, 0.f, 0.f, 1.f},
- .head_from_start_translation = {0.f, 0.f, 0.f},
- .timestamp_ns = 0,
- .sensor_from_start_rotation_velocity = {0.f, 0.f, 0.f}};
- return true;
- }
-
- const std::string prefix("--set=");
- if (arg.size() < 6 || arg.compare(0, prefix.size(), prefix) != 0) {
- return false;
- }
-
- // Tokenize by ','.
- std::regex split_by_comma("[,]+");
- std::sregex_token_iterator token_it(arg.begin() + prefix.size(), arg.end(),
- split_by_comma,
- -1 /* return inbetween parts */);
- std::sregex_token_iterator token_end;
-
- // Convert to float and store values.
- std::vector<float> values;
- for (; token_it != token_end; ++token_it) {
- std::string token = *(token_it);
- float value = 0.f;
- if (sscanf(token.c_str(), "%f", &value) != 1) {
- std::cerr << "Unable to parse --set value as float: " << token
- << std::endl;
- return false;
- } else {
- values.push_back(value);
- }
- }
-
- if (values.size() != 4 && values.size() != 7) {
- std::cerr << "Unable to parse --set, expected either 4 or 7 of values."
- << std::endl;
- return false;
- }
-
- float norm2 = values[0] * values[0] + values[1] * values[1] +
- values[2] * values[2] + values[3] * values[3];
- if (std::abs(norm2 - 1.f) > 1e-4) {
- if (norm2 < 1e-8) {
- std::cerr << "--set quaternion norm close to zero." << std::endl;
- return false;
- }
- float norm = std::sqrt(norm2);
- values[0] /= norm;
- values[1] /= norm;
- values[2] /= norm;
- values[3] /= norm;
- }
-
- out_state->head_from_start_rotation = {values[0], values[1], values[2],
- values[3]};
-
- if (values.size() == 7) {
- out_state->head_from_start_translation = {values[4], values[5], values[6]};
- } else {
- out_state->head_from_start_translation = {0.f, 0.f, 0.f};
- }
-
- out_state->timestamp_ns = 0;
- out_state->sensor_from_start_rotation_velocity = {0.f, 0.f, 0.f};
-
- return true;
-}
-
-// Parses the command line flag --mode.
-// Returns false if parsing fails.
-bool ParseSetMode(const std::string& arg, DvrPoseMode* mode) {
- const std::string prefix("--mode=");
- if (arg.size() < prefix.size() ||
- arg.compare(0, prefix.size(), prefix) != 0) {
- return false;
- }
-
- std::string value = arg.substr(prefix.size());
-
- if (value == "normal") {
- *mode = DVR_POSE_MODE_6DOF;
- return true;
- } else if (value == "head_turn:slow") {
- *mode = DVR_POSE_MODE_MOCK_HEAD_TURN_SLOW;
- return true;
- } else if (value == "head_turn:fast") {
- *mode = DVR_POSE_MODE_MOCK_HEAD_TURN_FAST;
- return true;
- } else if (value == "rotate:slow") {
- *mode = DVR_POSE_MODE_MOCK_ROTATE_SLOW;
- return true;
- } else if (value == "rotate:medium") {
- *mode = DVR_POSE_MODE_MOCK_ROTATE_MEDIUM;
- return true;
- } else if (value == "rotate:fast") {
- *mode = DVR_POSE_MODE_MOCK_ROTATE_FAST;
- return true;
- } else if (value == "circle_strafe") {
- *mode = DVR_POSE_MODE_MOCK_CIRCLE_STRAFE;
- return true;
- } else {
- return false;
- }
-}
-
-// Parses the command line flag --controller_log.
-// Returns false if parsing fails.
-bool ParseLogController(const std::string& arg, bool* log_enabled) {
- const std::string prefix("--log_controller=");
- if (arg.size() < prefix.size() ||
- arg.compare(0, prefix.size(), prefix) != 0) {
- return false;
- }
-
- std::string value = arg.substr(prefix.size());
-
- if (value == "false") {
- *log_enabled = false;
- return true;
- } else if (value == "true") {
- *log_enabled = true;
- return true;
- } else {
- return false;
- }
-}
-
-// The different actions that the tool can perform.
-enum class Action {
- Query, // Query the current pose.
- Set, // Set the pose and freeze.
- Unfreeze, // Set the pose mode to normal.
- SetMode, // Sets the pose mode.
- LogController, // Start/stop controller logging in sensord.
-};
-
-// The action to perform when no arguments are passed to the tool.
-constexpr Action kDefaultAction = Action::Query;
-
-} // namespace
-
-int main(int argc, char** argv) {
- Action action = kDefaultAction;
- DvrPoseState state;
- DvrPoseMode pose_mode = DVR_POSE_MODE_6DOF;
- bool log_controller = false;
-
- // Parse command-line arguments.
- for (int i = 1; i < argc; ++i) {
- const std::string arg = argv[i];
- if (ParseState(arg, &state) && action == kDefaultAction) {
- action = Action::Set;
- } else if (arg == "--unfreeze" && action == kDefaultAction) {
- action = Action::Unfreeze;
- } else if (ParseSetMode(arg, &pose_mode) && action == kDefaultAction) {
- action = Action::SetMode;
- } else if (ParseLogController(arg, &log_controller)) {
- action = Action::LogController;
- } else {
- PrintUsage(argv[0]);
- return 1;
- }
- }
-
- auto pose_client = dvrPoseCreate();
- if (!pose_client) {
- std::cerr << "Unable to create pose client." << std::endl;
- return 1;
- }
-
- switch (action) {
- case Action::Query: {
- ExitIfNegative(dvrPosePoll(pose_client, &state));
- uint64_t timestamp = state.timestamp_ns;
- const auto& rotation = state.head_from_start_rotation;
- const auto& translation = state.head_from_start_translation;
- const auto& rotation_velocity = state.sensor_from_start_rotation_velocity;
- quat q(rotation.w, rotation.x, rotation.y, rotation.z);
- vec3 angles = q.matrix().eulerAngles(0, 1, 2);
- angles = angles * 180.f / M_PI;
- vec3 x = q * vec3(1.0f, 0.0f, 0.0f);
- vec3 y = q * vec3(0.0f, 1.0f, 0.0f);
- vec3 z = q * vec3(0.0f, 0.0f, 1.0f);
-
- std::cout << "timestamp_ns: " << timestamp << std::endl
- << "rotation_quaternion: " << rotation.x << ", " << rotation.y
- << ", " << rotation.z << ", " << rotation.w << std::endl
- << "rotation_angles: " << angles.x() << ", " << angles.y()
- << ", " << angles.z() << std::endl
- << "translation: " << translation.x << ", " << translation.y
- << ", " << translation.z << std::endl
- << "rotation_velocity: " << rotation_velocity.x << ", "
- << rotation_velocity.y << ", " << rotation_velocity.z
- << std::endl
- << "axes: " << std::setprecision(3)
- << "x(" << x.x() << ", " << x.y() << ", " << x.z() << "), "
- << "y(" << y.x() << ", " << y.y() << ", " << y.z() << "), "
- << "z(" << z.x() << ", " << z.y() << ", " << z.z() << "), "
- << std::endl;
- break;
- }
- case Action::Set: {
- ExitIfNegative(dvrPoseFreeze(pose_client, &state));
- break;
- }
- case Action::Unfreeze: {
- ExitIfNegative(dvrPoseSetMode(pose_client, DVR_POSE_MODE_6DOF));
- break;
- }
- case Action::SetMode: {
- ExitIfNegative(dvrPoseSetMode(pose_client, pose_mode));
- break;
- }
- case Action::LogController: {
- ExitIfNegative(
- dvrPoseLogController(pose_client, log_controller));
- break;
- }
- }
-
- dvrPoseDestroy(pose_client);
-}
diff --git a/services/vr/sensord/Android.mk b/services/vr/sensord/Android.mk
deleted file mode 100644
index 638c9a8..0000000
--- a/services/vr/sensord/Android.mk
+++ /dev/null
@@ -1,76 +0,0 @@
-# Copyright (C) 2008 The Android Open Source Project
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-LOCAL_PATH := $(call my-dir)
-
-SENSORD_EXTEND ?= libsensordextensionstub
-
-sourceFiles := \
- pose_service.cpp \
- sensord.cpp \
- sensor_fusion.cpp \
- sensor_hal_thread.cpp \
- sensor_ndk_thread.cpp \
- sensor_service.cpp \
- sensor_thread.cpp \
-
-includeFiles += \
- $(LOCAL_PATH)/include
-
-staticLibraries := \
- libdvrcommon \
- libvrsensor \
- libperformance \
- libbufferhub \
- libpdx_default_transport \
- libposepredictor \
-
-sharedLibraries := \
- libandroid \
- libbase \
- libbinder \
- libcutils \
- liblog \
- libhardware \
- libutils \
- libui \
- $(SENSORD_EXTEND) \
-
-cFlags := -DLOG_TAG=\"sensord\" \
- -DTRACE=0
-
-include $(CLEAR_VARS)
-LOCAL_SRC_FILES := $(sourceFiles)
-LOCAL_CFLAGS := $(cFlags)
-LOCAL_STATIC_LIBRARIES := $(staticLibraries)
-LOCAL_SHARED_LIBRARIES := $(sharedLibraries)
-LOCAL_MODULE_CLASS := EXECUTABLES
-LOCAL_MODULE := sensord
-LOCAL_C_INCLUDES := $(includeFiles)
-LOCAL_C_INCLUDES += \
- $(call local-generated-sources-dir)/proto/frameworks/native/services/vr/sensord
-LOCAL_INIT_RC := sensord.rc
-include $(BUILD_EXECUTABLE)
-
-include $(CLEAR_VARS)
-LOCAL_STATIC_LIBRARIES := $(staticLibraries)
-LOCAL_SHARED_LIBRARIES := $(sharedLibraries)
-LOCAL_SRC_FILES := test/poselatencytest.cpp
-LOCAL_MODULE := poselatencytest
-include $(BUILD_EXECUTABLE)
-
-include $(CLEAR_VARS)
-LOCAL_MODULE := libsensordextensionstub
-LOCAL_SRC_FILES := sensord_extension.cpp
-include $(BUILD_SHARED_LIBRARY)
diff --git a/services/vr/sensord/pose_service.cpp b/services/vr/sensord/pose_service.cpp
deleted file mode 100644
index 75423bb..0000000
--- a/services/vr/sensord/pose_service.cpp
+++ /dev/null
@@ -1,649 +0,0 @@
-#define ATRACE_TAG ATRACE_TAG_INPUT
-#include "pose_service.h"
-
-#include <dlfcn.h>
-#include <errno.h>
-#include <time.h>
-
-#include <array>
-#include <cmath>
-#include <cstdint>
-#include <sstream>
-#include <type_traits>
-
-#include <cutils/properties.h>
-#include <cutils/trace.h>
-#include <dvr/performance_client_api.h>
-#include <dvr/pose_client.h>
-#include <hardware/sensors.h>
-#include <log/log.h>
-#include <pdx/default_transport/service_endpoint.h>
-#include <private/dvr/benchmark.h>
-#include <private/dvr/clock_ns.h>
-#include <private/dvr/platform_defines.h>
-#include <private/dvr/pose-ipc.h>
-#include <private/dvr/sensor_constants.h>
-#include <utils/Trace.h>
-
-using android::pdx::LocalChannelHandle;
-using android::pdx::default_transport::Endpoint;
-using android::pdx::Status;
-
-namespace android {
-namespace dvr {
-
-using Vector3d = vec3d;
-using Rotationd = quatd;
-using AngleAxisd = Eigen::AngleAxis<double>;
-
-namespace {
-// Wait a few seconds before checking if we need to disable sensors.
-static constexpr int64_t kSensorTimeoutNs = 5000000000ll;
-
-static constexpr float kTwoPi = 2.0 * M_PI;
-static constexpr float kDegToRad = M_PI / 180.f;
-
-// Head model code data.
-static constexpr float kDefaultNeckHorizontalOffset = 0.080f; // meters
-static constexpr float kDefaultNeckVerticalOffset = 0.075f; // meters
-
-static constexpr char kDisablePosePredictionProp[] =
- "persist.dvr.disable_predict";
-
-// Device type property for controlling classes of behavior that differ
-// between devices. If unset, defaults to kOrientationTypeSmartphone.
-static constexpr char kOrientationTypeProp[] = "ro.dvr.orientation_type";
-static constexpr char kEnableSensorRecordProp[] = "dvr.enable_6dof_recording";
-static constexpr char kEnableSensorPlayProp[] = "dvr.enable_6dof_playback";
-static constexpr char kEnableSensorPlayIdProp[] = "dvr.6dof_playback_id";
-static constexpr char kEnablePoseRecordProp[] = "dvr.enable_pose_recording";
-static constexpr char kPredictorTypeProp[] = "dvr.predictor_type";
-
-// Persistent buffer names.
-static constexpr char kPoseRingBufferName[] = "PoseService:RingBuffer";
-
-static constexpr int kDatasetIdLength = 36;
-static constexpr char kDatasetIdChars[] = "0123456789abcdef-";
-
-static constexpr int kLatencyWindowSize = 200;
-
-// These are the flags used by BufferProducer::CreatePersistentUncachedBlob,
-// plus PRIVATE_ADSP_HEAP to allow access from the DSP.
-static constexpr int kPoseRingBufferFlags =
- GRALLOC_USAGE_SW_READ_RARELY | GRALLOC_USAGE_SW_WRITE_RARELY |
- GRALLOC_USAGE_PRIVATE_UNCACHED | GRALLOC_USAGE_PRIVATE_ADSP_HEAP;
-
-std::string GetPoseModeString(DvrPoseMode mode) {
- switch (mode) {
- case DVR_POSE_MODE_6DOF:
- return "DVR_POSE_MODE_6DOF";
- case DVR_POSE_MODE_3DOF:
- return "DVR_POSE_MODE_3DOF";
- case DVR_POSE_MODE_MOCK_FROZEN:
- return "DVR_POSE_MODE_MOCK_FROZEN";
- case DVR_POSE_MODE_MOCK_HEAD_TURN_SLOW:
- return "DVR_POSE_MODE_MOCK_HEAD_TURN_SLOW";
- case DVR_POSE_MODE_MOCK_HEAD_TURN_FAST:
- return "DVR_POSE_MODE_MOCK_HEAD_TURN_FAST";
- case DVR_POSE_MODE_MOCK_ROTATE_SLOW:
- return "DVR_POSE_MODE_MOCK_ROTATE_SLOW";
- case DVR_POSE_MODE_MOCK_ROTATE_MEDIUM:
- return "DVR_POSE_MODE_MOCK_ROTATE_MEDIUM";
- case DVR_POSE_MODE_MOCK_ROTATE_FAST:
- return "DVR_POSE_MODE_MOCK_ROTATE_FAST";
- case DVR_POSE_MODE_MOCK_CIRCLE_STRAFE:
- return "DVR_POSE_MODE_MOCK_CIRCLE_STRAFE";
- default:
- return "Unknown pose mode";
- }
-}
-
-} // namespace
-
-PoseService::PoseService(SensorThread* sensor_thread)
- : BASE("PoseService", Endpoint::Create(DVR_POSE_SERVICE_CLIENT)),
- sensor_thread_(sensor_thread),
- last_sensor_usage_time_ns_(0),
- watchdog_shutdown_(false),
- sensors_on_(false),
- accelerometer_index_(-1),
- gyroscope_index_(-1),
- pose_mode_(DVR_POSE_MODE_6DOF),
- mapped_pose_buffer_(nullptr),
- vsync_count_(0),
- photon_timestamp_(0),
- // Will be updated by external service, but start with a non-zero value:
- display_period_ns_(16000000),
- sensor_latency_(kLatencyWindowSize) {
- last_known_pose_ = {
- .orientation = {1.0f, 0.0f, 0.0f, 0.0f},
- .translation = {0.0f, 0.0f, 0.0f, 0.0f},
- .angular_velocity = {0.0f, 0.0f, 0.0f, 0.0f},
- .velocity = {0.0f, 0.0f, 0.0f, 0.0f},
- .timestamp_ns = 0,
- .flags = DVR_POSE_FLAG_HEAD,
- .pad = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
- };
-
- switch (property_get_int32(kOrientationTypeProp, kOrientationTypePortrait)) {
- case kOrientationTypeLandscape:
- device_orientation_type_ = kOrientationTypeLandscape;
- break;
- default:
- device_orientation_type_ = kOrientationTypePortrait;
- break;
- }
-
- ring_buffer_ =
- BufferProducer::Create(kPoseRingBufferName, 0, 0, kPoseRingBufferFlags,
- sizeof(DvrPoseRingBuffer));
- if (!ring_buffer_) {
- ALOGE("PoseService::PoseService: Failed to create/get pose ring buffer!");
- return;
- }
-
- void* addr = nullptr;
- int ret =
- ring_buffer_->GetBlobReadWritePointer(sizeof(DvrPoseRingBuffer), &addr);
- if (ret < 0) {
- ALOGE("PoseService::PoseService: Failed to map pose ring buffer: %s",
- strerror(-ret));
- return;
- }
- memset(addr, 0, sizeof(DvrPoseRingBuffer));
- mapped_pose_buffer_ = static_cast<DvrPoseRingBuffer*>(addr);
- addr = nullptr;
-
- for (int i = 0; i < sensor_thread->GetSensorCount(); ++i) {
- if (sensor_thread->GetSensorType(i) == SENSOR_TYPE_ACCELEROMETER)
- accelerometer_index_ = i;
- if (sensor_thread->GetSensorType(i) == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED)
- gyroscope_index_ = i;
- }
- // If we failed to find the uncalibrated gyroscope, use the regular one.
- if (gyroscope_index_ < 0) {
- ALOGW("PoseService was unable to find uncalibrated gyroscope");
- for (int i = 0; i < sensor_thread->GetSensorCount(); ++i) {
- ALOGI("Type %d", sensor_thread->GetSensorType(i));
- if (sensor_thread->GetSensorType(i) == SENSOR_TYPE_GYROSCOPE)
- gyroscope_index_ = i;
- }
- }
-
- if (accelerometer_index_ < 0) {
- ALOGE("PoseService was unable to find accelerometer");
- }
- if (gyroscope_index_ < 0) {
- ALOGE("PoseService was unable to find gyroscope");
- }
-
- {
- std::lock_guard<std::mutex> lock(mutex_);
- KickSensorWatchDogThread();
- }
-
- // Read the persistent dvr flags before using them in SetPoseMode.
- enable_pose_prediction_ =
- property_get_bool(kDisablePosePredictionProp, 0) == 0;
-
- enable_sensor_recording_ = property_get_bool(kEnableSensorRecordProp, 0) == 1;
-
- enable_sensor_playback_ = property_get_bool(kEnableSensorPlayProp, 0) == 1;
-
- if (enable_sensor_playback_) {
- char dataset_id[PROPERTY_VALUE_MAX];
- property_get(kEnableSensorPlayIdProp, dataset_id, "");
- sensor_playback_id_ = std::string(dataset_id);
-
- if (sensor_playback_id_.length() != kDatasetIdLength ||
- sensor_playback_id_.find_first_not_of(kDatasetIdChars) !=
- std::string::npos) {
- ALOGE("Error: invalid playback id %s", sensor_playback_id_.c_str());
- sensor_playback_id_ = "";
- enable_sensor_playback_ = false;
- } else {
- ALOGI("Playback id %s", sensor_playback_id_.c_str());
- }
- }
-
- switch (property_get_int32(kPredictorTypeProp, 0)) {
- case 1:
- pose_predictor_ = posepredictor::Predictor::Create(
- posepredictor::PredictorType::Quadric);
- default:
- pose_predictor_ = posepredictor::Predictor::Create(
- posepredictor::PredictorType::Linear);
- }
-
- enable_pose_recording_ = property_get_bool(kEnablePoseRecordProp, 0) == 1;
-
- SetPoseMode(DVR_POSE_MODE_6DOF);
-}
-
-PoseService::~PoseService() {
- if (watchdog_thread_.get_id() != std::thread::id()) {
- {
- std::lock_guard<std::mutex> guard(mutex_);
- watchdog_shutdown_ = true;
- watchdog_condition_.notify_one();
- }
- watchdog_thread_.join();
- }
-}
-
-void PoseService::KickSensorWatchDogThread() {
- // This method is called every frame while rendering so we want to make sure
- // it is very light weight with synchronization.
- // TODO(jbates) For better performance, we can consider a lock-free atomic
- // solution instead of locking this mutex.
-
- // Update the usage time. The watchdog thread will poll this value to know
- // when to disable sensors.
- last_sensor_usage_time_ns_ = GetSystemClockNs();
-
- // If sensors are still on, there's nothing else to do.
- if (sensors_on_)
- return;
-
- // Enable sensors.
- ALOGI("Start using sensors.");
- sensors_on_ = true;
- if (accelerometer_index_ >= 0) {
- sensor_thread_->StartUsingSensor(accelerometer_index_);
- }
- if (gyroscope_index_ >= 0) {
- sensor_thread_->StartUsingSensor(gyroscope_index_);
- }
-
- // Tell the thread to wake up to disable the sensors when no longer needed.
- watchdog_condition_.notify_one();
-
- if (watchdog_thread_.get_id() == std::thread::id()) {
- // The sensor watchdog thread runs while sensors are in use. When no APIs
- // have requested sensors beyond a threshold (5 seconds), sensors are
- // disabled.
- watchdog_thread_ = std::thread([this] {
- std::unique_lock<std::mutex> lock(mutex_);
- while (!watchdog_shutdown_) {
- int64_t remaining_sensor_time_ns =
- last_sensor_usage_time_ns_ + kSensorTimeoutNs - GetSystemClockNs();
-
- if (remaining_sensor_time_ns > 0) {
- // Wait for the remaining usage time before checking again.
- watchdog_condition_.wait_for(
- lock, std::chrono::nanoseconds(remaining_sensor_time_ns));
- continue;
- }
-
- if (sensors_on_) {
- // Disable sensors.
- ALOGI("Stop using sensors.");
- sensors_on_ = false;
- if (accelerometer_index_ >= 0) {
- sensor_thread_->StopUsingSensor(accelerometer_index_);
- }
- if (gyroscope_index_ >= 0) {
- sensor_thread_->StopUsingSensor(gyroscope_index_);
- }
- }
-
- // Wait for sensors to be enabled again.
- watchdog_condition_.wait(lock);
- }
- });
- }
-}
-
-bool PoseService::IsInitialized() const {
- return BASE::IsInitialized() && ring_buffer_ && mapped_pose_buffer_;
-}
-
-void PoseService::WriteAsyncPoses(const Vector3d& start_t_head,
- const Rotationd& start_q_head,
- int64_t pose_timestamp) {
- if (enable_external_pose_) {
- return;
- }
-
- // If playing back data, the timestamps are different enough from the
- // current time that prediction doesn't work. This hack pretends that
- // there was one nanosecond of latency between the sensors and here.
- if (enable_sensor_playback_)
- pose_timestamp = GetSystemClockNs() - 1;
-
- // Feed the sample to the predictor
- AddPredictorPose(pose_predictor_.get(), start_t_head, start_q_head,
- pose_timestamp, &last_known_pose_);
-
- // Store one extra value, because the application is working on the next
- // frame and expects the minimum count from that frame on.
- for (uint32_t i = 0; i < kPoseAsyncBufferMinFutureCount + 1; ++i) {
- int64_t target_time = photon_timestamp_ + i * display_period_ns_;
-
- // TODO(jbates, cwolfe) For the DSP code, we may still want poses even when
- // the vsyncs are not ticking up. But it's important not to update the pose
- // data that's in the past so that applications have the most accurate
- // estimate of the last frame's *actual* pose, so that they can update
- // simulations and calculate collisions, etc.
- if (target_time < pose_timestamp) {
- // Already in the past, do not update this head pose slot.
- continue;
- }
-
- // Write to the actual shared memory ring buffer.
- uint32_t index = ((vsync_count_ + i) & kPoseAsyncBufferIndexMask);
-
- // Make a pose prediction
- if (enable_pose_prediction_) {
- PredictPose(pose_predictor_.get(), target_time,
- target_time + right_eye_photon_offset_ns_,
- mapped_pose_buffer_->ring + index);
- } else {
- mapped_pose_buffer_->ring[index] = last_known_pose_;
- }
- }
-}
-
-void PoseService::UpdatePoseMode() {
- ALOGI_IF(TRACE, "UpdatePoseMode: %f %f %f", last_known_pose_.translation[0],
- last_known_pose_.translation[1], last_known_pose_.translation[2]);
-
- const int64_t current_time_ns = GetSystemClockNs();
-
- const PoseState pose_state = sensor_fusion_.GetLatestPoseState();
-
- switch (pose_mode_) {
- case DVR_POSE_MODE_MOCK_HEAD_TURN_SLOW:
- case DVR_POSE_MODE_MOCK_HEAD_TURN_FAST:
- case DVR_POSE_MODE_MOCK_ROTATE_SLOW:
- case DVR_POSE_MODE_MOCK_ROTATE_MEDIUM:
- case DVR_POSE_MODE_MOCK_ROTATE_FAST:
- case DVR_POSE_MODE_MOCK_CIRCLE_STRAFE: {
- // Calculate a pose based on monotic system time.
- const Vector3d y_axis(0., 1., 0.);
- double time_s = current_time_ns / 1e9;
-
- // Generate fake yaw data.
- float yaw = 0.0f;
- Vector3d head_trans(0.0, 0.0, 0.0);
- switch (pose_mode_) {
- default:
- case DVR_POSE_MODE_MOCK_HEAD_TURN_SLOW:
- // Pan across 120 degrees in 15 seconds.
- yaw = std::cos(kTwoPi * time_s / 15.0) * 60.0 * kDegToRad;
- break;
- case DVR_POSE_MODE_MOCK_HEAD_TURN_FAST:
- // Pan across 120 degrees in 4 seconds.
- yaw = std::cos(kTwoPi * time_s / 4.0) * 60.0 * kDegToRad;
- break;
- case DVR_POSE_MODE_MOCK_ROTATE_SLOW:
- // Rotate 5 degrees per second.
- yaw = std::fmod(time_s * 5.0 * kDegToRad, kTwoPi);
- break;
- case DVR_POSE_MODE_MOCK_ROTATE_MEDIUM:
- // Rotate 30 degrees per second.
- yaw = std::fmod(time_s * 30.0 * kDegToRad, kTwoPi);
- break;
- case DVR_POSE_MODE_MOCK_ROTATE_FAST:
- // Rotate 90 degrees per second.
- yaw = std::fmod(time_s * 90.0 * kDegToRad, kTwoPi);
- break;
- case DVR_POSE_MODE_MOCK_CIRCLE_STRAFE:
- // Circle strafe around origin at distance of 3 meters.
- yaw = std::fmod(time_s * 30.0 * kDegToRad, kTwoPi);
- head_trans += 3.0 * Vector3d(sin(yaw), 0.0, cos(yaw));
- break;
- }
-
- // Calculate the simulated head rotation in an absolute "head" space.
- // This space is not related to start space and doesn't need a
- // reference.
- Rotationd head_rotation_in_head_space(AngleAxisd(yaw, y_axis));
-
- WriteAsyncPoses(head_trans, head_rotation_in_head_space, current_time_ns);
- break;
- }
- case DVR_POSE_MODE_MOCK_FROZEN: {
- // Even when frozen, we still provide a current timestamp, because
- // consumers may rely on it being monotonic.
-
- Rotationd start_from_head_rotation(
- frozen_state_.head_from_start_rotation.w,
- frozen_state_.head_from_start_rotation.x,
- frozen_state_.head_from_start_rotation.y,
- frozen_state_.head_from_start_rotation.z);
- Vector3d head_from_start_translation(
- frozen_state_.head_from_start_translation.x,
- frozen_state_.head_from_start_translation.y,
- frozen_state_.head_from_start_translation.z);
-
- WriteAsyncPoses(head_from_start_translation, start_from_head_rotation,
- current_time_ns);
- break;
- }
- case DVR_POSE_MODE_3DOF: {
- // Sensor fusion provides IMU-space data, transform to world space.
-
- // Constants to perform IMU orientation adjustments. Note that these
- // calculations will be optimized out in a release build.
- constexpr double k90DegInRad = 90.0 * M_PI / 180.0;
- const Vector3d kVecAxisX(1.0, 0.0, 0.0);
- const Vector3d kVecAxisY(0.0, 1.0, 0.0);
- const Vector3d kVecAxisZ(0.0, 0.0, 1.0);
- const Rotationd kRotX90(AngleAxisd(k90DegInRad, kVecAxisX));
-
- Rotationd start_from_head_rotation;
- if (device_orientation_type_ == kOrientationTypeLandscape) {
- const Rotationd kPostRotation =
- kRotX90 * Rotationd(AngleAxisd(-k90DegInRad, kVecAxisY));
- start_from_head_rotation =
- (pose_state.sensor_from_start_rotation * kPostRotation).inverse();
- } else if (device_orientation_type_ == kOrientationTypeLandscape180) {
- const Rotationd kPreRotation =
- Rotationd(AngleAxisd(k90DegInRad * 2.0, kVecAxisY)) *
- Rotationd(AngleAxisd(k90DegInRad * 2.0, kVecAxisZ));
- const Rotationd kPostRotation = kRotX90;
- start_from_head_rotation =
- (kPreRotation *
- pose_state.sensor_from_start_rotation * kPostRotation)
- .inverse();
- } else {
- const Rotationd kPreRotation =
- Rotationd(AngleAxisd(k90DegInRad, kVecAxisZ));
- const Rotationd kPostRotation = kRotX90;
- start_from_head_rotation =
- (kPreRotation * pose_state.sensor_from_start_rotation *
- kPostRotation)
- .inverse();
- }
- start_from_head_rotation.normalize();
-
- // Neck / head model code procedure for when no 6dof is available.
- // To apply the neck model, first translate the head pose to the new
- // center of eyes, then rotate around the origin (the original head
- // pos).
- Vector3d position =
- start_from_head_rotation * Vector3d(0.0, kDefaultNeckVerticalOffset,
- -kDefaultNeckHorizontalOffset);
-
- // Update the current latency model.
- if (pose_state.timestamp_ns != 0) {
- sensor_latency_.AddLatency(GetSystemClockNs() -
- pose_state.timestamp_ns);
- }
-
- // Update the timestamp with the expected latency.
- WriteAsyncPoses(
- position, start_from_head_rotation,
- pose_state.timestamp_ns + sensor_latency_.CurrentLatencyEstimate());
- break;
- }
- default:
- case DVR_POSE_MODE_6DOF:
- ALOGE("ERROR: invalid pose mode");
- break;
- }
-}
-
-pdx::Status<void> PoseService::HandleMessage(pdx::Message& msg) {
- pdx::Status<void> ret;
- const pdx::MessageInfo& info = msg.GetInfo();
- switch (info.op) {
- case DVR_POSE_NOTIFY_VSYNC: {
- std::lock_guard<std::mutex> guard(mutex_);
-
- // Kick the sensor thread, because we are still rendering.
- KickSensorWatchDogThread();
-
- const struct iovec data[] = {
- {.iov_base = &vsync_count_, .iov_len = sizeof(vsync_count_)},
- {.iov_base = &photon_timestamp_,
- .iov_len = sizeof(photon_timestamp_)},
- {.iov_base = &display_period_ns_,
- .iov_len = sizeof(display_period_ns_)},
- {.iov_base = &right_eye_photon_offset_ns_,
- .iov_len = sizeof(right_eye_photon_offset_ns_)},
- };
- ret = msg.ReadVectorAll(data);
- if (ret && !enable_external_pose_) {
- mapped_pose_buffer_->vsync_count = vsync_count_;
- }
-
- // TODO(jbates, eieio): make this async, no need to reply.
- REPLY_MESSAGE(msg, ret, error);
- }
- case DVR_POSE_POLL: {
- ATRACE_NAME("pose_poll");
- std::lock_guard<std::mutex> guard(mutex_);
-
- DvrPoseState client_state;
- client_state = {
- .head_from_start_rotation = {last_known_pose_.orientation[0],
- last_known_pose_.orientation[1],
- last_known_pose_.orientation[2],
- last_known_pose_.orientation[3]},
- .head_from_start_translation = {last_known_pose_.translation[0],
- last_known_pose_.translation[1],
- last_known_pose_.translation[2]},
- .timestamp_ns = static_cast<uint64_t>(last_known_pose_.timestamp_ns),
- .sensor_from_start_rotation_velocity = {
- last_known_pose_.angular_velocity[0],
- last_known_pose_.angular_velocity[1],
- last_known_pose_.angular_velocity[2]}};
-
- Btrace("Sensor data received",
- static_cast<int64_t>(client_state.timestamp_ns));
-
- Btrace("Pose polled");
-
- ret = msg.WriteAll(&client_state, sizeof(client_state));
- REPLY_MESSAGE(msg, ret, error);
- }
- case DVR_POSE_FREEZE: {
- {
- std::lock_guard<std::mutex> guard(mutex_);
-
- DvrPoseState frozen_state;
- ret = msg.ReadAll(&frozen_state, sizeof(frozen_state));
- if (!ret) {
- REPLY_ERROR(msg, ret.error(), error);
- }
- frozen_state_ = frozen_state;
- }
- SetPoseMode(DVR_POSE_MODE_MOCK_FROZEN);
- REPLY_MESSAGE(msg, ret, error);
- }
- case DVR_POSE_SET_MODE: {
- int mode;
- {
- std::lock_guard<std::mutex> guard(mutex_);
- ret = msg.ReadAll(&mode, sizeof(mode));
- if (!ret) {
- REPLY_ERROR(msg, ret.error(), error);
- }
- if (mode < 0 || mode >= DVR_POSE_MODE_COUNT) {
- REPLY_ERROR(msg, EINVAL, error);
- }
- }
- SetPoseMode(DvrPoseMode(mode));
- REPLY_MESSAGE(msg, ret, error);
- }
- case DVR_POSE_GET_MODE: {
- std::lock_guard<std::mutex> guard(mutex_);
- int mode = pose_mode_;
- ret = msg.WriteAll(&mode, sizeof(mode));
- REPLY_MESSAGE(msg, ret, error);
- }
- case DVR_POSE_GET_RING_BUFFER: {
- std::lock_guard<std::mutex> guard(mutex_);
-
- // Kick the sensor thread, because we have a new consumer.
- KickSensorWatchDogThread();
-
- Status<LocalChannelHandle> consumer_channel =
- ring_buffer_->CreateConsumer();
- REPLY_MESSAGE(msg, consumer_channel, error);
- }
- case DVR_POSE_GET_CONTROLLER_RING_BUFFER: {
- std::lock_guard<std::mutex> guard(mutex_);
- REPLY_ERROR(msg, EINVAL, error);
- }
- case DVR_POSE_LOG_CONTROLLER: {
- std::lock_guard<std::mutex> guard(mutex_);
- REPLY_ERROR(msg, EINVAL, error);
- }
- default:
- // Do not lock mutex_ here, because this may call the on*() handlers,
- // which will lock the mutex themselves.
- ret = Service::HandleMessage(msg);
- break;
- }
-error:
- return ret;
-}
-
-std::string PoseService::DumpState(size_t /*max_length*/) {
- DvrPoseMode pose_mode;
- {
- std::lock_guard<std::mutex> guard(mutex_);
- pose_mode = pose_mode_;
- }
-
- std::ostringstream stream;
- stream << "Pose mode: " << GetPoseModeString(pose_mode);
- return stream.str();
-}
-
-void PoseService::HandleEvents(const sensors_event_t* begin_events,
- const sensors_event_t* end_events) {
- ATRACE_NAME("PoseService::HandleEvents");
- std::lock_guard<std::mutex> guard(mutex_);
-
- for (const sensors_event_t* event = begin_events; event != end_events;
- ++event) {
- if (event->type == SENSOR_TYPE_ACCELEROMETER) {
- sensor_fusion_.ProcessAccelerometerSample(
- event->acceleration.x, event->acceleration.y, event->acceleration.z,
- event->timestamp);
- } else if (event->type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
- sensor_fusion_.ProcessGyroscopeSample(event->gyro.x, event->gyro.y,
- event->gyro.z, event->timestamp);
- }
- }
-
- UpdatePoseMode();
-}
-
-void PoseService::SetPoseMode(DvrPoseMode mode) {
- if (mode == DVR_POSE_MODE_6DOF) {
- // Only 3DoF is currently supported.
- mode = DVR_POSE_MODE_3DOF;
- }
-
- pose_mode_ = mode;
-
- sensor_thread_->SetPaused(false);
-}
-
-} // namespace dvr
-} // namespace android
diff --git a/services/vr/sensord/pose_service.h b/services/vr/sensord/pose_service.h
deleted file mode 100644
index 7b7adec..0000000
--- a/services/vr/sensord/pose_service.h
+++ /dev/null
@@ -1,149 +0,0 @@
-#ifndef ANDROID_DVR_SENSORD_POSE_SERVICE_H_
-#define ANDROID_DVR_SENSORD_POSE_SERVICE_H_
-
-#include <condition_variable>
-#include <forward_list>
-#include <mutex>
-#include <thread>
-#include <unordered_map>
-#include <vector>
-
-#include <dvr/pose_client.h>
-#include <pdx/service.h>
-#include <private/dvr/buffer_hub_client.h>
-#include <private/dvr/dvr_pose_predictor.h>
-#include <private/dvr/latency_model.h>
-#include <private/dvr/pose_client_internal.h>
-#include <private/dvr/ring_buffer.h>
-
-#include "sensor_fusion.h"
-#include "sensor_thread.h"
-
-namespace android {
-namespace dvr {
-
-// PoseService implements the HMD pose service over ServiceFS.
-class PoseService : public pdx::ServiceBase<PoseService> {
- public:
- ~PoseService() override;
-
- bool IsInitialized() const override;
- pdx::Status<void> HandleMessage(pdx::Message& msg) override;
- std::string DumpState(size_t max_length) override;
-
- // Handle events from the sensor HAL.
- // Safe to call concurrently with any other public member functions.
- void HandleEvents(const sensors_event_t* begin_events,
- const sensors_event_t* end_events);
-
- private:
- friend BASE;
-
- enum OrientationType {
- // Typical smartphone device (default).
- kOrientationTypePortrait = 1,
- // Landscape device.
- kOrientationTypeLandscape = 2,
- // 180 Landscape device.
- kOrientationTypeLandscape180 = 3,
- };
-
- // Initializes the service. Keeps a reference to sensor_thread, which must be
- // non-null.
- explicit PoseService(SensorThread* sensor_thread);
-
- // Kick the sensor watch dog thread which will robustly disable IMU usage
- // when there are no sensor data consumers.
- // The class mutex (mutex_) must be locked while calling this method.
- void KickSensorWatchDogThread();
-
- void UpdatePoseMode();
-
- // Update the async pose ring buffer with new pose data.
- // |start_t_head| Head position in start space.
- // |start_q_head| Head orientation quaternion in start space.
- // |pose_timestamp| System timestamp of pose data in seconds.
- // |pose_delta_time| Elapsed time in seconds between this pose and the last.
- void WriteAsyncPoses(const Eigen::Vector3<double>& start_t_head,
- const Eigen::Quaternion<double>& start_q_head,
- int64_t pose_timestamp);
-
- // Set the pose mode.
- void SetPoseMode(DvrPoseMode mode);
-
- // The abstraction around the sensor data.
- SensorThread* sensor_thread_;
-
- // Protects access to all member variables.
- std::mutex mutex_;
-
- // Watchdog thread data. The watchdog thread will ensure that sensor access
- // is disabled when nothing has been consuming it for a while.
- int64_t last_sensor_usage_time_ns_;
- std::thread watchdog_thread_;
- std::condition_variable watchdog_condition_;
- bool watchdog_shutdown_;
- bool sensors_on_;
-
- // Indices for the accelerometer and gyroscope sensors, or -1 if the sensor
- // wasn't present on construction.
- int accelerometer_index_;
- int gyroscope_index_;
-
- // The sensor fusion algorithm and its state.
- SensorFusion sensor_fusion_;
-
- // Current pose mode.
- DvrPoseMode pose_mode_;
-
- // State which is sent if pose_mode_ is DVR_POSE_MODE_MOCK_FROZEN.
- DvrPoseState frozen_state_;
-
- // Last known pose.
- DvrPoseAsync last_known_pose_;
-
- // If this flag is true, the pose published includes a small prediction of
- // where it'll be when it's consumed.
- bool enable_pose_prediction_;
-
- // Flag to turn on recording of raw sensor data
- bool enable_sensor_recording_;
-
- // Flag to log pose to a file
- bool enable_pose_recording_;
-
- // Flag to turn on playback from a saved dataset instead of using live data.
- bool enable_sensor_playback_;
-
- std::string sensor_playback_id_;
-
- // External pose generation.
- bool enable_external_pose_ = false;
-
- // The predictor to extrapolate pose samples.
- std::unique_ptr<posepredictor::Predictor> pose_predictor_;
-
- // Pose ring buffer.
- std::shared_ptr<BufferProducer> ring_buffer_;
- // Temporary mapped ring buffer.
- DvrPoseRingBuffer* mapped_pose_buffer_;
- // Current vsync info, updated by displayd.
- uint32_t vsync_count_;
- int64_t photon_timestamp_;
- int64_t display_period_ns_;
- int64_t right_eye_photon_offset_ns_ = 0;
-
- // To model the measurement - arrival latency.
- LatencyModel sensor_latency_;
-
- // Type for controlling pose orientation calculation.
- OrientationType device_orientation_type_;
-
- PoseService(const PoseService&) = delete;
- void operator=(const PoseService&) = delete;
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_SENSORD_POSE_SERVICE_H_
diff --git a/services/vr/sensord/sensor_fusion.cpp b/services/vr/sensord/sensor_fusion.cpp
deleted file mode 100644
index 5663ae4..0000000
--- a/services/vr/sensord/sensor_fusion.cpp
+++ /dev/null
@@ -1,348 +0,0 @@
-#include "sensor_fusion.h"
-
-#include <algorithm>
-#include <cmath>
-
-#include <private/dvr/eigen.h>
-
-namespace android {
-namespace dvr {
-
-namespace {
-
-// --- start of added bits for porting to eigen
-
-// In general, we prefer to add wrappers for things like Inverse() to minimize
-// the changes to the imported code, so that merging in upstream changes becomes
-// simpler.
-
-inline Matrix3d Inverse(const Matrix3d& matrix) { return matrix.inverse(); }
-inline Matrix3d Transpose(const Matrix3d& matrix) { return matrix.transpose(); }
-inline Matrix3d RotationMatrixNH(const Rotationd& rotation) {
- return rotation.toRotationMatrix();
-}
-inline double Length(const Vector3d& vector) { return vector.norm(); }
-
-using uint64 = uint64_t;
-
-// --- end of added bits for porting to eigen
-
-static const double kFiniteDifferencingEpsilon = 1e-7;
-static const double kEpsilon = 1e-15;
-// Default gyroscope frequency. This corresponds to 200 Hz.
-static const double kDefaultGyroscopeTimestep_s = 0.005f;
-// Maximum time between gyroscope before we start limiting the integration.
-static const double kMaximumGyroscopeSampleDelay_s = 0.04f;
-// Compute a first-order exponential moving average of changes in accel norm per
-// frame.
-static const double kSmoothingFactor = 0.5;
-// Minimum and maximum values used for accelerometer noise covariance matrix.
-// The smaller the sigma value, the more weight is given to the accelerometer
-// signal.
-static const double kMinAccelNoiseSigma = 0.75;
-static const double kMaxAccelNoiseSigma = 7.0;
-// Initial value for the diagonal elements of the different covariance matrices.
-static const double kInitialStateCovarianceValue = 25.0;
-static const double kInitialProcessCovarianceValue = 1.0;
-// Maximum accelerometer norm change allowed before capping it covariance to a
-// large value.
-static const double kMaxAccelNormChange = 0.15;
-// Timestep IIR filtering coefficient.
-static const double kTimestepFilterCoeff = 0.95;
-// Minimum number of sample for timestep filtering.
-static const uint32_t kTimestepFilterMinSamples = 10;
-
-// Z direction in start space.
-static const Vector3d kCanonicalZDirection(0.0, 0.0, 1.0);
-
-// Computes a axis angle rotation from the input vector.
-// angle = norm(a)
-// axis = a.normalized()
-// If norm(a) == 0, it returns an identity rotation.
-static Rotationd RotationFromVector(const Vector3d& a) {
- const double norm_a = Length(a);
- if (norm_a < kEpsilon) {
- return Rotationd::Identity();
- }
- return Rotationd(AngleAxisd(norm_a, a / norm_a));
-}
-
-// --- start of functions ported from pose_prediction.cc
-
-namespace pose_prediction {
-
-// Returns a rotation matrix based on the integration of the gyroscope_value
-// over the timestep_s in seconds.
-// TODO(pfg): Document the space better here.
-//
-// @param gyroscope_value gyroscope sensor values.
-// @param timestep_s integration period in seconds.
-// @return Integration of the gyroscope value the rotation is from Start to
-// Sensor Space.
-Rotationd GetRotationFromGyroscope(const Vector3d& gyroscope_value,
- double timestep_s) {
- const double velocity = Length(gyroscope_value);
-
- // When there is no rotation data return an identity rotation.
- if (velocity < kEpsilon) {
- return Rotationd::Identity();
- }
- // Since the gyroscope_value is a start from sensor transformation we need to
- // invert it to have a sensor from start transformation, hence the minus sign.
- // For more info:
- // http://developer.android.com/guide/topics/sensors/sensors_motion.html#sensors-motion-gyro
- return Rotationd(AngleAxisd(-timestep_s * velocity,
- gyroscope_value / velocity));
-}
-
-} // namespace pose_prediction
-
-// --- end of functions ported from pose_prediction.cc
-
-} // namespace
-
-SensorFusion::SensorFusion()
- : execute_reset_with_next_accelerometer_sample_(false) {
- ResetState();
-}
-
-void SensorFusion::Reset() {
- execute_reset_with_next_accelerometer_sample_ = true;
-}
-
-void SensorFusion::ResetState() {
- current_state_.timestamp_ns = 0;
- current_state_.sensor_from_start_rotation = Rotationd::Identity();
- current_state_.sensor_from_start_rotation_velocity = Vector3d::Zero();
-
- current_accelerometer_timestamp_ns_ = 0;
-
- state_covariance_ = Matrix3d::Identity() * kInitialStateCovarianceValue;
- process_covariance_ = Matrix3d::Identity() * kInitialProcessCovarianceValue;
- accelerometer_measurement_covariance_ =
- Matrix3d::Identity() * kMinAccelNoiseSigma * kMinAccelNoiseSigma;
- innovation_covariance_.setIdentity();
-
- accelerometer_measurement_jacobian_ = Matrix3d::Zero();
- kalman_gain_ = Matrix3d::Zero();
- innovation_ = Vector3d::Zero();
- accelerometer_measurement_ = Vector3d::Zero();
- prediction_ = Vector3d::Zero();
- control_input_ = Vector3d::Zero();
- state_update_ = Vector3d::Zero();
-
- moving_average_accelerometer_norm_change_ = 0.0;
-
- is_timestep_filter_initialized_ = false;
- is_gyroscope_filter_valid_ = false;
- is_aligned_with_gravity_ = false;
-}
-
-// Here I am doing something wrong relative to time stamps. The state timestamps
-// always correspond to the gyrostamps because it would require additional
-// extrapolation if I wanted to do otherwise.
-// TODO(pfg): investigate about published an updated pose after accelerometer
-// data was used for filtering.
-PoseState SensorFusion::GetLatestPoseState() const {
- std::unique_lock<std::mutex> lock(mutex_);
- return current_state_;
-}
-
-void SensorFusion::ProcessGyroscopeSample(float v_x, float v_y, float v_z,
- uint64 timestamp_ns) {
- std::unique_lock<std::mutex> lock(mutex_);
-
- // Don't accept gyroscope sample when waiting for a reset.
- if (execute_reset_with_next_accelerometer_sample_) {
- return;
- }
-
- // Discard outdated samples.
- if (current_state_.timestamp_ns >= timestamp_ns) {
- // TODO(pfg): Investigate why this happens.
- return;
- }
-
- // Checks that we received at least one gyroscope sample in the past.
- if (current_state_.timestamp_ns != 0) {
- // TODO(pfg): roll this in filter gyroscope timestep function.
- double current_timestep_s =
- static_cast<double>(timestamp_ns - current_state_.timestamp_ns) * 1e-9;
- if (current_timestep_s > kMaximumGyroscopeSampleDelay_s) {
- if (is_gyroscope_filter_valid_) {
- // Replaces the delta timestamp by the filtered estimates of the delta
- // time.
- current_timestep_s = filtered_gyroscope_timestep_s_;
- } else {
- current_timestep_s = kDefaultGyroscopeTimestep_s;
- }
- } else {
- FilterGyroscopeTimestep(current_timestep_s);
- }
-
- // Only integrate after receiving a accelerometer sample.
- if (is_aligned_with_gravity_) {
- const Rotationd rotation_from_gyroscope =
- pose_prediction::GetRotationFromGyroscope(Vector3d(v_x, v_y, v_z),
- current_timestep_s);
- current_state_.sensor_from_start_rotation =
- rotation_from_gyroscope * current_state_.sensor_from_start_rotation;
- current_state_.sensor_from_start_rotation.normalize();
- UpdateStateCovariance(RotationMatrixNH(rotation_from_gyroscope));
- state_covariance_ =
- state_covariance_ +
- (process_covariance_ * (current_timestep_s * current_timestep_s));
- }
- }
-
- // Saves gyroscope event for future prediction.
- current_state_.timestamp_ns = timestamp_ns;
- current_state_.sensor_from_start_rotation_velocity = Vector3d(v_x, v_y, v_z);
-}
-
-// TODO(pfg): move to rotation object for the input.
-Vector3d SensorFusion::ComputeInnovation(const Rotationd& pose) {
- const Vector3d predicted_down_direction =
- RotationMatrixNH(pose) * kCanonicalZDirection;
-
- const Rotationd rotation = Rotationd::FromTwoVectors(
- predicted_down_direction, accelerometer_measurement_);
- AngleAxisd angle_axis(rotation);
- return angle_axis.axis() * angle_axis.angle();
-}
-
-void SensorFusion::ComputeMeasurementJacobian() {
- for (int dof = 0; dof < 3; dof++) {
- // TODO(pfg): Create this delta rotation in the constructor and used unitX..
- Vector3d delta = Vector3d::Zero();
- delta[dof] = kFiniteDifferencingEpsilon;
-
- const Rotationd epsilon_rotation = RotationFromVector(delta);
- const Vector3d delta_rotation = ComputeInnovation(
- epsilon_rotation * current_state_.sensor_from_start_rotation);
-
- const Vector3d col =
- (innovation_ - delta_rotation) / kFiniteDifferencingEpsilon;
- accelerometer_measurement_jacobian_(0, dof) = col[0];
- accelerometer_measurement_jacobian_(1, dof) = col[1];
- accelerometer_measurement_jacobian_(2, dof) = col[2];
- }
-}
-
-void SensorFusion::ProcessAccelerometerSample(float acc_x, float acc_y,
- float acc_z,
- uint64 timestamp_ns) {
- std::unique_lock<std::mutex> lock(mutex_);
-
- // Discard outdated samples.
- if (current_accelerometer_timestamp_ns_ >= timestamp_ns) {
- // TODO(pfg): Investigate why this happens.
- return;
- }
-
- // Call reset state if required.
- if (execute_reset_with_next_accelerometer_sample_.exchange(false)) {
- ResetState();
- }
-
- accelerometer_measurement_ = Vector3d(acc_x, acc_y, acc_z);
- current_accelerometer_timestamp_ns_ = timestamp_ns;
-
- if (!is_aligned_with_gravity_) {
- // This is the first accelerometer measurement so it initializes the
- // orientation estimate.
- current_state_.sensor_from_start_rotation = Rotationd::FromTwoVectors(
- kCanonicalZDirection, accelerometer_measurement_);
- is_aligned_with_gravity_ = true;
-
- previous_accelerometer_norm_ = Length(accelerometer_measurement_);
- return;
- }
-
- UpdateMeasurementCovariance();
-
- innovation_ = ComputeInnovation(current_state_.sensor_from_start_rotation);
- ComputeMeasurementJacobian();
-
- // S = H * P * H' + R
- innovation_covariance_ = accelerometer_measurement_jacobian_ *
- state_covariance_ *
- Transpose(accelerometer_measurement_jacobian_) +
- accelerometer_measurement_covariance_;
-
- // K = P * H' * S^-1
- kalman_gain_ = state_covariance_ *
- Transpose(accelerometer_measurement_jacobian_) *
- Inverse(innovation_covariance_);
-
- // x_update = K*nu
- state_update_ = kalman_gain_ * innovation_;
-
- // P = (I - K * H) * P;
- state_covariance_ = (Matrix3d::Identity() -
- kalman_gain_ * accelerometer_measurement_jacobian_) *
- state_covariance_;
-
- // Updates pose and associate covariance matrix.
- const Rotationd rotation_from_state_update =
- RotationFromVector(state_update_);
-
- current_state_.sensor_from_start_rotation =
- rotation_from_state_update * current_state_.sensor_from_start_rotation;
- UpdateStateCovariance(RotationMatrixNH(rotation_from_state_update));
-}
-
-void SensorFusion::UpdateStateCovariance(const Matrix3d& motion_update) {
- state_covariance_ =
- motion_update * state_covariance_ * Transpose(motion_update);
-}
-
-void SensorFusion::FilterGyroscopeTimestep(double gyroscope_timestep_s) {
- if (!is_timestep_filter_initialized_) {
- // Initializes the filter.
- filtered_gyroscope_timestep_s_ = gyroscope_timestep_s;
- num_gyroscope_timestep_samples_ = 1;
- is_timestep_filter_initialized_ = true;
- return;
- }
-
- // Computes the IIR filter response.
- filtered_gyroscope_timestep_s_ =
- kTimestepFilterCoeff * filtered_gyroscope_timestep_s_ +
- (1 - kTimestepFilterCoeff) * gyroscope_timestep_s;
- ++num_gyroscope_timestep_samples_;
-
- if (num_gyroscope_timestep_samples_ > kTimestepFilterMinSamples) {
- is_gyroscope_filter_valid_ = true;
- }
-}
-
-void SensorFusion::UpdateMeasurementCovariance() {
- const double current_accelerometer_norm = Length(accelerometer_measurement_);
- // Norm change between current and previous accel readings.
- const double current_accelerometer_norm_change =
- std::abs(current_accelerometer_norm - previous_accelerometer_norm_);
- previous_accelerometer_norm_ = current_accelerometer_norm;
-
- moving_average_accelerometer_norm_change_ =
- kSmoothingFactor * current_accelerometer_norm_change +
- (1. - kSmoothingFactor) * moving_average_accelerometer_norm_change_;
-
- // If we hit the accel norm change threshold, we use the maximum noise sigma
- // for the accel covariance. For anything below that, we use a linear
- // combination between min and max sigma values.
- const double norm_change_ratio =
- moving_average_accelerometer_norm_change_ / kMaxAccelNormChange;
- const double accelerometer_noise_sigma = std::min(
- kMaxAccelNoiseSigma,
- kMinAccelNoiseSigma +
- norm_change_ratio * (kMaxAccelNoiseSigma - kMinAccelNoiseSigma));
-
- // Updates the accel covariance matrix with the new sigma value.
- accelerometer_measurement_covariance_ = Matrix3d::Identity() *
- accelerometer_noise_sigma *
- accelerometer_noise_sigma;
-}
-
-} // namespace dvr
-} // namespace android
diff --git a/services/vr/sensord/sensor_fusion.h b/services/vr/sensord/sensor_fusion.h
deleted file mode 100644
index 0ceae21..0000000
--- a/services/vr/sensord/sensor_fusion.h
+++ /dev/null
@@ -1,181 +0,0 @@
-#ifndef ANDROID_DVR_SENSORD_SENSOR_FUSION_H_
-#define ANDROID_DVR_SENSORD_SENSOR_FUSION_H_
-
-#include <atomic>
-#include <cstdlib>
-#include <mutex>
-
-#include <private/dvr/types.h>
-
-namespace android {
-namespace dvr {
-
-using Matrix3d = Eigen::Matrix<double, 3, 3>;
-using Rotationd = quatd;
-using Vector3d = vec3d;
-using AngleAxisd = Eigen::AngleAxisd;
-
-// Ported from GVR's pose_state.h.
-// Stores a 3dof pose plus derivatives. This can be used for prediction.
-struct PoseState {
- // Time in nanoseconds for the current pose.
- uint64_t timestamp_ns;
-
- // Rotation from Sensor Space to Start Space.
- Rotationd sensor_from_start_rotation;
-
- // First derivative of the rotation.
- // TODO(pfg): currently storing gyro data, switch to first derivative instead.
- Vector3d sensor_from_start_rotation_velocity;
-};
-
-// Sensor fusion class that implements an Extended Kalman Filter (EKF) to
-// estimate a 3D rotation from a gyroscope and and accelerometer.
-// This system only has one state, the pose. It does not estimate any velocity
-// or acceleration.
-//
-// To learn more about Kalman filtering one can read this article which is a
-// good introduction: http://en.wikipedia.org/wiki/Kalman_filter
-//
-// Start Space is :
-// z is up.
-// y is forward based on the first sensor data.
-// x = y \times z
-// Sensor Space follows the android specification {@link
-// http://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords}
-// See http://go/vr-coords for definitions of Start Space and Sensor Space.
-//
-// This is a port from GVR's SensorFusion code (See
-// https://cs/vr/gvr/sensors/sensor_fusion.h)
-// which in turn is a port from java of OrientationEKF (See
-// https://cs/java/com/google/vr/cardboard/vrtoolkit/vrtoolkit/src/main/java/com/google/vrtoolkit/cardboard/sensors/internal/OrientationEKF.java)
-class SensorFusion {
- public:
- SensorFusion();
- SensorFusion(const SensorFusion&) = delete;
- void operator=(const SensorFusion&) = delete;
-
- // Resets the state of the sensor fusion. It sets the velocity for
- // prediction to zero. The reset will happen with the next
- // accelerometer sample. Gyroscope sample will be discarded until a new
- // accelerometer sample arrives.
- void Reset();
-
- // Gets the PoseState representing the latest pose and derivatives at a
- // particular timestamp as estimated by SensorFusion.
- PoseState GetLatestPoseState() const;
-
- // Processes one gyroscope sample event. This updates the pose of the system
- // and the prediction model. The gyroscope data is assumed to be in axis angle
- // form. Angle = ||v|| and Axis = v / ||v||, with v = [v_x, v_y, v_z]^T.
- //
- // @param v_x velocity in x.
- // @param v_y velocity in y.
- // @param v_z velocity in z.
- // @param timestamp_ns gyroscope event timestamp in nanosecond.
- void ProcessGyroscopeSample(float v_x, float v_y, float v_z,
- uint64_t timestamp_ns);
-
- // Processes one accelerometer sample event. This updates the pose of the
- // system. If the Accelerometer norm changes too much between sample it is not
- // trusted as much.
- //
- // @param acc_x accelerometer data in x.
- // @param acc_y accelerometer data in y.
- // @param acc_z accelerometer data in z.
- // @param timestamp_ns accelerometer event timestamp in nanosecond.
- void ProcessAccelerometerSample(float acc_x, float acc_y, float acc_z,
- uint64_t timestamp_ns);
-
- private:
- // Estimates the average timestep between gyroscope event.
- void FilterGyroscopeTimestep(double gyroscope_timestep);
-
- // Updates the state covariance with an incremental motion. It changes the
- // space of the quadric.
- void UpdateStateCovariance(const Matrix3d& motion_update);
-
- // Computes the innovation vector of the Kalman based on the input pose.
- // It uses the latest measurement vector (i.e. accelerometer data), which must
- // be set prior to calling this function.
- Vector3d ComputeInnovation(const Rotationd& pose);
-
- // This computes the measurement_jacobian_ via numerical differentiation based
- // on the current value of sensor_from_start_rotation_.
- void ComputeMeasurementJacobian();
-
- // Updates the accelerometer covariance matrix.
- //
- // This looks at the norm of recent accelerometer readings. If it has changed
- // significantly, it means the phone receives additional acceleration than
- // just gravity, and so the down vector information gravity signal is noisier.
- //
- // TODO(dcoz,pfg): this function is very simple, we probably need something
- // more elaborated here once we have proper regression testing.
- void UpdateMeasurementCovariance();
-
- // Reset all internal states. This is not thread safe. Lock should be acquired
- // outside of it. This function is called in ProcessAccelerometerSample.
- void ResetState();
-
- // Current transformation from Sensor Space to Start Space.
- // x_sensor = sensor_from_start_rotation_ * x_start;
- PoseState current_state_;
-
- // Filtering of the gyroscope timestep started?
- bool is_timestep_filter_initialized_;
- // Filtered gyroscope timestep valid?
- bool is_gyroscope_filter_valid_;
- // Sensor fusion currently aligned with gravity? After initialization
- // it will requires a couple of accelerometer data for the system to get
- // aligned.
- bool is_aligned_with_gravity_;
-
- // Covariance of Kalman filter state (P in common formulation).
- Matrix3d state_covariance_;
- // Covariance of the process noise (Q in common formulation).
- Matrix3d process_covariance_;
- // Covariance of the accelerometer measurement (R in common formulation).
- Matrix3d accelerometer_measurement_covariance_;
- // Covariance of innovation (S in common formulation).
- Matrix3d innovation_covariance_;
- // Jacobian of the measurements (H in common formulation).
- Matrix3d accelerometer_measurement_jacobian_;
- // Gain of the Kalman filter (K in common formulation).
- Matrix3d kalman_gain_;
- // Parameter update a.k.a. innovation vector. (\nu in common formulation).
- Vector3d innovation_;
- // Measurement vector (z in common formulation).
- Vector3d accelerometer_measurement_;
- // Current prediction vector (g in common formulation).
- Vector3d prediction_;
- // Control input, currently this is only the gyroscope data (\mu in common
- // formulation).
- Vector3d control_input_;
- // Update of the state vector. (x in common formulation).
- Vector3d state_update_;
-
- // Time of the last accelerometer processed event.
- uint64_t current_accelerometer_timestamp_ns_;
-
- // Estimates of the timestep between gyroscope event in seconds.
- double filtered_gyroscope_timestep_s_;
- // Number of timestep samples processed so far by the filter.
- uint32_t num_gyroscope_timestep_samples_;
- // Norm of the accelerometer for the previous measurement.
- double previous_accelerometer_norm_;
- // Moving average of the accelerometer norm changes. It is computed for every
- // sensor datum.
- double moving_average_accelerometer_norm_change_;
-
- // Flag indicating if a state reset should be executed with the next
- // accelerometer sample.
- std::atomic<bool> execute_reset_with_next_accelerometer_sample_;
-
- mutable std::mutex mutex_;
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_SENSORD_SENSOR_FUSION_H_
diff --git a/services/vr/sensord/sensor_hal_thread.cpp b/services/vr/sensord/sensor_hal_thread.cpp
deleted file mode 100644
index c321d4f..0000000
--- a/services/vr/sensord/sensor_hal_thread.cpp
+++ /dev/null
@@ -1,158 +0,0 @@
-#include "sensor_hal_thread.h"
-
-#include <dvr/performance_client_api.h>
-#include <log/log.h>
-
-namespace android {
-namespace dvr {
-
-SensorHalThread::SensorHalThread(bool* out_success)
- : shutting_down_(false),
- paused_(false),
- sensor_module_(nullptr),
- sensor_device_(nullptr),
- sensor_list_(nullptr) {
- // Assume failure; we will change this to true on success.
- *out_success = false;
-
- // TODO(segal): module & device should be singletons.
- int32_t err = hw_get_module_by_class(SENSORS_HARDWARE_MODULE_ID, "platform",
- (hw_module_t const**)&sensor_module_);
-
- if (err) {
- ALOGE("couldn't load %s module (%s)", SENSORS_HARDWARE_MODULE_ID,
- strerror(-err));
- return;
- }
-
- err = sensors_open_1(&sensor_module_->common, &sensor_device_);
- if (err) {
- ALOGE("couldn't open device for module %s (%s)", SENSORS_HARDWARE_MODULE_ID,
- strerror(-err));
- return;
- }
-
- const int sensor_count =
- sensor_module_->get_sensors_list(sensor_module_, &sensor_list_);
-
- // Deactivate all of the sensors initially.
- sensor_user_count_.resize(sensor_count, 0);
- for (int i = 0; i < sensor_count; ++i) {
- err = sensor_device_->activate(
- reinterpret_cast<struct sensors_poll_device_t*>(sensor_device_),
- sensor_list_[i].handle, 0);
-
- if (err) {
- ALOGE("failed to deactivate sensor %d (%s)", i, strerror(-err));
- return;
- }
- }
-
- // At this point, we've successfully initialized everything.
- *out_success = true;
-}
-
-SensorHalThread::~SensorHalThread() {
- {
- std::unique_lock<std::mutex> lock(mutex_);
- shutting_down_ = true;
- condition_.notify_one();
- }
-
- // Implicitly joins *thread_ if it's running.
-}
-
-void SensorHalThread::StartPolling(const EventConsumer& consumer) {
- if (thread_) {
- ALOGE("SensorHalThread::Start() called but thread is already running!");
- return;
- }
-
- thread_.reset(new std::thread([this, consumer] {
- const int priority_error = dvrSetSchedulerClass(0, "sensors:high");
- LOG_ALWAYS_FATAL_IF(
- priority_error < 0,
- "SensorHalTread::StartPolling: Failed to set scheduler class: %s",
- strerror(-priority_error));
-
- for (;;) {
- for (;;) {
- std::unique_lock<std::mutex> lock(mutex_);
- if (shutting_down_)
- return;
- if (!paused_)
- break;
- condition_.wait(lock);
- }
- const int kMaxEvents = 100;
- sensors_event_t events[kMaxEvents];
- ssize_t event_count = 0;
- do {
- if (sensor_device_) {
- event_count = sensor_device_->poll(
- reinterpret_cast<struct sensors_poll_device_t*>(sensor_device_),
- events, kMaxEvents);
- } else {
- // When there is no sensor_device_, we still call the consumer at
- // regular intervals in case mock poses are in use. Note that this
- // will never be the case for production devices, but this helps
- // during bringup.
- usleep(5000);
- }
- } while (event_count == -EINTR);
- if (event_count == kMaxEvents)
- ALOGI("max events (%d) reached", kMaxEvents);
-
- if (event_count >= 0) {
- consumer(events, events + event_count);
- } else {
- ALOGE(
- "SensorHalThread::StartPolling: Error while polling sensor: %s "
- "(%zd)",
- strerror(-event_count), -event_count);
- }
- }
- }));
-}
-
-void SensorHalThread::SetPaused(bool is_paused) {
- std::unique_lock<std::mutex> lock(mutex_);
- paused_ = is_paused;
- condition_.notify_one();
-}
-
-void SensorHalThread::StartUsingSensor(const int sensor_index) {
- if (sensor_index < 0 || sensor_index >= GetSensorCount()) {
- ALOGE("StartUsingSensor(): sensor index %d out of range [0, %d)",
- sensor_index, GetSensorCount());
- return;
- }
-
- std::lock_guard<std::mutex> guard(user_count_mutex_);
- if (sensor_user_count_[sensor_index]++ == 0) {
- sensor_device_->activate(
- reinterpret_cast<struct sensors_poll_device_t*>(sensor_device_),
- sensor_list_[sensor_index].handle, 1);
- sensor_device_->setDelay(
- reinterpret_cast<struct sensors_poll_device_t*>(sensor_device_),
- sensor_list_[sensor_index].handle, 0);
- }
-}
-
-void SensorHalThread::StopUsingSensor(const int sensor_index) {
- if (sensor_index < 0 || sensor_index >= GetSensorCount()) {
- ALOGE("StopUsingSensor(): sensor index %d out of range [0, %d)",
- sensor_index, GetSensorCount());
- return;
- }
-
- std::lock_guard<std::mutex> guard(user_count_mutex_);
- if (--sensor_user_count_[sensor_index] == 0) {
- sensor_device_->activate(
- reinterpret_cast<struct sensors_poll_device_t*>(sensor_device_),
- sensor_list_[sensor_index].handle, 0);
- }
-}
-
-} // namespace dvr
-} // namespace android
diff --git a/services/vr/sensord/sensor_hal_thread.h b/services/vr/sensord/sensor_hal_thread.h
deleted file mode 100644
index 9220757..0000000
--- a/services/vr/sensord/sensor_hal_thread.h
+++ /dev/null
@@ -1,99 +0,0 @@
-#ifndef ANDROID_DVR_SENSORD_SENSOR_HAL_THREAD_H_
-#define ANDROID_DVR_SENSORD_SENSOR_HAL_THREAD_H_
-
-#include <hardware/sensors.h>
-
-#include <atomic>
-#include <memory>
-#include <mutex>
-#include <thread>
-#include <vector>
-
-#include "sensor_thread.h"
-
-namespace android {
-namespace dvr {
-
-// Manages initialization and polling of the sensor HAL. Polling is performed
-// continuously on a thread that passes events along to an arbitrary consumer.
-// All const member functions are thread-safe; otherwise, thread safety is noted
-// for each function.
-class SensorHalThread : public SensorThread {
- public:
- // Initializes the sensor HAL, but does not yet start polling (see Start()
- // below). Sets *out_success to true on success; otherwise, sets *out_success
- // to false and logs an error.
- explicit SensorHalThread(bool* out_success);
-
- // Tells the polling thread to shut down if it's running, and waits for it to
- // complete its polling loop.
- ~SensorHalThread() override;
-
- // Begins polling on the thread. The provided consumer will be notified of
- // events. Event notification occurs on the polling thread.
- // Calling Start() more than once on an instance of SensorHalThread is
- // invalid.
- void StartPolling(const EventConsumer& consumer) override;
-
- // Set whether the sensor polling thread is paused or not. This is useful
- // while we need to support both 3DoF and 6DoF codepaths. This 3DoF codepath
- // must be paused while the 6DoF codepath is using the IMU event stream.
- void SetPaused(bool is_paused) override;
-
- // Increase the number of users of the given sensor by one. Activates the
- // sensor if it wasn't already active.
- // Safe to call concurrently with any other functions in this class.
- void StartUsingSensor(int sensor_index) override;
-
- // Decrease the number of users of the given sensor by one. Deactivates the
- // sensor if its usage count has dropped to zero.
- // Safe to call concurrently with any other functions in this class.
- void StopUsingSensor(int sensor_index) override;
-
- // The number of sensors that are available. Returns a negative number if
- // initialization failed.
- int GetSensorCount() const override {
- return static_cast<int>(sensor_user_count_.size());
- }
-
- // The underlying sensor HAL data structure for the sensor at the given index.
- int GetSensorType(int index) const override {
- return sensor_list_[index].type;
- }
-
- private:
- // The actual thread on which we consume events.
- std::unique_ptr<std::thread> thread_;
-
- // Mutex for access to shutting_down_ and paused_ members.
- std::mutex mutex_;
-
- // Condition for signaling pause/unpause to the thread.
- std::condition_variable condition_;
-
- // If this member is set to true, the thread will stop running at its next
- // iteration. Only set with the mutex held and signal condition_ when changed.
- bool shutting_down_;
-
- // If this member is set to true, the thread will pause at its next
- // iteration. Only set with the mutex held and signal condition_ when changed.
- bool paused_;
-
- // HAL access
- struct sensors_module_t* sensor_module_;
- sensors_poll_device_1_t* sensor_device_;
-
- // Contiguous array of available sensors, owned by the sensor HAL.
- const sensor_t* sensor_list_;
-
- // Mutex that protects access to sensor_user_count_.data().
- std::mutex user_count_mutex_;
-
- // A count of how many users each sensor has. Protected by user_count_mutex.
- std::vector<int> sensor_user_count_;
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_SENSORD_SENSOR_HAL_THREAD_H_
diff --git a/services/vr/sensord/sensor_ndk_thread.cpp b/services/vr/sensord/sensor_ndk_thread.cpp
deleted file mode 100644
index 9c3abbc..0000000
--- a/services/vr/sensord/sensor_ndk_thread.cpp
+++ /dev/null
@@ -1,269 +0,0 @@
-#include "sensor_ndk_thread.h"
-
-#include <dvr/performance_client_api.h>
-#include <log/log.h>
-
-namespace android {
-namespace dvr {
-
-namespace {
-static constexpr int kLooperIdUser = 5;
-} // namespace
-
-SensorNdkThread::SensorNdkThread(bool* out_success)
- : shutting_down_(false),
- paused_(true),
- thread_started_(false),
- initialization_result_(false),
- looper_(nullptr),
- sensor_manager_(nullptr),
- event_queue_(nullptr),
- sensor_list_(nullptr),
- sensor_count_(0) {
- // Assume failure; we will change this to true on success.
- *out_success = false;
-
- // These structs are the same, but sanity check the sizes.
- static_assert(sizeof(sensors_event_t) == sizeof(ASensorEvent),
- "Error: sizeof(sensors_event_t) != sizeof(ASensorEvent)");
-
- thread_.reset(new std::thread([this] {
- const int priority_error = dvrSetSchedulerClass(0, "sensors:high");
- LOG_ALWAYS_FATAL_IF(
- priority_error < 0,
- "SensorHalTread::StartPolling: Failed to set scheduler class: %s",
- strerror(-priority_error));
-
- // Start ALooper and initialize sensor access.
- {
- std::unique_lock<std::mutex> lock(mutex_);
- InitializeSensors();
- thread_started_ = true;
- init_condition_.notify_one();
- // Continue on failure - the loop below will periodically retry.
- }
-
- EventConsumer consumer;
- for (;;) {
- for (;;) {
- std::unique_lock<std::mutex> lock(mutex_);
- UpdateSensorUse();
- if (!consumer)
- consumer = consumer_;
- if (shutting_down_)
- return;
- if (!paused_)
- break;
- condition_.wait(lock);
- }
-
- constexpr int kMaxEvents = 100;
- sensors_event_t events[kMaxEvents];
- ssize_t event_count = 0;
- if (initialization_result_) {
- int poll_fd, poll_events;
- void* poll_source;
- // Poll for events.
- int ident = ALooper_pollAll(-1, &poll_fd, &poll_events, &poll_source);
-
- if (ident != kLooperIdUser)
- continue;
-
- ASensorEvent* event = reinterpret_cast<ASensorEvent*>(&events[0]);
- event_count =
- ASensorEventQueue_getEvents(event_queue_, event, kMaxEvents);
-
- if (event_count == 0) {
- ALOGE("Detected sensor service failure, restarting sensors");
- // This happens when sensorservice has died and restarted. To avoid
- // spinning we need to restart the sensor access.
- DestroySensors();
- }
- } else {
- // When there is no sensor_device_, we still call the consumer at
- // regular intervals in case mock poses are in use. Note that this
- // will never be the case for production devices, but this helps
- // during bringup.
- usleep(5000);
- }
- if (event_count == kMaxEvents)
- ALOGI("max events (%d) reached", kMaxEvents);
-
- if (event_count >= 0) {
- consumer(events, events + event_count);
- } else {
- ALOGE(
- "SensorNdkThread::StartPolling: Error while polling sensor: %s "
- "(%zd)",
- strerror(-event_count), -event_count);
- }
- }
-
- // About to exit sensor thread, destroy sensor objects.
- DestroySensors();
- }));
-
- // Wait for thread to startup and initialize sensors so that we know whether
- // it succeeded.
- {
- std::unique_lock<std::mutex> lock(mutex_);
- while (!thread_started_)
- init_condition_.wait(lock);
- }
-
- // At this point, we've successfully initialized everything.
- // The NDK sensor thread will continue to retry on error, so assume success here.
- *out_success = true;
-}
-
-SensorNdkThread::~SensorNdkThread() {
- {
- if (looper_)
- ALooper_wake(looper_);
- std::unique_lock<std::mutex> lock(mutex_);
- shutting_down_ = true;
- condition_.notify_one();
- }
-
- thread_->join();
-}
-
-bool SensorNdkThread::InitializeSensors() {
- looper_ = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
- if (!looper_) {
- ALOGE("Failed to create ALooper.");
- return false;
- }
-
- // Prepare to monitor accelerometer
- sensor_manager_ = ASensorManager_getInstanceForPackage(nullptr);
- if (!sensor_manager_) {
- ALOGE("Failed to create ASensorManager.");
- return false;
- }
-
- event_queue_ = ASensorManager_createEventQueue(
- sensor_manager_, looper_, kLooperIdUser, nullptr, nullptr);
- if (!event_queue_) {
- ALOGE("Failed to create sensor EventQueue.");
- return false;
- }
-
- sensor_count_ = ASensorManager_getSensorList(sensor_manager_, &sensor_list_);
- ALOGI("Sensor count %d", sensor_count_);
-
- sensor_user_count_.resize(sensor_count_, 0);
-
- // To recover from sensorservice restart, enable the sensors that are already
- // requested.
- for (size_t sensor_index = 0; sensor_index < sensor_user_count_.size();
- ++sensor_index) {
- if (sensor_user_count_[sensor_index] > 0) {
- int result = ASensorEventQueue_registerSensor(
- event_queue_, sensor_list_[sensor_index], 0, 0);
- ALOGE_IF(result < 0, "ASensorEventQueue_registerSensor failed: %d",
- result);
- }
- }
-
- initialization_result_ = true;
- return true;
-}
-
-void SensorNdkThread::DestroySensors() {
- if (!event_queue_)
- return;
- for (size_t sensor_index = 0; sensor_index < sensor_user_count_.size();
- ++sensor_index) {
- if (sensor_user_count_[sensor_index] > 0) {
- ASensorEventQueue_disableSensor(event_queue_, sensor_list_[sensor_index]);
- }
- }
- ASensorManager_destroyEventQueue(sensor_manager_, event_queue_);
- event_queue_ = nullptr;
- initialization_result_ = false;
-}
-
-void SensorNdkThread::UpdateSensorUse() {
- if (!initialization_result_) {
- // Sleep for 1 second to avoid spinning during system instability.
- usleep(1000 * 1000);
- InitializeSensors();
- if (!initialization_result_)
- return;
- }
-
- if (!enable_sensors_.empty()) {
- for (int sensor_index : enable_sensors_) {
- if (sensor_user_count_[sensor_index]++ == 0) {
- int result = ASensorEventQueue_registerSensor(
- event_queue_, sensor_list_[sensor_index], 0, 0);
- ALOGE_IF(result < 0, "ASensorEventQueue_registerSensor failed: %d",
- result);
- }
- }
- enable_sensors_.clear();
- }
-
- if (!disable_sensors_.empty()) {
- for (int sensor_index : disable_sensors_) {
- if (--sensor_user_count_[sensor_index] == 0) {
- int result = ASensorEventQueue_disableSensor(
- event_queue_, sensor_list_[sensor_index]);
- ALOGE_IF(result < 0, "ASensorEventQueue_disableSensor failed: %d",
- result);
- }
- }
- disable_sensors_.clear();
- }
-}
-
-void SensorNdkThread::StartPolling(const EventConsumer& consumer) {
- {
- std::unique_lock<std::mutex> lock(mutex_);
- if (consumer_) {
- ALOGE("Already started sensor thread.");
- return;
- }
- consumer_ = consumer;
- }
- SetPaused(false);
-}
-
-void SensorNdkThread::SetPaused(bool is_paused) {
- std::unique_lock<std::mutex> lock(mutex_);
- // SetPaused may be called before we have StartPolling, make sure we have
- // an event consumer. Otherwise we defer until StartPolling is called.
- if (!consumer_)
- return;
- paused_ = is_paused;
- condition_.notify_one();
- ALooper_wake(looper_);
-}
-
-void SensorNdkThread::StartUsingSensor(const int sensor_index) {
- std::unique_lock<std::mutex> lock(mutex_);
- if (sensor_index < 0 || sensor_index >= sensor_count_) {
- ALOGE("StartUsingSensor(): sensor index %d out of range [0, %d)",
- sensor_index, sensor_count_);
- return;
- }
-
- enable_sensors_.push_back(sensor_index);
- ALooper_wake(looper_);
-}
-
-void SensorNdkThread::StopUsingSensor(const int sensor_index) {
- std::unique_lock<std::mutex> lock(mutex_);
- if (sensor_index < 0 || sensor_index >= sensor_count_) {
- ALOGE("StopUsingSensor(): sensor index %d out of range [0, %d)",
- sensor_index, sensor_count_);
- return;
- }
-
- disable_sensors_.push_back(sensor_index);
- ALooper_wake(looper_);
-}
-
-} // namespace dvr
-} // namespace android
diff --git a/services/vr/sensord/sensor_ndk_thread.h b/services/vr/sensord/sensor_ndk_thread.h
deleted file mode 100644
index eb3cf9d..0000000
--- a/services/vr/sensord/sensor_ndk_thread.h
+++ /dev/null
@@ -1,124 +0,0 @@
-#ifndef ANDROID_DVR_SENSORD_SENSOR_NDK_THREAD_H_
-#define ANDROID_DVR_SENSORD_SENSOR_NDK_THREAD_H_
-
-#include <android/sensor.h>
-#include <hardware/sensors.h>
-
-#include <atomic>
-#include <memory>
-#include <mutex>
-#include <thread>
-#include <vector>
-
-#include "sensor_thread.h"
-
-namespace android {
-namespace dvr {
-
-// Manages initialization and polling of the sensor data. Polling is performed
-// continuously on a thread that passes events along to an arbitrary consumer.
-// All const member functions are thread-safe; otherwise, thread safety is noted
-// for each function.
-class SensorNdkThread : public SensorThread {
- public:
- // Initializes the sensor access, but does not yet start polling (see Start()
- // below). Sets *out_success to true on success; otherwise, sets *out_success
- // to false and logs an error.
- explicit SensorNdkThread(bool* out_success);
-
- // Tells the polling thread to shut down if it's running, and waits for it to
- // complete its polling loop.
- ~SensorNdkThread() override;
-
- // Begins polling on the thread. The provided consumer will be notified of
- // events. Event notification occurs on the polling thread.
- // Calling Start() more than once on an instance of SensorNdkThread is
- // invalid.
- void StartPolling(const EventConsumer& consumer) override;
-
- // Set whether the sensor polling thread is paused or not. This is useful
- // while we need to support both 3DoF and 6DoF codepaths. This 3DoF codepath
- // must be paused while the 6DoF codepath is using the IMU event stream.
- void SetPaused(bool is_paused) override;
-
- // Increase the number of users of the given sensor by one. Activates the
- // sensor if it wasn't already active.
- // Safe to call concurrently with any other functions in this class.
- void StartUsingSensor(int sensor_index) override;
-
- // Decrease the number of users of the given sensor by one. Deactivates the
- // sensor if its usage count has dropped to zero.
- // Safe to call concurrently with any other functions in this class.
- void StopUsingSensor(int sensor_index) override;
-
- // The number of sensors that are available. Returns a negative number if
- // initialization failed.
- int GetSensorCount() const override { return sensor_count_; }
-
- // The underlying sensor HAL data structure for the sensor at the given index.
- int GetSensorType(int index) const override {
- return ASensor_getType(sensor_list_[index]);
- }
-
- private:
- // Initialize ALooper and sensor access on the thread.
- // Returns true on success, false on failure.
- bool InitializeSensors();
-
- // Destroy sensor access.
- void DestroySensors();
-
- // Start or stop requested sensors from the thread. Class mutex must already
- // be locked.
- void UpdateSensorUse();
-
- // The actual thread on which we consume events.
- std::unique_ptr<std::thread> thread_;
-
- // Mutex for access to shutting_down_ and paused_ members.
- std::mutex mutex_;
-
- // Condition for signaling pause/unpause to the thread.
- std::condition_variable condition_;
-
- // Condition for signaling thread initialization.
- std::condition_variable init_condition_;
-
- // If this member is set to true, the thread will stop running at its next
- // iteration. Only set with the mutex held and signal condition_ when changed.
- bool shutting_down_;
-
- // If this member is set to true, the thread will pause at its next
- // iteration. Only set with the mutex held and signal condition_ when changed.
- bool paused_;
-
- // Thread start hand shake to verify that sensor initialization succeeded.
- bool thread_started_;
-
- // Initialization result (true for success).
- bool initialization_result_;
-
- // The callback.
- EventConsumer consumer_;
-
- // Sensor access
- ALooper* looper_;
- ASensorManager* sensor_manager_;
- ASensorEventQueue* event_queue_;
-
- // Sensor list from NDK.
- ASensorList sensor_list_;
- int sensor_count_;
-
- // Requests to the sensor thread to enable or disable given sensors.
- std::vector<int> enable_sensors_;
- std::vector<int> disable_sensors_;
-
- // A count of how many users each sensor has. Protected by user_count_mutex.
- std::vector<int> sensor_user_count_;
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_SENSORD_SENSOR_NDK_THREAD_H_
diff --git a/services/vr/sensord/sensor_service.cpp b/services/vr/sensord/sensor_service.cpp
deleted file mode 100644
index a182a26..0000000
--- a/services/vr/sensord/sensor_service.cpp
+++ /dev/null
@@ -1,184 +0,0 @@
-#include "sensor_service.h"
-
-#include <hardware/sensors.h>
-#include <log/log.h>
-#include <pdx/default_transport/service_endpoint.h>
-#include <poll.h>
-#include <private/dvr/sensor-ipc.h>
-#include <time.h>
-
-using android::pdx::default_transport::Endpoint;
-
-namespace android {
-namespace dvr {
-
-SensorService::SensorService(SensorThread* sensor_thread)
- : BASE("SensorService", Endpoint::Create(DVR_SENSOR_SERVICE_CLIENT)),
- sensor_thread_(sensor_thread) {
- sensor_clients_.resize(sensor_thread_->GetSensorCount());
-
- for (int i = 0; i < sensor_thread_->GetSensorCount(); ++i)
- type_to_sensor_[sensor_thread_->GetSensorType(i)] = i;
-}
-
-std::shared_ptr<pdx::Channel> SensorService::OnChannelOpen(pdx::Message& msg) {
- std::lock_guard<std::mutex> guard(mutex_);
-
- const pdx::MessageInfo& info = msg.GetInfo();
-
- std::shared_ptr<SensorClient> client(
- new SensorClient(*this, info.pid, info.cid));
- AddClient(client);
- return client;
-}
-
-void SensorService::OnChannelClose(pdx::Message& /*msg*/,
- const std::shared_ptr<pdx::Channel>& chan) {
- std::lock_guard<std::mutex> guard(mutex_);
-
- auto client = std::static_pointer_cast<SensorClient>(chan);
- if (!client) {
- ALOGW("WARNING: SensorClient was NULL!\n");
- return;
- }
- RemoveClient(client);
-}
-
-void SensorService::AddClient(const std::shared_ptr<SensorClient>& client) {
- clients_.push_front(client);
-}
-
-void SensorService::RemoveClient(const std::shared_ptr<SensorClient>& client) {
- // First remove it from the clients associated with its sensor, if any.
- RemoveSensorClient(client.get());
-
- // Finally, remove it from the list of clients we're aware of, and decrease
- // its reference count.
- clients_.remove(client);
-}
-
-void SensorService::RemoveSensorClient(SensorClient* client) {
- if (!client->has_sensor())
- return;
-
- std::forward_list<SensorClient*>& sensor_clients =
- sensor_clients_[client->sensor()];
- sensor_clients.remove(client);
- sensor_thread_->StopUsingSensor(client->sensor());
-
- client->unset_sensor();
-}
-
-pdx::Status<void> SensorService::HandleMessage(pdx::Message& msg) {
- pdx::Status<void> ret;
- const pdx::MessageInfo& info = msg.GetInfo();
- switch (info.op) {
- case DVR_SENSOR_START: {
- std::lock_guard<std::mutex> guard(mutex_);
- // Associate this channel with the indicated sensor,
- // unless it already has an association. In that case,
- // fail.
- auto client = std::static_pointer_cast<SensorClient>(msg.GetChannel());
- if (client->has_sensor())
- REPLY_ERROR(msg, EINVAL, error);
- int sensor_type;
- if (!msg.ReadAll(&sensor_type, sizeof(sensor_type)))
- REPLY_ERROR(msg, EIO, error);
-
- // Find the sensor of the requested type.
- if (type_to_sensor_.find(sensor_type) == type_to_sensor_.end())
- REPLY_ERROR(msg, EINVAL, error);
- const int sensor_index = type_to_sensor_[sensor_type];
-
- sensor_clients_[sensor_index].push_front(client.get());
- client->set_sensor(sensor_index);
- sensor_thread_->StartUsingSensor(sensor_index);
-
- REPLY_SUCCESS(msg, 0, error);
- }
- case DVR_SENSOR_STOP: {
- std::lock_guard<std::mutex> guard(mutex_);
- auto client = std::static_pointer_cast<SensorClient>(msg.GetChannel());
- if (!client->has_sensor())
- REPLY_ERROR(msg, EINVAL, error);
- RemoveSensorClient(client.get());
- REPLY_SUCCESS(msg, 0, error);
- }
- case DVR_SENSOR_POLL: {
- std::lock_guard<std::mutex> guard(mutex_);
- auto client = std::static_pointer_cast<SensorClient>(msg.GetChannel());
-
- // Package up the events we've got for this client. Number of
- // events, followed by 0 or more sensor events, popped from
- // this client's queue until it's empty.
- int num_events = client->EventCount();
- sensors_event_t out_buffer[num_events];
- client->WriteEvents(out_buffer);
- struct iovec svec[] = {
- {.iov_base = &num_events, .iov_len = sizeof(num_events)},
- {.iov_base = out_buffer,
- .iov_len = num_events * sizeof(sensors_event_t)},
- };
- ret = msg.WriteVectorAll(svec, 2);
- if (!ret) {
- REPLY_ERROR(msg, EIO, error);
- }
- REPLY_SUCCESS(msg, 0, error);
- }
- default:
- // Do not lock mutex_ here, because this may call the on*() handlers,
- // which will lock the mutex themselves.
- ret = Service::HandleMessage(msg);
- break;
- }
-error:
- return ret;
-}
-
-void SensorService::EnqueueEvents(const sensors_event_t* begin_events,
- const sensors_event_t* end_events) {
- std::lock_guard<std::mutex> guard(mutex_);
-
- // Put the sensor values we got in the circular queue for each client that
- // cares about the given event.
- for (const sensors_event_t* event = begin_events; event != end_events;
- ++event) {
- const int sensor_index = type_to_sensor_[event->type];
- for (const auto& client : sensor_clients_[sensor_index]) {
- client->EnqueueEvent(*event);
- }
- }
-}
-
-void SensorClient::WriteEvents(sensors_event_t* buffer) {
- while (!event_queue_.Empty()) {
- *buffer = *(event_queue_.Top());
- event_queue_.Pop();
- ++buffer;
- }
-}
-
-void SensorClient::CircularQ::Push(const sensors_event_t& event) {
- if (count_ != 0 && head_ == tail_) {
- Pop(); // If we're full, throw away the oldest event.
- }
- events_[head_] = event;
- head_ = (head_ + 1) % kCqSize;
- ++count_;
-}
-
-const sensors_event_t* SensorClient::CircularQ::Top() const {
- if (count_ == 0)
- return nullptr;
- return &events_[tail_];
-}
-
-void SensorClient::CircularQ::Pop() {
- if (count_ == 0)
- return;
- tail_ = (tail_ + 1) % kCqSize;
- --count_;
-}
-
-} // namespace dvr
-} // namespace android
diff --git a/services/vr/sensord/sensor_service.h b/services/vr/sensord/sensor_service.h
deleted file mode 100644
index 6ea470b..0000000
--- a/services/vr/sensord/sensor_service.h
+++ /dev/null
@@ -1,132 +0,0 @@
-#ifndef ANDROID_DVR_SENSORD_SENSOR_SERVICE_H_
-#define ANDROID_DVR_SENSORD_SENSOR_SERVICE_H_
-
-#include <forward_list>
-#include <unordered_map>
-#include <vector>
-
-#include <pdx/service.h>
-#include <pthread.h>
-
-#include "sensor_thread.h"
-
-namespace android {
-namespace dvr {
-
-class SensorClient;
-
-/*
- * SensorService implements the sensor service over ServiceFS.
- * The sensor service provides an interface to one sensor over
- * each channel.
- */
-class SensorService : public pdx::ServiceBase<SensorService> {
- public:
- pdx::Status<void> HandleMessage(pdx::Message& msg) override;
- std::shared_ptr<pdx::Channel> OnChannelOpen(pdx::Message& msg) override;
- void OnChannelClose(pdx::Message& msg,
- const std::shared_ptr<pdx::Channel>& chan) override;
-
- // Enqueue the events in [begin_events, end_events) onto any clients that care
- // about them.
- // Safe to call concurrently with any other public member functions.
- void EnqueueEvents(const sensors_event_t* begin_events,
- const sensors_event_t* end_events);
-
- private:
- friend BASE;
-
- // Initializes the service. Keeps a reference to sensor_thread, which must be
- // non-null.
- explicit SensorService(SensorThread* sensor_thread);
-
- // The abstraction around the sensor HAL.
- SensorThread* sensor_thread_;
-
- // All of the clients we are connected to. This is the one place in this class
- // where we keep the SensorClient instances alive using shared_ptr instances.
- std::forward_list<std::shared_ptr<SensorClient>> clients_;
-
- // Map types back to sensor indexes.
- std::unordered_map<int, int> type_to_sensor_;
- // For each sensor, the list of clients that are connected to it.
- // Every entry in here must also be in clients_, so that its reference count
- // remains positive.
- std::vector<std::forward_list<SensorClient*>> sensor_clients_;
-
- // Protects access to all member variables.
- std::mutex mutex_;
-
- // None of the following functions is thread-safe; callers must lock mutex_
- // before calling one.
- void AddClient(const std::shared_ptr<SensorClient>& client);
- void RemoveClient(const std::shared_ptr<SensorClient>& client);
- // Dissociate the indicated client from its sensor, if it has one; otherwise
- // do nothing.
- void RemoveSensorClient(SensorClient* client);
-
- SensorService(const SensorService&) = delete;
- void operator=(const SensorService&) = delete;
-};
-
-/*
- * SensorClient manages the service-side per-client context for each client
- * using the service.
- */
-class SensorClient : public pdx::Channel {
- public:
- SensorClient(SensorService& /*service*/, int /*pid*/, int /*cid*/)
- : sensor_index_(-1), has_sensor_index_(false) {}
-
- bool has_sensor() const { return has_sensor_index_; }
- int sensor() const { return sensor_index_; }
- void set_sensor(int sensor) {
- sensor_index_ = sensor;
- has_sensor_index_ = true;
- }
- void unset_sensor() {
- sensor_index_ = -1;
- has_sensor_index_ = false;
- }
-
- int EventCount() const { return event_queue_.Count(); }
-
- // Push an event onto our queue.
- void EnqueueEvent(const sensors_event_t& event) { event_queue_.Push(event); }
-
- // Write all the events in our queue (and clear it) to the supplied
- // buffer. Buffer must be large enough.
- void WriteEvents(sensors_event_t* buffer);
-
- private:
- SensorClient(const SensorClient&) = delete;
- SensorClient& operator=(const SensorClient&) = delete;
-
- int sensor_index_ = -1;
- bool has_sensor_index_ = false;
- // Circular queue holds as-yet-unasked-for events for the sensor associated
- // with this client.
- class CircularQ {
- public:
- static const int kCqSize = 10;
- CircularQ() : head_(0), tail_(0), count_(0) {}
- ~CircularQ() {}
- void Push(const sensors_event_t& event);
- const sensors_event_t* Top() const;
- void Pop();
- bool Empty() const { return count_ == 0; }
- int Count() const { return count_; }
-
- private:
- sensors_event_t events_[kCqSize];
- int head_ = 0;
- int tail_ = 0;
- int count_ = 0;
- };
- CircularQ event_queue_;
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_SENSORD_SENSOR_SERVICE_H_
diff --git a/services/vr/sensord/sensor_thread.cpp b/services/vr/sensord/sensor_thread.cpp
deleted file mode 100644
index 01e4e7e..0000000
--- a/services/vr/sensord/sensor_thread.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include "sensor_thread.h"
-
-namespace android {
-namespace dvr {
-
-SensorThread::~SensorThread() {}
-
-} // namespace dvr
-} // namespace android
diff --git a/services/vr/sensord/sensor_thread.h b/services/vr/sensord/sensor_thread.h
deleted file mode 100644
index 46aba17..0000000
--- a/services/vr/sensord/sensor_thread.h
+++ /dev/null
@@ -1,58 +0,0 @@
-#ifndef ANDROID_DVR_SENSORD_SENSOR_THREAD_H_
-#define ANDROID_DVR_SENSORD_SENSOR_THREAD_H_
-
-#include <hardware/sensors.h>
-
-#include <functional>
-
-namespace android {
-namespace dvr {
-
-// Manages initialization and polling of the sensor data. Polling is performed
-// continuously on a thread that passes events along to an arbitrary consumer.
-// All const member functions are thread-safe; otherwise, thread safety is noted
-// for each function.
-class SensorThread {
- public:
- // A function type that can be called to provide it with new events.
- // [events_begin, events_end) forms a contiguous array of events.
- using EventConsumer = std::function<void(const sensors_event_t* events_begin,
- const sensors_event_t* events_end)>;
-
- // Tells the polling thread to shut down if it's running, and waits for it to
- // complete its polling loop.
- virtual ~SensorThread();
-
- // Begins polling on the thread. The provided consumer will be notified of
- // events. Event notification occurs on the polling thread.
- // Calling Start() more than once on an instance of SensorThread is
- // invalid.
- virtual void StartPolling(const EventConsumer& consumer) = 0;
-
- // Set whether the sensor polling thread is paused or not. This is useful
- // while we need to support both 3DoF and 6DoF codepaths. This 3DoF codepath
- // must be paused while the 6DoF codepath is using the IMU event stream.
- virtual void SetPaused(bool is_paused) = 0;
-
- // Increase the number of users of the given sensor by one. Activates the
- // sensor if it wasn't already active.
- // Safe to call concurrently with any other functions in this class.
- virtual void StartUsingSensor(int sensor_index) = 0;
-
- // Decrease the number of users of the given sensor by one. Deactivates the
- // sensor if its usage count has dropped to zero.
- // Safe to call concurrently with any other functions in this class.
- virtual void StopUsingSensor(int sensor_index) = 0;
-
- // The number of sensors that are available. Returns a negative number if
- // initialization failed.
- virtual int GetSensorCount() const = 0;
-
- // Get the sensor type for the sensor at the given index.
- virtual int GetSensorType(int index) const = 0;
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_SENSORD_SENSOR_THREAD_H_
diff --git a/services/vr/sensord/sensord.cpp b/services/vr/sensord/sensord.cpp
deleted file mode 100644
index db39152..0000000
--- a/services/vr/sensord/sensord.cpp
+++ /dev/null
@@ -1,91 +0,0 @@
-#define LOG_TAG "sensord"
-
-#include <string.h>
-
-#include <binder/ProcessState.h>
-
-#include <dvr/performance_client_api.h>
-#include <pdx/default_transport/service_dispatcher.h>
-#include <private/dvr/pose-ipc.h>
-#include <private/dvr/sensor-ipc.h>
-
-#include "pose_service.h"
-#include "sensor_hal_thread.h"
-#include "sensor_ndk_thread.h"
-#include "sensor_service.h"
-#include "sensor_thread.h"
-#include "sensord_extension.h"
-
-using android::dvr::PoseService;
-using android::dvr::SensorHalThread;
-using android::dvr::SensorNdkThread;
-using android::dvr::SensorService;
-using android::dvr::SensorThread;
-using android::pdx::Service;
-using android::pdx::ServiceDispatcher;
-using android::dvr::SensordExtension;
-
-int main(int, char**) {
- ALOGI("Starting up...");
-
- SensordExtension::run();
-
- // We need to be able to create endpoints with full perms.
- umask(0000);
-
- android::ProcessState::self()->startThreadPool();
-
- bool sensor_thread_succeeded = false;
-#ifdef SENSORD_USES_HAL
- std::unique_ptr<SensorThread> sensor_thread(
- new SensorHalThread(&sensor_thread_succeeded));
-#else
- std::unique_ptr<SensorThread> sensor_thread(
- new SensorNdkThread(&sensor_thread_succeeded));
-#endif
-
- if (!sensor_thread_succeeded) {
- ALOGE("ERROR: Failed to initialize SensorThread! No 3DoF!\n");
- }
-
- if (sensor_thread->GetSensorCount() == 0)
- ALOGW("No sensors found\n");
-
- auto sensor_service = SensorService::Create(sensor_thread.get());
- if (!sensor_service) {
- ALOGE("TERMINATING: failed to create SensorService!!!\n");
- return -1;
- }
-
- auto pose_service = PoseService::Create(sensor_thread.get());
- if (!pose_service) {
- ALOGE("TERMINATING: failed to create PoseService!!!\n");
- return -1;
- }
-
- std::unique_ptr<ServiceDispatcher> dispatcher =
- android::pdx::default_transport::ServiceDispatcher::Create();
- if (!dispatcher) {
- ALOGE("TERMINATING: failed to create ServiceDispatcher!!!\n");
- return -1;
- }
-
- dispatcher->AddService(sensor_service);
- dispatcher->AddService(pose_service);
-
- sensor_thread->StartPolling([sensor_service, pose_service](
- const sensors_event_t* events_begin, const sensors_event_t* events_end) {
- sensor_service->EnqueueEvents(events_begin, events_end);
- pose_service->HandleEvents(events_begin, events_end);
- });
-
- const int priority_error = dvrSetSchedulerClass(0, "sensors:low");
- LOG_ALWAYS_FATAL_IF(priority_error < 0,
- "SensorService: Failed to set scheduler class: %s",
- strerror(-priority_error));
-
- int ret = dispatcher->EnterDispatchLoop();
- ALOGI("Dispatch loop exited because: %s\n", strerror(-ret));
-
- return ret;
-}
diff --git a/services/vr/sensord/sensord.rc b/services/vr/sensord/sensord.rc
deleted file mode 100644
index 36cd377..0000000
--- a/services/vr/sensord/sensord.rc
+++ /dev/null
@@ -1,11 +0,0 @@
-on init
- mkdir /dev/socket/pdx/system/vr/pose 0775 system system
- mkdir /dev/socket/pdx/system/vr/sensors 0775 system system
-
-service sensord /system/bin/sensord
- class core
- user system
- group system camera sdcard_rw
- writepid /dev/cpuset/system/tasks
- socket pdx/system/vr/sensors/client stream 0666 system system
- socket pdx/system/vr/pose/client stream 0666 system system
diff --git a/services/vr/sensord/sensord_extension.cpp b/services/vr/sensord/sensord_extension.cpp
deleted file mode 100644
index 6cd7db3..0000000
--- a/services/vr/sensord/sensord_extension.cpp
+++ /dev/null
@@ -1,4 +0,0 @@
-#include "sensord_extension.h"
-
-void android::dvr::SensordExtension::run() {
-}
diff --git a/services/vr/sensord/sensord_extension.h b/services/vr/sensord/sensord_extension.h
deleted file mode 100644
index e553eed..0000000
--- a/services/vr/sensord/sensord_extension.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#ifndef ANDROID_DVR_SENSORD_EXTENSION_H_
-#define ANDROID_DVR_SENSORD_EXTENSION_H_
-
-namespace android {
-namespace dvr {
-
-// Allows sensord to be extended with additional code.
-class SensordExtension {
- public:
- static void run();
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_SENSORD_EXTENSION_H_
diff --git a/services/vr/sensord/test/poselatencytest.cpp b/services/vr/sensord/test/poselatencytest.cpp
deleted file mode 100644
index 615fc75..0000000
--- a/services/vr/sensord/test/poselatencytest.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-#include <dvr/pose_client.h>
-#include <inttypes.h>
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <time.h>
-#include <vector>
-
-// Creates a pose client and polls 30x for new data. Prints timestamp and
-// latency. Latency is calculated based on the difference between the
-// current clock and the timestamp from the Myriad, which has been synced
-// to QC time. Note that there is some clock drift and clocks are only sycned
-// when the FW is loaded.
-int main(int /*argc*/, char** /*argv*/) {
- DvrPose* pose_client = dvrPoseCreate();
- if (pose_client == nullptr) {
- printf("Unable to create pose client\n");
- return -1;
- }
-
- DvrPoseAsync last_state;
- DvrPoseAsync current_state;
- last_state.timestamp_ns = 0;
- current_state.timestamp_ns = 0;
-
- double avg_latency = 0;
- double min_latency = (float)UINT64_MAX;
- double max_latency = 0;
- double std = 0;
- std::vector<uint64_t> latency;
-
- int num_samples = 100;
- for (int i = 0; i < num_samples; ++i) {
- while (last_state.timestamp_ns == current_state.timestamp_ns) {
- uint32_t vsync_count = dvrPoseGetVsyncCount(pose_client);
- int err = dvrPoseGet(pose_client, vsync_count, ¤t_state);
- if (err) {
- printf("Error polling pose: %d\n", err);
- dvrPoseDestroy(pose_client);
- return err;
- }
- }
- struct timespec timespec;
- uint64_t timestamp, diff;
- clock_gettime(CLOCK_MONOTONIC, ×pec);
- timestamp =
- ((uint64_t)timespec.tv_sec * 1000000000) + (uint64_t)timespec.tv_nsec;
- if (timestamp < current_state.timestamp_ns) {
- printf("ERROR: excessive clock drift detected, reload FW to resync\n");
- return -1;
- }
- diff = timestamp - current_state.timestamp_ns;
- printf("%02d) ts = %" PRIu64 " time = %" PRIu64 "\n", i + 1,
- current_state.timestamp_ns, timestamp);
- printf("\tlatency: %" PRIu64 " ns (%" PRIu64 " us) (%" PRIu64 " ms)\n",
- diff, diff / 1000, diff / 1000000);
-
- avg_latency += diff;
- if (diff < min_latency) {
- min_latency = diff;
- }
- if (diff > max_latency) {
- max_latency = diff;
- }
- latency.push_back(diff);
-
- last_state = current_state;
- }
- avg_latency /= num_samples;
- for (unsigned int i = 0; i < latency.size(); i++) {
- std += pow(latency[i] - avg_latency, 2);
- }
- std /= latency.size();
- std = sqrt(std);
-
- printf("\n************************\n");
- printf("Avg latency = %lf ns (%lf us) (%lf ms)\n", avg_latency,
- avg_latency / 1000, avg_latency / 1000000);
- printf("Max latency = %lf ns (%lf us) (%lf ms)\n", max_latency,
- max_latency / 1000, max_latency / 1000000);
- printf("Min latency = %lf ns (%lf us) (%lf ms)\n", min_latency,
- min_latency / 1000, min_latency / 1000000);
- printf("Standard dev = %lf ns (%lf us) (%lf ms)\n", std, std / 1000,
- std / 1000000);
- printf("\n************************\n");
- return 0;
-}