gps_app.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394
  1. /**
  2. * gps_app.c - RT-Thread + lwgps GPS application (No %f version)
  3. * Using UART6 to drive AIR530 GPS module
  4. * Focus on getting latitude, longitude, altitude and satellite count
  5. *
  6. * 特点:不依赖 %f 格式化,适用于未启用 _printf_float 的系统
  7. * 方法:使用整数放大法将浮点数转为字符串(如 31.234567)
  8. */
  9. #include <rtthread.h>
  10. #include <rtdevice.h>
  11. #include <stdlib.h>
  12. #include <string.h>
  13. #include <lwgps/lwgps.h>
  14. #include "sys_config.h"
  15. #include "gps.h"
  16. #define DBG_TAG "gps"
  17. #define DBG_LVL DBG_INFO
  18. #include <rtdbg.h>
  19. /* GPS 配置参数 */
  20. #define GPS_UART_NAME "uart6" // 使用的串口设备名
  21. #define GPS_UART_BAUDRATE 9600 // 波特率
  22. #define GPS_BUFFER_SIZE 256 // 每次读取缓冲区大小
  23. /* 全局变量 */
  24. static rt_device_t gps_uart = RT_NULL; // UART 设备句柄
  25. static lwgps_t gps_handler; // lwGPS 句柄
  26. static rt_uint8_t gps_buffer[GPS_BUFFER_SIZE]; // 接收缓冲区
  27. static rt_sem_t gps_sem = RT_NULL; // 数据接收信号量
  28. /* 地球常数(Krasovsky 1940 椭球) */
  29. const double pi = 3.14159265358979324;
  30. const double a = 6378245.0; // 长半轴
  31. const double ee = 0.00669342162296594323; // 第一偏心率平方
  32. static char wgs84_lat[16], wgs84_lon[16];
  33. static char gcj02_lat[16], gcj02_lon[16];
  34. static char altitude[16], sats_used[4], sats_view[4];
  35. nmea_msg gpsx;
  36. volatile char Tran_lng[12] = "0";
  37. volatile char Tran_lat[12] = "0";
  38. static void reflash_gps(void);
  39. static void gps_get_location(void);
  40. /**
  41. * 判断坐标是否在中国大陆范围外(GCJ-02 转换用)
  42. */
  43. static rt_bool_t outOfChina(double lat, double lon)
  44. {
  45. if (lon < 72.004 || lon > 137.8347)
  46. return RT_TRUE;
  47. if (lat < 0.8293 || lat > 55.8271)
  48. return RT_TRUE;
  49. return RT_FALSE;
  50. }
  51. /**
  52. * 纬度偏移变换
  53. */
  54. static double transformLat(double x, double y)
  55. {
  56. double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(abs(x));
  57. ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
  58. ret += (20.0 * sin(y * pi) + 40.0 * sin(y / 3.0 * pi)) * 2.0 / 3.0;
  59. ret += (160.0 * sin(y / 12.0 * pi) + 320 * sin(y * pi / 30.0)) * 2.0 / 3.0;
  60. return ret;
  61. }
  62. /**
  63. * 经度偏移变换
  64. */
  65. static double transformLon(double x, double y)
  66. {
  67. double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(abs(x));
  68. ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
  69. ret += (20.0 * sin(x * pi) + 40.0 * sin(x / 3.0 * pi)) * 2.0 / 3.0;
  70. ret += (150.0 * sin(x / 12.0 * pi) + 300.0 * sin(x / 30.0 * pi)) * 2.0 / 3.0;
  71. return ret;
  72. }
  73. /**
  74. * WGS84 坐标转 GCJ-02(火星坐标系)
  75. */
  76. static void wgs84_to_gcj02(double wg_lat, double wg_lon, double *gcj_lat, double *gcj_lon)
  77. {
  78. if (outOfChina(wg_lat, wg_lon))
  79. {
  80. *gcj_lat = wg_lat;
  81. *gcj_lon = wg_lon;
  82. return;
  83. }
  84. double dlat = transformLat(wg_lon - 105.0, wg_lat - 35.0);
  85. double dlon = transformLon(wg_lon - 105.0, wg_lat - 35.0);
  86. double radlat = wg_lat / 180.0 * pi;
  87. double magic = sin(radlat);
  88. magic = 1 - ee * magic * magic;
  89. double sqrtmagic = sqrt(magic);
  90. dlat = (dlat * 180.0) / ((a * (1 - ee)) / (magic * sqrtmagic) * pi);
  91. dlon = (dlon * 180.0) / (a / sqrtmagic * cos(radlat) * pi);
  92. *gcj_lat = wg_lat + dlat;
  93. *gcj_lon = wg_lon + dlon;
  94. }
  95. /**
  96. * UART 接收回调函数:有数据到达时释放信号量
  97. */
  98. static rt_err_t gps_uart_rx_ind(rt_device_t dev, rt_size_t size)
  99. {
  100. if (size > 0)
  101. {
  102. rt_sem_release(gps_sem);
  103. }
  104. return RT_EOK;
  105. }
  106. /**
  107. * 发送 GPS 模块配置命令(切换模式、频率等)
  108. */
  109. static void gps_send_config(void)
  110. {
  111. const char *config_cmds[] = {
  112. "$PGKC115,0,0,1,0*2B\r\n", // GPS+BeiDou 模式
  113. "$PGKC242,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*37\r\n", // 输出语句配置
  114. "$PMTK220,1000*1F\r\n", // 1Hz 定位频率
  115. RT_NULL};
  116. LOG_I("Sending GPS configuration commands...");
  117. for (int i = 0; config_cmds[i] != RT_NULL; i++)
  118. {
  119. rt_device_write(gps_uart, 0, config_cmds[i], rt_strlen(config_cmds[i]));
  120. rt_thread_mdelay(200);
  121. }
  122. // 发送热启动命令
  123. const char *hot_start_cmd = "$PMTK101*32\r\n";
  124. rt_device_write(gps_uart, 0, hot_start_cmd, rt_strlen(hot_start_cmd));
  125. LOG_I("Hot start triggered.");
  126. }
  127. /**
  128. * 将浮点数格式化为字符串,使用整数放大法(不依赖 %f)
  129. * 示例:value=31.234567, decimals=6 → "31.234567"
  130. */
  131. static void double_to_str(char *str, double value, int decimals)
  132. {
  133. // 处理负数
  134. if (value < 0)
  135. {
  136. *str++ = '-';
  137. value = -value;
  138. }
  139. // 分离整数和小数部分
  140. long long_part = (long)value;
  141. double frac_part = value - (double)long_part;
  142. long frac_power = 1;
  143. for (int i = 0; i < decimals; i++)
  144. frac_power *= 10;
  145. // 四舍五入小数部分
  146. long frac_int = (long)(frac_part * frac_power + 0.5);
  147. // 拼接字符串
  148. if (decimals > 0)
  149. {
  150. rt_snprintf(str, 16, "%ld.%0*ld", long_part, decimals, frac_int);
  151. }
  152. else
  153. {
  154. rt_snprintf(str, 16, "%ld", long_part);
  155. }
  156. }
  157. /**
  158. * GPS 数据处理线程
  159. */
  160. static void gps_thread_entry(void *parameter)
  161. {
  162. rt_size_t recv_len;
  163. double gcj_lat, gcj_lon;
  164. static uint32_t timeout_count = 0;
  165. lwgps_init(&gps_handler);
  166. LOG_I("GPS thread running...");
  167. static char once = 1;
  168. while (1)
  169. {
  170. if (rt_sem_take(gps_sem, rt_tick_from_millisecond(2000)) == RT_EOK)
  171. {
  172. recv_len = rt_device_read(gps_uart, 0, gps_buffer, GPS_BUFFER_SIZE);
  173. if (recv_len > 0)
  174. {
  175. timeout_count = 0;
  176. lwgps_process(&gps_handler, (const char *)gps_buffer, recv_len);
  177. if (gps_handler.is_valid)
  178. {
  179. // 坐标转换
  180. wgs84_to_gcj02(gps_handler.latitude, gps_handler.longitude, &gcj_lat, &gcj_lon);
  181. // 格式化输出字符串(放大法)
  182. char lat_str[16], lon_str[16], alt_str[16];
  183. double_to_str(lat_str, gcj_lat, 6);
  184. double_to_str(lon_str, gcj_lon, 6);
  185. double_to_str(alt_str, gps_handler.altitude, 1);
  186. reflash_gps();
  187. if (once && gps_handler.sats_in_use > 5)
  188. {
  189. once = 0;
  190. gps_get_location();
  191. LOG_I("FIXED: Lat=%s, Lon=%s, Alt=%sm, Sats=%d\n\n",
  192. lat_str, lon_str, alt_str, gps_handler.sats_in_use);
  193. }
  194. }
  195. }
  196. }
  197. else
  198. {
  199. timeout_count++;
  200. if (timeout_count > 30)
  201. {
  202. LOG_E("GPS data timeout! Re-sending config...");
  203. gps_send_config();
  204. timeout_count = 0;
  205. gpsx.gpssta = 0;
  206. }
  207. }
  208. }
  209. }
  210. /**
  211. * GPS 应用初始化函数(自动执行)
  212. */
  213. static int gps_app_init(void)
  214. {
  215. rt_err_t result = RT_EOK;
  216. struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;
  217. LOG_I("Initializing GPS application...");
  218. // 查找 UART 设备
  219. gps_uart = rt_device_find(GPS_UART_NAME);
  220. if (gps_uart == RT_NULL)
  221. {
  222. LOG_E("UART device '%s' not found!", GPS_UART_NAME);
  223. return -RT_ERROR;
  224. }
  225. // 配置串口参数
  226. config.baud_rate = GPS_UART_BAUDRATE;
  227. config.data_bits = DATA_BITS_8;
  228. config.stop_bits = STOP_BITS_1;
  229. config.parity = PARITY_NONE;
  230. config.bufsz = 1024;
  231. if (rt_device_control(gps_uart, RT_DEVICE_CTRL_CONFIG, &config) != RT_EOK)
  232. {
  233. LOG_E("Failed to configure UART!");
  234. return -RT_ERROR;
  235. }
  236. // 创建信号量
  237. gps_sem = rt_sem_create("gps_sem", 0, RT_IPC_FLAG_FIFO);
  238. if (gps_sem == RT_NULL)
  239. {
  240. LOG_E("Failed to create semaphore!");
  241. return -RT_ERROR;
  242. }
  243. // 打开设备并设置中断接收回调
  244. if (rt_device_open(gps_uart, RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX) != RT_EOK)
  245. {
  246. LOG_E("Failed to open UART device!");
  247. rt_sem_delete(gps_sem);
  248. return -RT_ERROR;
  249. }
  250. rt_device_set_rx_indicate(gps_uart, gps_uart_rx_ind);
  251. // 发送初始配置
  252. gps_send_config();
  253. // 创建 GPS 处理线程
  254. rt_thread_t thread = rt_thread_create("gps",
  255. gps_thread_entry,
  256. RT_NULL,
  257. 2048,
  258. 15,
  259. 10);
  260. if (thread != RT_NULL)
  261. {
  262. rt_thread_startup(thread);
  263. LOG_I("GPS application started successfully.");
  264. }
  265. else
  266. {
  267. LOG_E("Failed to create GPS thread!");
  268. rt_device_close(gps_uart);
  269. rt_sem_delete(gps_sem);
  270. result = -RT_ERROR;
  271. }
  272. return result;
  273. }
  274. /**
  275. * 手动查询当前定位信息(MSH 命令)
  276. */
  277. static void reflash_gps(void)
  278. {
  279. if (gps_handler.is_valid)
  280. {
  281. double gcj_lat, gcj_lon;
  282. wgs84_to_gcj02(gps_handler.latitude, gps_handler.longitude, &gcj_lat, &gcj_lon);
  283. double_to_str(wgs84_lat, gps_handler.latitude, 6);
  284. double_to_str(wgs84_lon, gps_handler.longitude, 6);
  285. double_to_str(gcj02_lat, gcj_lat, 6);
  286. double_to_str(gcj02_lon, gcj_lon, 6);
  287. double_to_str(altitude, gps_handler.altitude, 1);
  288. rt_snprintf(sats_used, sizeof(sats_used), "%d", gps_handler.sats_in_use);
  289. rt_snprintf(sats_view, sizeof(sats_view), "%d", gps_handler.sats_in_view);
  290. if(gps_handler.sats_in_use > 5)
  291. {
  292. gpsx.posslnum = gps_handler.sats_in_use;
  293. gpsx.gpssta = 1;
  294. sprintf(Tran_lng , "%s" , wgs84_lon);
  295. sprintf(Tran_lat , "%s" , wgs84_lat);
  296. gpsx.altitude = (int)(gps_handler.altitude * 10);
  297. sprintf(sys_info.lng , "%s" , wgs84_lon);
  298. sprintf(sys_info.lat , "%s" , wgs84_lat);
  299. sprintf(sys_info.altitude , "%s" , altitude);
  300. }
  301. }
  302. }
  303. static void gps_get_location(void)
  304. {
  305. if (gps_handler.is_valid)
  306. {
  307. reflash_gps();
  308. rt_kprintf("\n\n === GPS Location ===\n");
  309. rt_kprintf(" WGS84:\n");
  310. rt_kprintf(" Lat: %s\n", wgs84_lat);
  311. rt_kprintf(" Lon: %s\n", wgs84_lon);
  312. rt_kprintf(" GCJ02:\n");
  313. rt_kprintf(" Lat: %s\n", gcj02_lat);
  314. rt_kprintf(" Lon: %s\n", gcj02_lon);
  315. rt_kprintf(" Altitude: %s m\n", altitude);
  316. rt_kprintf(" Satellites: %s used / %s viewed\n", sats_used, sats_view);
  317. rt_kprintf(" Status: Valid Fix\n");
  318. }
  319. else
  320. {
  321. char sats_view[4];
  322. rt_snprintf(sats_view, sizeof(sats_view), "%d", gps_handler.sats_in_view);
  323. rt_kprintf("No valid GPS fix.\n");
  324. rt_kprintf("Satellites in view: %s\n", sats_view);
  325. }
  326. }
  327. /**
  328. * 重新发送配置命令(MSH 命令)
  329. */
  330. static void gps_reconfig(int argc, char **argv)
  331. {
  332. rt_kprintf("Reconfiguring GPS module...\n");
  333. gps_send_config();
  334. rt_kprintf("Configuration sent.\n");
  335. }
  336. /* 导出 MSH 命令 */
  337. MSH_CMD_EXPORT(gps_get_location, Get current GPS location);
  338. MSH_CMD_EXPORT(gps_reconfig, Reconfigure GPS module);
  339. /* 自动初始化 */
  340. INIT_APP_EXPORT(gps_app_init);