From e3baeaa47f53f8e155b244fe79e261964964ed6d Mon Sep 17 00:00:00 2001 From: zsx <1543588107@qq.com> Date: Mon, 14 Oct 2024 22:11:43 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0GPS=E5=AE=9A=E4=BD=8Dnmea?= =?UTF-8?q?=E5=8D=8F=E8=AE=AE=E8=A7=A3=E6=9E=90=E5=8A=9F=E8=83=BD=20zsxfly?= =?UTF-8?q?20241014?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- custom/GPS/gps.mk | 6 + custom/GPS/inc/gps_config.h | 11 + custom/GPS/src/gps_config.c | 163 ++++++++ custom/custom.mk | 2 + custom/custom_main/custom_main.mk | 1 + custom/custom_main/inc/app_common.h | 53 +++ custom/custom_main/inc/app_uart.h | 32 ++ custom/custom_main/src/app_uart.c | 221 ++++++++++ custom/custom_main/src/custom_main.c | 75 ++++ custom/nmealib/inc/nmea/config.h | 51 +++ custom/nmealib/inc/nmea/context.h | 44 ++ custom/nmealib/inc/nmea/generate.h | 44 ++ custom/nmealib/inc/nmea/generator.h | 79 ++++ custom/nmealib/inc/nmea/gmath.h | 92 ++++ custom/nmealib/inc/nmea/info.h | 112 +++++ custom/nmealib/inc/nmea/nmea.h | 25 ++ custom/nmealib/inc/nmea/parse.h | 42 ++ custom/nmealib/inc/nmea/parser.h | 63 +++ custom/nmealib/inc/nmea/sentence.h | 168 ++++++++ custom/nmealib/inc/nmea/time.h | 47 +++ custom/nmealib/inc/nmea/tok.h | 30 ++ custom/nmealib/inc/nmea/units.h | 30 ++ custom/nmealib/nmealib.mk | 15 + custom/nmealib/src/context.c | 67 +++ custom/nmealib/src/generate.c | 229 ++++++++++ custom/nmealib/src/generator.c | 399 ++++++++++++++++++ custom/nmealib/src/gmath.c | 377 +++++++++++++++++ custom/nmealib/src/info.c | 21 + custom/nmealib/src/nmea.vcproj | 414 ++++++++++++++++++ custom/nmealib/src/parse.c | 601 +++++++++++++++++++++++++++ custom/nmealib/src/parser.c | 331 +++++++++++++++ custom/nmealib/src/sentence.c | 69 +++ custom/nmealib/src/time.c | 63 +++ custom/nmealib/src/tok.c | 250 +++++++++++ 34 files changed, 4227 insertions(+) create mode 100644 custom/GPS/gps.mk create mode 100644 custom/GPS/inc/gps_config.h create mode 100644 custom/GPS/src/gps_config.c create mode 100644 custom/custom_main/inc/app_common.h create mode 100644 custom/custom_main/inc/app_uart.h create mode 100644 custom/custom_main/src/app_uart.c create mode 100644 custom/nmealib/inc/nmea/config.h create mode 100644 custom/nmealib/inc/nmea/context.h create mode 100644 custom/nmealib/inc/nmea/generate.h create mode 100644 custom/nmealib/inc/nmea/generator.h create mode 100644 custom/nmealib/inc/nmea/gmath.h create mode 100644 custom/nmealib/inc/nmea/info.h create mode 100644 custom/nmealib/inc/nmea/nmea.h create mode 100644 custom/nmealib/inc/nmea/parse.h create mode 100644 custom/nmealib/inc/nmea/parser.h create mode 100644 custom/nmealib/inc/nmea/sentence.h create mode 100644 custom/nmealib/inc/nmea/time.h create mode 100644 custom/nmealib/inc/nmea/tok.h create mode 100644 custom/nmealib/inc/nmea/units.h create mode 100644 custom/nmealib/nmealib.mk create mode 100644 custom/nmealib/src/context.c create mode 100644 custom/nmealib/src/generate.c create mode 100644 custom/nmealib/src/generator.c create mode 100644 custom/nmealib/src/gmath.c create mode 100644 custom/nmealib/src/info.c create mode 100644 custom/nmealib/src/nmea.vcproj create mode 100644 custom/nmealib/src/parse.c create mode 100644 custom/nmealib/src/parser.c create mode 100644 custom/nmealib/src/sentence.c create mode 100644 custom/nmealib/src/time.c create mode 100644 custom/nmealib/src/tok.c diff --git a/custom/GPS/gps.mk b/custom/GPS/gps.mk new file mode 100644 index 0000000..0336747 --- /dev/null +++ b/custom/GPS/gps.mk @@ -0,0 +1,6 @@ + +CUSTOM_MAIN_DIR := custom/GPS + +OC_FILES += $(CUSTOM_MAIN_DIR)/src/gps_config.c + +INC += -I'$(CUSTOM_MAIN_DIR)/inc' \ No newline at end of file diff --git a/custom/GPS/inc/gps_config.h b/custom/GPS/inc/gps_config.h new file mode 100644 index 0000000..401337c --- /dev/null +++ b/custom/GPS/inc/gps_config.h @@ -0,0 +1,11 @@ +#ifndef GPS_CONFIG_H +#define GPS_CONFIG_H +#include "cm_os.h" +#include "nmea/nmea.h" + + +void gps_config_init(void); +/* 关闭串口 */ +void gps_config_close(void); + +#endif /* GPS_CONFIG_H */ \ No newline at end of file diff --git a/custom/GPS/src/gps_config.c b/custom/GPS/src/gps_config.c new file mode 100644 index 0000000..9bff314 --- /dev/null +++ b/custom/GPS/src/gps_config.c @@ -0,0 +1,163 @@ +#include "cm_iomux.h" +#include "cm_gpio.h" +#include "stdio.h" +#include "stdlib.h" +#include "stdarg.h" +#include "cm_os.h" +#include "cm_mem.h" +#include "cm_sys.h" +#include "cm_uart.h" + +#include "app_common.h" +#include "app_uart.h" + +#include "gps_config.h" + +#define GPS_URAT CM_UART_DEV_1 +#define GPS_RX_IOMUX UART1_RX_IOMUX +#define GPS_TX_IOMUX UART1_TX_IOMUX + +#define GPS_BUF_LEN 1024 + +static int gps_rev_len = 0; +static char gps_rev_data[GPS_BUF_LEN] = {0}; + +static osThreadId_t GPS_ThreadId = NULL; //串口数据接收、解析任务Handle +static void* gps_uart_sem = NULL; //串口数据接收、解析任务信号量 + + +/* 串口接收处理任务，平时使用信号量挂起，当收到接收事件后，释放信号量以触发读取任务 */ +static void gps_TaskHandle(void *param){ + int temp_len = 0; + int it = 0; + // nmeaPOS dpos; + nmeaINFO info; + nmeaPARSER parser; + + nmea_zero_INFO(&info); + nmea_parser_init(&parser); + while(1){ + if(gps_uart_sem != NULL){ + osSemaphoreAcquire(gps_uart_sem, osWaitForever);//阻塞 + } + temp_len = cm_uart_read(GPS_URAT, (void*)&gps_rev_data[gps_rev_len], GPS_BUF_LEN, 1000); + if(temp_len > 0){ + + nmea_parse(&parser, gps_rev_data, temp_len, &info); + // nmea_info2pos(&info, &dpos); + + // app_printf( + // "%03d, Lat: %f, Lon: %f, Sig: %d, Fix: %d, RL= %d\n", + // it++, dpos.lat, dpos.lon, info.sig, info.fix, temp_len + // ); + app_printf( + "%03d, Lat: %f, Lon: %f, Sig: %d, Fix: %d, RL= %d\n", + it++, info.lat, info.lon, info.sig, info.fix, temp_len + ); + // cm_uart_write(CM_UART_DEV_0, gps_rev_data, temp_len, 1000); + memset((void*)gps_rev_data, 0, temp_len); + temp_len = 0; + gps_rev_len = 0; + } + // temp_len = cm_uart_read(GPS_URAT, (void*)&gps_rev_data[gps_rev_len], GPS_BUF_LEN - gps_rev_len, 1000); + // if(gps_rev_len < GPS_BUF_LEN){ + // gps_rev_len += temp_len; + // }else{ + // gps_rev_len = 0; + // } + // app_printf("uart rev data len = %d\n", gps_rev_len); + + // if (gps_uart_sem != NULL && (strstr(gps_rev_data, "\r\n"))){ + // //处理收到数据事件 + // cm_uart_write(GPS_URAT, gps_rev_data, gps_rev_len, 1000); + + // memset((void*)gps_rev_data, 0, sizeof(gps_rev_data)); + // gps_rev_len = 0; + // } + } + // nmea_parser_destroy(&parser); +} + +// 串口事件回调函数 +static void gps_uart_event_callback(void *param, uint32_t type){ + // uart_event_msg_t msg = {0}; + if (CM_UART_EVENT_TYPE_RX_ARRIVED & type){ + /* 收到接收事件，触发其他线程执行读取数据 */ + osSemaphoreRelease(gps_uart_sem); + } + + if (CM_UART_EVENT_TYPE_RX_OVERFLOW & type){ + /* 收到溢出事件，触发其他线程处理溢出事件 */ + // msg.msg_type = type; + + // if (uart_event_queue != NULL){//向队列发送数据 + // osMessageQueuePut(uart_event_queue, &msg, 0, 0); + // } + } +} + +void gps_config_init(void){ + int32_t ret = -1; + + // 配置引脚复用 + cm_iomux_set_pin_func(GPS_RX_IOMUX); + cm_iomux_set_pin_func(GPS_TX_IOMUX); + + cm_iomux_set_pin_cmd(CM_IOMUX_PIN_28, CM_IOMUX_PINCMD3_PULL, CM_IOMUX_PINCMD3_FUNC2_PULL_HIGH); + // cm_iomux_set_pin_cmd(CM_IOMUX_PIN_29, CM_IOMUX_PINCMD3_PULL, CM_IOMUX_PINCMD3_FUNC2_PULL_HIGH); + + // 事件参数 + cm_uart_event_t uart_event = { + CM_UART_EVENT_TYPE_RX_ARRIVED | CM_UART_EVENT_TYPE_RX_OVERFLOW, //注册需要上报的事件类型 + "uart1", //用户参数 + gps_uart_event_callback //上报事件的回调函数 + }; + // 注册事件和回调函数 + ret = cm_uart_register_event(GPS_URAT, &uart_event); + if(ret != RET_SUCCESS){ + cm_log_printf(0, "uart register event err,ret=%d\n", ret); + return; + } + + // 配置参数 + cm_uart_cfg_t uart_cfg ={ + CM_UART_BYTE_SIZE_8, + CM_UART_PARITY_NONE, + CM_UART_STOP_BIT_ONE, + CM_UART_FLOW_CTRL_NONE, + CM_UART_BAUDRATE_9600, + 0 //配置为普通串口模式，若要配置为低功耗模式可改为1 + }; + // 开启串口 + ret = cm_uart_open(GPS_URAT, &uart_cfg); + if(ret != RET_SUCCESS){ + cm_log_printf(0, "uart init err,ret=%d\n", ret); + return; + } + + // 串口接收处理任务 + osThreadAttr_t gps_task_attr = {0}; + gps_task_attr.name = "gps_uart_task"; + gps_task_attr.stack_size = 2048; + gps_task_attr.priority= osPriorityNormal; + + GPS_ThreadId= osThreadNew(gps_TaskHandle, 0, &gps_task_attr); + + if(gps_uart_sem == NULL) { + gps_uart_sem = osSemaphoreNew(1, 0, NULL); + } + +} + +/* 关闭串口 */ +void gps_config_close(void){ + cm_uart_dev_e dev = GPS_URAT; + + if(0 == cm_uart_close(dev)){ + app_printf("uart%d close is ok\n", dev); + }else{ + app_printf("uart%d close is error\n", dev); + } +} + + diff --git a/custom/custom.mk b/custom/custom.mk index 2308d5b..25bee14 100644 --- a/custom/custom.mk +++ b/custom/custom.mk @@ -4,4 +4,6 @@ CUSTOM_DIR := custom ifeq ($(strip $(DEMO_SUPPORT)),n) include $(CUSTOM_DIR)/custom_main/custom_main.mk +include $(CUSTOM_DIR)/nmealib/nmealib.mk +include $(CUSTOM_DIR)/GPS/gps.mk endif \ No newline at end of file diff --git a/custom/custom_main/custom_main.mk b/custom/custom_main/custom_main.mk index e8369d0..b280537 100644 --- a/custom/custom_main/custom_main.mk +++ b/custom/custom_main/custom_main.mk @@ -3,4 +3,5 @@ CUSTOM_MAIN_DIR := custom/custom_main OC_FILES += $(CUSTOM_MAIN_DIR)/src/custom_main.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/app_uart.c INC += -I'$(CUSTOM_MAIN_DIR)/inc' \ No newline at end of file diff --git a/custom/custom_main/inc/app_common.h b/custom/custom_main/inc/app_common.h new file mode 100644 index 0000000..e35354e --- /dev/null +++ b/custom/custom_main/inc/app_common.h @@ -0,0 +1,53 @@ +#ifndef __app_COMMON_H__ +#define __app_COMMON_H__ + + +/**************************************************************************** + * Included Files + ****************************************************************************/ +#include +#include +#include +#include "cm_os.h" +#include "cm_iomux.h" + + +//uart0 +#define UART0_TX_IOMUX CM_IOMUX_PIN_18, CM_IOMUX_FUNC_FUNCTION1 +#define UART0_RX_IOMUX CM_IOMUX_PIN_17, CM_IOMUX_FUNC_FUNCTION1 + +//uart1 +#define UART1_RX_IOMUX CM_IOMUX_PIN_28, CM_IOMUX_FUNC_FUNCTION1 +#define UART1_TX_IOMUX CM_IOMUX_PIN_29, CM_IOMUX_FUNC_FUNCTION1 + +//uart2 +#define UART2_TX_IOMUX CM_IOMUX_PIN_50, CM_IOMUX_FUNC_FUNCTION3 +#define UART2_RX_IOMUX CM_IOMUX_PIN_51, CM_IOMUX_FUNC_FUNCTION3 + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifndef FALSE +#define FALSE (0U) +#endif + +#ifndef TRUE +#define TRUE (1U) +#endif + + +typedef enum{ + RET_BUSY = -2, + RET_ERROR =-1, + RET_SUCCESS = 0 +}CM_RET_E; + + + + +#endif /* __APP_COMMON_H__ */ + + + + diff --git a/custom/custom_main/inc/app_uart.h b/custom/custom_main/inc/app_uart.h new file mode 100644 index 0000000..4ebb603 --- /dev/null +++ b/custom/custom_main/inc/app_uart.h @@ -0,0 +1,32 @@ +/********************************************************* +* @file cm_demo_uart.h +* @brief ML302 OpenCPU main header file +* Copyright (c) 2019 China Mobile IOT. +* All rights reserved. +* created by CaoChi 2020/12/10 +********************************************************/ +#ifndef __CM_UART_DEMO_H__ +#define __CM_UART_DEMO_H__ +#include "cm_os.h" + +#define CM_PRT_INF(fmt,arg...) app_printf("[CM_INF]line %d in %s:"fmt,__LINE__,__FILE__,##arg) +#define CM_PRT_WRN(fmt,arg...) app_printf("[CM_WRN]line %d in %s:"fmt,__LINE__,__FILE__,##arg) +#define CM_PRT_ERR(fmt,arg...) app_printf("[CM_ERR]line %d in %s:"fmt,__LINE__,__FILE__,##arg) + +/** + * \brief 串口接收数据结构体 + */ +typedef struct +{ + unsigned char buf[1024]; + int len; +}cm_uart_recv_data_buf_t; + + +void app_uart_init(void); +void app_uart_close(void); + +void app_printf (char *str, ...); + + +#endif diff --git a/custom/custom_main/src/app_uart.c b/custom/custom_main/src/app_uart.c new file mode 100644 index 0000000..cea63c2 --- /dev/null +++ b/custom/custom_main/src/app_uart.c @@ -0,0 +1,221 @@ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "cm_iomux.h" +#include "cm_gpio.h" +#include "stdio.h" +#include "stdlib.h" +#include "stdarg.h" +#include "cm_os.h" +#include "cm_mem.h" +#include "cm_sys.h" +#include "cm_uart.h" +#include "app_uart.h" +#include "app_common.h" + + + +#define APP_UART_TASK_PRIORITY osPriorityNormal + +#define APP_URAT 0 + +//uart0 +#if (APP_URAT == 0) +#define APP_UARTTX_IOMUX CM_IOMUX_PIN_18, CM_IOMUX_FUNC_FUNCTION1 +#define APP_UARTRX_IOMUX CM_IOMUX_PIN_17, CM_IOMUX_FUNC_FUNCTION1 +#endif + +//uart1 +#if (APP_URAT == 1) +#define APP_UARTRX_IOMUX CM_IOMUX_PIN_28, CM_IOMUX_FUNC_FUNCTION1 +#define APP_UARTTX_IOMUX CM_IOMUX_PIN_29, CM_IOMUX_FUNC_FUNCTION1 +#endif + +#if (APP_URAT == 2) +#define APP_UARTTX_IOMUX CM_IOMUX_PIN_50, CM_IOMUX_FUNC_FUNCTION3 +#define APP_UARTRX_IOMUX CM_IOMUX_PIN_51, CM_IOMUX_FUNC_FUNCTION3 +#endif + + +typedef struct{ + int msg_type; +} uart_event_msg_t; + +#define UART_BUF_LEN 1024 + +static int rx_rev_len = 0; +static char rx_rev_data[UART_BUF_LEN] = {0}; + +static osThreadId_t os_UART_ThreadId = NULL; //串口数据接收、解析任务Handle +static osThreadId_t uart_event_thread = NULL; + +static void* g_uart_sem = NULL; +static osMessageQueueId_t uart_event_queue = NULL; + + +// 用于测试串口事件，用户可参考 +static void uart_event_task(void *arg){ + uart_event_msg_t msg = {0}; + + while (1) { + if (osMessageQueueGet(uart_event_queue, &msg, NULL, osWaitForever) == osOK) { + //cm_log_printf(0, "uart event msg type = %d\n", msg.msg_type); + if (CM_UART_EVENT_TYPE_RX_OVERFLOW & msg.msg_type){ + app_printf("CM_UART_EVENT_TYPE_RX_OVERFLOW... ...\r\n"); + cm_uart_read(APP_URAT, (void*)&rx_rev_data[0], UART_BUF_LEN, 1000); + cm_uart_read(APP_URAT, (void*)&rx_rev_data[0], UART_BUF_LEN, 1000); + cm_uart_read(APP_URAT, (void*)&rx_rev_data[0], UART_BUF_LEN, 1000); + cm_uart_read(APP_URAT, (void*)&rx_rev_data[0], UART_BUF_LEN, 1000); + } + } + } +} + +// 用于测试串口事件，用户可参考 +static int uart_event_task_create(void){ + if (uart_event_queue == NULL){ + uart_event_queue = osMessageQueueNew(10, sizeof(uart_event_msg_t), NULL); + } + + if (uart_event_thread == NULL) { + osThreadAttr_t attr1 = { + .name = "uart_event", + .priority = APP_UART_TASK_PRIORITY, + .stack_size = 1024, + }; + uart_event_thread = osThreadNew(uart_event_task, NULL, (const osThreadAttr_t*)&attr1); + } + + return 0; +} + + +/* 串口接收示例，平时使用信号量挂起，当收到接收事件后，释放信号量以触发读取任务 */ +static void Uart_TaskHandle(void *param){ + int temp_len = 0; + + while (1){ + if (g_uart_sem != NULL){ + osSemaphoreAcquire(g_uart_sem, osWaitForever);//阻塞 + } + if (rx_rev_len < UART_BUF_LEN){ + temp_len = cm_uart_read(APP_URAT, (void*)&rx_rev_data[rx_rev_len], UART_BUF_LEN - rx_rev_len, 1000); + rx_rev_len += temp_len; + } + app_printf("uart rev data len = %d\n", rx_rev_len); + + /* 后续用于SDK测试，用户可酌情参考*/ + if (g_uart_sem != NULL && (strstr(rx_rev_data, "\r\n"))){ + //处理收到数据事件 + cm_uart_write(APP_URAT, rx_rev_data, rx_rev_len, 1000); + + memset((void*)rx_rev_data, 0, sizeof(rx_rev_data)); + rx_rev_len = 0; + } + } +} + + +// 串口事件回调函数// 回调函数中不可输出LOG、串口打印、执行复杂任务或消耗过多资源，建议以信号量或消息队列形式控制其他线程执行任务 +static void app_uart_event_callback(void *param, uint32_t type){ + uart_event_msg_t msg = {0}; + if (CM_UART_EVENT_TYPE_RX_ARRIVED & type){ + /* 收到接收事件，触发其他线程执行读取数据 */ + osSemaphoreRelease(g_uart_sem); + } + + if (CM_UART_EVENT_TYPE_RX_OVERFLOW & type){ + /* 收到溢出事件，触发其他线程处理溢出事件 */ + msg.msg_type = type; + + if (uart_event_queue != NULL){//向队列发送数据 + osMessageQueuePut(uart_event_queue, &msg, 0, 0); + } + } +} + +void app_uart_init(void){ + int32_t ret = -1; + + // 配置引脚复用 + cm_iomux_set_pin_func(APP_UARTTX_IOMUX); + cm_iomux_set_pin_func(APP_UARTRX_IOMUX); + + cm_iomux_set_pin_cmd(CM_IOMUX_PIN_17, CM_IOMUX_PINCMD3_PULL, CM_IOMUX_PINCMD3_FUNC2_PULL_HIGH); + // cm_iomux_set_pin_cmd(CM_IOMUX_PIN_18, CM_IOMUX_PINCMD3_PULL, CM_IOMUX_PINCMD3_FUNC2_PULL_HIGH); + + // 事件参数 + cm_uart_event_t uart_event = { + CM_UART_EVENT_TYPE_RX_ARRIVED | CM_UART_EVENT_TYPE_RX_OVERFLOW, //注册需要上报的事件类型 + "uart0", //用户参数 + app_uart_event_callback //上报事件的回调函数 + }; + // 注册事件和回调函数 + ret = cm_uart_register_event(APP_URAT, &uart_event); + if(ret != RET_SUCCESS){ + cm_log_printf(0, "uart register event err,ret=%d\n", ret); + return; + } + + // 配置参数 + cm_uart_cfg_t uart_cfg = { + CM_UART_BYTE_SIZE_8, + CM_UART_PARITY_NONE, + CM_UART_STOP_BIT_ONE, + CM_UART_FLOW_CTRL_NONE, + CM_UART_BAUDRATE_9600, + 0 //配置为普通串口模式，若要配置为低功耗模式可改为1 + }; + // 开启串口 + ret = cm_uart_open(APP_URAT, &uart_cfg); + if (ret != RET_SUCCESS){ + cm_log_printf(0, "uart init err,ret=%d\n", ret); + return; + } + + // // 配置uart唤醒功能，使能边沿检测才具备唤醒功能，仅主串口具有唤醒功能，用于唤醒的数据并不能被uart接收，请在唤醒后再进行uart数传 + // cm_iomux_set_pin_cmd(APP_UARTRX_IOMUX , CM_IOMUX_PINCMD1_LPMEDEG, CM_IOMUX_PINCMD1_FUNC1_LPM_EDGE_RISE); + + // 串口接收处理任务 + osThreadAttr_t uart_task_attr = {0}; + uart_task_attr.name = "uart_task"; + uart_task_attr.stack_size = 2048; + uart_task_attr.priority= APP_UART_TASK_PRIORITY; + + os_UART_ThreadId= osThreadNew(Uart_TaskHandle, 0, &uart_task_attr); + + if (g_uart_sem == NULL) { + g_uart_sem = osSemaphoreNew(1, 0, NULL); + } + uart_event_task_create(); +} + +/* 关闭串口 */ +void app_uart_close(void){ + cm_uart_dev_e dev = CM_UART_DEV_0; + + if (0 == cm_uart_close(dev)){ + app_printf("uart%d close is ok\n", dev); + }else{ + app_printf("uart%d close is error\n", dev); + } +} + + +void app_printf(char *str, ...){ + static char s[600]; //This needs to be large enough to store the string TODO Change magic number + va_list args; + int len; + + if ((str == NULL) || (strlen(str) == 0)) + { + return; + } + + va_start(args, str); + len = vsnprintf((char*)s, 600, str, args); + va_end(args); + cm_uart_write(APP_URAT, s, len, 1000); +} diff --git a/custom/custom_main/src/custom_main.c b/custom/custom_main/src/custom_main.c index 88903a9..d066d3b 100644 --- a/custom/custom_main/src/custom_main.c +++ b/custom/custom_main/src/custom_main.c @@ -1,6 +1,81 @@ +#include "stdio.h" +#include "stdlib.h" +// #include "cm_demo_main.h" +// #include "cm_demo_uart.h" +// #include "cm_demo_asocket.h" +// #include "cm_demo_fs.h" +#include "cm_fs.h" +#include "cm_mem.h" +#include "cm_sys.h" +#include "cm_sim.h" +#include "cm_virt_at.h" +#include "cm_rtc.h" +#include "cm_gpio.h" +#include "cm_iomux.h" +// #include "cm_demo_i2c.h" +// #include "cm_demo_adc.h" +// #include "cm_demo_spi.h" +// #include "cm_demo_lcd.h" +// #include "cm_demo_gpio.h" +// #include "cm_demo_keypad.h" +// #include "cm_demo_pwm.h" +// #include "cm_demo_pm.h" +// #include "cm_demo_http.h" +// #include "cm_demo_mqtt.h" +// #include "cm_demo_ntp.h" +// #include "cm_demo_fota.h" +// #include "cm_demo_audio.h" +// #include "cm_demo_virt_at.h" +// #include "cm_demo_lbs.h" +// #include "cm_demo_tts.h" +// #include "cm_modem.h" +// #include "cm_demo_cJSON.h" +// #include "cm_demo_sd.h" +// #include "cm_demo_gnss.h" +// #include "cm_demo_aliyun.h" +// #include "cm_demo_wifiscan.h" +// #include "cm_demo_camera.h" +// #include "cm_demo_ftp.h" +// #include "cm_demo_lwip.h" +// #include "cm_demo_ssl.h" +#include "app_uart.h" +#include "gps_config.h" + +osThreadId_t OC_APP_TaskHandle; + +void my_appimg_enter(char *param){ + cm_gpio_cfg_t cfg = {0}; + + cfg.direction = CM_GPIO_DIRECTION_OUTPUT; + cfg.pull = CM_GPIO_PULL_UP; + + cm_iomux_set_pin_func(CM_IOMUX_PIN_16, CM_IOMUX_FUNC_FUNCTION1);//初始化之前一定要先设置引脚复用 + + cm_gpio_init(0, &cfg); + cm_gpio_set_level(0, 1); + + app_uart_init(); + gps_config_init(); + app_printf("Hello, world!\r\n"); + while(1){ + osDelay(300/5);//300ms + cm_gpio_set_level(0, 1); + osDelay(300/5);//300ms + cm_gpio_set_level(0, 0); + // app_printf("Hello, world!\r\n"); + } +} int cm_opencpu_entry(char * param) { + osThreadAttr_t app_task_attr = {0}; + app_task_attr.name = "main_task"; + app_task_attr.stack_size = 4096 * 2; + app_task_attr.priority = osPriorityNormal; + + OC_APP_TaskHandle = osThreadNew((osThreadFunc_t)my_appimg_enter,0,&app_task_attr); + + return 0; return 0; } diff --git a/custom/nmealib/inc/nmea/config.h b/custom/nmealib/inc/nmea/config.h new file mode 100644 index 0000000..5014662 --- /dev/null +++ b/custom/nmealib/inc/nmea/config.h @@ -0,0 +1,51 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: config.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_CONFIG_H__ +#define __NMEA_CONFIG_H__ + +#define NMEA_VERSION ("0.5.3") +#define NMEA_VERSION_MAJOR (0) +#define NMEA_VERSION_MINOR (5) +#define NMEA_VERSION_PATCH (3) + +#define NMEA_CONVSTR_BUF (256) +#define NMEA_TIMEPARSE_BUF (256) + +#if defined(WINCE) || defined(UNDER_CE) +# define NMEA_CE +#endif + +#if defined(WIN32) || defined(NMEA_CE) +# define NMEA_WIN +#else +# define NMEA_UNI +#endif + +#if defined(NMEA_WIN) && (_MSC_VER >= 1400) +# pragma warning(disable: 4996) /* declared deprecated */ +#endif + +#if defined(_MSC_VER) +# define NMEA_POSIX(x) _##x +# define NMEA_INLINE __inline +#else +# define NMEA_POSIX(x) x +# define NMEA_INLINE inline +#endif + +#if !defined(NDEBUG) && !defined(NMEA_CE) +# include +# define NMEA_ASSERT(x) assert(x) +#else +# define NMEA_ASSERT(x) +#endif + +#endif /* __NMEA_CONFIG_H__ */ diff --git a/custom/nmealib/inc/nmea/context.h b/custom/nmealib/inc/nmea/context.h new file mode 100644 index 0000000..24600ad --- /dev/null +++ b/custom/nmealib/inc/nmea/context.h @@ -0,0 +1,44 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: context.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_CONTEXT_H__ +#define __NMEA_CONTEXT_H__ + +#include "config.h" + +#define NMEA_DEF_PARSEBUFF (1024) +#define NMEA_MIN_PARSEBUFF (256) + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void (*nmeaTraceFunc)(const char *str, int str_size); +typedef void (*nmeaErrorFunc)(const char *str, int str_size); + +typedef struct _nmeaPROPERTY +{ + nmeaTraceFunc trace_func; + nmeaErrorFunc error_func; + int parse_buff_size; + +} nmeaPROPERTY; + +nmeaPROPERTY * nmea_property(); + +void nmea_trace(const char *str, ...); +void nmea_trace_buff(const char *buff, int buff_size); +void nmea_error(const char *str, ...); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_CONTEXT_H__ */ diff --git a/custom/nmealib/inc/nmea/generate.h b/custom/nmealib/inc/nmea/generate.h new file mode 100644 index 0000000..0d133b6 --- /dev/null +++ b/custom/nmealib/inc/nmea/generate.h @@ -0,0 +1,44 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generate.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_GENERATE_H__ +#define __NMEA_GENERATE_H__ + +#include "sentence.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int nmea_generate( + char *buff, int buff_sz, /* buffer */ + const nmeaINFO *info, /* source info */ + int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */ + ); + +int nmea_gen_GGA(char *buff, int buff_sz, nmeaGGA *pack); +int nmea_gen_GSA(char *buff, int buff_sz, nmeaGSA *pack); +int nmea_gen_GSV(char *buff, int buff_sz, nmeaGSV *pack); +int nmea_gen_RMC(char *buff, int buff_sz, nmeaRMC *pack); +int nmea_gen_VTG(char *buff, int buff_sz, nmeaVTG *pack); + +void nmea_info2GGA(const nmeaINFO *info, nmeaGGA *pack); +void nmea_info2GSA(const nmeaINFO *info, nmeaGSA *pack); +void nmea_info2RMC(const nmeaINFO *info, nmeaRMC *pack); +void nmea_info2VTG(const nmeaINFO *info, nmeaVTG *pack); + +int nmea_gsv_npack(int sat_count); +void nmea_info2GSV(const nmeaINFO *info, nmeaGSV *pack, int pack_idx); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GENERATE_H__ */ diff --git a/custom/nmealib/inc/nmea/generator.h b/custom/nmealib/inc/nmea/generator.h new file mode 100644 index 0000000..a97b91b --- /dev/null +++ b/custom/nmealib/inc/nmea/generator.h @@ -0,0 +1,79 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generator.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_GENERATOR_H__ +#define __NMEA_GENERATOR_H__ + +#include "info.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * high level + */ + +struct _nmeaGENERATOR; + +enum nmeaGENTYPE +{ + NMEA_GEN_NOISE = 0, + NMEA_GEN_STATIC, + NMEA_GEN_ROTATE, + + NMEA_GEN_SAT_STATIC, + NMEA_GEN_SAT_ROTATE, + NMEA_GEN_POS_RANDMOVE, + + NMEA_GEN_LAST +}; + +struct _nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info); +void nmea_destroy_generator(struct _nmeaGENERATOR *gen); + +int nmea_generate_from( + char *buff, int buff_sz, /* buffer */ + nmeaINFO *info, /* source info */ + struct _nmeaGENERATOR *gen, /* generator */ + int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */ + ); + +/* + * low level + */ + +typedef int (*nmeaNMEA_GEN_INIT)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_LOOP)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_RESET)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_DESTROY)(struct _nmeaGENERATOR *gen); + +typedef struct _nmeaGENERATOR +{ + void *gen_data; + nmeaNMEA_GEN_INIT init_call; + nmeaNMEA_GEN_LOOP loop_call; + nmeaNMEA_GEN_RESET reset_call; + nmeaNMEA_GEN_DESTROY destroy_call; + struct _nmeaGENERATOR *next; + +} nmeaGENERATOR; + +int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info); +int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info); +int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info); +void nmea_gen_destroy(nmeaGENERATOR *gen); +void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GENERATOR_H__ */ diff --git a/custom/nmealib/inc/nmea/gmath.h b/custom/nmealib/inc/nmea/gmath.h new file mode 100644 index 0000000..063dc63 --- /dev/null +++ b/custom/nmealib/inc/nmea/gmath.h @@ -0,0 +1,92 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: gmath.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_GMATH_H__ +#define __NMEA_GMATH_H__ + +#include "info.h" + +#define NMEA_PI (3.141592653589793) /**< PI value */ +#define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ +#define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ +#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ +#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ +#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ +#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ +#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * degree VS radian + */ + +double nmea_degree2radian(double val); +double nmea_radian2degree(double val); + +/* + * NDEG (NMEA degree) + */ + +double nmea_ndeg2degree(double val); +double nmea_degree2ndeg(double val); + +double nmea_ndeg2radian(double val); +double nmea_radian2ndeg(double val); + +/* + * DOP + */ + +double nmea_calc_pdop(double hdop, double vdop); +double nmea_dop2meters(double dop); +double nmea_meters2dop(double meters); + +/* + * positions work + */ + +void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos); +void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info); + +double nmea_distance( + const nmeaPOS *from_pos, + const nmeaPOS *to_pos + ); + +double nmea_distance_ellipsoid( + const nmeaPOS *from_pos, + const nmeaPOS *to_pos, + double *from_azimuth, + double *to_azimuth + ); + +int nmea_move_horz( + const nmeaPOS *start_pos, + nmeaPOS *end_pos, + double azimuth, + double distance + ); + +int nmea_move_horz_ellipsoid( + const nmeaPOS *start_pos, + nmeaPOS *end_pos, + double azimuth, + double distance, + double *end_azimuth + ); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GMATH_H__ */ diff --git a/custom/nmealib/inc/nmea/info.h b/custom/nmealib/inc/nmea/info.h new file mode 100644 index 0000000..46ec1cd --- /dev/null +++ b/custom/nmealib/inc/nmea/info.h @@ -0,0 +1,112 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: info.h 10 2007-11-15 14:50:15Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_INFO_H__ +#define __NMEA_INFO_H__ + +#include "time.h" + +#define NMEA_SIG_BAD (0) +#define NMEA_SIG_LOW (1) +#define NMEA_SIG_MID (2) +#define NMEA_SIG_HIGH (3) + +#define NMEA_FIX_BAD (1) +#define NMEA_FIX_2D (2) +#define NMEA_FIX_3D (3) + +#define NMEA_MAXSAT (12) +#define NMEA_SATINPACK (4) +#define NMEA_NSATPACKS (NMEA_MAXSAT / NMEA_SATINPACK) + +#define NMEA_DEF_LAT (5001.2621) +#define NMEA_DEF_LON (3613.0595) + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Position data in fractional degrees or radians + */ +typedef struct _nmeaPOS +{ + double lat; /**< Latitude */ + double lon; /**< Longitude */ + +} nmeaPOS; + +/** + * Information about satellite + * @see nmeaSATINFO + * @see nmeaGPGSV + */ +typedef struct _nmeaSATELLITE +{ + int id; /**< Satellite PRN number */ + int in_use; /**< Used in position fix */ + int elv; /**< Elevation in degrees, 90 maximum */ + int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */ + int sig; /**< Signal, 00-99 dB */ + +} nmeaSATELLITE; + +/** + * Information about all satellites in view + * @see nmeaINFO + * @see nmeaGPGSV + */ +typedef struct _nmeaSATINFO +{ + int inuse; /**< Number of satellites in use (not those in view) */ + int inview; /**< Total number of satellites in view */ + nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */ + +} nmeaSATINFO; + +/** + * Summary GPS information from all parsed packets, + * used also for generating NMEA stream + * @see nmea_parse + * @see nmea_GPGGA2info, nmea_...2info + */ +typedef struct _nmeaINFO +{ + int smask; /**< Mask specifying types of packages from which data have been obtained */ + + nmeaTIME utc; /**< UTC of position */ + + int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ + int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ + + double PDOP; /**< Position Dilution Of Precision */ + double HDOP; /**< Horizontal Dilution Of Precision */ + double VDOP; /**< Vertical Dilution Of Precision */ + + double lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */ + double lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */ + double elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */ + double speed; /**< Speed over the ground in kilometers/hour */ + double direction; /**< Track angle in degrees True */ + double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ + + nmeaSATINFO satinfo; /**< Satellites information */ + +} nmeaINFO; + +void nmea_zero_INFO(nmeaINFO *info); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_INFO_H__ */ diff --git a/custom/nmealib/inc/nmea/nmea.h b/custom/nmealib/inc/nmea/nmea.h new file mode 100644 index 0000000..6269223 --- /dev/null +++ b/custom/nmealib/inc/nmea/nmea.h @@ -0,0 +1,25 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: nmea.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_H__ +#define __NMEA_H__ + +#include "./config.h" +#include "./units.h" +#include "./gmath.h" +#include "./info.h" +#include "./sentence.h" +#include "./generate.h" +#include "./generator.h" +#include "./parse.h" +#include "./parser.h" +#include "./context.h" + +#endif /* __NMEA_H__ */ diff --git a/custom/nmealib/inc/nmea/parse.h b/custom/nmealib/inc/nmea/parse.h new file mode 100644 index 0000000..23d5ec2 --- /dev/null +++ b/custom/nmealib/inc/nmea/parse.h @@ -0,0 +1,42 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parse.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_PARSE_H__ +#define __NMEA_PARSE_H__ + +#include "sentence.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/************************************************************************/ +/* modified by SYAKYOU, Fri Jun 4 10:03:09 CST 2021 */ +/************************************************************************/ +typedef int (*pfn_nmea_parse)(const char *buff, int buff_sz, void *pack, const char* phead); +typedef void (*pfn_nmea_info)(void *pack, nmeaINFO *info); + +typedef struct { + const char* phead; + int ptype; + int pack_size; + pfn_nmea_parse pfn_parse; + pfn_nmea_info pfn_info; +} nmea_lookup_t; + +nmea_lookup_t* nmea_lookuptab(const char *buff, int buff_sz); + +int nmea_find_tail(const char *buff, int buff_sz, int *res_crc); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_PARSE_H__ */ diff --git a/custom/nmealib/inc/nmea/parser.h b/custom/nmealib/inc/nmea/parser.h new file mode 100644 index 0000000..66e2d8e --- /dev/null +++ b/custom/nmealib/inc/nmea/parser.h @@ -0,0 +1,63 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parser.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_PARSER_H__ +#define __NMEA_PARSER_H__ + +#include "info.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * high level + */ + +typedef struct _nmeaPARSER +{ + void *top_node; + void *end_node; + unsigned char *buffer; + int buff_size; + int buff_use; + +} nmeaPARSER; + +int nmea_parser_init(nmeaPARSER *parser); +void nmea_parser_destroy(nmeaPARSER *parser); + +int nmea_parse( + nmeaPARSER *parser, + const char *buff, int buff_sz, + nmeaINFO *info + ); + +/* + * low level + */ + +int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz); +int nmea_parser_buff_clear(nmeaPARSER *parser); +int nmea_parser_queue_clear(nmeaPARSER *parser); + +/************************************************************************/ +/* modified by SYAKYOU, Fri Jun 4 10:03:09 CST 2021 */ +/************************************************************************/ +void* nmea_parser_top(nmeaPARSER *parser); +void* nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr); +void* nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr); +void* nmea_parser_drop(nmeaPARSER *parser); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_PARSER_H__ */ diff --git a/custom/nmealib/inc/nmea/sentence.h b/custom/nmealib/inc/nmea/sentence.h new file mode 100644 index 0000000..0f829cd --- /dev/null +++ b/custom/nmealib/inc/nmea/sentence.h @@ -0,0 +1,168 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: sentence.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_SENTENCE_H__ +#define __NMEA_SENTENCE_H__ + +#include "info.h" + +/** + * Thu Jun 3 09:26:01 CST 2021, add by SYAKYOU. + * The sentence is as follows + * GNGSA, GBGSV, GNVTG, GNRMC, GNGGA, GNGLL + */ + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * NMEA packets type which parsed and generated by library + */ +enum nmeaPACKTYPE +{ + GPNON = 0x0000, /**< Unknown packet type. */ + GPGGA = 0x0001, /**< GGA - Essential fix data which provide 3D location and accuracy data. */ + GPGSA = 0x0002, /**< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values. */ + GPGSV = 0x0004, /**< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values. */ + GPRMC = 0x0008, /**< RMC - Recommended Minimum Specific GPS/TRANSIT Data. */ + GPVTG = 0x0010, /**< VTG - Actual track made good and speed over ground. */ + GNGGA = 0x0020, + GNGSA = 0x0040, + GBGSV = 0x0080, + GNRMC = 0x0100, + GNVTG = 0x0200, + GNGLL = 0x0400, + GPGLL = 0x0800, + GPZDA = 0x1000, + GNZDA = 0x2000, +}; + +/** + * GGA packet information structure (Global Positioning System Fix Data) + */ +typedef struct _nmeaGGA +{ + nmeaTIME utc; /**< UTC of position (just time) */ + double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ + char ns; /**< [N]orth or [S]outh */ + double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ + char ew; /**< [E]ast or [W]est */ + int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ + int satinuse; /**< Number of satellites in use (not those in view) */ + double HDOP; /**< Horizontal dilution of precision */ + double elv; /**< Antenna altitude above/below mean sea level (geoid) */ + char elv_units; /**< [M]eters (Antenna height unit) */ + double diff; /**< Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. '-' = geoid is below WGS-84 ellipsoid) */ + char diff_units; /**< [M]eters (Units of geoidal separation) */ + double dgps_age; /**< Time in seconds since last DGPS update */ + int dgps_sid; /**< DGPS station ID number */ + +} nmeaGGA; + +/** + * GSA packet information structure (Satellite status) + */ +typedef struct _nmeaGSA +{ + char fix_mode; /**< Mode (M = Manual, forced to operate in 2D or 3D; A = Automatic, 3D/2D) */ + int fix_type; /**< Type, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ + int sat_prn[NMEA_MAXSAT]; /**< PRNs of satellites used in position fix (null for unused fields) */ + double PDOP; /**< Dilution of precision */ + double HDOP; /**< Horizontal dilution of precision */ + double VDOP; /**< Vertical dilution of precision */ + +} nmeaGSA; + +/** + * GSV packet information structure (Satellites in view) + */ +typedef struct _nmeaGSV +{ + int pack_count; /**< Total number of messages of this type in this cycle */ + int pack_index; /**< Message number */ + int sat_count; /**< Total number of satellites in view */ + nmeaSATELLITE sat_data[NMEA_SATINPACK]; + +} nmeaGSV; + +/** + * RMC packet information structure (Recommended Minimum sentence C) + */ +typedef struct _nmeaRMC +{ + nmeaTIME utc; /**< UTC of position */ + char status; /**< Status (A = active or V = void) */ + double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ + char ns; /**< [N]orth or [S]outh */ + double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ + char ew; /**< [E]ast or [W]est */ + double speed; /**< Speed over the ground in knots */ + double direction; /**< Track angle in degrees True */ + double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ + char declin_ew; /**< [E]ast or [W]est */ + char mode; /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */ + +} nmeaRMC; + +/** + * VTG packet information structure (Track made good and ground speed) + */ +typedef struct _nmeaVTG +{ + double dir; /**< True track made good (degrees) */ + char dir_t; /**< Fixed text 'T' indicates that track made good is relative to true north */ + double dec; /**< Magnetic track made good */ + char dec_m; /**< Fixed text 'M' */ + double spn; /**< Ground speed, knots */ + char spn_n; /**< Fixed text 'N' indicates that speed over ground is in knots */ + double spk; /**< Ground speed, kilometers per hour */ + char spk_k; /**< Fixed text 'K' indicates that speed over ground is in kilometers/hour */ + +} nmeaVTG; + +void nmea_zero_GGA(nmeaGGA *pack); +void nmea_zero_GSA(nmeaGSA *pack); +void nmea_zero_GSV(nmeaGSV *pack); +void nmea_zero_RMC(nmeaRMC *pack); +void nmea_zero_VTG(nmeaVTG *pack); + +/************************************************************************/ +/* GLL packet information structure (Geographic position, latitude, longitude) */ +/************************************************************************/ +typedef struct _nmeaGLL +{ + double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ + char ns; /**< [N]orth or [S]outh */ + double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ + char ew; /**< [E]ast or [W]est */ + nmeaTIME utc; /**< UTC of position */ + char status; /**< Status (A = active or V = void) */ + char mode; /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */ +} nmeaGLL; + +void nmea_zero_GLL(nmeaGLL *pack); + +typedef struct _nmeaZDA +{ + nmeaTIME utc; + int local_time_hours; + int local_time_min; +} nmeaZDA; + +void nmea_zero_ZDA(nmeaZDA *pack); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_SENTENCE_H__ */ diff --git a/custom/nmealib/inc/nmea/time.h b/custom/nmealib/inc/nmea/time.h new file mode 100644 index 0000000..bbe59f6 --- /dev/null +++ b/custom/nmealib/inc/nmea/time.h @@ -0,0 +1,47 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: time.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_TIME_H__ +#define __NMEA_TIME_H__ + +#include "config.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Date and time data + * @see nmea_time_now + */ +typedef struct _nmeaTIME +{ + int year; /**< Years since 1900 */ + int mon; /**< Months since January - [0,11] */ + int day; /**< Day of the month - [1,31] */ + int hour; /**< Hours since midnight - [0,23] */ + int min; /**< Minutes after the hour - [0,59] */ + int sec; /**< Seconds after the minute - [0,59] */ + int hsec; /**< Hundredth part of second - [0,99] */ + +} nmeaTIME; + +/** + * \brief Get time now to nmeaTIME structure + */ +void nmea_time_now(nmeaTIME *t); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_TIME_H__ */ diff --git a/custom/nmealib/inc/nmea/tok.h b/custom/nmealib/inc/nmea/tok.h new file mode 100644 index 0000000..21557e5 --- /dev/null +++ b/custom/nmealib/inc/nmea/tok.h @@ -0,0 +1,30 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: tok.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_TOK_H__ +#define __NMEA_TOK_H__ + +#include "config.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int nmea_calc_crc(const char *buff, int buff_sz); +int nmea_atoi(const char *str, int str_sz, int radix); +double nmea_atof(const char *str, int str_sz); +int nmea_printf(char *buff, int buff_sz, const char *format, ...); +int nmea_scanf(const char *buff, int buff_sz, const char *format, ...); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_TOK_H__ */ diff --git a/custom/nmealib/inc/nmea/units.h b/custom/nmealib/inc/nmea/units.h new file mode 100644 index 0000000..767f980 --- /dev/null +++ b/custom/nmealib/inc/nmea/units.h @@ -0,0 +1,30 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: units.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_UNITS_H__ +#define __NMEA_UNITS_H__ + +#include "config.h" + +/* + * Distance units + */ + +#define NMEA_TUD_YARDS (1.0936) /**< Yeards, meter * NMEA_TUD_YARDS = yard */ +#define NMEA_TUD_KNOTS (1.852) /**< Knots, kilometer / NMEA_TUD_KNOTS = knot */ +#define NMEA_TUD_MILES (1.609) /**< Miles, kilometer / NMEA_TUD_MILES = mile */ + +/* + * Speed units + */ + +#define NMEA_TUS_MS (3.6) /**< Meters per seconds, (k/h) / NMEA_TUS_MS= (m/s) */ + +#endif /* __NMEA_UNITS_H__ */ diff --git a/custom/nmealib/nmealib.mk b/custom/nmealib/nmealib.mk new file mode 100644 index 0000000..f220272 --- /dev/null +++ b/custom/nmealib/nmealib.mk @@ -0,0 +1,15 @@ + +CUSTOM_MAIN_DIR := custom/nmealib + +OC_FILES += $(CUSTOM_MAIN_DIR)/src/context.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/generate.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/generator.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/gmath.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/info.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/parse.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/parser.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/sentence.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/time.c +OC_FILES += $(CUSTOM_MAIN_DIR)/src/tok.c + +INC += -I'$(CUSTOM_MAIN_DIR)/inc/' \ No newline at end of file diff --git a/custom/nmealib/src/context.c b/custom/nmealib/src/context.c new file mode 100644 index 0000000..89f0431 --- /dev/null +++ b/custom/nmealib/src/context.c @@ -0,0 +1,67 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: context.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/context.h" + +#include +#include +#include + +nmeaPROPERTY * nmea_property() +{ + static nmeaPROPERTY prop = { + 0, 0, NMEA_DEF_PARSEBUFF + }; + + return ∝ +} + +void nmea_trace(const char *str, ...) +{ + int size; + va_list arg_list; + char buff[NMEA_DEF_PARSEBUFF]; + nmeaTraceFunc func = nmea_property()->trace_func; + + if(func) + { + va_start(arg_list, str); + size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); + va_end(arg_list); + + if(size > 0) + (*func)(&buff[0], size); + } +} + +void nmea_trace_buff(const char *buff, int buff_size) +{ + nmeaTraceFunc func = nmea_property()->trace_func; + if(func && buff_size) + (*func)(buff, buff_size); +} + +void nmea_error(const char *str, ...) +{ + int size; + va_list arg_list; + char buff[NMEA_DEF_PARSEBUFF]; + nmeaErrorFunc func = nmea_property()->error_func; + + if(func) + { + va_start(arg_list, str); + size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); + va_end(arg_list); + + if(size > 0) + (*func)(&buff[0], size); + } +} diff --git a/custom/nmealib/src/generate.c b/custom/nmealib/src/generate.c new file mode 100644 index 0000000..533f08f --- /dev/null +++ b/custom/nmealib/src/generate.c @@ -0,0 +1,229 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generate.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/tok.h" +#include "nmea/sentence.h" +#include "nmea/generate.h" +#include "nmea/units.h" + +#include +#include +#include + +int nmea_gen_GGA(char *buff, int buff_sz, nmeaGGA *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGGA,%02d%02d%02d.%02d,%07.4f,%C,%07.4f,%C,%1d,%02d,%03.1f,%03.1f,%C,%03.1f,%C,%03.1f,%04d", + pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec, + pack->lat, pack->ns, pack->lon, pack->ew, + pack->sig, pack->satinuse, pack->HDOP, pack->elv, pack->elv_units, + pack->diff, pack->diff_units, pack->dgps_age, pack->dgps_sid); +} + +int nmea_gen_GSA(char *buff, int buff_sz, nmeaGSA *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGSA,%C,%1d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%03.1f,%03.1f,%03.1f", + pack->fix_mode, pack->fix_type, + pack->sat_prn[0], pack->sat_prn[1], pack->sat_prn[2], pack->sat_prn[3], pack->sat_prn[4], pack->sat_prn[5], + pack->sat_prn[6], pack->sat_prn[7], pack->sat_prn[8], pack->sat_prn[9], pack->sat_prn[10], pack->sat_prn[11], + pack->PDOP, pack->HDOP, pack->VDOP); +} + +int nmea_gen_GSV(char *buff, int buff_sz, nmeaGSV *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGSV,%1d,%1d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d", + pack->pack_count, pack->pack_index + 1, pack->sat_count, + pack->sat_data[0].id, pack->sat_data[0].elv, pack->sat_data[0].azimuth, pack->sat_data[0].sig, + pack->sat_data[1].id, pack->sat_data[1].elv, pack->sat_data[1].azimuth, pack->sat_data[1].sig, + pack->sat_data[2].id, pack->sat_data[2].elv, pack->sat_data[2].azimuth, pack->sat_data[2].sig, + pack->sat_data[3].id, pack->sat_data[3].elv, pack->sat_data[3].azimuth, pack->sat_data[3].sig); +} + +int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaRMC *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPRMC,%02d%02d%02d.%02d,%C,%07.4f,%C,%07.4f,%C,%03.1f,%03.1f,%02d%02d%02d,%03.1f,%C,%C", + pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec, + pack->status, pack->lat, pack->ns, pack->lon, pack->ew, + pack->speed, pack->direction, + pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100, + pack->declination, pack->declin_ew, pack->mode); +} + +int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaVTG *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPVTG,%.1f,%C,%.1f,%C,%.1f,%C,%.1f,%C", + pack->dir, pack->dir_t, + pack->dec, pack->dec_m, + pack->spn, pack->spn_n, + pack->spk, pack->spk_k); +} + +void nmea_info2GGA(const nmeaINFO *info, nmeaGGA *pack) +{ + nmea_zero_GGA(pack); + + pack->utc = info->utc; + pack->lat = fabs(info->lat); + pack->ns = ((info->lat > 0)?'N':'S'); + pack->lon = fabs(info->lon); + pack->ew = ((info->lon > 0)?'E':'W'); + pack->sig = info->sig; + pack->satinuse = info->satinfo.inuse; + pack->HDOP = info->HDOP; + pack->elv = info->elv; +} + +void nmea_info2GSA(const nmeaINFO *info, nmeaGSA *pack) +{ + int it; + + nmea_zero_GSA(pack); + + pack->fix_type = info->fix; + pack->PDOP = info->PDOP; + pack->HDOP = info->HDOP; + pack->VDOP = info->VDOP; + + for(it = 0; it < NMEA_MAXSAT; ++it) + { + pack->sat_prn[it] = + ((info->satinfo.sat[it].in_use)?info->satinfo.sat[it].id:0); + } +} + +int nmea_gsv_npack(int sat_count) +{ + int pack_count = (int)ceil(((double)sat_count) / NMEA_SATINPACK); + + if(0 == pack_count) + pack_count = 1; + + return pack_count; +} + +void nmea_info2GSV(const nmeaINFO *info, nmeaGSV *pack, int pack_idx) +{ + int sit, pit; + + nmea_zero_GSV(pack); + + pack->sat_count = (info->satinfo.inview <= NMEA_MAXSAT)?info->satinfo.inview:NMEA_MAXSAT; + pack->pack_count = nmea_gsv_npack(pack->sat_count); + + if(pack->pack_count == 0) + pack->pack_count = 1; + + if(pack_idx >= pack->pack_count) + pack->pack_index = pack_idx % pack->pack_count; + else + pack->pack_index = pack_idx; + + for(pit = 0, sit = pack->pack_index * NMEA_SATINPACK; pit < NMEA_SATINPACK; ++pit, ++sit) + pack->sat_data[pit] = info->satinfo.sat[sit]; +} + +void nmea_info2GPRMC(const nmeaINFO *info, nmeaRMC *pack) +{ + nmea_zero_RMC(pack); + + pack->utc = info->utc; + pack->status = ((info->sig > 0)?'A':'V'); + pack->lat = fabs(info->lat); + pack->ns = ((info->lat > 0)?'N':'S'); + pack->lon = fabs(info->lon); + pack->ew = ((info->lon > 0)?'E':'W'); + pack->speed = info->speed / NMEA_TUD_KNOTS; + pack->direction = info->direction; + pack->declination = info->declination; + pack->declin_ew = 'E'; + pack->mode = ((info->sig > 0)?'A':'N'); +} + +void nmea_info2GPVTG(const nmeaINFO *info, nmeaVTG *pack) +{ + nmea_zero_VTG(pack); + + pack->dir = info->direction; + pack->dec = info->declination; + pack->spn = info->speed / NMEA_TUD_KNOTS; + pack->spk = info->speed; +} + +int nmea_generate( + char *buff, int buff_sz, + const nmeaINFO *info, + int generate_mask + ) +{ + int gen_count = 0, gsv_it, gsv_count; + int pack_mask = generate_mask; + + nmeaGGA gga; + nmeaGSA gsa; + nmeaGSV gsv; + nmeaRMC rmc; + nmeaVTG vtg; + + if(!buff) + return 0; + + while(pack_mask) + { + if(pack_mask & GPGGA) + { + nmea_info2GGA(info, &gga); + gen_count += nmea_gen_GGA(buff + gen_count, buff_sz - gen_count, &gga); + pack_mask &= ~GPGGA; + } + else if(pack_mask & GPGSA) + { + nmea_info2GSA(info, &gsa); + gen_count += nmea_gen_GSA(buff + gen_count, buff_sz - gen_count, &gsa); + pack_mask &= ~GPGSA; + } + else if(pack_mask & GPGSV) + { + gsv_count = nmea_gsv_npack(info->satinfo.inview); + for(gsv_it = 0; gsv_it < gsv_count && buff_sz - gen_count > 0; ++gsv_it) + { + nmea_info2GSV(info, &gsv, gsv_it); + gen_count += nmea_gen_GSV(buff + gen_count, buff_sz - gen_count, &gsv); + } + pack_mask &= ~GPGSV; + } + else if(pack_mask & GPRMC) + { + nmea_info2RMC(info, &rmc); + gen_count += nmea_gen_RMC(buff + gen_count, buff_sz - gen_count, &rmc); + pack_mask &= ~GPRMC; + } + else if(pack_mask & GPVTG) + { + nmea_info2VTG(info, &vtg); + gen_count += nmea_gen_VTG(buff + gen_count, buff_sz - gen_count, &vtg); + pack_mask &= ~GPVTG; + } + else + break; + + if(buff_sz - gen_count <= 0) + break; + } + + return gen_count; +} diff --git a/custom/nmealib/src/generator.c b/custom/nmealib/src/generator.c new file mode 100644 index 0000000..7134daf --- /dev/null +++ b/custom/nmealib/src/generator.c @@ -0,0 +1,399 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generator.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/gmath.h" +#include "nmea/generate.h" +#include "nmea/generator.h" +#include "nmea/context.h" + +#include +#include + +#if defined(NMEA_WIN) && defined(_MSC_VER) +# pragma warning(disable: 4100) /* unreferenced formal parameter */ +#endif + +double nmea_random(double min, double max) +{ + static double rand_max = RAND_MAX; + double rand_val = rand(); + double bounds = max - min; + return min + (rand_val * bounds) / rand_max; +} + +/* + * low level + */ + +int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; int smask = info->smask; + nmeaGENERATOR *igen = gen; + + nmea_zero_INFO(info); + info->smask = smask; + + info->lat = NMEA_DEF_LAT; + info->lon = NMEA_DEF_LON; + + while(RetVal && igen) + { + if(igen->init_call) + RetVal = (*igen->init_call)(igen, info); + igen = igen->next; + } + + return RetVal; +} + +int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; + + if(gen->loop_call) + RetVal = (*gen->loop_call)(gen, info); + + if(RetVal && gen->next) + RetVal = nmea_gen_loop(gen->next, info); + + return RetVal; +} + +int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; + + if(gen->reset_call) + RetVal = (*gen->reset_call)(gen, info); + + return RetVal; +} + +void nmea_gen_destroy(nmeaGENERATOR *gen) +{ + if(gen->next) + { + nmea_gen_destroy(gen->next); + gen->next = 0; + } + + if(gen->destroy_call) + (*gen->destroy_call)(gen); + + free(gen); +} + +void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen) +{ + if(to->next) + nmea_gen_add(to->next, gen); + else + to->next = gen; +} + +int nmea_generate_from( + char *buff, int buff_sz, + nmeaINFO *info, + nmeaGENERATOR *gen, + int generate_mask + ) +{ + int retval; + + if(0 != (retval = nmea_gen_loop(gen, info))) + retval = nmea_generate(buff, buff_sz, info, generate_mask); + + return retval; +} + +/* + * NOISE generator + */ + +int nmea_igen_noise_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + return 1; +} + +int nmea_igen_noise_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + int in_use; + + info->sig = (int)nmea_random(1, 3); + info->PDOP = nmea_random(0, 9); + info->HDOP = nmea_random(0, 9); + info->VDOP = nmea_random(0, 9); + info->fix = (int)nmea_random(2, 3); + info->lat = nmea_random(0, 100); + info->lon = nmea_random(0, 100); + info->speed = nmea_random(0, 100); + info->direction = nmea_random(0, 360); + info->declination = nmea_random(0, 360); + info->elv = (int)nmea_random(-100, 100); + + info->satinfo.inuse = 0; + info->satinfo.inview = 0; + + for(it = 0; it < 12; ++it) + { + info->satinfo.sat[it].id = it; + info->satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3); + info->satinfo.sat[it].elv = (int)nmea_random(0, 90); + info->satinfo.sat[it].azimuth = (int)nmea_random(0, 359); + info->satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40)); + + if(in_use) + info->satinfo.inuse++; + if(info->satinfo.sat[it].sig > 0) + info->satinfo.inview++; + } + + return 1; +} + +int nmea_igen_noise_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + return 1; +} + +/* + * STATIC generator + */ + +int nmea_igen_static_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + nmea_time_now(&info->utc); + return 1; +}; + +int nmea_igen_static_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->satinfo.inuse = 4; + info->satinfo.inview = 4; + + info->satinfo.sat[0].id = 1; + info->satinfo.sat[0].in_use = 1; + info->satinfo.sat[0].elv = 50; + info->satinfo.sat[0].azimuth = 0; + info->satinfo.sat[0].sig = 99; + + info->satinfo.sat[1].id = 2; + info->satinfo.sat[1].in_use = 1; + info->satinfo.sat[1].elv = 50; + info->satinfo.sat[1].azimuth = 90; + info->satinfo.sat[1].sig = 99; + + info->satinfo.sat[2].id = 3; + info->satinfo.sat[2].in_use = 1; + info->satinfo.sat[2].elv = 50; + info->satinfo.sat[2].azimuth = 180; + info->satinfo.sat[2].sig = 99; + + info->satinfo.sat[3].id = 4; + info->satinfo.sat[3].in_use = 1; + info->satinfo.sat[3].elv = 50; + info->satinfo.sat[3].azimuth = 270; + info->satinfo.sat[3].sig = 99; + + return 1; +} + +int nmea_igen_static_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + + nmea_igen_static_reset(gen, info); + + return 1; +} + +/* + * SAT_ROTATE generator + */ + +int nmea_igen_rotate_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + int count = info->satinfo.inview; + double deg = 360 / (count?count:1); + double srt = (count?(info->satinfo.sat[0].azimuth):0) + 5; + + nmea_time_now(&info->utc); + + for(it = 0; it < count; ++it) + { + info->satinfo.sat[it].azimuth = + (int)((srt >= 360)?srt - 360:srt); + srt += deg; + } + + return 1; +}; + +int nmea_igen_rotate_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + double deg = 360 / 8; + double srt = 0; + + info->satinfo.inuse = 8; + info->satinfo.inview = 8; + + for(it = 0; it < info->satinfo.inview; ++it) + { + info->satinfo.sat[it].id = it + 1; + info->satinfo.sat[it].in_use = 1; + info->satinfo.sat[it].elv = 5; + info->satinfo.sat[it].azimuth = (int)srt; + info->satinfo.sat[it].sig = 80; + srt += deg; + } + + return 1; +} + +int nmea_igen_rotate_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + + nmea_igen_rotate_reset(gen, info); + + return 1; +} + +/* + * POS_RANDMOVE generator + */ + +int nmea_igen_pos_rmove_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + info->direction = info->declination = 0; + info->speed = 20; + return 1; +} + +int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + nmeaPOS crd; + + info->direction += nmea_random(-10, 10); + info->speed += nmea_random(-2, 3); + + if(info->direction < 0) + info->direction = 359 + info->direction; + if(info->direction > 359) + info->direction -= 359; + + if(info->speed > 40) + info->speed = 40; + if(info->speed < 1) + info->speed = 1; + + nmea_info2pos(info, &crd); + nmea_move_horz(&crd, &crd, info->direction, info->speed / 3600); + nmea_pos2info(&crd, info); + + info->declination = info->direction; + + return 1; +}; + +int nmea_igen_pos_rmove_destroy(nmeaGENERATOR *gen) +{ + return 1; +}; + +/* + * generator create + */ + +nmeaGENERATOR * __nmea_create_generator(int type, nmeaINFO *info) +{ + nmeaGENERATOR *gen = 0; + + switch(type) + { + case NMEA_GEN_NOISE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_noise_init; + gen->loop_call = &nmea_igen_noise_loop; + gen->reset_call = &nmea_igen_noise_reset; + } + break; + case NMEA_GEN_STATIC: + case NMEA_GEN_SAT_STATIC: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_static_init; + gen->loop_call = &nmea_igen_static_loop; + gen->reset_call = &nmea_igen_static_reset; + } + break; + case NMEA_GEN_SAT_ROTATE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_rotate_init; + gen->loop_call = &nmea_igen_rotate_loop; + gen->reset_call = &nmea_igen_rotate_reset; + } + break; + case NMEA_GEN_POS_RANDMOVE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_pos_rmove_init; + gen->loop_call = &nmea_igen_pos_rmove_loop; + gen->destroy_call = &nmea_igen_pos_rmove_destroy; + } + break; + case NMEA_GEN_ROTATE: + gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info); + nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info)); + break; + }; + + return gen; +} + +nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info) +{ + nmeaGENERATOR *gen = __nmea_create_generator(type, info); + + if(gen) + nmea_gen_init(gen, info); + + return gen; +} + +void nmea_destroy_generator(nmeaGENERATOR *gen) +{ + nmea_gen_destroy(gen); +} + +#if defined(NMEA_WIN) && defined(_MSC_VER) +# pragma warning(default: 4100) +#endif diff --git a/custom/nmealib/src/gmath.c b/custom/nmealib/src/gmath.c new file mode 100644 index 0000000..766daef --- /dev/null +++ b/custom/nmealib/src/gmath.c @@ -0,0 +1,377 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: gmath.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file gmath.h */ + +#include "nmea/gmath.h" + +#include +#include + +/** + * \fn nmea_degree2radian + * \brief Convert degree to radian + */ +double nmea_degree2radian(double val) +{ return (val * NMEA_PI180); } + +/** + * \fn nmea_radian2degree + * \brief Convert radian to degree + */ +double nmea_radian2degree(double val) +{ return (val / NMEA_PI180); } + +/** + * \brief Convert NDEG (NMEA degree) to fractional degree + */ +double nmea_ndeg2degree(double val) +{ + double deg = ((int)(val / 100)); + val = deg + (val - deg * 100) / 60; + return val; +} + +/** + * \brief Convert fractional degree to NDEG (NMEA degree) + */ +double nmea_degree2ndeg(double val) +{ + double int_part; + double fra_part; + fra_part = modf(val, &int_part); + val = int_part * 100 + fra_part * 60; + return val; +} + +/** + * \fn nmea_ndeg2radian + * \brief Convert NDEG (NMEA degree) to radian + */ +double nmea_ndeg2radian(double val) +{ return nmea_degree2radian(nmea_ndeg2degree(val)); } + +/** + * \fn nmea_radian2ndeg + * \brief Convert radian to NDEG (NMEA degree) + */ +double nmea_radian2ndeg(double val) +{ return nmea_degree2ndeg(nmea_radian2degree(val)); } + +/** + * \brief Calculate PDOP (Position Dilution Of Precision) factor + */ +double nmea_calc_pdop(double hdop, double vdop) +{ + return sqrt(pow(hdop, 2) + pow(vdop, 2)); +} + +double nmea_dop2meters(double dop) +{ return (dop * NMEA_DOP_FACTOR); } + +double nmea_meters2dop(double meters) +{ return (meters / NMEA_DOP_FACTOR); } + +/** + * \brief Calculate distance between two points + * \return Distance in meters + */ +double nmea_distance( + const nmeaPOS *from_pos, /**< From position in radians */ + const nmeaPOS *to_pos /**< To position in radians */ + ) +{ + double dist = ((double)NMEA_EARTHRADIUS_M) * acos( + sin(to_pos->lat) * sin(from_pos->lat) + + cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon) + ); + return dist; +} + +/** + * \brief Calculate distance between two points + * This function uses an algorithm for an oblate spheroid earth model. + * The algorithm is described here: + * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf + * \return Distance in meters + */ +double nmea_distance_ellipsoid( + const nmeaPOS *from_pos, /**< From position in radians */ + const nmeaPOS *to_pos, /**< To position in radians */ + double *from_azimuth, /**< (O) azimuth at "from" position in radians */ + double *to_azimuth /**< (O) azimuth at "to" position in radians */ + ) +{ + /* All variables */ + double f, a, b, sqr_a, sqr_b; + double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; + double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; + int remaining_steps; + double sqr_u, A, B, delta_sigma; + + /* Check input */ + NMEA_ASSERT(from_pos != 0); + NMEA_ASSERT(to_pos != 0); + + if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon)) + { /* Identical points */ + if ( from_azimuth != 0 ) + *from_azimuth = 0; + if ( to_azimuth != 0 ) + *to_azimuth = 0; + return 0; + } /* Identical points */ + + /* Earth geometry */ + f = NMEA_EARTH_FLATTENING; + a = NMEA_EARTH_SEMIMAJORAXIS_M; + b = (1 - f) * a; + sqr_a = a * a; + sqr_b = b * b; + + /* Calculation */ + L = to_pos->lon - from_pos->lon; + phi1 = from_pos->lat; + phi2 = to_pos->lat; + U1 = atan((1 - f) * tan(phi1)); + U2 = atan((1 - f) * tan(phi2)); + sin_U1 = sin(U1); + sin_U2 = sin(U2); + cos_U1 = cos(U1); + cos_U2 = cos(U2); + + /* Initialize iteration */ + sigma = 0; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + cos_2_sigmam = 0; + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sqr_cos_alpha = 0; + lambda = L; + sin_lambda = sin(lambda); + cos_lambda = cos(lambda); + delta_lambda = lambda; + remaining_steps = 20; + + while ((delta_lambda > 1e-12) && (remaining_steps > 0)) + { /* Iterate */ + /* Variables */ + double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; + + /* Calculation */ + tmp1 = cos_U2 * sin_lambda; + tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; + sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); + cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; + tan_sigma = sin_sigma / cos_sigma; + sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; + cos_alpha = cos(asin(sin_alpha)); + sqr_cos_alpha = cos_alpha * cos_alpha; + cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); + lambda_prev = lambda; + sigma = asin(sin_sigma); + lambda = L + + (1 - C) * f * sin_alpha + * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); + delta_lambda = lambda_prev - lambda; + if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; + sin_lambda = sin(lambda); + cos_lambda = cos(lambda); + remaining_steps--; + } /* Iterate */ + + /* More calculation */ + sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; + A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); + B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); + delta_sigma = B * sin_sigma * ( + cos_2_sigmam + B / 4 * ( + cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - + B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) + )); + + /* Calculate result */ + if ( from_azimuth != 0 ) + { + double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); + *from_azimuth = atan(tan_alpha_1); + } + if ( to_azimuth != 0 ) + { + double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); + *to_azimuth = atan(tan_alpha_2); + } + + return b * A * (sigma - delta_sigma); +} + +/** + * \brief Horizontal move of point position + */ +int nmea_move_horz( + const nmeaPOS *start_pos, /**< Start position in radians */ + nmeaPOS *end_pos, /**< Result position in radians */ + double azimuth, /**< Azimuth (degree) [0, 359] */ + double distance /**< Distance (km) */ + ) +{ + nmeaPOS p1 = *start_pos; + int RetVal = 1; + + distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ + azimuth = nmea_degree2radian(azimuth); + + end_pos->lat = asin( + sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); + end_pos->lon = p1.lon + atan2( + sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat)); + + if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) + { + end_pos->lat = 0; end_pos->lon = 0; + RetVal = 0; + } + + return RetVal; +} + +/** + * \brief Horizontal move of point position + * This function uses an algorithm for an oblate spheroid earth model. + * The algorithm is described here: + * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf + */ +int nmea_move_horz_ellipsoid( + const nmeaPOS *start_pos, /**< Start position in radians */ + nmeaPOS *end_pos, /**< (O) Result position in radians */ + double azimuth, /**< Azimuth in radians */ + double distance, /**< Distance (km) */ + double *end_azimuth /**< (O) Azimuth at end position in radians */ + ) +{ + /* Variables */ + double f, a, b, sqr_a, sqr_b; + double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1; + double tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B; + double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma; + int remaining_steps; + double tmp1, phi2, lambda, C, L; + + /* Check input */ + NMEA_ASSERT(start_pos != 0); + NMEA_ASSERT(end_pos != 0); + + if (fabs(distance) < 1e-12) + { /* No move */ + *end_pos = *start_pos; + if ( end_azimuth != 0 ) *end_azimuth = azimuth; + return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); + } /* No move */ + + /* Earth geometry */ + f = NMEA_EARTH_FLATTENING; + a = NMEA_EARTH_SEMIMAJORAXIS_M; + b = (1 - f) * a; + sqr_a = a * a; + sqr_b = b * b; + + /* Calculation */ + phi1 = start_pos->lat; + tan_U1 = (1 - f) * tan(phi1); + cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1); + sin_U1 = tan_U1 * cos_U1; + s = distance; + alpha1 = azimuth; + sin_alpha1 = sin(alpha1); + cos_alpha1 = cos(alpha1); + tan_sigma1 = tan_U1 / cos_alpha1; + sigma1 = atan2(tan_U1, cos_alpha1); + sin_alpha = cos_U1 * sin_alpha1; + sqr_cos_alpha = 1 - sin_alpha * sin_alpha; + cos_alpha = sqrt(sqr_cos_alpha); + sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; + A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); + B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); + + /* Initialize iteration */ + sigma_initial = s / (b * A); + sigma = sigma_initial; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + cos_2_sigmam = cos(2 * sigma1 + sigma); + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + delta_sigma = 0; + sigma_prev = 2 * NMEA_PI; + remaining_steps = 20; + + while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0)) + { /* Iterate */ + cos_2_sigmam = cos(2 * sigma1 + sigma); + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + delta_sigma = B * sin_sigma * ( + cos_2_sigmam + B / 4 * ( + cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - + B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) + )); + sigma_prev = sigma; + sigma = sigma_initial + delta_sigma; + remaining_steps --; + } /* Iterate */ + + /* Calculate result */ + tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1); + phi2 = atan2( + sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1, + (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1) + ); + lambda = atan2( + sin_sigma * sin_alpha1, + cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1 + ); + C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); + L = lambda - + (1 - C) * f * sin_alpha * ( + sigma + C * sin_sigma * + (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)) + ); + + /* Result */ + end_pos->lon = start_pos->lon + L; + end_pos->lat = phi2; + if ( end_azimuth != 0 ) + { + *end_azimuth = atan2( + sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1 + ); + } + return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); +} + +/** + * \brief Convert position from INFO to radians position + */ +void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos) +{ + pos->lat = nmea_ndeg2radian(info->lat); + pos->lon = nmea_ndeg2radian(info->lon); +} + +/** + * \brief Convert radians position to INFOs position + */ +void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info) +{ + info->lat = nmea_radian2ndeg(pos->lat); + info->lon = nmea_radian2ndeg(pos->lon); +} diff --git a/custom/nmealib/src/info.c b/custom/nmealib/src/info.c new file mode 100644 index 0000000..1d531ff --- /dev/null +++ b/custom/nmealib/src/info.c @@ -0,0 +1,21 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: info.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include + +#include "nmea/info.h" + +void nmea_zero_INFO(nmeaINFO *info) +{ + memset(info, 0, sizeof(nmeaINFO)); + nmea_time_now(&info->utc); + info->sig = NMEA_SIG_BAD; + info->fix = NMEA_FIX_BAD; +} diff --git a/custom/nmealib/src/nmea.vcproj b/custom/nmealib/src/nmea.vcproj new file mode 100644 index 0000000..0815f1d --- /dev/null +++ b/custom/nmealib/src/nmea.vcproj @@ -0,0 +1,414 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/custom/nmealib/src/parse.c b/custom/nmealib/src/parse.c new file mode 100644 index 0000000..5da6df0 --- /dev/null +++ b/custom/nmealib/src/parse.c @@ -0,0 +1,601 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/** + * \file parse.h + * \brief Functions of a low level for analysis of + * packages of NMEA stream. + */ + +#include "nmea/tok.h" +#include "nmea/parse.h" +#include "nmea/context.h" +#include "nmea/gmath.h" +#include "nmea/units.h" + +#include +#include + +// forwarding declare. +int nmea_parse_GGA(const char *buff, int buff_sz, void *pack, const char* phead); +int nmea_parse_GSA(const char *buff, int buff_sz, void *pack, const char* phead); +int nmea_parse_GSV(const char *buff, int buff_sz, void *pack, const char* phead); +int nmea_parse_RMC(const char *buff, int buff_sz, void *pack, const char* phead); +int nmea_parse_VTG(const char *buff, int buff_sz, void *pack, const char* phead); +int nmea_parse_GLL(const char *buff, int buff_sz, void *pack, const char* phead); +int nmea_parse_ZDA(const char *buff, int buff_sz, void *pack, const char* phead); + +void nmea_GGA2info(void *pack, nmeaINFO *info); +void nmea_GSA2info(void *pack, nmeaINFO *info); +void nmea_GSV2info(void *pack, nmeaINFO *info); +void nmea_RMC2info(void *pack, nmeaINFO *info); +void nmea_VTG2info(void *pack, nmeaINFO *info); +void nmea_GLL2info(void *pack, nmeaINFO *info); +void nmea_ZDA2info(void *pack, nmeaINFO *info); + + +int _nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res) +{ + int success = 0; + + switch(buff_sz) + { + case sizeof("hhmmss") - 1: + success = (3 == nmea_scanf(buff, buff_sz, + "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec) + )); + break; + case sizeof("hhmmss.s") - 1: + case sizeof("hhmmss.ss") - 1: + case sizeof("hhmmss.sss") - 1: + success = (4 == nmea_scanf(buff, buff_sz, + "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec) + )); + break; + default: + nmea_error("Parse of time error (format error)!"); + success = 0; + break; + } + + return (success?0:-1); +} + +/** + * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC). + * @param buff a constant character pointer of packets buffer. + * @param buff_sz buffer size. + * @param res_crc a integer pointer for return CRC of packet (must be defined). + * @return Number of bytes to packet tail. + */ +int nmea_find_tail(const char *buff, int buff_sz, int *res_crc) +{ + static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */; + + const char *end_buff = buff + buff_sz; + int nread = 0; + int crc = 0; + + NMEA_ASSERT(buff && res_crc); + + *res_crc = -1; + + for(;buff < end_buff; ++buff, ++nread) + { + if(('$' == *buff) && nread) + { + buff = 0; + break; + } + else if('*' == *buff) + { + if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4]) + { + *res_crc = nmea_atoi(buff + 1, 2, 16); + nread = buff_sz - (int)(end_buff - (buff + tail_sz)); + if(*res_crc != crc) + { + *res_crc = -1; + buff = 0; + } + } + + break; + } + else if(nread) + crc ^= (int)*buff; + } + + if(*res_crc < 0 && buff) + nread = 0; + + return nread; +} + +nmea_lookup_t* nmea_lookuptab(const char *buff, int buff_sz) +{ + static nmea_lookup_t tab[] = { + { "GPGGA", GPGGA, sizeof(nmeaGGA), nmea_parse_GGA, nmea_GGA2info }, + { "GPGSA", GPGSA, sizeof(nmeaGSA), nmea_parse_GSA, nmea_GSA2info }, + { "GPGSV", GPGSV, sizeof(nmeaGSV), nmea_parse_GSV, nmea_GSV2info }, + { "GPRMC", GPRMC, sizeof(nmeaRMC), nmea_parse_RMC, nmea_RMC2info }, + { "GPVTG", GPVTG, sizeof(nmeaVTG), nmea_parse_VTG, nmea_VTG2info }, + { "GPGLL", GPGLL, sizeof(nmeaGLL), nmea_parse_GLL, nmea_GLL2info }, + { "GPZDA", GPZDA, sizeof(nmeaZDA), nmea_parse_ZDA, nmea_ZDA2info }, + { "GNGGA", GNGGA, sizeof(nmeaGGA), nmea_parse_GGA, nmea_GGA2info }, + { "GNGSA", GNGSA, sizeof(nmeaGSA), nmea_parse_GSA, nmea_GSA2info }, + { "GBGSV", GBGSV, sizeof(nmeaGSV), nmea_parse_GSV, nmea_GSV2info }, + { "GNRMC", GNRMC, sizeof(nmeaRMC), nmea_parse_RMC, nmea_RMC2info }, + { "GNVTG", GNVTG, sizeof(nmeaVTG), nmea_parse_VTG, nmea_VTG2info }, + { "GNGLL", GNGLL, sizeof(nmeaGLL), nmea_parse_GLL, nmea_GLL2info }, + { "GNZDA", GNZDA, sizeof(nmeaZDA), nmea_parse_ZDA, nmea_ZDA2info }, + { NULL, GPNON, 0, NULL, NULL } + }; + + nmea_lookup_t* p; + + NMEA_ASSERT(buff); + + if(buff_sz < 5) + return NULL; + + for (p = tab; p->ptype; ++p) { + if (0 == memcmp(buff, p->phead, strlen(p->phead))) + return p; + } + return NULL; +} + +/** +* \brief Parse GGA packet from buffer. +* @param buff a constant character pointer of packet buffer. +* @param buff_sz buffer size. +* @param pack a pointer of packet which will filled by function. +* @return 1 (true) - if parsed successfully or 0 (false) - if fail. +*/ +int nmea_parse_GGA(const char *buff, int buff_sz, void *mem, const char* phead) +{ + char time_buff[NMEA_TIMEPARSE_BUF]; + char talker_format[] = "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*"; + nmeaGGA *pack = (nmeaGGA*)mem; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGGA)); + + nmea_trace_buff(buff, buff_sz); + + memcpy(&talker_format[1], phead, strlen(phead)); + + if(14 != nmea_scanf(buff, buff_sz, + talker_format, + &(time_buff[0]), + &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), + &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units), + &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid))) + { + nmea_error("%s parse error!", phead); + return 0; + } + + if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) + { + nmea_error("%s time parse error!", phead); + return 0; + } + + return 1; +} + + +/** +* \brief Parse GSA packet from buffer. +* @param buff a constant character pointer of packet buffer. +* @param buff_sz buffer size. +* @param pack a pointer of packet which will filled by function. +* @return 1 (true) - if parsed successfully or 0 (false) - if fail. +*/ +int nmea_parse_GSA(const char *buff, int buff_sz, void *mem, const char* phead) +{ + char talker_format[] = "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*"; + nmeaGSA* pack = (nmeaGSA*)mem; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGSA)); + + nmea_trace_buff(buff, buff_sz); + + memcpy(&talker_format[1], phead, strlen(phead)); + + if(17 != nmea_scanf(buff, buff_sz, + talker_format, + &(pack->fix_mode), &(pack->fix_type), + &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]), + &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]), + &(pack->PDOP), &(pack->HDOP), &(pack->VDOP))) + { + nmea_error("%s parse error!", phead); + return 0; + } + + return 1; +} + + +/** +* \brief Parse GSV packet from buffer. +* @param buff a constant character pointer of packet buffer. +* @param buff_sz buffer size. +* @param pack a pointer of packet which will filled by function. +* @return 1 (true) - if parsed successfully or 0 (false) - if fail. +*/ +int nmea_parse_GSV(const char *buff, int buff_sz, void *mem, const char* phead) +{ + int nsen, nsat; + char talker_format[] = "$GPGSV,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d*"; + nmeaGSV *pack = (nmeaGSV*)mem; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGSV)); + + nmea_trace_buff(buff, buff_sz); + + memcpy(&talker_format[1], phead, strlen(phead)); + nsen = nmea_scanf(buff, buff_sz, + talker_format, + &(pack->pack_count), &(pack->pack_index), &(pack->sat_count), + &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig), + &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig), + &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig), + &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig)); + + nsat = (pack->pack_index - 1) * NMEA_SATINPACK; + nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; + nsat = nsat * 4 + 3 /* first three sentence`s */; + + if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3)) + { + nmea_error("%s parse error!", phead); + return 0; + } + + return 1; +} + +/** +* \brief Parse RMC packet from buffer. +* @param buff a constant character pointer of packet buffer. +* @param buff_sz buffer size. +* @param pack a pointer of packet which will filled by function. +* @return 1 (true) - if parsed successfully or 0 (false) - if fail. +*/ +int nmea_parse_RMC(const char *buff, int buff_sz, void *mem, const char* phead) +{ + int nsen; + char time_buff[NMEA_TIMEPARSE_BUF]; + char talker_format[] = "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*"; + nmeaRMC* pack = (nmeaRMC*)mem; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaRMC)); + + nmea_trace_buff(buff, buff_sz); + + memcpy(&talker_format[1], phead, strlen(phead)); + nsen = nmea_scanf(buff, buff_sz, + talker_format, + &(time_buff[0]), + &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), + &(pack->speed), &(pack->direction), + &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year), + &(pack->declination), &(pack->declin_ew), &(pack->mode)); + + if(nsen != 13 && nsen != 14) + { + nmea_error("%s parse error!", phead); + return 0; + } + + if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) + { + nmea_error("%s time parse error!", phead); + return 0; + } + + pack->utc.year += 2000; + + return 1; +} + +/** +* \brief Parse VTG packet from buffer. +* @param buff a constant character pointer of packet buffer. +* @param buff_sz buffer size. +* @param pack a pointer of packet which will filled by function. +* @return 1 (true) - if parsed successfully or 0 (false) - if fail. +*/ +int nmea_parse_VTG(const char *buff, int buff_sz, void *mem, const char* phead) +{ + char talker_format[] = "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*"; + nmeaVTG* pack = (nmeaVTG*)mem; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaVTG)); + + nmea_trace_buff(buff, buff_sz); + + memcpy(&talker_format[1], phead, strlen(phead)); + if(8 != nmea_scanf(buff, buff_sz, + talker_format, + &(pack->dir), &(pack->dir_t), + &(pack->dec), &(pack->dec_m), + &(pack->spn), &(pack->spn_n), + &(pack->spk), &(pack->spk_k))) + { + nmea_error("%s parse error!", phead); + return 0; + } + + if( pack->dir_t != 'T' || + pack->dec_m != 'M' || + pack->spn_n != 'N' || + pack->spk_k != 'K') + { + nmea_error("%s parse error (format error)!", phead); + return 0; + } + + return 1; +} +int nmea_parse_GLL(const char *buff, int buff_sz, void *mem, const char* phead) +{ + int nsen; + char time_buff[NMEA_TIMEPARSE_BUF]; + char talker_format[] = "$GPGLL,%f,%C,%f,%C,%s,%C,%C*"; + nmeaGLL* pack = (nmeaGLL*)mem; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGLL)); + + nmea_trace_buff(buff, buff_sz); + + memcpy(&talker_format[1], phead, strlen(phead)); + nsen = nmea_scanf(buff, buff_sz, + talker_format, + &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), + &(time_buff[0]), &(pack->status), &(pack->mode)); + + if(nsen != 7) + { + nmea_error("%s parse error!", phead); + return 0; + } + + if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) + { + nmea_error("%s time parse error!", phead); + return 0; + } + + return 1; +} +int nmea_parse_ZDA(const char *buff, int buff_sz, void *mem, const char* phead) +{ + int nsen; + char time_buff[NMEA_TIMEPARSE_BUF]; + char talker_format[] = "$GPZDA,%s,%d,%d,%d,%d,%d*"; + nmeaZDA* pack = (nmeaZDA*)mem; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaZDA)); + + nmea_trace_buff(buff, buff_sz); + + memcpy(&talker_format[1], phead, strlen(phead)); + nsen = nmea_scanf(buff, buff_sz, + talker_format, + &(time_buff[0]), &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year), + &(pack->local_time_min), &(pack->local_time_hours)); + + if(nsen != 6) + { + nmea_error("%s parse error!", phead); + return 0; + } + + if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) + { + nmea_error("%s time parse error!", phead); + return 0; + } + + return 1; +} + + +/** +* \brief Fill nmeaINFO structure by GGA packet data. +* @param pack a pointer of packet structure. +* @param info a pointer of summary information structure. +*/ +void nmea_GGA2info(void *mem, nmeaINFO *info) +{ + nmeaGGA* pack = (nmeaGGA*)mem; + NMEA_ASSERT(pack && info); + + info->utc.hour = pack->utc.hour; + info->utc.min = pack->utc.min; + info->utc.sec = pack->utc.sec; + info->utc.hsec = pack->utc.hsec; + info->sig = pack->sig; + info->HDOP = pack->HDOP; + info->elv = pack->elv; + info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); + info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); +} + +/** +* \brief Fill nmeaINFO structure by GSA packet data. +* @param pack a pointer of packet structure. +* @param info a pointer of summary information structure. +*/ +void nmea_GSA2info(void *mem, nmeaINFO *info) +{ + int i, j, nuse = 0; + nmeaGSA* pack = (nmeaGSA*)mem; + + NMEA_ASSERT(pack && info); + + info->fix = pack->fix_type; + info->PDOP = pack->PDOP; + info->HDOP = pack->HDOP; + info->VDOP = pack->VDOP; + + for(i = 0; i < NMEA_MAXSAT; ++i) + { + for(j = 0; j < info->satinfo.inview; ++j) + { + if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id) + { + info->satinfo.sat[j].in_use = 1; + nuse++; + } + } + } + + info->satinfo.inuse = nuse; +} + +/** +* \brief Fill nmeaINFO structure by GSV packet data. +* @param pack a pointer of packet structure. +* @param info a pointer of summary information structure. +*/ +void nmea_GSV2info(void *mem, nmeaINFO *info) +{ + int isat, isi, nsat; + nmeaGSV *pack = (nmeaGSV*)mem; + + NMEA_ASSERT(pack && info); + + if(pack->pack_index > pack->pack_count || + pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT) + return; + + if(pack->pack_index < 1) + pack->pack_index = 1; + + info->satinfo.inview = pack->sat_count; + + nsat = (pack->pack_index - 1) * NMEA_SATINPACK; + nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; + + for(isat = 0; isat < nsat; ++isat) + { + isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat; + info->satinfo.sat[isi].id = pack->sat_data[isat].id; + info->satinfo.sat[isi].elv = pack->sat_data[isat].elv; + info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth; + info->satinfo.sat[isi].sig = pack->sat_data[isat].sig; + } +} + +/** +* \brief Fill nmeaINFO structure by RMC packet data. +* @param pack a pointer of packet structure. +* @param info a pointer of summary information structure. +*/ +void nmea_RMC2info(void *mem, nmeaINFO *info) +{ + nmeaRMC *pack = (nmeaRMC*)mem; + NMEA_ASSERT(pack && info); + + if('A' == pack->status) + { + if(NMEA_SIG_BAD == info->sig) + info->sig = NMEA_SIG_MID; + if(NMEA_FIX_BAD == info->fix) + info->fix = NMEA_FIX_2D; + } + else if('V' == pack->status) + { + info->sig = NMEA_SIG_BAD; + info->fix = NMEA_FIX_BAD; + } + + info->utc = pack->utc; + info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); + info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); + info->speed = pack->speed * NMEA_TUD_KNOTS; + info->direction = pack->direction; +} + +/** +* \brief Fill nmeaINFO structure by VTG packet data. +* @param pack a pointer of packet structure. +* @param info a pointer of summary information structure. +*/ +void nmea_VTG2info(void *mem, nmeaINFO *info) +{ + nmeaVTG *pack = (nmeaVTG*)mem; + NMEA_ASSERT(pack && info); + + info->direction = pack->dir; + info->declination = pack->dec; + info->speed = pack->spk; +} + +void nmea_GLL2info(void *mem, nmeaINFO *info) +{ + nmeaGLL *pack = (nmeaGLL*)mem; + NMEA_ASSERT(pack && info); + + if('A' == pack->status) + { + if(NMEA_SIG_BAD == info->sig) + info->sig = NMEA_SIG_MID; + if(NMEA_FIX_BAD == info->fix) + info->fix = NMEA_FIX_2D; + } + else if('V' == pack->status) + { + info->sig = NMEA_SIG_BAD; + info->fix = NMEA_FIX_BAD; + } + + info->utc.hour = pack->utc.hour; + info->utc.min = pack->utc.min; + info->utc.sec = pack->utc.sec; + info->utc.hsec = pack->utc.hsec; + info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); + info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); +} + +void nmea_ZDA2info(void *mem, nmeaINFO *info) +{ + nmeaZDA *pack = (nmeaZDA*)mem; + info->utc.year = pack->utc.year; + info->utc.mon = pack->utc.mon; + info->utc.day = pack->utc.day; + info->utc.hour = pack->utc.hour; + info->utc.min = pack->utc.min; + info->utc.sec = pack->utc.sec; + info->utc.hsec = pack->utc.hsec; +} \ No newline at end of file diff --git a/custom/nmealib/src/parser.c b/custom/nmealib/src/parser.c new file mode 100644 index 0000000..1fd48ff --- /dev/null +++ b/custom/nmealib/src/parser.c @@ -0,0 +1,331 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parser.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/** + * \file parser.h + */ + +#include "nmea/tok.h" +#include "nmea/parse.h" +#include "nmea/parser.h" +#include "nmea/context.h" + +#include +#include + +typedef struct _nmeaParserNODE +{ + nmea_lookup_t* lookup; + void *pack; + struct _nmeaParserNODE *next_node; + +} nmeaParserNODE; + +/* + * high level + */ + +/** + * \brief Initialization of parser object + * @return true (1) - success or false (0) - fail + */ +int nmea_parser_init(nmeaPARSER *parser) +{ + int resv = 0; + int buff_size = nmea_property()->parse_buff_size; + + NMEA_ASSERT(parser); + + if(buff_size < NMEA_MIN_PARSEBUFF) + buff_size = NMEA_MIN_PARSEBUFF; + + memset(parser, 0, sizeof(nmeaPARSER)); + + if(0 == (parser->buffer = malloc(buff_size))) + nmea_error("Insufficient memory!"); + else + { + parser->buff_size = buff_size; + resv = 1; + } + + return resv; +} + +/** + * \brief Destroy parser object + */ +void nmea_parser_destroy(nmeaPARSER *parser) +{ + NMEA_ASSERT(parser && parser->buffer); + free(parser->buffer); + nmea_parser_queue_clear(parser); + memset(parser, 0, sizeof(nmeaPARSER)); +} + +/** + * \brief Analysis of buffer and put results to information structure + * @return Number of packets wos parsed + */ +int nmea_parse( + nmeaPARSER *parser, + const char *buff, int buff_sz, + nmeaINFO *info + ) +{ + int nread = 0; + void *pack = 0; + nmea_lookup_t *lookup = NULL; + + NMEA_ASSERT(parser && parser->buffer); + + nmea_parser_push(parser, buff, buff_sz); + + while (lookup = (nmea_lookup_t*)nmea_parser_pop(parser, &pack)) + { + nread++; + + (*lookup->pfn_info)(pack, info); + info->smask |= lookup->ptype; + free(pack); + } + + return nread; +} + +/* + * low level + */ + +int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz) +{ + int nparsed = 0, crc, sen_sz; + nmeaParserNODE *node = 0; + nmea_lookup_t *lookup; + + NMEA_ASSERT(parser && parser->buffer); + + /* clear unuse buffer (for debug) */ + /* + memset( + parser->buffer + parser->buff_use, 0, + parser->buff_size - parser->buff_use + ); + */ + + /* add */ + if(parser->buff_use + buff_sz >= parser->buff_size) + nmea_parser_buff_clear(parser); + + memcpy(parser->buffer + parser->buff_use, buff, buff_sz); + parser->buff_use += buff_sz; + + /* parse */ + for(;;node = 0) + { + sen_sz = nmea_find_tail( + (const char *)parser->buffer + nparsed, + (int)parser->buff_use - nparsed, &crc); + + if(!sen_sz) + { + if(nparsed) + memcpy( + parser->buffer, + parser->buffer + nparsed, + parser->buff_use -= nparsed); + break; + } + else if(crc >= 0) + { + lookup = nmea_lookuptab( + (const char *)parser->buffer + nparsed + 1, + parser->buff_use - nparsed - 1); + + if(0 == (node = malloc(sizeof(nmeaParserNODE)))) + goto mem_fail; + + node->pack = 0; + + if(0 == (node->pack = malloc(lookup->pack_size))) + goto mem_fail; + //node->packType = lookup->ptype; + node->lookup = lookup; + if (!(*lookup->pfn_parse)( + (const char *)parser->buffer + nparsed, + sen_sz, node->pack, lookup->phead)) + { + free(node); + node = 0; + } + + if(node) + { + if(parser->end_node) + ((nmeaParserNODE *)parser->end_node)->next_node = node; + parser->end_node = node; + if(!parser->top_node) + parser->top_node = node; + node->next_node = 0; + } + } + + nparsed += sen_sz; + } + + return nparsed; + +mem_fail: + if(node) + free(node); + + nmea_error("Insufficient memory!"); + + return -1; +} + +/** + * \brief Analysis of buffer and keep results into parser + * @return Number of bytes wos parsed from buffer + */ +int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz) +{ + int nparse, nparsed = 0; + + do + { + if(buff_sz > parser->buff_size) + nparse = parser->buff_size; + else + nparse = buff_sz; + + nparsed += nmea_parser_real_push( + parser, buff, nparse); + + buff_sz -= nparse; + + } while(buff_sz); + + return nparsed; +} + +/** + * \brief Get type of top packet keeped into parser + * @return nmea_lookup_t's pointer of packet + * @see nmea_lookuptab + */ +void* nmea_parser_top(nmeaPARSER *parser) +{ + nmea_lookup_t *lookup = NULL; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + NMEA_ASSERT(parser && parser->buffer); + + if(node) + lookup = node->lookup; + + return lookup; +} + +/** + * \brief Withdraw top packet from parser + * @return nmea_lookup_t's pointer of packet + * @see nmea_lookuptab + */ +void* nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr) +{ + nmea_lookup_t *lookup = NULL; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + *pack_ptr = node->pack; + //retval = node->packType; + lookup = node->lookup; + parser->top_node = node->next_node; + if(!parser->top_node) + parser->end_node = 0; + free(node); + } + + return lookup; +} + +/** + * \brief Get top packet from parser without withdraw + * @return nmea_lookup_t's pointer of packet + * @see nmea_lookuptab + */ +void* nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr) +{ + nmea_lookup_t *lookup = NULL; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + *pack_ptr = node->pack; + lookup = node->lookup; + } + + return lookup; +} + +/** + * \brief Delete top packet from parser + * @return Deleted nmea_lookup_t's pointer of packet + * @see nmea_lookuptab + */ +void* nmea_parser_drop(nmeaPARSER *parser) +{ + nmea_lookup_t *lookup = NULL; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + if(node->pack) + free(node->pack); + lookup = node->lookup; + parser->top_node = node->next_node; + if(!parser->top_node) + parser->end_node = 0; + free(node); + } + + return lookup; +} + +/** + * \brief Clear cache of parser + * @return true (1) - success + */ +int nmea_parser_buff_clear(nmeaPARSER *parser) +{ + NMEA_ASSERT(parser && parser->buffer); + parser->buff_use = 0; + return 1; +} + +/** + * \brief Clear packets queue into parser + * @return true (1) - success + */ +int nmea_parser_queue_clear(nmeaPARSER *parser) +{ + NMEA_ASSERT(parser); + while(parser->top_node) + nmea_parser_drop(parser); + return 1; +} diff --git a/custom/nmealib/src/sentence.c b/custom/nmealib/src/sentence.c new file mode 100644 index 0000000..4ba6a64 --- /dev/null +++ b/custom/nmealib/src/sentence.c @@ -0,0 +1,69 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: sentence.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/sentence.h" + +#include + +void nmea_zero_GGA(nmeaGGA *pack) +{ + memset(pack, 0, sizeof(nmeaGGA)); + nmea_time_now(&pack->utc); + pack->ns = 'N'; + pack->ew = 'E'; + pack->elv_units = 'M'; + pack->diff_units = 'M'; +} + +void nmea_zero_GSA(nmeaGSA *pack) +{ + memset(pack, 0, sizeof(nmeaGSA)); + pack->fix_mode = 'A'; + pack->fix_type = NMEA_FIX_BAD; +} + +void nmea_zero_GSV(nmeaGSV *pack) +{ + memset(pack, 0, sizeof(nmeaGSV)); +} + +void nmea_zero_RMC(nmeaRMC *pack) +{ + memset(pack, 0, sizeof(nmeaRMC)); + nmea_time_now(&pack->utc); + pack->status = 'V'; + pack->ns = 'N'; + pack->ew = 'E'; + pack->declin_ew = 'E'; +} + +void nmea_zero_VTG(nmeaVTG *pack) +{ + memset(pack, 0, sizeof(nmeaVTG)); + pack->dir_t = 'T'; + pack->dec_m = 'M'; + pack->spn_n = 'N'; + pack->spk_k = 'K'; +} + +////////////////////////////////////////////////////////////////////////// +void nmea_zero_GLL(nmeaGLL *pack) +{ + memset(pack, 0, sizeof(nmeaGLL)); + nmea_time_now(&pack->utc); + pack->ns = 'N'; + pack->ew = 'E'; + pack->status = 'V'; +} + +void nmea_zero_ZDA(nmeaZDA *pack) +{ + memset(pack, 0, sizeof(nmeaZDA)); +} \ No newline at end of file diff --git a/custom/nmealib/src/time.c b/custom/nmealib/src/time.c new file mode 100644 index 0000000..85bd700 --- /dev/null +++ b/custom/nmealib/src/time.c @@ -0,0 +1,63 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: time.c 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +/*! \file time.h */ + +#include "nmea/time.h" + +#ifdef NMEA_WIN +# pragma warning(disable: 4201) +# pragma warning(disable: 4214) +# pragma warning(disable: 4115) +# include +# pragma warning(default: 4201) +# pragma warning(default: 4214) +# pragma warning(default: 4115) +#else +# include +#endif + +#ifdef NMEA_WIN + +void nmea_time_now(nmeaTIME *stm) +{ + SYSTEMTIME st; + + GetSystemTime(&st); + + stm->year = st.wYear; + stm->mon = st.wMonth; + stm->day = st.wDay; + stm->hour = st.wHour; + stm->min = st.wMinute; + stm->sec = st.wSecond; + stm->hsec = st.wMilliseconds / 10; +} + +#else /* NMEA_WIN */ + +void nmea_time_now(nmeaTIME *stm) +{ + time_t lt; + struct tm *tt; + + time(<); + tt = gmtime(<); + + stm->year = tt->tm_year; + stm->mon = tt->tm_mon; + stm->day = tt->tm_mday; + stm->hour = tt->tm_hour; + stm->min = tt->tm_min; + stm->sec = tt->tm_sec; + stm->hsec = 0; +} + +#endif diff --git a/custom/nmealib/src/tok.c b/custom/nmealib/src/tok.c new file mode 100644 index 0000000..585bdef --- /dev/null +++ b/custom/nmealib/src/tok.c @@ -0,0 +1,250 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: tok.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file tok.h */ + +#include "nmea/tok.h" + +#include +#include +#include +#include +#include +#include + +#define NMEA_TOKS_COMPARE (1) +#define NMEA_TOKS_PERCENT (2) +#define NMEA_TOKS_WIDTH (3) +#define NMEA_TOKS_TYPE (4) + +/** + * \brief Calculate control sum of binary buffer + */ +int nmea_calc_crc(const char *buff, int buff_sz) +{ + int chsum = 0, + it; + + for(it = 0; it < buff_sz; ++it) + chsum ^= (int)buff[it]; + + return chsum; +} + +/** + * \brief Convert string to number + */ +int nmea_atoi(const char *str, int str_sz, int radix) +{ + char *tmp_ptr; + char buff[NMEA_CONVSTR_BUF]; + int res = 0; + + if(str_sz < NMEA_CONVSTR_BUF) + { + memcpy(&buff[0], str, str_sz); + buff[str_sz] = '\0'; + res = strtol(&buff[0], &tmp_ptr, radix); + } + + return res; +} + +/** + * \brief Convert string to fraction number + */ +double nmea_atof(const char *str, int str_sz) +{ + char *tmp_ptr; + char buff[NMEA_CONVSTR_BUF]; + double res = 0; + + if(str_sz < NMEA_CONVSTR_BUF) + { + memcpy(&buff[0], str, str_sz); + buff[str_sz] = '\0'; + res = strtod(&buff[0], &tmp_ptr); + } + + return res; +} + +/** + * \brief Formating string (like standart printf) with CRC tail (*CRC) + */ +int nmea_printf(char *buff, int buff_sz, const char *format, ...) +{ + int retval, add = 0; + va_list arg_ptr; + + if(buff_sz <= 0) + return 0; + + va_start(arg_ptr, format); + + retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr); + + if(retval > 0) + { + add = NMEA_POSIX(snprintf)( + buff + retval, buff_sz - retval, "*%02x\r\n", + nmea_calc_crc(buff + 1, retval - 1)); + } + + retval += add; + + if(retval < 0 || retval > buff_sz) + { + memset(buff, ' ', buff_sz); + retval = buff_sz; + } + + va_end(arg_ptr); + + return retval; +} + +/** + * \brief Analyse string (specificate for NMEA sentences) + */ +int nmea_scanf(const char *buff, int buff_sz, const char *format, ...) +{ + const char *beg_tok; + const char *end_buf = buff + buff_sz; + + va_list arg_ptr; + int tok_type = NMEA_TOKS_COMPARE; + int width = 0; + const char *beg_fmt = 0; + int snum = 0, unum = 0; + + int tok_count = 0; + void *parg_target; + + va_start(arg_ptr, format); + + for(; *format && buff < end_buf; ++format) + { + switch(tok_type) + { + case NMEA_TOKS_COMPARE: + if('%' == *format) + tok_type = NMEA_TOKS_PERCENT; + else if(*buff++ != *format) + goto fail; + break; + case NMEA_TOKS_PERCENT: + width = 0; + beg_fmt = format; + tok_type = NMEA_TOKS_WIDTH; + case NMEA_TOKS_WIDTH: + if(isdigit(*format)) + break; + { + tok_type = NMEA_TOKS_TYPE; + if(format > beg_fmt) + width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10); + } + case NMEA_TOKS_TYPE: + beg_tok = buff; + + if(!width && ('c' == *format || 'C' == *format) && *buff != format[1]) + width = 1; + + if(width) + { + if(buff + width <= end_buf) + buff += width; + else + goto fail; + } + else + { + if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff)))) + buff = end_buf; + } + + if(buff > end_buf) + goto fail; + + tok_type = NMEA_TOKS_COMPARE; + tok_count++; + + parg_target = 0; width = (int)(buff - beg_tok); + + switch(*format) + { + case 'c': + case 'C': + parg_target = (void *)va_arg(arg_ptr, char *); + if(width && 0 != (parg_target)) + *((char *)parg_target) = *beg_tok; + break; + case 's': + case 'S': + parg_target = (void *)va_arg(arg_ptr, char *); + if(width && 0 != (parg_target)) + { + memcpy(parg_target, beg_tok, width); + ((char *)parg_target)[width] = '\0'; + } + break; + case 'f': + case 'g': + case 'G': + case 'e': + case 'E': + parg_target = (void *)va_arg(arg_ptr, double *); + if(width && 0 != (parg_target)) + *((double *)parg_target) = nmea_atof(beg_tok, width); + break; + }; + + if(parg_target) + break; + if(0 == (parg_target = (void *)va_arg(arg_ptr, int *))) + break; + if(!width) + break; + + switch(*format) + { + case 'd': + case 'i': + snum = nmea_atoi(beg_tok, width, 10); + memcpy(parg_target, &snum, sizeof(int)); + break; + case 'u': + unum = nmea_atoi(beg_tok, width, 10); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + case 'x': + case 'X': + unum = nmea_atoi(beg_tok, width, 16); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + case 'o': + unum = nmea_atoi(beg_tok, width, 8); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + default: + goto fail; + }; + + break; + }; + } + +fail: + + va_end(arg_ptr); + + return tok_count; +}