1 /*
2 * imu_service.cpp - interfaces for EIS algorithm to access imu sensor
3 *
4 * Copyright (c) 2021 Rockchip Electronics Co., Ltd.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 * Author: Cody Xie <cody.xie@rock-chips.com>
19 */
20 #include "imu_service.h"
21
22 #include <iostream>
23 #include <fstream>
24
25 #include "xcam_common.h"
26 #include "xcam_log.h"
27
28 using namespace XCam;
29
30 namespace RkCam {
31
EisImuData(std::shared_ptr<EisImuAdaptor> imu,mems_sensor_type_t type,mems_sensor_event_t * data,size_t count)32 EisImuData::EisImuData(std::shared_ptr<EisImuAdaptor> imu, mems_sensor_type_t type,
33 mems_sensor_event_t* data, size_t count)
34 : imu_(imu), type_(type), data_(data), count_(count) {
35 XCAM_ASSERT(imu_.get() != nullptr && data_ != nullptr);
36 }
37
~EisImuData()38 EisImuData::~EisImuData() { imu_->FreeData(data_); }
39
GetType() const40 mems_sensor_type_t EisImuData::GetType() const { return type_; }
41
GetData() const42 mems_sensor_event_t* EisImuData::GetData() const { return data_; }
43
GetCount() const44 size_t EisImuData::GetCount() const { return count_; }
45
EisImuAdaptor(const rk_aiq_mems_sensor_intf_t intf,const mems_sensor_type_t type)46 EisImuAdaptor::EisImuAdaptor(const rk_aiq_mems_sensor_intf_t intf, const mems_sensor_type_t type)
47 : intf_(intf), type_(type), ctx_(nullptr), handle_(nullptr) {
48 XCAM_ASSERT(intf.createContext != nullptr && intf.getSensorList != nullptr);
49 }
50
~EisImuAdaptor()51 EisImuAdaptor::~EisImuAdaptor() { DeInit(); }
52
Init(float sample_rate)53 XCamReturn EisImuAdaptor::Init(float sample_rate) {
54 mems_sensor_list_t sensors_list;
55 mems_sensor_capabilities_t caps;
56
57 if (ctx_ == nullptr) {
58 ctx_ = intf_.createContext();
59 if (ctx_ == nullptr) {
60 LOGE_AEIS("Create IMU context failed");
61 return XCAM_RETURN_ERROR_FAILED;
62 }
63 }
64
65 memset(&sensors_list, 0, sizeof(mems_sensor_list_t));
66 auto result = intf_.getSensorList(ctx_, type_, &sensors_list);
67 if (result == SENSOR_NO_ERR && sensors_list.count > 0) {
68 LOGV_AEIS("Get supported sensor list: ");
69 for (int32_t i = 0; i < sensors_list.count; i++) {
70 LOGV_AEIS("%d: %s", i, sensors_list.key_list[i]);
71 }
72 key_ = std::string(sensors_list.key_list[0]);
73 intf_.releaseSensorList(&sensors_list);
74 } else {
75 LOGE_AEIS("Init IMU get no dev");
76 intf_.destroyContext(ctx_);
77 ctx_ = nullptr;
78 return XCAM_RETURN_ERROR_FAILED;
79 }
80
81 memset(&caps, 0, sizeof(mems_sensor_capabilities_t));
82 const char* c = key_.c_str();
83 result = intf_.getSensorCapabilities(ctx_, type_, c, &caps);
84 if (result) {
85 LOGE_AEIS("Get IMU capabilities failed");
86 intf_.destroyContext(ctx_);
87 ctx_ = nullptr;
88 return XCAM_RETURN_ERROR_FAILED;
89 }
90
91 for (int32_t i = 0; i < caps.num_sample_rates; i++) {
92 LOGE_AEIS("Get IMU capabilities: %d:sample_cap: %f", i, caps.sample_rates[i]);
93 }
94
95 float matched_sample_rate = 0.0f;
96 float max_sample_rate = 0.0f;
97 for (int i = 0; i < caps.num_sample_rates; i++) {
98 if (max_sample_rate < caps.sample_rates[i]) {
99 max_sample_rate = caps.sample_rates[i];
100 }
101 if (sample_rate == caps.sample_rates[i]) {
102 matched_sample_rate = sample_rate;
103 break;
104 }
105 }
106 intf_.releaseSensorCapabilities(&caps);
107 if (0.0 == matched_sample_rate) {
108 matched_sample_rate = max_sample_rate;
109 }
110
111 XCAM_ASSERT(matched_sample_rate != 0.0f);
112 if (matched_sample_rate != 0.0) {
113 mems_sensor_config_t sensor_cfg;
114 memset(&sensor_cfg, 0, sizeof(mems_sensor_config_t));
115 result = intf_.getConfig(ctx_, type_, c, &sensor_cfg);
116 if (result != SENSOR_NO_ERR) {
117 LOGE_AEIS("Get current IMU config failed");
118 intf_.destroyContext(ctx_);
119 ctx_ = nullptr;
120 return XCAM_RETURN_ERROR_FAILED;
121 }
122 LOGD_AEIS("Get current IMU config: %f", sensor_cfg.sample_rate);
123 sensor_cfg.sample_rate = matched_sample_rate;
124
125 result = intf_.setConfig(ctx_, type_, c, sensor_cfg);
126 if (result != SENSOR_NO_ERR) {
127 LOGE_AEIS("Set IMU config failed\n");
128 intf_.destroyContext(ctx_);
129 ctx_ = nullptr;
130 return XCAM_RETURN_ERROR_FAILED;
131 }
132 LOGD_AEIS("Set IMU config succeed %f", matched_sample_rate);
133 }
134
135 handle_ = intf_.createHandle(ctx_, type_, key_.c_str(), 200, 7);
136 if (handle_ == nullptr) {
137 LOGE_AEIS("create IMU handle failed");
138 intf_.destroyContext(ctx_);
139 ctx_ = nullptr;
140 return XCAM_RETURN_ERROR_FAILED;
141 }
142
143 return XCAM_RETURN_NO_ERROR;
144 }
145
DeInit()146 XCamReturn EisImuAdaptor::DeInit() {
147 XCAM_ASSERT(intf_.createContext != nullptr);
148 if (ctx_ != nullptr) {
149 if (handle_ != nullptr) {
150 intf_.destroyHandle(handle_);
151 }
152 intf_.destroyContext(ctx_);
153 }
154
155 return XCAM_RETURN_NO_ERROR;
156 }
157
GetData(size_t * num_sample)158 mems_sensor_event_t* EisImuAdaptor::GetData(size_t* num_sample) {
159 XCAM_ASSERT(ctx_ != nullptr && handle_ != nullptr);
160 size_t count = 0;
161
162 auto data = reinterpret_cast<mems_sensor_event_t*>(intf_.getData(handle_, &count));
163 *num_sample = count;
164
165 #ifdef DEBUG
166 const char *dump_env = std::getenv("eis_dump_imu");
167 int dump = 0;
168 if (dump_env) {
169 dump = atoi(dump_env);
170 }
171
172 if (dump > 0 && data != nullptr && count > 0) {
173 std::ofstream ofs("/data/imu.txt", std::ios::app);
174 if (ofs.is_open()) {
175 for (size_t i = 0; i < count; i++) {
176 ofs << data[i].id << "," << data[i].timestamp_us << "," << data[i].all.gyro.x << ","
177 << data[i].all.gyro.y << "," << data[i].all.gyro.z << std::endl;
178 }
179 }
180 ofs.close();
181 }
182 #endif
183
184 return data;
185 }
186
FreeData(mems_sensor_event_t * data)187 void EisImuAdaptor::FreeData(mems_sensor_event_t* data) {
188 XCAM_ASSERT(ctx_ != nullptr && handle_ != nullptr);
189 intf_.releaseSamplesData(handle_, data);
190 }
191
operator ()(ServiceParam<imu_param> & p)192 TaskResult ImuTask::operator()(ServiceParam<imu_param>& p) {
193 auto payload = p.payload;
194 size_t count = 0;
195
196 auto data = imu_->GetData(&count);
197 if (count > 0 && data != nullptr) {
198 payload->data =
199 std::unique_ptr<EisImuData>(new EisImuData(imu_, imu_->GetType(), data, count));
200 payload->time_point = std::chrono::system_clock::now();
201 } else {
202 payload->data = nullptr;
203 payload->time_point = std::chrono::system_clock::now();
204 return TaskResult::kFailed;
205 }
206
207 return TaskResult::kSuccess;
208 }
209
210 } // namespace RkCam
211