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 - 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 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include <stdio.h>              // sprintf()
  28
  29 #include <simgear/constants.h>
  30 #include <simgear/debug/logstream.hxx>
  31 #include <simgear/io/iochannel.hxx>
  32
  33 #include <FDM/flight.hxx>
  34
  35 #include "pve.hxx"
  36
  37
  38 FGPVE::FGPVE() {
  39 }
  40
  41
  42 FGPVE::~FGPVE() {
  43 }
  44
  45
  46 // "PVE" (ProVision Entertainment) output format (for some sort of
  47 // motion platform)
  48 //
  49 // Outputs a 5-byte data packet defined as follows:
  50 //
  51 // First bite:  ASCII character "P" ( 0x50 or 80 decimal )
  52 // Second byte:  "roll" value (1-255) 1 being 0* and 255 being 359*
  53 // Third byte:  "pitch" value (1-255) 1 being 0* and 255 being 359*
  54 // Fourth byte:  "heave" value (or vertical acceleration?)
  55
  56 bool FGPVE::gen_message() {
  57     // cout << "generating pve message" << endl;
  58     FGInterface *f = cur_fdm_state;
  59
  60     // get roll and pitch, convert to degrees
  61     double roll_deg = f->get_Phi() * SGD_RADIANS_TO_DEGREES;
  62     while ( roll_deg <= -180.0 ) {
  63         roll_deg += 360.0;
  64     }
  65     while ( roll_deg > 180.0 ) {
  66         roll_deg -= 360.0;
  67     }
  68
  69     double pitch_deg = f->get_Theta() * SGD_RADIANS_TO_DEGREES;
  70     while ( pitch_deg <= -180.0 ) {
  71         pitch_deg += 360.0;
  72     }
  73     while ( pitch_deg > 180.0 ) {
  74         pitch_deg -= 360.0;
  75     }
  76
  77     short int heave = (int)(f->get_W_body() * 128.0);
  78
  79     // scale roll and pitch to output format (1 - 255)
  80     // straight && level == (128, 128)
  81
  82     short int roll = (int)(roll_deg * 32768 / 180.0);
  83     short int pitch = (int)(pitch_deg * 32768 / 180.0);
  84
  85     unsigned char roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2;
  86     roll_b1 = roll >> 8;
  87     roll_b2 = roll & 0x00ff;
  88     pitch_b1 = pitch >> 8;
  89     pitch_b2 = pitch & 0x00ff;
  90     heave_b1 = heave >> 8;
  91     heave_b2 = heave & 0x00ff;
  92
  93     sprintf( buf, "p%c%c%c%c%c%c\n",
  94              roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2 );
  95     length = 8;
  96
  97     // printf( "p [ %u %u ]  [ %u %u ]  [ %u %u ]\n",
  98     //         roll_b1, roll_b2, pitch_b1, pitch_b2, heave_b1, heave_b2 );
  99
 100     SG_LOG( SG_IO, SG_INFO, "roll=" << roll << " pitch=" << pitch <<
 101             " heave=" << heave );
 102
 103     return true;
 104 }
 105
 106
 107 // parse RUL message
 108 bool FGPVE::parse_message() {
 109     SG_LOG( SG_IO, SG_ALERT, "PVE input not supported" );
 110
 111     return false;
 112 }
 113
 114
 115 // process work for this port
 116 bool FGPVE::process() {
 117     SGIOChannel *io = get_io_channel();
 118
 119     if ( get_direction() == SG_IO_OUT ) {
 120         gen_message();
 121         if ( ! io->write( buf, length ) ) {
 122             SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
 123             return false;
 124         }
 125     } else if ( get_direction() == SG_IO_IN ) {
 126         SG_LOG( SG_IO, SG_ALERT, "in direction not supported for RUL." );
 127         return false;
 128     }
 129
 130     return true;
 131 }