221 lines
6.5 KiB
C++
221 lines
6.5 KiB
C++
// ray.cxx -- "RayWoodworth" motion chair support
|
|
//
|
|
// Written by Alexander Perry, started May 2000
|
|
//
|
|
// Copyright (C) 2000, Alexander Perry, alex.perry@ieee.org
|
|
//
|
|
// This program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License as
|
|
// published by the Free Software Foundation; either version 2 of the
|
|
// License, or (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful, but
|
|
// WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
// General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program; if not, write to the Free Software
|
|
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
//
|
|
// $Id$
|
|
|
|
|
|
#include <simgear/constants.h>
|
|
#include <simgear/debug/logstream.hxx>
|
|
#include <simgear/io/iochannel.hxx>
|
|
|
|
#include <FDM/flight.hxx>
|
|
|
|
#include "ray.hxx"
|
|
|
|
|
|
FGRAY::FGRAY() {
|
|
chair_rising = 0.0;
|
|
chair_height = 0.0;
|
|
chair_heading = 0.0;
|
|
chair_vertical[0] = 0.0;
|
|
chair_vertical[1] = 0.0;
|
|
// chair_FILE = stderr;
|
|
chair_FILE = 0;
|
|
}
|
|
|
|
|
|
FGRAY::~FGRAY() {
|
|
}
|
|
|
|
|
|
// Ray Woodworth (949) 262-9118 has a three axis motion chair.
|
|
//
|
|
// It expects +/- 5V signals for full scale. In channel order, axes are:
|
|
// roll, pitch, yaw, sway, surge, heave
|
|
// The drivers are capable of generating (in the same order)
|
|
// +/- 30deg, 30deg, 30deg, 12in, 12in, 12in
|
|
// The signs of the motion are such that positive volts gives
|
|
// head right, head back, feet right, body right, body back, body up
|
|
//
|
|
// In this code implementation, the voltage outputs are generated
|
|
// using a ComputerBoards DDA06/Jr card and the associated Linux driver.
|
|
// Data is written to the device /dev/dda06jr-A as byte triplets;
|
|
// The first byte is the channel number (0-5 respectively) and
|
|
// the remaining two bytes are an unsigned short for the signal.
|
|
|
|
|
|
bool FGRAY::gen_message() {
|
|
// cout << "generating RayWoodworth message" << endl;
|
|
FGInterface *f = cur_fdm_state;
|
|
int axis, subaxis;
|
|
const double fullscale[6] = { -0.5, -0.5, -0.5, /* radians */
|
|
-0.3, -0.3, -0.15 /* meters */ };
|
|
|
|
/* Figure out how big our timesteps are */
|
|
double dt = 0.05; /* seconds */
|
|
|
|
/* get basic information about gravity */
|
|
double grav_acc = -9.81;
|
|
double vert_acc = f->get_A_Z_pilot() * 0.3;
|
|
if ( -3.0 < vert_acc )
|
|
vert_acc = -3.0;
|
|
|
|
for ( axis = 0; axis < 3; axis++ )
|
|
{ /* Compute each angular axis together with the linear
|
|
axis which is coupled by smooth coordinated flight
|
|
*/
|
|
double ang_pos;
|
|
double lin_pos, lin_acc;
|
|
|
|
/* Retrieve the desired components */
|
|
switch ( axis ) {
|
|
case 0: ang_pos = f->get_Phi();
|
|
lin_acc = f->get_A_Y_pilot() * 0.3;
|
|
break;
|
|
case 1: ang_pos = f->get_Theta();
|
|
lin_acc = f->get_A_X_pilot() * 0.3;
|
|
break;
|
|
case 2: ang_pos = f->get_Psi();
|
|
lin_acc = grav_acc - vert_acc;
|
|
break;
|
|
default:
|
|
ang_pos = 0.0;
|
|
lin_acc = 0.0;
|
|
break;
|
|
}
|
|
|
|
/* Make sure the angles are reasonable onscale */
|
|
/* We use an asymmetric mapping so that the chair behaves
|
|
reasonably when upside down. Otherwise it oscillates. */
|
|
while ( ang_pos < -2*SGD_PI/3 ) {
|
|
ang_pos += 2 * SGD_PI;
|
|
}
|
|
while ( ang_pos > 4*SGD_PI/3 ) {
|
|
ang_pos -= 2 * SGD_PI;
|
|
}
|
|
|
|
/* Tell interested parties what the situation is */
|
|
if (chair_FILE) {
|
|
fprintf ( chair_FILE, "RAY %s, %8.3f rad %8.3f m/s/s =>",
|
|
((axis==0)?"Roll ":((axis==1)?"Pitch":"Yaw ")),
|
|
ang_pos, lin_acc );
|
|
}
|
|
|
|
/* The upward direction and axis are special cases */
|
|
if ( axis == 2 )
|
|
{
|
|
/* heave */
|
|
/* Integrate vertical acceleration into velocity,
|
|
diluted by 50% and with a 0.2 second high pass */
|
|
chair_rising += ( lin_acc - chair_rising ) * dt * 0.5;
|
|
/* Integrate velocity into position, 0.2 sec high pass */
|
|
chair_height += ( chair_rising - chair_height ) * dt * 0.5;
|
|
lin_pos = chair_height;
|
|
|
|
/* yaw */
|
|
/* Make sure that we walk through North cleanly */
|
|
if ( fabs ( ang_pos - chair_heading ) > SGD_PI )
|
|
{ /* Need to swing chair by 360 degrees */
|
|
if ( ang_pos < chair_heading )
|
|
chair_heading -= 2 * SGD_PI;
|
|
else chair_heading += 2 * SGD_PI;
|
|
}
|
|
/* Remove the chair heading from the true heading */
|
|
ang_pos -= chair_heading;
|
|
/* Wash out the error at 5 sec timeconstant because
|
|
a standard rate turn is 3 deg/sec and the chair
|
|
can just about represent 30 degrees full scale. */
|
|
chair_heading += ang_pos * dt * 0.2;
|
|
/* If they turn fast, at 90 deg error subtract 30 deg */
|
|
if ( fabs(ang_pos) > SGD_PI / 2 )
|
|
chair_heading += ang_pos / 3;
|
|
|
|
} else
|
|
{ /* 3 second low pass to find attitude and gravity vector */
|
|
chair_vertical[axis] += ( dt / 3 ) *
|
|
( lin_acc / vert_acc + ang_pos
|
|
- chair_vertical[axis] );
|
|
/* find out how much linear acceleration is left */
|
|
lin_acc -= chair_vertical[axis] * vert_acc;
|
|
/* reposition the pilot tilt relative to the chair */
|
|
ang_pos -= chair_vertical[axis];
|
|
/* integrate linear acceleration into a position */
|
|
lin_pos = lin_acc; /* HACK */
|
|
}
|
|
|
|
/* Tell interested parties what we'll do */
|
|
if ( chair_FILE ) {
|
|
fprintf ( chair_FILE, " %8.3f deg %8.3f cm.\n",
|
|
ang_pos * 60.0, lin_pos * 100.0 );
|
|
}
|
|
|
|
/* Write the resulting numbers to the command buffer */
|
|
/* The first pass number is linear, second pass is angle */
|
|
for ( subaxis = axis; subaxis < 6; subaxis += 3 )
|
|
{ unsigned short *dac;
|
|
/* Select the DAC in the command buffer */
|
|
buf [ 3*subaxis ] = subaxis;
|
|
dac = (unsigned short *) ( buf + 1 + 3*subaxis );
|
|
/* Select the relevant number to put there */
|
|
double propose = ( subaxis < 3 ) ? ang_pos : lin_pos;
|
|
/* Scale to the hardware's full scale range */
|
|
propose /= fullscale [ subaxis ];
|
|
/* Use a sine shaped washout on all axes */
|
|
if ( propose < -SGD_PI / 2 ) *dac = 0x0000; else
|
|
if ( propose > SGD_PI / 2 ) *dac = 0xFFFF; else
|
|
*dac = (unsigned short) ( 32767 *
|
|
( 1.0 + sin ( propose ) ) );
|
|
}
|
|
|
|
/* That concludes the per-axis calculations */
|
|
}
|
|
|
|
/* Tell the caller what we did */
|
|
length = 18;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// parse RUL message
|
|
bool FGRAY::parse_message() {
|
|
SG_LOG( SG_IO, SG_ALERT, "RAY input not supported" );
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// process work for this port
|
|
bool FGRAY::process() {
|
|
SGIOChannel *io = get_io_channel();
|
|
|
|
if ( get_direction() == SG_IO_OUT ) {
|
|
gen_message();
|
|
if ( ! io->write( buf, length ) ) {
|
|
SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
|
|
return false;
|
|
}
|
|
} else if ( get_direction() == SG_IO_IN ) {
|
|
SG_LOG( SG_IO, SG_ALERT, "in direction not supported for RAY." );
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|