1 /* Copyright (c) 2014, 2020 The Linux Foundation. All rights reserved.
2 *
3 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are
5 * met:
6 * * Redistributions of source code must retain the above copyright
7 * notice, this list of conditions and the following disclaimer.
8 * * Redistributions in binary form must reproduce the above
9 * copyright notice, this list of conditions and the following
10 * disclaimer in the documentation and/or other materials provided
11 * with the distribution.
12 * * Neither the name of The Linux Foundation, nor the names of its
13 * contributors may be used to endorse or promote products derived
14 * from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
17 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
20 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
24 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
25 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
26 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 *
28 */
29 #define LOG_NDEBUG 0
30 #define LOG_TAG "LocSvc_misc_utils"
31 #include <stdio.h>
32 #include <string.h>
33 #include <inttypes.h>
34 #include <dlfcn.h>
35 #include <math.h>
36 #include <log_util.h>
37 #include <loc_misc_utils.h>
38 #include <ctype.h>
39 #include <fcntl.h>
40 #include <inttypes.h>
41
42 #ifndef MSEC_IN_ONE_SEC
43 #define MSEC_IN_ONE_SEC 1000ULL
44 #endif
45 #define GET_MSEC_FROM_TS(ts) ((ts.tv_sec * MSEC_IN_ONE_SEC) + (ts.tv_nsec + 500000)/1000000)
46
loc_util_split_string(char * raw_string,char ** split_strings_ptr,int max_num_substrings,char delimiter)47 int loc_util_split_string(char *raw_string, char **split_strings_ptr,
48 int max_num_substrings, char delimiter)
49 {
50 int raw_string_index=0;
51 int num_split_strings=0;
52 unsigned char end_string=0;
53 int raw_string_length=0;
54
55 if(!raw_string || !split_strings_ptr) {
56 LOC_LOGE("%s:%d]: NULL parameters", __func__, __LINE__);
57 num_split_strings = -1;
58 goto err;
59 }
60 LOC_LOGD("%s:%d]: raw string: %s\n", __func__, __LINE__, raw_string);
61 raw_string_length = strlen(raw_string) + 1;
62 split_strings_ptr[num_split_strings] = &raw_string[raw_string_index];
63 for(raw_string_index=0; raw_string_index < raw_string_length; raw_string_index++) {
64 if(raw_string[raw_string_index] == '\0')
65 end_string=1;
66 if((raw_string[raw_string_index] == delimiter) || end_string) {
67 raw_string[raw_string_index] = '\0';
68 if (num_split_strings < max_num_substrings) {
69 LOC_LOGD("%s:%d]: split string: %s\n",
70 __func__, __LINE__, split_strings_ptr[num_split_strings]);
71 }
72 num_split_strings++;
73 if(((raw_string_index + 1) < raw_string_length) &&
74 (num_split_strings < max_num_substrings)) {
75 split_strings_ptr[num_split_strings] = &raw_string[raw_string_index+1];
76 }
77 else {
78 break;
79 }
80 }
81 if(end_string)
82 break;
83 }
84 err:
85 LOC_LOGD("%s:%d]: num_split_strings: %d\n", __func__, __LINE__, num_split_strings);
86 return num_split_strings;
87 }
88
loc_util_trim_space(char * org_string)89 void loc_util_trim_space(char *org_string)
90 {
91 char *scan_ptr, *write_ptr;
92 char *first_nonspace = NULL, *last_nonspace = NULL;
93
94 if(org_string == NULL) {
95 LOC_LOGE("%s:%d]: NULL parameter", __func__, __LINE__);
96 goto err;
97 }
98
99 scan_ptr = write_ptr = org_string;
100
101 while (*scan_ptr) {
102 //Find the first non-space character
103 if ( !isspace(*scan_ptr) && first_nonspace == NULL) {
104 first_nonspace = scan_ptr;
105 }
106 //Once the first non-space character is found in the
107 //above check, keep shifting the characters to the left
108 //to replace the spaces
109 if (first_nonspace != NULL) {
110 *(write_ptr++) = *scan_ptr;
111 //Keep track of which was the last non-space character
112 //encountered
113 //last_nonspace will not be updated in the case where
114 //the string ends with spaces
115 if ( !isspace(*scan_ptr)) {
116 last_nonspace = write_ptr;
117 }
118 }
119 scan_ptr++;
120 }
121 //Add NULL terminator after the last non-space character
122 if (last_nonspace) { *last_nonspace = '\0'; }
123 err:
124 return;
125 }
126
logDlError(const char * failedCall)127 inline void logDlError(const char* failedCall) {
128 const char * err = dlerror();
129 LOC_LOGe("%s error: %s", failedCall, (nullptr == err) ? "unknown" : err);
130 }
131
dlGetSymFromLib(void * & libHandle,const char * libName,const char * symName)132 void* dlGetSymFromLib(void*& libHandle, const char* libName, const char* symName)
133 {
134 void* sym = nullptr;
135 if ((nullptr != libHandle || nullptr != libName) && nullptr != symName) {
136 if (nullptr == libHandle) {
137 libHandle = dlopen(libName, RTLD_NOW);
138 if (nullptr == libHandle) {
139 logDlError("dlopen");
140 }
141 }
142 // NOT else, as libHandle gets assigned 5 line above
143 if (nullptr != libHandle) {
144 sym = dlsym(libHandle, symName);
145 if (nullptr == sym) {
146 logDlError("dlsym");
147 }
148 }
149 } else {
150 LOC_LOGe("Either libHandle (%p) or libName (%p) must not be null; "
151 "symName (%p) can not be null.", libHandle, libName, symName);
152 }
153
154 return sym;
155 }
156
getQTimerTickCount()157 uint64_t getQTimerTickCount()
158 {
159 uint64_t qTimerCount = 0;
160 #if __aarch64__
161 asm volatile("mrs %0, cntvct_el0" : "=r" (qTimerCount));
162 #elif defined (__i386__) || defined (__x86_64__)
163 /* Qtimer not supported in x86 architecture */
164 qTimerCount = 0;
165 #else
166 asm volatile("mrrc p15, 1, %Q0, %R0, c14" : "=r" (qTimerCount));
167 #endif
168
169 return qTimerCount;
170 }
171
getQTimerDeltaNanos()172 uint64_t getQTimerDeltaNanos()
173 {
174 char qtimer_val_string[100];
175 char *temp;
176 uint64_t local_qtimer = 0, remote_qtimer = 0;
177 int mdm_fd = -1, wlan_fd = -1, ret = 0;
178 uint64_t delta = 0;
179
180 memset(qtimer_val_string, '\0', sizeof(qtimer_val_string));
181
182 char devNode[] = "/sys/bus/mhi/devices/0306_00.01.00/time_us";
183 for (; devNode[27] < 3 && mdm_fd < 0; devNode[27]++) {
184 mdm_fd = ::open(devNode, O_RDONLY);
185 if (mdm_fd < 0) {
186 LOC_LOGe("MDM open file: %s error: %s", devNode, strerror(errno));
187 }
188 }
189 if (mdm_fd > 0) {
190 ret = read(mdm_fd, qtimer_val_string, sizeof(qtimer_val_string)-1);
191 ::close(mdm_fd);
192 if (ret < 0) {
193 LOC_LOGe("MDM read time_us file error: %s", strerror(errno));
194 } else {
195 temp = qtimer_val_string;
196 temp = strchr(temp, ':');
197 temp = temp + 2;
198 local_qtimer = atoll(temp);
199
200 temp = strchr(temp, ':');
201 temp = temp + 2;
202 remote_qtimer = atoll(temp);
203
204 if (local_qtimer >= remote_qtimer) {
205 delta = (local_qtimer - remote_qtimer) * 1000;
206 }
207 LOC_LOGv("qtimer values in microseconds: local:%" PRIi64 " remote:%" PRIi64 ""
208 " delta in nanoseconds:%" PRIi64 "",
209 local_qtimer, remote_qtimer, delta);
210 }
211 }
212 return delta;
213 }
214
getQTimerFreq()215 uint64_t getQTimerFreq()
216 {
217 #if __aarch64__
218 uint64_t val = 0;
219 asm volatile("mrs %0, cntfrq_el0" : "=r" (val));
220 #elif defined (__i386__) || defined (__x86_64__)
221 /* Qtimer not supported in x86 architecture */
222 uint64_t val = 0;
223 #else
224 uint32_t val = 0;
225 asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (val));
226 #endif
227 return val;
228 }
229
getBootTimeMilliSec()230 uint64_t getBootTimeMilliSec()
231 {
232 struct timespec curTs;
233 clock_gettime(CLOCK_BOOTTIME, &curTs);
234 return (uint64_t)GET_MSEC_FROM_TS(curTs);
235 }
236
237 // Used for convert position/velocity from GSNS antenna based to VRP based
Matrix_MxV(float a[3][3],float b[3],float c[3])238 void Matrix_MxV(float a[3][3], float b[3], float c[3]) {
239 int i, j;
240
241 for (i=0; i<3; i++) {
242 c[i] = 0.0f;
243 for (j=0; j<3; j++)
244 c[i] += a[i][j] * b[j];
245 }
246 }
247
248 // Used for convert position/velocity from GNSS antenna based to VRP based
Matrix_Skew(float a[3],float c[3][3])249 void Matrix_Skew(float a[3], float c[3][3]) {
250 c[0][0] = 0.0f;
251 c[0][1] = -a[2];
252 c[0][2] = a[1];
253 c[1][0] = a[2];
254 c[1][1] = 0.0f;
255 c[1][2] = -a[0];
256 c[2][0] = -a[1];
257 c[2][1] = a[0];
258 c[2][2] = 0.0f;
259 }
260
261 // Used for convert position/velocity from GNSS antenna based to VRP based
Euler2Dcm(float euler[3],float dcm[3][3])262 void Euler2Dcm(float euler[3], float dcm[3][3]) {
263 float cr = 0.0, sr = 0.0, cp = 0.0, sp = 0.0, ch = 0.0, sh = 0.0;
264
265 cr = cosf(euler[0]);
266 sr = sinf(euler[0]);
267 cp = cosf(euler[1]);
268 sp = sinf(euler[1]);
269 ch = cosf(euler[2]);
270 sh = sinf(euler[2]);
271
272 dcm[0][0] = cp * ch;
273 dcm[0][1] = (sp*sr*ch) - (cr*sh);
274 dcm[0][2] = (cr*sp*ch) + (sh*sr);
275
276 dcm[1][0] = cp * sh;
277 dcm[1][1] = (sr*sp*sh) + (cr*ch);
278 dcm[1][2] = (cr*sp*sh) - (sr*ch);
279
280 dcm[2][0] = -sp;
281 dcm[2][1] = sr * cp;
282 dcm[2][2] = cr * cp;
283 }
284
285 // Used for convert position from GSNS based to VRP based
286 // The converted position will be stored in the llaInfo parameter.
287 #define A6DOF_WGS_A (6378137.0f)
288 #define A6DOF_WGS_B (6335439.0f)
289 #define A6DOF_WGS_E2 (0.00669437999014f)
loc_convert_lla_gnss_to_vrp(double lla[3],float rollPitchYaw[3],float leverArm[3])290 void loc_convert_lla_gnss_to_vrp(double lla[3], float rollPitchYaw[3],
291 float leverArm[3]) {
292 LOC_LOGv("lla: %f, %f, %f, lever arm: %f %f %f, "
293 "rollpitchyaw: %f %f %f",
294 lla[0], lla[1], lla[2],
295 leverArm[0], leverArm[1], leverArm[2],
296 rollPitchYaw[0], rollPitchYaw[1], rollPitchYaw[2]);
297
298 float cnb[3][3];
299 memset(cnb, 0, sizeof(cnb));
300 Euler2Dcm(rollPitchYaw, cnb);
301
302 float sl = sin(lla[0]);
303 float cl = cos(lla[0]);
304 float sf = 1.0f / (1.0f - A6DOF_WGS_E2 * sl* sl);
305 float sfr = sqrtf(sf);
306
307 float rn = A6DOF_WGS_B * sf * sfr + lla[2];
308 float re = A6DOF_WGS_A * sfr + lla[2];
309
310 float deltaNEU[3];
311
312 // gps_pos_lla = imu_pos_lla + Cbn*la_b .* [1/geo.Rn; 1/(geo.Re*geo.cL); -1];
313 Matrix_MxV(cnb, leverArm, deltaNEU);
314
315 // NED to lla conversion
316 lla[0] = lla[0] + deltaNEU[0] / rn;
317 lla[1] = lla[1] + deltaNEU[1] / (re * cl);
318 lla[2] = lla[2] + deltaNEU[2];
319 }
320
321 // Used for convert velocity from GSNS based to VRP based
322 // The converted velocity will be stored in the enuVelocity parameter.
loc_convert_velocity_gnss_to_vrp(float enuVelocity[3],float rollPitchYaw[3],float rollPitchYawRate[3],float leverArm[3])323 void loc_convert_velocity_gnss_to_vrp(float enuVelocity[3], float rollPitchYaw[3],
324 float rollPitchYawRate[3], float leverArm[3]) {
325
326 LOC_LOGv("enu velocity: %f, %f, %f, lever arm: %f %f %f, roll pitch yaw: %f %f %f,"
327 "rollpitchyawRate: %f %f %f",
328 enuVelocity[0], enuVelocity[1], enuVelocity[2],
329 leverArm[0], leverArm[1], leverArm[2],
330 rollPitchYaw[0], rollPitchYaw[1], rollPitchYaw[2],
331 rollPitchYawRate[0], rollPitchYawRate[1], rollPitchYawRate[2]);
332
333 float cnb[3][3];
334 memset(cnb, 0, sizeof(cnb));
335 Euler2Dcm(rollPitchYaw, cnb);
336
337 float skewLA[3][3];
338 memset(skewLA, 0, sizeof(skewLA));
339 Matrix_Skew(leverArm, skewLA);
340
341 float tmp[3];
342 float deltaEnuVelocity[3];
343 memset(tmp, 0, sizeof(tmp));
344 memset(deltaEnuVelocity, 0, sizeof(deltaEnuVelocity));
345 Matrix_MxV(skewLA, rollPitchYawRate, tmp);
346 Matrix_MxV(cnb, tmp, deltaEnuVelocity);
347
348 enuVelocity[0] = enuVelocity[0] - deltaEnuVelocity[0];
349 enuVelocity[1] = enuVelocity[1] - deltaEnuVelocity[1];
350 enuVelocity[2] = enuVelocity[2] - deltaEnuVelocity[2];
351 }
352