#include "nmea.h"
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <stdarg.h>
#include <time.h>
//十六进制转十进制
static int hex2int(char c)
{
if (c >= '0' && c <= '9')
return c - '0';
if (c >= 'A' && c <= 'F')
return c - 'A' + 10;
if (c >= 'a' && c <= 'f')
return c - 'a' + 10;
return -1;
}
uint8_t minmea_checksum(const char *sentence)
{
// Support senteces with or without the starting dollar sign.
if (*sentence == '$')
sentence++;
uint8_t checksum = 0x00;
// The optional checksum is an XOR of all bytes between "$" and "*".
while (*sentence && *sentence != '*')
checksum ^= *sentence++;
return checksum;
}
//NMEA通讯协议所规定的通讯语句都已是以ASCII码为基础的，NMEA - 0183协议语句的数据格式如下：
//“$”为语句起始标志；“，”为域分隔符；“ *”为校验码识别符，
//其后面的两位数为校验码，代表了“$”和“*”之间所有字符的按位异或值（不包括这两个字符）；
//“／”为终止符，所有的语句必须以来结束，也就是ASCII 字符的“回车”（十六进制的0D）和“换行”（十六进制的0A）
/*
检查协议格式是否正确
sentence： 协议语句
strict： 是否是严格模式
*/
bool minmea_check(const char *sentence, bool strict)
{
uint8_t checksum = 0x00;
//长度检测
if (strlen(sentence) > MINMEA_MAX_LENGTH + 3)
return false;
//协议必须以"$"开头
if (*sentence++ != '$')
return false;
//取“$”和“*”之间所有字节的异或结果进行校验
while (*sentence && *sentence != '*' && isprint((unsigned char) *sentence))
checksum ^= *sentence++;
// “ *”校验码识别符存在
if (*sentence == '*') {
sentence++;
int upper = hex2int(*sentence++);
if (upper == -1)
return false;
int lower = hex2int(*sentence++);
if (lower == -1)
return false;
int expected = upper << 4 | lower;
// 校验码是否匹配
if (checksum != expected)
return false;
} else if (strict) {
//严格模式下 返回失败
return false;
}
//结尾是换行标志
if (*sentence && strcmp(sentence, "\n") && strcmp(sentence, "\r\n"))
return false;
return true;
}
static inline bool minmea_isfield(char c)
{
return isprint((unsigned char) c) && c != ',' && c != '*';
}
/**
* 根据给定的格式解析协议字段，格式如下：
* c - 单字符
* d - 经纬度方向
* f - 经纬度值
* i - 数值
* s - 字符串
* t - 信息源标识符
* T - 时间/日期
*/
bool minmea_scan(const char *sentence, const char *format, ...)
{
bool result = false;
bool optional = false;
va_list ap;
va_start(ap, format);
const char *field = sentence;
#define next_field() \
do { \
/* 处理写一个字段*/ \
while (minmea_isfield(*sentence)) \
sentence++; \
/* ','开头 */ \
if (*sentence == ',') { \
sentence++; \
field = sentence; \
} else { \
field = NULL; \
} \
} while (0)
while (*format) {
char type = *format++;
if (type == ';') {
//所有其他字段都是可选的
optional = true;
continue;
}
if (!field && !optional) {
//字段不是我们要的
goto parse_error;
}
switch (type) {
case 'c': { // 单字符字段，类型：char
char value = '\0';
if (field && minmea_isfield(*field))
value = *field;
*va_arg(ap, char *) = value;
}
break;
case 'd': { // 单字符字段，表示方向的东南西北，类型：int
int value = 0;
if (field && minmea_isfield(*field)) {
switch (*field) {
case 'N':
case 'E':
value = 1;
break;
case 'S':
case 'W':
value = -1;
break;
default:
goto parse_error;
}
}
*va_arg(ap, int *) = value;
}
break;
case 'f': { // 经纬度的度分表示 类型：struct minmea_float
int sign = 0;
int_least32_t value = -1;
int_least32_t scale = 0;
if (field) {
while (minmea_isfield(*field)) {
if (*field == '+' && !sign && value == -1) {
sign = 1;
} else if (*field == '-' && !sign && value == -1) {
sign = -1;
} else if (isdigit((unsigned char) *field)) {
int digit = *field - '0';
if (value == -1)
value = 0;
if (value > (INT_LEAST32_MAX-digit) / 10) {
if (scale) {
break;
} else {
goto parse_error;
}
}
value = (10 * value) + digit;
if (scale)
scale *= 10;
} else if (*field == '.' && scale == 0) {
scale = 1;
} else if (*field == ' ') {
//字段开头是空格的
if (sign != 0 || value != -1 || scale != 0)
goto parse_error;
} else {
goto parse_error;
}
field++;
}
}
if ((sign || scale) && value == -1)
goto parse_error;
if (value == -1) {
//没有数字
value = 0;
scale = 0;
} else if (scale == 0) {
//没有小数部分，分
scale = 1;
}
if (sign)
value *= sign;
*va_arg(ap, struct minmea_float *) = (struct minmea_float) {
value, scale
};
}
break;
case 'i': { // int类型的值, default 0 类型：int
int value = 0;
if (field) {
char *endptr;
value = strtol(field, &endptr, 10);
if (minmea_isfield(*endptr))
goto parse_error;
}
*va_arg(ap, int *) = value;
}
break;
case 's': { // 字符串类型 类型：char *
char *buf = va_arg(ap, char *);
if (field) {
while (minmea_isfield(*field))
*buf++ = *field++;
}
*buf = '\0';
}
break;
case 't': { // NMEA 协议信息源字段 类型：char *
// 这个字段是必须的
if (!field)
goto parse_error;
if (field[0] != '$')
goto parse_error;
for (int f=0; f<5; f++)
if (!minmea_isfield(field[1+f]))
goto parse_error;
char *buf = va_arg(ap, char *);
memcpy(buf, field+1, 5);
buf[5] = '\0';
}
break;
case 'D': { // 日期类型的年月日 (int, int, int), 默认-1
struct minmea_date *date = va_arg(ap, struct minmea_date *);
int d = -1, m = -1, y = -1;
if (field && minmea_isfield(*field)) {
// 年月日共六位dd-mm-yy
for (int f=0; f<6; f++)
if (!isdigit((unsigned char) field[f]))
