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