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
27 # include <cstdlib> // atoi()
29 # include <stdlib.h> // atoi()
36 #include <Debug/logstream.hxx>
37 #include <Aircraft/aircraft.hxx>
38 #include <Include/fg_constants.h>
39 #include <Serial/serial.hxx>
40 #include <Time/fg_time.hxx>
42 #include "options.hxx"
44 #include "fg_serial.hxx"
49 // support an arbitrary number of serial channels. Each channel can
50 // be assigned to an arbitrary port. Bi-directional communication is
51 // supported by the underlying layer, but probably will never be
54 typedef vector < fgIOCHANNEL > io_container;
55 typedef io_container::iterator io_iterator;
56 typedef io_container::const_iterator const_io_iterator;
58 // define the four channels
59 io_container port_list;
62 fgIOCHANNEL::fgIOCHANNEL() :
63 kind( FG_SERIAL_DISABLED ),
69 fgIOCHANNEL::~fgIOCHANNEL() {
73 // configure a port based on the config string
74 static fgIOCHANNEL parse_port_config( const string& config )
78 string::size_type begin, end;
82 FG_LOG( FG_SERIAL, FG_INFO, "Parse serial port config: " << config );
85 end = config.find(",", begin);
86 if ( end == string::npos ) {
90 p.device = config.substr(begin, end - begin);
92 FG_LOG( FG_SERIAL, FG_INFO, " device = " << p.device );
95 end = config.find(",", begin);
96 if ( end == string::npos ) {
100 p.format = config.substr(begin, end - begin);
102 FG_LOG( FG_SERIAL, FG_INFO, " format = " << p.format );
105 end = config.find(",", begin);
106 if ( end == string::npos ) {
110 p.baud = config.substr(begin, end - begin);
112 FG_LOG( FG_SERIAL, FG_INFO, " baud = " << p.baud );
115 p.direction = config.substr(begin);
116 FG_LOG( FG_SERIAL, FG_INFO, " direction = " << p.direction );
118 p.valid_config = true;
124 // configure a port based on the config info
125 static bool config_port( fgIOCHANNEL &p )
127 if ( p.port.is_enabled() ) {
128 FG_LOG( FG_SERIAL, FG_ALERT, "This shouldn't happen, but the port "
129 << "is already in use, ignoring" );
133 if ( ! p.port.open_port( p.device ) ) {
134 FG_LOG( FG_SERIAL, FG_ALERT, "Error opening device: " << p.device );
138 // cout << "fd = " << p.port.fd << endl;
140 if ( ! p.port.set_baud( atoi( p.baud.c_str() ) ) ) {
141 FG_LOG( FG_SERIAL, FG_ALERT, "Error setting baud: " << p.baud );
145 if ( p.format == "nmea" ) {
146 if ( p.direction == "out" ) {
147 p.kind = fgIOCHANNEL::FG_SERIAL_NMEA_OUT;
148 } else if ( p.direction == "in" ) {
149 p.kind = fgIOCHANNEL::FG_SERIAL_NMEA_IN;
151 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
154 } else if ( p.format == "garmin" ) {
155 if ( p.direction == "out" ) {
156 p.kind = fgIOCHANNEL::FG_SERIAL_GARMIN_OUT;
157 } else if ( p.direction == "in" ) {
158 p.kind = fgIOCHANNEL::FG_SERIAL_GARMIN_IN;
160 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
163 } else if ( p.format == "fgfs" ) {
164 if ( p.direction == "out" ) {
165 p.kind = fgIOCHANNEL::FG_SERIAL_FGFS_OUT;
166 } else if ( p.direction == "in" ) {
167 p.kind = fgIOCHANNEL::FG_SERIAL_FGFS_IN;
169 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown direction" );
173 FG_LOG( FG_SERIAL, FG_ALERT, "Unknown format" );
181 // step through the port config streams (from fgOPTIONS) and setup
182 // serial port channels for each
183 void fgSerialInit() {
186 str_container port_options_list = current_options.get_port_options_list();
188 // we could almost do this in a single step except pushing a valid
189 // port onto the port list copies the structure and destroys the
190 // original, which closes the port and frees up the fd ... doh!!!
192 // parse the configuration strings and store the results in stub
193 // fgIOCHANNEL structures
194 const_str_iterator current_str = port_options_list.begin();
195 const_str_iterator last_str = port_options_list.end();
196 for ( ; current_str != last_str; ++current_str ) {
197 port = parse_port_config( *current_str );
198 if ( port.valid_config ) {
199 result = config_port( port );
201 port_list.push_back( port );
208 char calc_nmea_cksum(char *sentence) {
209 unsigned char sum = 0;
212 // printf("%s\n", sentence);
214 len = strlen(sentence);
216 for ( i = 1; i < len; i++ ) {
217 // printf("%c", sentence[i]);
222 // printf("sum = %02x\n", sum);
227 static void send_nmea_out( fgIOCHANNEL *p ) {
228 char rmc[256], gga[256];
229 char rmc_sum[10], gga_sum[10];
236 // run once every two seconds
237 if ( p->last_time == cur_time_params.cur_time ) {
240 p->last_time = cur_time_params.cur_time;
241 if ( cur_time_params.cur_time % 2 != 0 ) {
245 f = current_aircraft.fdm_state;
246 t = &cur_time_params;
249 sprintf( utc, "%02d%02d%02d",
250 t->gmt->tm_hour, t->gmt->tm_min, t->gmt->tm_sec );
253 double latd = f->get_Latitude() * RAD_TO_DEG;
261 min = (latd - (double)deg) * 60.0;
262 sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
265 double lond = f->get_Longitude() * RAD_TO_DEG;
273 min = (lond - (double)deg) * 60.0;
274 sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
277 sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
280 sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
283 sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
285 char altitude_ft[10];
286 sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
289 sprintf( date, "%02d%02d%02d",
290 t->gmt->tm_mday, t->gmt->tm_mon+1, t->gmt->tm_year );
292 // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
293 sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,0.000,E",
294 utc, lat, lon, speed, heading, date );
295 sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
297 sprintf( gga, "GPGGA,%s,%s,%s,1,,,%s,F,,,,",
298 utc, lat, lon, altitude_ft );
299 sprintf( gga_sum, "%02X", calc_nmea_cksum(gga) );
302 FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
303 FG_LOG( FG_SERIAL, FG_DEBUG, gga );
306 string rmc_sentence = "$";
309 rmc_sentence += rmc_sum;
310 rmc_sentence += "\n";
311 p->port.write_port(rmc_sentence);
312 // cout << rmc_sentence;
315 string gga_sentence = "$";
318 gga_sentence += gga_sum;
319 gga_sentence += "\n";
320 p->port.write_port(gga_sentence);
321 // cout << gga_sentence;
324 static void read_nmea_in( fgIOCHANNEL *p ) {
327 static void send_garmin_out( fgIOCHANNEL *p ) {
328 char rmc[256], rmc_sum[256], rmz[256], rmz_sum[256];
335 // run once per second
336 if ( p->last_time == cur_time_params.cur_time ) {
339 p->last_time = cur_time_params.cur_time;
340 if ( cur_time_params.cur_time % 2 != 0 ) {
344 f = current_aircraft.fdm_state;
345 t = &cur_time_params;
348 sprintf( utc, "%02d%02d%02d",
349 t->gmt->tm_hour, t->gmt->tm_min, t->gmt->tm_sec );
352 double latd = f->get_Latitude() * RAD_TO_DEG;
360 min = (latd - (double)deg) * 60.0;
361 sprintf( lat, "%02d%06.3f,%c", abs(deg), min, dir);
364 double lond = f->get_Longitude() * RAD_TO_DEG;
372 min = (lond - (double)deg) * 60.0;
373 sprintf( lon, "%03d%06.3f,%c", abs(deg), min, dir);
376 sprintf( speed, "%05.1f", f->get_V_equiv_kts() );
379 sprintf( heading, "%05.1f", f->get_Psi() * RAD_TO_DEG );
382 sprintf( altitude_m, "%02d", (int)(f->get_Altitude() * FEET_TO_METER) );
384 char altitude_ft[10];
385 sprintf( altitude_ft, "%02d", (int)f->get_Altitude() );
388 sprintf( date, "%02d%02d%02d",
389 t->gmt->tm_mday, t->gmt->tm_mon+1, t->gmt->tm_year );
391 // $GPRMC,HHMMSS,A,DDMM.MMM,N,DDDMM.MMM,W,XXX.X,XXX.X,DDMMYY,XXX.X,E*XX
392 sprintf( rmc, "GPRMC,%s,A,%s,%s,%s,%s,%s,000.0,E",
393 utc, lat, lon, speed, heading, date );
394 sprintf( rmc_sum, "%02X", calc_nmea_cksum(rmc) );
396 // sprintf( gga, "$GPGGA,%s,%s,%s,1,04,0.0,%s,M,00.0,M,,*00\r\n",
397 // utc, lat, lon, altitude_m );
399 sprintf( rmz, "PGRMZ,%s,f,3", altitude_ft );
400 sprintf( rmz_sum, "%02X", calc_nmea_cksum(rmz) );
402 FG_LOG( FG_SERIAL, FG_DEBUG, rmc );
403 FG_LOG( FG_SERIAL, FG_DEBUG, rmz );
406 string rmc_sentence = "$";
409 rmc_sentence += rmc_sum;
410 rmc_sentence += "\n";
411 p->port.write_port(rmc_sentence);
412 // cout << rmc_sentence;
414 // RMZ sentence (garmin proprietary)
415 string rmz_sentence = "$";
418 rmz_sentence += rmz_sum;
419 rmz_sentence += "\n";
420 p->port.write_port(rmz_sentence);
421 // cout << rmz_sentence;
424 static void read_garmin_in( fgIOCHANNEL *p ) {
427 static void send_fgfs_out( fgIOCHANNEL *p ) {
430 static void read_fgfs_in( fgIOCHANNEL *p ) {
434 // one more level of indirection ...
435 static void process_port( fgIOCHANNEL *p ) {
436 if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_OUT ) {
438 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_NMEA_IN ) {
440 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_OUT ) {
442 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_GARMIN_IN ) {
444 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_OUT ) {
446 } else if ( p->kind == fgIOCHANNEL::FG_SERIAL_FGFS_IN ) {
452 // process any serial port work
453 void fgSerialProcess() {
456 io_iterator current = port_list.begin();
457 io_iterator last = port_list.end();
459 for ( ; current != last; ++current ) {
461 if ( port->kind != fgIOCHANNEL::FG_SERIAL_DISABLED ) {
462 process_port ( port );