b8441c2836d3564f7b22bc2daca1aab7656c2fae
[olsrd.git] / lib / pud / nmealib / src / generate.c
1 /*
2  * This file is part of nmealib.
3  *
4  * Copyright (c) 2008 Timur Sinitsyn
5  * Copyright (c) 2011 Ferry Huberts
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * This library is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program. If not, see <http://www.gnu.org/licenses/>.
19  */
20
21 #include <nmea/generate.h>
22
23 #include <math.h>
24
25 #include <nmea/tok.h>
26 #include <nmea/units.h>
27
28 int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack)
29 {
30     return nmea_printf(buff, buff_sz,
31         "$GPGGA,%02d%02d%02d.%02d,%09.4f,%C,%010.4f,%C,%1d,%02d,%03.1f,%03.1f,%C,%03.1f,%C,%03.1f,%04d",
32         pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec,
33         pack->lat, pack->ns, pack->lon, pack->ew,
34         pack->sig, pack->satinuse, pack->HDOP, pack->elv, pack->elv_units,
35         pack->diff, pack->diff_units, pack->dgps_age, pack->dgps_sid);
36 }
37
38 int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack)
39 {
40     return nmea_printf(buff, buff_sz,
41         "$GPGSA,%C,%1d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%03.1f,%03.1f,%03.1f",
42         pack->fix_mode, pack->fix_type,
43         pack->sat_prn[0], pack->sat_prn[1], pack->sat_prn[2], pack->sat_prn[3], pack->sat_prn[4], pack->sat_prn[5],
44         pack->sat_prn[6], pack->sat_prn[7], pack->sat_prn[8], pack->sat_prn[9], pack->sat_prn[10], pack->sat_prn[11],
45         pack->PDOP, pack->HDOP, pack->VDOP);
46 }
47
48 int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack)
49 {
50     return nmea_printf(buff, buff_sz,
51         "$GPGSV,%1d,%1d,%02d,"
52         "%02d,%02d,%03d,%02d,"
53         "%02d,%02d,%03d,%02d,"
54         "%02d,%02d,%03d,%02d,"
55         "%02d,%02d,%03d,%02d",
56         pack->pack_count, pack->pack_index + 1, pack->sat_count,
57         pack->sat_data[0].id, pack->sat_data[0].elv, pack->sat_data[0].azimuth, pack->sat_data[0].sig,
58         pack->sat_data[1].id, pack->sat_data[1].elv, pack->sat_data[1].azimuth, pack->sat_data[1].sig,
59         pack->sat_data[2].id, pack->sat_data[2].elv, pack->sat_data[2].azimuth, pack->sat_data[2].sig,
60         pack->sat_data[3].id, pack->sat_data[3].elv, pack->sat_data[3].azimuth, pack->sat_data[3].sig);
61 }
62
63 int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack)
64 {
65     return nmea_printf(buff, buff_sz,
66         "$GPRMC,%02d%02d%02d.%02d,%C,%09.4f,%C,%010.4f,%C,%03.1f,%03.1f,%02d%02d%02d,%03.1f,%C,%C",
67         pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec,
68         pack->status, pack->lat, pack->ns, pack->lon, pack->ew,
69         pack->speed, pack->direction,
70         pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100,
71         pack->declination, pack->declin_ew, pack->mode);
72 }
73
74 int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack)
75 {
76     return nmea_printf(buff, buff_sz,
77         "$GPVTG,%.1f,%C,%.1f,%C,%.1f,%C,%.1f,%C",
78         pack->dir, pack->dir_t,
79         pack->dec, pack->dec_m,
80         pack->spn, pack->spn_n,
81         pack->spk, pack->spk_k);
82 }
83
84 void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack)
85 {
86     nmea_zero_GPGGA(pack);
87
88     pack->utc = info->utc;
89     pack->lat = fabs(info->lat);
90     pack->ns = ((info->lat > 0)?'N':'S');
91     pack->lon = fabs(info->lon);
92     pack->ew = ((info->lon > 0)?'E':'W');
93     pack->sig = info->sig;
94     pack->satinuse = info->satinfo.inuse;
95     pack->HDOP = info->HDOP;
96     pack->elv = info->elv;
97 }
98
99 void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack)
100 {
101     int it;
102
103     nmea_zero_GPGSA(pack);
104
105     pack->fix_type = info->fix;
106     pack->PDOP = info->PDOP;
107     pack->HDOP = info->HDOP;
108     pack->VDOP = info->VDOP;
109
110     for(it = 0; it < NMEA_MAXSAT; ++it)
111     {
112         pack->sat_prn[it] =
113             ((info->satinfo.sat[it].in_use)?info->satinfo.sat[it].id:0);
114     }
115 }
116
117 int nmea_gsv_npack(int sat_count)
118 {
119     int pack_count = lrint(ceil(((double)sat_count) / (double)NMEA_SATINPACK));
120
121     if(0 == pack_count)
122         pack_count = 1;
123
124     return pack_count;
125 }
126
127 void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx)
128 {
129     int sit, pit;
130
131     nmea_zero_GPGSV(pack);
132
133     pack->sat_count = (info->satinfo.inview <= NMEA_MAXSAT)?info->satinfo.inview:NMEA_MAXSAT;
134     pack->pack_count = nmea_gsv_npack(pack->sat_count);
135
136     if(pack->pack_count == 0)
137         pack->pack_count = 1;
138
139     if(pack_idx >= pack->pack_count)
140         pack->pack_index = pack_idx % pack->pack_count;
141     else
142         pack->pack_index = pack_idx;
143
144     for(pit = 0, sit = pack->pack_index * NMEA_SATINPACK; pit < NMEA_SATINPACK; ++pit, ++sit)
145         pack->sat_data[pit] = info->satinfo.sat[sit];
146 }
147
148 void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack)
149 {
150     nmea_zero_GPRMC(pack);
151
152     pack->utc = info->utc;
153     pack->status = ((info->sig > 0)?'A':'V');
154     pack->lat = fabs(info->lat);
155     pack->ns = ((info->lat > 0)?'N':'S');
156     pack->lon = fabs(info->lon);
157     pack->ew = ((info->lon > 0)?'E':'W');
158     pack->speed = info->speed / NMEA_TUD_KNOTS;
159     pack->direction = info->direction;
160     pack->declination = info->declination;
161     pack->declin_ew = 'E';
162     pack->mode = ((info->sig > 0)?'A':'N');
163 }
164
165 void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack)
166 {
167     nmea_zero_GPVTG(pack);
168
169     pack->dir = info->direction;
170     pack->dec = info->declination;
171     pack->spn = info->speed / NMEA_TUD_KNOTS;
172     pack->spk = info->speed;
173 }
174
175 int nmea_generate(
176     char *buff, int buff_sz,
177     const nmeaINFO *info,
178     int generate_mask
179     )
180 {
181     int gen_count = 0, gsv_it, gsv_count;
182     int pack_mask = generate_mask;
183
184     nmeaGPGGA gga;
185     nmeaGPGSA gsa;
186     nmeaGPGSV gsv;
187     nmeaGPRMC rmc;
188     nmeaGPVTG vtg;
189
190     if(!buff)
191         return 0;
192
193     while(pack_mask)
194     {
195         if(pack_mask & GPGGA)
196         {
197             nmea_info2GPGGA(info, &gga);
198             gen_count += nmea_gen_GPGGA(buff + gen_count, buff_sz - gen_count, &gga);
199             pack_mask &= ~GPGGA;
200         }
201         else if(pack_mask & GPGSA)
202         {
203             nmea_info2GPGSA(info, &gsa);
204             gen_count += nmea_gen_GPGSA(buff + gen_count, buff_sz - gen_count, &gsa);
205             pack_mask &= ~GPGSA;
206         }
207         else if(pack_mask & GPGSV)
208         {
209             gsv_count = nmea_gsv_npack(info->satinfo.inview);
210             for(gsv_it = 0; gsv_it < gsv_count && buff_sz - gen_count > 0; ++gsv_it)
211             {
212                 nmea_info2GPGSV(info, &gsv, gsv_it);
213                 gen_count += nmea_gen_GPGSV(buff + gen_count, buff_sz - gen_count, &gsv);
214             }
215             pack_mask &= ~GPGSV;
216         }
217         else if(pack_mask & GPRMC)
218         {
219             nmea_info2GPRMC(info, &rmc);
220             gen_count += nmea_gen_GPRMC(buff + gen_count, buff_sz - gen_count, &rmc);
221             pack_mask &= ~GPRMC;
222         }
223         else if(pack_mask & GPVTG)
224         {
225             nmea_info2GPVTG(info, &vtg);
226             gen_count += nmea_gen_GPVTG(buff + gen_count, buff_sz - gen_count, &vtg);
227             pack_mask &= ~GPVTG;
228         }
229         else
230             break;
231
232         if(buff_sz - gen_count <= 0)
233             break;
234     }
235
236     return gen_count;
237 }