src/Network/ray.cxx

   1 // ray.cxx -- "RayWoodworth" motion chair support
   2 //
   3 // Written by Alexander Perry, started May 2000
   4 //
   5 // Copyright (C) 2000, Alexander Perry, alex.perry@ieee.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., 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 <simgear/constants.h>
  28 #include <simgear/debug/logstream.hxx>
  29 #include <simgear/io/iochannel.hxx>
  30
  31 #include <FDM/flight.hxx>
  32
  33 #include "ray.hxx"
  34
  35
  36 FGRAY::FGRAY() {
  37         chair_rising = 0.0;
  38         chair_height = 0.0;
  39         chair_heading = 0.0;
  40         chair_vertical[0] = 0.0;
  41         chair_vertical[1] = 0.0;
  42 //      chair_FILE = stderr;
  43         chair_FILE = 0;
  44 }
  45
  46
  47 FGRAY::~FGRAY() {
  48 }
  49
  50
  51 // Ray Woodworth (949) 262-9118 has a three axis motion chair.
  52 //
  53 // It expects +/- 5V signals for full scale.  In channel order, axes are:
  54 //      roll, pitch, yaw, sway, surge, heave
  55 // The drivers are capable of generating (in the same order)
  56 //      +/- 30deg, 30deg, 30deg, 12in, 12in, 12in
  57 // The signs of the motion are such that positive volts gives
  58 //      head right, head back, feet right, body right, body back, body up
  59 //
  60 // In this code implementation, the voltage outputs are generated
  61 // using a ComputerBoards DDA06/Jr card and the associated Linux driver.
  62 // Data is written to the device /dev/dda06jr-A as byte triplets;
  63 // The first byte is the channel number (0-5 respectively) and
  64 // the remaining two bytes are an unsigned short for the signal.
  65
  66
  67 bool FGRAY::gen_message() {
  68     // cout << "generating RayWoodworth message" << endl;
  69     FGInterface *f = cur_fdm_state;
  70     int axis, subaxis;
  71     const double fullscale[6] = { -0.5, -0.5, -0.5, /* radians */
  72                                   -0.3, -0.3, -0.15  /* meters */ };
  73
  74     /* Figure out how big our timesteps are */
  75     double dt = 0.05; /* seconds */
  76
  77     /* get basic information about gravity */
  78     double grav_acc = -9.81;
  79     double vert_acc = f->get_A_Z_pilot() * 0.3;
  80     if ( -3.0 < vert_acc )
  81         vert_acc = -3.0;
  82
  83     for ( axis = 0; axis < 3; axis++ )
  84     {   /* Compute each angular axis together with the linear
  85            axis which is coupled by smooth coordinated flight
  86         */
  87         double ang_pos;
  88         double lin_pos, lin_acc;
  89
  90         /* Retrieve the desired components */
  91         switch ( axis ) {
  92         case 0: ang_pos = f->get_Phi();
  93                 lin_acc = f->get_A_Y_pilot() * 0.3;
  94                 break;
  95         case 1: ang_pos = f->get_Theta();
  96                 lin_acc = f->get_A_X_pilot() * 0.3;
  97                 break;
  98         case 2: ang_pos = f->get_Psi();
  99                 lin_acc = grav_acc - vert_acc;
 100                 break;
 101         default:
 102                 ang_pos = 0.0;
 103                 lin_acc = 0.0;
 104                 break;
 105         }
 106
 107         /* Make sure the angles are reasonable onscale */
 108         /* We use an asymmetric mapping so that the chair behaves
 109            reasonably when upside down.  Otherwise it oscillates. */
 110         while ( ang_pos < -SGD_2PI/3 ) {
 111                 ang_pos += SGD_2PI;
 112         }
 113         while ( ang_pos >  2*SGD_2PI/3 ) {
 114                 ang_pos -= SGD_2PI;
 115         }
 116
 117         /* Tell interested parties what the situation is */
 118         if (chair_FILE) {
 119             fprintf ( chair_FILE, "RAY %s, %8.3f rad %8.3f m/s/s  =>",
 120                       ((axis==0)?"Roll ":((axis==1)?"Pitch":"Yaw  ")),
 121                       ang_pos, lin_acc );
 122         }
 123
 124         /* The upward direction and axis are special cases */
 125         if ( axis == 2 )
 126         {
 127         /* heave */
 128                 /* Integrate vertical acceleration into velocity,
 129                    diluted by 50% and with a 0.2 second high pass */
 130                 chair_rising += ( lin_acc - chair_rising ) * dt * 0.5;
 131                 /* Integrate velocity into position, 0.2 sec high pass */
 132                 chair_height += ( chair_rising - chair_height ) * dt * 0.5;
 133                 lin_pos = chair_height;
 134
 135         /* yaw */
 136                 /* Make sure that we walk through North cleanly */
 137                 if ( fabs ( ang_pos - chair_heading ) > SGD_PI )
 138                 {       /* Need to swing chair by 360 degrees */
 139                         if ( ang_pos < chair_heading )
 140                                 chair_heading -= SGD_2PI;
 141                         else    chair_heading +=  SGD_2PI;
 142                 }
 143                 /* Remove the chair heading from the true heading */
 144                 ang_pos -= chair_heading;
 145                 /* Wash out the error at 5 sec timeconstant because
 146                    a standard rate turn is 3 deg/sec and the chair
 147                    can just about represent 30 degrees full scale.  */
 148                 chair_heading += ang_pos * dt * 0.2;
 149                 /* If they turn fast, at 90 deg error subtract 30 deg */
 150                 if ( fabs(ang_pos) > SGD_PI_2 )
 151                         chair_heading += ang_pos / 3;
 152
 153         } else
 154         {       /* 3 second low pass to find attitude and gravity vector */
 155                 chair_vertical[axis] += ( dt / 3 ) *
 156                         ( lin_acc / vert_acc + ang_pos
 157                                 - chair_vertical[axis] );
 158                 /* find out how much linear acceleration is left */
 159                 lin_acc -= chair_vertical[axis] * vert_acc;
 160                 /* reposition the pilot tilt relative to the chair */
 161                 ang_pos -= chair_vertical[axis];
 162                 /* integrate linear acceleration into a position */
 163                 lin_pos = lin_acc; /* HACK */
 164         }
 165
 166         /* Tell interested parties what we'll do */
 167         if ( chair_FILE ) {
 168             fprintf ( chair_FILE, "  %8.3f deg %8.3f cm.\n",
 169                       ang_pos * 60.0, lin_pos * 100.0 );
 170         }
 171
 172         /* Write the resulting numbers to the command buffer */
 173         /* The first pass number is linear, second pass is angle */
 174         for ( subaxis = axis; subaxis < 6; subaxis += 3 )
 175         {       unsigned short *dac;
 176                 /* Select the DAC in the command buffer */
 177                 buf [ 3*subaxis ] = subaxis;
 178                 dac = (unsigned short *) ( buf + 1 + 3*subaxis );
 179                 /* Select the relevant number to put there */
 180                 double propose = ( subaxis < 3 ) ? ang_pos : lin_pos;
 181                 /* Scale to the hardware's full scale range */
 182                 propose /= fullscale [ subaxis ];
 183                 /* Use a sine shaped washout on all axes */
 184                 if ( propose < -SGD_PI_2 ) *dac = 0x0000; else
 185                 if ( propose >  SGD_PI_2 ) *dac = 0xFFFF; else
 186                    *dac = (unsigned short) ( 32767 *
 187                                 ( 1.0 + sin ( propose ) ) );
 188         }
 189
 190         /* That concludes the per-axis calculations */
 191     }
 192
 193     /* Tell the caller what we did */
 194     length = 18;
 195
 196     return true;
 197 }
 198
 199
 200 // parse RUL message
 201 bool FGRAY::parse_message() {
 202     SG_LOG( SG_IO, SG_ALERT, "RAY input not supported" );
 203
 204     return false;
 205 }
 206
 207
 208 // process work for this port
 209 bool FGRAY::process() {
 210     SGIOChannel *io = get_io_channel();
 211
 212     if ( get_direction() == SG_IO_OUT ) {
 213         gen_message();
 214         if ( ! io->write( buf, length ) ) {
 215             SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
 216             return false;
 217         }
 218     } else if ( get_direction() == SG_IO_IN ) {
 219         SG_LOG( SG_IO, SG_ALERT, "in direction not supported for RAY." );
 220         return false;
 221     }
 222
 223     return true;
 224 }