Naresh Munagala | 55842c3 | 2020-03-10 16:24:03 +0530 | [diff] [blame] | 1 | /* Copyright (c) 2014, 2020 The Linux Foundation. All rights reserved. |
Deven Patel | a376de4 | 2016-03-15 12:20:01 -0700 | [diff] [blame] | 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 | */ |
Baili Feng | 4c9c783 | 2017-07-03 21:00:31 +0800 | [diff] [blame] | 29 | #define LOG_NDEBUG 0 |
| 30 | #define LOG_TAG "LocSvc_misc_utils" |
Deven Patel | a376de4 | 2016-03-15 12:20:01 -0700 | [diff] [blame] | 31 | #include <stdio.h> |
| 32 | #include <string.h> |
Mike Cailean | 8884e4c | 2020-06-30 15:34:00 -0700 | [diff] [blame] | 33 | #include <inttypes.h> |
Kevin Tang | ea72a70 | 2019-02-25 13:31:49 -0800 | [diff] [blame] | 34 | #include <dlfcn.h> |
Wei (GPS SD) Chen | beeffc5 | 2020-09-03 13:29:47 -0700 | [diff] [blame] | 35 | #include <math.h> |
Kevin Tang | 61de97e | 2016-09-12 17:20:55 -0700 | [diff] [blame] | 36 | #include <log_util.h> |
Deven Patel | a376de4 | 2016-03-15 12:20:01 -0700 | [diff] [blame] | 37 | #include <loc_misc_utils.h> |
| 38 | #include <ctype.h> |
Mike Cailean | 8884e4c | 2020-06-30 15:34:00 -0700 | [diff] [blame] | 39 | #include <fcntl.h> |
| 40 | #include <inttypes.h> |
Deven Patel | a376de4 | 2016-03-15 12:20:01 -0700 | [diff] [blame] | 41 | |
Hoss Zhou | 5aa2f7f | 2020-08-05 20:15:43 +0800 | [diff] [blame] | 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 | |
Deven Patel | a376de4 | 2016-03-15 12:20:01 -0700 | [diff] [blame] | 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'; |
Saurabh Srivastava | 24dd04d | 2020-07-13 16:15:02 +0530 | [diff] [blame] | 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 | } |
Deven Patel | a376de4 | 2016-03-15 12:20:01 -0700 | [diff] [blame] | 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 | |
| 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 | } |
Kevin Tang | ea72a70 | 2019-02-25 13:31:49 -0800 | [diff] [blame] | 126 | |
| 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 | |
| 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 | } |
Bhavna Sharma | f9dca44 | 2019-07-25 11:33:50 -0700 | [diff] [blame] | 156 | |
| 157 | uint64_t getQTimerTickCount() |
| 158 | { |
| 159 | uint64_t qTimerCount = 0; |
| 160 | #if __aarch64__ |
| 161 | asm volatile("mrs %0, cntvct_el0" : "=r" (qTimerCount)); |
Harikrishnan Hariharan | 5b35562 | 2020-05-06 15:10:47 +0530 | [diff] [blame] | 162 | #elif defined (__i386__) || defined (__x86_64__) |
| 163 | /* Qtimer not supported in x86 architecture */ |
| 164 | qTimerCount = 0; |
Bhavna Sharma | f9dca44 | 2019-07-25 11:33:50 -0700 | [diff] [blame] | 165 | #else |
| 166 | asm volatile("mrrc p15, 1, %Q0, %R0, c14" : "=r" (qTimerCount)); |
| 167 | #endif |
| 168 | |
| 169 | return qTimerCount; |
| 170 | } |
Naresh Munagala | 55842c3 | 2020-03-10 16:24:03 +0530 | [diff] [blame] | 171 | |
Mike Cailean | 8884e4c | 2020-06-30 15:34:00 -0700 | [diff] [blame] | 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 | |
Naresh Munagala | 55842c3 | 2020-03-10 16:24:03 +0530 | [diff] [blame] | 215 | uint64_t getQTimerFreq() |
| 216 | { |
| 217 | #if __aarch64__ |
| 218 | uint64_t val = 0; |
| 219 | asm volatile("mrs %0, cntfrq_el0" : "=r" (val)); |
Harikrishnan Hariharan | 5b35562 | 2020-05-06 15:10:47 +0530 | [diff] [blame] | 220 | #elif defined (__i386__) || defined (__x86_64__) |
| 221 | /* Qtimer not supported in x86 architecture */ |
| 222 | uint64_t val = 0; |
Naresh Munagala | 55842c3 | 2020-03-10 16:24:03 +0530 | [diff] [blame] | 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 | } |
Hoss Zhou | 5aa2f7f | 2020-08-05 20:15:43 +0800 | [diff] [blame] | 229 | |
| 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 | } |
Kevin Tang | 3beea52 | 2020-10-11 20:58:57 -0700 | [diff] [blame] | 236 | |
Wei (GPS SD) Chen | beeffc5 | 2020-09-03 13:29:47 -0700 | [diff] [blame] | 237 | // Used for convert position/velocity from GSNS antenna based to VRP based |
| 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 |
| 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 |
| 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) |
| 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 | |
Wei (GPS SD) Chen | 393236f | 2020-10-23 10:38:24 -0700 | [diff] [blame] | 310 | float deltaNEU[3]; |
Wei (GPS SD) Chen | beeffc5 | 2020-09-03 13:29:47 -0700 | [diff] [blame] | 311 | |
| 312 | // gps_pos_lla = imu_pos_lla + Cbn*la_b .* [1/geo.Rn; 1/(geo.Re*geo.cL); -1]; |
Wei (GPS SD) Chen | 393236f | 2020-10-23 10:38:24 -0700 | [diff] [blame] | 313 | Matrix_MxV(cnb, leverArm, deltaNEU); |
Wei (GPS SD) Chen | beeffc5 | 2020-09-03 13:29:47 -0700 | [diff] [blame] | 314 | |
| 315 | // NED to lla conversion |
Wei (GPS SD) Chen | 393236f | 2020-10-23 10:38:24 -0700 | [diff] [blame] | 316 | lla[0] = lla[0] + deltaNEU[0] / rn; |
| 317 | lla[1] = lla[1] + deltaNEU[1] / (re * cl); |
| 318 | lla[2] = lla[2] + deltaNEU[2]; |
Wei (GPS SD) Chen | beeffc5 | 2020-09-03 13:29:47 -0700 | [diff] [blame] | 319 | } |
| 320 | |
| 321 | // Used for convert velocity from GSNS based to VRP based |
| 322 | // The converted velocity will be stored in the enuVelocity parameter. |
| 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 | } |