##// END OF EJS Templates
jro_hard » gen_acq
RSS
codigo fuente de librería NMEAlib++ basado en el codigo original de:...
aaguilar -
r96:97
parent child
Show More
Show file before | Show file after | Hide comments
@@ -0,0 +1,34
1 /*
2 *
3 * NMEA library
4 * URL: http://nmea.sourceforge.net
5 * Author: Tim (xtimor@gmail.com)
6 * Licence: http://www.gnu.org/licenses/lgpl.html
7 * $Id: config.h 17 2008-03-11 11:56:11Z xtimor $
8 *
9 */
10
11 #ifndef __NMEA_CONFIG_H__
12 #define __NMEA_CONFIG_H__
13
14 #define NMEA_VERSION ("0.5.3")
15 #define NMEA_VERSION_MAJOR (0)
16 #define NMEA_VERSION_MINOR (5)
17 #define NMEA_VERSION_PATCH (3)
18
19 #define NMEA_CONVSTR_BUF (256)
20 #define NMEA_TIMEPARSE_BUF (256)
21
22 #define NMEA_UNI
23
24 #define NMEA_POSIX(x) x
25 #define NMEA_INLINE inline
26
27 #if !defined(NDEBUG) && !defined(NMEA_CE)
28 # include <assert.h>
29 # define NMEA_ASSERT(x) assert(x)
30 #else
31 # define NMEA_ASSERT(x)
32 #endif
33
34 #endif /* __NMEA_CONFIG_H__ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,67
1 /*
2 *
3 * NMEA library
4 * URL: http://nmea.sourceforge.net
5 * Author: Tim (xtimor@gmail.com)
6 * Licence: http://www.gnu.org/licenses/lgpl.html
7 * $Id: context.c 17 2008-03-11 11:56:11Z xtimor $
8 *
9 */
10
11 #include "context.h"
12
13 #include <string.h>
14 #include <stdarg.h>
15 #include <stdio.h>
16
17 nmeaPROPERTY * nmea_property()
18 {
19 static nmeaPROPERTY prop = {
20 0, 0, NMEA_DEF_PARSEBUFF
21 };
22
23 return &prop;
24 }
25
26 void nmea_trace(const char *str, ...)
27 {
28 int size;
29 va_list arg_list;
30 char buff[NMEA_DEF_PARSEBUFF];
31 nmeaTraceFunc func = nmea_property()->trace_func;
32
33 if(func)
34 {
35 va_start(arg_list, str);
36 size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list);
37 va_end(arg_list);
38
39 if(size > 0)
40 (*func)(&buff[0], size);
41 }
42 }
43
44 void nmea_trace_buff(const char *buff, int buff_size)
45 {
46 nmeaTraceFunc func = nmea_property()->trace_func;
47 if(func && buff_size)
48 (*func)(buff, buff_size);
49 }
50
51 void nmea_error(const char *str, ...)
52 {
53 int size;
54 va_list arg_list;
55 char buff[NMEA_DEF_PARSEBUFF];
56 nmeaErrorFunc func = nmea_property()->error_func;
57
58 if(func)
59 {
60 va_start(arg_list, str);
61 size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list);
62 va_end(arg_list);
63
64 if(size > 0)
65 (*func)(&buff[0], size);
66 }
67 }
Show file before | Show file after | Hide comments
@@ -0,0 +1,44
1 /*
2 *
3 * NMEA library
4 * URL: http://nmea.sourceforge.net
5 * Author: Tim (xtimor@gmail.com)
6 * Licence: http://www.gnu.org/licenses/lgpl.html
7 * $Id: context.h 4 2007-08-27 13:11:03Z xtimor $
8 *
9 */
10
11 #ifndef __NMEA_CONTEXT_H__
12 #define __NMEA_CONTEXT_H__
13
14 #include "config.h"
15
16 #define NMEA_DEF_PARSEBUFF (1024)
17 #define NMEA_MIN_PARSEBUFF (256)
18
19 #ifdef __cplusplus
20 extern "C" {
21 #endif
22
23 typedef void (*nmeaTraceFunc)(const char *str, int str_size);
24 typedef void (*nmeaErrorFunc)(const char *str, int str_size);
25
26 typedef struct _nmeaPROPERTY
27 {
28 nmeaTraceFunc trace_func;
29 nmeaErrorFunc error_func;
30 int parse_buff_size;
31
32 } nmeaPROPERTY;
33
34 nmeaPROPERTY * nmea_property();
35
36 void nmea_trace(const char *str, ...);
37 void nmea_trace_buff(const char *buff, int buff_size);
38 void nmea_error(const char *str, ...);
39
40 #ifdef __cplusplus
41 }
42 #endif
43
44 #endif /* __NMEA_CONTEXT_H__ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,48
1 /*
2 * nemeaCode.cpp
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #include "nmeaCode.h"
9
10 #include <string.h>
11 #include <stdarg.h>
12 #include <stdio.h>
13 #include <time.h>
14 #include <math.h>
15 #include <float.h>
16 #include <limits.h>
17 #include <ctype.h>
18 #include <stdlib.h>
19
20 nmeaCode::nmeaCode(char* name=nullptr):
21 codebuff(nullptr){
22
23 }
24
25 nmeaCode::~nmeaCode() {
26 // TODO Auto-generated destructor stub
27 }
28
29 virtual void nmeaCode::code_init() {
30
31 }
32 virtual int nmeaCode::code_gen(char *buff, int buff_sz) {
33 return 0;
34 }
35
36 virtual int nmeaCode::code_parse(const char *buff, int buff_sz) {
37 return 0;
38 }
39
40
41 ////////////////////////////////////////////////////////////////////////////////////////
42
43
44
45
46
47
48
Show file before | Show file after | Hide comments
@@ -0,0 +1,32
1 /*
2 * nemeaCode.h
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #ifndef NMEALIB_NMEALIB___NMEACODE_H_
9 #define NMEALIB_NMEALIB___NMEACODE_H_
10
11 #include "nmea_defs.h"
12
13 class nmeaCode {
14 public:
15 nmeaCode(char* name=nullptr);
16 virtual ~nmeaCode();
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
24
25 private:
26 const char codename[8];
27 const int type;
28 char *codebuff;
29
30 };
31
32 #endif /* NMEALIB_NMEALIB___NMEACODE_H_ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,194
1 /*
2 * nmeaGenerator.cpp
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #include <string.h>
9 #include <stdlib.h>
10 #include "nmeaGenerator.h"
11
12
13 nmeaGenerator::nmeaGenerator() {
14 // TODO Auto-generated constructor stub
15
16 }
17
18 nmeaGenerator::~nmeaGenerator() {
19 // TODO Auto-generated destructor stub
20 }
21
22
23 virtual int nmeaGenerator::code_info2nmea(const nmeaInfo *info, nmeaCode* code)
24 {
25 return 1;
26 }
27
28
29 /*
30 * low level
31 */
32
33 int nmeaGenerator::nmea_gen_init(nmeaInfo *info)
34 {
35 int RetVal = 1; int smask = info->smask;
36
37 info->code_init_INFO();
38 info->smask = smask;
39
40 info->lat = NMEA_DEF_LAT;
41 info->lon = NMEA_DEF_LON;
42
43 if(init_call)
44 RetVal = (*init_call)(this, info);
45
46 if(RetVal && next)
47 RetVal = next->nmea_gen_init(info);
48
49 return RetVal;
50 }
51
52 int nmeaGenerator::nmea_gen_loop( nmeaInfo *info)
53 {
54 int RetVal = 1;
55
56 if(loop_call)
57 RetVal = (*loop_call)(this, info);
58
59 if(RetVal && next)
60 RetVal = next->nmea_gen_loop(info);
61
62 return RetVal;
63 }
64
65 int nmeaGenerator::nmea_gen_reset(nmeaInfo *info)
66 {
67 int RetVal = 1;
68
69 if(reset_call)
70 RetVal = (*reset_call)(this, info);
71
72 return RetVal;
73 }
74
75 void nmeaGenerator::nmea_gen_destroy()
76 {
77 if(next)
78 {
79 next->nmea_gen_destroy();
80 next = 0;
81 }
82
83 if(destroy_call)
84 (*destroy_call)(this);
85
86 free(this);
87 }
88
89 void nmeaGenerator::nmea_gen_add(nmeaGenerator *gen)
90 {
91 if(next)
92 next->nmea_gen_add(gen);
93 else
94 next = gen;
95 }
96
97 int nmeaGenerator::nmea_generate_from(
98 char *buff, int buff_sz,
99 nmeaInfo *info,
100 nmeaCode *code
101 )
102 {
103 int retval;
104
105 if(0 != (retval = nmea_gen_loop(info)))
106 retval = nmea_generate(buff, buff_sz, info, code);
107
108 return retval;
109 }
110
111
112
113 int nmeaGenerator::nmea_igen_pos_rmove_destroy(void)
114 {
115 return 1;
116 };
117
118 /*
119 * generator create
120 */
121
122 nmeaGenerator* nmeaGenerator::__nmea_create_generator(int type, nmeaInfo *info)
123 {
124 nmeaGenerator* gen = nullptr;
125 switch(type)
126 {
127 case NMEA_GEN_NOISE:
128 gen = new nmeaGenerator();
129 gen->init_call = &info->nmea_igen_noise_init;
130 gen->loop_call = &info->nmea_igen_noise_loop;
131 gen->reset_call = &info->nmea_igen_noise_reset;
132 break;
133 case NMEA_GEN_STATIC:
134 case NMEA_GEN_SAT_STATIC:
135 gen = new nmeaGenerator();
136 gen->init_call = &info->nmea_igen_static_init;
137 gen->loop_call = &info->nmea_igen_static_loop;
138 gen->reset_call = &info->nmea_igen_static_reset;
139 break;
140 case NMEA_GEN_SAT_ROTATE:
141 gen = new nmeaGenerator();
142 gen->init_call = &info->nmea_igen_rotate_init;
143 gen->loop_call = &info->nmea_igen_rotate_loop;
144 gen->reset_call = &info->nmea_igen_rotate_reset;
145 break;
146 case NMEA_GEN_POS_RANDMOVE:
147 gen = new nmeaGenerator();
148 gen->init_call = &info->nmea_igen_pos_rmove_init;
149 gen->loop_call = &info->nmea_igen_pos_rmove_loop;
150 gen->destroy_call = &nmea_igen_pos_rmove_destroy;
151 break;
152 case NMEA_GEN_ROTATE:
153 gen = new nmeaGenerator();
154 gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info);
155 gen->nmea_gen_add(__nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info));
156 break;
157 };
158 return this;
159 }
160
161 nmeaGenerator * nmeaGenerator::nmea_create_generator(int type, nmeaInfo *info)
162 {
163 nmeaGenerator *gen = __nmea_create_generator(type, info);
164
165 if(gen)
166 gen->nmea_gen_init(info);
167
168 return gen;
169 }
170
171 void nmeaGenerator::nmea_destroy_generator(void)
172 {
173 nmea_gen_destroy();
174 }
175
176
177
178 int nmeaGenerator::nmea_generate(char *buff, int buff_sz, const nmeaInfo *info, nmeaCode *code)
179 {
180 int gen_count = 0;
181
182 if(!buff)
183 return 0;
184
185 while(code)
186 {
187 code_info2nmea(info, code)
188 gen_count += code->code_gen(buff + gen_count, buff_sz - gen_count);
189 code += 1;
190 if(buff_sz - gen_count <= 0)
191 break;
192 }
193 return gen_count;
194 }
Show file before | Show file after | Hide comments
@@ -0,0 +1,70
1 /*
2 * nmeaGenerator.h
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #ifndef NMEALIB_NMEALIB___NMEAGENERATOR_H_
9 #define NMEALIB_NMEALIB___NMEAGENERATOR_H_
10
11 #include "nmea_defs.h"
12 #include "nmeaCode.h"
13 #include "nmeaInfo.h"
14
15 /*
16 * low level
17 */
18
19 typedef int (*nmeaNMEA_GEN_INIT)(nmeaGenerator *gen, nmeaInfo *info);
20 typedef int (*nmeaNMEA_GEN_LOOP)(nmeaGenerator *gen, nmeaInfo *info);
21 typedef int (*nmeaNMEA_GEN_RESET)(nmeaGenerator *gen, nmeaInfo *info);
22 typedef int (*nmeaNMEA_GEN_DESTROY)(nmeaGenerator *gen);
23
24 enum nmeaGENTYPE
25 {
26 NMEA_GEN_NOISE = 0,
27 NMEA_GEN_STATIC,
28 NMEA_GEN_ROTATE,
29
30 NMEA_GEN_SAT_STATIC,
31 NMEA_GEN_SAT_ROTATE,
32 NMEA_GEN_POS_RANDMOVE,
33
34 NMEA_GEN_LAST
35 };
36
37 class nmeaGenerator {
38 public:
39 nmeaGenerator();
40 virtual ~nmeaGenerator();
41
42
43 virtual int code_info2nmea(const nmeaInfo *info, nmeaCode* code);
44
45 int nmea_gen_init(nmeaInfo *info);
46 int nmea_gen_loop(nmeaInfo *info);
47 int nmea_gen_reset(nmeaInfo *info);
48 void nmea_gen_destroy(void);
49 void nmea_gen_add(nmeaGenerator *from);
50 int nmea_generate(char *buff, int buff_sz, const nmeaInfo *info, nmeaCode* code);
51
52 private:
53 void *gen_data;
54 nmeaNMEA_GEN_INIT init_call;
55 nmeaNMEA_GEN_LOOP loop_call;
56 nmeaNMEA_GEN_RESET reset_call;
57 nmeaNMEA_GEN_DESTROY destroy_call;
58 nmeaGenerator *next;
59
60 int nmea_generate_from(char *buff, int buff_sz,nmeaInfo *info, nmeaCode *code);
61 int nmea_igen_pos_rmove_destroy(void);
62
63
64 nmeaGenerator * __nmea_create_generator(int type, nmeaInfo *info);
65 nmeaGenerator * nmea_create_generator(int type, nmeaInfo *info);
66 void nmea_destroy_generator(void);
67
68 };
69
70 #endif /* NMEALIB_NMEALIB___NMEAGENERATOR_H_ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,257
1 /*
2 * nmeaInfo.cpp
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #include "utils.h"
9 #include "nmeaInfo.h"
10
11 nmeaInfo::nmeaInfo() {
12 // TODO Auto-generated constructor stub
13
14 }
15
16 nmeaInfo::~nmeaInfo() {
17 // TODO Auto-generated destructor stub
18 }
19
20 void nmeaInfo::code_init_INFO()
21 {
22
23 code_time_now();
24 sig = NMEA_SIG_BAD;
25 fix = NMEA_FIX_BAD;
26 }
27
28 static void nmeaInfo::code_time_now() {
29 time_t lt;
30 struct tm *tt;
31
32 time(&lt);
33 tt = gmtime(&lt);
34
35 utc.year = tt->tm_year;
36 utc.mon = tt->tm_mon;
37 utc.day = tt->tm_mday;
38 utc.hour = tt->tm_hour;
39 utc.min = tt->tm_min;
40 utc.sec = tt->tm_sec;
41 utc.hsec = 0;
42 }
43
44 /**
45 * \brief Convert position from INFO to radians position
46 */
47 void nmeaInfo::code_info2pos(nmeaPOS *pos) {
48 pos->lat = code_ndeg2radian(lat);
49 pos->lon = code_ndeg2radian(lon);
50 }
51
52 /**
53 * \brief Convert radians position to INFOs position
54 */
55 void nmeaInfo::code_pos2info(const nmeaPOS *pos) {
56 lat = code_radian2ndeg(pos->lat);
57 lon = code_radian2ndeg(pos->lon);
58 }
59
60
61
62 /*
63 * NOISE generator
64 */
65
66 int nmeaInfo::nmea_igen_noise_init()
67 {
68 return 1;
69 }
70
71 int nmeaInfo::nmea_igen_noise_loop()
72 {
73 int it;
74 int in_use;
75
76 sig = (int)nmea_random(1, 3);
77 PDOP = nmea_random(0, 9);
78 HDOP = nmea_random(0, 9);
79 VDOP = nmea_random(0, 9);
80 fix = (int)nmea_random(2, 3);
81 lat = nmea_random(0, 100);
82 lon = nmea_random(0, 100);
83 speed = nmea_random(0, 100);
84 direction = nmea_random(0, 360);
85 declination = nmea_random(0, 360);
86 elv = (int)nmea_random(-100, 100);
87
88 satinfo.inuse = 0;
89 satinfo.inview = 0;
90
91 for(it = 0; it < 12; ++it)
92 {
93 satinfo.sat[it].id = it;
94 satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3);
95 satinfo.sat[it].elv = (int)nmea_random(0, 90);
96 satinfo.sat[it].azimuth = (int)nmea_random(0, 359);
97 satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40));
98
99 if(in_use)
100 satinfo.inuse++;
101 if(satinfo.sat[it].sig > 0)
102 satinfo.inview++;
103 }
104
105 return 1;
106 }
107
108 int nmeaInfo::nmea_igen_noise_reset()
109 {
110 return 1;
111 }
112
113 /*
114 * STATIC generator
115 */
116
117 int nmeaInfo::nmea_igen_static_loop()
118 {
119 code_time_now();
120 return 1;
121 };
122
123 int nmeaInfo::nmea_igen_static_reset()
124 {
125 satinfo.inuse = 4;
126 satinfo.inview = 4;
127
128 satinfo.sat[0].id = 1;
129 satinfo.sat[0].in_use = 1;
130 satinfo.sat[0].elv = 50;
131 satinfo.sat[0].azimuth = 0;
132 satinfo.sat[0].sig = 99;
133
134 satinfo.sat[1].id = 2;
135 satinfo.sat[1].in_use = 1;
136 satinfo.sat[1].elv = 50;
137 satinfo.sat[1].azimuth = 90;
138 satinfo.sat[1].sig = 99;
139
140 satinfo.sat[2].id = 3;
141 satinfo.sat[2].in_use = 1;
142 satinfo.sat[2].elv = 50;
143 satinfo.sat[2].azimuth = 180;
144 satinfo.sat[2].sig = 99;
145
146 satinfo.sat[3].id = 4;
147 satinfo.sat[3].in_use = 1;
148 satinfo.sat[3].elv = 50;
149 satinfo.sat[3].azimuth = 270;
150 satinfo.sat[3].sig = 99;
151
152 return 1;
153 }
154
155 int nmeaInfo::nmea_igen_static_init()
156 {
157 sig = 3;
158 fix = 3;
159
160 nmea_igen_static_reset();
161
162 return 1;
163 }
164
165 /*
166 * SAT_ROTATE generator
167 */
168
169 int nmeaInfo::nmea_igen_rotate_loop()
170 {
171 int it;
172 int count = satinfo.inview;
173 double deg = 360 / (count?count:1);
174 double srt = (count?(satinfo.sat[0].azimuth):0) + 5;
175
176 code_time_now();
177
178 for(it = 0; it < count; ++it)
179 {
180 satinfo.sat[it].azimuth =
181 (int)((srt >= 360)?srt - 360:srt);
182 srt += deg;
183 }
184
185 return 1;
186 };
187
188 int nmeaInfo::nmea_igen_rotate_reset()
189 {
190 int it;
191 double deg = 360 / 8;
192 double srt = 0;
193
194 satinfo.inuse = 8;
195 satinfo.inview = 8;
196
197 for(it = 0; it < satinfo.inview; ++it)
198 {
199 satinfo.sat[it].id = it + 1;
200 satinfo.sat[it].in_use = 1;
201 satinfo.sat[it].elv = 5;
202 satinfo.sat[it].azimuth = (int)srt;
203 satinfo.sat[it].sig = 80;
204 srt += deg;
205 }
206
207 return 1;
208 }
209
210 int nmeaInfo::nmea_igen_rotate_init()
211 {
212 sig = 3;
213 fix = 3;
214
215 nmea_igen_rotate_reset();
216
217 return 1;
218 }
219
220 /*
221 * POS_RANDMOVE generator
222 */
223
224 int nmeaInfo::nmea_igen_pos_rmove_init()
225 {
226 sig = 3;
227 fix = 3;
228 direction = declination = 0;
229 speed = 20;
230 return 1;
231 }
232
233 int nmeaInfo::nmea_igen_pos_rmove_loop()
234 {
235 nmeaPOS crd;
236
237 direction += nmea_random(-10, 10);
238 speed += nmea_random(-2, 3);
239
240 if(direction < 0)
241 direction = 359 + direction;
242 if(direction > 359)
243 direction -= 359;
244
245 if(speed > 40)
246 speed = 40;
247 if(speed < 1)
248 speed = 1;
249
250 code_info2pos(&crd);
251 code_move_horz(&crd, &crd, direction, speed / 3600);
252 code_pos2info(&crd);
253
254 declination = direction;
255
256 return 1;
257 };
Show file before | Show file after | Hide comments
@@ -0,0 +1,84
1 /*
2 * nmeaInfo.h
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #ifndef NMEALIB___NMEAINFO_H_
9 #define NMEALIB___NMEAINFO_H_
10
11 #include "nmea_defs.h"
12 /**
13 * Information about satellite
14 * @see nmeaSATINFO
15 * @see nmeaGPGSV
16 */
17 typedef struct _nmeaSATELLITE
18 {
19 int id; /**< Satellite PRN number */
20 int in_use; /**< Used in position fix */
21 int elv; /**< Elevation in degrees, 90 maximum */
22 int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */
23 int sig; /**< Signal, 00-99 dB */
24
25 } nmeaSATELLITE;
26
27 /**
28 * Information about all satellites in view
29 * @see nmeaINFO
30 * @see nmeaGPGSV
31 */
32 typedef struct _nmeaSATINFO
33 {
34 int inuse; /**< Number of satellites in use (not those in view) */
35 int inview; /**< Total number of satellites in view */
36 nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */
37
38 } nmeaSATINFO;
39
40 /**
41 * Summary GPS information from all parsed packets,
42 * used also for generating NMEA stream
43 * @see nmea_parse
44 * @see nmea_GPGGA2info, nmea_...2info
45 */
46 class nmeaInfo {
47 public:
48 nmeaInfo();
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);
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();
66
67 private:
68 int smask; /**< Mask specifying types of packages from which data have been obtained */
69 nmeaTIME utc; /**< UTC of position */
70 int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */
71 int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */
72 double PDOP; /**< Position Dilution Of Precision */
73 double HDOP; /**< Horizontal Dilution Of Precision */
74 double VDOP; /**< Vertical Dilution Of Precision */
75 double lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */
76 double lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */
77 double elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */
78 double speed; /**< Speed over the ground in kilometers/hour */
79 double direction; /**< Track angle in degrees True */
80 double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */
81 nmeaSATINFO satinfo; /**< Satellites information */
82 };
83
84 #endif /* NMEALIB___NMEAINFO_H_ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,309
1 /*
2 * nmeaParser.cpp
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #include "nmea_defs.h"
9 #include "nmeaParser.h"
10
11 /**
12 * \brief Initialization of parser object
13 */
14 nmeaParser::nmeaParser():
15 top_node(nullptr),end_node(nullptr),
16 buffer(nullptr),buff_size(0),
17 buff_use(0){
18 int buff_sz = nmea_property()->parse_buff_size;
19 NMEA_ASSERT(this);
20
21 if(buff_sz < NMEA_MIN_PARSEBUFF)
22 buff_sz = NMEA_MIN_PARSEBUFF;
23
24 if(0 == (buffer = malloc(buff_size)))
25 nmea_error("Insufficient memory!");
26 delete this;
27 else
28 {
29 buff_size = buff_sz;
30
31 }
32 }
33
34 /**
35 * \brief Destroy parser object
36 */
37 nmeaParser::~nmeaParser() {
38 NMEA_ASSERT(this && buffer);
39 free(buffer);
40 nmea_parser_queue_clear();
41 }
42
43 /**
44 * \brief Analysis of buffer and put results to information structure
45 * @return Number of packets wos parsed
46 */
47 int nmeaParser::nmea_parse(const char *buff, int buff_sz, nmeaInfo *info )
48 {
49 int ptype, nread = 0;
50 nmeaCode *pack = 0;
51
52 NMEA_ASSERT(this && buffer);
53
54 nmea_parser_push(buff, buff_sz);
55
56 while(GPNON != (ptype = nmea_parser_pop(&pack)))
57 {
58 nread++;
59 code_2info(pack, info);
60 free(pack);
61 }
62
63 return nread;
64 }
65
66 /*
67 * low level
68 */
69
70 int nmeaParser::nmea_parser_real_push(const char *buff, int buff_sz)
71 {
72 int nparsed = 0, crc, sen_sz, ptype;
73 nmeaParserNODE *node = 0;
74
75 NMEA_ASSERT(this && buffer);
76
77 /* add */
78 if(buff_use + buff_sz >= buff_size)
79 nmea_parser_buff_clear(parser);
80
81 memcpy(buffer + buff_use, buff, buff_sz);
82 buff_use += buff_sz;
83
84 /* parse */
85 for(;;node = 0)
86 {
87 sen_sz = nmea_find_tail(
88 (const char *)buffer + nparsed,
89 (int)buff_use - nparsed, &crc);
90
91 if(!sen_sz)
92 {
93 if(nparsed)
94 memcpy(
95 buffer,
96 buffer + nparsed,
97 buff_use -= nparsed);
98 break;
99 }
100 else if(crc >= 0)
101 {
102 if(!node->pack->code_parse( (const char *)buffer + nparsed,
103 sen_sz)
104 {
105 free(node);
106 node = 0;
107 }
108
109 if(node)
110 {
111 if(end_node)
112 ((nmeaParserNODE *)end_node)->next_node = node;
113 end_node = node;
114 if(!top_node)
115 top_node = node;
116 node->next_node = 0;
117 }
118 }
119
120 nparsed += sen_sz;
121 }
122 return nparsed;
123 }
124
125 /**
126 * \brief Analysis of buffer and keep results into parser
127 * @return Number of bytes wos parsed from buffer
128 */
129 int nmeaParser::nmea_parser_push(const char *buff, int buff_sz)
130 {
131 int nparse, nparsed = 0;
132
133 do
134 {
135 if(buff_sz > buff_size)
136 nparse = buff_size;
137 else
138 nparse = buff_sz;
139
140 nparsed += nmea_parser_real_push(
141 buff, nparse);
142
143 buff_sz -= nparse;
144
145 } while(buff_sz);
146
147 return nparsed;
148 }
149
150 /**
151 * \brief Get type of top packet keeped into parser
152 * @return Type of packet
153 * @see nmeaPACKTYPE
154 */
155 int nmeaParser::nmea_parser_top()
156 {
157 int retval = GPNON;
158 nmeaParserNODE *node = (nmeaParserNODE *)top_node;
159
160 NMEA_ASSERT(this && buffer);
161
162 if(node)
163 retval = node->pack->type;
164
165 return retval;
166 }
167
168 /**
169 * \brief Withdraw top packet from parser
170 * @return Received packet type
171 * @see nmeaPACKTYPE
172 */
173 int nmeaParser::nmea_parser_pop( nmeaCode **pack_ptr)
174 {
175 int retval = GPNON;
176 nmeaParserNODE *node = (nmeaParserNODE *)top_node;
177
178 NMEA_ASSERT(this && buffer);
179
180 if(node)
181 {
182 *pack_ptr = node->pack;
183 retval = node->pack->type;
184 top_node = node->next_node;
185 if(!top_node)
186 end_node = 0;
187 free(node);
188 }
189 return retval;
190 }
191
192 /**
193 * \brief Get top packet from parser without withdraw
194 * @return Received packet type
195 * @see nmeaPACKTYPE
196 */
197 int nmeaParser::nmea_parser_peek(nmeaCode **pack_ptr)
198 {
199 int retval = GPNON;
200 nmeaParserNODE *node = (nmeaParserNODE *)top_node;
201
202 NMEA_ASSERT(this && buffer);
203
204 if(node)
205 {
206 *pack_ptr = node->pack;
207 retval = node->pack->type;
208 }
209
210 return retval;
211 }
212
213 /**
214 * \brief Delete top packet from parser
215 * @return Deleted packet type
216 * @see nmeaPACKTYPE
217 */
218 int nmeaParser::nmea_parser_drop()
219 {
220 int retval = GPNON;
221 nmeaParserNODE *node = (nmeaParserNODE *)top_node;
222
223 NMEA_ASSERT(this && buffer);
224
225 if(node)
226 {
227 if(node->pack)
228 free(node->pack);
229 retval = node->pack->type;
230 top_node = node->next_node;
231 if(!top_node)
232 end_node = 0;
233 free(node);
234 }
235
236 return retval;
237 }
238
239 /**
240 * \brief Clear cache of parser
241 * @return true (1) - success
242 */
243 int nmeaParser::nmea_parser_buff_clear()
244 {
245 NMEA_ASSERT(this && buffer);
246 buff_use = 0;
247 return 1;
248 }
249
250 /**
251 * \brief Clear packets queue into parser
252 * @return true (1) - success
253 */
254 int nmeaParser::nmea_parser_queue_clear()
255 {
256 NMEA_ASSERT(this);
257 while(top_node)
258 nmea_parser_drop();
259 return 1;
260 }
261
262 virtual void nmeaParser::code_2info(nmeaCode* code, nmeaInfo* info) {
263
264 }
265
266
267 int nmeaParser::nmea_find_tail(const char *buff, int buff_sz, int *res_crc)
268 {
269 static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */;
270
271 const char *end_buff = buff + buff_sz;
272 int nread = 0;
273 int crc = 0;
274
275 NMEA_ASSERT(buff && res_crc);
276
277 *res_crc = -1;
278
279 for(;buff < end_buff; ++buff, ++nread)
280 {
281 if(('$' == *buff) && nread)
282 {
283 buff = 0;
284 break;
285 }
286 else if('*' == *buff)
287 {
288 if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4])
289 {
290 *res_crc = code_atoi(buff + 1, 2, 16);
291 nread = buff_sz - (int)(end_buff - (buff + tail_sz));
292 if(*res_crc != crc)
293 {
294 *res_crc = -1;
295 buff = 0;
296 }
297 }
298
299 break;
300 }
301 else if(nread)
302 crc ^= (int)*buff;
303 }
304
305 if(*res_crc < 0 && buff)
306 nread = 0;
307
308 return nread;
309 }
Show file before | Show file after | Hide comments
@@ -0,0 +1,46
1 /*
2 * nmeaParser.h
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #ifndef NMEALIB_NMEALIB___NMEAPARSER_H_
9 #define NMEALIB_NMEALIB___NMEAPARSER_H_
10
11 typedef struct _nmeaParserNODE
12 {
13 nmeaCode* pack;
14 struct _nmeaParserNODE *next_node;
15
16 } nmeaParserNODE;
17
18 class nmeaParser {
19 public:
20 nmeaParser();
21 virtual ~nmeaParser();
22
23 int nmea_find_tail(const char *buff, int buff_sz, int *res_crc)
24 int nmea_parse( const char *buff, int buff_sz, nmeaInfo *info );
25
26 int nmea_parser_push(const char *buff, int buff_sz);
27 int nmea_parser_top(void);
28 int nmea_parser_pop(nmeaCode **pack_ptr);
29 int nmea_parser_peek(nmeaCode **pack_ptr);
30 int nmea_parser_drop(void);
31 int nmea_parser_buff_clear(void);
32 int nmea_parser_queue_clear(void);
33
34 virtual void code_2info(nmeaCode* code, nmeaInfo* info);
35
36 private:
37 void *top_node;
38 void *end_node;
39 unsigned char *buffer;
40 int buff_size;
41 int buff_use;
42
43 int nmea_parser_real_push(const char *buff, int buff_sz);
44 };
45
46 #endif /* NMEALIB_NMEALIB___NMEAPARSER_H_ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,107
1 /*
2 * nmea_defs.h
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #ifndef NMEALIB_NMEALIB___NMEA_DEFS_H_
9 #define NMEALIB_NMEALIB___NMEA_DEFS_H_
10
11 #include "context.h"
12 /**
13 *
14 */
15 #define code_PI (3.141592653589793) /**< PI value */
16 #define code_PI180 (code_PI / 180) /**< PI division by 180 */
17 #define code_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */
18 #define code_EARTHRADIUS_M (code_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */
19 #define code_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */
20 #define code_EARTH_SEMIMAJORAXIS_KM (code_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */
21 #define code_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */
22 #define code_DOP_FACTOR (5) /**< Factor for translating DOP to meters */
23
24
25 /*
26 * Distance units
27 */
28 #define NMEA_TUD_YARDS (1.0936) /**< Yeards, meter * NMEA_TUD_YARDS = yard */
29 #define NMEA_TUD_KNOTS (1.852) /**< Knots, kilometer / NMEA_TUD_KNOTS = knot */
30 #define NMEA_TUD_MILES (1.609) /**< Miles, kilometer / NMEA_TUD_MILES = mile */
31
32 /*
33 * Speed units
34 */
35 #define NMEA_TUS_MS (3.6) /**< Meters per seconds, (k/h) / NMEA_TUS_MS= (m/s) */
36
37
38 #define NMEA_SIG_BAD (0)
39 #define NMEA_SIG_LOW (1)
40 #define NMEA_SIG_MID (2)
41 #define NMEA_SIG_HIGH (3)
42
43 #define NMEA_FIX_BAD (1)
44 #define NMEA_FIX_2D (2)
45 #define NMEA_FIX_3D (3)
46
47 #define NMEA_MAXSAT (12)
48 #define NMEA_SATINPACK (4)
49 #define NMEA_NSATPACKS (NMEA_MAXSAT / NMEA_SATINPACK)
50
51 #define NMEA_DEF_LAT (5001.2621)
52 #define NMEA_DEF_LON (3613.0595)
53
54 #define NMEA_TOKS_COMPARE (1)
55 #define NMEA_TOKS_PERCENT (2)
56 #define NMEA_TOKS_WIDTH (3)
57 #define NMEA_TOKS_TYPE (4)
58
59
60 /**
61 * NMEA packets type which parsed and generated by library
62 */
63 enum nmeaPACKTYPE
64 {
65 GPNON = 0x0000, /**< Unknown packet type. */
66 GPGGA = 0x0001, /**< GGA - Essential fix data which provide 3D location and accuracy data. */
67 GPGSA = 0x0002, /**< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values. */
68 GPGSV = 0x0004, /**< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values. */
69 GPRMC = 0x0008, /**< RMC - Recommended Minimum Specific GPS/TRANSIT Data. */
70 GPVTG = 0x0010, /**< VTG - Actual track made good and speed over ground. */
71 GPALM = 0x0020,
72 GPGLL = 0x0040,
73 PGRMI = 0x0080,
74 PGRMC = 0x0100,
75 PGRMC1 = 0x0200,
76 PGRMO = 0x0400
77 };
78
79
80 /**
81 * Position data in fractional degrees or radians
82 */
83 typedef struct _nmeaPOS
84 {
85 double lat; /**< Latitude */
86 double lon; /**< Longitude */
87
88 } nmeaPOS;
89
90
91 /**
92 * Date and time data
93 * @see nmea_time_now
94 */
95 typedef struct _nmeaTIME
96 {
97 int year; /**< Years since 1900 */
98 int mon; /**< Months since January - [0,11] */
99 int day; /**< Day of the month - [1,31] */
100 int hour; /**< Hours since midnight - [0,23] */
101 int min; /**< Minutes after the hour - [0,59] */
102 int sec; /**< Seconds after the minute - [0,59] */
103 int hsec; /**< Hundredth part of second - [0,99] */
104
105 } nmeaTIME;
106
107 #endif /* NMEALIB_NMEALIB___NMEA_DEFS_H_ */
Show file before | Show file after | Hide comments
This diff has been collapsed as it changes many lines, (590 lines changed) Show them Hide them
@@ -0,0 +1,590
1 /*
2 * utils.cpp
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #include <stdlib.h>
9 #include <stdarg.h>
10 #include "utils.h"
11
12 /**
13 * \fn code_degree2radian
14 * \brief Convert degree to radian
15 */
16 double code_degree2radian(double val) {
17 return (val * code_PI180);
18 }
19
20 /**
21 * \fn code_radian2degree
22 * \brief Convert radian to degree
23 */
24 double code_radian2degree(double val) {
25 return (val / code_PI180);
26 }
27
28 /**
29 * \brief Convert NDEG (NMEA degree) to fractional degree
30 */
31 double code_ndeg2degree(double val) {
32 double deg = ((int)(val / 100));
33 val = deg + (val - deg * 100) / 60;
34 return val;
35 }
36
37 /**
38 * \brief Convert fractional degree to NDEG (NMEA degree)
39 */
40 double code_degree2ndeg(double val) {
41 double int_part;
42 double fra_part;
43 fra_part = modf(val, &int_part);
44 val = int_part * 100 + fra_part * 60;
45 return val;
46 }
47
48 /**
49 * \fn code_ndeg2radian
50 * \brief Convert NDEG (NMEA degree) to radian
51 */
52 double code_ndeg2radian(double val) {
53 return code_degree2radian(code_ndeg2degree(val));
54 }
55
56 /**
57 * \fn code_radian2ndeg
58 * \brief Convert radian to NDEG (NMEA degree)
59 */
60 double code_radian2ndeg(double val) {
61 return code_degree2ndeg(code_radian2degree(val));
62 }
63
64 /**
65 * \brief Calculate PDOP (Position Dilution Of Precision) factor
66 */
67 double code_calc_pdop(double hdop, double vdop) {
68 return sqrt(pow(hdop, 2) + pow(vdop, 2));
69 }
70
71 double code_dop2meters(double dop) {
72 return (dop * code_DOP_FACTOR);
73 }
74
75 double code_meters2dop(double meters) {
76 return (meters / code_DOP_FACTOR);
77 }
78
79 /**
80 * \brief Calculate distance between two points
81 * \return Distance in meters
82 */
83 double code_distance(
84 const nmeaPOS *from_pos, /**< From position in radians */
85 const nmeaPOS *to_pos /**< To position in radians */
86 )
87 {
88 double dist = ((double)code_EARTHRADIUS_M) * acos(
89 sin(to_pos->lat) * sin(from_pos->lat) +
90 cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)
91 );
92 return dist;
93 }
94
95 /**
96 * \brief Calculate distance between two points
97 * This function uses an algorithm for an oblate spheroid earth model.
98 * The algorithm is described here:
99 * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
100 * \return Distance in meters
101 */
102 double code_distance_ellipsoid(
103 const nmeaPOS *from_pos, /**< From position in radians */
104 const nmeaPOS *to_pos, /**< To position in radians */
105 double *from_azimuth, /**< (O) azimuth at "from" position in radians */
106 double *to_azimuth /**< (O) azimuth at "to" position in radians */
107 )
108 {
109 /* All variables */
110 double f, a, b, sqr_a, sqr_b;
111 double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
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 int remaining_steps;
114 double sqr_u, A, B, delta_sigma;
115
116 /* Check input */
117 NMEA_ASSERT(from_pos != 0);
118 NMEA_ASSERT(to_pos != 0);
119
120 if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon))
121 { /* Identical points */
122 if ( from_azimuth != 0 )
123 *from_azimuth = 0;
124 if ( to_azimuth != 0 )
125 *to_azimuth = 0;
126 return 0;
127 } /* Identical points */
128
129 /* Earth geometry */
130 f = code_EARTH_FLATTENING;
131 a = code_EARTH_SEMIMAJORAXIS_M;
132 b = (1 - f) * a;
133 sqr_a = a * a;
134 sqr_b = b * b;
135
136 /* Calculation */
137 L = to_pos->lon - from_pos->lon;
138 phi1 = from_pos->lat;
139 phi2 = to_pos->lat;
140 U1 = atan((1 - f) * tan(phi1));
141 U2 = atan((1 - f) * tan(phi2));
142 sin_U1 = sin(U1);
143 sin_U2 = sin(U2);
144 cos_U1 = cos(U1);
145 cos_U2 = cos(U2);
146
147 /* Initialize iteration */
148 sigma = 0;
149 sin_sigma = sin(sigma);
150 cos_sigma = cos(sigma);
151 cos_2_sigmam = 0;
152 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
153 sqr_cos_alpha = 0;
154 lambda = L;
155 sin_lambda = sin(lambda);
156 cos_lambda = cos(lambda);
157 delta_lambda = lambda;
158 remaining_steps = 20;
159
160 while ((delta_lambda > 1e-12) && (remaining_steps > 0))
161 { /* Iterate */
162 /* Variables */
163 double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev;
164
165 /* Calculation */
166 tmp1 = cos_U2 * sin_lambda;
167 tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
168 sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);
169 cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
170 tan_sigma = sin_sigma / cos_sigma;
171 sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
172 cos_alpha = cos(asin(sin_alpha));
173 sqr_cos_alpha = cos_alpha * cos_alpha;
174 cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
175 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
176 C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
177 lambda_prev = lambda;
178 sigma = asin(sin_sigma);
179 lambda = L +
180 (1 - C) * f * sin_alpha
181 * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));
182 delta_lambda = lambda_prev - lambda;
183 if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
184 sin_lambda = sin(lambda);
185 cos_lambda = cos(lambda);
186 remaining_steps--;
187 } /* Iterate */
188
189 /* More calculation */
190 sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
191 A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
192 B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
193 delta_sigma = B * sin_sigma * (
194 cos_2_sigmam + B / 4 * (
195 cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
196 B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
197 ));
198
199 /* Calculate result */
200 if ( from_azimuth != 0 )
201 {
202 double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
203 *from_azimuth = atan(tan_alpha_1);
204 }
205 if ( to_azimuth != 0 )
206 {
207 double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
208 *to_azimuth = atan(tan_alpha_2);
209 }
210
211 return b * A * (sigma - delta_sigma);
212
213 }
214 /**
215 * \brief Horizontal move of point position
216 */
217 int code_move_horz(
218 const nmeaPOS *start_pos, /**< Start position in radians */
219 nmeaPOS *end_pos, /**< Result position in radians */
220 double azimuth, /**< Azimuth (degree) [0, 359] */
221 double distance /**< Distance (km) */
222 )
223 {
224 nmeaPOS p1 = *start_pos;
225 int RetVal = 1;
226
227 distance /= code_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
228 azimuth = code_degree2radian(azimuth);
229
230 end_pos->lat = asin(
231 sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
232 end_pos->lon = p1.lon + atan2(
233 sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
234
235 if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon))
236 {
237 end_pos->lat = 0; end_pos->lon = 0;
238 RetVal = 0;
239 }
240
241 return RetVal;
242 }
243
244 /**
245 * \brief Horizontal move of point position
246 * This function uses an algorithm for an oblate spheroid earth model.
247 * The algorithm is described here:
248 * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
249 */
250 int code_move_horz_ellipsoid(
251 const nmeaPOS *start_pos, /**< Start position in radians */
252 nmeaPOS *end_pos, /**< (O) Result position in radians */
253 double azimuth, /**< Azimuth in radians */
254 double distance, /**< Distance (km) */
255 double *end_azimuth /**< (O) Azimuth at end position in radians */
256 )
257 {
258 /* Variables */
259 double f, a, b, sqr_a, sqr_b;
260 double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1;
261 double tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B;
262 double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma;
263 int remaining_steps;
264 double tmp1, phi2, lambda, C, L;
265
266 /* Check input */
267 NMEA_ASSERT(start_pos != 0);
268 NMEA_ASSERT(end_pos != 0);
269
270 if (fabs(distance) < 1e-12)
271 { /* No move */
272 *end_pos = *start_pos;
273 if ( end_azimuth != 0 ) *end_azimuth = azimuth;
274 return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
275 } /* No move */
276
277 /* Earth geometry */
278 f = code_EARTH_FLATTENING;
279 a = code_EARTH_SEMIMAJORAXIS_M;
280 b = (1 - f) * a;
281 sqr_a = a * a;
282 sqr_b = b * b;
283
284 /* Calculation */
285 phi1 = start_pos->lat;
286 tan_U1 = (1 - f) * tan(phi1);
287 cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1);
288 sin_U1 = tan_U1 * cos_U1;
289 s = distance;
290 alpha1 = azimuth;
291 sin_alpha1 = sin(alpha1);
292 cos_alpha1 = cos(alpha1);
293 tan_sigma1 = tan_U1 / cos_alpha1;
294 sigma1 = atan2(tan_U1, cos_alpha1);
295 sin_alpha = cos_U1 * sin_alpha1;
296 sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
297 cos_alpha = sqrt(sqr_cos_alpha);
298 sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
299 A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
300 B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
301
302 /* Initialize iteration */
303 sigma_initial = s / (b * A);
304 sigma = sigma_initial;
305 sin_sigma = sin(sigma);
306 cos_sigma = cos(sigma);
307 cos_2_sigmam = cos(2 * sigma1 + sigma);
308 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
309 delta_sigma = 0;
310 sigma_prev = 2 * code_PI;
311 remaining_steps = 20;
312
313 while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
314 { /* Iterate */
315 cos_2_sigmam = cos(2 * sigma1 + sigma);
316 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
317 sin_sigma = sin(sigma);
318 cos_sigma = cos(sigma);
319 delta_sigma = B * sin_sigma * (
320 cos_2_sigmam + B / 4 * (
321 cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
322 B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
323 ));
324 sigma_prev = sigma;
325 sigma = sigma_initial + delta_sigma;
326 remaining_steps --;
327 } /* Iterate */
328
329 /* Calculate result */
330 tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
331 phi2 = atan2(
332 sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
333 (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
334 );
335 lambda = atan2(
336 sin_sigma * sin_alpha1,
337 cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
338 );
339 C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
340 L = lambda -
341 (1 - C) * f * sin_alpha * (
342 sigma + C * sin_sigma *
343 (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
344 );
345
346 /* Result */
347 end_pos->lon = start_pos->lon + L;
348 end_pos->lat = phi2;
349 if ( end_azimuth != 0 )
350 {
351 *end_azimuth = atan2(
352 sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
353 );
354 }
355 return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
356 }
357
358 /**
359 * \brief Calculate control sum of binary buffer
360 */
361 int code_calc_crc(const char *buff, int buff_sz)
362 {
363 int chsum = 0,
364 it;
365
366 for(it = 0; it < buff_sz; ++it)
367 chsum ^= (int)buff[it];
368
369 return chsum;
370 }
371
372 /**
373 * \brief Convert string to number
374 */
375 int code_atoi(const char *str, int str_sz, int radix)
376 {
377 char *tmp_ptr;
378 char buff[NMEA_CONVSTR_BUF];
379 int res = 0;
380
381 if(str_sz < NMEA_CONVSTR_BUF)
382 {
383 memcpy(&buff[0], str, str_sz);
384 buff[str_sz] = '\0';
385 res = strtol(&buff[0], &tmp_ptr, radix);
386 }
387
388 return res;
389 }
390
391 /**
392 * \brief Convert string to fraction number
393 */
394 double code_atof(const char *str, int str_sz)
395 {
396 char *tmp_ptr;
397 char buff[NMEA_CONVSTR_BUF];
398 double res = 0;
399
400 if(str_sz < NMEA_CONVSTR_BUF)
401 {
402 memcpy(&buff[0], str, str_sz);
403 buff[str_sz] = '\0';
404 res = strtod(&buff[0], &tmp_ptr);
405 }
406
407 return res;
408 }
409
410 /**
411 * \brief Formating string (like standart printf) with CRC tail (*CRC)
412 */
413 int code_printf(char *buff, int buff_sz, const char *format, ...)
414 {
415 int retval, add = 0;
416 va_list arg_ptr;
417
418 if(buff_sz <= 0)
419 return 0;
420
421 va_start(arg_ptr, format);
422
423 retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr);
424
425 if(retval > 0)
426 {
427 add = NMEA_POSIX(snprintf)(
428 buff + retval, buff_sz - retval, "*%02x\r\n",
429 code_calc_crc(buff + 1, retval - 1));
430 }
431
432 retval += add;
433
434 if(retval < 0 || retval > buff_sz)
435 {
436 memset(buff, ' ', buff_sz);
437 retval = buff_sz;
438 }
439
440 va_end(arg_ptr);
441
442 return retval;
443 }
444
445 /**
446 * \brief Analyse string (specificate for NMEA sentences)
447 */
448 int code_scanf(const char *buff, int buff_sz, const char *format, ...)
449 {
450 const char *beg_tok;
451 const char *end_buf = buff + buff_sz;
452
453 va_list arg_ptr;
454 int tok_type = NMEA_TOKS_COMPARE;
455 int width = 0;
456 const char *beg_fmt = 0;
457 int snum = 0, unum = 0;
458
459 int tok_count = 0;
460 void *parg_target;
461
462 va_start(arg_ptr, format);
463
464 for(; *format && buff < end_buf; ++format)
465 {
466 switch(tok_type)
467 {
468 case NMEA_TOKS_COMPARE:
469 if('%' == *format)
470 tok_type = NMEA_TOKS_PERCENT;
471 else if(*buff++ != *format)
472 goto fail;
473 break;
474 case NMEA_TOKS_PERCENT:
475 width = 0;
476 beg_fmt = format;
477 tok_type = NMEA_TOKS_WIDTH;
478 case NMEA_TOKS_WIDTH:
479 if(isdigit(*format))
480 break;
481 {
482 tok_type = NMEA_TOKS_TYPE;
483 if(format > beg_fmt)
484 width = code_atoi(beg_fmt, (int)(format - beg_fmt), 10);
485 }
486 case NMEA_TOKS_TYPE:
487 beg_tok = buff;
488
489 if(!width && ('c' == *format || 'C' == *format) && *buff != format[1])
490 width = 1;
491
492 if(width)
493 {
494 if(buff + width <= end_buf)
495 buff += width;
496 else
497 goto fail;
498 }
499 else
500 {
501 if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff))))
502 buff = end_buf;
503 }
504
505 if(buff > end_buf)
506 goto fail;
507
508 tok_type = NMEA_TOKS_COMPARE;
509 tok_count++;
510
511 parg_target = 0; width = (int)(buff - beg_tok);
512
513 switch(*format)
514 {
515 case 'c':
516 case 'C':
517 parg_target = (void *)va_arg(arg_ptr, char *);
518 if(width && 0 != (parg_target))
519 *((char *)parg_target) = *beg_tok;
520 break;
521 case 's':
522 case 'S':
523 parg_target = (void *)va_arg(arg_ptr, char *);
524 if(width && 0 != (parg_target))
525 {
526 memcpy(parg_target, beg_tok, width);
527 ((char *)parg_target)[width] = '\0';
528 }
529 break;
530 case 'f':
531 case 'g':
532 case 'G':
533 case 'e':
534 case 'E':
535 parg_target = (void *)va_arg(arg_ptr, double *);
536 if(width && 0 != (parg_target))
537 *((double *)parg_target) = code_atof(beg_tok, width);
538 break;
539 };
540
541 if(parg_target)
542 break;
543 if(0 == (parg_target = (void *)va_arg(arg_ptr, int *)))
544 break;
545 if(!width)
546 break;
547
548 switch(*format)
549 {
550 case 'd':
551 case 'i':
552 snum = code_atoi(beg_tok, width, 10);
553 memcpy(parg_target, &snum, sizeof(int));
554 break;
555 case 'u':
556 unum = code_atoi(beg_tok, width, 10);
557 memcpy(parg_target, &unum, sizeof(unsigned int));
558 break;
559 case 'x':
560 case 'X':
561 unum = code_atoi(beg_tok, width, 16);
562 memcpy(parg_target, &unum, sizeof(unsigned int));
563 break;
564 case 'o':
565 unum = code_atoi(beg_tok, width, 8);
566 memcpy(parg_target, &unum, sizeof(unsigned int));
567 break;
568 default:
569 goto fail;
570 };
571
572 break;
573 };
574 }
575
576 fail:
577
578 va_end(arg_ptr);
579
580 return tok_count;
581 }
582
583
584 double nmeaGenerator::nmea_random(double min, double max)
585 {
586 static double rand_max = RAND_MAX;
587 double rand_val = rand();
588 double bounds = max - min;
589 return min + (rand_val * bounds) / rand_max;
590 }
Show file before | Show file after | Hide comments
@@ -0,0 +1,38
1 /*
2 * utils.h
3 *
4 * Created on: Oct 21, 2014
5 * Author: Alan Aguilar Sologuren
6 */
7
8 #ifndef NMEALIB___UTILS_H_
9 #define NMEALIB___UTILS_H_
10
11 #include "nmea_defs.h"
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,
24 double *from_azimuth, double *to_azimuth);
25 int code_move_horz(const nmeaPOS *start_pos, nmeaPOS *end_pos,
26 double azimuth, double distance);
27 int code_move_horz_ellipsoid(const nmeaPOS *start_pos, nmeaPOS *end_pos,
28 double azimuth, double distance, double *end_azimuth );
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, ...);
35
36 double nmea_random(double min, double max);
37
38 #endif /* NMEALIB___UTILS_H_ */
General Comments 0
You need to be logged in to leave comments. Login now