flightgear/src/Network/ray.cxx

// 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/debug/logstream.hxx>
#include <simgear/math/fg_geodesy.hxx>

#include <FDM/flight.hxx>
#include <Time/fg_time.hxx>

#include "iochannel.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;
}


FGRAY::~FGRAY() {
}


// Ray Woodworth's motion chair has between 3 and 5 axes installed.
// 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, 15deg, 12in, 12in, 12in
//
// 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.8, -0.8, -0.25, /* 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 */
	while ( ang_pos < -M_PI ) {
		ang_pos += 2 * M_PI;
	}
	while ( ang_pos > M_PI ) {
		ang_pos -= 2 * M_PI;
	}

	/* Tell interested parties what the situation is */
	printf ( "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 ) > M_PI )
		{	/* Need to swing chair by 360 degrees */
			if ( ang_pos < chair_heading )
				chair_heading -= 2 * M_PI;
			else	chair_heading += 2 * M_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 represent 15 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) > M_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 */
	printf ( "  %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 < -M_PI / 2 ) *dac = 0x0000; else
		if ( propose >  M_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;

    /* Log bytes for debug */
//    for ( axis = 0; axis < length; axis++ )
//        printf ( "%02x ", (unsigned int) (unsigned char) buf[axis] );
//    printf ( "\n" );

    return true;
}


// parse RUL message
bool FGRAY::parse_message() {
    FG_LOG( FG_IO, FG_ALERT, "RAY input not supported" );

    return false;
}


// process work for this port
bool FGRAY::process() {
    FGIOChannel *io = get_io_channel();

    if ( get_direction() == out ) {
	gen_message();
	if ( ! io->write( buf, length ) ) {
	    FG_LOG( FG_IO, FG_ALERT, "Error writing data." );
	    return false;
	}
    } else if ( get_direction() == in ) {
	FG_LOG( FG_IO, FG_ALERT, "in direction not supported for RAY." );
	return false;
    }

    return true;
}