This repository has been archived by the owner on Sep 4, 2018. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
NMEA.c
125 lines (115 loc) · 2.89 KB
/
NMEA.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
/*
A library for building pseudo-NMEA packets
*/
#include "NMEA.h"
#include <ctype.h> // isprint
#include <string.h> // strlen
#include <stdio.h> // sprintf
#include <stdlib.h> // strtoul
#define g_CRC_init 0xFF
// return the calculated checksum of msg
unsigned char NMEA_CRC(NMEA_msg_t *const msg)
{
unsigned char i;
msg->State = e_NMEA_STAR;
msg->Checksum = g_CRC_init;
for (i = 0; i < msg->Length; i++)
msg->Checksum ^= msg->String[i];
return msg->Checksum;
}
unsigned char NMEA_argc(NMEA_msg_t *const msg)
{
unsigned char i;
msg->Arguments = 0;
for (i = 0; i < msg->Length; i++)
msg->Arguments += msg->String[i] == e_NMEA_DELIMIT;
return msg->Arguments;
}
char NMEA_start(NMEA_msg_t *const msg, const char *const command)
{
unsigned char l = strlen(command);
msg->State = e_NMEA_DOLLAR;
msg->Arguments = 0;
for (msg->Length = 0; msg->Length < l; msg->Length++)
msg->String[msg->Length] = command[msg->Length];
return msg->Length;
}
char NMEA_add(NMEA_msg_t *const msg, const char *const arg)
{
unsigned char i, l = strlen(arg);
msg->String[msg->Length++] = e_NMEA_DELIMIT;
for (i = 0; i < l; i++)
msg->String[msg->Length + i] = arg[i];
msg->Length += i;
msg->String[msg->Length] = 0;
msg->Arguments++;
return l;
}
char NMEA_finish(NMEA_msg_t *const msg, char *const out)
{
unsigned char i;
out[0] = msg->State;
for (i = 0; i < msg->Length; i++)
out[1 + i] = msg->String[i];
NMEA_CRC(msg);
out[i + 1] = msg->State;
sprintf(out + i + 2,"%02X", msg->Checksum);
msg->State = e_NMEA_VALID;
return i + 4;
}
unsigned char NMEA_get(NMEA_msg_t *const msg, unsigned char index, char *const buf)
{
unsigned char i = 0, j = 0;
if (msg->State != e_NMEA_VALID || msg->Arguments < index)
return 0;
// navigate to index'th argument
while (index > 0)
if(msg->String[i++] == e_NMEA_DELIMIT)
index--;
while (msg->String[i] != e_NMEA_DELIMIT && isprint(msg->String[i]))
buf[j++] = msg->String[i++];
buf[j] = 0;
return j;
}
char NMEA_parse_byte(NMEA_msg_t *const msg, const unsigned char byte)
{
static char STARS, STAR[2];
if (byte == e_NMEA_DOLLAR)
{
msg->Length = 0;
return msg->State = byte;
}
if (!isprint(byte))
return msg->State = e_NMEA_INVALID;
if (byte == e_NMEA_STAR)
{
STARS = 0;
return msg->State = byte;
}
switch (msg->State)
{
case e_NMEA_DOLLAR:
msg->String[msg->Length++] = byte;
break;
case e_NMEA_STAR:
STAR[STARS++] = byte;
if(STARS < 2)
return msg->State;
if(NMEA_CRC(msg) != strtoul(STAR, NULL, 16))
return msg->State == e_NMEA_INVALID;
NMEA_argc(msg);
msg->State = e_NMEA_VALID;
break;
}
}
char NMEA_parse_string(NMEA_msg_t *const msg, const char *const str)
{
unsigned char i = 0, l = strlen(str);
for(i = 0; i < l; i++)
{
NMEA_parse_byte(msg, str[i]);
if(msg->State == e_NMEA_VALID)
return i + 1;
}
return 0;
}