Main/fg_serial.cxx

   1 // fg_serial.cxx -- higher level serial port management routines
   2 //
   3 // Written by Curtis Olson, started November 1998.
   4 //
   5 // Copyright (C) 1998  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 // (Log is kept at end of this file)
  23
  24
  25 #include <Include/compiler.h>
  26
  27 #ifdef FG_HAVE_STD_INCLUDES
  28 #  include <cstdlib>    // atoi()
  29 #else
  30 #  include <stdlib.h>   // atoi()
  31 #endif
  32
  33 #include STL_STRING
  34 #include STL_IOSTREAM
  35 #include <vector>
  36 #include "Include/fg_stl_config.h"
  37
  38 FG_USING_NAMESPACE(std);
  39
  40 #include <Aircraft/aircraft.hxx>
  41 #include <Debug/logstream.hxx>
  42 #include <Include/fg_constants.h>
  43 #include <Serial/serial.hxx>
  44 #include <Time/fg_time.hxx>
  45
  46 #include "options.hxx"
  47
  48 #include "fg_serial.hxx"
  49
  50
  51 // support an arbitrary number of serial channels.  Each channel can
  52 // be assigned to an arbitrary port.  Bi-directional communication is
  53 // supported by the underlying layer, but probably will never be
  54 // needed by FGFS?
  55
  56 typedef vector < fgIOCHANNEL > io_container;
  57 typedef io_container::iterator io_iterator;
  58 typedef io_container::const_iterator const_io_iterator;
  59
  60 // define the four channels
  61 io_container port_list;
  62
  63
  64 fgIOCHANNEL::fgIOCHANNEL() :
  65     kind( FG_SERIAL_DISABLED ),
  66     valid_config( false )
  67 {
  68 }
  69
  70
  71 fgIOCHANNEL::~fgIOCHANNEL() {
  72 }
  73
  74
  75 // configure a port based on the config string
  76 static fgIOCHANNEL parse_port_config( const string& config )
  77 {
  78     fgIOCHANNEL p;
  79
  80     string::size_type begin, end;
  81
  82     begin = 0;
  83
  84     FG_LOG( FG_SERIAL, FG_INFO, "Parse serial port config: " << config );
  85
  86     // device name
  87     end = config.find(",", begin);
  88     if ( end == string::npos ) {
  89         return p;
  90     }
  91
  92     p.device = config.substr(begin, end - begin);
  93     begin = end + 1;
  94     FG_LOG( FG_SERIAL, FG_INFO, "  device = " << p.device );
  95
  96     // format
  97     end = config.find(",", begin);
  98     if ( end == string::npos ) {
  99         return p;
 100     }
 101
 102     p.format = config.substr(begin, end - begin);
 103     begin = end + 1;
 104     FG_LOG( FG_SERIAL, FG_INFO, "  format = " << p.format );
 105
 106     // baud
 107     end = config.find(",", begin);
 108     if ( end == string::npos ) {
 109         return p;
 110     }
 111
 112     p.baud = config.substr(begin, end - begin);
 113     begin = end + 1;
 114     FG_LOG( FG_SERIAL, FG_INFO, "  baud = " << p.baud );
 115
 116     // direction
 117     p.direction = config.substr(begin);
 118     FG_LOG( FG_SERIAL, FG_INFO, "  direction = " << p.direction );
 119
 120     p.valid_config = true;
 121
 122     return p;
 123 }
 124
 125
 126 // configure a port based on the config info
 127 static bool config_port( fgIOCHANNEL &p )
 128 {
 129     if ( p.port.is_enabled() ) {
 130         FG_LOG( FG_SERIAL, FG_ALERT, "This shouldn't happen, but the port "
 131                 << "is already in use, ignoring" );
 132         return false;
 133     }
 134
 135     if ( ! p.port.open_port( p.device ) ) {
 136         FG_LOG( FG_SERIAL, FG_ALERT, "Error opening device: " << p.device );
 137         return false;
 138     }
 139
 140     // cout << "fd = " << p.port.fd << endl;
 141
 142     if ( ! p.port.set_baud( atoi( p.baud.c_str() ) ) ) {
 143         FG_LOG( FG_SERIAL, FG_ALERT, "Error setting baud: " << p.baud );
 144         return false;
 145     }
 146
 147     if ( p.format == "nmea" ) {
 148         if ( p.direction == "out" ) {
 149             p.kind = fgIOCHANNEL::FG_SERIAL_NMEA_OUT;
 150         } else if ( p.direction == "in" ) {
 151             p.kind = fgIOCHANNEL::FG_SERIAL_NMEA_IN;
 152         } else {
 153             FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
 154             return false;
 155         }
 156     } else if ( p.format == "garmin" ) {
 157         if ( p.direction == "out" ) {
 158             p.kind = fgIOCHANNEL::FG_SERIAL_GARMIN_OUT;
 159         } else if ( p.direction == "in" ) {
 160             p.kind = fgIOCHANNEL::FG_SERIAL_GARMIN_IN;
 161         } else {
 162             FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
 163             return false;
 164         }
 165     } else if ( p.format == "fgfs" ) {
 166         if ( p.direction == "out" ) {
 167             p.kind = fgIOCHANNEL::FG_SERIAL_FGFS_OUT;
 168         } else if ( p.direction == "in" ) {
 169             p.kind = fgIOCHANNEL::FG_SERIAL_FGFS_IN;
 170         } else {
 171             FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
 172             return false;
 173         }
 174     } else {
 175         FG_LOG( FG_SERIAL, FG_ALERT, "Unknown format" );
 176         return false;
 177     }
 178
 179     return true;
 180 }
 181
 182
 183 // step through the port config streams (from fgOPTIONS) and setup
 184 // serial port channels for each
 185 void fgSerialInit() {
 186     fgIOCHANNEL port;
 187     bool result;
 188     str_container port_options_list = current_options.get_port_options_list();
 189
 190     // we could almost do this in a single step except pushing a valid
 191     // port onto the port list copies the structure and destroys the
 192     // original, which closes the port and frees up the fd ... doh!!!
 193
 194     // parse the configuration strings and store the results in stub
 195     // fgIOCHANNEL structures
 196     const_str_iterator current_str = port_options_list.begin();
 197     const_str_iterator last_str = port_options_list.end();
 198     for ( ; current_str != last_str; ++current_str ) {
 199         port = parse_port_config( *current_str );
 200         if ( port.valid_config ) {
 201             result = config_port( port );
 202             if ( result ) {
 203                 port_list.push_back( port );
 204             }
 205         }
 206     }
 207 }
 208
 209
 210 char calc_nmea_cksum(char *sentence) {
 211     unsigned char sum = 0;
 212     int i, len;
 213
 214     // printf("%s\n", sentence);
 215
 216     len = strlen(sentence);
 217     sum = sentence[0];
 218     for ( i = 1; i < len; i++ ) {
 219         // printf("%c", sentence[i]);
 220         sum ^= sentence[i];
 221     }
 222     // printf("\n");
 223
 224     // printf("sum = %02x\n", sum);
 225     return sum;
 226 }
 227
 228
 229 static void send_nmea_out( fgIOCHANNEL *p ) {
 230     char rmc[256], gga[256];
 231     char rmc_sum[10], gga_sum[10];
 232     char dir;
 233     int deg;
 234     double min;
 235     FGInterface *f;
 236     fgTIME *t;
 237
 238     // run once every two seconds
 239     if ( p->last_time == cur_time_params.cur_time ) {
 240         return;
 241     }
 242     p->last_time = cur_time_params.cur_time;
 243     if ( cur_time_params.cur_time % 2 != 0 ) {
 244         return;
 245     }
 246
 247     f = current_aircraft.fdm_state;
 248     t = &cur_time_params;
 249
 250     char utc[10];
 251     sprintf( utc, "%02d%02d%02d",
 252              t->gmt->tm_hour, t->gmt->tm_min, t->gmt->tm_sec );
 253
 254     char lat[20];
 255     double latd = f->get_Latitude() * RAD_TO_DEG;
 256     if ( latd < 0.0 ) {
 257         latd *= -1.0;
 258         dir = 'S';
 259     } else {
 260         dir = 'N';
 261     }
 262     deg = (int)(latd);
 263     min = (latd - (double)deg) * 60.0;
 264     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
 265
 266     char lon[20];
 267     double lond = f->get_Longitude() * RAD_TO_DEG;
 268     if ( lond < 0.0 ) {
 269         lond *= -1.0;
 270         dir = 'W';
 271     } else {
 272         dir = 'E';
 273     }
 274     deg = (int)(lond);
 275     min = (lond - (double)deg) * 60.0;
 276     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
 277
 278     char speed[10];
 279     sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
 280
 281     char heading[10];
 282     sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
 283
 284     char altitude_m[10];
 285     sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
 286
 287     char altitude_ft[10];
 288     sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
 289
 290     char date[10];
 291     sprintf( date, "%02d%02d%02d",
 292              t->gmt->tm_mday, t->gmt->tm_mon+1, t->gmt->tm_year );
 293
 294     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
 295     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,0.000,E",
 296              utc, lat, lon, speed, heading, date );
 297     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
 298
 299     sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,",
 300              utc, lat, lon, altitude_ft );
 301     sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) );
 302
 303
 304     FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
 305     FG_LOG( FG_SERIAL, FG_DEBUG, gga );
 306
 307     // RMC sentence
 308     string rmc_sentence = "$";
 309     rmc_sentence += rmc;
 310     rmc_sentence += "*";
 311     rmc_sentence += rmc_sum;
 312     rmc_sentence += "\n";
 313     p->port.write_port(rmc_sentence);
 314     // cout << rmc_sentence;
 315
 316     // GGA sentence
 317     string gga_sentence = "$";
 318     gga_sentence += gga;
 319     gga_sentence += "*";
 320     gga_sentence += gga_sum;
 321     gga_sentence += "\n";
 322     p->port.write_port(gga_sentence);
 323     // cout << gga_sentence;
 324 }
 325
 326 static void read_nmea_in( fgIOCHANNEL *p ) {
 327 }
 328
 329 static void send_garmin_out( fgIOCHANNEL *p ) {
 330     char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256];
 331     char dir;
 332     int deg;
 333     double min;
 334     FGInterface *f;
 335     fgTIME *t;
 336
 337     // run once per second
 338     if ( p->last_time == cur_time_params.cur_time ) {
 339         return;
 340     }
 341     p->last_time = cur_time_params.cur_time;
 342     if ( cur_time_params.cur_time % 2 != 0 ) {
 343         return;
 344     }
 345
 346     f = current_aircraft.fdm_state;
 347     t = &cur_time_params;
 348
 349     char utc[10];
 350     sprintf( utc, "%02d%02d%02d",
 351              t->gmt->tm_hour, t->gmt->tm_min, t->gmt->tm_sec );
 352
 353     char lat[20];
 354     double latd = f->get_Latitude() * RAD_TO_DEG;
 355     if ( latd < 0.0 ) {
 356         latd *= -1.0;
 357         dir = 'S';
 358     } else {
 359         dir = 'N';
 360     }
 361     deg = (int)(latd);
 362     min = (latd - (double)deg) * 60.0;
 363     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
 364
 365     char lon[20];
 366     double lond = f->get_Longitude() * RAD_TO_DEG;
 367     if ( lond < 0.0 ) {
 368         lond *= -1.0;
 369         dir = 'W';
 370     } else {
 371         dir = 'E';
 372     }
 373     deg = (int)(lond);
 374     min = (lond - (double)deg) * 60.0;
 375     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
 376
 377     char speed[10];
 378     sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
 379
 380     char heading[10];
 381     sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
 382
 383     char altitude_m[10];
 384     sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
 385
 386     char altitude_ft[10];
 387     sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
 388
 389     char date[10];
 390     sprintf( date, "%02d%02d%02d",
 391              t->gmt->tm_mday, t->gmt->tm_mon+1, t->gmt->tm_year );
 392
 393     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
 394     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,000.0,E",
 395              utc, lat, lon, speed, heading, date );
 396     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
 397
 398     // sprintf( gga, "$GPGGA,%s,%s,%s,1,04,0.0,%s,M,00.0,M,,*00\r\n",
 399     //          utc, lat, lon, altitude_m );
 400
 401     sprintf( rmz, "PGRMZ,%s,f,3", altitude_ft );
 402     sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) );
 403
 404     FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
 405     FG_LOG( FG_SERIAL, FG_DEBUG, rmz );
 406
 407     // RMC sentence
 408     string rmc_sentence = "$";
 409     rmc_sentence += rmc;
 410     rmc_sentence += "*";
 411     rmc_sentence += rmc_sum;
 412     rmc_sentence += "\n";
 413     p->port.write_port(rmc_sentence);
 414     // cout << rmc_sentence;
 415
 416     // RMZ sentence (garmin proprietary)
 417     string rmz_sentence = "$";
 418     rmz_sentence += rmz;
 419     rmz_sentence += "*";
 420     rmz_sentence += rmz_sum;
 421     rmz_sentence += "\n";
 422     p->port.write_port(rmz_sentence);
 423     // cout << rmz_sentence;
 424 }
 425
 426 static void read_garmin_in( fgIOCHANNEL *p ) {
 427 }
 428
 429 static void send_fgfs_out( fgIOCHANNEL *p ) {
 430 }
 431
 432 static void read_fgfs_in( fgIOCHANNEL *p ) {
 433 }
 434
 435
 436 // one more level of indirection ...
 437 static void process_port( fgIOCHANNEL *p ) {
 438     if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_OUT ) {
 439         send_nmea_out(p);
 440     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_IN ) {
 441         read_nmea_in(p);
 442     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_OUT ) {
 443         send_garmin_out(p);
 444     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_IN ) {
 445         read_garmin_in(p);
 446     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_OUT ) {
 447         send_fgfs_out(p);
 448     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_IN ) {
 449         read_fgfs_in(p);
 450     }
 451 }
 452
 453
 454 // process any serial port work
 455 void fgSerialProcess() {
 456     fgIOCHANNEL *port;
 457
 458     io_iterator current = port_list.begin();
 459     io_iterator last = port_list.end();
 460
 461     for ( ; current != last; ++current ) {
 462         port = current;
 463         if ( port->kind != fgIOCHANNEL::FG_SERIAL_DISABLED ) {
 464             process_port ( port );
 465         }
 466     }
 467 }
 468
 469
 470 // $Log$
 471 // Revision 1.11  1999/02/05 21:29:11  curt
 472 // Modifications to incorporate Jon S. Berndts flight model code.
 473 //
 474 // Revision 1.10  1999/01/21 00:55:01  curt
 475 // Fixed some problems with timing of output strings.
 476 // Added checksum support for nmea and garmin output.
 477 //
 478 // Revision 1.9  1999/01/20 13:42:26  curt
 479 // Tweaked FDM interface.
 480 // Testing check sum support for NMEA serial output.
 481 //
 482 // Revision 1.8  1999/01/19 20:57:04  curt
 483 // MacOS portability changes contributed by "Robert Puyol" <puyol@abvent.fr>
 484 //
 485 // Revision 1.7  1998/12/05 15:54:21  curt
 486 // Renamed class fgFLIGHT to class FGState as per request by JSB.
 487 //
 488 // Revision 1.6  1998/12/03 01:17:18  curt
 489 // Converted fgFLIGHT to a class.
 490 //
 491 // Revision 1.5  1998/11/30 17:43:32  curt
 492 // Lots of tweaking to get serial output to actually work.
 493 //
 494 // Revision 1.4  1998/11/25 01:33:58  curt
 495 // Support for an arbitrary number of serial ports.
 496 //
 497 // Revision 1.3  1998/11/23 20:51:51  curt
 498 // Tweaking serial stuff.
 499 //
 500 // Revision 1.2  1998/11/19 13:53:25  curt
 501 // Added a "Garmin" mode.
 502 //
 503 // Revision 1.1  1998/11/16 13:57:42  curt
 504 // Initial revision.
 505 //