1 // fg_serial.cxx -- higher level serial port management routines
3 // Written by Curtis Olson, started November 1998.
5 // Copyright (C) 1998 Curtis L. Olson - curt@flightgear.org
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.
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.
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.
24 #include <Include/compiler.h>
26 #ifdef FG_HAVE_STD_INCLUDES
28 # include <cstdlib> // atoi()
31 # include <stdlib.h> // atoi()
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>
44 #include "options.hxx"
46 #include "fg_serial.hxx"
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
56 typedef vector < fgIOCHANNEL > io_container;
57 typedef io_container::iterator io_iterator;
58 typedef io_container::const_iterator const_io_iterator;
60 // define the four channels
61 io_container port_list;
64 fgIOCHANNEL::fgIOCHANNEL() :
65 kind( FG_SERIAL_DISABLED ),
71 fgIOCHANNEL::~fgIOCHANNEL() {
75 // configure a port based on the config string
76 static fgIOCHANNEL parse_port_config( const string& config )
80 string::size_type begin, end;
84 FG_LOG( FG_SERIAL, FG_INFO, "Parse serial port config: " << config );
87 end = config.find(",", begin);
88 if ( end == string::npos ) {
92 p.device = config.substr(begin, end - begin);
94 FG_LOG( FG_SERIAL, FG_INFO, " device = " << p.device );
97 end = config.find(",", begin);
98 if ( end == string::npos ) {
102 p.format = config.substr(begin, end - begin);
104 FG_LOG( FG_SERIAL, FG_INFO, " format = " << p.format );
107 end = config.find(",", begin);
108 if ( end == string::npos ) {
112 p.baud = config.substr(begin, end - begin);
114 FG_LOG( FG_SERIAL, FG_INFO, " baud = " << p.baud );
117 p.direction = config.substr(begin);
118 FG_LOG( FG_SERIAL, FG_INFO, " direction = " << p.direction );
120 p.valid_config = true;
126 // configure a port based on the config info
127 static bool config_port( fgIOCHANNEL &p )
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" );
135 if ( ! p.port.open_port( p.device ) ) {
136 FG_LOG( FG_SERIAL, FG_ALERT, "Error opening device: " << p.device );
140 // cout << "fd = " << p.port.fd << endl;
142 if ( ! p.port.set_baud( atoi( p.baud.c_str() ) ) ) {
143 FG_LOG( FG_SERIAL, FG_ALERT, "Error setting baud: " << p.baud );
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;
153 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
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;
162 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
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;
171 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
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" );
181 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
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" );
192 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
196 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown format" );
204 // step through the port config streams (from fgOPTIONS) and setup
205 // serial port channels for each
206 void fgSerialInit() {
209 str_container port_options_list = current_options.get_port_options_list();
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!!!
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 );
224 port_list.push_back( port );
231 char calc_nmea_cksum(char *sentence) {
232 unsigned char sum = 0;
235 // printf("%s\n", sentence);
237 len = strlen(sentence);
239 for ( i = 1; i < len; i++ ) {
240 // printf("%c", sentence[i]);
245 // printf("sum = %02x\n", sum);
250 static void send_nmea_out( fgIOCHANNEL *p ) {
251 char rmc[256], gga[256];
252 char rmc_sum[10], gga_sum[10];
259 f = current_aircraft.fdm_state;
260 t = FGTime::cur_time_params;
262 // run once every two seconds
263 if ( p->last_time == t->get_cur_time() ) {
266 p->last_time = t->get_cur_time();
267 if ( t->get_cur_time() % 2 != 0 ) {
272 sprintf( utc, "%02d%02d%02d",
273 t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
276 double latd = f->get_Latitude() * RAD_TO_DEG;
284 min = (latd - (double)deg) * 60.0;
285 sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
288 double lond = f->get_Longitude() * RAD_TO_DEG;
296 min = (lond - (double)deg) * 60.0;
297 sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
300 sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
303 sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
306 sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
308 char altitude_ft[10];
309 sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
312 sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
313 t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
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) );
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) );
325 FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
326 FG_LOG( FG_SERIAL, FG_DEBUG, gga );
329 string rmc_sentence = "$";
332 rmc_sentence += rmc_sum;
333 rmc_sentence += "\n";
334 p->port.write_port(rmc_sentence);
335 // cout << rmc_sentence;
338 string gga_sentence = "$";
341 gga_sentence += gga_sum;
342 gga_sentence += "\n";
343 p->port.write_port(gga_sentence);
344 // cout << gga_sentence;
347 static void read_nmea_in( fgIOCHANNEL *p ) {
350 static void send_garmin_out( fgIOCHANNEL *p ) {
351 char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256];
358 f = current_aircraft.fdm_state;
359 t = FGTime::cur_time_params;
361 // run once per second
362 if ( p->last_time == t->get_cur_time() ) {
365 p->last_time = t->get_cur_time();
366 if ( t->get_cur_time() % 2 != 0 ) {
371 sprintf( utc, "%02d%02d%02d",
372 t->getGmt()->tm_hour, t->getGmt()->tm_min, t->getGmt()->tm_sec );
375 double latd = f->get_Latitude() * RAD_TO_DEG;
383 min = (latd - (double)deg) * 60.0;
384 sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
387 double lond = f->get_Longitude() * RAD_TO_DEG;
395 min = (lond - (double)deg) * 60.0;
396 sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
399 sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
402 sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
405 sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
407 char altitude_ft[10];
408 sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
411 sprintf( date, "%02d%02d%02d", t->getGmt()->tm_mday,
412 t->getGmt()->tm_mon+1, t->getGmt()->tm_year );
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) );
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 );
422 sprintf( rmz, "PGRMZ,%s,f,3", altitude_ft );
423 sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) );
425 FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
426 FG_LOG( FG_SERIAL, FG_DEBUG, rmz );
429 string rmc_sentence = "$";
432 rmc_sentence += rmc_sum;
433 rmc_sentence += "\n";
434 p->port.write_port(rmc_sentence);
435 // cout << rmc_sentence;
437 // RMZ sentence (garmin proprietary)
438 string rmz_sentence = "$";
441 rmz_sentence += rmz_sum;
442 rmz_sentence += "\n";
443 p->port.write_port(rmz_sentence);
444 // cout << rmz_sentence;
447 static void read_garmin_in( fgIOCHANNEL *p ) {
450 static void send_fgfs_out( fgIOCHANNEL *p ) {
453 static void read_fgfs_in( fgIOCHANNEL *p ) {
457 // "RUL" output format (for some sort of motion platform)
459 // The Baud rate is 2400 , one start bit, eight data bits, 1 stop bit,
462 // For position it requires a 3-byte data packet defined as follows:
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*
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
472 static void send_rul_out( fgIOCHANNEL *p ) {
478 f = current_aircraft.fdm_state;
479 t = FGTime::cur_time_params;
481 // run as often as possibleonce per second
483 // this runs once per second
484 // if ( p->last_time == t->get_cur_time() ) {
487 // p->last_time = t->get_cur_time();
488 // if ( t->get_cur_time() % 2 != 0 ) {
492 // get roll and pitch, convert to degrees
493 double roll_deg = f->get_Phi() * RAD_TO_DEG;
494 while ( roll_deg < -180.0 ) {
497 while ( roll_deg > 180.0 ) {
501 double pitch_deg = f->get_Theta() * RAD_TO_DEG;
502 while ( pitch_deg < -180.0 ) {
505 while ( pitch_deg > 180.0 ) {
509 // scale roll and pitch to output format (1 - 255)
510 // straight && level == (128, 128)
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;
515 sprintf( rul, "p%c%c\n", roll, pitch);
517 FG_LOG( FG_SERIAL, FG_INFO, "p " << roll << " " << pitch );
519 string rul_sentence = rul;
520 p->port.write_port(rul_sentence);
524 // "PVE" (ProVision Entertainment) output format (for some sort of
527 // Outputs a 5-byte data packet defined as follows:
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?)
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
538 static void send_pve_out( fgIOCHANNEL *p ) {
544 f = current_aircraft.fdm_state;
545 t = FGTime::cur_time_params;
547 // run as often as possibleonce per second
549 // this runs once per second
550 // if ( p->last_time == t->get_cur_time() ) {
553 // p->last_time = t->get_cur_time();
554 // if ( t->get_cur_time() % 2 != 0 ) {
558 // get roll and pitch, convert to degrees
559 int roll_deg = (int)(f->get_Phi() * RAD_TO_DEG);
560 while ( roll_deg < -180 ) {
563 while ( roll_deg > 179 ) {
567 int pitch_deg = (int)(f->get_Theta() * RAD_TO_DEG);
568 while ( pitch_deg < -180 ) {
571 while ( pitch_deg > 179 ) {
575 int heave = (int)(f->get_W_body());
577 // scale roll and pitch to output format (1 - 255)
578 // straight && level == (128, 128)
580 int roll = (int)( (roll_deg+180.0) * 255.0 / 360.0) + 1;
581 int pitch = (int)( (pitch_deg+180.0) * 255.0 / 360.0) + 1;
583 sprintf( pve, "p%c%c\n", roll, pitch);
585 FG_LOG( FG_SERIAL, FG_INFO, "roll=" << roll << " pitch=" << pitch <<
586 " heave=" << heave );
588 string pve_sentence = pve;
589 p->port.write_port(pve_sentence);
593 // one more level of indirection ...
594 static void process_port( fgIOCHANNEL *p ) {
595 if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_OUT ) {
597 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_IN ) {
599 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_OUT ) {
601 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_IN ) {
603 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_OUT ) {
605 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_IN ) {
607 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_RUL_OUT ) {
609 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_PVE_OUT ) {
615 // process any serial port work
616 void fgSerialProcess() {
619 io_iterator current = port_list.begin();
620 io_iterator last = port_list.end();
622 for ( ; current != last; ++current ) {
624 if ( port->kind != fgIOCHANNEL::FG_SERIAL_DISABLED ) {
625 process_port ( port );