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