| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394 |
- /**
- * gps_app.c - RT-Thread + lwgps GPS application (No %f version)
- * Using UART6 to drive AIR530 GPS module
- * Focus on getting latitude, longitude, altitude and satellite count
- *
- * 特点:不依赖 %f 格式化,适用于未启用 _printf_float 的系统
- * 方法:使用整数放大法将浮点数转为字符串(如 31.234567)
- */
- #include <rtthread.h>
- #include <rtdevice.h>
- #include <stdlib.h>
- #include <string.h>
- #include <lwgps/lwgps.h>
- #include "sys_config.h"
- #include "gps.h"
- #define DBG_TAG "gps"
- #define DBG_LVL DBG_INFO
- #include <rtdbg.h>
- /* GPS 配置参数 */
- #define GPS_UART_NAME "uart6" // 使用的串口设备名
- #define GPS_UART_BAUDRATE 9600 // 波特率
- #define GPS_BUFFER_SIZE 256 // 每次读取缓冲区大小
- /* 全局变量 */
- static rt_device_t gps_uart = RT_NULL; // UART 设备句柄
- static lwgps_t gps_handler; // lwGPS 句柄
- static rt_uint8_t gps_buffer[GPS_BUFFER_SIZE]; // 接收缓冲区
- static rt_sem_t gps_sem = RT_NULL; // 数据接收信号量
- /* 地球常数(Krasovsky 1940 椭球) */
- const double pi = 3.14159265358979324;
- const double a = 6378245.0; // 长半轴
- const double ee = 0.00669342162296594323; // 第一偏心率平方
- static char wgs84_lat[16], wgs84_lon[16];
- static char gcj02_lat[16], gcj02_lon[16];
- static char altitude[16], sats_used[4], sats_view[4];
- nmea_msg gpsx;
- volatile char Tran_lng[12] = "0";
- volatile char Tran_lat[12] = "0";
- static void reflash_gps(void);
- static void gps_get_location(void);
- /**
- * 判断坐标是否在中国大陆范围外(GCJ-02 转换用)
- */
- static rt_bool_t outOfChina(double lat, double lon)
- {
- if (lon < 72.004 || lon > 137.8347)
- return RT_TRUE;
- if (lat < 0.8293 || lat > 55.8271)
- return RT_TRUE;
- return RT_FALSE;
- }
- /**
- * 纬度偏移变换
- */
- static double transformLat(double x, double y)
- {
- double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(abs(x));
- ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
- ret += (20.0 * sin(y * pi) + 40.0 * sin(y / 3.0 * pi)) * 2.0 / 3.0;
- ret += (160.0 * sin(y / 12.0 * pi) + 320 * sin(y * pi / 30.0)) * 2.0 / 3.0;
- return ret;
- }
- /**
- * 经度偏移变换
- */
- static double transformLon(double x, double y)
- {
- double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(abs(x));
- ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
- ret += (20.0 * sin(x * pi) + 40.0 * sin(x / 3.0 * pi)) * 2.0 / 3.0;
- ret += (150.0 * sin(x / 12.0 * pi) + 300.0 * sin(x / 30.0 * pi)) * 2.0 / 3.0;
- return ret;
- }
- /**
- * WGS84 坐标转 GCJ-02(火星坐标系)
- */
- static void wgs84_to_gcj02(double wg_lat, double wg_lon, double *gcj_lat, double *gcj_lon)
- {
- if (outOfChina(wg_lat, wg_lon))
- {
- *gcj_lat = wg_lat;
- *gcj_lon = wg_lon;
- return;
- }
- double dlat = transformLat(wg_lon - 105.0, wg_lat - 35.0);
- double dlon = transformLon(wg_lon - 105.0, wg_lat - 35.0);
- double radlat = wg_lat / 180.0 * pi;
- double magic = sin(radlat);
- magic = 1 - ee * magic * magic;
- double sqrtmagic = sqrt(magic);
- dlat = (dlat * 180.0) / ((a * (1 - ee)) / (magic * sqrtmagic) * pi);
- dlon = (dlon * 180.0) / (a / sqrtmagic * cos(radlat) * pi);
- *gcj_lat = wg_lat + dlat;
- *gcj_lon = wg_lon + dlon;
- }
- /**
- * UART 接收回调函数:有数据到达时释放信号量
- */
- static rt_err_t gps_uart_rx_ind(rt_device_t dev, rt_size_t size)
- {
- if (size > 0)
- {
- rt_sem_release(gps_sem);
- }
- return RT_EOK;
- }
- /**
- * 发送 GPS 模块配置命令(切换模式、频率等)
- */
- static void gps_send_config(void)
- {
- const char *config_cmds[] = {
- "$PGKC115,0,0,1,0*2B\r\n", // GPS+BeiDou 模式
- "$PGKC242,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*37\r\n", // 输出语句配置
- "$PMTK220,1000*1F\r\n", // 1Hz 定位频率
- RT_NULL};
- LOG_I("Sending GPS configuration commands...");
- for (int i = 0; config_cmds[i] != RT_NULL; i++)
- {
- rt_device_write(gps_uart, 0, config_cmds[i], rt_strlen(config_cmds[i]));
- rt_thread_mdelay(200);
- }
- // 发送热启动命令
- const char *hot_start_cmd = "$PMTK101*32\r\n";
- rt_device_write(gps_uart, 0, hot_start_cmd, rt_strlen(hot_start_cmd));
- LOG_I("Hot start triggered.");
- }
- /**
- * 将浮点数格式化为字符串,使用整数放大法(不依赖 %f)
- * 示例:value=31.234567, decimals=6 → "31.234567"
- */
- static void double_to_str(char *str, double value, int decimals)
- {
- // 处理负数
- if (value < 0)
- {
- *str++ = '-';
- value = -value;
- }
- // 分离整数和小数部分
- long long_part = (long)value;
- double frac_part = value - (double)long_part;
- long frac_power = 1;
- for (int i = 0; i < decimals; i++)
- frac_power *= 10;
- // 四舍五入小数部分
- long frac_int = (long)(frac_part * frac_power + 0.5);
- // 拼接字符串
- if (decimals > 0)
- {
- rt_snprintf(str, 16, "%ld.%0*ld", long_part, decimals, frac_int);
- }
- else
- {
- rt_snprintf(str, 16, "%ld", long_part);
- }
- }
- /**
- * GPS 数据处理线程
- */
- static void gps_thread_entry(void *parameter)
- {
- rt_size_t recv_len;
- double gcj_lat, gcj_lon;
- static uint32_t timeout_count = 0;
- lwgps_init(&gps_handler);
- LOG_I("GPS thread running...");
- static char once = 1;
- while (1)
- {
- if (rt_sem_take(gps_sem, rt_tick_from_millisecond(2000)) == RT_EOK)
- {
- recv_len = rt_device_read(gps_uart, 0, gps_buffer, GPS_BUFFER_SIZE);
- if (recv_len > 0)
- {
- timeout_count = 0;
- lwgps_process(&gps_handler, (const char *)gps_buffer, recv_len);
- if (gps_handler.is_valid)
- {
- // 坐标转换
- wgs84_to_gcj02(gps_handler.latitude, gps_handler.longitude, &gcj_lat, &gcj_lon);
- // 格式化输出字符串(放大法)
- char lat_str[16], lon_str[16], alt_str[16];
- double_to_str(lat_str, gcj_lat, 6);
- double_to_str(lon_str, gcj_lon, 6);
- double_to_str(alt_str, gps_handler.altitude, 1);
- reflash_gps();
- if (once && gps_handler.sats_in_use > 5)
- {
- once = 0;
- gps_get_location();
- LOG_I("FIXED: Lat=%s, Lon=%s, Alt=%sm, Sats=%d\n\n",
- lat_str, lon_str, alt_str, gps_handler.sats_in_use);
- }
- }
- }
- }
- else
- {
- timeout_count++;
- if (timeout_count > 30)
- {
- LOG_E("GPS data timeout! Re-sending config...");
- gps_send_config();
- timeout_count = 0;
- gpsx.gpssta = 0;
- }
- }
- }
- }
- /**
- * GPS 应用初始化函数(自动执行)
- */
- static int gps_app_init(void)
- {
- rt_err_t result = RT_EOK;
- struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;
- LOG_I("Initializing GPS application...");
- // 查找 UART 设备
- gps_uart = rt_device_find(GPS_UART_NAME);
- if (gps_uart == RT_NULL)
- {
- LOG_E("UART device '%s' not found!", GPS_UART_NAME);
- return -RT_ERROR;
- }
- // 配置串口参数
- config.baud_rate = GPS_UART_BAUDRATE;
- config.data_bits = DATA_BITS_8;
- config.stop_bits = STOP_BITS_1;
- config.parity = PARITY_NONE;
- config.bufsz = 1024;
- if (rt_device_control(gps_uart, RT_DEVICE_CTRL_CONFIG, &config) != RT_EOK)
- {
- LOG_E("Failed to configure UART!");
- return -RT_ERROR;
- }
- // 创建信号量
- gps_sem = rt_sem_create("gps_sem", 0, RT_IPC_FLAG_FIFO);
- if (gps_sem == RT_NULL)
- {
- LOG_E("Failed to create semaphore!");
- return -RT_ERROR;
- }
- // 打开设备并设置中断接收回调
- if (rt_device_open(gps_uart, RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX) != RT_EOK)
- {
- LOG_E("Failed to open UART device!");
- rt_sem_delete(gps_sem);
- return -RT_ERROR;
- }
- rt_device_set_rx_indicate(gps_uart, gps_uart_rx_ind);
- // 发送初始配置
- gps_send_config();
- // 创建 GPS 处理线程
- rt_thread_t thread = rt_thread_create("gps",
- gps_thread_entry,
- RT_NULL,
- 2048,
- 15,
- 10);
- if (thread != RT_NULL)
- {
- rt_thread_startup(thread);
- LOG_I("GPS application started successfully.");
- }
- else
- {
- LOG_E("Failed to create GPS thread!");
- rt_device_close(gps_uart);
- rt_sem_delete(gps_sem);
- result = -RT_ERROR;
- }
- return result;
- }
- /**
- * 手动查询当前定位信息(MSH 命令)
- */
- static void reflash_gps(void)
- {
- if (gps_handler.is_valid)
- {
- double gcj_lat, gcj_lon;
- wgs84_to_gcj02(gps_handler.latitude, gps_handler.longitude, &gcj_lat, &gcj_lon);
- double_to_str(wgs84_lat, gps_handler.latitude, 6);
- double_to_str(wgs84_lon, gps_handler.longitude, 6);
- double_to_str(gcj02_lat, gcj_lat, 6);
- double_to_str(gcj02_lon, gcj_lon, 6);
- double_to_str(altitude, gps_handler.altitude, 1);
- rt_snprintf(sats_used, sizeof(sats_used), "%d", gps_handler.sats_in_use);
- rt_snprintf(sats_view, sizeof(sats_view), "%d", gps_handler.sats_in_view);
- if(gps_handler.sats_in_use > 5)
- {
- gpsx.posslnum = gps_handler.sats_in_use;
- gpsx.gpssta = 1;
- sprintf(Tran_lng , "%s" , wgs84_lon);
- sprintf(Tran_lat , "%s" , wgs84_lat);
- gpsx.altitude = (int)(gps_handler.altitude * 10);
- sprintf(sys_info.lng , "%s" , wgs84_lon);
- sprintf(sys_info.lat , "%s" , wgs84_lat);
- sprintf(sys_info.altitude , "%s" , altitude);
- }
- }
- }
- static void gps_get_location(void)
- {
- if (gps_handler.is_valid)
- {
- reflash_gps();
- rt_kprintf("\n\n === GPS Location ===\n");
- rt_kprintf(" WGS84:\n");
- rt_kprintf(" Lat: %s\n", wgs84_lat);
- rt_kprintf(" Lon: %s\n", wgs84_lon);
- rt_kprintf(" GCJ02:\n");
- rt_kprintf(" Lat: %s\n", gcj02_lat);
- rt_kprintf(" Lon: %s\n", gcj02_lon);
- rt_kprintf(" Altitude: %s m\n", altitude);
- rt_kprintf(" Satellites: %s used / %s viewed\n", sats_used, sats_view);
- rt_kprintf(" Status: Valid Fix\n");
- }
- else
- {
- char sats_view[4];
- rt_snprintf(sats_view, sizeof(sats_view), "%d", gps_handler.sats_in_view);
- rt_kprintf("No valid GPS fix.\n");
- rt_kprintf("Satellites in view: %s\n", sats_view);
- }
- }
- /**
- * 重新发送配置命令(MSH 命令)
- */
- static void gps_reconfig(int argc, char **argv)
- {
- rt_kprintf("Reconfiguring GPS module...\n");
- gps_send_config();
- rt_kprintf("Configuration sent.\n");
- }
- /* 导出 MSH 命令 */
- MSH_CMD_EXPORT(gps_get_location, Get current GPS location);
- MSH_CMD_EXPORT(gps_reconfig, Reconfigure GPS module);
- /* 自动初始化 */
- INIT_APP_EXPORT(gps_app_init);
|