src/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
  23
  24 #include <Include/compiler.h>
  25
  26 #ifdef FG_HAVE_STD_INCLUDES
  27 #  include <cmath>
  28 #  include <cstdlib>    // atoi()
  29 #else
  30 #  include <math.h>
  31 #  include <stdlib.h>   // atoi()
  32 #endif
  33
  34 #include STL_STRING
  35 #include STL_IOSTREAM
  36 #include <vector>
  37
  38 #include <Debug/logstream.hxx>
  39 #include <Aircraft/aircraft.hxx>
  40 #include <Include/fg_constants.h>
  41 #include <Serial/serial.hxx>
  42 #include <Time/fg_time.hxx>
  43
  44 #include "options.hxx"
  45
  46 #include "fg_serial.hxx"
  47
  48 FG_USING_STD(string);
  49 FG_USING_STD(vector);
  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 if ( p.format == "rul" ) {
 175         if ( p.direction == "out" ) {
 176             p.kind = fgIOCHANNEL::FG_SERIAL_RUL_OUT;
 177         } else if ( p.direction == "in" ) {
 178             FG_LOG( FG_SERIAL, FG_ALERT, "RUL format is outgoing only" );
 179             return false;
 180         } else {
 181             FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
 182             return false;
 183         }
 184     } else {
 185         FG_LOG( FG_SERIAL, FG_ALERT, "Unknown format" );
 186         return false;
 187     }
 188
 189     return true;
 190 }
 191
 192
 193 // step through the port config streams (from fgOPTIONS) and setup
 194 // serial port channels for each
 195 void fgSerialInit() {
 196     fgIOCHANNEL port;
 197     bool result;
 198     str_container port_options_list = current_options.get_port_options_list();
 199
 200     // we could almost do this in a single step except pushing a valid
 201     // port onto the port list copies the structure and destroys the
 202     // original, which closes the port and frees up the fd ... doh!!!
 203
 204     // parse the configuration strings and store the results in stub
 205     // fgIOCHANNEL structures
 206     const_str_iterator current_str = port_options_list.begin();
 207     const_str_iterator last_str = port_options_list.end();
 208     for ( ; current_str != last_str; ++current_str ) {
 209         port = parse_port_config( *current_str );
 210         if ( port.valid_config ) {
 211             result = config_port( port );
 212             if ( result ) {
 213                 port_list.push_back( port );
 214             }
 215         }
 216     }
 217 }
 218
 219
 220 char calc_nmea_cksum(char *sentence) {
 221     unsigned char sum = 0;
 222     int i, len;
 223
 224     // printf("%s\n", sentence);
 225
 226     len = strlen(sentence);
 227     sum = sentence[0];
 228     for ( i = 1; i < len; i++ ) {
 229         // printf("%c", sentence[i]);
 230         sum ^= sentence[i];
 231     }
 232     // printf("\n");
 233
 234     // printf("sum = %02x\n", sum);
 235     return sum;
 236 }
 237
 238
 239 static void send_nmea_out( fgIOCHANNEL *p ) {
 240     char rmc[256], gga[256];
 241     char rmc_sum[10], gga_sum[10];
 242     char dir;
 243     int deg;
 244     double min;
 245     FGInterface *f;
 246     FGTime *t;
 247
 248     f = current_aircraft.fdm_state;
 249     t = FGTime::cur_time_params;
 250
 251     // run once every two seconds
 252     if ( p->last_time == t->get_cur_time() ) {
 253         return;
 254     }
 255     p->last_time = t->get_cur_time();
 256     if ( t->get_cur_time() % 2 != 0 ) {
 257         return;
 258     }
 259
 260     char utc[10];
 261     sprintf( utc, "%02d%02d%02d",
 262              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
 263
 264     char lat[20];
 265     double latd = f->get_Latitude() * RAD_TO_DEG;
 266     if ( latd < 0.0 ) {
 267         latd *= -1.0;
 268         dir = 'S';
 269     } else {
 270         dir = 'N';
 271     }
 272     deg = (int)(latd);
 273     min = (latd - (double)deg) * 60.0;
 274     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
 275
 276     char lon[20];
 277     double lond = f->get_Longitude() * RAD_TO_DEG;
 278     if ( lond < 0.0 ) {
 279         lond *= -1.0;
 280         dir = 'W';
 281     } else {
 282         dir = 'E';
 283     }
 284     deg = (int)(lond);
 285     min = (lond - (double)deg) * 60.0;
 286     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
 287
 288     char speed[10];
 289     sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
 290
 291     char heading[10];
 292     sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
 293
 294     char altitude_m[10];
 295     sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
 296
 297     char altitude_ft[10];
 298     sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
 299
 300     char date[10];
 301     sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
 302              t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
 303
 304     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
 305     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,0.000,E",
 306              utc, lat, lon, speed, heading, date );
 307     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
 308
 309     sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,",
 310              utc, lat, lon, altitude_ft );
 311     sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) );
 312
 313
 314     FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
 315     FG_LOG( FG_SERIAL, FG_DEBUG, gga );
 316
 317     // RMC sentence
 318     string rmc_sentence = "$";
 319     rmc_sentence += rmc;
 320     rmc_sentence += "*";
 321     rmc_sentence += rmc_sum;
 322     rmc_sentence += "\n";
 323     p->port.write_port(rmc_sentence);
 324     // cout << rmc_sentence;
 325
 326     // GGA sentence
 327     string gga_sentence = "$";
 328     gga_sentence += gga;
 329     gga_sentence += "*";
 330     gga_sentence += gga_sum;
 331     gga_sentence += "\n";
 332     p->port.write_port(gga_sentence);
 333     // cout << gga_sentence;
 334 }
 335
 336 static void read_nmea_in( fgIOCHANNEL *p ) {
 337 }
 338
 339 static void send_garmin_out( fgIOCHANNEL *p ) {
 340     char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256];
 341     char dir;
 342     int deg;
 343     double min;
 344     FGInterface *f;
 345     FGTime *t;
 346
 347     f = current_aircraft.fdm_state;
 348     t = FGTime::cur_time_params;
 349
 350     // run once per second
 351     if ( p->last_time == t->get_cur_time() ) {
 352         return;
 353     }
 354     p->last_time = t->get_cur_time();
 355     if ( t->get_cur_time() % 2 != 0 ) {
 356         return;
 357     }
 358
 359     char utc[10];
 360     sprintf( utc, "%02d%02d%02d",
 361              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
 362
 363     char lat[20];
 364     double latd = f->get_Latitude() * RAD_TO_DEG;
 365     if ( latd < 0.0 ) {
 366         latd *= -1.0;
 367         dir = 'S';
 368     } else {
 369         dir = 'N';
 370     }
 371     deg = (int)(latd);
 372     min = (latd - (double)deg) * 60.0;
 373     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
 374
 375     char lon[20];
 376     double lond = f->get_Longitude() * RAD_TO_DEG;
 377     if ( lond < 0.0 ) {
 378         lond *= -1.0;
 379         dir = 'W';
 380     } else {
 381         dir = 'E';
 382     }
 383     deg = (int)(lond);
 384     min = (lond - (double)deg) * 60.0;
 385     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
 386
 387     char speed[10];
 388     sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
 389
 390     char heading[10];
 391     sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
 392
 393     char altitude_m[10];
 394     sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
 395
 396     char altitude_ft[10];
 397     sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
 398
 399     char date[10];
 400     sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
 401              t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
 402
 403     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
 404     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,000.0,E",
 405              utc, lat, lon, speed, heading, date );
 406     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
 407
 408     // sprintf( gga, "$GPGGA,%s,%s,%s,1,04,0.0,%s,M,00.0,M,,*00\r\n",
 409     //          utc, lat, lon, altitude_m );
 410
 411     sprintf( rmz, "PGRMZ,%s,f,3", altitude_ft );
 412     sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) );
 413
 414     FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
 415     FG_LOG( FG_SERIAL, FG_DEBUG, rmz );
 416
 417     // RMC sentence
 418     string rmc_sentence = "$";
 419     rmc_sentence += rmc;
 420     rmc_sentence += "*";
 421     rmc_sentence += rmc_sum;
 422     rmc_sentence += "\n";
 423     p->port.write_port(rmc_sentence);
 424     // cout << rmc_sentence;
 425
 426     // RMZ sentence (garmin proprietary)
 427     string rmz_sentence = "$";
 428     rmz_sentence += rmz;
 429     rmz_sentence += "*";
 430     rmz_sentence += rmz_sum;
 431     rmz_sentence += "\n";
 432     p->port.write_port(rmz_sentence);
 433     // cout << rmz_sentence;
 434 }
 435
 436 static void read_garmin_in( fgIOCHANNEL *p ) {
 437 }
 438
 439 static void send_fgfs_out( fgIOCHANNEL *p ) {
 440 }
 441
 442 static void read_fgfs_in( fgIOCHANNEL *p ) {
 443 }
 444
 445
 446 // "RUL" output format (for some sort of motion platform)
 447 //
 448 // The Baud rate is 2400 , one start bit, eight data bits, 1 stop bit,
 449 // no parity.
 450 //
 451 // For position it requires a 3-byte data packet defined as follows:
 452 //
 453 // First bite: ascII character"p" ( 80 decimal )
 454 // Second byte X pos. (1-255) 1 being 0* and 255 being 359*
 455 // Third byte Y pos.( 1-255) 1 being 0* and 255 359*
 456 //
 457 // So sending 80 127 127 to the two axis motors will position on 180*
 458 // The RS- 232 port is a nine pin connector and the only pins used are
 459 // 3&5.
 460
 461 static void send_rul_out( fgIOCHANNEL *p ) {
 462     char rul[256];
 463
 464     FGInterface *f;
 465     FGTime *t;
 466
 467     f = current_aircraft.fdm_state;
 468     t = FGTime::cur_time_params;
 469
 470     // run as often as possibleonce per second
 471
 472     // this runs once per second
 473     // if ( p->last_time == t->get_cur_time() ) {
 474     //    return;
 475     // }
 476     // p->last_time = t->get_cur_time();
 477     // if ( t->get_cur_time() % 2 != 0 ) {
 478     //    return;
 479     // }
 480
 481     // get roll and pitch, convert to degrees
 482     double roll_deg = f->get_Phi() * RAD_TO_DEG;
 483     while ( roll_deg < -180.0 ) {
 484         roll_deg += 360.0;
 485     }
 486     while ( roll_deg > 180.0 ) {
 487         roll_deg -= 360.0;
 488     }
 489
 490     double pitch_deg = f->get_Theta() * RAD_TO_DEG;
 491     while ( pitch_deg < -180.0 ) {
 492         pitch_deg += 360.0;
 493     }
 494     while ( pitch_deg > 180.0 ) {
 495         pitch_deg -= 360.0;
 496     }
 497
 498     // scale roll and pitch to output format (1 - 255)
 499     // straight && level == (128, 128)
 500
 501     int roll = (int)( (roll_deg+180.0) * 255.0 / 360.0) + 1;
 502     int pitch = (int)( (pitch_deg+180.0) * 255.0 / 360.0) + 1;
 503
 504     sprintf( rul, "p%c%c\n", roll, pitch);
 505
 506     FG_LOG( FG_SERIAL, FG_INFO, "p " << roll << " " << pitch );
 507
 508     string rul_sentence = rul;
 509     p->port.write_port(rul_sentence);
 510 }
 511
 512
 513 // one more level of indirection ...
 514 static void process_port( fgIOCHANNEL *p ) {
 515     if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_OUT ) {
 516         send_nmea_out(p);
 517     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_IN ) {
 518         read_nmea_in(p);
 519     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_OUT ) {
 520         send_garmin_out(p);
 521     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_IN ) {
 522         read_garmin_in(p);
 523     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_OUT ) {
 524         send_fgfs_out(p);
 525     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_IN ) {
 526         read_fgfs_in(p);
 527     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_RUL_OUT ) {
 528         send_rul_out(p);
 529     }
 530 }
 531
 532
 533 // process any serial port work
 534 void fgSerialProcess() {
 535     fgIOCHANNEL *port;
 536
 537     io_iterator current = port_list.begin();
 538     io_iterator last = port_list.end();
 539
 540     for ( ; current != last; ++current ) {
 541         port = current;
 542         if ( port->kind != fgIOCHANNEL::FG_SERIAL_DISABLED ) {
 543             process_port ( port );
 544         }
 545     }
 546 }