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.
22 // (Log is kept at end of this file)
25 #include <Include/compiler.h>
27 #ifdef FG_HAVE_STD_INCLUDES
28 # include <cstdlib> // atoi()
30 # include <stdlib.h> // atoi()
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>
43 #include "options.hxx"
45 #include "fg_serial.hxx"
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
55 typedef vector < fgIOCHANNEL > io_container;
56 typedef io_container::iterator io_iterator;
57 typedef io_container::const_iterator const_io_iterator;
59 // define the four channels
60 io_container port_list;
63 fgIOCHANNEL::fgIOCHANNEL() :
64 kind( FG_SERIAL_DISABLED ),
70 fgIOCHANNEL::~fgIOCHANNEL() {
74 // configure a port based on the config string
75 static fgIOCHANNEL parse_port_config( const string& config )
79 string::size_type begin, end;
83 FG_LOG( FG_SERIAL, FG_INFO, "Parse serial port config: " << config );
86 end = config.find(",", begin);
87 if ( end == string::npos ) {
91 p.device = config.substr(begin, end - begin);
93 FG_LOG( FG_SERIAL, FG_INFO, " device = " << p.device );
96 end = config.find(",", begin);
97 if ( end == string::npos ) {
101 p.format = config.substr(begin, end - begin);
103 FG_LOG( FG_SERIAL, FG_INFO, " format = " << p.format );
106 end = config.find(",", begin);
107 if ( end == string::npos ) {
111 p.baud = config.substr(begin, end - begin);
113 FG_LOG( FG_SERIAL, FG_INFO, " baud = " << p.baud );
116 p.direction = config.substr(begin);
117 FG_LOG( FG_SERIAL, FG_INFO, " direction = " << p.direction );
119 p.valid_config = true;
125 // configure a port based on the config info
126 static bool config_port( fgIOCHANNEL &p )
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" );
134 if ( ! p.port.open_port( p.device ) ) {
135 FG_LOG( FG_SERIAL, FG_ALERT, "Error opening device: " << p.device );
139 // cout << "fd = " << p.port.fd << endl;
141 if ( ! p.port.set_baud( atoi( p.baud.c_str() ) ) ) {
142 FG_LOG( FG_SERIAL, FG_ALERT, "Error setting baud: " << p.baud );
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;
152 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
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;
161 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
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;
170 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
174 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown format" );
182 // step through the port config streams (from fgOPTIONS) and setup
183 // serial port channels for each
184 void fgSerialInit() {
187 str_container port_options_list = current_options.get_port_options_list();
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!!!
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 );
202 port_list.push_back( port );
209 char calc_nmea_cksum(char *sentence) {
210 unsigned char sum = 0;
213 // printf("%s\n", sentence);
215 len = strlen(sentence);
217 for ( i = 1; i < len; i++ ) {
218 // printf("%c", sentence[i]);
223 // printf("sum = %02x\n", sum);
228 static void send_nmea_out( fgIOCHANNEL *p ) {
229 char rmc[256], gga[256];
230 char rmc_sum[10], gga_sum[10];
237 // run once every two seconds
238 if ( p->last_time == cur_time_params.cur_time ) {
241 p->last_time = cur_time_params.cur_time;
242 if ( cur_time_params.cur_time % 2 != 0 ) {
246 f = current_aircraft.fdm_state;
247 t = &cur_time_params;
250 sprintf( utc, "%02d%02d%02d",
251 t->gmt->tm_hour, t->gmt->tm_min, t->gmt->tm_sec );
254 double latd = f->get_Latitude() * RAD_TO_DEG;
262 min = (latd - (double)deg) * 60.0;
263 sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
266 double lond = f->get_Longitude() * RAD_TO_DEG;
274 min = (lond - (double)deg) * 60.0;
275 sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
278 sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
281 sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
284 sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
286 char altitude_ft[10];
287 sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
290 sprintf( date, "%02d%02d%02d",
291 t->gmt->tm_mday, t->gmt->tm_mon+1, t->gmt->tm_year );
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) );
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) );
303 FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
304 FG_LOG( FG_SERIAL, FG_DEBUG, gga );
307 string rmc_sentence = "$";
310 rmc_sentence += rmc_sum;
311 rmc_sentence += "\n";
312 p->port.write_port(rmc_sentence);
313 // cout << rmc_sentence;
316 string gga_sentence = "$";
319 gga_sentence += gga_sum;
320 gga_sentence += "\n";
321 p->port.write_port(gga_sentence);
322 // cout << gga_sentence;
325 static void read_nmea_in( fgIOCHANNEL *p ) {
328 static void send_garmin_out( fgIOCHANNEL *p ) {
329 char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256];
336 // run once per second
337 if ( p->last_time == cur_time_params.cur_time ) {
340 p->last_time = cur_time_params.cur_time;
341 if ( cur_time_params.cur_time % 2 != 0 ) {
345 f = current_aircraft.fdm_state;
346 t = &cur_time_params;
349 sprintf( utc, "%02d%02d%02d",
350 t->gmt->tm_hour, t->gmt->tm_min, t->gmt->tm_sec );
353 double latd = f->get_Latitude() * RAD_TO_DEG;
361 min = (latd - (double)deg) * 60.0;
362 sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
365 double lond = f->get_Longitude() * RAD_TO_DEG;
373 min = (lond - (double)deg) * 60.0;
374 sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
377 sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
380 sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
383 sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
385 char altitude_ft[10];
386 sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
389 sprintf( date, "%02d%02d%02d",
390 t->gmt->tm_mday, t->gmt->tm_mon+1, t->gmt->tm_year );
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) );
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 );
400 sprintf( rmz, "PGRMZ,%s,f,3", altitude_ft );
401 sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) );
403 FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
404 FG_LOG( FG_SERIAL, FG_DEBUG, rmz );
407 string rmc_sentence = "$";
410 rmc_sentence += rmc_sum;
411 rmc_sentence += "\n";
412 p->port.write_port(rmc_sentence);
413 // cout << rmc_sentence;
415 // RMZ sentence (garmin proprietary)
416 string rmz_sentence = "$";
419 rmz_sentence += rmz_sum;
420 rmz_sentence += "\n";
421 p->port.write_port(rmz_sentence);
422 // cout << rmz_sentence;
425 static void read_garmin_in( fgIOCHANNEL *p ) {
428 static void send_fgfs_out( fgIOCHANNEL *p ) {
431 static void read_fgfs_in( fgIOCHANNEL *p ) {
435 // one more level of indirection ...
436 static void process_port( fgIOCHANNEL *p ) {
437 if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_OUT ) {
439 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_IN ) {
441 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_OUT ) {
443 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_IN ) {
445 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_OUT ) {
447 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_IN ) {
453 // process any serial port work
454 void fgSerialProcess() {
457 io_iterator current = port_list.begin();
458 io_iterator last = port_list.end();
460 for ( ; current != last; ++current ) {
462 if ( port->kind != fgIOCHANNEL::FG_SERIAL_DISABLED ) {
463 process_port ( port );
470 // Revision 1.1 1999/04/05 21:32:46 curt
473 // Revision 1.13 1999/03/02 01:03:16 curt
474 // Tweaks for building with native SGI compilers.
476 // Revision 1.12 1999/02/26 22:09:50 curt
477 // Added initial support for native SGI compilers.
479 // Revision 1.11 1999/02/05 21:29:11 curt
480 // Modifications to incorporate Jon S. Berndts flight model code.
482 // Revision 1.10 1999/01/21 00:55:01 curt
483 // Fixed some problems with timing of output strings.
484 // Added checksum support for nmea and garmin output.
486 // Revision 1.9 1999/01/20 13:42:26 curt
487 // Tweaked FDM interface.
488 // Testing check sum support for NMEA serial output.
490 // Revision 1.8 1999/01/19 20:57:04 curt
491 // MacOS portability changes contributed by "Robert Puyol" <puyol@abvent.fr>
493 // Revision 1.7 1998/12/05 15:54:21 curt
494 // Renamed class fgFLIGHT to class FGState as per request by JSB.
496 // Revision 1.6 1998/12/03 01:17:18 curt
497 // Converted fgFLIGHT to a class.
499 // Revision 1.5 1998/11/30 17:43:32 curt
500 // Lots of tweaking to get serial output to actually work.
502 // Revision 1.4 1998/11/25 01:33:58 curt
503 // Support for an arbitrary number of serial ports.
505 // Revision 1.3 1998/11/23 20:51:51 curt
506 // Tweaking serial stuff.
508 // Revision 1.2 1998/11/19 13:53:25 curt
509 // Added a "Garmin" mode.
511 // Revision 1.1 1998/11/16 13:57:42 curt