537 lines
19 KiB
C
537 lines
19 KiB
C
/*==========================================================================================================
|
||
Copyright (c) 2021, Quectel Wireless Solutions Co., Ltd. All rights reserved.
|
||
Quectel Wireless Solutions Proprietary and Confidential.
|
||
===========================================================================================================*/
|
||
/*New version does not need NTP sync*/
|
||
#include <stdio.h>
|
||
#include <string.h>
|
||
#include <stdlib.h>
|
||
#include "ql_application.h"
|
||
#include "ql_log.h"
|
||
#include "ql_gnss.h"
|
||
#include "ql_rtos.h"
|
||
#include "ql_ntp.h"
|
||
#include "ql_data_call.h"
|
||
#include "sockets.h"
|
||
#include "netdb.h"
|
||
#include "app_inc.h"
|
||
|
||
// 新增宏定义（原代码中未定义）
|
||
#define GNSS_FAST_MODE_HOLD_TIME (5 * 60 * 1000) // FAST模式持续5分钟后切SLOW
|
||
|
||
// Global variables
|
||
gnss_mode_t g_gnss_mode = GNSS_MODE_SLOW; // Default SLOW mode (power save)
|
||
gps_location_info_t g_gps_location = {0}; // Global GPS position structure
|
||
|
||
// ====== SLOW MODE 卫星搜索控制（新逻辑） ======
|
||
static volatile int g_gnss_sat_count = 0; // 当前可见卫星数量（从GSV解析）
|
||
|
||
static u8 gnss_is_open = 0;
|
||
|
||
// 新逻辑使用的常量
|
||
#define GNSS_SEARCH_INTERVAL 900 // 固定搜索间隔：15分钟 = 900秒
|
||
#define GNSS_MAX_SEARCH_TIME 180 // 最大搜索时间：3分钟 = 180秒
|
||
#define GNSS_SAT_CHECK_TIMEOUT_60 30 // 60秒卫星检测超时
|
||
#define GNSS_SAT_CHECK_TIMEOUT_90 60 // 60秒卫星检测超时
|
||
|
||
#define GNSS_SAT_MIN_COUNT 4 // 1分钟时要求的最小卫星数
|
||
|
||
u8 get_gnss_open_status(void)
|
||
{
|
||
return gnss_is_open;
|
||
}
|
||
|
||
/*============================================================================
|
||
* NMEA parsing function：From $--GGA sentence extract lat/lon/FS
|
||
* Input: nmea_data - NMEA raw data
|
||
* Output: Return 1 on success, Return 0 on failure
|
||
*
|
||
* NMEA format: $--GGA,<time>,<Lat>,<N/S>,<Lon>,<E/W>,<FS>,<NoSV>,<HDOP>,<msl>,<M>,<Altref>,<M>,<DiffAge>,<DiffStation>*cs
|
||
* FS (Fix Status): 0=无效, 1=GPS有效, 2=DGPS, 4=RTK固定, 5=RTK浮动, 6=估算
|
||
* Latitude format: DDMM.MMMMM (degree-minute format) -> Convert to degrees
|
||
* Longitude format: DDDMM.MMMMM (degree-minute format) -> Convert to degrees
|
||
*===========================================================================*/
|
||
static int parse_gnmea_coordinates(const char *nmea_data, double *latitude, double *longitude, int *fix_status)
|
||
{
|
||
char buffer[512];
|
||
char *token;
|
||
char *saveptr;
|
||
int field_index = 0;
|
||
char lat_str[32] = {0};
|
||
char lat_dir[4] = {0};
|
||
char lon_str[32] = {0};
|
||
char lon_dir[4] = {0};
|
||
char fs_str[8] = {0};
|
||
|
||
if (nmea_data == NULL || latitude == NULL || longitude == NULL || fix_status == NULL) {
|
||
return 0;
|
||
}
|
||
|
||
// Copy to modifiable buffer
|
||
strncpy(buffer, nmea_data, sizeof(buffer) - 1);
|
||
buffer[sizeof(buffer) - 1] = '\0';
|
||
|
||
// Check if GGA sentence
|
||
if (strstr(buffer, "GGA") == NULL) {
|
||
return 0;
|
||
}
|
||
|
||
// Use strtok_r to split fields
|
||
token = strtok_r(buffer, ",", &saveptr);
|
||
|
||
while (token != NULL) {
|
||
switch (field_index) {
|
||
case 2: // Latitude field (DDMM.MMMMM format)
|
||
strncpy(lat_str, token, sizeof(lat_str) - 1);
|
||
break;
|
||
case 3: // Latitude direction (N/S)
|
||
strncpy(lat_dir, token, sizeof(lat_dir) - 1);
|
||
break;
|
||
case 4: // Longitude field (DDDMM.MMMMM format)
|
||
strncpy(lon_str, token, sizeof(lon_str) - 1);
|
||
break;
|
||
case 5: // Longitude direction (E/W)
|
||
strncpy(lon_dir, token, sizeof(lon_dir) - 1);
|
||
break;
|
||
case 6: // Fix Status (FS): 0=无效, 1=GPS, 2=DGPS, 4=RTK固定, 5=RTK浮动, 6=估算
|
||
strncpy(fs_str, token, sizeof(fs_str) - 1);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
field_index++;
|
||
token = strtok_r(NULL, ",", &saveptr);
|
||
}
|
||
|
||
// Parse Fix Status: 0=invalid, others=valid
|
||
if (strlen(fs_str) == 0) {
|
||
*fix_status = 0; // No FS field, treat as invalid
|
||
} else {
|
||
*fix_status = atoi(fs_str);
|
||
}
|
||
|
||
// Check if valid data obtained (FS must be non-zero for valid position)
|
||
if (strlen(lat_str) == 0 || strlen(lon_str) == 0 || *fix_status == 0) {
|
||
return 0;
|
||
}
|
||
|
||
// Parse Latitude: DDMM.MMMMM -> degree
|
||
// Example: 2311.71298 -> 23 + 11.71298/60 = 23.1952163
|
||
{
|
||
double lat_deg = 0, lat_min = 0;
|
||
int lat_deg_int = 0;
|
||
|
||
// Extract degree part (first 2 digits for latitude)
|
||
lat_deg_int = (int)(atof(lat_str) / 100);
|
||
lat_min = atof(lat_str) - (lat_deg_int * 100);
|
||
*latitude = lat_deg_int + (lat_min / 60.0);
|
||
|
||
// South latitude is negative
|
||
if (lat_dir[0] == 'S' || lat_dir[0] == 's') {
|
||
*latitude = -(*latitude);
|
||
}
|
||
}
|
||
|
||
// Parse Longitude: DDDMM.MMMMM -> degree
|
||
// Example: 11328.63118 -> 113 + 28.63118/60 = 113.4771863
|
||
{
|
||
double lon_deg = 0, lon_min = 0;
|
||
int lon_deg_int = 0;
|
||
|
||
// Extract degree part (first 3 digits for longitude)
|
||
lon_deg_int = (int)(atof(lon_str) / 100);
|
||
lon_min = atof(lon_str) - (lon_deg_int * 100);
|
||
*longitude = lon_deg_int + (lon_min / 60.0);
|
||
|
||
// West longitude is negative
|
||
if (lon_dir[0] == 'W' || lon_dir[0] == 'w') {
|
||
*longitude = -(*longitude);
|
||
}
|
||
}
|
||
|
||
return 1; // parse success
|
||
}
|
||
|
||
|
||
/*============================================================================
|
||
* Parse GSV sentence to get total satellites in view
|
||
* GSV format: $xxGSV,<total_msgs>,<msg_num>,<total_sats>,<sat1_prn>,<elev>,<azim>,<cn0>,...*cs
|
||
* Field 3 (index 3): Total number of satellites in view
|
||
* Return: satellite count on success, -1 on failure
|
||
*===========================================================================*/
|
||
static int parse_gsv_sat_count(const char *nmea_data)
|
||
{
|
||
char buffer[512];
|
||
char *token;
|
||
char *saveptr;
|
||
int field_index = 0;
|
||
int sat_count = -1;
|
||
|
||
if (nmea_data == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
strncpy(buffer, nmea_data, sizeof(buffer) - 1);
|
||
buffer[sizeof(buffer) - 1] = '\0';
|
||
|
||
// Check if GSV sentence
|
||
if (strstr(buffer, "GSV") == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
token = strtok_r(buffer, ",", &saveptr);
|
||
while (token != NULL) {
|
||
if (field_index == 3) { // Total satellites in view
|
||
sat_count = atoi(token);
|
||
break;
|
||
}
|
||
field_index++;
|
||
token = strtok_r(NULL, ",", &saveptr);
|
||
}
|
||
|
||
return sat_count;
|
||
}
|
||
|
||
|
||
/*============================================================================
|
||
* GNSS callback function：Receive NMEA data, parse and save to global structure
|
||
*
|
||
* Fix Status (FS)判断逻辑：
|
||
* FS = 0 : 无效定位（No Fix）
|
||
* FS = 1 : GPS定位有效（SPS）
|
||
* FS = 2 : DGPS定位有效
|
||
* FS = 4 : RTK固定解
|
||
* FS = 5 : RTK浮点解
|
||
* FS = 6 : 估算（Dead Reckoning）
|
||
* FS >= 1 均为有效定位
|
||
*===========================================================================*/
|
||
void quec_gnss_callback(char *data, int len)
|
||
{
|
||
int i;
|
||
char nmea_buffer[512];
|
||
int fix_status = 0;
|
||
|
||
// 打印原始 NMEA 数据
|
||
for (i = 0; i < len; i++) {
|
||
printf("%c", data[i]);
|
||
}
|
||
|
||
// 在 data 中定位 GGA 句子的起止位置
|
||
// GGA 格式: $xxGGA,...,*CS\r\n
|
||
char *gga_start = NULL;
|
||
char *gga_end = NULL;
|
||
|
||
for (i = 0; i < len - 5; i++) {
|
||
// 找 "$xxGGA" (兼容 GPGGA, GBGGA, GNGGA 等)
|
||
if (data[i] == '$' &&
|
||
data[i + 3] == 'G' &&
|
||
data[i + 4] == 'G' &&
|
||
data[i + 5] == 'A') {
|
||
|
||
gga_start = &data[i];
|
||
|
||
// 从 GGA 起始位置往后找行尾 (\r\n 或 \n)
|
||
int j;
|
||
for (j = i; j < len; j++) {
|
||
if (data[j] == '\r' || data[j] == '\n') {
|
||
break;
|
||
}
|
||
}
|
||
gga_end = &data[j]; // 指向 \r 或 \n 的位置
|
||
|
||
break; // 只取第一条 GGA
|
||
}
|
||
}
|
||
|
||
// 找到 GGA 句子且长度合法
|
||
if (gga_start != NULL && gga_end != NULL && gga_end > gga_start) {
|
||
int gga_len = gga_end - gga_start;
|
||
|
||
if (gga_len > 0 && gga_len < sizeof(nmea_buffer)) {
|
||
// 拷贝 GGA 句子到缓冲区
|
||
memcpy(nmea_buffer, gga_start, gga_len);
|
||
nmea_buffer[gga_len] = '\0';
|
||
|
||
// 保存原始 NMEA 到全局结构
|
||
int raw_len = (gga_len < sizeof(g_gps_location.raw_nmea) - 1) ?
|
||
gga_len : sizeof(g_gps_location.raw_nmea) - 1;
|
||
memcpy(g_gps_location.raw_nmea, nmea_buffer, raw_len);
|
||
g_gps_location.raw_nmea[raw_len] = '\0';
|
||
|
||
// 解析坐标和定位状态
|
||
double lat = 0, lon = 0;
|
||
if (parse_gnmea_coordinates(nmea_buffer, &lat, &lon, &fix_status)) {
|
||
g_gps_location.latitude = lat;
|
||
g_gps_location.longitude = lon;
|
||
// FS = 0 无效, FS >= 1 有效
|
||
g_gps_location.is_valid = (fix_status >= 1) ? 1 : 0;
|
||
} else {
|
||
g_gps_location.is_valid = 0;
|
||
printf("\n[GNSS Parse failed] FS=%d, %s\n",
|
||
fix_status,
|
||
(fix_status == 0) ? "No Fix (Invalid)" : "Format error");
|
||
}
|
||
}
|
||
}
|
||
|
||
// ====== 解析 GSV 获取卫星数量 ======
|
||
// GSV 格式: $xxGSV,<total_msgs>,<msg_num>,<total_sats>,...*cs
|
||
char *gsv_start = NULL;
|
||
char *gsv_end = NULL;
|
||
|
||
for (i = 0; i < len - 5; i++) {
|
||
if (data[i] == '$' &&
|
||
data[i + 3] == 'G' &&
|
||
data[i + 4] == 'S' &&
|
||
data[i + 5] == 'V') {
|
||
|
||
gsv_start = &data[i];
|
||
|
||
int j;
|
||
for (j = i; j < len; j++) {
|
||
if (data[j] == '\r' || data[j] == '\n') {
|
||
break;
|
||
}
|
||
}
|
||
gsv_end = &data[j];
|
||
|
||
break; // 只取第一条 GSV
|
||
}
|
||
}
|
||
|
||
if (gsv_start != NULL && gsv_end != NULL && gsv_end > gsv_start) {
|
||
int gsv_len = gsv_end - gsv_start;
|
||
if (gsv_len > 0 && gsv_len < sizeof(nmea_buffer)) {
|
||
memcpy(nmea_buffer, gsv_start, gsv_len);
|
||
nmea_buffer[gsv_len] = '\0';
|
||
|
||
int sat_count = parse_gsv_sat_count(nmea_buffer);
|
||
if (sat_count >= 0) {
|
||
g_gnss_sat_count = sat_count;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/*============================================================================
|
||
* Set GNSS positioning mode
|
||
* Parameter: mode - GNSS_MODE_FAST(Fast30s) or GNSS_MODE_SLOW(Slow5min)
|
||
*===========================================================================*/
|
||
void set_gnss_mode(gnss_mode_t mode)
|
||
{
|
||
g_gnss_mode = mode;
|
||
printf("[GNSSMode] Set to %s Mode\n",
|
||
(mode == GNSS_MODE_FAST) ? "Fast (30s)" : "Slow (5min)");
|
||
}
|
||
|
||
gnss_mode_t get_gnss_mode(void)
|
||
{
|
||
return g_gnss_mode;
|
||
}
|
||
|
||
/*============================================================================
|
||
* Get current positioning interval (seconds) - 保留但不再使用
|
||
*===========================================================================*/
|
||
static int get_gnss_interval(void)
|
||
{
|
||
return 180; // 仅保留，新逻辑已固定15分钟
|
||
}
|
||
|
||
/*============================================================================
|
||
* Get global GPS position info
|
||
* Return: Pointer to global structure
|
||
*===========================================================================*/
|
||
gps_location_info_t* get_gps_location_info(void)
|
||
{
|
||
return &g_gps_location;
|
||
}
|
||
|
||
/*============================================================================
|
||
* Print current GPS position info (For debug)
|
||
*===========================================================================*/
|
||
void print_gps_location_info(void)
|
||
{
|
||
if (g_gps_location.is_valid) {
|
||
printf("[GPSinfo] Latitude: %.8f, Longitude: %.8f\n",
|
||
g_gps_location.latitude, g_gps_location.longitude);
|
||
printf("[GPSinfo] rawNMEA: %s\n", g_gps_location.raw_nmea);
|
||
} else {
|
||
printf("[GPSinfo] No valid position data\n");
|
||
}
|
||
}
|
||
|
||
|
||
/*============================================================================
|
||
* GNSS test task main function
|
||
*
|
||
* FAST MODE: GPS持续开启，每45秒上报一次定位，持续5分钟后自动切到SLOW
|
||
* SLOW MODE: 固定每15分钟搜索一次，搜索逻辑：
|
||
* - 开启GPS后，30秒内检测是否搜到卫星（通过GSV解析sat_count）
|
||
* - 若30秒内卫星数为0，立即关闭GPS，等待15分钟
|
||
* - 若30秒内搜到卫星，继续等待至1分钟，检查卫星数量是否 >= 3
|
||
* - 若1分钟时卫星数 < 3，立即关闭GPS，等待15分钟
|
||
* - 若卫星数 >= 3，继续定位，但总搜索时间最长3分钟
|
||
* - 3分钟后关闭GPS，无论是否获得有效定位，等待15分钟
|
||
*===========================================================================*/
|
||
static void quec_gnss_test(void * argv)
|
||
{
|
||
|
||
u32 fast_mode_start_time = 0;
|
||
u32 slow_mode_start_time = 0;
|
||
u32 slow_next_wake_time = 0; // SLOW模式下下次唤醒时间
|
||
|
||
// 初始化 GNSS 配置
|
||
ql_gnss_cfgsys_set(1); // BDS
|
||
ql_gnss_cfgmsg_set(31); // GGA output
|
||
ql_gnss_agps_cfg(AGPS_GNSS_ENABLE, 0);
|
||
ql_gnss_apflash_cfg(APFLASH_GNSS_ENABLE);
|
||
ql_set_gnss_nmea_mode(PACKET_MODE);
|
||
|
||
while (1)
|
||
{
|
||
if (g_gnss_mode == GNSS_MODE_FAST)
|
||
{
|
||
/* ========== FAST MODE ========== */
|
||
if (fast_mode_start_time == 0) {
|
||
fast_mode_start_time = hal_os_up_time_ms();
|
||
printf("[GNSS] Enter FAST mode\n");
|
||
}
|
||
|
||
if (!gnss_is_open) {
|
||
gnss_is_open = 1;
|
||
app_set_app_operation(1);
|
||
ql_gnss_open(quec_gnss_callback);
|
||
printf("[GNSS] FAST: GPS OPEN\n");
|
||
}
|
||
|
||
// 5分钟超时切SLOW
|
||
if (hal_os_up_time_ms() - fast_mode_start_time >= GNSS_FAST_MODE_HOLD_TIME) {
|
||
printf("[GNSS] FAST 5min timeout -> SLOW\n");
|
||
g_gnss_mode = GNSS_MODE_SLOW;
|
||
fast_mode_start_time = 0;
|
||
slow_mode_start_time = 0;
|
||
slow_next_wake_time = 0;
|
||
|
||
gnss_is_open = 0;
|
||
printf("[GNSS] SLOW: GPS CLOSED ret:%d\n", ql_gnss_close());
|
||
}
|
||
}
|
||
else
|
||
{
|
||
/* ========== SLOW MODE ========== */
|
||
u32 now = hal_os_up_time_ms();
|
||
|
||
// 首次进入或重置后初始化
|
||
if (slow_mode_start_time == 0) {
|
||
slow_mode_start_time = now;
|
||
slow_next_wake_time = now; // 立即尝试第一次定位
|
||
printf("[GNSS] Enter SLOW mode (fixed 15min interval)\n");
|
||
}
|
||
|
||
// 检查是否到达唤醒时间
|
||
if (now < slow_next_wake_time) {
|
||
printf("[GNSS] SLOW: next wake in %d s\n",
|
||
(slow_next_wake_time - now) / 1000);
|
||
}
|
||
else if (!gnss_is_open) {
|
||
/* ===== 开始新一轮搜索 ===== */
|
||
u32 search_start_time = now;
|
||
g_gnss_sat_count = 0;
|
||
g_gps_location.is_valid = 0;
|
||
|
||
gnss_is_open = 1;
|
||
app_set_app_operation(1);
|
||
ql_gnss_open(quec_gnss_callback);
|
||
|
||
printf("[GNSS] SLOW: GPS OPEN, search start\n");
|
||
|
||
/* ---- 第一阶段：60秒卫星检测 ---- */
|
||
int sat_found = 0;
|
||
int elapsed = 0;
|
||
while (elapsed < GNSS_SAT_CHECK_TIMEOUT_60) {
|
||
ql_rtos_task_sleep_s(1);
|
||
elapsed++;
|
||
if (g_gnss_sat_count > 1) {
|
||
sat_found = 1;
|
||
printf("[GNSS] SLOW: Sat found! count=%d, after %ds\n",
|
||
g_gnss_sat_count, elapsed);
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!sat_found) {
|
||
// 60秒无卫星 → 立即关闭，等待15分钟
|
||
printf("[GNSS] SLOW: No satellite in 30s, stop ret:%d.\n", ql_gnss_close());
|
||
gnss_is_open = 0;
|
||
slow_next_wake_time = search_start_time + (GNSS_SEARCH_INTERVAL * 1000);
|
||
printf("[GNSS] SLOW: GPS CLOSED, next search at +%ds\n", GNSS_SEARCH_INTERVAL);
|
||
continue;
|
||
}
|
||
|
||
/* ---- 第二阶段：等待至90分钟，检查卫星数量是否 >= 3 ---- */
|
||
// 此时已过 elapsed 秒 (<=30)，再等待至总时间达到60秒
|
||
while (elapsed < GNSS_SAT_CHECK_TIMEOUT_90) {
|
||
ql_rtos_task_sleep_s(1);
|
||
elapsed++;
|
||
}
|
||
|
||
// 1.5分钟时刻检查卫星数
|
||
int sat_count_now = g_gnss_sat_count;
|
||
printf("[GNSS] SLOW: 1min check, satellite count = %d\n", sat_count_now);
|
||
if (sat_count_now < GNSS_SAT_MIN_COUNT) {
|
||
printf("[GNSS] SLOW: Sat count < %d, stop search ret:%d.\n", GNSS_SAT_MIN_COUNT,ql_gnss_close());
|
||
gnss_is_open = 0;
|
||
slow_next_wake_time = search_start_time + (GNSS_SEARCH_INTERVAL * 1000);
|
||
printf("[GNSS] SLOW: GPS CLOSED, next search at +%ds\n", GNSS_SEARCH_INTERVAL);
|
||
continue;
|
||
}
|
||
|
||
/* ---- 第三阶段：卫星足够，继续定位，但总时间不超过3分钟 ---- */
|
||
printf("[GNSS] SLOW: Sat count OK, continue for fix (max %ds total)\n",
|
||
GNSS_MAX_SEARCH_TIME);
|
||
|
||
// 继续等待直到总时间达到 GNSS_MAX_SEARCH_TIME (180秒)
|
||
while (elapsed < GNSS_MAX_SEARCH_TIME) {
|
||
ql_rtos_task_sleep_s(1);
|
||
elapsed++;
|
||
|
||
// 如果获得有效定位，打印提示（但继续等待至3分钟）
|
||
if (g_gps_location.is_valid) {
|
||
printf("[GNSS] SLOW: Fix obtained at %ds!\n", elapsed);
|
||
print_gps_location_info();
|
||
// 若希望在定位成功后立即关闭，可在此 break，但此处按“最长3分钟”处理
|
||
}
|
||
}
|
||
|
||
// 3分钟时间到，关闭GPS
|
||
printf("[GNSS] SLOW: Max search time reached, stop.\n");
|
||
if (g_gps_location.is_valid) {
|
||
printf("[GNSS] SLOW: Final valid position:\n");
|
||
print_gps_location_info();
|
||
} else {
|
||
printf("[GNSS] SLOW: No valid fix within 3min.\n");
|
||
}
|
||
|
||
printf("[GNSS] SLOW: GPS CLOSED ret:%d",ql_gnss_close());
|
||
gnss_is_open = 0;
|
||
g_gps_location.is_valid = 0; // 重置状态
|
||
|
||
// 设置下次唤醒时间为 15分钟 后
|
||
slow_next_wake_time = search_start_time + (GNSS_SEARCH_INTERVAL * 1000);
|
||
printf("[GNSS] SLOW: GPS CLOSED, next search at +%ds\n", GNSS_SEARCH_INTERVAL);
|
||
}
|
||
// else: GPS正在运行中（理论上不会到这里）
|
||
}
|
||
|
||
// 统一10秒sleep，避免任务空转
|
||
ql_rtos_task_sleep_s(5);
|
||
}
|
||
}
|
||
|
||
void app_gnss_init(void)
|
||
{
|
||
ql_task_t gnssTaskRef = NULL;
|
||
ql_rtos_task_create(&gnssTaskRef, 1024*4, 100, "gnss_Task_Ref", quec_gnss_test, NULL);
|
||
}
|