| /* Copyright (c) 2014, 2020 The Linux Foundation. All rights reserved. |
| * |
| * Redistribution and use in source and binary forms, with or without |
| * modification, are permitted provided that the following conditions are |
| * met: |
| * * Redistributions of source code must retain the above copyright |
| * notice, this list of conditions and the following disclaimer. |
| * * Redistributions in binary form must reproduce the above |
| * copyright notice, this list of conditions and the following |
| * disclaimer in the documentation and/or other materials provided |
| * with the distribution. |
| * * Neither the name of The Linux Foundation, nor the names of its |
| * contributors may be used to endorse or promote products derived |
| * from this software without specific prior written permission. |
| * |
| * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED |
| * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF |
| * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT |
| * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS |
| * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR |
| * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, |
| * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE |
| * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN |
| * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| * |
| */ |
| #define LOG_NDEBUG 0 |
| #define LOG_TAG "LocSvc_misc_utils" |
| #include <stdio.h> |
| #include <string.h> |
| #include <inttypes.h> |
| #include <dlfcn.h> |
| #include <math.h> |
| #include <log_util.h> |
| #include <loc_misc_utils.h> |
| #include <ctype.h> |
| #include <fcntl.h> |
| #include <inttypes.h> |
| |
| #ifndef MSEC_IN_ONE_SEC |
| #define MSEC_IN_ONE_SEC 1000ULL |
| #endif |
| #define GET_MSEC_FROM_TS(ts) ((ts.tv_sec * MSEC_IN_ONE_SEC) + (ts.tv_nsec + 500000)/1000000) |
| |
| int loc_util_split_string(char *raw_string, char **split_strings_ptr, |
| int max_num_substrings, char delimiter) |
| { |
| int raw_string_index=0; |
| int num_split_strings=0; |
| unsigned char end_string=0; |
| int raw_string_length=0; |
| |
| if(!raw_string || !split_strings_ptr) { |
| LOC_LOGE("%s:%d]: NULL parameters", __func__, __LINE__); |
| num_split_strings = -1; |
| goto err; |
| } |
| LOC_LOGD("%s:%d]: raw string: %s\n", __func__, __LINE__, raw_string); |
| raw_string_length = strlen(raw_string) + 1; |
| split_strings_ptr[num_split_strings] = &raw_string[raw_string_index]; |
| for(raw_string_index=0; raw_string_index < raw_string_length; raw_string_index++) { |
| if(raw_string[raw_string_index] == '\0') |
| end_string=1; |
| if((raw_string[raw_string_index] == delimiter) || end_string) { |
| raw_string[raw_string_index] = '\0'; |
| if (num_split_strings < max_num_substrings) { |
| LOC_LOGD("%s:%d]: split string: %s\n", |
| __func__, __LINE__, split_strings_ptr[num_split_strings]); |
| } |
| num_split_strings++; |
| if(((raw_string_index + 1) < raw_string_length) && |
| (num_split_strings < max_num_substrings)) { |
| split_strings_ptr[num_split_strings] = &raw_string[raw_string_index+1]; |
| } |
| else { |
| break; |
| } |
| } |
| if(end_string) |
| break; |
| } |
| err: |
| LOC_LOGD("%s:%d]: num_split_strings: %d\n", __func__, __LINE__, num_split_strings); |
| return num_split_strings; |
| } |
| |
| void loc_util_trim_space(char *org_string) |
| { |
| char *scan_ptr, *write_ptr; |
| char *first_nonspace = NULL, *last_nonspace = NULL; |
| |
| if(org_string == NULL) { |
| LOC_LOGE("%s:%d]: NULL parameter", __func__, __LINE__); |
| goto err; |
| } |
| |
| scan_ptr = write_ptr = org_string; |
| |
| while (*scan_ptr) { |
| //Find the first non-space character |
| if ( !isspace(*scan_ptr) && first_nonspace == NULL) { |
| first_nonspace = scan_ptr; |
| } |
| //Once the first non-space character is found in the |
| //above check, keep shifting the characters to the left |
| //to replace the spaces |
| if (first_nonspace != NULL) { |
| *(write_ptr++) = *scan_ptr; |
| //Keep track of which was the last non-space character |
| //encountered |
| //last_nonspace will not be updated in the case where |
| //the string ends with spaces |
| if ( !isspace(*scan_ptr)) { |
| last_nonspace = write_ptr; |
| } |
| } |
| scan_ptr++; |
| } |
| //Add NULL terminator after the last non-space character |
| if (last_nonspace) { *last_nonspace = '\0'; } |
| err: |
| return; |
| } |
| |
| inline void logDlError(const char* failedCall) { |
| const char * err = dlerror(); |
| LOC_LOGe("%s error: %s", failedCall, (nullptr == err) ? "unknown" : err); |
| } |
| |
| void* dlGetSymFromLib(void*& libHandle, const char* libName, const char* symName) |
| { |
| void* sym = nullptr; |
| if ((nullptr != libHandle || nullptr != libName) && nullptr != symName) { |
| if (nullptr == libHandle) { |
| libHandle = dlopen(libName, RTLD_NOW); |
| if (nullptr == libHandle) { |
| logDlError("dlopen"); |
| } |
| } |
| // NOT else, as libHandle gets assigned 5 line above |
| if (nullptr != libHandle) { |
| sym = dlsym(libHandle, symName); |
| if (nullptr == sym) { |
| logDlError("dlsym"); |
| } |
| } |
| } else { |
| LOC_LOGe("Either libHandle (%p) or libName (%p) must not be null; " |
| "symName (%p) can not be null.", libHandle, libName, symName); |
| } |
| |
| return sym; |
| } |
| |
| uint64_t getQTimerTickCount() |
| { |
| uint64_t qTimerCount = 0; |
| #if __aarch64__ |
| asm volatile("mrs %0, cntvct_el0" : "=r" (qTimerCount)); |
| #elif defined (__i386__) || defined (__x86_64__) |
| /* Qtimer not supported in x86 architecture */ |
| qTimerCount = 0; |
| #else |
| asm volatile("mrrc p15, 1, %Q0, %R0, c14" : "=r" (qTimerCount)); |
| #endif |
| |
| return qTimerCount; |
| } |
| |
| uint64_t getQTimerDeltaNanos() |
| { |
| char qtimer_val_string[100]; |
| char *temp; |
| uint64_t local_qtimer = 0, remote_qtimer = 0; |
| int mdm_fd = -1, wlan_fd = -1, ret = 0; |
| uint64_t delta = 0; |
| |
| memset(qtimer_val_string, '\0', sizeof(qtimer_val_string)); |
| |
| char devNode[] = "/sys/bus/mhi/devices/0306_00.01.00/time_us"; |
| for (; devNode[27] < 3 && mdm_fd < 0; devNode[27]++) { |
| mdm_fd = ::open(devNode, O_RDONLY); |
| if (mdm_fd < 0) { |
| LOC_LOGe("MDM open file: %s error: %s", devNode, strerror(errno)); |
| } |
| } |
| if (mdm_fd > 0) { |
| ret = read(mdm_fd, qtimer_val_string, sizeof(qtimer_val_string)-1); |
| ::close(mdm_fd); |
| if (ret < 0) { |
| LOC_LOGe("MDM read time_us file error: %s", strerror(errno)); |
| } else { |
| temp = qtimer_val_string; |
| temp = strchr(temp, ':'); |
| temp = temp + 2; |
| local_qtimer = atoll(temp); |
| |
| temp = strchr(temp, ':'); |
| temp = temp + 2; |
| remote_qtimer = atoll(temp); |
| |
| if (local_qtimer >= remote_qtimer) { |
| delta = (local_qtimer - remote_qtimer) * 1000; |
| } |
| LOC_LOGv("qtimer values in microseconds: local:%" PRIi64 " remote:%" PRIi64 "" |
| " delta in nanoseconds:%" PRIi64 "", |
| local_qtimer, remote_qtimer, delta); |
| } |
| } |
| return delta; |
| } |
| |
| uint64_t getQTimerFreq() |
| { |
| #if __aarch64__ |
| uint64_t val = 0; |
| asm volatile("mrs %0, cntfrq_el0" : "=r" (val)); |
| #elif defined (__i386__) || defined (__x86_64__) |
| /* Qtimer not supported in x86 architecture */ |
| uint64_t val = 0; |
| #else |
| uint32_t val = 0; |
| asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (val)); |
| #endif |
| return val; |
| } |
| |
| uint64_t getBootTimeMilliSec() |
| { |
| struct timespec curTs; |
| clock_gettime(CLOCK_BOOTTIME, &curTs); |
| return (uint64_t)GET_MSEC_FROM_TS(curTs); |
| } |
| |
| // Used for convert position/velocity from GSNS antenna based to VRP based |
| void Matrix_MxV(float a[3][3], float b[3], float c[3]) { |
| int i, j; |
| |
| for (i=0; i<3; i++) { |
| c[i] = 0.0f; |
| for (j=0; j<3; j++) |
| c[i] += a[i][j] * b[j]; |
| } |
| } |
| |
| // Used for convert position/velocity from GNSS antenna based to VRP based |
| void Matrix_Skew(float a[3], float c[3][3]) { |
| c[0][0] = 0.0f; |
| c[0][1] = -a[2]; |
| c[0][2] = a[1]; |
| c[1][0] = a[2]; |
| c[1][1] = 0.0f; |
| c[1][2] = -a[0]; |
| c[2][0] = -a[1]; |
| c[2][1] = a[0]; |
| c[2][2] = 0.0f; |
| } |
| |
| // Used for convert position/velocity from GNSS antenna based to VRP based |
| void Euler2Dcm(float euler[3], float dcm[3][3]) { |
| float cr = 0.0, sr = 0.0, cp = 0.0, sp = 0.0, ch = 0.0, sh = 0.0; |
| |
| cr = cosf(euler[0]); |
| sr = sinf(euler[0]); |
| cp = cosf(euler[1]); |
| sp = sinf(euler[1]); |
| ch = cosf(euler[2]); |
| sh = sinf(euler[2]); |
| |
| dcm[0][0] = cp * ch; |
| dcm[0][1] = (sp*sr*ch) - (cr*sh); |
| dcm[0][2] = (cr*sp*ch) + (sh*sr); |
| |
| dcm[1][0] = cp * sh; |
| dcm[1][1] = (sr*sp*sh) + (cr*ch); |
| dcm[1][2] = (cr*sp*sh) - (sr*ch); |
| |
| dcm[2][0] = -sp; |
| dcm[2][1] = sr * cp; |
| dcm[2][2] = cr * cp; |
| } |
| |
| // Used for convert position from GSNS based to VRP based |
| // The converted position will be stored in the llaInfo parameter. |
| #define A6DOF_WGS_A (6378137.0f) |
| #define A6DOF_WGS_B (6335439.0f) |
| #define A6DOF_WGS_E2 (0.00669437999014f) |
| void loc_convert_lla_gnss_to_vrp(double lla[3], float rollPitchYaw[3], |
| float leverArm[3]) { |
| LOC_LOGv("lla: %f, %f, %f, lever arm: %f %f %f, " |
| "rollpitchyaw: %f %f %f", |
| lla[0], lla[1], lla[2], |
| leverArm[0], leverArm[1], leverArm[2], |
| rollPitchYaw[0], rollPitchYaw[1], rollPitchYaw[2]); |
| |
| float cnb[3][3]; |
| memset(cnb, 0, sizeof(cnb)); |
| Euler2Dcm(rollPitchYaw, cnb); |
| |
| float sl = sin(lla[0]); |
| float cl = cos(lla[0]); |
| float sf = 1.0f / (1.0f - A6DOF_WGS_E2 * sl* sl); |
| float sfr = sqrtf(sf); |
| |
| float rn = A6DOF_WGS_B * sf * sfr + lla[2]; |
| float re = A6DOF_WGS_A * sfr + lla[2]; |
| |
| float deltaNEU[3]; |
| |
| // gps_pos_lla = imu_pos_lla + Cbn*la_b .* [1/geo.Rn; 1/(geo.Re*geo.cL); -1]; |
| Matrix_MxV(cnb, leverArm, deltaNEU); |
| |
| // NED to lla conversion |
| lla[0] = lla[0] + deltaNEU[0] / rn; |
| lla[1] = lla[1] + deltaNEU[1] / (re * cl); |
| lla[2] = lla[2] + deltaNEU[2]; |
| } |
| |
| // Used for convert velocity from GSNS based to VRP based |
| // The converted velocity will be stored in the enuVelocity parameter. |
| void loc_convert_velocity_gnss_to_vrp(float enuVelocity[3], float rollPitchYaw[3], |
| float rollPitchYawRate[3], float leverArm[3]) { |
| |
| LOC_LOGv("enu velocity: %f, %f, %f, lever arm: %f %f %f, roll pitch yaw: %f %f %f," |
| "rollpitchyawRate: %f %f %f", |
| enuVelocity[0], enuVelocity[1], enuVelocity[2], |
| leverArm[0], leverArm[1], leverArm[2], |
| rollPitchYaw[0], rollPitchYaw[1], rollPitchYaw[2], |
| rollPitchYawRate[0], rollPitchYawRate[1], rollPitchYawRate[2]); |
| |
| float cnb[3][3]; |
| memset(cnb, 0, sizeof(cnb)); |
| Euler2Dcm(rollPitchYaw, cnb); |
| |
| float skewLA[3][3]; |
| memset(skewLA, 0, sizeof(skewLA)); |
| Matrix_Skew(leverArm, skewLA); |
| |
| float tmp[3]; |
| float deltaEnuVelocity[3]; |
| memset(tmp, 0, sizeof(tmp)); |
| memset(deltaEnuVelocity, 0, sizeof(deltaEnuVelocity)); |
| Matrix_MxV(skewLA, rollPitchYawRate, tmp); |
| Matrix_MxV(cnb, tmp, deltaEnuVelocity); |
| |
| enuVelocity[0] = enuVelocity[0] - deltaEnuVelocity[0]; |
| enuVelocity[1] = enuVelocity[1] - deltaEnuVelocity[1]; |
| enuVelocity[2] = enuVelocity[2] - deltaEnuVelocity[2]; |
| } |
