← Back to branch summary

~ubuntu-branches/ubuntu/quantal/flightgear/quantal

« back to all changes in this revision

Viewing changes to src/Network/atlas.cxx

  • Committer: Bazaar Package Importer
  • Author(s): Ove Kaaven
  • Date: 2002-03-27 21:50:15 UTC
  • Revision ID: james.westby@ubuntu.com-20020327215015-0rvi3o8iml0a8s93
Tags: upstream-0.7.9
Import upstream version 0.7.9

Show diffs side-by-side

added added

removed removed

Lines of Context:
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: atlas.cxx,v 1.6 2001/03/26 16:22:31 curt Exp $
 
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
    char rmc[256], gga[256], patla[256];
 
67
    char rmc_sum[10], gga_sum[10], patla_sum[10];
 
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,0.000,E",
 
121
             utc, lat, lon, speed, heading, date );
 
122
    sprintf( rmc_sum, "%02X", calc_atlas_cksum(rmc) );
 
123
 
 
124
    sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,",
 
125
             utc, lat, lon, altitude_ft );
 
126
    sprintf( gga_sum, "%02X", calc_atlas_cksum(gga) );
 
127
 
 
128
    sprintf( patla, "PATLA,%.2f,%.1f,%.2f,%.1f,%.0f",
 
129
             current_radiostack->get_nav1_freq(),
 
130
             current_radiostack->get_nav1_sel_radial(),
 
131
             current_radiostack->get_nav2_freq(),
 
132
             current_radiostack->get_nav2_sel_radial(),
 
133
             current_radiostack->get_adf_freq() );
 
134
    sprintf( patla_sum, "%02X", calc_atlas_cksum(patla) );
 
135
 
 
136
    SG_LOG( SG_IO, SG_DEBUG, rmc );
 
137
    SG_LOG( SG_IO, SG_DEBUG, gga );
 
138
    SG_LOG( SG_IO, SG_DEBUG, patla );
 
139
 
 
140
    string atlas_sentence;
 
141
 
 
142
    // RMC sentence
 
143
    atlas_sentence = "$";
 
144
    atlas_sentence += rmc;
 
145
    atlas_sentence += "*";
 
146
    atlas_sentence += rmc_sum;
 
147
    atlas_sentence += "\n";
 
148
 
 
149
    // GGA sentence
 
150
    atlas_sentence += "$";
 
151
    atlas_sentence += gga;
 
152
    atlas_sentence += "*";
 
153
    atlas_sentence += gga_sum;
 
154
    atlas_sentence += "\n";
 
155
 
 
156
    // PATLA sentence
 
157
    atlas_sentence += "$";
 
158
    atlas_sentence += patla;
 
159
    atlas_sentence += "*";
 
160
    atlas_sentence += patla_sum;
 
161
    atlas_sentence += "\n";
 
162
 
 
163
    cout << atlas_sentence;
 
164
 
 
165
    length = atlas_sentence.length();
 
166
    strncpy( buf, atlas_sentence.c_str(), length );
 
167
 
 
168
    return true;
 
169
}
 
170
 
 
171
 
 
172
// parse Atlas message.  messages will look something like the
 
173
// following:
 
174
//
 
175
// $GPRMC,163227,A,3321.173,N,11039.855,W,000.1,270.0,171199,0.000,E*61
 
176
// $GPGGA,163227,3321.173,N,11039.855,W,1,,,3333,F,,,,*0F
 
177
 
 
178
bool FGAtlas::parse_message() {
 
179
    SG_LOG( SG_IO, SG_INFO, "parse atlas message" );
 
180
 
 
181
    string msg = buf;
 
182
    msg = msg.substr( 0, length );
 
183
    SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
 
184
 
 
185
    string::size_type begin_line, end_line, begin, end;
 
186
    begin_line = begin = 0;
 
187
 
 
188
    // extract out each line
 
189
    end_line = msg.find("\n", begin_line);
 
190
    while ( end_line != string::npos ) {
 
191
        string line = msg.substr(begin_line, end_line - begin_line);
 
192
        begin_line = end_line + 1;
 
193
        SG_LOG( SG_IO, SG_INFO, "  input line = " << line );
 
194
 
 
195
        // leading character
 
196
        string start = msg.substr(begin, 1);
 
197
        ++begin;
 
198
        SG_LOG( SG_IO, SG_INFO, "  start = " << start );
 
199
 
 
200
        // sentence
 
201
        end = msg.find(",", begin);
 
202
        if ( end == string::npos ) {
 
203
            return false;
 
204
        }
 
205
    
 
206
        string sentence = msg.substr(begin, end - begin);
 
207
        begin = end + 1;
 
208
        SG_LOG( SG_IO, SG_INFO, "  sentence = " << sentence );
 
209
 
 
210
        double lon_deg, lon_min, lat_deg, lat_min;
 
211
        double lon, lat, speed, heading, altitude;
 
212
 
 
213
        if ( sentence == "GPRMC" ) {
 
214
            // time
 
215
            end = msg.find(",", begin);
 
216
            if ( end == string::npos ) {
 
217
                return false;
 
218
            }
 
219
    
 
220
            string utc = msg.substr(begin, end - begin);
 
221
            begin = end + 1;
 
222
            SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
 
223
 
 
224
            // junk
 
225
            end = msg.find(",", begin);
 
226
            if ( end == string::npos ) {
 
227
                return false;
 
228
            }
 
229
    
 
230
            string junk = msg.substr(begin, end - begin);
 
231
            begin = end + 1;
 
232
            SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 
233
 
 
234
            // lat val
 
235
            end = msg.find(",", begin);
 
236
            if ( end == string::npos ) {
 
237
                return false;
 
238
            }
 
239
    
 
240
            string lat_str = msg.substr(begin, end - begin);
 
241
            begin = end + 1;
 
242
 
 
243
            lat_deg = atof( lat_str.substr(0, 2).c_str() );
 
244
            lat_min = atof( lat_str.substr(2).c_str() );
 
245
 
 
246
            // lat dir
 
247
            end = msg.find(",", begin);
 
248
            if ( end == string::npos ) {
 
249
                return false;
 
250
            }
 
251
    
 
252
            string lat_dir = msg.substr(begin, end - begin);
 
253
            begin = end + 1;
 
254
 
 
255
            lat = lat_deg + ( lat_min / 60.0 );
 
256
            if ( lat_dir == "S" ) {
 
257
                lat *= -1;
 
258
            }
 
259
 
 
260
            cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
 
261
            SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
 
262
 
 
263
            // lon val
 
264
            end = msg.find(",", begin);
 
265
            if ( end == string::npos ) {
 
266
                return false;
 
267
            }
 
268
    
 
269
            string lon_str = msg.substr(begin, end - begin);
 
270
            begin = end + 1;
 
271
 
 
272
            lon_deg = atof( lon_str.substr(0, 3).c_str() );
 
273
            lon_min = atof( lon_str.substr(3).c_str() );
 
274
 
 
275
            // lon dir
 
276
            end = msg.find(",", begin);
 
277
            if ( end == string::npos ) {
 
278
                return false;
 
279
            }
 
280
    
 
281
            string lon_dir = msg.substr(begin, end - begin);
 
282
            begin = end + 1;
 
283
 
 
284
            lon = lon_deg + ( lon_min / 60.0 );
 
285
            if ( lon_dir == "W" ) {
 
286
                lon *= -1;
 
287
            }
 
288
 
 
289
            cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
 
290
            SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
 
291
 
 
292
#if 0
 
293
            double sl_radius, lat_geoc;
 
294
            sgGeodToGeoc( cur_fdm_state->get_Latitude(), 
 
295
                          cur_fdm_state->get_Altitude(), 
 
296
                          &sl_radius, &lat_geoc );
 
297
            cur_fdm_state->set_Geocentric_Position( lat_geoc, 
 
298
                           cur_fdm_state->get_Longitude(), 
 
299
                           sl_radius + cur_fdm_state->get_Altitude() );
 
300
#endif
 
301
 
 
302
            // speed
 
303
            end = msg.find(",", begin);
 
304
            if ( end == string::npos ) {
 
305
                return false;
 
306
            }
 
307
    
 
308
            string speed_str = msg.substr(begin, end - begin);
 
309
            begin = end + 1;
 
310
            speed = atof( speed_str.c_str() );
 
311
            cur_fdm_state->set_V_calibrated_kts( speed );
 
312
            // cur_fdm_state->set_V_ground_speed( speed );
 
313
            SG_LOG( SG_IO, SG_INFO, "  speed = " << speed );
 
314
 
 
315
            // heading
 
316
            end = msg.find(",", begin);
 
317
            if ( end == string::npos ) {
 
318
                return false;
 
319
            }
 
320
    
 
321
            string hdg_str = msg.substr(begin, end - begin);
 
322
            begin = end + 1;
 
323
            heading = atof( hdg_str.c_str() );
 
324
            cur_fdm_state->set_Euler_Angles( cur_fdm_state->get_Phi(), 
 
325
                                             cur_fdm_state->get_Theta(), 
 
326
                                             heading * SGD_DEGREES_TO_RADIANS );
 
327
            SG_LOG( SG_IO, SG_INFO, "  heading = " << heading );
 
328
        } else if ( sentence == "GPGGA" ) {
 
329
            // time
 
330
            end = msg.find(",", begin);
 
331
            if ( end == string::npos ) {
 
332
                return false;
 
333
            }
 
334
    
 
335
            string utc = msg.substr(begin, end - begin);
 
336
            begin = end + 1;
 
337
            SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
 
338
 
 
339
            // lat val
 
340
            end = msg.find(",", begin);
 
341
            if ( end == string::npos ) {
 
342
                return false;
 
343
            }
 
344
    
 
345
            string lat_str = msg.substr(begin, end - begin);
 
346
            begin = end + 1;
 
347
 
 
348
            lat_deg = atof( lat_str.substr(0, 2).c_str() );
 
349
            lat_min = atof( lat_str.substr(2).c_str() );
 
350
 
 
351
            // lat dir
 
352
            end = msg.find(",", begin);
 
353
            if ( end == string::npos ) {
 
354
                return false;
 
355
            }
 
356
    
 
357
            string lat_dir = msg.substr(begin, end - begin);
 
358
            begin = end + 1;
 
359
 
 
360
            lat = lat_deg + ( lat_min / 60.0 );
 
361
            if ( lat_dir == "S" ) {
 
362
                lat *= -1;
 
363
            }
 
364
 
 
365
            // cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
 
366
            SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
 
367
 
 
368
            // lon val
 
369
            end = msg.find(",", begin);
 
370
            if ( end == string::npos ) {
 
371
                return false;
 
372
            }
 
373
    
 
374
            string lon_str = msg.substr(begin, end - begin);
 
375
            begin = end + 1;
 
376
 
 
377
            lon_deg = atof( lon_str.substr(0, 3).c_str() );
 
378
            lon_min = atof( lon_str.substr(3).c_str() );
 
379
 
 
380
            // lon dir
 
381
            end = msg.find(",", begin);
 
382
            if ( end == string::npos ) {
 
383
                return false;
 
384
            }
 
385
    
 
386
            string lon_dir = msg.substr(begin, end - begin);
 
387
            begin = end + 1;
 
388
 
 
389
            lon = lon_deg + ( lon_min / 60.0 );
 
390
            if ( lon_dir == "W" ) {
 
391
                lon *= -1;
 
392
            }
 
393
 
 
394
            // cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
 
395
            SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
 
396
 
 
397
            // junk
 
398
            end = msg.find(",", begin);
 
399
            if ( end == string::npos ) {
 
400
                return false;
 
401
            }
 
402
    
 
403
            string junk = msg.substr(begin, end - begin);
 
404
            begin = end + 1;
 
405
            SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 
406
 
 
407
            // junk
 
408
            end = msg.find(",", begin);
 
409
            if ( end == string::npos ) {
 
410
                return false;
 
411
            }
 
412
    
 
413
            junk = msg.substr(begin, end - begin);
 
414
            begin = end + 1;
 
415
            SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 
416
 
 
417
            // junk
 
418
            end = msg.find(",", begin);
 
419
            if ( end == string::npos ) {
 
420
                return false;
 
421
            }
 
422
    
 
423
            junk = msg.substr(begin, end - begin);
 
424
            begin = end + 1;
 
425
            SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 
426
 
 
427
            // altitude
 
428
            end = msg.find(",", begin);
 
429
            if ( end == string::npos ) {
 
430
                return false;
 
431
            }
 
432
    
 
433
            string alt_str = msg.substr(begin, end - begin);
 
434
            altitude = atof( alt_str.c_str() );
 
435
            begin = end + 1;
 
436
 
 
437
            // altitude units
 
438
            end = msg.find(",", begin);
 
439
            if ( end == string::npos ) {
 
440
                return false;
 
441
            }
 
442
    
 
443
            string alt_units = msg.substr(begin, end - begin);
 
444
            begin = end + 1;
 
445
 
 
446
            if ( alt_units != (string)"F" ) {
 
447
                altitude *= SG_METER_TO_FEET;
 
448
            }
 
449
 
 
450
            cur_fdm_state->set_Altitude( altitude );
 
451
    
 
452
            SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
 
453
 
 
454
        } else if ( sentence == "PATLA" ) {
 
455
            // nav1 freq
 
456
            end = msg.find(",", begin);
 
457
            if ( end == string::npos ) {
 
458
                return false;
 
459
            }
 
460
    
 
461
            string nav1_freq = msg.substr(begin, end - begin);
 
462
            begin = end + 1;
 
463
            SG_LOG( SG_IO, SG_INFO, "  nav1_freq = " << nav1_freq );
 
464
 
 
465
            // nav1 selected radial
 
466
            end = msg.find(",", begin);
 
467
            if ( end == string::npos ) {
 
468
                return false;
 
469
            }
 
470
    
 
471
            string nav1_rad = msg.substr(begin, end - begin);
 
472
            begin = end + 1;
 
473
            SG_LOG( SG_IO, SG_INFO, "  nav1_rad = " << nav1_rad );
 
474
 
 
475
            // nav2 freq
 
476
            end = msg.find(",", begin);
 
477
            if ( end == string::npos ) {
 
478
                return false;
 
479
            }
 
480
    
 
481
            string nav2_freq = msg.substr(begin, end - begin);
 
482
            begin = end + 1;
 
483
            SG_LOG( SG_IO, SG_INFO, "  nav2_freq = " << nav2_freq );
 
484
 
 
485
            // nav2 selected radial
 
486
            end = msg.find(",", begin);
 
487
            if ( end == string::npos ) {
 
488
                return false;
 
489
            }
 
490
    
 
491
            string nav2_rad = msg.substr(begin, end - begin);
 
492
            begin = end + 1;
 
493
            SG_LOG( SG_IO, SG_INFO, "  nav2_rad = " << nav2_rad );
 
494
 
 
495
            // adf freq
 
496
            end = msg.find("*", begin);
 
497
            if ( end == string::npos ) {
 
498
                return false;
 
499
            }
 
500
    
 
501
            string adf_freq = msg.substr(begin, end - begin);
 
502
            begin = end + 1;
 
503
            SG_LOG( SG_IO, SG_INFO, "  adf_freq = " << adf_freq );
 
504
        }
 
505
 
 
506
        // printf("%.8f %.8f\n", lon, lat);
 
507
 
 
508
        begin = begin_line;
 
509
        end_line = msg.find("\n", begin_line);
 
510
    }
 
511
 
 
512
    return true;
 
513
}
 
514
 
 
515
 
 
516
// open hailing frequencies
 
517
bool FGAtlas::open() {
 
518
    if ( is_enabled() ) {
 
519
        SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel " 
 
520
                << "is already in use, ignoring" );
 
521
        return false;
 
522
    }
 
523
 
 
524
    SGIOChannel *io = get_io_channel();
 
525
 
 
526
    if ( ! io->open( get_direction() ) ) {
 
527
        SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 
528
        return false;
 
529
    }
 
530
 
 
531
    set_enabled( true );
 
532
 
 
533
    return true;
 
534
}
 
535
 
 
536
 
 
537
// process work for this port
 
538
bool FGAtlas::process() {
 
539
    SGIOChannel *io = get_io_channel();
 
540
 
 
541
    if ( get_direction() == SG_IO_OUT ) {
 
542
        gen_message();
 
543
        if ( ! io->write( buf, length ) ) {
 
544
            SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
 
545
            return false;
 
546
        }
 
547
    } else if ( get_direction() == SG_IO_IN ) {
 
548
        if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 
549
            parse_message();
 
550
        } else {
 
551
            SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
 
552
            return false;
 
553
        }
 
554
        if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 
555
            parse_message();
 
556
        } else {
 
557
            SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
 
558
            return false;
 
559
        }
 
560
    }
 
561
 
 
562
    return true;
 
563
}
 
564
 
 
565
 
 
566
// close the channel
 
567
bool FGAtlas::close() {
 
568
    SGIOChannel *io = get_io_channel();
 
569
 
 
570
    set_enabled( false );
 
571
 
 
572
    if ( ! io->close() ) {
 
573
        return false;
 
574
    }
 
575
 
 
576
    return true;
 
577
}