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