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 if ( p.format == "pve" ) {
 185         if ( p.direction == "out" ) {
 186             p.kind = fgIOCHANNEL::FG_SERIAL_PVE_OUT;
 187         } else if ( p.direction == "in" ) {
 188             FG_LOG( FG_SERIAL, FG_ALERT,
 189                     "ProVision Entertainment format is outgoing only" );
 190             return false;
 191         } else {
 192             FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
 193             return false;
 194         }
 195     } else {
 196         FG_LOG( FG_SERIAL, FG_ALERT, "Unknown format" );
 197         return false;
 198     }
 199
 200     return true;
 201 }
 202
 203
 204 // step through the port config streams (from fgOPTIONS) and setup
 205 // serial port channels for each
 206 void fgSerialInit() {
 207     fgIOCHANNEL port;
 208     bool result;
 209     str_container port_options_list = current_options.get_port_options_list();
 210
 211     // we could almost do this in a single step except pushing a valid
 212     // port onto the port list copies the structure and destroys the
 213     // original, which closes the port and frees up the fd ... doh!!!
 214
 215     // parse the configuration strings and store the results in stub
 216     // fgIOCHANNEL structures
 217     const_str_iterator current_str = port_options_list.begin();
 218     const_str_iterator last_str = port_options_list.end();
 219     for ( ; current_str != last_str; ++current_str ) {
 220         port = parse_port_config( *current_str );
 221         if ( port.valid_config ) {
 222             result = config_port( port );
 223             if ( result ) {
 224                 port_list.push_back( port );
 225             }
 226         }
 227     }
 228 }
 229
 230
 231 char calc_nmea_cksum(char *sentence) {
 232     unsigned char sum = 0;
 233     int i, len;
 234
 235     // printf("%s\n", sentence);
 236
 237     len = strlen(sentence);
 238     sum = sentence[0];
 239     for ( i = 1; i < len; i++ ) {
 240         // printf("%c", sentence[i]);
 241         sum ^= sentence[i];
 242     }
 243     // printf("\n");
 244
 245     // printf("sum = %02x\n", sum);
 246     return sum;
 247 }
 248
 249
 250 static void send_nmea_out( fgIOCHANNEL *p ) {
 251     char rmc[256], gga[256];
 252     char rmc_sum[10], gga_sum[10];
 253     char dir;
 254     int deg;
 255     double min;
 256     FGInterface *f;
 257     FGTime *t;
 258
 259     f = current_aircraft.fdm_state;
 260     t = FGTime::cur_time_params;
 261
 262     // run once every two seconds
 263     if ( p->last_time == t->get_cur_time() ) {
 264         return;
 265     }
 266     p->last_time = t->get_cur_time();
 267     if ( t->get_cur_time() % 2 != 0 ) {
 268         return;
 269     }
 270
 271     char utc[10];
 272     sprintf( utc, "%02d%02d%02d",
 273              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
 274
 275     char lat[20];
 276     double latd = f->get_Latitude() * RAD_TO_DEG;
 277     if ( latd < 0.0 ) {
 278         latd *= -1.0;
 279         dir = 'S';
 280     } else {
 281         dir = 'N';
 282     }
 283     deg = (int)(latd);
 284     min = (latd - (double)deg) * 60.0;
 285     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
 286
 287     char lon[20];
 288     double lond = f->get_Longitude() * RAD_TO_DEG;
 289     if ( lond < 0.0 ) {
 290         lond *= -1.0;
 291         dir = 'W';
 292     } else {
 293         dir = 'E';
 294     }
 295     deg = (int)(lond);
 296     min = (lond - (double)deg) * 60.0;
 297     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
 298
 299     char speed[10];
 300     sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
 301
 302     char heading[10];
 303     sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
 304
 305     char altitude_m[10];
 306     sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
 307
 308     char altitude_ft[10];
 309     sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
 310
 311     char date[10];
 312     sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
 313              t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
 314
 315     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
 316     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,0.000,E",
 317              utc, lat, lon, speed, heading, date );
 318     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
 319
 320     sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,",
 321              utc, lat, lon, altitude_ft );
 322     sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) );
 323
 324
 325     FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
 326     FG_LOG( FG_SERIAL, FG_DEBUG, gga );
 327
 328     // RMC sentence
 329     string rmc_sentence = "$";
 330     rmc_sentence += rmc;
 331     rmc_sentence += "*";
 332     rmc_sentence += rmc_sum;
 333     rmc_sentence += "\n";
 334     p->port.write_port(rmc_sentence);
 335     // cout << rmc_sentence;
 336
 337     // GGA sentence
 338     string gga_sentence = "$";
 339     gga_sentence += gga;
 340     gga_sentence += "*";
 341     gga_sentence += gga_sum;
 342     gga_sentence += "\n";
 343     p->port.write_port(gga_sentence);
 344     // cout << gga_sentence;
 345 }
 346
 347 static void read_nmea_in( fgIOCHANNEL *p ) {
 348 }
 349
 350 static void send_garmin_out( fgIOCHANNEL *p ) {
 351     char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256];
 352     char dir;
 353     int deg;
 354     double min;
 355     FGInterface *f;
 356     FGTime *t;
 357
 358     f = current_aircraft.fdm_state;
 359     t = FGTime::cur_time_params;
 360
 361     // run once per second
 362     if ( p->last_time == t->get_cur_time() ) {
 363         return;
 364     }
 365     p->last_time = t->get_cur_time();
 366     if ( t->get_cur_time() % 2 != 0 ) {
 367         return;
 368     }
 369
 370     char utc[10];
 371     sprintf( utc, "%02d%02d%02d",
 372              t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
 373
 374     char lat[20];
 375     double latd = f->get_Latitude() * RAD_TO_DEG;
 376     if ( latd < 0.0 ) {
 377         latd *= -1.0;
 378         dir = 'S';
 379     } else {
 380         dir = 'N';
 381     }
 382     deg = (int)(latd);
 383     min = (latd - (double)deg) * 60.0;
 384     sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
 385
 386     char lon[20];
 387     double lond = f->get_Longitude() * RAD_TO_DEG;
 388     if ( lond < 0.0 ) {
 389         lond *= -1.0;
 390         dir = 'W';
 391     } else {
 392         dir = 'E';
 393     }
 394     deg = (int)(lond);
 395     min = (lond - (double)deg) * 60.0;
 396     sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
 397
 398     char speed[10];
 399     sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
 400
 401     char heading[10];
 402     sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
 403
 404     char altitude_m[10];
 405     sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
 406
 407     char altitude_ft[10];
 408     sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
 409
 410     char date[10];
 411     sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
 412              t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
 413
 414     // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
 415     sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,000.0,E",
 416              utc, lat, lon, speed, heading, date );
 417     sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
 418
 419     // sprintf( gga, "$GPGGA,%s,%s,%s,1,04,0.0,%s,M,00.0,M,,*00\r\n",
 420     //          utc, lat, lon, altitude_m );
 421
 422     sprintf( rmz, "PGRMZ,%s,f,3", altitude_ft );
 423     sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) );
 424
 425     FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
 426     FG_LOG( FG_SERIAL, FG_DEBUG, rmz );
 427
 428     // RMC sentence
 429     string rmc_sentence = "$";
 430     rmc_sentence += rmc;
 431     rmc_sentence += "*";
 432     rmc_sentence += rmc_sum;
 433     rmc_sentence += "\n";
 434     p->port.write_port(rmc_sentence);
 435     // cout << rmc_sentence;
 436
 437     // RMZ sentence (garmin proprietary)
 438     string rmz_sentence = "$";
 439     rmz_sentence += rmz;
 440     rmz_sentence += "*";
 441     rmz_sentence += rmz_sum;
 442     rmz_sentence += "\n";
 443     p->port.write_port(rmz_sentence);
 444     // cout << rmz_sentence;
 445 }
 446
 447 static void read_garmin_in( fgIOCHANNEL *p ) {
 448 }
 449
 450 static void send_fgfs_out( fgIOCHANNEL *p ) {
 451 }
 452
 453 static void read_fgfs_in( fgIOCHANNEL *p ) {
 454 }
 455
 456
 457 // "RUL" output format (for some sort of motion platform)
 458 //
 459 // The Baud rate is 2400 , one start bit, eight data bits, 1 stop bit,
 460 // no parity.
 461 //
 462 // For position it requires a 3-byte data packet defined as follows:
 463 //
 464 // First bite: ascII character "P" ( 0x50 or 80 decimal )
 465 // Second byte X pos. (1-255) 1 being 0* and 255 being 359*
 466 // Third byte Y pos.( 1-255) 1 being 0* and 255 359*
 467 //
 468 // So sending 80 127 127 to the two axis motors will position on 180*
 469 // The RS- 232 port is a nine pin connector and the only pins used are
 470 // 3&5.
 471
 472 static void send_rul_out( fgIOCHANNEL *p ) {
 473     char rul[256];
 474
 475     FGInterface *f;
 476     FGTime *t;
 477
 478     f = current_aircraft.fdm_state;
 479     t = FGTime::cur_time_params;
 480
 481     // run as often as possibleonce per second
 482
 483     // this runs once per second
 484     // if ( p->last_time == t->get_cur_time() ) {
 485     //    return;
 486     // }
 487     // p->last_time = t->get_cur_time();
 488     // if ( t->get_cur_time() % 2 != 0 ) {
 489     //    return;
 490     // }
 491
 492     // get roll and pitch, convert to degrees
 493     double roll_deg = f->get_Phi() * RAD_TO_DEG;
 494     while ( roll_deg < -180.0 ) {
 495         roll_deg += 360.0;
 496     }
 497     while ( roll_deg > 180.0 ) {
 498         roll_deg -= 360.0;
 499     }
 500
 501     double pitch_deg = f->get_Theta() * RAD_TO_DEG;
 502     while ( pitch_deg < -180.0 ) {
 503         pitch_deg += 360.0;
 504     }
 505     while ( pitch_deg > 180.0 ) {
 506         pitch_deg -= 360.0;
 507     }
 508
 509     // scale roll and pitch to output format (1 - 255)
 510     // straight && level == (128, 128)
 511
 512     int roll = (int)( (roll_deg+180.0) * 255.0 / 360.0) + 1;
 513     int pitch = (int)( (pitch_deg+180.0) * 255.0 / 360.0) + 1;
 514
 515     sprintf( rul, "p%c%c\n", roll, pitch);
 516
 517     FG_LOG( FG_SERIAL, FG_INFO, "p " << roll << " " << pitch );
 518
 519     string rul_sentence = rul;
 520     p->port.write_port(rul_sentence);
 521 }
 522
 523
 524 // "PVE" (ProVision Entertainment) output format (for some sort of
 525 // motion platform)
 526 //
 527 // Outputs a 5-byte data packet defined as follows:
 528 //
 529 // First bite:  ASCII character "P" ( 0x50 or 80 decimal )
 530 // Second byte:  "roll" value (1-255) 1 being 0* and 255 being 359*
 531 // Third byte:  "pitch" value (1-255) 1 being 0* and 255 being 359*
 532 // Fourth byte:  "heave" value (or vertical acceleration?)
 533 //
 534 // So sending 80 127 127 to the two axis motors will position on 180*
 535 // The RS- 232 port is a nine pin connector and the only pins used are
 536 // 3&5.
 537
 538 static void send_pve_out( fgIOCHANNEL *p ) {
 539     char pve[256];
 540
 541     FGInterface *f;
 542     FGTime *t;
 543
 544
 545     f = current_aircraft.fdm_state;
 546     t = FGTime::cur_time_params;
 547
 548     // run as often as possibleonce per second
 549
 550     // this runs once per second
 551     // if ( p->last_time == t->get_cur_time() ) {
 552     //    return;
 553     // }
 554     // p->last_time = t->get_cur_time();
 555     // if ( t->get_cur_time() % 2 != 0 ) {
 556     //    return;
 557     // }
 558
 559     // get roll and pitch, convert to degrees
 560     double roll_deg = f->get_Phi() * RAD_TO_DEG;
 561     while ( roll_deg <= -180.0 ) {
 562         roll_deg += 360.0;
 563     }
 564     while ( roll_deg > 180.0 ) {
 565         roll_deg -= 360.0;
 566     }
 567
 568     double pitch_deg = f->get_Theta() * RAD_TO_DEG;
 569     while ( pitch_deg <= -180.0 ) {
 570         pitch_deg += 360.0;
 571     }
 572     while ( pitch_deg > 180.0 ) {
 573         pitch_deg -= 360.0;
 574     }
 575
 576     short int heave = (int)(f->get_W_body() * 128.0);
 577
 578     // scale roll and pitch to output format (1 - 255)
 579     // straight && level == (128, 128)
 580
 581     short int roll = (int)(roll_deg * 32768 / 180.0);
 582     short int pitch = (int)(pitch_deg * 32768 / 180.0);
 583
 584     unsigned char roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2;
 585     roll_b1 = roll >> 8;
 586     roll_b2 = roll & 0x00ff;
 587     pitch_b1 = pitch >> 8;
 588     pitch_b2 = pitch & 0x00ff;
 589     heave_b1 = heave >> 8;
 590     heave_b2 = heave & 0x00ff;
 591
 592     sprintf( pve, "p%c%c%c%c%c%c\n",
 593              roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2 );
 594
 595     // printf( "p [ %u %u ]  [ %u %u ]  [ %u %u ]\n",
 596     //         roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2 );
 597
 598     FG_LOG( FG_SERIAL, FG_INFO, "roll=" << roll << " pitch=" << pitch <<
 599             " heave=" << heave );
 600
 601     string pve_sentence = pve;
 602     p->port.write_port(pve_sentence);
 603 }
 604
 605
 606 // one more level of indirection ...
 607 static void process_port( fgIOCHANNEL *p ) {
 608     if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_OUT ) {
 609         send_nmea_out(p);
 610     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_IN ) {
 611         read_nmea_in(p);
 612     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_OUT ) {
 613         send_garmin_out(p);
 614     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_IN ) {
 615         read_garmin_in(p);
 616     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_OUT ) {
 617         send_fgfs_out(p);
 618     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_IN ) {
 619         read_fgfs_in(p);
 620     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_RUL_OUT ) {
 621         send_rul_out(p);
 622     } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_PVE_OUT ) {
 623         send_pve_out(p);
 624     }
 625 }
 626
 627
 628 // process any serial port work
 629 void fgSerialProcess() {
 630     fgIOCHANNEL *port;
 631
 632     io_iterator current = port_list.begin();
 633     io_iterator last = port_list.end();
 634
 635     for ( ; current != last; ++current ) {
 636         port = current;
 637         if ( port->kind != fgIOCHANNEL::FG_SERIAL_DISABLED ) {
 638             process_port ( port );
 639         }
 640     }
 641 }