/* * Copyright (c) HiSilicon (Shanghai) Technologies Co., Ltd. 2024-2024. All rights reserved. * Description: pgnss encode * Author: * Create: */ #include "pgnss_encode.h" #include "fs_user_common.h" #include "gnss_datatypes.h" #include "osal_task.h" #include "rtc_api.h" #include "securec.h" #include "sys_config.h" #include "tiot_service_interface.h" #include #include #include #define ENABLE_PRINT_INFO 1 #define ENABLE_DEBUG 1 #if ENABLE_PRINT_INFO #define static_print_info(...) sys_gps_log_i(__VA_ARGS__) //一般信息打印宏控制 #define static_print_warn(...) sys_gps_log_w(__VA_ARGS__) //警告信息打印一般常开 #define static_print_error(...) sys_gps_log_e(__VA_ARGS__) //错误信息打印一般常开 #if ENABLE_DEBUG #define static_print_debug(...) sys_gps_log_d(__VA_ARGS__) #else #define static_print_debug(...) #endif #else #define static_print_info(...) #define static_print_warn(...) #define static_print_error(...) #endif #define GPS_PATH TJD_FS_DIR_GPS "/" #define GNSS_ENCODE_BUFF_MAX_LEN 9300 #define FILE_PATH_LEN 1024 #define GLO_EPH_VALID_TIME 900 #define OTHER_EPH_VALID_TIME 7200 #define ONE_CENTURY_YEARS 100 #define FOUR_CENTURY_YEARS 400 #define LEAP_YEAR_LENGTH 4 #define UNIX_TIME_START_YEAR 1970 // start year of unix time #define TIME_UNIT 1000 #define SECONDS_PER_MIN (60) #define SECONDS_PER_HOUR (60 * SECONDS_PER_MIN) #define SECONDS_PER_DAY (24 * SECONDS_PER_HOUR) #define SECONDS_PER_NORMAL_YEAR (365 * SECONDS_PER_DAY) #define NON_GLO_GNSS_TYPES 4 #define GLO_EPH_NUM_ONE_FILE 8 #define OTHER_ASSIST_TYPES 7 #define INJECT_SLEEP_MS_TIME 500 #define PGNSS_FILE_MAX_SIZE 1024000 /** days per month on normal and leap year */ int32_t g_normMonthDays[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; int32_t g_leapMonthDays[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; // 监听消息 static uint8_t g_ackBuff[REPORT_MAX_BYTES] = {0}; /* 获取begin和end年份之间的闰年数 */ int32_t GetLeapYearNum(uint16_t begin, uint16_t end) { if (end <= begin || end < 1) { return 0; } int32_t numPer400Year = (end - 1) / FOUR_CENTURY_YEARS - begin / FOUR_CENTURY_YEARS; int32_t numPer100Year = (end - 1) / ONE_CENTURY_YEARS - begin / ONE_CENTURY_YEARS; int32_t numPer4Year = (end - 1) / LEAP_YEAR_LENGTH - begin / LEAP_YEAR_LENGTH; return (numPer400Year + numPer4Year - numPer100Year); } /* 闰年判断 */ int32_t CheckIfLeapYear(uint16_t year) { if ((year % ONE_CENTURY_YEARS) == 0) { if ((year % FOUR_CENTURY_YEARS) == 0) { return 1; } } else { if ((year % LEAP_YEAR_LENGTH) == 0) { return 1; } } return 0; } /* 将日历时(年月日时分秒) 转换为 unix时间(1970年1月1日起的时间秒数) */ int32_t Utc2UnixTime(GnssUtcTime utcTime, uint32_t *unixTime) { if (unixTime == NULL) { static_print_debug("error: [Utc2UnixTime] null pointer!"); return 0; } if (utcTime.month > 12 || utcTime.valid == 0) { static_print_debug("error: [Utc2UnixTime] utcTime invalid!"); return 0; } // 判断是否为闰年 int32_t *monthDays = (CheckIfLeapYear(utcTime.year) == 1) ? g_leapMonthDays : g_normMonthDays; // 计算从1970.01.01开始的unix 秒数 uint32_t timestamp = 0; timestamp += (uint32_t)(utcTime.year - UNIX_TIME_START_YEAR) * SECONDS_PER_NORMAL_YEAR; timestamp += (uint32_t)GetLeapYearNum(UNIX_TIME_START_YEAR, utcTime.year) * SECONDS_PER_DAY; for (int32_t i = 1; i < utcTime.month; i++) { timestamp += (uint32_t)monthDays[i - 1] * SECONDS_PER_DAY; } timestamp += (uint32_t)(utcTime.day - 1) * SECONDS_PER_DAY; timestamp += (uint32_t)utcTime.hour * SECONDS_PER_HOUR; timestamp += (uint32_t)utcTime.minute * SECONDS_PER_MIN; uint32_t timestampSec = (uint32_t)(utcTime.second + utcTime.ms / TIME_UNIT); *unixTime = timestamp + timestampSec; return 1; } /* 计算命令内容校准和 */ static uint16_t CalcChecksum(uint8_t *buff, uint16_t len) { uint16_t checksum = 0; uint16_t i = 0; while (i++ < len) { checksum += *buff; buff++; } return checksum; } int32_t PgnssInjectUtcTime(tiot_handle handle, GnssUtcTime *utcTime, uint32_t *ts) { uint8_t *buff = (uint8_t *)malloc(GNSS_ENCODE_BUFF_MAX_LEN); if (buff == NULL) { static_print_error("malloc failed in PgnssINjectUtcTime"); return 0; } memset_s(buff, GNSS_ENCODE_BUFF_MAX_LEN, 0, GNSS_ENCODE_BUFF_MAX_LEN); GnssMsg *hdr = (GnssMsg *)buff; hdr->cmd = 0x0404; hdr->sequence = 0; errno_t copyRet = memcpy_s(hdr->data, (GNSS_ENCODE_BUFF_MAX_LEN - sizeof(GnssMsg)), utcTime, sizeof(GnssUtcTime)); if (copyRet != EOK) { static_print_debug("memcpy UtcTime failed! Errno is %d", copyRet); free(buff); return 0; } hdr->dataLength = sizeof(GnssUtcTime); hdr->checkSum = CalcChecksum(hdr->data, hdr->dataLength); int32_t ret = tiot_service_write(handle, buff, (sizeof(GnssMsg) + hdr->dataLength)); if (ret < 0) { static_print_error("PgnssInjectUtcTime error: %d", ret); free(buff); return 0; } // if (Utc2UnixTime((*utcTime), ts) == 0) { // static_print_error("error: Utc2UnixTime error"); // free(buff); // return 0; // } uint64_t time_stamp = 0; tjd_driver_rtc_get_ops()->get_timestamp(&time_stamp); *ts = time_stamp; free(buff); return 1; } static uint16_t FillGnssMessageData(uint8_t *inBuff, uint32_t inLen, uint8_t *outBuff, uint32_t outLen) { errno_t ret = memcpy_s(outBuff, outLen, inBuff, inLen); if (ret != EOK) { static_print_debug("memcpy XgnssData failed! Errno is %d", ret); return 0; } return inLen; } static int32_t EncodeGnssMsg(uint8_t *inBuff, uint32_t inLen, uint8_t *outBuff, uint32_t outLen) { uint16_t len = FillGnssMessageData(inBuff, inLen, outBuff, outLen); if (len > 0) { return 1; } return 0; } static void ListenAck(tiot_handle handle) { int32_t i = 0; for (;;) { // 监听 int32_t ret = tiot_service_read(handle, g_ackBuff, REPORT_MAX_BYTES, LISTEN_TIME_THR); if (ret > 0) { uint16_t *type = (uint16_t *)g_ackBuff; // 二进制消息类型 if (type[0] == 0) { g_ackBuff[ret] = '\0'; } i++; } if (i >= 1) { break; } } } static int32_t EncodeAndInjectCmd(tiot_handle handle, GnssCmdType cmdType, uint8_t *inBuff, uint32_t inLen) { // 获取GNSS业务消息头 int32_t len = 0; uint8_t *buff = (uint8_t *)malloc(GNSS_ENCODE_BUFF_MAX_LEN); if (buff == NULL) { static_print_debug("error: malloc failed in EncodeAndInjectCmd"); return 0; } memset_s(buff, GNSS_ENCODE_BUFF_MAX_LEN, 0, GNSS_ENCODE_BUFF_MAX_LEN); GnssMsg *gnssHdr = (GnssMsg *)buff; len += sizeof(GnssMsg); gnssHdr->sequence = 0; int32_t retLen = EncodeGnssMsg(inBuff, inLen, buff + len, (GNSS_ENCODE_BUFF_MAX_LEN - len)); if (retLen != 1) { static_print_debug("error: Encode Cmd failed: %d", cmdType); free(buff); return retLen; } gnssHdr->cmd = cmdType; gnssHdr->dataLength = inLen; gnssHdr->checkSum = CalcChecksum(gnssHdr->data, gnssHdr->dataLength); int32_t ret = tiot_service_write(handle, buff, (sizeof(GnssMsg) + gnssHdr->dataLength)); if (ret < 0) { static_print_debug("error: Inject Cmd failed: %d", cmdType); free(buff); return 0; } ListenAck(handle); static_print_debug("Inject Cmd SUCCESS: %x", cmdType); free(buff); return 1; } void PgnssSendFileContent(tiot_handle handle, const char *fileName, GnssCmdType cmdType) { FILE *fp = fopen(fileName, "rb"); if (fp == NULL) { static_print_error("error: open file failed: %s", fileName); return; } fseek(fp, 0, SEEK_END); int32_t len = ftell(fp); fseek(fp, 0, SEEK_SET); if (len < 0 || len > PGNSS_FILE_MAX_SIZE) { fclose(fp); static_print_error("error: get file len failed: %s", fileName); return; } uint8_t *buff = (uint8_t *)malloc(len); if (buff == NULL) { fclose(fp); static_print_error("error: malloc file len failed: %s", fileName); return; } int32_t dataSize = fread(buff, len, 1, fp); if (dataSize < 0) { fclose(fp); static_print_error("error: read file failed: %s\n", fileName); free(buff); return; } fclose(fp); EncodeAndInjectCmd(handle, cmdType, buff, len); free(buff); } static int32_t FormatFileName(char *fileName, char *dName) { int32_t ret = snprintf_s(fileName, FILE_PATH_LEN, (FILE_PATH_LEN - 1), "%s%s", GPS_PATH "", dName); if (ret <= 0) { return 0; } return 1; } int32_t GetEphFileName(int32_t ts, const char *prefix, char *fileName) { int32_t interval = (strcmp(prefix, "GLO") == 0) ? GLO_EPH_VALID_TIME : OTHER_EPH_VALID_TIME; DIR *dir = opendir(GPS_PATH ""); if (dir == NULL) { static_print_error("Failed to open /user/tjd_gps directory."); return 0; } int32_t time, ret; struct dirent *entry; for (entry = readdir(dir); entry != NULL; entry = readdir(dir)) { // 检查文件名 if (strstr(entry->d_name, ".eph") != NULL) { char *p = strrchr(entry->d_name, '_'); if (p != NULL) { char *q = strstr(entry->d_name, prefix); if (q != NULL) { time = atoi(p + 1); if (abs(time - ts) <= interval) { ret = FormatFileName(fileName, entry->d_name); closedir(dir); return ret; } } } } } closedir(dir); return 0; } void PgnssSendEphFile(tiot_handle handle, const char *prefix, GnssCmdType cmdType, uint32_t ts) { char fileName[FILE_PATH_LEN] = {0}; int32_t ret = GetEphFileName(ts, prefix, fileName); if (ret == 0) { static_print_error("error: GetEphFileName failed\n"); return; } static_print_debug("GetEphFileName success"); PgnssSendFileContent(handle, fileName, cmdType); } void PgnssInjectAllEph(tiot_handle handle, uint32_t ts) { PgnssSendEphFile(handle, "GPS", CMD_PGNSS_INJECT_EPH_GPS, ts); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GPS eph success"); PgnssSendEphFile(handle, "GLO", CMD_PGNSS_INJECT_EPH_GANSS, ts); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GLO eph success"); PgnssSendEphFile(handle, "BDS", CMD_PGNSS_INJECT_EPH_GANSS, ts); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send BDS eph success"); PgnssSendEphFile(handle, "GAL", CMD_PGNSS_INJECT_EPH_GANSS, ts); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GAL eph success"); PgnssSendEphFile(handle, "QZS", CMD_PGNSS_INJECT_EPH_GANSS, ts); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send QZS eph success"); } void PgnssInjectOtherAssist(tiot_handle handle) { PgnssSendFileContent(handle, GPS_PATH "GPS_UTC.dat", CMD_PGNSS_INJECT_UTC_GPS); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GPS_UTC success"); PgnssSendFileContent(handle, GPS_PATH "GPS_RTI.dat", CMD_PGNSS_INJECT_RTI_GPS); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GPS_RTI success"); PgnssSendFileContent(handle, GPS_PATH "GPS_ION.dat", CMD_PGNSS_INJECT_ION_GPS); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GPS_ION success"); PgnssSendFileContent(handle, GPS_PATH "GLO_RTI.dat", CMD_PGNSS_INJECT_RTI_GANSS); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GLO_RTI success"); PgnssSendFileContent(handle, GPS_PATH "GLO_AUX.dat", CMD_PGNSS_INJECT_AUX_GANSS); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GLO_AUX success"); PgnssSendFileContent(handle, GPS_PATH "BDS_RTI.dat", CMD_PGNSS_INJECT_RTI_GANSS); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send BDS_RTI success"); PgnssSendFileContent(handle, GPS_PATH "GAL_RTI.dat", CMD_PGNSS_INJECT_RTI_GANSS); osal_msleep(INJECT_SLEEP_MS_TIME); static_print_debug("send GAL_RTI success"); } void PgnssInject(tiot_handle handle, uint32_t ts) { // 注入Pgnss辅助数据 PgnssInjectAllEph(handle, ts); PgnssInjectOtherAssist(handle); } // 以下为使用PGNSS解析数据合并输出接口生成的星历文件进行注入的代码 static int32_t GetNonGloEphFileName(int32_t ts, char *fileName) { const char prefix[] = "NonGlo"; DIR *dir = opendir(GPS_PATH ""); if (dir == NULL) { static_print_error("Failed to open /user/tjd_gps directory."); return 0; } int32_t time, ret; int32_t minDist = OTHER_EPH_VALID_TIME; int32_t closestTime = 0; struct dirent *entry; for (entry = readdir(dir); entry != NULL; entry = readdir(dir)) { // 检查文件名 if (strstr(entry->d_name, ".eph") != NULL) { char *p = strrchr(entry->d_name, '_'); if (p != NULL) { char *q = strstr(entry->d_name, prefix); if (q != NULL) { time = atoi(p + 1); // 查找星历文件时间戳与当前时间相差2小时以内且距离最近的 if (abs(time - ts) < minDist) { minDist = abs(time - ts); closestTime = time; } } } } } if (closestTime != 0) { ret = snprintf_s(fileName, FILE_PATH_LEN, (FILE_PATH_LEN - 1), "%s%d%s", GPS_PATH "NonGlo_", closestTime, ".eph"); closedir(dir); static_print_debug("closest eph fileName:%s", fileName); return ((ret <= 0) ? 0 : 1); } closedir(dir); return 0; } static int32_t GetGloEphFileName(int32_t ts, char *fileName) { const char prefix[] = "GLO"; DIR *dir = opendir(GPS_PATH ""); if (dir == NULL) { static_print_debug("Failed to open /user/tjd_gps directory."); return 0; } int32_t ephTime, ret; struct dirent *entry; for (entry = readdir(dir); entry != NULL; entry = readdir(dir)) { // 检查文件名 if (strstr(entry->d_name, ".eph") != NULL) { char *p = strrchr(entry->d_name, '_'); if (p != NULL) { char *q = strstr(entry->d_name, prefix); if (q != NULL) { ephTime = atoi(p + 1); static_print_debug("ephTime:%ld, ts:%ld", ephTime, ts); // 当前时间大于GLO星历文件时间戳,则需要选取相差2小时以内的星历 // 当前时间小于GLO星历文件时间戳,则需要选取相差小于15分钟的星历 if (((ts - ephTime) >= 0 && abs(ephTime - ts) < OTHER_EPH_VALID_TIME) || ((ts - ephTime) <= 0 && abs(ephTime - ts) < GLO_EPH_VALID_TIME)) { ret = FormatFileName(fileName, entry->d_name); static_print_debug("glo eph fileName:%s", fileName); closedir(dir); return ret; } } } } } closedir(dir); return 0; } static void PgnssSendNonGloEph(tiot_handle handle, const char *fileName) { FILE *fp = fopen(fileName, "rb"); if (fp == NULL) { static_print_error("error: open file failed: %s\n", fileName); return; } fseek(fp, 0, SEEK_END); int32_t len = ftell(fp); fseek(fp, 0, SEEK_SET); if (len < 0 || len > PGNSS_FILE_MAX_SIZE) { fclose(fp); static_print_error("error: get file len failed: %s\n", fileName); return; } uint8_t *buff = (uint8_t *)malloc(len); if (buff == NULL) { fclose(fp); static_print_error("error: malloc file len failed: %s\n", fileName); return; } int32_t dataSize = fread(buff, len, 1, fp); if (dataSize < 0) { fclose(fp); static_print_error("error: read file failed: %s\n", fileName); free(buff); return; } fclose(fp); int32_t fileLen = 0; for (int32_t i = 0; i < NON_GLO_GNSS_TYPES && fileLen <= len; i++) { EphHeadInfo *ephHdr = (EphHeadInfo *)(buff + fileLen); EncodeAndInjectCmd(handle, ephHdr->cmd, (buff + fileLen + sizeof(EphHeadInfo)), ephHdr->len); fileLen += (ephHdr->len + sizeof(EphHeadInfo)); osal_msleep(INJECT_SLEEP_MS_TIME); } free(buff); } static void PgnssSendGloEph(tiot_handle handle, const char *fileName, int32_t ts) { FILE *fp = fopen(fileName, "rb"); if (fp == NULL) { static_print_error("error: open file failed: %s\n", fileName); return; } fseek(fp, 0, SEEK_END); int32_t len = ftell(fp); fseek(fp, 0, SEEK_SET); if (len < 0 || len > PGNSS_FILE_MAX_SIZE) { fclose(fp); static_print_error("error: get file len failed: %s\n", fileName); return; } uint8_t *buff = (uint8_t *)malloc(len); if (buff == NULL) { fclose(fp); static_print_error("error: malloc file len failed: %s\n", fileName); return; } int32_t dataSize = fread(buff, len, 1, fp); if (dataSize < 0) { fclose(fp); static_print_error("error: read file failed: %s\n", fileName); free(buff); return; } fclose(fp); int32_t fileLen = 0; for (int32_t i = 0; i < GLO_EPH_NUM_ONE_FILE && fileLen <= len; i++) { EphHeadInfo *ephHdr = (EphHeadInfo *)(buff + fileLen); static_print_debug("glo eph, time:%u, ts:%d", ephHdr->time, ts); if (abs((int32_t)ephHdr->time - ts) < GLO_EPH_VALID_TIME) { EncodeAndInjectCmd(handle, ephHdr->cmd, (buff + fileLen + sizeof(EphHeadInfo)), ephHdr->len); osal_msleep(INJECT_SLEEP_MS_TIME); break; } fileLen += (ephHdr->len + sizeof(EphHeadInfo)); } free(buff); } static void PgnssInjectAllEphLessFiles(tiot_handle handle, uint32_t ts) { char fileName[FILE_PATH_LEN] = {0}; if (GetNonGloEphFileName(ts, fileName) == 0) { static_print_error("GetNonGloEphFileName failed"); return; } PgnssSendNonGloEph(handle, fileName); if (GetGloEphFileName(ts, fileName) == 0) { static_print_error("GetGloEphFileName failed"); return; } PgnssSendGloEph(handle, fileName, ts); } static void PgnssSendOtherAssistInfo(tiot_handle handle, const char *fileName) { FILE *fp = fopen(fileName, "rb"); if (fp == NULL) { static_print_error("open file failed: %s", fileName); return; } fseek(fp, 0, SEEK_END); int32_t len = ftell(fp); fseek(fp, 0, SEEK_SET); if (len < 0 || len > PGNSS_FILE_MAX_SIZE) { fclose(fp); static_print_error("get file len failed: %s", fileName); return; } uint8_t *buff = (uint8_t *)malloc(len); if (buff == NULL) { fclose(fp); static_print_error("malloc file len failed: %s", fileName); return; } int32_t dataSize = fread(buff, len, 1, fp); if (dataSize < 0) { fclose(fp); static_print_error("read file failed: %s", fileName); free(buff); return; } fclose(fp); int32_t fileLen = 0; for (int32_t i = 0; i < OTHER_ASSIST_TYPES && fileLen <= len; i++) { OtherAssistHeadInfo *otherHdr = (OtherAssistHeadInfo *)(buff + fileLen); EncodeAndInjectCmd(handle, otherHdr->cmd, (buff + fileLen + sizeof(OtherAssistHeadInfo)), otherHdr->len); fileLen += (otherHdr->len + sizeof(OtherAssistHeadInfo)); osal_msleep(INJECT_SLEEP_MS_TIME); } free(buff); } static void PgnssInjectOtherAssistLessFiles(tiot_handle handle) { PgnssSendOtherAssistInfo(handle, GPS_PATH "AssistInfo.dat"); } // 注入PGNSS解析数据合并输出接口生成的星历文件 void PgnssInjectLessFiles(tiot_handle handle, uint32_t ts) { // 注入Pgnss辅助数据 PgnssInjectAllEphLessFiles(handle, ts); PgnssInjectOtherAssistLessFiles(handle); }