Home | History | Annotate | Download | only in libvrsensor 
      1 #define LOG_TAG "PoseClient"
      2 #include <dvr/dvr_shared_buffers.h>
      3 #include <dvr/pose_client.h>
      4 
      5 #include <stdint.h>
      6 
      7 #include <log/log.h>
      8 #include <pdx/client.h>
      9 #include <pdx/default_transport/client_channel_factory.h>
     10 #include <pdx/file_handle.h>
     11 #include <private/dvr/buffer_hub_client.h>
     12 #include <private/dvr/buffer_hub_queue_client.h>
     13 #include <private/dvr/display_client.h>
     14 #include <private/dvr/pose-ipc.h>
     15 #include <private/dvr/shared_buffer_helpers.h>
     16 
     17 using android::dvr::ConsumerQueue;
     18 using android::pdx::LocalHandle;
     19 using android::pdx::LocalChannelHandle;
     20 using android::pdx::Status;
     21 using android::pdx::Transaction;
     22 
     23 namespace android {
     24 namespace dvr {
     25 namespace {
     26 
     27 typedef CPUMappedBroadcastRing<DvrPoseRing> SensorPoseRing;
     28 
     29 constexpr static int32_t MAX_CONTROLLERS = 2;
     30 }  // namespace
     31 
     32 // PoseClient is a remote interface to the pose service in sensord.
     33 class PoseClient : public pdx::ClientBase<PoseClient> {
     34  public:
     35   ~PoseClient() override {}
     36 
     37   // Casts C handle into an instance of this class.
     38   static PoseClient* FromC(DvrPoseClient* client) {
     39     return reinterpret_cast<PoseClient*>(client);
     40   }
     41 
     42   // Polls the pose service for the current state and stores it in *state.
     43   // Returns zero on success, a negative error code otherwise.
     44   int Poll(DvrPose* state) {
     45     // Allocate the helper class to access the sensor pose buffer.
     46     if (sensor_pose_buffer_ == nullptr) {
     47       sensor_pose_buffer_ = std::make_unique<SensorPoseRing>(
     48           DvrGlobalBuffers::kSensorPoseBuffer, CPUUsageMode::READ_RARELY);
     49     }
     50 
     51     if (state) {
     52       if (sensor_pose_buffer_->GetNewest(state)) {
     53         return 0;
     54       } else {
     55         return -EAGAIN;
     56       }
     57     }
     58 
     59     return -EINVAL;
     60   }
     61 
     62   int GetPose(uint32_t vsync_count, DvrPoseAsync* out_pose) {
     63     const auto vsync_buffer = GetVsyncBuffer();
     64     if (vsync_buffer) {
     65       *out_pose =
     66           vsync_buffer
     67               ->vsync_poses[vsync_count & DvrVsyncPoseBuffer::kIndexMask];
     68       return 0;
     69     } else {
     70       return -EAGAIN;
     71     }
     72   }
     73 
     74   uint32_t GetVsyncCount() {
     75     const auto vsync_buffer = GetVsyncBuffer();
     76     if (vsync_buffer) {
     77       return vsync_buffer->vsync_count;
     78     }
     79 
     80     return 0;
     81   }
     82 
     83   int GetControllerPose(int32_t controller_id, uint32_t vsync_count,
     84                         DvrPoseAsync* out_pose) {
     85     if (controller_id < 0 || controller_id >= MAX_CONTROLLERS) {
     86       return -EINVAL;
     87     }
     88     if (!controllers_[controller_id].mapped_pose_buffer) {
     89       int ret = GetControllerRingBuffer(controller_id);
     90       if (ret < 0)
     91         return ret;
     92     }
     93     *out_pose =
     94         controllers_[controller_id]
     95             .mapped_pose_buffer[vsync_count & DvrVsyncPoseBuffer::kIndexMask];
     96     return 0;
     97   }
     98 
     99   int LogController(bool enable) {
    100     Transaction trans{*this};
    101     Status<int> status = trans.Send<int>(DVR_POSE_LOG_CONTROLLER, &enable,
    102                                          sizeof(enable), nullptr, 0);
    103     ALOGE_IF(!status, "Pose LogController() failed because: %s",
    104              status.GetErrorMessage().c_str());
    105     return ReturnStatusOrError(status);
    106   }
    107 
    108   // Freezes the pose to the provided state. Future poll operations will return
    109   // this state until a different state is frozen or SetMode() is called with a
    110   // different mode.
    111   // Returns zero on success, a negative error code otherwise.
    112   int Freeze(const DvrPose& frozen_state) {
    113     Transaction trans{*this};
    114     Status<int> status = trans.Send<int>(DVR_POSE_FREEZE, &frozen_state,
    115                                          sizeof(frozen_state), nullptr, 0);
    116     ALOGE_IF(!status, "Pose Freeze() failed because: %s\n",
    117              status.GetErrorMessage().c_str());
    118     return ReturnStatusOrError(status);
    119   }
    120 
    121   // Sets the data mode for the pose service.
    122   int SetMode(DvrPoseMode mode) {
    123     Transaction trans{*this};
    124     Status<int> status =
    125         trans.Send<int>(DVR_POSE_SET_MODE, &mode, sizeof(mode), nullptr, 0);
    126     ALOGE_IF(!status, "Pose SetPoseMode() failed because: %s",
    127              status.GetErrorMessage().c_str());
    128     return ReturnStatusOrError(status);
    129   }
    130 
    131   // Gets the data mode for the pose service.
    132   int GetMode(DvrPoseMode* out_mode) {
    133     int mode;
    134     Transaction trans{*this};
    135     Status<int> status =
    136         trans.Send<int>(DVR_POSE_GET_MODE, nullptr, 0, &mode, sizeof(mode));
    137     ALOGE_IF(!status, "Pose GetPoseMode() failed because: %s",
    138              status.GetErrorMessage().c_str());
    139     if (status)
    140       *out_mode = DvrPoseMode(mode);
    141     return ReturnStatusOrError(status);
    142   }
    143 
    144   int GetTangoReaderHandle(uint64_t data_type, ConsumerQueue** queue_out) {
    145     // Get buffer.
    146     Transaction trans{*this};
    147     Status<LocalChannelHandle> status = trans.Send<LocalChannelHandle>(
    148         DVR_POSE_GET_TANGO_READER, &data_type, sizeof(data_type), nullptr, 0);
    149 
    150     if (!status) {
    151       ALOGE("PoseClient GetTangoReaderHandle() failed because: %s",
    152             status.GetErrorMessage().c_str());
    153       *queue_out = nullptr;
    154       return -status.error();
    155     }
    156 
    157     std::unique_ptr<ConsumerQueue> consumer_queue =
    158         ConsumerQueue::Import(status.take());
    159     *queue_out = consumer_queue.release();
    160     return 0;
    161   }
    162 
    163   int DataCapture(const DvrPoseDataCaptureRequest* request) {
    164     Transaction trans{*this};
    165     Status<int> status = trans.Send<int>(DVR_POSE_DATA_CAPTURE, request,
    166                                          sizeof(*request), nullptr, 0);
    167     ALOGE_IF(!status, "PoseClient DataCapture() failed because: %s\n",
    168              status.GetErrorMessage().c_str());
    169     return ReturnStatusOrError(status);
    170   }
    171 
    172   int DataReaderDestroy(uint64_t data_type) {
    173     Transaction trans{*this};
    174     Status<int> status = trans.Send<int>(DVR_POSE_TANGO_READER_DESTROY,
    175                                          &data_type, sizeof(data_type), nullptr,
    176                                          0);
    177     ALOGE_IF(!status, "PoseClient DataReaderDestroy() failed because: %s\n",
    178              status.GetErrorMessage().c_str());
    179     return ReturnStatusOrError(status);
    180   }
    181 
    182   // Enables or disables all pose processing from sensors
    183   int EnableSensors(bool enabled) {
    184     Transaction trans{*this};
    185     Status<int> status = trans.Send<int>(DVR_POSE_SENSORS_ENABLE, &enabled,
    186                                          sizeof(enabled), nullptr, 0);
    187     ALOGE_IF(!status, "Pose EnableSensors() failed because: %s\n",
    188              status.GetErrorMessage().c_str());
    189     return ReturnStatusOrError(status);
    190   }
    191 
    192   int GetRingBuffer(DvrPoseRingBufferInfo* out_info) {
    193     // First time mapping the buffer?
    194     const auto vsync_buffer = GetVsyncBuffer();
    195     if (vsync_buffer) {
    196       if (out_info) {
    197         out_info->min_future_count = DvrVsyncPoseBuffer::kMinFutureCount;
    198         out_info->total_count = DvrVsyncPoseBuffer::kSize;
    199         out_info->buffer = vsync_buffer->vsync_poses;
    200       }
    201       return -EINVAL;
    202     }
    203 
    204     return -EAGAIN;
    205   }
    206 
    207   int GetControllerRingBuffer(int32_t controller_id) {
    208     if (controller_id < 0 || controller_id >= MAX_CONTROLLERS) {
    209       return -EINVAL;
    210     }
    211     ControllerClientState& client_state = controllers_[controller_id];
    212     if (client_state.pose_buffer.get()) {
    213       return 0;
    214     }
    215 
    216     Transaction trans{*this};
    217     Status<LocalChannelHandle> status = trans.Send<LocalChannelHandle>(
    218         DVR_POSE_GET_CONTROLLER_RING_BUFFER, &controller_id,
    219         sizeof(controller_id), nullptr, 0);
    220     if (!status) {
    221       return -status.error();
    222     }
    223 
    224     auto buffer = BufferConsumer::Import(status.take());
    225     if (!buffer) {
    226       ALOGE("Pose failed to import ring buffer");
    227       return -EIO;
    228     }
    229     constexpr size_t size = DvrVsyncPoseBuffer::kSize * sizeof(DvrPoseAsync);
    230     void* addr = nullptr;
    231     int ret = buffer->GetBlobReadOnlyPointer(size, &addr);
    232     if (ret < 0 || !addr) {
    233       ALOGE("Pose failed to map ring buffer: ret:%d, addr:%p", ret, addr);
    234       return -EIO;
    235     }
    236     client_state.pose_buffer.swap(buffer);
    237     client_state.mapped_pose_buffer = static_cast<const DvrPoseAsync*>(addr);
    238     ALOGI(
    239         "Mapped controller %d pose data translation %f,%f,%f quat %f,%f,%f,%f",
    240         controller_id, client_state.mapped_pose_buffer[0].position[0],
    241         client_state.mapped_pose_buffer[0].position[1],
    242         client_state.mapped_pose_buffer[0].position[2],
    243         client_state.mapped_pose_buffer[0].orientation[0],
    244         client_state.mapped_pose_buffer[0].orientation[1],
    245         client_state.mapped_pose_buffer[0].orientation[2],
    246         client_state.mapped_pose_buffer[0].orientation[3]);
    247     return 0;
    248   }
    249 
    250  private:
    251   friend BASE;
    252 
    253   // Set up a channel to the pose service.
    254   PoseClient()
    255       : BASE(pdx::default_transport::ClientChannelFactory::Create(
    256             DVR_POSE_SERVICE_CLIENT)) {
    257     // TODO(eieio): Cache the pose and make timeout 0 so that the API doesn't
    258     // block while waiting for the pose service to come back up.
    259     EnableAutoReconnect(kInfiniteTimeout);
    260   }
    261 
    262   PoseClient(const PoseClient&) = delete;
    263   PoseClient& operator=(const PoseClient&) = delete;
    264 
    265   const DvrVsyncPoseBuffer* GetVsyncBuffer() {
    266     if (mapped_vsync_pose_buffer_ == nullptr) {
    267       if (vsync_pose_buffer_ == nullptr) {
    268         // The constructor tries mapping it so we do not need TryMapping after.
    269         vsync_pose_buffer_ = std::make_unique<CPUMappedBuffer>(
    270             DvrGlobalBuffers::kVsyncPoseBuffer, CPUUsageMode::READ_OFTEN);
    271       } else if (vsync_pose_buffer_->IsMapped() == false) {
    272         vsync_pose_buffer_->TryMapping();
    273       }
    274 
    275       if (vsync_pose_buffer_->IsMapped()) {
    276         mapped_vsync_pose_buffer_ =
    277             static_cast<DvrVsyncPoseBuffer*>(vsync_pose_buffer_->Address());
    278       }
    279     }
    280 
    281     return mapped_vsync_pose_buffer_;
    282   }
    283 
    284   // The vsync pose buffer if already mapped.
    285   std::unique_ptr<CPUMappedBuffer> vsync_pose_buffer_;
    286 
    287   // The direct sensor pose buffer.
    288   std::unique_ptr<SensorPoseRing> sensor_pose_buffer_;
    289 
    290   const DvrVsyncPoseBuffer* mapped_vsync_pose_buffer_ = nullptr;
    291 
    292   struct ControllerClientState {
    293     std::unique_ptr<BufferConsumer> pose_buffer;
    294     const DvrPoseAsync* mapped_pose_buffer = nullptr;
    295   };
    296   ControllerClientState controllers_[MAX_CONTROLLERS];
    297 };
    298 
    299 int dvrPoseClientGetDataReaderHandle(DvrPoseClient* client, uint64_t type,
    300                                      ConsumerQueue** queue_out) {
    301   return PoseClient::FromC(client)->GetTangoReaderHandle(type, queue_out);
    302 }
    303 
    304 }  // namespace dvr
    305 }  // namespace android
    306 
    307 using android::dvr::PoseClient;
    308 
    309 extern "C" {
    310 
    311 DvrPoseClient* dvrPoseClientCreate() {
    312   auto* client = PoseClient::Create().release();
    313   return reinterpret_cast<DvrPoseClient*>(client);
    314 }
    315 
    316 void dvrPoseClientDestroy(DvrPoseClient* client) {
    317   delete PoseClient::FromC(client);
    318 }
    319 
    320 int dvrPoseClientGet(DvrPoseClient* client, uint32_t vsync_count,
    321                      DvrPoseAsync* out_pose) {
    322   return PoseClient::FromC(client)->GetPose(vsync_count, out_pose);
    323 }
    324 
    325 uint32_t dvrPoseClientGetVsyncCount(DvrPoseClient* client) {
    326   return PoseClient::FromC(client)->GetVsyncCount();
    327 }
    328 
    329 int dvrPoseClientGetController(DvrPoseClient* client, int32_t controller_id,
    330                                uint32_t vsync_count, DvrPoseAsync* out_pose) {
    331   return PoseClient::FromC(client)->GetControllerPose(controller_id,
    332                                                       vsync_count, out_pose);
    333 }
    334 
    335 int dvrPoseClientLogController(DvrPoseClient* client, bool enable) {
    336   return PoseClient::FromC(client)->LogController(enable);
    337 }
    338 
    339 int dvrPoseClientPoll(DvrPoseClient* client, DvrPose* state) {
    340   return PoseClient::FromC(client)->Poll(state);
    341 }
    342 
    343 int dvrPoseClientFreeze(DvrPoseClient* client, const DvrPose* frozen_state) {
    344   return PoseClient::FromC(client)->Freeze(*frozen_state);
    345 }
    346 
    347 int dvrPoseClientModeSet(DvrPoseClient* client, DvrPoseMode mode) {
    348   return PoseClient::FromC(client)->SetMode(mode);
    349 }
    350 
    351 int dvrPoseClientModeGet(DvrPoseClient* client, DvrPoseMode* mode) {
    352   return PoseClient::FromC(client)->GetMode(mode);
    353 }
    354 
    355 int dvrPoseClientSensorsEnable(DvrPoseClient* client, bool enabled) {
    356   return PoseClient::FromC(client)->EnableSensors(enabled);
    357 }
    358 
    359 int dvrPoseClientDataCapture(DvrPoseClient* client,
    360                              const DvrPoseDataCaptureRequest* request) {
    361   return PoseClient::FromC(client)->DataCapture(request);
    362 }
    363 
    364 int dvrPoseClientDataReaderDestroy(DvrPoseClient* client, uint64_t data_type) {
    365   return PoseClient::FromC(client)->DataReaderDestroy(data_type);
    366 }
    367 
    368 }  // extern "C"
    369