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