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