From 1d3290a98e9069bd4d79ba9c9fb0dc0feceac306 Mon Sep 17 00:00:00 2001 From: Luke Song Date: Thu, 13 Apr 2017 10:42:48 -0700 Subject: [PATCH] Move sensord Move service to more device-specific directory. Bug: 36996994 Test: Build targets Change-Id: Ie0befa422d6cc114826d6a7e145d7f8d4da61185 --- cmds/vr/pose/Android.mk | 35 - cmds/vr/pose/pose.cpp | 280 -------- services/vr/sensord/Android.mk | 76 -- services/vr/sensord/pose_service.cpp | 697 ------------------- services/vr/sensord/pose_service.h | 168 ----- services/vr/sensord/sensor_fusion.cpp | 348 --------- services/vr/sensord/sensor_fusion.h | 181 ----- services/vr/sensord/sensor_hal_thread.cpp | 158 ----- services/vr/sensord/sensor_hal_thread.h | 99 --- services/vr/sensord/sensor_ndk_thread.cpp | 269 ------- services/vr/sensord/sensor_ndk_thread.h | 124 ---- services/vr/sensord/sensor_service.cpp | 184 ----- services/vr/sensord/sensor_service.h | 132 ---- services/vr/sensord/sensor_thread.cpp | 9 - services/vr/sensord/sensor_thread.h | 58 -- services/vr/sensord/sensord.cpp | 91 --- services/vr/sensord/sensord.rc | 11 - services/vr/sensord/sensord_extension.cpp | 4 - services/vr/sensord/sensord_extension.h | 16 - services/vr/sensord/test/poselatencytest.cpp | 87 --- 20 files changed, 3027 deletions(-) delete mode 100644 cmds/vr/pose/Android.mk delete mode 100644 cmds/vr/pose/pose.cpp delete mode 100644 services/vr/sensord/Android.mk delete mode 100644 services/vr/sensord/pose_service.cpp delete mode 100644 services/vr/sensord/pose_service.h delete mode 100644 services/vr/sensord/sensor_fusion.cpp delete mode 100644 services/vr/sensord/sensor_fusion.h delete mode 100644 services/vr/sensord/sensor_hal_thread.cpp delete mode 100644 services/vr/sensord/sensor_hal_thread.h delete mode 100644 services/vr/sensord/sensor_ndk_thread.cpp delete mode 100644 services/vr/sensord/sensor_ndk_thread.h delete mode 100644 services/vr/sensord/sensor_service.cpp delete mode 100644 services/vr/sensord/sensor_service.h delete mode 100644 services/vr/sensord/sensor_thread.cpp delete mode 100644 services/vr/sensord/sensor_thread.h delete mode 100644 services/vr/sensord/sensord.cpp delete mode 100644 services/vr/sensord/sensord.rc delete mode 100644 services/vr/sensord/sensord_extension.cpp delete mode 100644 services/vr/sensord/sensord_extension.h delete mode 100644 services/vr/sensord/test/poselatencytest.cpp diff --git a/cmds/vr/pose/Android.mk b/cmds/vr/pose/Android.mk deleted file mode 100644 index 8be3214f4e..0000000000 --- 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 faceb67e9c..0000000000 --- a/cmds/vr/pose/pose.cpp +++ /dev/null @@ -1,280 +0,0 @@ -// pose is a utility to query and manipulate the current pose via the pose -// service. - -#include -#include -#include -#include -#include -#include - -#include -#include - -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, float, motion_sickness.\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 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 if (value == "float") { - *mode = DVR_POSE_MODE_FLOAT; - return true; - } else if (value == "motion_sickness") { - *mode = DVR_POSE_MODE_MOCK_MOTION_SICKNESS; - 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 638c9a8dea..0000000000 --- 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 edf4998bce..0000000000 --- a/services/vr/sensord/pose_service.cpp +++ /dev/null @@ -1,697 +0,0 @@ -#define ATRACE_TAG ATRACE_TAG_INPUT -#include "pose_service.h" - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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; - -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"; - case DVR_POSE_MODE_FLOAT: - return "DVR_POSE_MODE_FLOAT"; - case DVR_POSE_MODE_MOCK_MOTION_SICKNESS: - return "DVR_POSE_MODE_MOCK_MOTION_SICKNESS"; - 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(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 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 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 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: - case DVR_POSE_MODE_FLOAT: { - // 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(); - - Vector3d position; - switch (pose_mode_) { - default: - case DVR_POSE_MODE_3DOF: - // 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). - position = start_from_head_rotation * - Vector3d(0.0, kDefaultNeckVerticalOffset, - -kDefaultNeckHorizontalOffset); - break; - case DVR_POSE_MODE_FLOAT: - // Change position a bit in facing direction. - mock_pos_offset_ += start_from_head_rotation.toRotationMatrix() * Vector3d(0, 0, -0.01); - ResetMockDeviatedPosition(); - position = mock_pos_offset_; - break; - } - - // 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; - } - case DVR_POSE_MODE_MOCK_MOTION_SICKNESS: { - double phase = std::sin(current_time_ns / 1e9) + 1; - // Randomize 3rd order rotation axis on phase minimum. - if (phase > mock_prev_phase_ && mock_diff_phase_ < 0) - mock_rot_axis_2_ = RandVector(); - mock_diff_phase_ = phase - mock_prev_phase_; - mock_prev_phase_ = phase; - - // Rotate axes all the way down. - mock_rot_axis_2_ = AngleAxisd(0.004 * phase, mock_rot_axis_3_) * mock_rot_axis_2_; - mock_rot_axis_1_ = AngleAxisd(0.002 * (std::sin(current_time_ns / 5e8 + M_PI / 2) + 1), mock_rot_axis_2_) * mock_rot_axis_1_; - Rotationd rotation = Rotationd(AngleAxisd(fmod(current_time_ns / 2e9, kTwoPi), mock_rot_axis_1_)); - - // Change position a bit. - mock_pos_offset_ += rotation.toRotationMatrix() * Vector3d(0, 0, 0.003 * (std::sin(current_time_ns / 6e8) + 1)); - ResetMockDeviatedPosition(); - - WriteAsyncPoses(mock_pos_offset_, rotation, current_time_ns); - break; - } - default: - case DVR_POSE_MODE_6DOF: - ALOGE("ERROR: invalid pose mode"); - break; - } -} - -void PoseService::ResetMockDeviatedPosition() { - if (mock_pos_offset_[1] < -1) mock_pos_offset_[1] = 2; - if (mock_pos_offset_[1] > 30) mock_pos_offset_[1] = 2; - if (abs(mock_pos_offset_[0]) > 30) mock_pos_offset_[0] = mock_pos_offset_[2] = 0; - if (abs(mock_pos_offset_[2]) > 30) mock_pos_offset_[0] = mock_pos_offset_[2] = 0; -} - -pdx::Status PoseService::HandleMessage(pdx::Message& msg) { - pdx::Status ret; - const pdx::MessageInfo& info = msg.GetInfo(); - switch (info.op) { - case DVR_POSE_NOTIFY_VSYNC: { - std::lock_guard 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 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(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(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 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 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 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 guard(mutex_); - - // Kick the sensor thread, because we have a new consumer. - KickSensorWatchDogThread(); - - Status consumer_channel = - ring_buffer_->CreateConsumer(); - REPLY_MESSAGE(msg, consumer_channel, error); - } - case DVR_POSE_GET_CONTROLLER_RING_BUFFER: { - std::lock_guard guard(mutex_); - REPLY_ERROR(msg, EINVAL, error); - } - case DVR_POSE_LOG_CONTROLLER: { - std::lock_guard 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 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 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; - } else if (mode == DVR_POSE_MODE_MOCK_MOTION_SICKNESS) { - mock_rot_axis_1_ = RandVector(); - mock_rot_axis_2_ = RandVector(); - mock_rot_axis_3_ = RandVector(); - } - - 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 acf41248cd..0000000000 --- a/services/vr/sensord/pose_service.h +++ /dev/null @@ -1,168 +0,0 @@ -#ifndef ANDROID_DVR_SENSORD_POSE_SERVICE_H_ -#define ANDROID_DVR_SENSORD_POSE_SERVICE_H_ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#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 { - public: - ~PoseService() override; - - bool IsInitialized() const override; - pdx::Status 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& start_t_head, - const Eigen::Quaternion& 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_; - - // Position offset for use in pose modes. - Eigen::Vector3d mock_pos_offset_; - - // Phase data for DVR_POSE_MODE_MOCK_MOTION_SICKNESS. - double mock_prev_phase_, mock_diff_phase_; - - // Axis data for DVR_POSE_MODE_MOCK_MOTION_SICKNESS. - Eigen::Vector3d mock_rot_axis_1_, mock_rot_axis_2_, mock_rot_axis_3_; - - // Return a random normalized 3d vector. - static Eigen::Vector3d RandVector() { - Eigen::Vector3d vec = Eigen::Vector3d::Random(); - vec.normalize(); - return vec; - } - - // Reset mock_pos_offset_ if strayed too far - void ResetMockDeviatedPosition(); - - // 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 pose_predictor_; - - // Pose ring buffer. - std::shared_ptr 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 5663ae4ff7..0000000000 --- a/services/vr/sensord/sensor_fusion.cpp +++ /dev/null @@ -1,348 +0,0 @@ -#include "sensor_fusion.h" - -#include -#include - -#include - -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 lock(mutex_); - return current_state_; -} - -void SensorFusion::ProcessGyroscopeSample(float v_x, float v_y, float v_z, - uint64 timestamp_ns) { - std::unique_lock 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(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 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 0ceae21c62..0000000000 --- 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 -#include -#include - -#include - -namespace android { -namespace dvr { - -using Matrix3d = Eigen::Matrix; -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 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 c321d4f6da..0000000000 --- a/services/vr/sensord/sensor_hal_thread.cpp +++ /dev/null @@ -1,158 +0,0 @@ -#include "sensor_hal_thread.h" - -#include -#include - -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(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 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 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(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 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 guard(user_count_mutex_); - if (sensor_user_count_[sensor_index]++ == 0) { - sensor_device_->activate( - reinterpret_cast(sensor_device_), - sensor_list_[sensor_index].handle, 1); - sensor_device_->setDelay( - reinterpret_cast(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 guard(user_count_mutex_); - if (--sensor_user_count_[sensor_index] == 0) { - sensor_device_->activate( - reinterpret_cast(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 92207579d4..0000000000 --- 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 - -#include -#include -#include -#include -#include - -#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(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 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 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 9c3abbce61..0000000000 --- a/services/vr/sensord/sensor_ndk_thread.cpp +++ /dev/null @@ -1,269 +0,0 @@ -#include "sensor_ndk_thread.h" - -#include -#include - -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 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 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(&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 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 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 lock(mutex_); - if (consumer_) { - ALOGE("Already started sensor thread."); - return; - } - consumer_ = consumer; - } - SetPaused(false); -} - -void SensorNdkThread::SetPaused(bool is_paused) { - std::unique_lock 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 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 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 eb3cf9d767..0000000000 --- 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 -#include - -#include -#include -#include -#include -#include - -#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 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 enable_sensors_; - std::vector disable_sensors_; - - // A count of how many users each sensor has. Protected by user_count_mutex. - std::vector 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 a182a262ab..0000000000 --- a/services/vr/sensord/sensor_service.cpp +++ /dev/null @@ -1,184 +0,0 @@ -#include "sensor_service.h" - -#include -#include -#include -#include -#include -#include - -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 SensorService::OnChannelOpen(pdx::Message& msg) { - std::lock_guard guard(mutex_); - - const pdx::MessageInfo& info = msg.GetInfo(); - - std::shared_ptr client( - new SensorClient(*this, info.pid, info.cid)); - AddClient(client); - return client; -} - -void SensorService::OnChannelClose(pdx::Message& /*msg*/, - const std::shared_ptr& chan) { - std::lock_guard guard(mutex_); - - auto client = std::static_pointer_cast(chan); - if (!client) { - ALOGW("WARNING: SensorClient was NULL!\n"); - return; - } - RemoveClient(client); -} - -void SensorService::AddClient(const std::shared_ptr& client) { - clients_.push_front(client); -} - -void SensorService::RemoveClient(const std::shared_ptr& 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& sensor_clients = - sensor_clients_[client->sensor()]; - sensor_clients.remove(client); - sensor_thread_->StopUsingSensor(client->sensor()); - - client->unset_sensor(); -} - -pdx::Status SensorService::HandleMessage(pdx::Message& msg) { - pdx::Status ret; - const pdx::MessageInfo& info = msg.GetInfo(); - switch (info.op) { - case DVR_SENSOR_START: { - std::lock_guard 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(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 guard(mutex_); - auto client = std::static_pointer_cast(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 guard(mutex_); - auto client = std::static_pointer_cast(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 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 6ea470b06e..0000000000 --- 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 -#include -#include - -#include -#include - -#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 { - public: - pdx::Status HandleMessage(pdx::Message& msg) override; - std::shared_ptr OnChannelOpen(pdx::Message& msg) override; - void OnChannelClose(pdx::Message& msg, - const std::shared_ptr& 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> clients_; - - // Map types back to sensor indexes. - std::unordered_map 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> 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& client); - void RemoveClient(const std::shared_ptr& 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 01e4e7e4d1..0000000000 --- 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 46aba17af7..0000000000 --- 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 - -#include - -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; - - // 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 db3915201d..0000000000 --- a/services/vr/sensord/sensord.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#define LOG_TAG "sensord" - -#include - -#include - -#include -#include -#include -#include - -#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 sensor_thread( - new SensorHalThread(&sensor_thread_succeeded)); -#else - std::unique_ptr 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 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 36cd377369..0000000000 --- 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 6cd7db3691..0000000000 --- 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 e553eed755..0000000000 --- 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 615fc758d9..0000000000 --- a/services/vr/sensord/test/poselatencytest.cpp +++ /dev/null @@ -1,87 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -// 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 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; -}