添加GPS定位nmea协议解析功能 zsxfly20241014
This commit is contained in:
zsx
parent
9bb9f43b47
commit
e3baeaa47f
|
|
@ -0,0 +1,6 @@
|
|||
|
||||
CUSTOM_MAIN_DIR := custom/GPS
|
||||
|
||||
OC_FILES += $(CUSTOM_MAIN_DIR)/src/gps_config.c
|
||||
|
||||
INC += -I'$(CUSTOM_MAIN_DIR)/inc'
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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'
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
#ifndef __app_COMMON_H__
|
||||
#define __app_COMMON_H__
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#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__ */
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <assert.h>
|
||||
# define NMEA_ASSERT(x) assert(x)
|
||||
#else
|
||||
# define NMEA_ASSERT(x)
|
||||
#endif
|
||||
|
||||
#endif /* __NMEA_CONFIG_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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -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/'
|
||||
|
|
@ -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 <string.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -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 <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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
|
||||
|
|
@ -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 <math.h>
|
||||
#include <float.h>
|
||||
|
||||
/**
|
||||
* \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);
|
||||
}
|
||||
|
|
@ -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 <string.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -0,0 +1,414 @@
|
|||
<?xml version="1.0" encoding="windows-1251"?>
|
||||
<VisualStudioProject
|
||||
ProjectType="Visual C++"
|
||||
Version="9.00"
|
||||
Name="nmea"
|
||||
ProjectGUID="{462522F4-3517-4507-A9C9-1D51DEBC4824}"
|
||||
RootNamespace="nmea"
|
||||
Keyword="Win32Proj"
|
||||
TargetFrameworkVersion="131072"
|
||||
>
|
||||
<Platforms>
|
||||
<Platform
|
||||
Name="Win32"
|
||||
/>
|
||||
<Platform
|
||||
Name="Pocket PC 2003 (ARMV4)"
|
||||
/>
|
||||
</Platforms>
|
||||
<ToolFiles>
|
||||
</ToolFiles>
|
||||
<Configurations>
|
||||
<Configuration
|
||||
Name="Debug|Win32"
|
||||
OutputDirectory="../build/$(PlatformName)/$(ConfigurationName)/$(ProjectName)"
|
||||
IntermediateDirectory="../build/$(PlatformName)/$(ConfigurationName)/$(ProjectName)"
|
||||
ConfigurationType="4"
|
||||
CharacterSet="2"
|
||||
>
|
||||
<Tool
|
||||
Name="VCPreBuildEventTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCustomBuildTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCXMLDataGeneratorTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCWebServiceProxyGeneratorTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCMIDLTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCLCompilerTool"
|
||||
Optimization="0"
|
||||
AdditionalIncludeDirectories="../include"
|
||||
PreprocessorDefinitions="WIN32;_DEBUG;_LIB"
|
||||
MinimalRebuild="true"
|
||||
ExceptionHandling="0"
|
||||
BasicRuntimeChecks="3"
|
||||
RuntimeLibrary="3"
|
||||
UsePrecompiledHeader="0"
|
||||
WarningLevel="4"
|
||||
Detect64BitPortabilityProblems="true"
|
||||
DebugInformationFormat="3"
|
||||
CompileAs="1"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCManagedResourceCompilerTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCResourceCompilerTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPreLinkEventTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCLibrarianTool"
|
||||
OutputFile="../lib/$(ProjectName)_d.lib"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCALinkTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCXDCMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCBscMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCFxCopTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPostBuildEventTool"
|
||||
/>
|
||||
</Configuration>
|
||||
<Configuration
|
||||
Name="Debug|Pocket PC 2003 (ARMV4)"
|
||||
OutputDirectory="../build/$(PlatformName)/$(ConfigurationName)/$(ProjectName)"
|
||||
IntermediateDirectory="../build/$(PlatformName)/$(ConfigurationName)/$(ProjectName)"
|
||||
ConfigurationType="4"
|
||||
CharacterSet="2"
|
||||
>
|
||||
<Tool
|
||||
Name="VCPreBuildEventTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCustomBuildTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCXMLDataGeneratorTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCWebServiceProxyGeneratorTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCMIDLTool"
|
||||
TargetEnvironment="1"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCLCompilerTool"
|
||||
ExecutionBucket="7"
|
||||
Optimization="0"
|
||||
AdditionalIncludeDirectories="../include"
|
||||
PreprocessorDefinitions="_DEBUG;_WIN32_WCE=$(CEVER);UNDER_CE;$(PLATFORMDEFINES);WINCE;DEBUG;_WINDOWS;$(ARCHFAM);$(_ARCHFAM_);_LIB"
|
||||
MinimalRebuild="true"
|
||||
ExceptionHandling="0"
|
||||
RuntimeLibrary="1"
|
||||
BufferSecurityCheck="false"
|
||||
UsePrecompiledHeader="0"
|
||||
WarningLevel="4"
|
||||
DebugInformationFormat="3"
|
||||
CompileAs="1"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCManagedResourceCompilerTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCResourceCompilerTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPreLinkEventTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCLibrarianTool"
|
||||
OutputFile="../lib/$(ProjectName)_ppc_d.lib"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCALinkTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCXDCMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCBscMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCFxCopTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCodeSignTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPostBuildEventTool"
|
||||
/>
|
||||
<DeploymentTool
|
||||
ForceDirty="-1"
|
||||
RemoteDirectory=""
|
||||
RegisterOutput="0"
|
||||
AdditionalFiles=""
|
||||
/>
|
||||
<DebuggerTool
|
||||
/>
|
||||
</Configuration>
|
||||
<Configuration
|
||||
Name="Release|Win32"
|
||||
OutputDirectory="../build/$(PlatformName)/$(ConfigurationName)/$(ProjectName)"
|
||||
IntermediateDirectory="../build/$(PlatformName)/$(ConfigurationName)/$(ProjectName)"
|
||||
ConfigurationType="4"
|
||||
CharacterSet="2"
|
||||
WholeProgramOptimization="1"
|
||||
>
|
||||
<Tool
|
||||
Name="VCPreBuildEventTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCustomBuildTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCXMLDataGeneratorTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCWebServiceProxyGeneratorTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCMIDLTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCLCompilerTool"
|
||||
AdditionalIncludeDirectories="../include"
|
||||
PreprocessorDefinitions="WIN32;NDEBUG;_LIB"
|
||||
ExceptionHandling="0"
|
||||
RuntimeLibrary="0"
|
||||
UsePrecompiledHeader="0"
|
||||
WarningLevel="4"
|
||||
Detect64BitPortabilityProblems="false"
|
||||
DebugInformationFormat="3"
|
||||
CompileAs="1"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCManagedResourceCompilerTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCResourceCompilerTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPreLinkEventTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCLibrarianTool"
|
||||
OutputFile="../lib/$(ProjectName).lib"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCALinkTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCXDCMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCBscMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCFxCopTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPostBuildEventTool"
|
||||
/>
|
||||
</Configuration>
|
||||
<Configuration
|
||||
Name="Release|Pocket PC 2003 (ARMV4)"
|
||||
OutputDirectory="../build/$(PlatformName)/$(ConfigurationName)/$(ProjectName)"
|
||||
IntermediateDirectory="../build/$(PlatformName)/$(ConfigurationName)/$(ProjectName)"
|
||||
ConfigurationType="4"
|
||||
CharacterSet="2"
|
||||
WholeProgramOptimization="1"
|
||||
>
|
||||
<Tool
|
||||
Name="VCPreBuildEventTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCustomBuildTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCXMLDataGeneratorTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCWebServiceProxyGeneratorTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCMIDLTool"
|
||||
TargetEnvironment="1"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCLCompilerTool"
|
||||
ExecutionBucket="7"
|
||||
AdditionalIncludeDirectories="../include"
|
||||
PreprocessorDefinitions="NDEBUG;_WIN32_WCE=$(CEVER);UNDER_CE;$(PLATFORMDEFINES);WINCE;_WINDOWS;$(ARCHFAM);$(_ARCHFAM_);_LIB"
|
||||
ExceptionHandling="0"
|
||||
RuntimeLibrary="0"
|
||||
BufferSecurityCheck="false"
|
||||
UsePrecompiledHeader="0"
|
||||
WarningLevel="3"
|
||||
DebugInformationFormat="3"
|
||||
CompileAs="1"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCManagedResourceCompilerTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCResourceCompilerTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPreLinkEventTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCLibrarianTool"
|
||||
OutputFile="../lib/$(ProjectName)_ppc.lib"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCALinkTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCXDCMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCBscMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCFxCopTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCCodeSignTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPostBuildEventTool"
|
||||
/>
|
||||
<DeploymentTool
|
||||
ForceDirty="-1"
|
||||
RemoteDirectory=""
|
||||
RegisterOutput="0"
|
||||
AdditionalFiles=""
|
||||
/>
|
||||
<DebuggerTool
|
||||
/>
|
||||
</Configuration>
|
||||
</Configurations>
|
||||
<References>
|
||||
</References>
|
||||
<Files>
|
||||
<File
|
||||
RelativePath="..\include\nmea\config.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\context.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\context.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\generate.c"
|
||||
>
|
||||
<FileConfiguration
|
||||
Name="Debug|Pocket PC 2003 (ARMV4)"
|
||||
>
|
||||
<Tool
|
||||
Name="VCCLCompilerTool"
|
||||
WarningLevel="3"
|
||||
/>
|
||||
</FileConfiguration>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\generate.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\generator.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\generator.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\gmath.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\gmath.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\info.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\info.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\nmea.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\parse.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\parse.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\parser.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\parser.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\sentence.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\sentence.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\time.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\time.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\tok.c"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\tok.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\include\nmea\units.h"
|
||||
>
|
||||
</File>
|
||||
</Files>
|
||||
<Globals>
|
||||
</Globals>
|
||||
</VisualStudioProject>
|
||||
|
|
@ -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 <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
|
@ -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 <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -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 <string.h>
|
||||
|
||||
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));
|
||||
}
|
||||
|
|
@ -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 <windows.h>
|
||||
# pragma warning(default: 4201)
|
||||
# pragma warning(default: 4214)
|
||||
# pragma warning(default: 4115)
|
||||
#else
|
||||
# include <time.h>
|
||||
#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
|
||||
|
|
@ -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 <stdarg.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
#include <string.h>
|
||||
#include <limits.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
Loading…
Reference in New Issue