Alex Vakulenko | e4eec20 | 2017-01-27 14:41:04 -0800 | [diff] [blame^] | 1 | #include "sensor_ndk_thread.h" |
| 2 | |
| 3 | #include <cutils/log.h> |
| 4 | #include <dvr/performance_client_api.h> |
| 5 | |
| 6 | namespace android { |
| 7 | namespace dvr { |
| 8 | |
| 9 | namespace { |
| 10 | static constexpr int kLooperIdUser = 5; |
| 11 | } // namespace |
| 12 | |
| 13 | SensorNdkThread::SensorNdkThread(bool* out_success) |
| 14 | : shutting_down_(false), |
| 15 | paused_(true), |
| 16 | thread_started_(false), |
| 17 | initialization_result_(false), |
| 18 | looper_(nullptr), |
| 19 | sensor_manager_(nullptr), |
| 20 | event_queue_(nullptr), |
| 21 | sensor_list_(nullptr), |
| 22 | sensor_count_(0) { |
| 23 | // Assume failure; we will change this to true on success. |
| 24 | *out_success = false; |
| 25 | |
| 26 | // These structs are the same, but sanity check the sizes. |
| 27 | static_assert(sizeof(sensors_event_t) == sizeof(ASensorEvent), |
| 28 | "Error: sizeof(sensors_event_t) != sizeof(ASensorEvent)"); |
| 29 | |
| 30 | thread_.reset(new std::thread([this] { |
| 31 | const int priority_error = dvrSetSchedulerClass(0, "sensors:high"); |
| 32 | LOG_ALWAYS_FATAL_IF( |
| 33 | priority_error < 0, |
| 34 | "SensorHalTread::StartPolling: Failed to set scheduler class: %s", |
| 35 | strerror(-priority_error)); |
| 36 | |
| 37 | // Start ALooper and initialize sensor access. |
| 38 | { |
| 39 | std::unique_lock<std::mutex> lock(mutex_); |
| 40 | initialization_result_ = InitializeSensors(); |
| 41 | thread_started_ = true; |
| 42 | init_condition_.notify_one(); |
| 43 | if (!initialization_result_) |
| 44 | return; |
| 45 | } |
| 46 | |
| 47 | EventConsumer consumer; |
| 48 | for (;;) { |
| 49 | for (;;) { |
| 50 | std::unique_lock<std::mutex> lock(mutex_); |
| 51 | UpdateSensorUse(); |
| 52 | if (!consumer) |
| 53 | consumer = consumer_; |
| 54 | if (shutting_down_) |
| 55 | return; |
| 56 | if (!paused_) |
| 57 | break; |
| 58 | condition_.wait(lock); |
| 59 | } |
| 60 | |
| 61 | constexpr int kMaxEvents = 100; |
| 62 | sensors_event_t events[kMaxEvents]; |
| 63 | ssize_t event_count = 0; |
| 64 | if (looper_ && sensor_manager_) { |
| 65 | int poll_fd, poll_events; |
| 66 | void* poll_source; |
| 67 | // Poll for events. |
| 68 | int ident = ALooper_pollAll(-1, &poll_fd, &poll_events, &poll_source); |
| 69 | |
| 70 | if (ident != kLooperIdUser) |
| 71 | continue; |
| 72 | |
| 73 | ASensorEvent* event = reinterpret_cast<ASensorEvent*>(&events[0]); |
| 74 | event_count = |
| 75 | ASensorEventQueue_getEvents(event_queue_, event, kMaxEvents); |
| 76 | |
| 77 | if (event_count == 0) { |
| 78 | ALOGE("Detected sensor service failure, restarting sensors"); |
| 79 | // This happens when sensorservice has died and restarted. To avoid |
| 80 | // spinning we need to restart the sensor access. |
| 81 | DestroySensors(); |
| 82 | InitializeSensors(); |
| 83 | } |
| 84 | } else { |
| 85 | // When there is no sensor_device_, we still call the consumer at |
| 86 | // regular intervals in case mock poses are in use. Note that this |
| 87 | // will never be the case for production devices, but this helps |
| 88 | // during bringup. |
| 89 | usleep(5000); |
| 90 | } |
| 91 | if (event_count == kMaxEvents) |
| 92 | ALOGI("max events (%d) reached", kMaxEvents); |
| 93 | |
| 94 | if (event_count >= 0) { |
| 95 | consumer(events, events + event_count); |
| 96 | } else { |
| 97 | ALOGE( |
| 98 | "SensorNdkThread::StartPolling: Error while polling sensor: %s " |
| 99 | "(%zd)", |
| 100 | strerror(-event_count), -event_count); |
| 101 | } |
| 102 | } |
| 103 | |
| 104 | // About to exit sensor thread, destroy sensor objects. |
| 105 | DestroySensors(); |
| 106 | })); |
| 107 | |
| 108 | // Wait for thread to startup and initialize sensors so that we know whether |
| 109 | // it succeeded. |
| 110 | { |
| 111 | std::unique_lock<std::mutex> lock(mutex_); |
| 112 | while (!thread_started_) |
| 113 | init_condition_.wait(lock); |
| 114 | } |
| 115 | |
| 116 | // At this point, we've successfully initialized everything. |
| 117 | *out_success = initialization_result_; |
| 118 | } |
| 119 | |
| 120 | SensorNdkThread::~SensorNdkThread() { |
| 121 | { |
| 122 | if (looper_) |
| 123 | ALooper_wake(looper_); |
| 124 | std::unique_lock<std::mutex> lock(mutex_); |
| 125 | shutting_down_ = true; |
| 126 | condition_.notify_one(); |
| 127 | } |
| 128 | |
| 129 | thread_->join(); |
| 130 | } |
| 131 | |
| 132 | bool SensorNdkThread::InitializeSensors() { |
| 133 | looper_ = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS); |
| 134 | if (!looper_) { |
| 135 | ALOGE("Failed to create ALooper."); |
| 136 | return false; |
| 137 | } |
| 138 | |
| 139 | // Prepare to monitor accelerometer |
| 140 | sensor_manager_ = ASensorManager_getInstanceForPackage(nullptr); |
| 141 | if (!sensor_manager_) { |
| 142 | ALOGE("Failed to create ASensorManager."); |
| 143 | return false; |
| 144 | } |
| 145 | |
| 146 | event_queue_ = ASensorManager_createEventQueue( |
| 147 | sensor_manager_, looper_, kLooperIdUser, nullptr, nullptr); |
| 148 | if (!event_queue_) { |
| 149 | ALOGE("Failed to create sensor EventQueue."); |
| 150 | return false; |
| 151 | } |
| 152 | |
| 153 | sensor_count_ = ASensorManager_getSensorList(sensor_manager_, &sensor_list_); |
| 154 | ALOGI("Sensor count %d", sensor_count_); |
| 155 | |
| 156 | sensor_user_count_.resize(sensor_count_, 0); |
| 157 | |
| 158 | // To recover from sensorservice restart, enable the sensors that are already |
| 159 | // requested. |
| 160 | for (size_t sensor_index = 0; sensor_index < sensor_user_count_.size(); |
| 161 | ++sensor_index) { |
| 162 | if (sensor_user_count_[sensor_index] > 0) { |
| 163 | int result = ASensorEventQueue_registerSensor( |
| 164 | event_queue_, sensor_list_[sensor_index], 0, 0); |
| 165 | ALOGE_IF(result < 0, "ASensorEventQueue_registerSensor failed: %d", |
| 166 | result); |
| 167 | } |
| 168 | } |
| 169 | |
| 170 | return true; |
| 171 | } |
| 172 | |
| 173 | void SensorNdkThread::DestroySensors() { |
| 174 | for (size_t sensor_index = 0; sensor_index < sensor_user_count_.size(); |
| 175 | ++sensor_index) { |
| 176 | if (sensor_user_count_[sensor_index] > 0) { |
| 177 | ASensorEventQueue_disableSensor(event_queue_, sensor_list_[sensor_index]); |
| 178 | } |
| 179 | } |
| 180 | ASensorManager_destroyEventQueue(sensor_manager_, event_queue_); |
| 181 | } |
| 182 | |
| 183 | void SensorNdkThread::UpdateSensorUse() { |
| 184 | if (!enable_sensors_.empty()) { |
| 185 | for (int sensor_index : enable_sensors_) { |
| 186 | if (sensor_user_count_[sensor_index]++ == 0) { |
| 187 | int result = ASensorEventQueue_registerSensor( |
| 188 | event_queue_, sensor_list_[sensor_index], 0, 0); |
| 189 | ALOGE_IF(result < 0, "ASensorEventQueue_registerSensor failed: %d", |
| 190 | result); |
| 191 | } |
| 192 | } |
| 193 | enable_sensors_.clear(); |
| 194 | } |
| 195 | |
| 196 | if (!disable_sensors_.empty()) { |
| 197 | for (int sensor_index : disable_sensors_) { |
| 198 | if (--sensor_user_count_[sensor_index] == 0) { |
| 199 | int result = ASensorEventQueue_disableSensor( |
| 200 | event_queue_, sensor_list_[sensor_index]); |
| 201 | ALOGE_IF(result < 0, "ASensorEventQueue_disableSensor failed: %d", |
| 202 | result); |
| 203 | } |
| 204 | } |
| 205 | disable_sensors_.clear(); |
| 206 | } |
| 207 | } |
| 208 | |
| 209 | void SensorNdkThread::StartPolling(const EventConsumer& consumer) { |
| 210 | { |
| 211 | std::unique_lock<std::mutex> lock(mutex_); |
| 212 | if (consumer_) { |
| 213 | ALOGE("Already started sensor thread."); |
| 214 | return; |
| 215 | } |
| 216 | consumer_ = consumer; |
| 217 | } |
| 218 | SetPaused(false); |
| 219 | } |
| 220 | |
| 221 | void SensorNdkThread::SetPaused(bool is_paused) { |
| 222 | std::unique_lock<std::mutex> lock(mutex_); |
| 223 | // SetPaused may be called before we have StartPolling, make sure we have |
| 224 | // an event consumer. Otherwise we defer until StartPolling is called. |
| 225 | if (!consumer_) |
| 226 | return; |
| 227 | paused_ = is_paused; |
| 228 | condition_.notify_one(); |
| 229 | ALooper_wake(looper_); |
| 230 | } |
| 231 | |
| 232 | void SensorNdkThread::StartUsingSensor(const int sensor_index) { |
| 233 | std::unique_lock<std::mutex> lock(mutex_); |
| 234 | if (sensor_index < 0 || sensor_index >= sensor_count_) { |
| 235 | ALOGE("StartUsingSensor(): sensor index %d out of range [0, %d)", |
| 236 | sensor_index, sensor_count_); |
| 237 | return; |
| 238 | } |
| 239 | |
| 240 | enable_sensors_.push_back(sensor_index); |
| 241 | ALooper_wake(looper_); |
| 242 | } |
| 243 | |
| 244 | void SensorNdkThread::StopUsingSensor(const int sensor_index) { |
| 245 | std::unique_lock<std::mutex> lock(mutex_); |
| 246 | if (sensor_index < 0 || sensor_index >= sensor_count_) { |
| 247 | ALOGE("StopUsingSensor(): sensor index %d out of range [0, %d)", |
| 248 | sensor_index, sensor_count_); |
| 249 | return; |
| 250 | } |
| 251 | |
| 252 | disable_sensors_.push_back(sensor_index); |
| 253 | ALooper_wake(looper_); |
| 254 | } |
| 255 | |
| 256 | } // namespace dvr |
| 257 | } // namespace android |