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