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