| @@ -1,48 +1,48 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * nemeaCode.cpp No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #include "nmeaCode.h" No newline at end of file |
|
|
9 | 9 | No newline at end of file |
|
|
10 | 10 | #include <string.h> No newline at end of file |
|
|
11 | 11 | #include <stdarg.h> No newline at end of file |
|
|
12 | 12 | #include <stdio.h> No newline at end of file |
|
|
13 | 13 | #include <time.h> No newline at end of file |
|
|
14 | 14 | #include <math.h> No newline at end of file |
|
|
15 | 15 | #include <float.h> No newline at end of file |
|
|
16 | 16 | #include <limits.h> No newline at end of file |
|
|
17 | 17 | #include <ctype.h> No newline at end of file |
|
|
18 | 18 | #include <stdlib.h> No newline at end of file |
|
|
19 | 19 | No newline at end of file |
|
|
20 | 20 | nmeaCode::nmeaCode(char* name=nullptr): No newline at end of file |
|
|
21 | 21 | codebuff(nullptr){ No newline at end of file |
|
|
22 | 22 | No newline at end of file |
|
|
23 | 23 | } No newline at end of file |
|
|
24 | 24 | No newline at end of file |
|
|
25 | 25 | nmeaCode::~nmeaCode() { No newline at end of file |
|
|
26 | 26 | // TODO Auto-generated destructor stub No newline at end of file |
|
|
27 | 27 | } No newline at end of file |
|
|
28 | 28 | |
|
|
No newline at end of file | ||
|
|
29 | virtual void nmeaCode::code_init() { No newline at end of file | |
|
|
30 | 29 | |
|
|
30 | No newline at end of file | |
|
|
31 | } | |
|
|
No newline at end of file | ||
|
|
31 | No newline at end of file | |
|
|
32 | virtual int nmeaCode::code_gen(char *buff, int buff_sz) { | |
|
|
No newline at end of file | ||
|
|
32 | No newline at end of file | |
|
|
33 | return 0; No newline at end of file | |
|
|
No newline at end of file | ||
|
|
33 | strcpy(codebuff, cd); | |
|
|
No newline at end of file | ||
|
|
34 | return true; No newline at end of file | |
|
|
34 | 35 | } No newline at end of file |
|
|
35 | 36 | |
|
|
37 | No newline at end of file | |
|
|
36 | virtual int nmeaCode::code_parse(const char *buff, int buff_sz) { | |
|
|
No newline at end of file | ||
|
|
38 | No newline at end of file | |
|
|
37 | return 0; No newline at end of file | |
|
|
38 | 39 | } No newline at end of file |
|
|
39 | 40 | No newline at end of file |
|
|
40 | 41 | No newline at end of file |
|
|
41 | 42 | //////////////////////////////////////////////////////////////////////////////////////// No newline at end of file |
|
|
42 | 43 | No newline at end of file |
|
|
43 | 44 | No newline at end of file |
|
|
44 | 45 | No newline at end of file |
|
|
45 | 46 | No newline at end of file |
|
|
46 | 47 | No newline at end of file |
|
|
47 | 48 | No newline at end of file |
|
|
48 | 49 |
|
| @@ -1,32 +1,29 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * nemeaCode.h No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #ifndef NMEALIB_NMEALIB___NMEACODE_H_ No newline at end of file |
|
|
9 | 9 | #define NMEALIB_NMEALIB___NMEACODE_H_ No newline at end of file |
|
|
10 | 10 | No newline at end of file |
|
|
11 | 11 | #include "nmea_defs.h" No newline at end of file |
|
|
12 | 12 | No newline at end of file |
|
|
13 | 13 | class nmeaCode { No newline at end of file |
|
|
14 | 14 | public: No newline at end of file |
|
|
15 | 15 | nmeaCode(char* name=nullptr); No newline at end of file |
|
|
16 | 16 | virtual ~nmeaCode(); No newline at end of file |
|
|
17 | 17 | |
|
|
18 | No newline at end of file | |
|
|
18 | virtual void code_init(void); | |
|
|
No newline at end of file | ||
|
|
19 | No newline at end of file | |
|
|
19 | virtual int code_gen(char *buff, int buff_sz);// TODO para generator | |
|
|
No newline at end of file | ||
|
|
20 | virtual int code_parse(const char *buff, int buff_sz);// TODO para parser | |
|
|
No newline at end of file | ||
|
|
21 | ||
|
|
No newline at end of file | ||
|
|
22 | ||
|
|
No newline at end of file | ||
|
|
23 | No newline at end of file | |
|
|
24 | 20 | No newline at end of file |
|
|
25 | 21 | private: No newline at end of file |
|
|
22 | size_t size; No newline at end of file | |
|
|
26 | 23 | const char codename[8]; No newline at end of file |
|
|
27 | 24 | const int type; No newline at end of file |
|
|
28 | 25 | char *codebuff; No newline at end of file |
|
|
29 | 26 | No newline at end of file |
|
|
30 | 27 | }; No newline at end of file |
|
|
31 | 28 | No newline at end of file |
|
|
32 | 29 | #endif /* NMEALIB_NMEALIB___NMEACODE_H_ */ No newline at end of file |
| @@ -1,194 +1,194 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * nmeaGenerator.cpp No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #include <string.h> No newline at end of file |
|
|
9 | 9 | #include <stdlib.h> No newline at end of file |
|
|
10 | 10 | #include "nmeaGenerator.h" No newline at end of file |
|
|
11 | 11 | No newline at end of file |
|
|
12 | 12 | No newline at end of file |
|
|
13 | 13 | nmeaGenerator::nmeaGenerator() { No newline at end of file |
|
|
14 | 14 | // TODO Auto-generated constructor stub No newline at end of file |
|
|
15 | 15 | No newline at end of file |
|
|
16 | 16 | } No newline at end of file |
|
|
17 | 17 | No newline at end of file |
|
|
18 | 18 | nmeaGenerator::~nmeaGenerator() { No newline at end of file |
|
|
19 | 19 | // TODO Auto-generated destructor stub No newline at end of file |
|
|
20 | 20 | } No newline at end of file |
|
|
21 | 21 | No newline at end of file |
|
|
22 | 22 | |
|
|
23 | No newline at end of file | |
|
|
23 | virtual int nmeaGenerator::code_info2nmea(const nmeaInfo *info, nmeaCode* code) No newline at end of file | |
|
|
24 | 24 | { No newline at end of file |
|
|
25 | 25 | return 1; No newline at end of file |
|
|
26 | 26 | } No newline at end of file |
|
|
27 | 27 | No newline at end of file |
|
|
28 | 28 | No newline at end of file |
|
|
29 | 29 | /* No newline at end of file |
|
|
30 | 30 | * low level No newline at end of file |
|
|
31 | 31 | */ No newline at end of file |
|
|
32 | 32 | No newline at end of file |
|
|
33 | 33 | int nmeaGenerator::nmea_gen_init(nmeaInfo *info) No newline at end of file |
|
|
34 | 34 | { No newline at end of file |
|
|
35 | 35 | int RetVal = 1; int smask = info->smask; No newline at end of file |
|
|
36 | 36 | |
|
|
37 | No newline at end of file | |
|
|
37 | info->code_init_INFO(); No newline at end of file | |
|
|
38 | 38 | info->smask = smask; No newline at end of file |
|
|
39 | 39 | No newline at end of file |
|
|
40 | 40 | info->lat = NMEA_DEF_LAT; No newline at end of file |
|
|
41 | 41 | info->lon = NMEA_DEF_LON; No newline at end of file |
|
|
42 | 42 | No newline at end of file |
|
|
43 | 43 | if(init_call) No newline at end of file |
|
|
44 | 44 | RetVal = (*init_call)(this, info); No newline at end of file |
|
|
45 | 45 | No newline at end of file |
|
|
46 | 46 | if(RetVal && next) No newline at end of file |
|
|
47 | 47 | RetVal = next->nmea_gen_init(info); No newline at end of file |
|
|
48 | 48 | No newline at end of file |
|
|
49 | 49 | return RetVal; No newline at end of file |
|
|
50 | 50 | } No newline at end of file |
|
|
51 | 51 | No newline at end of file |
|
|
52 | 52 | int nmeaGenerator::nmea_gen_loop( nmeaInfo *info) No newline at end of file |
|
|
53 | 53 | { No newline at end of file |
|
|
54 | 54 | int RetVal = 1; No newline at end of file |
|
|
55 | 55 | No newline at end of file |
|
|
56 | 56 | if(loop_call) No newline at end of file |
|
|
57 | 57 | RetVal = (*loop_call)(this, info); No newline at end of file |
|
|
58 | 58 | No newline at end of file |
|
|
59 | 59 | if(RetVal && next) No newline at end of file |
|
|
60 | 60 | RetVal = next->nmea_gen_loop(info); No newline at end of file |
|
|
61 | 61 | No newline at end of file |
|
|
62 | 62 | return RetVal; No newline at end of file |
|
|
63 | 63 | } No newline at end of file |
|
|
64 | 64 | No newline at end of file |
|
|
65 | 65 | int nmeaGenerator::nmea_gen_reset(nmeaInfo *info) No newline at end of file |
|
|
66 | 66 | { No newline at end of file |
|
|
67 | 67 | int RetVal = 1; No newline at end of file |
|
|
68 | 68 | No newline at end of file |
|
|
69 | 69 | if(reset_call) No newline at end of file |
|
|
70 | 70 | RetVal = (*reset_call)(this, info); No newline at end of file |
|
|
71 | 71 | No newline at end of file |
|
|
72 | 72 | return RetVal; No newline at end of file |
|
|
73 | 73 | } No newline at end of file |
|
|
74 | 74 | No newline at end of file |
|
|
75 | 75 | void nmeaGenerator::nmea_gen_destroy() No newline at end of file |
|
|
76 | 76 | { No newline at end of file |
|
|
77 | 77 | if(next) No newline at end of file |
|
|
78 | 78 | { No newline at end of file |
|
|
79 | 79 | next->nmea_gen_destroy(); No newline at end of file |
|
|
80 | 80 | next = 0; No newline at end of file |
|
|
81 | 81 | } No newline at end of file |
|
|
82 | 82 | No newline at end of file |
|
|
83 | 83 | if(destroy_call) No newline at end of file |
|
|
84 | 84 | (*destroy_call)(this); No newline at end of file |
|
|
85 | 85 | No newline at end of file |
|
|
86 | 86 | free(this); No newline at end of file |
|
|
87 | 87 | } No newline at end of file |
|
|
88 | 88 | No newline at end of file |
|
|
89 | 89 | void nmeaGenerator::nmea_gen_add(nmeaGenerator *gen) No newline at end of file |
|
|
90 | 90 | { No newline at end of file |
|
|
91 | 91 | if(next) No newline at end of file |
|
|
92 | 92 | next->nmea_gen_add(gen); No newline at end of file |
|
|
93 | 93 | else No newline at end of file |
|
|
94 | 94 | next = gen; No newline at end of file |
|
|
95 | 95 | } No newline at end of file |
|
|
96 | 96 | No newline at end of file |
|
|
97 | 97 | int nmeaGenerator::nmea_generate_from( No newline at end of file |
|
|
98 | 98 | char *buff, int buff_sz, No newline at end of file |
|
|
99 | 99 | nmeaInfo *info, No newline at end of file |
|
|
100 | 100 | nmeaCode *code No newline at end of file |
|
|
101 | 101 | ) No newline at end of file |
|
|
102 | 102 | { No newline at end of file |
|
|
103 | 103 | int retval; No newline at end of file |
|
|
104 | 104 | No newline at end of file |
|
|
105 | 105 | if(0 != (retval = nmea_gen_loop(info))) No newline at end of file |
|
|
106 | 106 | retval = nmea_generate(buff, buff_sz, info, code); No newline at end of file |
|
|
107 | 107 | No newline at end of file |
|
|
108 | 108 | return retval; No newline at end of file |
|
|
109 | 109 | } No newline at end of file |
|
|
110 | 110 | No newline at end of file |
|
|
111 | 111 | No newline at end of file |
|
|
112 | 112 | No newline at end of file |
|
|
113 | 113 | int nmeaGenerator::nmea_igen_pos_rmove_destroy(void) No newline at end of file |
|
|
114 | 114 | { No newline at end of file |
|
|
115 | 115 | return 1; No newline at end of file |
|
|
116 | 116 | }; No newline at end of file |
|
|
117 | 117 | No newline at end of file |
|
|
118 | 118 | /* No newline at end of file |
|
|
119 | 119 | * generator create No newline at end of file |
|
|
120 | 120 | */ No newline at end of file |
|
|
121 | 121 | No newline at end of file |
|
|
122 | 122 | nmeaGenerator* nmeaGenerator::__nmea_create_generator(int type, nmeaInfo *info) No newline at end of file |
|
|
123 | 123 | { No newline at end of file |
|
|
124 | 124 | nmeaGenerator* gen = nullptr; No newline at end of file |
|
|
125 | 125 | switch(type) No newline at end of file |
|
|
126 | 126 | { No newline at end of file |
|
|
127 | 127 | case NMEA_GEN_NOISE: No newline at end of file |
|
|
128 | 128 | gen = new nmeaGenerator(); No newline at end of file |
|
|
129 | 129 | gen->init_call = &info->nmea_igen_noise_init; No newline at end of file |
|
|
130 | 130 | gen->loop_call = &info->nmea_igen_noise_loop; No newline at end of file |
|
|
131 | 131 | gen->reset_call = &info->nmea_igen_noise_reset; No newline at end of file |
|
|
132 | 132 | break; No newline at end of file |
|
|
133 | 133 | case NMEA_GEN_STATIC: No newline at end of file |
|
|
134 | 134 | case NMEA_GEN_SAT_STATIC: No newline at end of file |
|
|
135 | 135 | gen = new nmeaGenerator(); No newline at end of file |
|
|
136 | 136 | gen->init_call = &info->nmea_igen_static_init; No newline at end of file |
|
|
137 | 137 | gen->loop_call = &info->nmea_igen_static_loop; No newline at end of file |
|
|
138 | 138 | gen->reset_call = &info->nmea_igen_static_reset; No newline at end of file |
|
|
139 | 139 | break; No newline at end of file |
|
|
140 | 140 | case NMEA_GEN_SAT_ROTATE: No newline at end of file |
|
|
141 | 141 | gen = new nmeaGenerator(); No newline at end of file |
|
|
142 | 142 | gen->init_call = &info->nmea_igen_rotate_init; No newline at end of file |
|
|
143 | 143 | gen->loop_call = &info->nmea_igen_rotate_loop; No newline at end of file |
|
|
144 | 144 | gen->reset_call = &info->nmea_igen_rotate_reset; No newline at end of file |
|
|
145 | 145 | break; No newline at end of file |
|
|
146 | 146 | case NMEA_GEN_POS_RANDMOVE: No newline at end of file |
|
|
147 | 147 | gen = new nmeaGenerator(); No newline at end of file |
|
|
148 | 148 | gen->init_call = &info->nmea_igen_pos_rmove_init; No newline at end of file |
|
|
149 | 149 | gen->loop_call = &info->nmea_igen_pos_rmove_loop; No newline at end of file |
|
|
150 | 150 | gen->destroy_call = &nmea_igen_pos_rmove_destroy; No newline at end of file |
|
|
151 | 151 | break; No newline at end of file |
|
|
152 | 152 | case NMEA_GEN_ROTATE: No newline at end of file |
|
|
153 | 153 | gen = new nmeaGenerator(); No newline at end of file |
|
|
154 | 154 | gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info); No newline at end of file |
|
|
155 | 155 | gen->nmea_gen_add(__nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info)); No newline at end of file |
|
|
156 | 156 | break; No newline at end of file |
|
|
157 | 157 | }; No newline at end of file |
|
|
158 | 158 | return this; No newline at end of file |
|
|
159 | 159 | } No newline at end of file |
|
|
160 | 160 | No newline at end of file |
|
|
161 | 161 | nmeaGenerator * nmeaGenerator::nmea_create_generator(int type, nmeaInfo *info) No newline at end of file |
|
|
162 | 162 | { No newline at end of file |
|
|
163 | 163 | nmeaGenerator *gen = __nmea_create_generator(type, info); No newline at end of file |
|
|
164 | 164 | No newline at end of file |
|
|
165 | 165 | if(gen) No newline at end of file |
|
|
166 | 166 | gen->nmea_gen_init(info); No newline at end of file |
|
|
167 | 167 | No newline at end of file |
|
|
168 | 168 | return gen; No newline at end of file |
|
|
169 | 169 | } No newline at end of file |
|
|
170 | 170 | No newline at end of file |
|
|
171 | 171 | void nmeaGenerator::nmea_destroy_generator(void) No newline at end of file |
|
|
172 | 172 | { No newline at end of file |
|
|
173 | 173 | nmea_gen_destroy(); No newline at end of file |
|
|
174 | 174 | } No newline at end of file |
|
|
175 | 175 | No newline at end of file |
|
|
176 | 176 | No newline at end of file |
|
|
177 | 177 | No newline at end of file |
|
|
178 | 178 | int nmeaGenerator::nmea_generate(char *buff, int buff_sz, const nmeaInfo *info, nmeaCode *code) No newline at end of file |
|
|
179 | 179 | { No newline at end of file |
|
|
180 | 180 | int gen_count = 0; No newline at end of file |
|
|
181 | 181 | No newline at end of file |
|
|
182 | 182 | if(!buff) No newline at end of file |
|
|
183 | 183 | return 0; No newline at end of file |
|
|
184 | 184 | No newline at end of file |
|
|
185 | 185 | while(code) No newline at end of file |
|
|
186 | 186 | { |
|
|
187 | No newline at end of file | |
|
|
187 | code_info2nmea(info, code) No newline at end of file | |
|
|
188 | 188 | gen_count += code->code_gen(buff + gen_count, buff_sz - gen_count); No newline at end of file |
|
|
189 | 189 | code += 1; No newline at end of file |
|
|
190 | 190 | if(buff_sz - gen_count <= 0) No newline at end of file |
|
|
191 | 191 | break; No newline at end of file |
|
|
192 | 192 | } No newline at end of file |
|
|
193 | 193 | return gen_count; No newline at end of file |
|
|
194 | 194 | } No newline at end of file |
| @@ -1,70 +1,70 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * nmeaGenerator.h No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #ifndef NMEALIB_NMEALIB___NMEAGENERATOR_H_ No newline at end of file |
|
|
9 | 9 | #define NMEALIB_NMEALIB___NMEAGENERATOR_H_ No newline at end of file |
|
|
10 | 10 | No newline at end of file |
|
|
11 | 11 | #include "nmea_defs.h" No newline at end of file |
|
|
12 | 12 | #include "nmeaCode.h" No newline at end of file |
|
|
13 | 13 | #include "nmeaInfo.h" No newline at end of file |
|
|
14 | 14 | No newline at end of file |
|
|
15 | 15 | /* No newline at end of file |
|
|
16 | 16 | * low level No newline at end of file |
|
|
17 | 17 | */ No newline at end of file |
|
|
18 | 18 | No newline at end of file |
|
|
19 | 19 | typedef int (*nmeaNMEA_GEN_INIT)(nmeaGenerator *gen, nmeaInfo *info); No newline at end of file |
|
|
20 | 20 | typedef int (*nmeaNMEA_GEN_LOOP)(nmeaGenerator *gen, nmeaInfo *info); No newline at end of file |
|
|
21 | 21 | typedef int (*nmeaNMEA_GEN_RESET)(nmeaGenerator *gen, nmeaInfo *info); No newline at end of file |
|
|
22 | 22 | typedef int (*nmeaNMEA_GEN_DESTROY)(nmeaGenerator *gen); No newline at end of file |
|
|
23 | 23 | No newline at end of file |
|
|
24 | 24 | enum nmeaGENTYPE No newline at end of file |
|
|
25 | 25 | { No newline at end of file |
|
|
26 | 26 | NMEA_GEN_NOISE = 0, No newline at end of file |
|
|
27 | 27 | NMEA_GEN_STATIC, No newline at end of file |
|
|
28 | 28 | NMEA_GEN_ROTATE, No newline at end of file |
|
|
29 | 29 | No newline at end of file |
|
|
30 | 30 | NMEA_GEN_SAT_STATIC, No newline at end of file |
|
|
31 | 31 | NMEA_GEN_SAT_ROTATE, No newline at end of file |
|
|
32 | 32 | NMEA_GEN_POS_RANDMOVE, No newline at end of file |
|
|
33 | 33 | No newline at end of file |
|
|
34 | 34 | NMEA_GEN_LAST No newline at end of file |
|
|
35 | 35 | }; No newline at end of file |
|
|
36 | 36 | No newline at end of file |
|
|
37 | 37 | class nmeaGenerator { No newline at end of file |
|
|
38 | 38 | public: No newline at end of file |
|
|
39 | 39 | nmeaGenerator(); No newline at end of file |
|
|
40 | 40 | virtual ~nmeaGenerator(); No newline at end of file |
|
|
41 | 41 | No newline at end of file |
|
|
42 | 42 | |
|
|
43 | No newline at end of file | |
|
|
43 | virtual int code_info2nmea(const nmeaInfo *info, nmeaCode* code); No newline at end of file | |
|
|
44 | 44 | No newline at end of file |
|
|
45 | 45 | int nmea_gen_init(nmeaInfo *info); No newline at end of file |
|
|
46 | 46 | int nmea_gen_loop(nmeaInfo *info); No newline at end of file |
|
|
47 | 47 | int nmea_gen_reset(nmeaInfo *info); No newline at end of file |
|
|
48 | 48 | void nmea_gen_destroy(void); No newline at end of file |
|
|
49 | 49 | void nmea_gen_add(nmeaGenerator *from); No newline at end of file |
|
|
50 | 50 | int nmea_generate(char *buff, int buff_sz, const nmeaInfo *info, nmeaCode* code); No newline at end of file |
|
|
51 | 51 | No newline at end of file |
|
|
52 | 52 | private: No newline at end of file |
|
|
53 | 53 | void *gen_data; No newline at end of file |
|
|
54 | 54 | nmeaNMEA_GEN_INIT init_call; No newline at end of file |
|
|
55 | 55 | nmeaNMEA_GEN_LOOP loop_call; No newline at end of file |
|
|
56 | 56 | nmeaNMEA_GEN_RESET reset_call; No newline at end of file |
|
|
57 | 57 | nmeaNMEA_GEN_DESTROY destroy_call; No newline at end of file |
|
|
58 | 58 | nmeaGenerator *next; No newline at end of file |
|
|
59 | 59 | No newline at end of file |
|
|
60 | 60 | int nmea_generate_from(char *buff, int buff_sz,nmeaInfo *info, nmeaCode *code); No newline at end of file |
|
|
61 | 61 | int nmea_igen_pos_rmove_destroy(void); No newline at end of file |
|
|
62 | 62 | No newline at end of file |
|
|
63 | 63 | No newline at end of file |
|
|
64 | 64 | nmeaGenerator * __nmea_create_generator(int type, nmeaInfo *info); No newline at end of file |
|
|
65 | 65 | nmeaGenerator * nmea_create_generator(int type, nmeaInfo *info); No newline at end of file |
|
|
66 | 66 | void nmea_destroy_generator(void); No newline at end of file |
|
|
67 | 67 | No newline at end of file |
|
|
68 | 68 | }; No newline at end of file |
|
|
69 | 69 | No newline at end of file |
|
|
70 | 70 | #endif /* NMEALIB_NMEALIB___NMEAGENERATOR_H_ */ No newline at end of file |
| @@ -1,257 +1,257 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * nmeaInfo.cpp No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #include "utils.h" No newline at end of file |
|
|
9 | 9 | #include "nmeaInfo.h" No newline at end of file |
|
|
10 | 10 | No newline at end of file |
|
|
11 | 11 | nmeaInfo::nmeaInfo() { No newline at end of file |
|
|
12 | 12 | // TODO Auto-generated constructor stub No newline at end of file |
|
|
13 | 13 | No newline at end of file |
|
|
14 | 14 | } No newline at end of file |
|
|
15 | 15 | No newline at end of file |
|
|
16 | 16 | nmeaInfo::~nmeaInfo() { No newline at end of file |
|
|
17 | 17 | // TODO Auto-generated destructor stub No newline at end of file |
|
|
18 | 18 | } No newline at end of file |
|
|
19 | 19 | |
|
|
20 | No newline at end of file | |
|
|
20 | void nmeaInfo::code_init_INFO() | |
|
|
No newline at end of file | ||
|
|
21 | No newline at end of file | |
|
|
21 | { | |
|
|
No newline at end of file | ||
|
|
22 | No newline at end of file | |
|
|
22 | ||
|
|
No newline at end of file | ||
|
|
23 | No newline at end of file | |
|
|
23 | code_time_now(); No newline at end of file | |
|
|
24 | 24 | sig = NMEA_SIG_BAD; No newline at end of file |
|
|
25 | 25 | fix = NMEA_FIX_BAD; No newline at end of file |
|
|
26 | 26 | } No newline at end of file |
|
|
27 | 27 | |
|
|
28 | No newline at end of file | |
|
|
28 | static void nmeaInfo::code_time_now() { No newline at end of file | |
|
|
29 | 29 | time_t lt; No newline at end of file |
|
|
30 | 30 | struct tm *tt; No newline at end of file |
|
|
31 | 31 | No newline at end of file |
|
|
32 | 32 | time(<); No newline at end of file |
|
|
33 | 33 | tt = gmtime(<); No newline at end of file |
|
|
34 | 34 | No newline at end of file |
|
|
35 | 35 | utc.year = tt->tm_year; No newline at end of file |
|
|
36 | 36 | utc.mon = tt->tm_mon; No newline at end of file |
|
|
37 | 37 | utc.day = tt->tm_mday; No newline at end of file |
|
|
38 | 38 | utc.hour = tt->tm_hour; No newline at end of file |
|
|
39 | 39 | utc.min = tt->tm_min; No newline at end of file |
|
|
40 | 40 | utc.sec = tt->tm_sec; No newline at end of file |
|
|
41 | 41 | utc.hsec = 0; No newline at end of file |
|
|
42 | 42 | } No newline at end of file |
|
|
43 | 43 | No newline at end of file |
|
|
44 | 44 | /** No newline at end of file |
|
|
45 | 45 | * \brief Convert position from INFO to radians position No newline at end of file |
|
|
46 | 46 | */ |
|
|
47 | No newline at end of file | |
|
|
47 | void nmeaInfo::code_info2pos(nmeaPOS *pos) { No newline at end of file | |
|
|
48 | 48 | pos->lat = code_ndeg2radian(lat); No newline at end of file |
|
|
49 | 49 | pos->lon = code_ndeg2radian(lon); No newline at end of file |
|
|
50 | 50 | } No newline at end of file |
|
|
51 | 51 | No newline at end of file |
|
|
52 | 52 | /** No newline at end of file |
|
|
53 | 53 | * \brief Convert radians position to INFOs position No newline at end of file |
|
|
54 | 54 | */ |
|
|
55 | No newline at end of file | |
|
|
55 | void nmeaInfo::code_pos2info(const nmeaPOS *pos) { No newline at end of file | |
|
|
56 | 56 | lat = code_radian2ndeg(pos->lat); No newline at end of file |
|
|
57 | 57 | lon = code_radian2ndeg(pos->lon); No newline at end of file |
|
|
58 | 58 | } No newline at end of file |
|
|
59 | 59 | No newline at end of file |
|
|
60 | 60 | No newline at end of file |
|
|
61 | 61 | No newline at end of file |
|
|
62 | 62 | /* No newline at end of file |
|
|
63 | 63 | * NOISE generator No newline at end of file |
|
|
64 | 64 | */ No newline at end of file |
|
|
65 | 65 | No newline at end of file |
|
|
66 | 66 | int nmeaInfo::nmea_igen_noise_init() No newline at end of file |
|
|
67 | 67 | { No newline at end of file |
|
|
68 | 68 | return 1; No newline at end of file |
|
|
69 | 69 | } No newline at end of file |
|
|
70 | 70 | No newline at end of file |
|
|
71 | 71 | int nmeaInfo::nmea_igen_noise_loop() No newline at end of file |
|
|
72 | 72 | { No newline at end of file |
|
|
73 | 73 | int it; No newline at end of file |
|
|
74 | 74 | int in_use; No newline at end of file |
|
|
75 | 75 | No newline at end of file |
|
|
76 | 76 | sig = (int)nmea_random(1, 3); No newline at end of file |
|
|
77 | 77 | PDOP = nmea_random(0, 9); No newline at end of file |
|
|
78 | 78 | HDOP = nmea_random(0, 9); No newline at end of file |
|
|
79 | 79 | VDOP = nmea_random(0, 9); No newline at end of file |
|
|
80 | 80 | fix = (int)nmea_random(2, 3); No newline at end of file |
|
|
81 | 81 | lat = nmea_random(0, 100); No newline at end of file |
|
|
82 | 82 | lon = nmea_random(0, 100); No newline at end of file |
|
|
83 | 83 | speed = nmea_random(0, 100); No newline at end of file |
|
|
84 | 84 | direction = nmea_random(0, 360); No newline at end of file |
|
|
85 | 85 | declination = nmea_random(0, 360); No newline at end of file |
|
|
86 | 86 | elv = (int)nmea_random(-100, 100); No newline at end of file |
|
|
87 | 87 | No newline at end of file |
|
|
88 | 88 | satinfo.inuse = 0; No newline at end of file |
|
|
89 | 89 | satinfo.inview = 0; No newline at end of file |
|
|
90 | 90 | No newline at end of file |
|
|
91 | 91 | for(it = 0; it < 12; ++it) No newline at end of file |
|
|
92 | 92 | { No newline at end of file |
|
|
93 | 93 | satinfo.sat[it].id = it; No newline at end of file |
|
|
94 | 94 | satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3); No newline at end of file |
|
|
95 | 95 | satinfo.sat[it].elv = (int)nmea_random(0, 90); No newline at end of file |
|
|
96 | 96 | satinfo.sat[it].azimuth = (int)nmea_random(0, 359); No newline at end of file |
|
|
97 | 97 | satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40)); No newline at end of file |
|
|
98 | 98 | No newline at end of file |
|
|
99 | 99 | if(in_use) No newline at end of file |
|
|
100 | 100 | satinfo.inuse++; No newline at end of file |
|
|
101 | 101 | if(satinfo.sat[it].sig > 0) No newline at end of file |
|
|
102 | 102 | satinfo.inview++; No newline at end of file |
|
|
103 | 103 | } No newline at end of file |
|
|
104 | 104 | No newline at end of file |
|
|
105 | 105 | return 1; No newline at end of file |
|
|
106 | 106 | } No newline at end of file |
|
|
107 | 107 | No newline at end of file |
|
|
108 | 108 | int nmeaInfo::nmea_igen_noise_reset() No newline at end of file |
|
|
109 | 109 | { No newline at end of file |
|
|
110 | 110 | return 1; No newline at end of file |
|
|
111 | 111 | } No newline at end of file |
|
|
112 | 112 | No newline at end of file |
|
|
113 | 113 | /* No newline at end of file |
|
|
114 | 114 | * STATIC generator No newline at end of file |
|
|
115 | 115 | */ No newline at end of file |
|
|
116 | 116 | No newline at end of file |
|
|
117 | 117 | int nmeaInfo::nmea_igen_static_loop() No newline at end of file |
|
|
118 | 118 | { |
|
|
119 | No newline at end of file | |
|
|
119 | code_time_now(); No newline at end of file | |
|
|
120 | 120 | return 1; No newline at end of file |
|
|
121 | 121 | }; No newline at end of file |
|
|
122 | 122 | No newline at end of file |
|
|
123 | 123 | int nmeaInfo::nmea_igen_static_reset() No newline at end of file |
|
|
124 | 124 | { No newline at end of file |
|
|
125 | 125 | satinfo.inuse = 4; No newline at end of file |
|
|
126 | 126 | satinfo.inview = 4; No newline at end of file |
|
|
127 | 127 | No newline at end of file |
|
|
128 | 128 | satinfo.sat[0].id = 1; No newline at end of file |
|
|
129 | 129 | satinfo.sat[0].in_use = 1; No newline at end of file |
|
|
130 | 130 | satinfo.sat[0].elv = 50; No newline at end of file |
|
|
131 | 131 | satinfo.sat[0].azimuth = 0; No newline at end of file |
|
|
132 | 132 | satinfo.sat[0].sig = 99; No newline at end of file |
|
|
133 | 133 | No newline at end of file |
|
|
134 | 134 | satinfo.sat[1].id = 2; No newline at end of file |
|
|
135 | 135 | satinfo.sat[1].in_use = 1; No newline at end of file |
|
|
136 | 136 | satinfo.sat[1].elv = 50; No newline at end of file |
|
|
137 | 137 | satinfo.sat[1].azimuth = 90; No newline at end of file |
|
|
138 | 138 | satinfo.sat[1].sig = 99; No newline at end of file |
|
|
139 | 139 | No newline at end of file |
|
|
140 | 140 | satinfo.sat[2].id = 3; No newline at end of file |
|
|
141 | 141 | satinfo.sat[2].in_use = 1; No newline at end of file |
|
|
142 | 142 | satinfo.sat[2].elv = 50; No newline at end of file |
|
|
143 | 143 | satinfo.sat[2].azimuth = 180; No newline at end of file |
|
|
144 | 144 | satinfo.sat[2].sig = 99; No newline at end of file |
|
|
145 | 145 | No newline at end of file |
|
|
146 | 146 | satinfo.sat[3].id = 4; No newline at end of file |
|
|
147 | 147 | satinfo.sat[3].in_use = 1; No newline at end of file |
|
|
148 | 148 | satinfo.sat[3].elv = 50; No newline at end of file |
|
|
149 | 149 | satinfo.sat[3].azimuth = 270; No newline at end of file |
|
|
150 | 150 | satinfo.sat[3].sig = 99; No newline at end of file |
|
|
151 | 151 | No newline at end of file |
|
|
152 | 152 | return 1; No newline at end of file |
|
|
153 | 153 | } No newline at end of file |
|
|
154 | 154 | No newline at end of file |
|
|
155 | 155 | int nmeaInfo::nmea_igen_static_init() No newline at end of file |
|
|
156 | 156 | { No newline at end of file |
|
|
157 | 157 | sig = 3; No newline at end of file |
|
|
158 | 158 | fix = 3; No newline at end of file |
|
|
159 | 159 | No newline at end of file |
|
|
160 | 160 | nmea_igen_static_reset(); No newline at end of file |
|
|
161 | 161 | No newline at end of file |
|
|
162 | 162 | return 1; No newline at end of file |
|
|
163 | 163 | } No newline at end of file |
|
|
164 | 164 | No newline at end of file |
|
|
165 | 165 | /* No newline at end of file |
|
|
166 | 166 | * SAT_ROTATE generator No newline at end of file |
|
|
167 | 167 | */ No newline at end of file |
|
|
168 | 168 | No newline at end of file |
|
|
169 | 169 | int nmeaInfo::nmea_igen_rotate_loop() No newline at end of file |
|
|
170 | 170 | { No newline at end of file |
|
|
171 | 171 | int it; No newline at end of file |
|
|
172 | 172 | int count = satinfo.inview; No newline at end of file |
|
|
173 | 173 | double deg = 360 / (count?count:1); No newline at end of file |
|
|
174 | 174 | double srt = (count?(satinfo.sat[0].azimuth):0) + 5; No newline at end of file |
|
|
175 | 175 | |
|
|
176 | No newline at end of file | |
|
|
176 | code_time_now(); No newline at end of file | |
|
|
177 | 177 | No newline at end of file |
|
|
178 | 178 | for(it = 0; it < count; ++it) No newline at end of file |
|
|
179 | 179 | { No newline at end of file |
|
|
180 | 180 | satinfo.sat[it].azimuth = No newline at end of file |
|
|
181 | 181 | (int)((srt >= 360)?srt - 360:srt); No newline at end of file |
|
|
182 | 182 | srt += deg; No newline at end of file |
|
|
183 | 183 | } No newline at end of file |
|
|
184 | 184 | No newline at end of file |
|
|
185 | 185 | return 1; No newline at end of file |
|
|
186 | 186 | }; No newline at end of file |
|
|
187 | 187 | No newline at end of file |
|
|
188 | 188 | int nmeaInfo::nmea_igen_rotate_reset() No newline at end of file |
|
|
189 | 189 | { No newline at end of file |
|
|
190 | 190 | int it; No newline at end of file |
|
|
191 | 191 | double deg = 360 / 8; No newline at end of file |
|
|
192 | 192 | double srt = 0; No newline at end of file |
|
|
193 | 193 | No newline at end of file |
|
|
194 | 194 | satinfo.inuse = 8; No newline at end of file |
|
|
195 | 195 | satinfo.inview = 8; No newline at end of file |
|
|
196 | 196 | No newline at end of file |
|
|
197 | 197 | for(it = 0; it < satinfo.inview; ++it) No newline at end of file |
|
|
198 | 198 | { No newline at end of file |
|
|
199 | 199 | satinfo.sat[it].id = it + 1; No newline at end of file |
|
|
200 | 200 | satinfo.sat[it].in_use = 1; No newline at end of file |
|
|
201 | 201 | satinfo.sat[it].elv = 5; No newline at end of file |
|
|
202 | 202 | satinfo.sat[it].azimuth = (int)srt; No newline at end of file |
|
|
203 | 203 | satinfo.sat[it].sig = 80; No newline at end of file |
|
|
204 | 204 | srt += deg; No newline at end of file |
|
|
205 | 205 | } No newline at end of file |
|
|
206 | 206 | No newline at end of file |
|
|
207 | 207 | return 1; No newline at end of file |
|
|
208 | 208 | } No newline at end of file |
|
|
209 | 209 | No newline at end of file |
|
|
210 | 210 | int nmeaInfo::nmea_igen_rotate_init() No newline at end of file |
|
|
211 | 211 | { No newline at end of file |
|
|
212 | 212 | sig = 3; No newline at end of file |
|
|
213 | 213 | fix = 3; No newline at end of file |
|
|
214 | 214 | No newline at end of file |
|
|
215 | 215 | nmea_igen_rotate_reset(); No newline at end of file |
|
|
216 | 216 | No newline at end of file |
|
|
217 | 217 | return 1; No newline at end of file |
|
|
218 | 218 | } No newline at end of file |
|
|
219 | 219 | No newline at end of file |
|
|
220 | 220 | /* No newline at end of file |
|
|
221 | 221 | * POS_RANDMOVE generator No newline at end of file |
|
|
222 | 222 | */ No newline at end of file |
|
|
223 | 223 | No newline at end of file |
|
|
224 | 224 | int nmeaInfo::nmea_igen_pos_rmove_init() No newline at end of file |
|
|
225 | 225 | { No newline at end of file |
|
|
226 | 226 | sig = 3; No newline at end of file |
|
|
227 | 227 | fix = 3; No newline at end of file |
|
|
228 | 228 | direction = declination = 0; No newline at end of file |
|
|
229 | 229 | speed = 20; No newline at end of file |
|
|
230 | 230 | return 1; No newline at end of file |
|
|
231 | 231 | } No newline at end of file |
|
|
232 | 232 | No newline at end of file |
|
|
233 | 233 | int nmeaInfo::nmea_igen_pos_rmove_loop() No newline at end of file |
|
|
234 | 234 | { No newline at end of file |
|
|
235 | 235 | nmeaPOS crd; No newline at end of file |
|
|
236 | 236 | No newline at end of file |
|
|
237 | 237 | direction += nmea_random(-10, 10); No newline at end of file |
|
|
238 | 238 | speed += nmea_random(-2, 3); No newline at end of file |
|
|
239 | 239 | No newline at end of file |
|
|
240 | 240 | if(direction < 0) No newline at end of file |
|
|
241 | 241 | direction = 359 + direction; No newline at end of file |
|
|
242 | 242 | if(direction > 359) No newline at end of file |
|
|
243 | 243 | direction -= 359; No newline at end of file |
|
|
244 | 244 | No newline at end of file |
|
|
245 | 245 | if(speed > 40) No newline at end of file |
|
|
246 | 246 | speed = 40; No newline at end of file |
|
|
247 | 247 | if(speed < 1) No newline at end of file |
|
|
248 | 248 | speed = 1; No newline at end of file |
|
|
249 | 249 | |
|
|
250 | No newline at end of file | |
|
|
250 | code_info2pos(&crd); No newline at end of file | |
|
|
251 | 251 | code_move_horz(&crd, &crd, direction, speed / 3600); |
|
|
252 | No newline at end of file | |
|
|
252 | code_pos2info(&crd); No newline at end of file | |
|
|
253 | 253 | No newline at end of file |
|
|
254 | 254 | declination = direction; No newline at end of file |
|
|
255 | 255 | No newline at end of file |
|
|
256 | 256 | return 1; No newline at end of file |
|
|
257 | 257 | }; No newline at end of file |
| @@ -1,84 +1,92 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * nmeaInfo.h No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #ifndef NMEALIB___NMEAINFO_H_ No newline at end of file |
|
|
9 | 9 | #define NMEALIB___NMEAINFO_H_ No newline at end of file |
|
|
10 | 10 | No newline at end of file |
|
|
11 | 11 | #include "nmea_defs.h" No newline at end of file |
|
|
12 | 12 | /** No newline at end of file |
|
|
13 | 13 | * Information about satellite No newline at end of file |
|
|
14 | 14 | * @see nmeaSATINFO No newline at end of file |
|
|
15 | 15 | * @see nmeaGPGSV No newline at end of file |
|
|
16 | 16 | */ No newline at end of file |
|
|
17 | 17 | typedef struct _nmeaSATELLITE No newline at end of file |
|
|
18 | 18 | { No newline at end of file |
|
|
19 | 19 | int id; /**< Satellite PRN number */ No newline at end of file |
|
|
20 | 20 | int in_use; /**< Used in position fix */ No newline at end of file |
|
|
21 | 21 | int elv; /**< Elevation in degrees, 90 maximum */ No newline at end of file |
|
|
22 | 22 | int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */ No newline at end of file |
|
|
23 | 23 | int sig; /**< Signal, 00-99 dB */ No newline at end of file |
|
|
24 | 24 | No newline at end of file |
|
|
25 | 25 | } nmeaSATELLITE; No newline at end of file |
|
|
26 | 26 | No newline at end of file |
|
|
27 | 27 | /** No newline at end of file |
|
|
28 | 28 | * Information about all satellites in view No newline at end of file |
|
|
29 | 29 | * @see nmeaINFO No newline at end of file |
|
|
30 | 30 | * @see nmeaGPGSV No newline at end of file |
|
|
31 | 31 | */ No newline at end of file |
|
|
32 | 32 | typedef struct _nmeaSATINFO No newline at end of file |
|
|
33 | 33 | { No newline at end of file |
|
|
34 | 34 | int inuse; /**< Number of satellites in use (not those in view) */ No newline at end of file |
|
|
35 | 35 | int inview; /**< Total number of satellites in view */ No newline at end of file |
|
|
36 | 36 | nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */ No newline at end of file |
|
|
37 | 37 | No newline at end of file |
|
|
38 | 38 | } nmeaSATINFO; No newline at end of file |
|
|
39 | 39 | No newline at end of file |
|
|
40 | 40 | /** No newline at end of file |
|
|
41 | 41 | * Summary GPS information from all parsed packets, No newline at end of file |
|
|
42 | 42 | * used also for generating NMEA stream No newline at end of file |
|
|
43 | 43 | * @see nmea_parse No newline at end of file |
|
|
44 | 44 | * @see nmea_GPGGA2info, nmea_...2info No newline at end of file |
|
|
45 | 45 | */ No newline at end of file |
|
|
46 | 46 | class nmeaInfo { No newline at end of file |
|
|
47 | 47 | public: No newline at end of file |
|
|
48 | 48 | nmeaInfo(); No newline at end of file |
|
|
49 | 49 | virtual ~nmeaInfo(); |
|
|
50 | No newline at end of file | |
|
|
50 | void code_init_INFO(); | |
|
|
No newline at end of file | ||
|
|
51 | No newline at end of file | |
|
|
51 | static void code_time_now(); | |
|
|
No newline at end of file | ||
|
|
52 | No newline at end of file | |
|
|
52 | void code_info2pos(nmeaPOS *pos); | |
|
|
No newline at end of file | ||
|
|
53 | No newline at end of file | |
|
|
53 | void code_pos2info(const nmeaPOS *pos); No newline at end of file | |
|
|
54 | 54 | |
|
|
55 | No newline at end of file | |
|
|
55 | int nmea_igen_noise_init(); | |
|
|
No newline at end of file | ||
|
|
56 | No newline at end of file | |
|
|
56 | int nmea_igen_noise_loop(); | |
|
|
No newline at end of file | ||
|
|
57 | No newline at end of file | |
|
|
57 | int nmea_igen_noise_reset(); | |
|
|
No newline at end of file | ||
|
|
58 | No newline at end of file | |
|
|
58 | int nmea_igen_static_loop(); | |
|
|
No newline at end of file | ||
|
|
59 | No newline at end of file | |
|
|
59 | int nmea_igen_static_reset(); | |
|
|
No newline at end of file | ||
|
|
60 | No newline at end of file | |
|
|
60 | int nmea_igen_static_init(); | |
|
|
No newline at end of file | ||
|
|
61 | No newline at end of file | |
|
|
61 | int nmea_igen_rotate_loop(); | |
|
|
No newline at end of file | ||
|
|
62 | No newline at end of file | |
|
|
62 | int nmea_igen_rotate_reset(); | |
|
|
No newline at end of file | ||
|
|
63 | No newline at end of file | |
|
|
63 | int nmea_igen_rotate_init(); | |
|
|
No newline at end of file | ||
|
|
64 | No newline at end of file | |
|
|
64 | int nmea_igen_pos_rmove_init(); | |
|
|
No newline at end of file | ||
|
|
65 | No newline at end of file | |
|
|
65 | int nmea_igen_pos_rmove_loop(); No newline at end of file | |
|
|
66 | 66 | No newline at end of file |
|
|
67 | 67 | private: No newline at end of file |
|
|
68 | 68 | int smask; /**< Mask specifying types of packages from which data have been obtained */ No newline at end of file |
|
|
69 | 69 | nmeaTIME utc; /**< UTC of position */ No newline at end of file |
|
|
70 | 70 | int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ No newline at end of file |
|
|
71 | 71 | int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ No newline at end of file |
|
|
72 | 72 | double PDOP; /**< Position Dilution Of Precision */ No newline at end of file |
|
|
73 | 73 | double HDOP; /**< Horizontal Dilution Of Precision */ No newline at end of file |
|
|
74 | 74 | double VDOP; /**< Vertical Dilution Of Precision */ No newline at end of file |
|
|
75 | 75 | double lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */ No newline at end of file |
|
|
76 | 76 | double lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */ No newline at end of file |
|
|
77 | 77 | double elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */ No newline at end of file |
|
|
78 | 78 | double speed; /**< Speed over the ground in kilometers/hour */ No newline at end of file |
|
|
79 | 79 | double direction; /**< Track angle in degrees True */ No newline at end of file |
|
|
80 | 80 | double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ No newline at end of file |
|
|
81 | char ns; | |
|
|
No newline at end of file | ||
|
|
82 | char ew; | |
|
|
No newline at end of file | ||
|
|
83 | double satinuse; | |
|
|
No newline at end of file | ||
|
|
84 | char elv_units; | |
|
|
No newline at end of file | ||
|
|
85 | double diff; | |
|
|
No newline at end of file | ||
|
|
86 | char diff_units; | |
|
|
No newline at end of file | ||
|
|
87 | double dgps_age; | |
|
|
No newline at end of file | ||
|
|
88 | double dgps_sid; No newline at end of file | |
|
|
81 | 89 | nmeaSATINFO satinfo; /**< Satellites information */ No newline at end of file |
|
|
82 | 90 | }; No newline at end of file |
|
|
83 | 91 | No newline at end of file |
|
|
84 | 92 | #endif /* NMEALIB___NMEAINFO_H_ */ No newline at end of file |
| @@ -1,46 +1,120 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * nmeaParser.h No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #ifndef NMEALIB_NMEALIB___NMEAPARSER_H_ No newline at end of file |
|
|
9 | 9 | #define NMEALIB_NMEALIB___NMEAPARSER_H_ No newline at end of file |
|
|
10 | 10 | No newline at end of file |
|
|
11 | 11 | typedef struct _nmeaParserNODE No newline at end of file |
|
|
12 | 12 | { No newline at end of file |
|
|
13 | 13 | nmeaCode* pack; No newline at end of file |
|
|
14 | 14 | struct _nmeaParserNODE *next_node; No newline at end of file |
|
|
15 | 15 | No newline at end of file |
|
|
16 | 16 | } nmeaParserNODE; No newline at end of file |
|
|
17 | 17 | No newline at end of file |
|
|
18 | 18 | class nmeaParser { No newline at end of file |
|
|
19 | 19 | public: No newline at end of file |
|
|
20 | 20 | nmeaParser(); No newline at end of file |
|
|
21 | 21 | virtual ~nmeaParser(); No newline at end of file |
|
|
22 | 22 | No newline at end of file |
|
|
23 | 23 | int nmea_find_tail(const char *buff, int buff_sz, int *res_crc) No newline at end of file |
|
|
24 | 24 | int nmea_parse( const char *buff, int buff_sz, nmeaInfo *info ); No newline at end of file |
|
|
25 | 25 | No newline at end of file |
|
|
26 | 26 | int nmea_parser_push(const char *buff, int buff_sz); No newline at end of file |
|
|
27 | 27 | int nmea_parser_top(void); No newline at end of file |
|
|
28 | 28 | int nmea_parser_pop(nmeaCode **pack_ptr); No newline at end of file |
|
|
29 | 29 | int nmea_parser_peek(nmeaCode **pack_ptr); No newline at end of file |
|
|
30 | 30 | int nmea_parser_drop(void); No newline at end of file |
|
|
31 | 31 | int nmea_parser_buff_clear(void); No newline at end of file |
|
|
32 | 32 | int nmea_parser_queue_clear(void); No newline at end of file |
|
|
33 | 33 | |
|
|
34 | No newline at end of file | |
|
|
34 | virtual void code_2info(nmeaCode* code, nmeaInfo* info); No newline at end of file | |
|
|
35 | 35 | No newline at end of file |
|
|
36 | 36 | private: No newline at end of file |
|
|
37 | 37 | void *top_node; No newline at end of file |
|
|
38 | 38 | void *end_node; No newline at end of file |
|
|
39 | 39 | unsigned char *buffer; No newline at end of file |
|
|
40 | 40 | int buff_size; No newline at end of file |
|
|
41 | 41 | int buff_use; No newline at end of file |
|
|
42 | 42 | No newline at end of file |
|
|
43 | 43 | int nmea_parser_real_push(const char *buff, int buff_sz); No newline at end of file |
|
|
44 | 44 | }; No newline at end of file |
|
|
45 | 45 | No newline at end of file |
|
|
46 | ||
|
|
No newline at end of file | ||
|
|
47 | class nmeaGPALMParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
48 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
49 | }; | |
|
|
No newline at end of file | ||
|
|
50 | ||
|
|
No newline at end of file | ||
|
|
51 | //codigos NMEA que se pueden enviar al receptor GPS GARMIN | |
|
|
No newline at end of file | ||
|
|
52 | class nmeaPGRMIParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
53 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
54 | }; | |
|
|
No newline at end of file | ||
|
|
55 | ||
|
|
No newline at end of file | ||
|
|
56 | class nmeaPGRMCParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
57 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
58 | }; | |
|
|
No newline at end of file | ||
|
|
59 | ||
|
|
No newline at end of file | ||
|
|
60 | class nmeaPGRMC1Parser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
61 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
62 | }; | |
|
|
No newline at end of file | ||
|
|
63 | ||
|
|
No newline at end of file | ||
|
|
64 | class nmeaPGRMOParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
65 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
66 | }; | |
|
|
No newline at end of file | ||
|
|
67 | ||
|
|
No newline at end of file | ||
|
|
68 | ||
|
|
No newline at end of file | ||
|
|
69 | //Codigos NMEA que puede transmitir el receptor GNSS | |
|
|
No newline at end of file | ||
|
|
70 | ||
|
|
No newline at end of file | ||
|
|
71 | class nmeaGPGGAParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
72 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
73 | }; | |
|
|
No newline at end of file | ||
|
|
74 | ||
|
|
No newline at end of file | ||
|
|
75 | class nmeaGPGSAParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
76 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
77 | }; | |
|
|
No newline at end of file | ||
|
|
78 | ||
|
|
No newline at end of file | ||
|
|
79 | class nmeaGPGSVParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
80 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
81 | }; | |
|
|
No newline at end of file | ||
|
|
82 | ||
|
|
No newline at end of file | ||
|
|
83 | class nmeaGPRMCParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
84 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
85 | }; | |
|
|
No newline at end of file | ||
|
|
86 | ||
|
|
No newline at end of file | ||
|
|
87 | class nmeaGPVTGParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
88 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
89 | }; | |
|
|
No newline at end of file | ||
|
|
90 | ||
|
|
No newline at end of file | ||
|
|
91 | class nmeaGPGLLParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
92 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
93 | }; | |
|
|
No newline at end of file | ||
|
|
94 | ||
|
|
No newline at end of file | ||
|
|
95 | //codigos de propiedad de GARMIN | |
|
|
No newline at end of file | ||
|
|
96 | class nmeaPGRMEParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
97 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
98 | }; | |
|
|
No newline at end of file | ||
|
|
99 | ||
|
|
No newline at end of file | ||
|
|
100 | class nmeaPGRMFParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
101 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
102 | }; | |
|
|
No newline at end of file | ||
|
|
103 | ||
|
|
No newline at end of file | ||
|
|
104 | class nmeaPGRMMParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
105 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
106 | }; | |
|
|
No newline at end of file | ||
|
|
107 | ||
|
|
No newline at end of file | ||
|
|
108 | class nmeaPGRMTParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
109 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
110 | }; | |
|
|
No newline at end of file | ||
|
|
111 | ||
|
|
No newline at end of file | ||
|
|
112 | class nmeaPGRMVParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
113 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
114 | }; | |
|
|
No newline at end of file | ||
|
|
115 | ||
|
|
No newline at end of file | ||
|
|
116 | class nmeaPGRMBParser:nmeaParser { | |
|
|
No newline at end of file | ||
|
|
117 | void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); | |
|
|
No newline at end of file | ||
|
|
118 | }; | |
|
|
No newline at end of file | ||
|
|
119 | No newline at end of file | |
|
|
46 | 120 | #endif /* NMEALIB_NMEALIB___NMEAPARSER_H_ */ No newline at end of file |
| @@ -1,590 +1,590 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * utils.cpp No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #include <stdlib.h> No newline at end of file |
|
|
9 | 9 | #include <stdarg.h> No newline at end of file |
|
|
10 | 10 | #include "utils.h" No newline at end of file |
|
|
11 | 11 | No newline at end of file |
|
|
12 | 12 | /** No newline at end of file |
|
|
13 | 13 | * \fn code_degree2radian No newline at end of file |
|
|
14 | 14 | * \brief Convert degree to radian No newline at end of file |
|
|
15 | 15 | */ |
|
|
16 | No newline at end of file | |
|
|
16 | double code_degree2radian(double val) { No newline at end of file | |
|
|
17 | 17 | return (val * code_PI180); No newline at end of file |
|
|
18 | 18 | } No newline at end of file |
|
|
19 | 19 | No newline at end of file |
|
|
20 | 20 | /** No newline at end of file |
|
|
21 | 21 | * \fn code_radian2degree No newline at end of file |
|
|
22 | 22 | * \brief Convert radian to degree No newline at end of file |
|
|
23 | 23 | */ |
|
|
24 | No newline at end of file | |
|
|
24 | double code_radian2degree(double val) { No newline at end of file | |
|
|
25 | 25 | return (val / code_PI180); No newline at end of file |
|
|
26 | 26 | } No newline at end of file |
|
|
27 | 27 | No newline at end of file |
|
|
28 | 28 | /** No newline at end of file |
|
|
29 | 29 | * \brief Convert NDEG (NMEA degree) to fractional degree No newline at end of file |
|
|
30 | 30 | */ |
|
|
31 | No newline at end of file | |
|
|
31 | double code_ndeg2degree(double val) { No newline at end of file | |
|
|
32 | 32 | double deg = ((int)(val / 100)); No newline at end of file |
|
|
33 | 33 | val = deg + (val - deg * 100) / 60; No newline at end of file |
|
|
34 | 34 | return val; No newline at end of file |
|
|
35 | 35 | } No newline at end of file |
|
|
36 | 36 | No newline at end of file |
|
|
37 | 37 | /** No newline at end of file |
|
|
38 | 38 | * \brief Convert fractional degree to NDEG (NMEA degree) No newline at end of file |
|
|
39 | 39 | */ |
|
|
40 | No newline at end of file | |
|
|
40 | double code_degree2ndeg(double val) { No newline at end of file | |
|
|
41 | 41 | double int_part; No newline at end of file |
|
|
42 | 42 | double fra_part; No newline at end of file |
|
|
43 | 43 | fra_part = modf(val, &int_part); No newline at end of file |
|
|
44 | 44 | val = int_part * 100 + fra_part * 60; No newline at end of file |
|
|
45 | 45 | return val; No newline at end of file |
|
|
46 | 46 | } No newline at end of file |
|
|
47 | 47 | No newline at end of file |
|
|
48 | 48 | /** No newline at end of file |
|
|
49 | 49 | * \fn code_ndeg2radian No newline at end of file |
|
|
50 | 50 | * \brief Convert NDEG (NMEA degree) to radian No newline at end of file |
|
|
51 | 51 | */ No newline at end of file |
|
|
52 | 52 | double code_ndeg2radian(double val) { |
|
|
53 | No newline at end of file | |
|
|
53 | return code_degree2radian(code_ndeg2degree(val)); No newline at end of file | |
|
|
54 | 54 | } No newline at end of file |
|
|
55 | 55 | No newline at end of file |
|
|
56 | 56 | /** No newline at end of file |
|
|
57 | 57 | * \fn code_radian2ndeg No newline at end of file |
|
|
58 | 58 | * \brief Convert radian to NDEG (NMEA degree) No newline at end of file |
|
|
59 | 59 | */ |
|
|
60 | No newline at end of file | |
|
|
60 | double code_radian2ndeg(double val) { | |
|
|
No newline at end of file | ||
|
|
61 | No newline at end of file | |
|
|
61 | return code_degree2ndeg(code_radian2degree(val)); No newline at end of file | |
|
|
62 | 62 | } No newline at end of file |
|
|
63 | 63 | No newline at end of file |
|
|
64 | 64 | /** No newline at end of file |
|
|
65 | 65 | * \brief Calculate PDOP (Position Dilution Of Precision) factor No newline at end of file |
|
|
66 | 66 | */ |
|
|
67 | No newline at end of file | |
|
|
67 | double code_calc_pdop(double hdop, double vdop) { No newline at end of file | |
|
|
68 | 68 | return sqrt(pow(hdop, 2) + pow(vdop, 2)); No newline at end of file |
|
|
69 | 69 | } No newline at end of file |
|
|
70 | 70 | |
|
|
71 | No newline at end of file | |
|
|
71 | double code_dop2meters(double dop) { No newline at end of file | |
|
|
72 | 72 | return (dop * code_DOP_FACTOR); No newline at end of file |
|
|
73 | 73 | } No newline at end of file |
|
|
74 | 74 | |
|
|
75 | No newline at end of file | |
|
|
75 | double code_meters2dop(double meters) { No newline at end of file | |
|
|
76 | 76 | return (meters / code_DOP_FACTOR); No newline at end of file |
|
|
77 | 77 | } No newline at end of file |
|
|
78 | 78 | No newline at end of file |
|
|
79 | 79 | /** No newline at end of file |
|
|
80 | 80 | * \brief Calculate distance between two points No newline at end of file |
|
|
81 | 81 | * \return Distance in meters No newline at end of file |
|
|
82 | 82 | */ |
|
|
83 | No newline at end of file | |
|
|
83 | double code_distance( No newline at end of file | |
|
|
84 | 84 | const nmeaPOS *from_pos, /**< From position in radians */ No newline at end of file |
|
|
85 | 85 | const nmeaPOS *to_pos /**< To position in radians */ No newline at end of file |
|
|
86 | 86 | ) No newline at end of file |
|
|
87 | 87 | { No newline at end of file |
|
|
88 | 88 | double dist = ((double)code_EARTHRADIUS_M) * acos( No newline at end of file |
|
|
89 | 89 | sin(to_pos->lat) * sin(from_pos->lat) + No newline at end of file |
|
|
90 | 90 | cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon) No newline at end of file |
|
|
91 | 91 | ); No newline at end of file |
|
|
92 | 92 | return dist; No newline at end of file |
|
|
93 | 93 | } No newline at end of file |
|
|
94 | 94 | No newline at end of file |
|
|
95 | 95 | /** No newline at end of file |
|
|
96 | 96 | * \brief Calculate distance between two points No newline at end of file |
|
|
97 | 97 | * This function uses an algorithm for an oblate spheroid earth model. No newline at end of file |
|
|
98 | 98 | * The algorithm is described here: No newline at end of file |
|
|
99 | 99 | * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf No newline at end of file |
|
|
100 | 100 | * \return Distance in meters No newline at end of file |
|
|
101 | 101 | */ |
|
|
102 | No newline at end of file | |
|
|
102 | double code_distance_ellipsoid( No newline at end of file | |
|
|
103 | 103 | const nmeaPOS *from_pos, /**< From position in radians */ No newline at end of file |
|
|
104 | 104 | const nmeaPOS *to_pos, /**< To position in radians */ No newline at end of file |
|
|
105 | 105 | double *from_azimuth, /**< (O) azimuth at "from" position in radians */ No newline at end of file |
|
|
106 | 106 | double *to_azimuth /**< (O) azimuth at "to" position in radians */ No newline at end of file |
|
|
107 | 107 | ) No newline at end of file |
|
|
108 | 108 | { No newline at end of file |
|
|
109 | 109 | /* All variables */ No newline at end of file |
|
|
110 | 110 | double f, a, b, sqr_a, sqr_b; No newline at end of file |
|
|
111 | 111 | double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; No newline at end of file |
|
|
112 | 112 | double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; No newline at end of file |
|
|
113 | 113 | int remaining_steps; No newline at end of file |
|
|
114 | 114 | double sqr_u, A, B, delta_sigma; No newline at end of file |
|
|
115 | 115 | No newline at end of file |
|
|
116 | 116 | /* Check input */ No newline at end of file |
|
|
117 | 117 | NMEA_ASSERT(from_pos != 0); No newline at end of file |
|
|
118 | 118 | NMEA_ASSERT(to_pos != 0); No newline at end of file |
|
|
119 | 119 | No newline at end of file |
|
|
120 | 120 | if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon)) No newline at end of file |
|
|
121 | 121 | { /* Identical points */ No newline at end of file |
|
|
122 | 122 | if ( from_azimuth != 0 ) No newline at end of file |
|
|
123 | 123 | *from_azimuth = 0; No newline at end of file |
|
|
124 | 124 | if ( to_azimuth != 0 ) No newline at end of file |
|
|
125 | 125 | *to_azimuth = 0; No newline at end of file |
|
|
126 | 126 | return 0; No newline at end of file |
|
|
127 | 127 | } /* Identical points */ No newline at end of file |
|
|
128 | 128 | No newline at end of file |
|
|
129 | 129 | /* Earth geometry */ No newline at end of file |
|
|
130 | 130 | f = code_EARTH_FLATTENING; No newline at end of file |
|
|
131 | 131 | a = code_EARTH_SEMIMAJORAXIS_M; No newline at end of file |
|
|
132 | 132 | b = (1 - f) * a; No newline at end of file |
|
|
133 | 133 | sqr_a = a * a; No newline at end of file |
|
|
134 | 134 | sqr_b = b * b; No newline at end of file |
|
|
135 | 135 | No newline at end of file |
|
|
136 | 136 | /* Calculation */ No newline at end of file |
|
|
137 | 137 | L = to_pos->lon - from_pos->lon; No newline at end of file |
|
|
138 | 138 | phi1 = from_pos->lat; No newline at end of file |
|
|
139 | 139 | phi2 = to_pos->lat; No newline at end of file |
|
|
140 | 140 | U1 = atan((1 - f) * tan(phi1)); No newline at end of file |
|
|
141 | 141 | U2 = atan((1 - f) * tan(phi2)); No newline at end of file |
|
|
142 | 142 | sin_U1 = sin(U1); No newline at end of file |
|
|
143 | 143 | sin_U2 = sin(U2); No newline at end of file |
|
|
144 | 144 | cos_U1 = cos(U1); No newline at end of file |
|
|
145 | 145 | cos_U2 = cos(U2); No newline at end of file |
|
|
146 | 146 | No newline at end of file |
|
|
147 | 147 | /* Initialize iteration */ No newline at end of file |
|
|
148 | 148 | sigma = 0; No newline at end of file |
|
|
149 | 149 | sin_sigma = sin(sigma); No newline at end of file |
|
|
150 | 150 | cos_sigma = cos(sigma); No newline at end of file |
|
|
151 | 151 | cos_2_sigmam = 0; No newline at end of file |
|
|
152 | 152 | sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; No newline at end of file |
|
|
153 | 153 | sqr_cos_alpha = 0; No newline at end of file |
|
|
154 | 154 | lambda = L; No newline at end of file |
|
|
155 | 155 | sin_lambda = sin(lambda); No newline at end of file |
|
|
156 | 156 | cos_lambda = cos(lambda); No newline at end of file |
|
|
157 | 157 | delta_lambda = lambda; No newline at end of file |
|
|
158 | 158 | remaining_steps = 20; No newline at end of file |
|
|
159 | 159 | No newline at end of file |
|
|
160 | 160 | while ((delta_lambda > 1e-12) && (remaining_steps > 0)) No newline at end of file |
|
|
161 | 161 | { /* Iterate */ No newline at end of file |
|
|
162 | 162 | /* Variables */ No newline at end of file |
|
|
163 | 163 | double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; No newline at end of file |
|
|
164 | 164 | No newline at end of file |
|
|
165 | 165 | /* Calculation */ No newline at end of file |
|
|
166 | 166 | tmp1 = cos_U2 * sin_lambda; No newline at end of file |
|
|
167 | 167 | tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; No newline at end of file |
|
|
168 | 168 | sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); No newline at end of file |
|
|
169 | 169 | cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; No newline at end of file |
|
|
170 | 170 | tan_sigma = sin_sigma / cos_sigma; No newline at end of file |
|
|
171 | 171 | sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; No newline at end of file |
|
|
172 | 172 | cos_alpha = cos(asin(sin_alpha)); No newline at end of file |
|
|
173 | 173 | sqr_cos_alpha = cos_alpha * cos_alpha; No newline at end of file |
|
|
174 | 174 | cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; No newline at end of file |
|
|
175 | 175 | sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; No newline at end of file |
|
|
176 | 176 | C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); No newline at end of file |
|
|
177 | 177 | lambda_prev = lambda; No newline at end of file |
|
|
178 | 178 | sigma = asin(sin_sigma); No newline at end of file |
|
|
179 | 179 | lambda = L + No newline at end of file |
|
|
180 | 180 | (1 - C) * f * sin_alpha No newline at end of file |
|
|
181 | 181 | * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); No newline at end of file |
|
|
182 | 182 | delta_lambda = lambda_prev - lambda; No newline at end of file |
|
|
183 | 183 | if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; No newline at end of file |
|
|
184 | 184 | sin_lambda = sin(lambda); No newline at end of file |
|
|
185 | 185 | cos_lambda = cos(lambda); No newline at end of file |
|
|
186 | 186 | remaining_steps--; No newline at end of file |
|
|
187 | 187 | } /* Iterate */ No newline at end of file |
|
|
188 | 188 | No newline at end of file |
|
|
189 | 189 | /* More calculation */ No newline at end of file |
|
|
190 | 190 | sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; No newline at end of file |
|
|
191 | 191 | A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); No newline at end of file |
|
|
192 | 192 | B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); No newline at end of file |
|
|
193 | 193 | delta_sigma = B * sin_sigma * ( No newline at end of file |
|
|
194 | 194 | cos_2_sigmam + B / 4 * ( No newline at end of file |
|
|
195 | 195 | cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - No newline at end of file |
|
|
196 | 196 | B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) No newline at end of file |
|
|
197 | 197 | )); No newline at end of file |
|
|
198 | 198 | No newline at end of file |
|
|
199 | 199 | /* Calculate result */ No newline at end of file |
|
|
200 | 200 | if ( from_azimuth != 0 ) No newline at end of file |
|
|
201 | 201 | { No newline at end of file |
|
|
202 | 202 | double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); No newline at end of file |
|
|
203 | 203 | *from_azimuth = atan(tan_alpha_1); No newline at end of file |
|
|
204 | 204 | } No newline at end of file |
|
|
205 | 205 | if ( to_azimuth != 0 ) No newline at end of file |
|
|
206 | 206 | { No newline at end of file |
|
|
207 | 207 | double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); No newline at end of file |
|
|
208 | 208 | *to_azimuth = atan(tan_alpha_2); No newline at end of file |
|
|
209 | 209 | } No newline at end of file |
|
|
210 | 210 | No newline at end of file |
|
|
211 | 211 | return b * A * (sigma - delta_sigma); No newline at end of file |
|
|
212 | 212 | No newline at end of file |
|
|
213 | 213 | } No newline at end of file |
|
|
214 | 214 | /** No newline at end of file |
|
|
215 | 215 | * \brief Horizontal move of point position No newline at end of file |
|
|
216 | 216 | */ |
|
|
217 | No newline at end of file | |
|
|
217 | int code_move_horz( No newline at end of file | |
|
|
218 | 218 | const nmeaPOS *start_pos, /**< Start position in radians */ No newline at end of file |
|
|
219 | 219 | nmeaPOS *end_pos, /**< Result position in radians */ No newline at end of file |
|
|
220 | 220 | double azimuth, /**< Azimuth (degree) [0, 359] */ No newline at end of file |
|
|
221 | 221 | double distance /**< Distance (km) */ No newline at end of file |
|
|
222 | 222 | ) No newline at end of file |
|
|
223 | 223 | { No newline at end of file |
|
|
224 | 224 | nmeaPOS p1 = *start_pos; No newline at end of file |
|
|
225 | 225 | int RetVal = 1; No newline at end of file |
|
|
226 | 226 | No newline at end of file |
|
|
227 | 227 | distance /= code_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ |
|
|
228 | No newline at end of file | |
|
|
228 | azimuth = code_degree2radian(azimuth); No newline at end of file | |
|
|
229 | 229 | No newline at end of file |
|
|
230 | 230 | end_pos->lat = asin( No newline at end of file |
|
|
231 | 231 | sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); No newline at end of file |
|
|
232 | 232 | end_pos->lon = p1.lon + atan2( No newline at end of file |
|
|
233 | 233 | sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat)); No newline at end of file |
|
|
234 | 234 | No newline at end of file |
|
|
235 | 235 | if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) No newline at end of file |
|
|
236 | 236 | { No newline at end of file |
|
|
237 | 237 | end_pos->lat = 0; end_pos->lon = 0; No newline at end of file |
|
|
238 | 238 | RetVal = 0; No newline at end of file |
|
|
239 | 239 | } No newline at end of file |
|
|
240 | 240 | No newline at end of file |
|
|
241 | 241 | return RetVal; No newline at end of file |
|
|
242 | 242 | } No newline at end of file |
|
|
243 | 243 | No newline at end of file |
|
|
244 | 244 | /** No newline at end of file |
|
|
245 | 245 | * \brief Horizontal move of point position No newline at end of file |
|
|
246 | 246 | * This function uses an algorithm for an oblate spheroid earth model. No newline at end of file |
|
|
247 | 247 | * The algorithm is described here: No newline at end of file |
|
|
248 | 248 | * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf No newline at end of file |
|
|
249 | 249 | */ |
|
|
250 | No newline at end of file | |
|
|
250 | int code_move_horz_ellipsoid( No newline at end of file | |
|
|
251 | 251 | const nmeaPOS *start_pos, /**< Start position in radians */ No newline at end of file |
|
|
252 | 252 | nmeaPOS *end_pos, /**< (O) Result position in radians */ No newline at end of file |
|
|
253 | 253 | double azimuth, /**< Azimuth in radians */ No newline at end of file |
|
|
254 | 254 | double distance, /**< Distance (km) */ No newline at end of file |
|
|
255 | 255 | double *end_azimuth /**< (O) Azimuth at end position in radians */ No newline at end of file |
|
|
256 | 256 | ) No newline at end of file |
|
|
257 | 257 | { No newline at end of file |
|
|
258 | 258 | /* Variables */ No newline at end of file |
|
|
259 | 259 | double f, a, b, sqr_a, sqr_b; No newline at end of file |
|
|
260 | 260 | double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1; No newline at end of file |
|
|
261 | 261 | double tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B; No newline at end of file |
|
|
262 | 262 | double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma; No newline at end of file |
|
|
263 | 263 | int remaining_steps; No newline at end of file |
|
|
264 | 264 | double tmp1, phi2, lambda, C, L; No newline at end of file |
|
|
265 | 265 | No newline at end of file |
|
|
266 | 266 | /* Check input */ No newline at end of file |
|
|
267 | 267 | NMEA_ASSERT(start_pos != 0); No newline at end of file |
|
|
268 | 268 | NMEA_ASSERT(end_pos != 0); No newline at end of file |
|
|
269 | 269 | No newline at end of file |
|
|
270 | 270 | if (fabs(distance) < 1e-12) No newline at end of file |
|
|
271 | 271 | { /* No move */ No newline at end of file |
|
|
272 | 272 | *end_pos = *start_pos; No newline at end of file |
|
|
273 | 273 | if ( end_azimuth != 0 ) *end_azimuth = azimuth; No newline at end of file |
|
|
274 | 274 | return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); No newline at end of file |
|
|
275 | 275 | } /* No move */ No newline at end of file |
|
|
276 | 276 | No newline at end of file |
|
|
277 | 277 | /* Earth geometry */ No newline at end of file |
|
|
278 | 278 | f = code_EARTH_FLATTENING; No newline at end of file |
|
|
279 | 279 | a = code_EARTH_SEMIMAJORAXIS_M; No newline at end of file |
|
|
280 | 280 | b = (1 - f) * a; No newline at end of file |
|
|
281 | 281 | sqr_a = a * a; No newline at end of file |
|
|
282 | 282 | sqr_b = b * b; No newline at end of file |
|
|
283 | 283 | No newline at end of file |
|
|
284 | 284 | /* Calculation */ No newline at end of file |
|
|
285 | 285 | phi1 = start_pos->lat; No newline at end of file |
|
|
286 | 286 | tan_U1 = (1 - f) * tan(phi1); No newline at end of file |
|
|
287 | 287 | cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1); No newline at end of file |
|
|
288 | 288 | sin_U1 = tan_U1 * cos_U1; No newline at end of file |
|
|
289 | 289 | s = distance; No newline at end of file |
|
|
290 | 290 | alpha1 = azimuth; No newline at end of file |
|
|
291 | 291 | sin_alpha1 = sin(alpha1); No newline at end of file |
|
|
292 | 292 | cos_alpha1 = cos(alpha1); No newline at end of file |
|
|
293 | 293 | tan_sigma1 = tan_U1 / cos_alpha1; No newline at end of file |
|
|
294 | 294 | sigma1 = atan2(tan_U1, cos_alpha1); No newline at end of file |
|
|
295 | 295 | sin_alpha = cos_U1 * sin_alpha1; No newline at end of file |
|
|
296 | 296 | sqr_cos_alpha = 1 - sin_alpha * sin_alpha; No newline at end of file |
|
|
297 | 297 | cos_alpha = sqrt(sqr_cos_alpha); No newline at end of file |
|
|
298 | 298 | sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; No newline at end of file |
|
|
299 | 299 | A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); No newline at end of file |
|
|
300 | 300 | B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); No newline at end of file |
|
|
301 | 301 | No newline at end of file |
|
|
302 | 302 | /* Initialize iteration */ No newline at end of file |
|
|
303 | 303 | sigma_initial = s / (b * A); No newline at end of file |
|
|
304 | 304 | sigma = sigma_initial; No newline at end of file |
|
|
305 | 305 | sin_sigma = sin(sigma); No newline at end of file |
|
|
306 | 306 | cos_sigma = cos(sigma); No newline at end of file |
|
|
307 | 307 | cos_2_sigmam = cos(2 * sigma1 + sigma); No newline at end of file |
|
|
308 | 308 | sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; No newline at end of file |
|
|
309 | 309 | delta_sigma = 0; No newline at end of file |
|
|
310 | 310 | sigma_prev = 2 * code_PI; No newline at end of file |
|
|
311 | 311 | remaining_steps = 20; No newline at end of file |
|
|
312 | 312 | No newline at end of file |
|
|
313 | 313 | while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0)) No newline at end of file |
|
|
314 | 314 | { /* Iterate */ No newline at end of file |
|
|
315 | 315 | cos_2_sigmam = cos(2 * sigma1 + sigma); No newline at end of file |
|
|
316 | 316 | sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; No newline at end of file |
|
|
317 | 317 | sin_sigma = sin(sigma); No newline at end of file |
|
|
318 | 318 | cos_sigma = cos(sigma); No newline at end of file |
|
|
319 | 319 | delta_sigma = B * sin_sigma * ( No newline at end of file |
|
|
320 | 320 | cos_2_sigmam + B / 4 * ( No newline at end of file |
|
|
321 | 321 | cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - No newline at end of file |
|
|
322 | 322 | B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) No newline at end of file |
|
|
323 | 323 | )); No newline at end of file |
|
|
324 | 324 | sigma_prev = sigma; No newline at end of file |
|
|
325 | 325 | sigma = sigma_initial + delta_sigma; No newline at end of file |
|
|
326 | 326 | remaining_steps --; No newline at end of file |
|
|
327 | 327 | } /* Iterate */ No newline at end of file |
|
|
328 | 328 | No newline at end of file |
|
|
329 | 329 | /* Calculate result */ No newline at end of file |
|
|
330 | 330 | tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1); No newline at end of file |
|
|
331 | 331 | phi2 = atan2( No newline at end of file |
|
|
332 | 332 | sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1, No newline at end of file |
|
|
333 | 333 | (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1) No newline at end of file |
|
|
334 | 334 | ); No newline at end of file |
|
|
335 | 335 | lambda = atan2( No newline at end of file |
|
|
336 | 336 | sin_sigma * sin_alpha1, No newline at end of file |
|
|
337 | 337 | cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1 No newline at end of file |
|
|
338 | 338 | ); No newline at end of file |
|
|
339 | 339 | C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); No newline at end of file |
|
|
340 | 340 | L = lambda - No newline at end of file |
|
|
341 | 341 | (1 - C) * f * sin_alpha * ( No newline at end of file |
|
|
342 | 342 | sigma + C * sin_sigma * No newline at end of file |
|
|
343 | 343 | (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)) No newline at end of file |
|
|
344 | 344 | ); No newline at end of file |
|
|
345 | 345 | No newline at end of file |
|
|
346 | 346 | /* Result */ No newline at end of file |
|
|
347 | 347 | end_pos->lon = start_pos->lon + L; No newline at end of file |
|
|
348 | 348 | end_pos->lat = phi2; No newline at end of file |
|
|
349 | 349 | if ( end_azimuth != 0 ) No newline at end of file |
|
|
350 | 350 | { No newline at end of file |
|
|
351 | 351 | *end_azimuth = atan2( No newline at end of file |
|
|
352 | 352 | sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1 No newline at end of file |
|
|
353 | 353 | ); No newline at end of file |
|
|
354 | 354 | } No newline at end of file |
|
|
355 | 355 | return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); No newline at end of file |
|
|
356 | 356 | } No newline at end of file |
|
|
357 | 357 | No newline at end of file |
|
|
358 | 358 | /** No newline at end of file |
|
|
359 | 359 | * \brief Calculate control sum of binary buffer No newline at end of file |
|
|
360 | 360 | */ |
|
|
361 | No newline at end of file | |
|
|
361 | int code_calc_crc(const char *buff, int buff_sz) No newline at end of file | |
|
|
362 | 362 | { No newline at end of file |
|
|
363 | 363 | int chsum = 0, No newline at end of file |
|
|
364 | 364 | it; No newline at end of file |
|
|
365 | 365 | No newline at end of file |
|
|
366 | 366 | for(it = 0; it < buff_sz; ++it) No newline at end of file |
|
|
367 | 367 | chsum ^= (int)buff[it]; No newline at end of file |
|
|
368 | 368 | No newline at end of file |
|
|
369 | 369 | return chsum; No newline at end of file |
|
|
370 | 370 | } No newline at end of file |
|
|
371 | 371 | No newline at end of file |
|
|
372 | 372 | /** No newline at end of file |
|
|
373 | 373 | * \brief Convert string to number No newline at end of file |
|
|
374 | 374 | */ |
|
|
375 | No newline at end of file | |
|
|
375 | int code_atoi(const char *str, int str_sz, int radix) No newline at end of file | |
|
|
376 | 376 | { No newline at end of file |
|
|
377 | 377 | char *tmp_ptr; No newline at end of file |
|
|
378 | 378 | char buff[NMEA_CONVSTR_BUF]; No newline at end of file |
|
|
379 | 379 | int res = 0; No newline at end of file |
|
|
380 | 380 | No newline at end of file |
|
|
381 | 381 | if(str_sz < NMEA_CONVSTR_BUF) No newline at end of file |
|
|
382 | 382 | { No newline at end of file |
|
|
383 | 383 | memcpy(&buff[0], str, str_sz); No newline at end of file |
|
|
384 | 384 | buff[str_sz] = '\0'; No newline at end of file |
|
|
385 | 385 | res = strtol(&buff[0], &tmp_ptr, radix); No newline at end of file |
|
|
386 | 386 | } No newline at end of file |
|
|
387 | 387 | No newline at end of file |
|
|
388 | 388 | return res; No newline at end of file |
|
|
389 | 389 | } No newline at end of file |
|
|
390 | 390 | No newline at end of file |
|
|
391 | 391 | /** No newline at end of file |
|
|
392 | 392 | * \brief Convert string to fraction number No newline at end of file |
|
|
393 | 393 | */ |
|
|
394 | No newline at end of file | |
|
|
394 | double code_atof(const char *str, int str_sz) No newline at end of file | |
|
|
395 | 395 | { No newline at end of file |
|
|
396 | 396 | char *tmp_ptr; No newline at end of file |
|
|
397 | 397 | char buff[NMEA_CONVSTR_BUF]; No newline at end of file |
|
|
398 | 398 | double res = 0; No newline at end of file |
|
|
399 | 399 | No newline at end of file |
|
|
400 | 400 | if(str_sz < NMEA_CONVSTR_BUF) No newline at end of file |
|
|
401 | 401 | { No newline at end of file |
|
|
402 | 402 | memcpy(&buff[0], str, str_sz); No newline at end of file |
|
|
403 | 403 | buff[str_sz] = '\0'; No newline at end of file |
|
|
404 | 404 | res = strtod(&buff[0], &tmp_ptr); No newline at end of file |
|
|
405 | 405 | } No newline at end of file |
|
|
406 | 406 | No newline at end of file |
|
|
407 | 407 | return res; No newline at end of file |
|
|
408 | 408 | } No newline at end of file |
|
|
409 | 409 | No newline at end of file |
|
|
410 | 410 | /** No newline at end of file |
|
|
411 | 411 | * \brief Formating string (like standart printf) with CRC tail (*CRC) No newline at end of file |
|
|
412 | 412 | */ |
|
|
413 | No newline at end of file | |
|
|
413 | int code_printf(char *buff, int buff_sz, const char *format, ...) No newline at end of file | |
|
|
414 | 414 | { No newline at end of file |
|
|
415 | 415 | int retval, add = 0; No newline at end of file |
|
|
416 | 416 | va_list arg_ptr; No newline at end of file |
|
|
417 | 417 | No newline at end of file |
|
|
418 | 418 | if(buff_sz <= 0) No newline at end of file |
|
|
419 | 419 | return 0; No newline at end of file |
|
|
420 | 420 | No newline at end of file |
|
|
421 | 421 | va_start(arg_ptr, format); No newline at end of file |
|
|
422 | 422 | No newline at end of file |
|
|
423 | 423 | retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr); No newline at end of file |
|
|
424 | 424 | No newline at end of file |
|
|
425 | 425 | if(retval > 0) No newline at end of file |
|
|
426 | 426 | { No newline at end of file |
|
|
427 | 427 | add = NMEA_POSIX(snprintf)( No newline at end of file |
|
|
428 | 428 | buff + retval, buff_sz - retval, "*%02x\r\n", |
|
|
429 | No newline at end of file | |
|
|
429 | code_calc_crc(buff + 1, retval - 1)); No newline at end of file | |
|
|
430 | 430 | } No newline at end of file |
|
|
431 | 431 | No newline at end of file |
|
|
432 | 432 | retval += add; No newline at end of file |
|
|
433 | 433 | No newline at end of file |
|
|
434 | 434 | if(retval < 0 || retval > buff_sz) No newline at end of file |
|
|
435 | 435 | { No newline at end of file |
|
|
436 | 436 | memset(buff, ' ', buff_sz); No newline at end of file |
|
|
437 | 437 | retval = buff_sz; No newline at end of file |
|
|
438 | 438 | } No newline at end of file |
|
|
439 | 439 | No newline at end of file |
|
|
440 | 440 | va_end(arg_ptr); No newline at end of file |
|
|
441 | 441 | No newline at end of file |
|
|
442 | 442 | return retval; No newline at end of file |
|
|
443 | 443 | } No newline at end of file |
|
|
444 | 444 | No newline at end of file |
|
|
445 | 445 | /** No newline at end of file |
|
|
446 | 446 | * \brief Analyse string (specificate for NMEA sentences) No newline at end of file |
|
|
447 | 447 | */ |
|
|
448 | No newline at end of file | |
|
|
448 | int code_scanf(const char *buff, int buff_sz, const char *format, ...) No newline at end of file | |
|
|
449 | 449 | { No newline at end of file |
|
|
450 | 450 | const char *beg_tok; No newline at end of file |
|
|
451 | 451 | const char *end_buf = buff + buff_sz; No newline at end of file |
|
|
452 | 452 | No newline at end of file |
|
|
453 | 453 | va_list arg_ptr; No newline at end of file |
|
|
454 | 454 | int tok_type = NMEA_TOKS_COMPARE; No newline at end of file |
|
|
455 | 455 | int width = 0; No newline at end of file |
|
|
456 | 456 | const char *beg_fmt = 0; No newline at end of file |
|
|
457 | 457 | int snum = 0, unum = 0; No newline at end of file |
|
|
458 | 458 | No newline at end of file |
|
|
459 | 459 | int tok_count = 0; No newline at end of file |
|
|
460 | 460 | void *parg_target; No newline at end of file |
|
|
461 | 461 | No newline at end of file |
|
|
462 | 462 | va_start(arg_ptr, format); No newline at end of file |
|
|
463 | 463 | No newline at end of file |
|
|
464 | 464 | for(; *format && buff < end_buf; ++format) No newline at end of file |
|
|
465 | 465 | { No newline at end of file |
|
|
466 | 466 | switch(tok_type) No newline at end of file |
|
|
467 | 467 | { No newline at end of file |
|
|
468 | 468 | case NMEA_TOKS_COMPARE: No newline at end of file |
|
|
469 | 469 | if('%' == *format) No newline at end of file |
|
|
470 | 470 | tok_type = NMEA_TOKS_PERCENT; No newline at end of file |
|
|
471 | 471 | else if(*buff++ != *format) No newline at end of file |
|
|
472 | 472 | goto fail; No newline at end of file |
|
|
473 | 473 | break; No newline at end of file |
|
|
474 | 474 | case NMEA_TOKS_PERCENT: No newline at end of file |
|
|
475 | 475 | width = 0; No newline at end of file |
|
|
476 | 476 | beg_fmt = format; No newline at end of file |
|
|
477 | 477 | tok_type = NMEA_TOKS_WIDTH; No newline at end of file |
|
|
478 | 478 | case NMEA_TOKS_WIDTH: No newline at end of file |
|
|
479 | 479 | if(isdigit(*format)) No newline at end of file |
|
|
480 | 480 | break; No newline at end of file |
|
|
481 | 481 | { No newline at end of file |
|
|
482 | 482 | tok_type = NMEA_TOKS_TYPE; No newline at end of file |
|
|
483 | 483 | if(format > beg_fmt) |
|
|
484 | No newline at end of file | |
|
|
484 | width = code_atoi(beg_fmt, (int)(format - beg_fmt), 10); No newline at end of file | |
|
|
485 | 485 | } No newline at end of file |
|
|
486 | 486 | case NMEA_TOKS_TYPE: No newline at end of file |
|
|
487 | 487 | beg_tok = buff; No newline at end of file |
|
|
488 | 488 | No newline at end of file |
|
|
489 | 489 | if(!width && ('c' == *format || 'C' == *format) && *buff != format[1]) No newline at end of file |
|
|
490 | 490 | width = 1; No newline at end of file |
|
|
491 | 491 | No newline at end of file |
|
|
492 | 492 | if(width) No newline at end of file |
|
|
493 | 493 | { No newline at end of file |
|
|
494 | 494 | if(buff + width <= end_buf) No newline at end of file |
|
|
495 | 495 | buff += width; No newline at end of file |
|
|
496 | 496 | else No newline at end of file |
|
|
497 | 497 | goto fail; No newline at end of file |
|
|
498 | 498 | } No newline at end of file |
|
|
499 | 499 | else No newline at end of file |
|
|
500 | 500 | { No newline at end of file |
|
|
501 | 501 | if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff)))) No newline at end of file |
|
|
502 | 502 | buff = end_buf; No newline at end of file |
|
|
503 | 503 | } No newline at end of file |
|
|
504 | 504 | No newline at end of file |
|
|
505 | 505 | if(buff > end_buf) No newline at end of file |
|
|
506 | 506 | goto fail; No newline at end of file |
|
|
507 | 507 | No newline at end of file |
|
|
508 | 508 | tok_type = NMEA_TOKS_COMPARE; No newline at end of file |
|
|
509 | 509 | tok_count++; No newline at end of file |
|
|
510 | 510 | No newline at end of file |
|
|
511 | 511 | parg_target = 0; width = (int)(buff - beg_tok); No newline at end of file |
|
|
512 | 512 | No newline at end of file |
|
|
513 | 513 | switch(*format) No newline at end of file |
|
|
514 | 514 | { No newline at end of file |
|
|
515 | 515 | case 'c': No newline at end of file |
|
|
516 | 516 | case 'C': No newline at end of file |
|
|
517 | 517 | parg_target = (void *)va_arg(arg_ptr, char *); No newline at end of file |
|
|
518 | 518 | if(width && 0 != (parg_target)) No newline at end of file |
|
|
519 | 519 | *((char *)parg_target) = *beg_tok; No newline at end of file |
|
|
520 | 520 | break; No newline at end of file |
|
|
521 | 521 | case 's': No newline at end of file |
|
|
522 | 522 | case 'S': No newline at end of file |
|
|
523 | 523 | parg_target = (void *)va_arg(arg_ptr, char *); No newline at end of file |
|
|
524 | 524 | if(width && 0 != (parg_target)) No newline at end of file |
|
|
525 | 525 | { No newline at end of file |
|
|
526 | 526 | memcpy(parg_target, beg_tok, width); No newline at end of file |
|
|
527 | 527 | ((char *)parg_target)[width] = '\0'; No newline at end of file |
|
|
528 | 528 | } No newline at end of file |
|
|
529 | 529 | break; No newline at end of file |
|
|
530 | 530 | case 'f': No newline at end of file |
|
|
531 | 531 | case 'g': No newline at end of file |
|
|
532 | 532 | case 'G': No newline at end of file |
|
|
533 | 533 | case 'e': No newline at end of file |
|
|
534 | 534 | case 'E': No newline at end of file |
|
|
535 | 535 | parg_target = (void *)va_arg(arg_ptr, double *); No newline at end of file |
|
|
536 | 536 | if(width && 0 != (parg_target)) |
|
|
537 | No newline at end of file | |
|
|
537 | *((double *)parg_target) = code_atof(beg_tok, width); No newline at end of file | |
|
|
538 | 538 | break; No newline at end of file |
|
|
539 | 539 | }; No newline at end of file |
|
|
540 | 540 | No newline at end of file |
|
|
541 | 541 | if(parg_target) No newline at end of file |
|
|
542 | 542 | break; No newline at end of file |
|
|
543 | 543 | if(0 == (parg_target = (void *)va_arg(arg_ptr, int *))) No newline at end of file |
|
|
544 | 544 | break; No newline at end of file |
|
|
545 | 545 | if(!width) No newline at end of file |
|
|
546 | 546 | break; No newline at end of file |
|
|
547 | 547 | No newline at end of file |
|
|
548 | 548 | switch(*format) No newline at end of file |
|
|
549 | 549 | { No newline at end of file |
|
|
550 | 550 | case 'd': No newline at end of file |
|
|
551 | 551 | case 'i': |
|
|
552 | No newline at end of file | |
|
|
552 | snum = code_atoi(beg_tok, width, 10); No newline at end of file | |
|
|
553 | 553 | memcpy(parg_target, &snum, sizeof(int)); No newline at end of file |
|
|
554 | 554 | break; No newline at end of file |
|
|
555 | 555 | case 'u': |
|
|
556 | No newline at end of file | |
|
|
556 | unum = code_atoi(beg_tok, width, 10); No newline at end of file | |
|
|
557 | 557 | memcpy(parg_target, &unum, sizeof(unsigned int)); No newline at end of file |
|
|
558 | 558 | break; No newline at end of file |
|
|
559 | 559 | case 'x': No newline at end of file |
|
|
560 | 560 | case 'X': |
|
|
561 | No newline at end of file | |
|
|
561 | unum = code_atoi(beg_tok, width, 16); No newline at end of file | |
|
|
562 | 562 | memcpy(parg_target, &unum, sizeof(unsigned int)); No newline at end of file |
|
|
563 | 563 | break; No newline at end of file |
|
|
564 | 564 | case 'o': |
|
|
565 | No newline at end of file | |
|
|
565 | unum = code_atoi(beg_tok, width, 8); No newline at end of file | |
|
|
566 | 566 | memcpy(parg_target, &unum, sizeof(unsigned int)); No newline at end of file |
|
|
567 | 567 | break; No newline at end of file |
|
|
568 | 568 | default: No newline at end of file |
|
|
569 | 569 | goto fail; No newline at end of file |
|
|
570 | 570 | }; No newline at end of file |
|
|
571 | 571 | No newline at end of file |
|
|
572 | 572 | break; No newline at end of file |
|
|
573 | 573 | }; No newline at end of file |
|
|
574 | 574 | } No newline at end of file |
|
|
575 | 575 | No newline at end of file |
|
|
576 | 576 | fail: No newline at end of file |
|
|
577 | 577 | No newline at end of file |
|
|
578 | 578 | va_end(arg_ptr); No newline at end of file |
|
|
579 | 579 | No newline at end of file |
|
|
580 | 580 | return tok_count; No newline at end of file |
|
|
581 | 581 | } No newline at end of file |
|
|
582 | 582 | No newline at end of file |
|
|
583 | 583 | |
|
|
584 | No newline at end of file | |
|
|
584 | double nmeaGenerator::nmea_random(double min, double max) No newline at end of file | |
|
|
585 | 585 | { No newline at end of file |
|
|
586 | 586 | static double rand_max = RAND_MAX; No newline at end of file |
|
|
587 | 587 | double rand_val = rand(); No newline at end of file |
|
|
588 | 588 | double bounds = max - min; No newline at end of file |
|
|
589 | 589 | return min + (rand_val * bounds) / rand_max; No newline at end of file |
|
|
590 | 590 | } No newline at end of file |
| @@ -1,38 +1,38 | |||
|
|
1 | 1 | /* No newline at end of file |
|
|
2 | 2 | * utils.h No newline at end of file |
|
|
3 | 3 | * No newline at end of file |
|
|
4 | 4 | * Created on: Oct 21, 2014 No newline at end of file |
|
|
5 | 5 | * Author: Alan Aguilar Sologuren No newline at end of file |
|
|
6 | 6 | */ No newline at end of file |
|
|
7 | 7 | No newline at end of file |
|
|
8 | 8 | #ifndef NMEALIB___UTILS_H_ No newline at end of file |
|
|
9 | 9 | #define NMEALIB___UTILS_H_ No newline at end of file |
|
|
10 | 10 | No newline at end of file |
|
|
11 | 11 | #include "nmea_defs.h" No newline at end of file |
|
|
12 | 12 | |
|
|
13 | No newline at end of file | |
|
|
13 | double code_degree2radian(double val); | |
|
|
No newline at end of file | ||
|
|
14 | No newline at end of file | |
|
|
14 | double code_radian2degree(double val); | |
|
|
No newline at end of file | ||
|
|
15 | No newline at end of file | |
|
|
15 | double code_ndeg2degree(double val); | |
|
|
No newline at end of file | ||
|
|
16 | No newline at end of file | |
|
|
16 | double code_degree2ndeg(double val); | |
|
|
No newline at end of file | ||
|
|
17 | No newline at end of file | |
|
|
17 | double code_ndeg2radian(double val); | |
|
|
No newline at end of file | ||
|
|
18 | No newline at end of file | |
|
|
18 | double code_radian2ndeg(double val); | |
|
|
No newline at end of file | ||
|
|
19 | No newline at end of file | |
|
|
19 | double code_calc_pdop(double hdop, double vdop); | |
|
|
No newline at end of file | ||
|
|
20 | No newline at end of file | |
|
|
20 | double code_dop2meters(double dop); | |
|
|
No newline at end of file | ||
|
|
21 | No newline at end of file | |
|
|
21 | double code_meters2dop(double meters); | |
|
|
No newline at end of file | ||
|
|
22 | No newline at end of file | |
|
|
22 | double code_distance(const nmeaPOS *from_pos, const nmeaPOS *to_pos); | |
|
|
No newline at end of file | ||
|
|
23 | No newline at end of file | |
|
|
23 | double code_distance_ellipsoid(const nmeaPOS *from_pos, const nmeaPOS *to_pos, No newline at end of file | |
|
|
24 | 24 | double *from_azimuth, double *to_azimuth); |
|
|
25 | No newline at end of file | |
|
|
25 | int code_move_horz(const nmeaPOS *start_pos, nmeaPOS *end_pos, No newline at end of file | |
|
|
26 | 26 | double azimuth, double distance); |
|
|
27 | No newline at end of file | |
|
|
27 | int code_move_horz_ellipsoid(const nmeaPOS *start_pos, nmeaPOS *end_pos, No newline at end of file | |
|
|
28 | 28 | double azimuth, double distance, double *end_azimuth ); No newline at end of file |
|
|
29 | 29 | |
|
|
30 | No newline at end of file | |
|
|
30 | int code_calc_crc(const char *buff, int buff_sz); | |
|
|
No newline at end of file | ||
|
|
31 | No newline at end of file | |
|
|
31 | int code_atoi(const char *str, int str_sz, int radix); | |
|
|
No newline at end of file | ||
|
|
32 | No newline at end of file | |
|
|
32 | double code_atof(const char *str, int str_sz); | |
|
|
No newline at end of file | ||
|
|
33 | No newline at end of file | |
|
|
33 | int code_printf(char *buff, int buff_sz, const char *format, ...); | |
|
|
No newline at end of file | ||
|
|
34 | No newline at end of file | |
|
|
34 | int code_scanf(const char *buff, int buff_sz, const char *format, ...); No newline at end of file | |
|
|
35 | 35 | |
|
|
36 | No newline at end of file | |
|
|
36 | double nmea_random(double min, double max); No newline at end of file | |
|
|
37 | 37 | No newline at end of file |
|
|
38 | 38 | #endif /* NMEALIB___UTILS_H_ */ No newline at end of file |
General Comments 0
You need to be logged in to leave comments.
Login now