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 <stdlib.h> // atoi()
29 #include "Include/fg_stl_config.h"
31 #ifdef NEEDNAMESPACESTD
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>
41 #include "options.hxx"
43 #include "fg_serial.hxx"
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
51 typedef vector < fgIOCHANNEL > io_container;
52 typedef io_container::iterator io_iterator;
53 typedef io_container::const_iterator const_io_iterator;
55 // define the four channels
56 io_container port_list;
59 fgIOCHANNEL::fgIOCHANNEL() :
60 kind( FG_SERIAL_DISABLED ),
66 fgIOCHANNEL::~fgIOCHANNEL() {
70 // configure a port based on the config string
71 static fgIOCHANNEL parse_port_config( const string& config )
75 string::size_type begin, end;
79 FG_LOG( FG_SERIAL, FG_INFO, "Parse serial port config: " << config );
82 end = config.find(",", begin);
83 if ( end == string::npos ) {
87 p.device = config.substr(begin, end - begin);
89 FG_LOG( FG_SERIAL, FG_INFO, " device = " << p.device );
92 end = config.find(",", begin);
93 if ( end == string::npos ) {
97 p.format = config.substr(begin, end - begin);
99 FG_LOG( FG_SERIAL, FG_INFO, " format = " << p.format );
102 end = config.find(",", begin);
103 if ( end == string::npos ) {
107 p.baud = config.substr(begin, end - begin);
109 FG_LOG( FG_SERIAL, FG_INFO, " baud = " << p.baud );
112 p.direction = config.substr(begin);
113 FG_LOG( FG_SERIAL, FG_INFO, " direction = " << p.direction );
115 p.valid_config = true;
121 // configure a port based on the config info
122 static bool config_port( fgIOCHANNEL &p )
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" );
130 if ( ! p.port.open_port( p.device ) ) {
131 FG_LOG( FG_SERIAL, FG_ALERT, "Error opening device: " << p.device );
135 // cout << "fd = " << p.port.fd << endl;
137 if ( ! p.port.set_baud( atoi( p.baud.c_str() ) ) ) {
138 FG_LOG( FG_SERIAL, FG_ALERT, "Error setting baud: " << p.baud );
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;
148 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
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;
157 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
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;
166 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
170 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown format" );
178 // step through the port config streams (from fgOPTIONS) and setup
179 // serial port channels for each
180 void fgSerialInit() {
183 str_container port_options_list = current_options.get_port_options_list();
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!!!
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 );
198 port_list.push_back( port );
205 char calc_nmea_cksum(char *sentence) {
206 unsigned char sum = 0;
209 // printf("%s\n", sentence);
211 len = strlen(sentence);
213 for ( i = 1; i < len; i++ ) {
214 // printf("%c", sentence[i]);
219 // printf("sum = %02x\n", sum);
224 static void send_nmea_out( fgIOCHANNEL& p ) {
225 char rmc[256], gga[256];
226 char rmc_sum[10], gga_sum[10];
233 // run once every two seconds
234 if ( p.last_time == cur_time_params.cur_time ) {
237 p.last_time = cur_time_params.cur_time;
238 if ( cur_time_params.cur_time % 2 != 0 ) {
242 f = current_aircraft.flight;
243 t = &cur_time_params;
246 sprintf( utc, "%02d%02d%02d",
247 t->gmt->tm_hour, t->gmt->tm_min, t->gmt->tm_sec );
250 double latd = f->get_Latitude() * RAD_TO_DEG;
258 min = (latd - (double)deg) * 60.0;
259 sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
262 double lond = f->get_Longitude() * RAD_TO_DEG;
270 min = (lond - (double)deg) * 60.0;
271 sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
274 sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
277 sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
280 sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
282 char altitude_ft[10];
283 sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
286 sprintf( date, "%02d%02d%02d",
287 t->gmt->tm_mday, t->gmt->tm_mon+1, t->gmt->tm_year );
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)*/ );
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)*/ );
299 FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
300 FG_LOG( FG_SERIAL, FG_DEBUG, gga );
303 string rmc_sentence = "$";
306 rmc_sentence += rmc_sum;
307 rmc_sentence += "\n";
308 p.port.write_port(rmc_sentence);
309 cout << rmc_sentence;
312 string gga_sentence = "$";
315 gga_sentence += gga_sum;
316 gga_sentence += "\n";
317 p.port.write_port(gga_sentence);
318 cout << gga_sentence;
321 static void read_nmea_in( fgIOCHANNEL& p ) {
324 static void send_garmin_out( fgIOCHANNEL& p ) {
325 char rmc[256], rmz[256];
332 // run once per second
333 if ( p.last_time == cur_time_params.cur_time ) {
336 p.last_time = cur_time_params.cur_time;
337 if ( cur_time_params.cur_time % 2 != 0 ) {
341 f = current_aircraft.flight;
342 t = &cur_time_params;
345 sprintf( utc, "%02d%02d%02d",
346 t->gmt->tm_hour, t->gmt->tm_min, t->gmt->tm_sec );
349 double latd = f->get_Latitude() * RAD_TO_DEG;
357 min = (latd - (double)deg) * 60.0;
358 sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
361 double lond = f->get_Longitude() * RAD_TO_DEG;
369 min = (lond - (double)deg) * 60.0;
370 sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
373 sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
376 sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
379 sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
381 char altitude_ft[10];
382 sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
385 sprintf( date, "%02d%02d%02d",
386 t->gmt->tm_mday, t->gmt->tm_mon+1, t->gmt->tm_year );
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 );
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 );
395 sprintf( rmz, "$PGRMZ,%s,f,3*00\r\n", altitude_ft );
397 FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
398 FG_LOG( FG_SERIAL, FG_DEBUG, rmz );
401 p.port.write_port(rmc);
404 // RMZ sentence (garmin proprietary)
405 p.port.write_port(rmz);
409 static void read_garmin_in( fgIOCHANNEL& p ) {
412 static void send_fgfs_out( fgIOCHANNEL& p ) {
415 static void read_fgfs_in( fgIOCHANNEL& p ) {
419 // one more level of indirection ...
420 static void process_port( fgIOCHANNEL& p ) {
421 if ( p.kind == fgIOCHANNEL::FG_SERIAL_NMEA_OUT ) {
423 } else if ( p.kind == fgIOCHANNEL::FG_SERIAL_NMEA_IN ) {
425 } else if ( p.kind == fgIOCHANNEL::FG_SERIAL_GARMIN_OUT ) {
427 } else if ( p.kind == fgIOCHANNEL::FG_SERIAL_GARMIN_IN ) {
429 } else if ( p.kind == fgIOCHANNEL::FG_SERIAL_FGFS_OUT ) {
431 } else if ( p.kind == fgIOCHANNEL::FG_SERIAL_FGFS_IN ) {
437 // process any serial port work
438 void fgSerialProcess() {
441 const_io_iterator current = port_list.begin();
442 const_io_iterator last = port_list.end();
444 for ( ; current != last; ++current ) {
446 if ( port.kind != fgIOCHANNEL::FG_SERIAL_DISABLED ) {
447 process_port ( port );
454 // Revision 1.6 1998/12/03 01:17:18 curt
455 // Converted fgFLIGHT to a class.
457 // Revision 1.5 1998/11/30 17:43:32 curt
458 // Lots of tweaking to get serial output to actually work.
460 // Revision 1.4 1998/11/25 01:33:58 curt
461 // Support for an arbitrary number of serial ports.
463 // Revision 1.3 1998/11/23 20:51:51 curt
464 // Tweaking serial stuff.
466 // Revision 1.2 1998/11/19 13:53:25 curt
467 // Added a "Garmin" mode.
469 // Revision 1.1 1998/11/16 13:57:42 curt