Merge "Move sensord" into oc-dev

# 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)

// pose is a utility to query and manipulate the current pose via the pose

// service.

#include <cmath>

#include <cstdio>

#include <iomanip>

#include <iostream>

#include <regex>

#include <vector>

#include <private/dvr/types.h>

#include <dvr/pose_client.h>

using android::dvr::vec3;

using android::dvr::quat;

namespace {

// Prints usage information to stderr.

void PrintUsage(const char* executable_name) {

  std::cerr << "Usage: " << executable_name

            << " [--identity|--set=...|--unfreeze]\n"

            << "\n"

            << "  no arguments: display the current pose.\n"

            << "  --identity: freeze the pose to the identity pose.\n"

            << "  --set=rx,ry,rz,rw[,px,py,pz]: freeze the pose to the given "

               "state. rx,ry,rz,rw are interpreted as rotation quaternion. "

               " px, py, pz as position (0,0,0 if omitted).\n"

            << "  --mode=mode: sets mode to one of normal, head_turn:slow, "

               "head_turn:fast, rotate:slow, rotate:medium, rotate:fast, "

               "circle_strafe.\n"

            << "  --unfreeze: sets the mode to normal.\n"

            << "  --log_controller=[true|false]: starts and stops controller"

               " logs\n"

            << std::endl;

}

// If return_code is negative, print out its corresponding string description

// and exit the program with a non-zero exit code.

void ExitIfNegative(int return_code) {

  if (return_code < 0) {

    std::cerr << "Error: " << strerror(-return_code) << std::endl;

    std::exit(1);

  }

}

// Parses the following command line flags:

// --identity

// --set=rx,ry,rz,rw[,px,py,pz]

// Returns false if parsing fails.

bool ParseState(const std::string& arg, DvrPoseState* out_state) {

  if (arg == "--identity") {

    *out_state = {.head_from_start_rotation = {0.f, 0.f, 0.f, 1.f},

                  .head_from_start_translation = {0.f, 0.f, 0.f},

                  .timestamp_ns = 0,

                  .sensor_from_start_rotation_velocity = {0.f, 0.f, 0.f}};

    return true;

  }

  const std::string prefix("--set=");

  if (arg.size() < 6 || arg.compare(0, prefix.size(), prefix) != 0) {

    return false;

  }

  // Tokenize by ','.

  std::regex split_by_comma("[,]+");

  std::sregex_token_iterator token_it(arg.begin() + prefix.size(), arg.end(),

                                      split_by_comma,

                                      -1 /* return inbetween parts */);

  std::sregex_token_iterator token_end;

  // Convert to float and store values.

  std::vector<float> values;

  for (; token_it != token_end; ++token_it) {

    std::string token = *(token_it);

    float value = 0.f;

    if (sscanf(token.c_str(), "%f", &value) != 1) {

      std::cerr << "Unable to parse --set value as float: " << token

                << std::endl;

      return false;

    } else {

      values.push_back(value);

    }

  }

  if (values.size() != 4 && values.size() != 7) {

    std::cerr << "Unable to parse --set, expected either 4 or 7 of values."

              << std::endl;

    return false;

  }

  float norm2 = values[0] * values[0] + values[1] * values[1] +

                values[2] * values[2] + values[3] * values[3];

  if (std::abs(norm2 - 1.f) > 1e-4) {

    if (norm2 < 1e-8) {

      std::cerr << "--set quaternion norm close to zero." << std::endl;

      return false;

    }

    float norm = std::sqrt(norm2);

    values[0] /= norm;

    values[1] /= norm;

    values[2] /= norm;

    values[3] /= norm;

  }

  out_state->head_from_start_rotation = {values[0], values[1], values[2],

                                         values[3]};

  if (values.size() == 7) {

    out_state->head_from_start_translation = {values[4], values[5], values[6]};

  } else {

    out_state->head_from_start_translation = {0.f, 0.f, 0.f};

  }

  out_state->timestamp_ns = 0;

  out_state->sensor_from_start_rotation_velocity = {0.f, 0.f, 0.f};

  return true;

}

// Parses the command line flag --mode.

// Returns false if parsing fails.

bool ParseSetMode(const std::string& arg, DvrPoseMode* mode) {

  const std::string prefix("--mode=");

  if (arg.size() < prefix.size() ||

      arg.compare(0, prefix.size(), prefix) != 0) {

    return false;

  }

  std::string value = arg.substr(prefix.size());

  if (value == "normal") {

    *mode = DVR_POSE_MODE_6DOF;

    return true;

  } else if (value == "head_turn:slow") {

    *mode = DVR_POSE_MODE_MOCK_HEAD_TURN_SLOW;

    return true;

  } else if (value == "head_turn:fast") {

    *mode = DVR_POSE_MODE_MOCK_HEAD_TURN_FAST;

    return true;

  } else if (value == "rotate:slow") {

    *mode = DVR_POSE_MODE_MOCK_ROTATE_SLOW;

    return true;

  } else if (value == "rotate:medium") {

    *mode = DVR_POSE_MODE_MOCK_ROTATE_MEDIUM;

    return true;

  } else if (value == "rotate:fast") {

    *mode = DVR_POSE_MODE_MOCK_ROTATE_FAST;

    return true;

  } else if (value == "circle_strafe") {

    *mode = DVR_POSE_MODE_MOCK_CIRCLE_STRAFE;

    return true;

  } else {

    return false;

  }

}

// Parses the command line flag --controller_log.

// Returns false if parsing fails.

bool ParseLogController(const std::string& arg, bool* log_enabled) {

  const std::string prefix("--log_controller=");

  if (arg.size() < prefix.size() ||

      arg.compare(0, prefix.size(), prefix) != 0) {

    return false;

  }

  std::string value = arg.substr(prefix.size());

  if (value == "false") {

    *log_enabled = false;

    return true;

  } else if (value == "true") {

    *log_enabled = true;

    return true;

  } else {

    return false;

  }

}

// The different actions that the tool can perform.

enum class Action {

  Query,                 // Query the current pose.

  Set,                   // Set the pose and freeze.

  Unfreeze,              // Set the pose mode to normal.

  SetMode,               // Sets the pose mode.

  LogController,         // Start/stop controller logging in sensord.

};

// The action to perform when no arguments are passed to the tool.

constexpr Action kDefaultAction = Action::Query;

}  // namespace

int main(int argc, char** argv) {

  Action action = kDefaultAction;

  DvrPoseState state;

  DvrPoseMode pose_mode = DVR_POSE_MODE_6DOF;

  bool log_controller = false;

  // Parse command-line arguments.

  for (int i = 1; i < argc; ++i) {

    const std::string arg = argv[i];

    if (ParseState(arg, &state) && action == kDefaultAction) {

      action = Action::Set;

    } else if (arg == "--unfreeze" && action == kDefaultAction) {

      action = Action::Unfreeze;

    } else if (ParseSetMode(arg, &pose_mode) && action == kDefaultAction) {

      action = Action::SetMode;

    } else if (ParseLogController(arg, &log_controller)) {

      action = Action::LogController;

    } else {

      PrintUsage(argv[0]);

      return 1;

    }

  }

  auto pose_client = dvrPoseCreate();

  if (!pose_client) {

    std::cerr << "Unable to create pose client." << std::endl;

    return 1;

  }

  switch (action) {

    case Action::Query: {

      ExitIfNegative(dvrPosePoll(pose_client, &state));

      uint64_t timestamp = state.timestamp_ns;

      const auto& rotation = state.head_from_start_rotation;

      const auto& translation = state.head_from_start_translation;

      const auto& rotation_velocity = state.sensor_from_start_rotation_velocity;

      quat q(rotation.w, rotation.x, rotation.y, rotation.z);

      vec3 angles = q.matrix().eulerAngles(0, 1, 2);

      angles = angles * 180.f / M_PI;

      vec3 x = q * vec3(1.0f, 0.0f, 0.0f);

      vec3 y = q * vec3(0.0f, 1.0f, 0.0f);

      vec3 z = q * vec3(0.0f, 0.0f, 1.0f);

      std::cout << "timestamp_ns: " << timestamp << std::endl

                << "rotation_quaternion: " << rotation.x << ", " << rotation.y

                << ", " << rotation.z << ", " << rotation.w << std::endl

                << "rotation_angles: " << angles.x() << ", " << angles.y()

                << ", " << angles.z() << std::endl

                << "translation: " << translation.x << ", " << translation.y

                << ", " << translation.z << std::endl

                << "rotation_velocity: " << rotation_velocity.x << ", "

                << rotation_velocity.y << ", " << rotation_velocity.z

                << std::endl

                << "axes: " << std::setprecision(3)

                << "x(" << x.x() << ", " << x.y() << ", " << x.z() << "), "

                << "y(" << y.x() << ", " << y.y() << ", " << y.z() << "), "

                << "z(" << z.x() << ", " << z.y() << ", " << z.z() << "), "

                << std::endl;

      break;

    }

    case Action::Set: {

      ExitIfNegative(dvrPoseFreeze(pose_client, &state));

      break;

    }

    case Action::Unfreeze: {

      ExitIfNegative(dvrPoseSetMode(pose_client, DVR_POSE_MODE_6DOF));

      break;

    }

    case Action::SetMode: {

      ExitIfNegative(dvrPoseSetMode(pose_client, pose_mode));

      break;

    }

    case Action::LogController: {

      ExitIfNegative(

          dvrPoseLogController(pose_client, log_controller));

      break;

    }

  }

  dvrPoseDestroy(pose_client);

}

# 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)

#ifndef ANDROID_DVR_SENSORD_POSE_SERVICE_H_

#define ANDROID_DVR_SENSORD_POSE_SERVICE_H_

#include <condition_variable>

#include <forward_list>

#include <mutex>

#include <thread>

#include <unordered_map>

#include <vector>

#include <dvr/pose_client.h>

#include <pdx/service.h>

#include <private/dvr/buffer_hub_client.h>

#include <private/dvr/dvr_pose_predictor.h>

#include <private/dvr/latency_model.h>

#include <private/dvr/pose_client_internal.h>

#include <private/dvr/ring_buffer.h>

#include "sensor_fusion.h"

#include "sensor_thread.h"

namespace android {

namespace dvr {

// PoseService implements the HMD pose service over ServiceFS.

class PoseService : public pdx::ServiceBase<PoseService> {

 public:

  ~PoseService() override;

  bool IsInitialized() const override;

  pdx::Status<void> HandleMessage(pdx::Message& msg) override;

  std::string DumpState(size_t max_length) override;

  // Handle events from the sensor HAL.

  // Safe to call concurrently with any other public member functions.

  void HandleEvents(const sensors_event_t* begin_events,

                    const sensors_event_t* end_events);

 private:

  friend BASE;

  enum OrientationType {

    // Typical smartphone device (default).

    kOrientationTypePortrait = 1,

    // Landscape device.

    kOrientationTypeLandscape = 2,

    // 180 Landscape device.

    kOrientationTypeLandscape180 = 3,

  };

  // Initializes the service. Keeps a reference to sensor_thread, which must be

  // non-null.

  explicit PoseService(SensorThread* sensor_thread);

  // Kick the sensor watch dog thread which will robustly disable IMU usage

  // when there are no sensor data consumers.

  // The class mutex (mutex_) must be locked while calling this method.

  void KickSensorWatchDogThread();

  void UpdatePoseMode();

  // Update the async pose ring buffer with new pose data.

  // |start_t_head| Head position in start space.

  // |start_q_head| Head orientation quaternion in start space.

  // |pose_timestamp| System timestamp of pose data in seconds.

  // |pose_delta_time| Elapsed time in seconds between this pose and the last.

  void WriteAsyncPoses(const Eigen::Vector3<double>& start_t_head,

                       const Eigen::Quaternion<double>& start_q_head,

                       int64_t pose_timestamp);

  // Set the pose mode.

  void SetPoseMode(DvrPoseMode mode);

  // The abstraction around the sensor data.

  SensorThread* sensor_thread_;

  // Protects access to all member variables.

  std::mutex mutex_;

  // Watchdog thread data. The watchdog thread will ensure that sensor access

  // is disabled when nothing has been consuming it for a while.

  int64_t last_sensor_usage_time_ns_;

  std::thread watchdog_thread_;

  std::condition_variable watchdog_condition_;

  bool watchdog_shutdown_;

  bool sensors_on_;

  // Indices for the accelerometer and gyroscope sensors, or -1 if the sensor

  // wasn't present on construction.

  int accelerometer_index_;

  int gyroscope_index_;

  // The sensor fusion algorithm and its state.

  SensorFusion sensor_fusion_;

  // Current pose mode.

  DvrPoseMode pose_mode_;

  // State which is sent if pose_mode_ is DVR_POSE_MODE_MOCK_FROZEN.

  DvrPoseState frozen_state_;

  // Last known pose.

  DvrPoseAsync last_known_pose_;

  // If this flag is true, the pose published includes a small prediction of

  // where it'll be when it's consumed.

  bool enable_pose_prediction_;

  // Flag to turn on recording of raw sensor data

  bool enable_sensor_recording_;

  // Flag to log pose to a file

  bool enable_pose_recording_;

  // Flag to turn on playback from a saved dataset instead of using live data.

  bool enable_sensor_playback_;

  std::string sensor_playback_id_;

  // External pose generation.

  bool enable_external_pose_ = false;

  // The predictor to extrapolate pose samples.

  std::unique_ptr<posepredictor::Predictor> pose_predictor_;

  // Pose ring buffer.

  std::shared_ptr<BufferProducer> ring_buffer_;

  // Temporary mapped ring buffer.

  DvrPoseRingBuffer* mapped_pose_buffer_;

  // Current vsync info, updated by displayd.

  uint32_t vsync_count_;

  int64_t photon_timestamp_;

  int64_t display_period_ns_;

  int64_t right_eye_photon_offset_ns_ = 0;

  // To model the measurement - arrival latency.

  LatencyModel sensor_latency_;

  // Type for controlling pose orientation calculation.

  OrientationType device_orientation_type_;

  PoseService(const PoseService&) = delete;

  void operator=(const PoseService&) = delete;

};

}  // namespace dvr

}  // namespace android

#endif  // ANDROID_DVR_SENSORD_POSE_SERVICE_H_