src/Network/pve.cxx

   1 // pve.cxx -- "PVE" protocal class (for Provision Entertainment)
   2 //
   3 // Written by Curtis Olson, started November 1999.
   4 //
   5 // Copyright (C) 1999  Curtis L. Olson - curt@flightgear.org
   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., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #include <Debug/logstream.hxx>
  25 #include <FDM/flight.hxx>
  26 #include <Math/fg_geodesy.hxx>
  27 #include <Time/fg_time.hxx>
  28
  29 #include "iochannel.hxx"
  30 #include "pve.hxx"
  31
  32
  33 FGPVE::FGPVE() {
  34 }
  35
  36
  37 FGPVE::~FGPVE() {
  38 }
  39
  40
  41 // generate Garmin message
  42 bool FGPVE::gen_message() {
  43     // cout << "generating pve message" << endl;
  44     FGInterface *f = cur_fdm_state;
  45
  46     // get roll and pitch, convert to degrees
  47     double roll_deg = f->get_Phi() * RAD_TO_DEG;
  48     while ( roll_deg <= -180.0 ) {
  49         roll_deg += 360.0;
  50     }
  51     while ( roll_deg > 180.0 ) {
  52         roll_deg -= 360.0;
  53     }
  54
  55     double pitch_deg = f->get_Theta() * RAD_TO_DEG;
  56     while ( pitch_deg <= -180.0 ) {
  57         pitch_deg += 360.0;
  58     }
  59     while ( pitch_deg > 180.0 ) {
  60         pitch_deg -= 360.0;
  61     }
  62
  63     short int heave = (int)(f->get_W_body() * 128.0);
  64
  65     // scale roll and pitch to output format (1 - 255)
  66     // straight && level == (128, 128)
  67
  68     short int roll = (int)(roll_deg * 32768 / 180.0);
  69     short int pitch = (int)(pitch_deg * 32768 / 180.0);
  70
  71     unsigned char roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2;
  72     roll_b1 = roll >> 8;
  73     roll_b2 = roll & 0x00ff;
  74     pitch_b1 = pitch >> 8;
  75     pitch_b2 = pitch & 0x00ff;
  76     heave_b1 = heave >> 8;
  77     heave_b2 = heave & 0x00ff;
  78
  79     sprintf( buf, "p%c%c%c%c%c%c\n",
  80              roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2 );
  81     length = 8;
  82
  83     // printf( "p [ %u %u ]  [ %u %u ]  [ %u %u ]\n",
  84     //         roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2 );
  85
  86     FG_LOG( FG_IO, FG_INFO, "roll=" << roll << " pitch=" << pitch <<
  87             " heave=" << heave );
  88
  89     return true;
  90 }
  91
  92
  93 // parse RUL message
  94 bool FGPVE::parse_message() {
  95     FG_LOG( FG_IO, FG_ALERT, "PVE input not supported" );
  96
  97     return false;
  98 }
  99
 100
 101 // process work for this port
 102 bool FGPVE::process() {
 103     FGIOChannel *io = get_io_channel();
 104
 105     if ( get_direction() == out ) {
 106         gen_message();
 107         if ( ! io->write( buf, length ) ) {
 108             FG_LOG( FG_IO, FG_ALERT, "Error writing data." );
 109             return false;
 110         }
 111     } else if ( get_direction() == in ) {
 112         FG_LOG( FG_IO, FG_ALERT, "in direction not supported for RUL." );
 113         return false;
 114     }
 115
 116     return true;
 117 }