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