]> git.mxchange.org Git - flightgear.git/blob - src/Network/nmea.cxx
Make spoilers and speed-brake position accessible
[flightgear.git] / src / Network / nmea.cxx
1 // nmea.cxx -- NMEA 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 "nmea.hxx"
33
34 SG_USING_NAMESPACE(std);
35
36
37 FGNMEA::FGNMEA() {
38 }
39
40 FGNMEA::~FGNMEA() {
41 }
42
43
44 // calculate the nmea check sum
45 static char calc_nmea_cksum(char *sentence) {
46     unsigned char sum = 0;
47     int i, len;
48
49     // cout << sentence << endl;
50
51     len = strlen(sentence);
52     sum = sentence[0];
53     for ( i = 1; i < len; i++ ) {
54         // cout << sentence[i];
55         sum ^= sentence[i];
56     }
57     // cout << endl;
58
59     // printf("sum = %02x\n", sum);
60     return sum;
61 }
62
63
64 // generate NMEA message
65 bool FGNMEA::gen_message() {
66     // cout << "generating nmea message" << endl;
67
68     char rmc[256], gga[256];
69     char rmc_sum[10], gga_sum[10];
70     char dir;
71     int deg;
72     double min;
73
74     SGTime *t = globals->get_time_params();
75
76     char utc[10];
77     sprintf( utc, "%02d%02d%02d", 
78              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
79
80     char lat[20];
81     double latd = cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES;
82     if ( latd < 0.0 ) {
83         latd *= -1.0;
84         dir = 'S';
85     } else {
86         dir = 'N';
87     }
88     deg = (int)(latd);
89     min = (latd - (double)deg) * 60.0;
90     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
91
92     char lon[20];
93     double lond = cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES;
94     if ( lond < 0.0 ) {
95         lond *= -1.0;
96         dir = 'W';
97     } else {
98         dir = 'E';
99     }
100     deg = (int)(lond);
101     min = (lond - (double)deg) * 60.0;
102     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
103
104     char speed[10];
105     sprintf( speed, "%05.1f", cur_fdm_state->get_V_equiv_kts() );
106
107     char heading[10];
108     sprintf( heading, "%05.1f", cur_fdm_state->get_Psi() * SGD_RADIANS_TO_DEGREES );
109
110     char altitude_m[10];
111     sprintf( altitude_m, "%02d", 
112              (int)(cur_fdm_state->get_Altitude() * SG_FEET_TO_METER) );
113
114     char altitude_ft[10];
115     sprintf( altitude_ft, "%02d", (int)cur_fdm_state->get_Altitude() );
116
117     char date[10];
118     sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday, 
119              t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
120
121     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
122     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,0.000,E",
123              utc, lat, lon, speed, heading, date );
124     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
125
126     sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,",
127              utc, lat, lon, altitude_ft );
128     sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) );
129
130
131     SG_LOG( SG_IO, SG_DEBUG, rmc );
132     SG_LOG( SG_IO, SG_DEBUG, gga );
133
134     string nmea_sentence;
135
136     // RMC sentence
137     nmea_sentence = "$";
138     nmea_sentence += rmc;
139     nmea_sentence += "*";
140     nmea_sentence += rmc_sum;
141     nmea_sentence += "\n";
142
143     // GGA sentence
144     nmea_sentence += "$";
145     nmea_sentence += gga;
146     nmea_sentence += "*";
147     nmea_sentence += gga_sum;
148     nmea_sentence += "\n";
149
150     cout << nmea_sentence;
151
152     length = nmea_sentence.length();
153     strncpy( buf, nmea_sentence.c_str(), length );
154
155     return true;
156 }
157
158
159 // parse NMEA message.  messages will look something like the
160 // following:
161 //
162 // $GPRMC,163227,A,3321.173,N,11039.855,W,000.1,270.0,171199,0.000,E*61
163 // $GPGGA,163227,3321.173,N,11039.855,W,1,,,3333,F,,,,*0F
164
165 bool FGNMEA::parse_message() {
166     SG_LOG( SG_IO, SG_INFO, "parse nmea message" );
167
168     string msg = buf;
169     msg = msg.substr( 0, length );
170     SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
171
172     string::size_type begin_line, end_line, begin, end;
173     begin_line = begin = 0;
174
175     // extract out each line
176     end_line = msg.find("\n", begin_line);
177     while ( end_line != string::npos ) {
178         string line = msg.substr(begin_line, end_line - begin_line);
179         begin_line = end_line + 1;
180         SG_LOG( SG_IO, SG_INFO, "  input line = " << line );
181
182         // leading character
183         string start = msg.substr(begin, 1);
184         ++begin;
185         SG_LOG( SG_IO, SG_INFO, "  start = " << start );
186
187         // sentence
188         end = msg.find(",", begin);
189         if ( end == string::npos ) {
190             return false;
191         }
192     
193         string sentence = msg.substr(begin, end - begin);
194         begin = end + 1;
195         SG_LOG( SG_IO, SG_INFO, "  sentence = " << sentence );
196
197         double lon_deg, lon_min, lat_deg, lat_min;
198         double lon, lat, speed, heading, altitude;
199
200         if ( sentence == "GPRMC" ) {
201             // time
202             end = msg.find(",", begin);
203             if ( end == string::npos ) {
204                 return false;
205             }
206     
207             string utc = msg.substr(begin, end - begin);
208             begin = end + 1;
209             SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
210
211             // junk
212             end = msg.find(",", begin);
213             if ( end == string::npos ) {
214                 return false;
215             }
216     
217             string junk = msg.substr(begin, end - begin);
218             begin = end + 1;
219             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
220
221             // lat val
222             end = msg.find(",", begin);
223             if ( end == string::npos ) {
224                 return false;
225             }
226     
227             string lat_str = msg.substr(begin, end - begin);
228             begin = end + 1;
229
230             lat_deg = atof( lat_str.substr(0, 2).c_str() );
231             lat_min = atof( lat_str.substr(2).c_str() );
232
233             // lat dir
234             end = msg.find(",", begin);
235             if ( end == string::npos ) {
236                 return false;
237             }
238     
239             string lat_dir = msg.substr(begin, end - begin);
240             begin = end + 1;
241
242             lat = lat_deg + ( lat_min / 60.0 );
243             if ( lat_dir == "S" ) {
244                 lat *= -1;
245             }
246
247             cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
248             SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
249
250             // lon val
251             end = msg.find(",", begin);
252             if ( end == string::npos ) {
253                 return false;
254             }
255     
256             string lon_str = msg.substr(begin, end - begin);
257             begin = end + 1;
258
259             lon_deg = atof( lon_str.substr(0, 3).c_str() );
260             lon_min = atof( lon_str.substr(3).c_str() );
261
262             // lon dir
263             end = msg.find(",", begin);
264             if ( end == string::npos ) {
265                 return false;
266             }
267     
268             string lon_dir = msg.substr(begin, end - begin);
269             begin = end + 1;
270
271             lon = lon_deg + ( lon_min / 60.0 );
272             if ( lon_dir == "W" ) {
273                 lon *= -1;
274             }
275
276             cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
277             SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
278
279 #if 0
280             double sl_radius, lat_geoc;
281             sgGeodToGeoc( cur_fdm_state->get_Latitude(), 
282                           cur_fdm_state->get_Altitude(), 
283                           &sl_radius, &lat_geoc );
284             cur_fdm_state->set_Geocentric_Position( lat_geoc, 
285                            cur_fdm_state->get_Longitude(), 
286                            sl_radius + cur_fdm_state->get_Altitude() );
287 #endif
288
289             // speed
290             end = msg.find(",", begin);
291             if ( end == string::npos ) {
292                 return false;
293             }
294     
295             string speed_str = msg.substr(begin, end - begin);
296             begin = end + 1;
297             speed = atof( speed_str.c_str() );
298             cur_fdm_state->set_V_calibrated_kts( speed );
299             // cur_fdm_state->set_V_ground_speed( speed );
300             SG_LOG( SG_IO, SG_INFO, "  speed = " << speed );
301
302             // heading
303             end = msg.find(",", begin);
304             if ( end == string::npos ) {
305                 return false;
306             }
307     
308             string hdg_str = msg.substr(begin, end - begin);
309             begin = end + 1;
310             heading = atof( hdg_str.c_str() );
311             cur_fdm_state->set_Euler_Angles( cur_fdm_state->get_Phi(), 
312                                              cur_fdm_state->get_Theta(), 
313                                              heading * SGD_DEGREES_TO_RADIANS );
314             SG_LOG( SG_IO, SG_INFO, "  heading = " << heading );
315         } else if ( sentence == "GPGGA" ) {
316             // time
317             end = msg.find(",", begin);
318             if ( end == string::npos ) {
319                 return false;
320             }
321     
322             string utc = msg.substr(begin, end - begin);
323             begin = end + 1;
324             SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
325
326             // lat val
327             end = msg.find(",", begin);
328             if ( end == string::npos ) {
329                 return false;
330             }
331     
332             string lat_str = msg.substr(begin, end - begin);
333             begin = end + 1;
334
335             lat_deg = atof( lat_str.substr(0, 2).c_str() );
336             lat_min = atof( lat_str.substr(2).c_str() );
337
338             // lat dir
339             end = msg.find(",", begin);
340             if ( end == string::npos ) {
341                 return false;
342             }
343     
344             string lat_dir = msg.substr(begin, end - begin);
345             begin = end + 1;
346
347             lat = lat_deg + ( lat_min / 60.0 );
348             if ( lat_dir == "S" ) {
349                 lat *= -1;
350             }
351
352             // cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
353             SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
354
355             // lon val
356             end = msg.find(",", begin);
357             if ( end == string::npos ) {
358                 return false;
359             }
360     
361             string lon_str = msg.substr(begin, end - begin);
362             begin = end + 1;
363
364             lon_deg = atof( lon_str.substr(0, 3).c_str() );
365             lon_min = atof( lon_str.substr(3).c_str() );
366
367             // lon dir
368             end = msg.find(",", begin);
369             if ( end == string::npos ) {
370                 return false;
371             }
372     
373             string lon_dir = msg.substr(begin, end - begin);
374             begin = end + 1;
375
376             lon = lon_deg + ( lon_min / 60.0 );
377             if ( lon_dir == "W" ) {
378                 lon *= -1;
379             }
380
381             // cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
382             SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
383
384             // junk
385             end = msg.find(",", begin);
386             if ( end == string::npos ) {
387                 return false;
388             }
389     
390             string junk = msg.substr(begin, end - begin);
391             begin = end + 1;
392             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
393
394             // junk
395             end = msg.find(",", begin);
396             if ( end == string::npos ) {
397                 return false;
398             }
399     
400             junk = msg.substr(begin, end - begin);
401             begin = end + 1;
402             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
403
404             // junk
405             end = msg.find(",", begin);
406             if ( end == string::npos ) {
407                 return false;
408             }
409     
410             junk = msg.substr(begin, end - begin);
411             begin = end + 1;
412             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
413
414             // altitude
415             end = msg.find(",", begin);
416             if ( end == string::npos ) {
417                 return false;
418             }
419     
420             string alt_str = msg.substr(begin, end - begin);
421             altitude = atof( alt_str.c_str() );
422             begin = end + 1;
423
424             // altitude units
425             end = msg.find(",", begin);
426             if ( end == string::npos ) {
427                 return false;
428             }
429     
430             string alt_units = msg.substr(begin, end - begin);
431             begin = end + 1;
432
433             if ( alt_units != (string)"F" ) {
434                 altitude *= SG_METER_TO_FEET;
435             }
436
437             cur_fdm_state->set_Altitude( altitude );
438     
439             SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
440
441         }
442
443         // printf("%.8f %.8f\n", lon, lat);
444
445         begin = begin_line;
446         end_line = msg.find("\n", begin_line);
447     }
448
449     return true;
450 }
451
452
453 // open hailing frequencies
454 bool FGNMEA::open() {
455     if ( is_enabled() ) {
456         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel " 
457                 << "is already in use, ignoring" );
458         return false;
459     }
460
461     SGIOChannel *io = get_io_channel();
462
463     if ( ! io->open( get_direction() ) ) {
464         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
465         return false;
466     }
467
468     set_enabled( true );
469
470     return true;
471 }
472
473
474 // process work for this port
475 bool FGNMEA::process() {
476     SGIOChannel *io = get_io_channel();
477
478     if ( get_direction() == SG_IO_OUT ) {
479         gen_message();
480         if ( ! io->write( buf, length ) ) {
481             SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
482             return false;
483         }
484     } else if ( get_direction() == SG_IO_IN ) {
485         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
486             parse_message();
487         } else {
488             SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
489             return false;
490         }
491         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
492             parse_message();
493         } else {
494             SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
495             return false;
496         }
497     }
498
499     return true;
500 }
501
502
503 // close the channel
504 bool FGNMEA::close() {
505     SGIOChannel *io = get_io_channel();
506
507     set_enabled( false );
508
509     if ( ! io->close() ) {
510         return false;
511     }
512
513     return true;
514 }