]> git.mxchange.org Git - flightgear.git/blob - src/Network/garmin.cxx
Set the initial log-level back to info
[flightgear.git] / src / Network / garmin.cxx
1 // garmin.cxx -- Garmin protocal class
2 //
3 // Written by Curtis Olson, started November 1999.
4 //
5 // Copyright (C) 1999  Curtis L. Olson - curt@flightgear.org
6 //
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
11 //
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15 // 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, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
20 //
21 // $Id$
22
23
24 #include <simgear/debug/logstream.hxx>
25 #include <simgear/math/sg_geodesy.hxx>
26 #include <simgear/io/iochannel.hxx>
27 #include <simgear/timing/sg_time.hxx>
28
29 #include <FDM/flight.hxx>
30 #include <Main/globals.hxx>
31
32 #include "garmin.hxx"
33
34 SG_USING_NAMESPACE(std);
35
36 FGGarmin::FGGarmin() {
37 }
38
39 FGGarmin::~FGGarmin() {
40 }
41
42
43 // calculate the garmin check sum
44 static char calc_nmea_cksum(char *sentence) {
45     unsigned char sum = 0;
46     int i, len;
47
48     // cout << sentence << endl;
49
50     len = strlen(sentence);
51     sum = sentence[0];
52     for ( i = 1; i < len; i++ ) {
53         // cout << sentence[i];
54         sum ^= sentence[i];
55     }
56     // cout << endl;
57
58     // printf("sum = %02x\n", sum);
59     return sum;
60 }
61
62
63 // generate Garmin message
64 bool FGGarmin::gen_message() {
65     // cout << "generating garmin message" << endl;
66
67     char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256];
68     char dir;
69     int deg;
70     double min;
71
72     SGTime *t = globals->get_time_params();
73
74     char utc[10];
75     sprintf( utc, "%02d%02d%02d", 
76              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
77
78     char lat[20];
79     double latd = cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES;
80     if ( latd < 0.0 ) {
81         latd *= -1.0;
82         dir = 'S';
83     } else {
84         dir = 'N';
85     }
86     deg = (int)(latd);
87     min = (latd - (double)deg) * 60.0;
88     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
89
90     char lon[20];
91     double lond = cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES;
92     if ( lond < 0.0 ) {
93         lond *= -1.0;
94         dir = 'W';
95     } else {
96         dir = 'E';
97     }
98     deg = (int)(lond);
99     min = (lond - (double)deg) * 60.0;
100     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
101
102     char speed[10];
103     sprintf( speed, "%05.1f", cur_fdm_state->get_V_equiv_kts() );
104
105     char heading[10];
106     sprintf( heading, "%05.1f", cur_fdm_state->get_Psi() * SGD_RADIANS_TO_DEGREES );
107
108     char altitude_m[10];
109     sprintf( altitude_m, "%02d", 
110              (int)(cur_fdm_state->get_Altitude() * SG_FEET_TO_METER) );
111
112     char altitude_ft[10];
113     sprintf( altitude_ft, "%02d", (int)cur_fdm_state->get_Altitude() );
114
115     char date[10];
116     sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday, 
117              t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
118
119     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
120     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,000.0,E",
121              utc, lat, lon, speed, heading, date );
122     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
123
124     // sprintf( gga, "$GPGGA,%s,%s,%s,1,04,0.0,%s,M,00.0,M,,*00\r\n",
125     //          utc, lat, lon, altitude_m );
126
127     sprintf( rmz, "PGRMZ,%s,f,3", altitude_ft );
128     sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) );
129
130     SG_LOG( SG_IO, SG_DEBUG, rmc );
131     SG_LOG( SG_IO, SG_DEBUG, rmz );
132
133     string garmin_sentence;
134
135     // RMC sentence
136     garmin_sentence = "$";
137     garmin_sentence += rmc;
138     garmin_sentence += "*";
139     garmin_sentence += rmc_sum;
140     garmin_sentence += "\n";
141
142     // RMZ sentence (garmin proprietary)
143     garmin_sentence += "$";
144     garmin_sentence += rmz;
145     garmin_sentence += "*";
146     garmin_sentence += rmz_sum;
147     garmin_sentence += "\n";
148
149     cout << garmin_sentence;
150
151     length = garmin_sentence.length();
152     strncpy( buf, garmin_sentence.c_str(), length );
153
154     return true;
155 }
156
157
158 // parse Garmin message
159 bool FGGarmin::parse_message() {
160     SG_LOG( SG_IO, SG_INFO, "parse garmin message" );
161
162     string msg = buf;
163     msg = msg.substr( 0, length );
164     SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
165
166     string::size_type begin_line, end_line, begin, end;
167     begin_line = begin = 0;
168
169     // extract out each line
170     end_line = msg.find("\n", begin_line);
171     while ( end_line != string::npos ) {
172         string line = msg.substr(begin_line, end_line - begin_line);
173         begin_line = end_line + 1;
174         SG_LOG( SG_IO, SG_INFO, "  input line = " << line );
175
176         // leading character
177         string start = msg.substr(begin, 1);
178         ++begin;
179         SG_LOG( SG_IO, SG_INFO, "  start = " << start );
180
181         // sentence
182         end = msg.find(",", begin);
183         if ( end == string::npos ) {
184             return false;
185         }
186     
187         string sentence = msg.substr(begin, end - begin);
188         begin = end + 1;
189         SG_LOG( SG_IO, SG_INFO, "  sentence = " << sentence );
190
191         double lon_deg, lon_min, lat_deg, lat_min;
192         double lon, lat, speed, heading, altitude;
193
194         if ( sentence == "GPRMC" ) {
195             // time
196             end = msg.find(",", begin);
197             if ( end == string::npos ) {
198                 return false;
199             }
200     
201             string utc = msg.substr(begin, end - begin);
202             begin = end + 1;
203             SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
204
205             // junk
206             end = msg.find(",", begin);
207             if ( end == string::npos ) {
208                 return false;
209             }
210     
211             string junk = msg.substr(begin, end - begin);
212             begin = end + 1;
213             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
214
215             // lat val
216             end = msg.find(",", begin);
217             if ( end == string::npos ) {
218                 return false;
219             }
220     
221             string lat_str = msg.substr(begin, end - begin);
222             begin = end + 1;
223
224             lat_deg = atof( lat_str.substr(0, 2).c_str() );
225             lat_min = atof( lat_str.substr(2).c_str() );
226
227             // lat dir
228             end = msg.find(",", begin);
229             if ( end == string::npos ) {
230                 return false;
231             }
232     
233             string lat_dir = msg.substr(begin, end - begin);
234             begin = end + 1;
235
236             lat = lat_deg + ( lat_min / 60.0 );
237             if ( lat_dir == "S" ) {
238                 lat *= -1;
239             }
240
241             cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
242             SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
243
244             // lon val
245             end = msg.find(",", begin);
246             if ( end == string::npos ) {
247                 return false;
248             }
249     
250             string lon_str = msg.substr(begin, end - begin);
251             begin = end + 1;
252
253             lon_deg = atof( lon_str.substr(0, 3).c_str() );
254             lon_min = atof( lon_str.substr(3).c_str() );
255
256             // lon dir
257             end = msg.find(",", begin);
258             if ( end == string::npos ) {
259                 return false;
260             }
261     
262             string lon_dir = msg.substr(begin, end - begin);
263             begin = end + 1;
264
265             lon = lon_deg + ( lon_min / 60.0 );
266             if ( lon_dir == "W" ) {
267                 lon *= -1;
268             }
269
270             cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
271             SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
272
273 #if 0
274             double sl_radius, lat_geoc;
275             sgGeodToGeoc( cur_fdm_state->get_Latitude(), 
276                           cur_fdm_state->get_Altitude(), 
277                           &sl_radius, &lat_geoc );
278             cur_fdm_state->set_Geocentric_Position( lat_geoc, 
279                            cur_fdm_state->get_Longitude(), 
280                            sl_radius + cur_fdm_state->get_Altitude() );
281 #endif
282
283             // speed
284             end = msg.find(",", begin);
285             if ( end == string::npos ) {
286                 return false;
287             }
288     
289             string speed_str = msg.substr(begin, end - begin);
290             begin = end + 1;
291             speed = atof( speed_str.c_str() );
292             cur_fdm_state->set_V_calibrated_kts( speed );
293             // cur_fdm_state->set_V_ground_speed( speed );
294             SG_LOG( SG_IO, SG_INFO, "  speed = " << speed );
295
296             // heading
297             end = msg.find(",", begin);
298             if ( end == string::npos ) {
299                 return false;
300             }
301     
302             string hdg_str = msg.substr(begin, end - begin);
303             begin = end + 1;
304             heading = atof( hdg_str.c_str() );
305             cur_fdm_state->set_Euler_Angles( cur_fdm_state->get_Phi(), 
306                                              cur_fdm_state->get_Theta(), 
307                                              heading * SGD_DEGREES_TO_RADIANS );
308             SG_LOG( SG_IO, SG_INFO, "  heading = " << heading );
309         } else if ( sentence == "PGRMZ" ) {
310             // altitude
311             end = msg.find(",", begin);
312             if ( end == string::npos ) {
313                 return false;
314             }
315     
316             string alt_str = msg.substr(begin, end - begin);
317             altitude = atof( alt_str.c_str() );
318             begin = end + 1;
319
320             // altitude units
321             end = msg.find(",", begin);
322             if ( end == string::npos ) {
323                 return false;
324             }
325     
326             string alt_units = msg.substr(begin, end - begin);
327             begin = end + 1;
328
329             if ( alt_units != (string)"F" && alt_units != (string)"f" ) {
330                 altitude *= SG_METER_TO_FEET;
331             }
332
333             cur_fdm_state->set_Altitude( altitude );
334     
335             SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
336
337         }
338
339         // printf("%.8f %.8f\n", lon, lat);
340
341         begin = begin_line;
342         end_line = msg.find("\n", begin_line);
343     }
344
345     return true;
346 }
347
348
349 // open hailing frequencies
350 bool FGGarmin::open() {
351     if ( is_enabled() ) {
352         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel " 
353                 << "is already in use, ignoring" );
354         return false;
355     }
356
357     SGIOChannel *io = get_io_channel();
358
359     if ( ! io->open( get_direction() ) ) {
360         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
361         return false;
362     }
363
364     set_enabled( true );
365
366     return true;
367 }
368
369
370 // process work for this port
371 bool FGGarmin::process() {
372     SGIOChannel *io = get_io_channel();
373
374     if ( get_direction() == SG_IO_OUT ) {
375         gen_message();
376         if ( ! io->write( buf, length ) ) {
377             SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
378             return false;
379         }
380     } else if ( get_direction() == SG_IO_IN ) {
381         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
382             SG_LOG( SG_IO, SG_ALERT, "Success reading data." );
383             if ( parse_message() ) {
384                 SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
385             } else {
386                 SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
387             }
388         } else {
389             SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
390             return false;
391         }
392         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
393             SG_LOG( SG_IO, SG_ALERT, "Success reading data." );
394             if ( parse_message() ) {
395                 SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
396             } else {
397                 SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
398             }
399         } else {
400             SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
401             return false;
402         }
403     }
404
405     return true;
406 }
407
408
409 // close the channel
410 bool FGGarmin::close() {
411     SGIOChannel *io = get_io_channel();
412
413     set_enabled( false );
414
415     if ( ! io->close() ) {
416         return false;
417     }
418
419     return true;
420 }