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