src/Network/AV400.cxx

   1 // AV400.cxx -- Garmin 400 series protocal class
   2 //
   3 // Written by Curtis Olson, started August 2006.
   4 //
   5 // Copyright (C) 2006  Curtis L. Olson - http://www.flightgear.org/~curt
   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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20 //
  21 // $Id$
  22
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include "config.h"
  26 #endif
  27
  28 #include <simgear/debug/logstream.hxx>
  29 #include <simgear/math/sg_geodesy.hxx>
  30 #include <simgear/io/iochannel.hxx>
  31 #include <simgear/timing/sg_time.hxx>
  32
  33 #include <FDM/flight.hxx>
  34 #include <Main/fg_props.hxx>
  35 #include <Main/globals.hxx>
  36
  37 #include "AV400.hxx"
  38
  39 FGAV400::FGAV400() {
  40 }
  41
  42 FGAV400::~FGAV400() {
  43 }
  44
  45
  46 // calculate the garmin check sum
  47 static char calc_nmea_cksum(char *sentence) {
  48     unsigned char sum = 0;
  49     int i, len;
  50
  51     // cout << sentence << endl;
  52
  53     len = strlen(sentence);
  54     sum = sentence[0];
  55     for ( i = 1; i < len; i++ ) {
  56         // cout << sentence[i];
  57         sum ^= sentence[i];
  58     }
  59     // cout << endl;
  60
  61     // printf("sum = %02x\n", sum);
  62     return sum;
  63 }
  64
  65
  66 // generate AV400 message
  67 bool FGAV400::gen_message() {
  68     // cout << "generating garmin message" << endl;
  69
  70     char msg_z[32], msg_A[32], msg_B[32], msg_C[32], msg_D[32];
  71     char msg_Q[32], msg_T[32], msg_type2[256];
  72     // the following could be implemented, but currently are unused
  73     // char msg_E[32], msg_G[32], msg_I[32], msg_K[32], msg_L[32], msg_S[32];
  74     // char msg_l[32];
  75
  76     char dir;
  77     int deg;
  78     double min;
  79
  80     // create msg_z
  81     sprintf( msg_z, "z%05.0f\r\n", cur_fdm_state->get_Altitude() );
  82
  83     // create msg_A
  84     sprintf( msg_A, "A");
  85
  86     double latd = cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES;
  87     if ( latd < 0.0 ) {
  88         latd = -latd;
  89         dir = 'S';
  90     } else {
  91         dir = 'N';
  92     }
  93     deg = (int)latd;
  94     min = (latd - (double)deg) * 60.0 * 100.0;
  95     sprintf( msg_A, "A%c %02d %04.0f\r\n", dir, deg, min);
  96
  97     // create msg_B
  98     double lond = cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES;
  99     if ( lond < 0.0 ) {
 100         lond = -lond;
 101         dir = 'W';
 102     } else {
 103         dir = 'E';
 104     }
 105     deg = (int)lond;
 106     min = (lond - (double)deg) * 60.0 * 100.0;
 107     sprintf( msg_B, "B%c %03d %04.0f\r\n", dir, deg, min);
 108
 109     // create msg_C
 110     float magdeg = fgGetDouble( "/environment/magnetic-variation-deg" );
 111     double vn = fgGetDouble( "/velocities/speed-north-fps" );
 112     double ve = fgGetDouble( "/velocities/speed-east-fps" );
 113     double gnd_trk_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
 114     double gnd_trk_mag = gnd_trk_true - magdeg;
 115     if ( gnd_trk_mag < 0.0 ) { gnd_trk_mag += 360.0; }
 116     if ( gnd_trk_mag >= 360.0 ) { gnd_trk_mag -= 360.0; }
 117     sprintf( msg_C, "C%03.0f\r\n", gnd_trk_mag);
 118
 119     // create msg_D
 120     double speed_kt = sqrt( vn*vn + ve*ve ) * SG_FPS_TO_KT;
 121     if ( speed_kt > 999.0 ) {
 122         speed_kt = 999.0;
 123     }
 124     sprintf( msg_D, "D%03.0f\r\n", speed_kt);
 125
 126     // create msg_E (not implemented)
 127     // create msg_G (not implemented)
 128     // create msg_I (not implemented)
 129     // create msg_K (not implemented)
 130     // create msg_L (not implemented)
 131
 132     // create msg_Q
 133     if ( magdeg < 0.0 ) {
 134         magdeg = -magdeg;
 135         dir = 'W';
 136     } else {
 137         dir = 'E';
 138     }
 139     sprintf( msg_Q, "Q%c%03.0f\r\n", dir, magdeg * 10.0 );
 140
 141     // create msg_S (not implemented)
 142
 143     // create msg_T
 144     sprintf( msg_T, "T---------\r\n" );
 145
 146     // create msg_l (not implemented)
 147
 148     // sentence type 2
 149     sprintf( msg_type2, "w01%c\r\n", (char)65 );
 150
 151     // assemble message
 152     string sentence;
 153     sentence += '\002';         // STX
 154     sentence += msg_z;          // altitude
 155     sentence += msg_A;          // latitude
 156     sentence += msg_B;          // longitude
 157     sentence += msg_C;          // ground track
 158     sentence += msg_D;          // ground speed (kt)
 159     // sentence += "E-----\r\n";
 160     // sentence += "G-----\r\n";
 161     // sentence += "I----\r\n";
 162     // sentence += "K-----\r\n";
 163     // sentence += "L----\r\n";
 164     sentence += msg_Q;          // magvar
 165     // sentence += "S-----\r\n";
 166     sentence += msg_T;          // end of type 1 messages (must be sent)
 167     sentence += msg_type2;      // type2 message
 168     // sentence += "l------\r\n";
 169     sentence += '\003';         // ETX
 170
 171     // cout << sentence;
 172     length = sentence.length();
 173     // cout << endl << "length = " << length << endl;
 174     strncpy( buf, sentence.c_str(), length );
 175
 176     return true;
 177 }
 178
 179
 180 // parse AV400 message
 181 bool FGAV400::parse_message() {
 182     SG_LOG( SG_IO, SG_INFO, "parse garmin message" );
 183
 184     string msg = buf;
 185     msg = msg.substr( 0, length );
 186     SG_LOG( SG_IO, SG_INFO, "entire message = " << msg );
 187
 188     string::size_type begin_line, end_line, begin, end;
 189     begin_line = begin = 0;
 190
 191     // extract out each line
 192     end_line = msg.find("\n", begin_line);
 193     while ( end_line != string::npos ) {
 194         string line = msg.substr(begin_line, end_line - begin_line);
 195         begin_line = end_line + 1;
 196         SG_LOG( SG_IO, SG_INFO, "  input line = " << line );
 197
 198         // leading character
 199         string start = msg.substr(begin, 1);
 200         ++begin;
 201         SG_LOG( SG_IO, SG_INFO, "  start = " << start );
 202
 203         // sentence
 204         end = msg.find(",", begin);
 205         if ( end == string::npos ) {
 206             return false;
 207         }
 208
 209         string sentence = msg.substr(begin, end - begin);
 210         begin = end + 1;
 211         SG_LOG( SG_IO, SG_INFO, "  sentence = " << sentence );
 212
 213         double lon_deg, lon_min, lat_deg, lat_min;
 214         double lon, lat, speed, heading, altitude;
 215
 216         if ( sentence == "GPRMC" ) {
 217             // time
 218             end = msg.find(",", begin);
 219             if ( end == string::npos ) {
 220                 return false;
 221             }
 222
 223             string utc = msg.substr(begin, end - begin);
 224             begin = end + 1;
 225             SG_LOG( SG_IO, SG_INFO, "  utc = " << utc );
 226
 227             // junk
 228             end = msg.find(",", begin);
 229             if ( end == string::npos ) {
 230                 return false;
 231             }
 232
 233             string junk = msg.substr(begin, end - begin);
 234             begin = end + 1;
 235             SG_LOG( SG_IO, SG_INFO, "  junk = " << junk );
 236
 237             // lat val
 238             end = msg.find(",", begin);
 239             if ( end == string::npos ) {
 240                 return false;
 241             }
 242
 243             string lat_str = msg.substr(begin, end - begin);
 244             begin = end + 1;
 245
 246             lat_deg = atof( lat_str.substr(0, 2).c_str() );
 247             lat_min = atof( lat_str.substr(2).c_str() );
 248
 249             // lat dir
 250             end = msg.find(",", begin);
 251             if ( end == string::npos ) {
 252                 return false;
 253             }
 254
 255             string lat_dir = msg.substr(begin, end - begin);
 256             begin = end + 1;
 257
 258             lat = lat_deg + ( lat_min / 60.0 );
 259             if ( lat_dir == "S" ) {
 260                 lat *= -1;
 261             }
 262
 263             cur_fdm_state->set_Latitude( lat * SGD_DEGREES_TO_RADIANS );
 264             SG_LOG( SG_IO, SG_INFO, "  lat = " << lat );
 265
 266             // lon val
 267             end = msg.find(",", begin);
 268             if ( end == string::npos ) {
 269                 return false;
 270             }
 271
 272             string lon_str = msg.substr(begin, end - begin);
 273             begin = end + 1;
 274
 275             lon_deg = atof( lon_str.substr(0, 3).c_str() );
 276             lon_min = atof( lon_str.substr(3).c_str() );
 277
 278             // lon dir
 279             end = msg.find(",", begin);
 280             if ( end == string::npos ) {
 281                 return false;
 282             }
 283
 284             string lon_dir = msg.substr(begin, end - begin);
 285             begin = end + 1;
 286
 287             lon = lon_deg + ( lon_min / 60.0 );
 288             if ( lon_dir == "W" ) {
 289                 lon *= -1;
 290             }
 291
 292             cur_fdm_state->set_Longitude( lon * SGD_DEGREES_TO_RADIANS );
 293             SG_LOG( SG_IO, SG_INFO, "  lon = " << lon );
 294
 295 #if 0
 296             double sl_radius, lat_geoc;
 297             sgGeodToGeoc( cur_fdm_state->get_Latitude(),
 298                           cur_fdm_state->get_Altitude(),
 299                           &sl_radius, &lat_geoc );
 300             cur_fdm_state->set_Geocentric_Position( lat_geoc,
 301                            cur_fdm_state->get_Longitude(),
 302                            sl_radius + cur_fdm_state->get_Altitude() );
 303 #endif
 304
 305             // speed
 306             end = msg.find(",", begin);
 307             if ( end == string::npos ) {
 308                 return false;
 309             }
 310
 311             string speed_str = msg.substr(begin, end - begin);
 312             begin = end + 1;
 313             speed = atof( speed_str.c_str() );
 314             cur_fdm_state->set_V_calibrated_kts( speed );
 315             // cur_fdm_state->set_V_ground_speed( speed );
 316             SG_LOG( SG_IO, SG_INFO, "  speed = " << speed );
 317
 318             // heading
 319             end = msg.find(",", begin);
 320             if ( end == string::npos ) {
 321                 return false;
 322             }
 323
 324             string hdg_str = msg.substr(begin, end - begin);
 325             begin = end + 1;
 326             heading = atof( hdg_str.c_str() );
 327             cur_fdm_state->set_Euler_Angles( cur_fdm_state->get_Phi(),
 328                                              cur_fdm_state->get_Theta(),
 329                                              heading * SGD_DEGREES_TO_RADIANS );
 330             SG_LOG( SG_IO, SG_INFO, "  heading = " << heading );
 331         } else if ( sentence == "PGRMZ" ) {
 332             // altitude
 333             end = msg.find(",", begin);
 334             if ( end == string::npos ) {
 335                 return false;
 336             }
 337
 338             string alt_str = msg.substr(begin, end - begin);
 339             altitude = atof( alt_str.c_str() );
 340             begin = end + 1;
 341
 342             // altitude units
 343             end = msg.find(",", begin);
 344             if ( end == string::npos ) {
 345                 return false;
 346             }
 347
 348             string alt_units = msg.substr(begin, end - begin);
 349             begin = end + 1;
 350
 351             if ( alt_units != "F" && alt_units != "f" ) {
 352                 altitude *= SG_METER_TO_FEET;
 353             }
 354
 355             cur_fdm_state->set_Altitude( altitude );
 356
 357             SG_LOG( SG_IO, SG_INFO, " altitude  = " << altitude );
 358
 359         }
 360
 361         // printf("%.8f %.8f\n", lon, lat);
 362
 363         begin = begin_line;
 364         end_line = msg.find("\n", begin_line);
 365     }
 366
 367     return true;
 368 }
 369
 370
 371 // open hailing frequencies
 372 bool FGAV400::open() {
 373     if ( is_enabled() ) {
 374         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
 375                 << "is already in use, ignoring" );
 376         return false;
 377     }
 378
 379     SGIOChannel *io = get_io_channel();
 380
 381     if ( ! io->open( get_direction() ) ) {
 382         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 383         return false;
 384     }
 385
 386     set_enabled( true );
 387
 388     return true;
 389 }
 390
 391
 392 // process work for this port
 393 bool FGAV400::process() {
 394     SGIOChannel *io = get_io_channel();
 395
 396     if ( get_direction() == SG_IO_OUT ) {
 397         gen_message();
 398         if ( ! io->write( buf, length ) ) {
 399             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 400             return false;
 401         }
 402     } else if ( get_direction() == SG_IO_IN ) {
 403         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 404             SG_LOG( SG_IO, SG_ALERT, "Success reading data." );
 405             if ( parse_message() ) {
 406                 SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
 407             } else {
 408                 SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
 409             }
 410         } else {
 411             SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
 412             return false;
 413         }
 414         if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
 415             SG_LOG( SG_IO, SG_ALERT, "Success reading data." );
 416             if ( parse_message() ) {
 417                 SG_LOG( SG_IO, SG_ALERT, "Success parsing data." );
 418             } else {
 419                 SG_LOG( SG_IO, SG_ALERT, "Error parsing data." );
 420             }
 421         } else {
 422             SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
 423             return false;
 424         }
 425     }
 426
 427     return true;
 428 }
 429
 430
 431 // close the channel
 432 bool FGAV400::close() {
 433     SGIOChannel *io = get_io_channel();
 434
 435     set_enabled( false );
 436
 437     if ( ! io->close() ) {
 438         return false;
 439     }
 440
 441     return true;
 442 }