From 1e5c61fcbd6f93c3d323efd0780cb32cb1f26f32 Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Tue, 30 May 2000 17:01:09 +0000
Subject: [PATCH] Alex Perry has added support for Ray Woodworth's motion chair
 which has 3-5 axes installed.

---
 src/Main/fg_io.cxx      |   4 +
 src/Main/options.cxx    |   2 +
 src/Network/Makefile.am |   1 +
 src/Network/ray.cxx     | 216 ++++++++++++++++++++++++++++++++++++++++
 src/Network/ray.hxx     |  60 +++++++++++
 5 files changed, 283 insertions(+)
 create mode 100644 src/Network/ray.cxx
 create mode 100644 src/Network/ray.hxx

diff --git a/src/Main/fg_io.cxx b/src/Main/fg_io.cxx
index 42fa059c3..a54f4c0ca 100644
--- a/src/Main/fg_io.cxx
+++ b/src/Main/fg_io.cxx
@@ -40,6 +40,7 @@
 #include <Network/garmin.hxx>
 #include <Network/nmea.hxx>
 #include <Network/pve.hxx>
+#include <Network/ray.hxx>
 #include <Network/rul.hxx>
 #include <Network/joyclient.hxx>
 
@@ -84,6 +85,9 @@ static FGProtocol *parse_port_config( const string& config )
     } else if ( protocol == "pve" ) {
 	FGPVE *pve = new FGPVE;
 	io = pve;
+    } else if ( protocol == "ray" ) {
+	FGRAY *ray = new FGRAY;
+	io = ray;
     } else if ( protocol == "rul" ) {
 	FGRUL *rul = new FGRUL;
 	io = rul;
diff --git a/src/Main/options.cxx b/src/Main/options.cxx
index 2eac4f9a4..9222bf2f5 100644
--- a/src/Main/options.cxx
+++ b/src/Main/options.cxx
@@ -805,6 +805,8 @@ int fgOPTIONS::parse_option( const string& arg ) {
 	parse_channel( "nmea", arg.substr(7) );
     } else if ( arg.find( "--pve=" ) != string::npos ) {
 	parse_channel( "pve", arg.substr(6) );
+    } else if ( arg.find( "--ray=" ) != string::npos ) {
+	parse_channel( "ray", arg.substr(6) );
     } else if ( arg.find( "--rul=" ) != string::npos ) {
 	parse_channel( "rul", arg.substr(6) );
     } else if ( arg.find( "--joyclient=" ) != string::npos ) {
diff --git a/src/Network/Makefile.am b/src/Network/Makefile.am
index 63a1dc0b4..3f200aea4 100644
--- a/src/Network/Makefile.am
+++ b/src/Network/Makefile.am
@@ -10,6 +10,7 @@ libNetwork_a_SOURCES = \
 	garmin.cxx garmin.hxx \
 	nmea.cxx nmea.hxx \
 	pve.cxx pve.hxx \
+	ray.cxx ray.hxx \
 	rul.cxx rul.hxx \
 	joyclient.cxx joyclient.hxx
 
diff --git a/src/Network/ray.cxx b/src/Network/ray.cxx
new file mode 100644
index 000000000..749a9629b
--- /dev/null
+++ b/src/Network/ray.cxx
@@ -0,0 +1,216 @@
+// 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;
+}
diff --git a/src/Network/ray.hxx b/src/Network/ray.hxx
new file mode 100644
index 000000000..1739ef022
--- /dev/null
+++ b/src/Network/ray.hxx
@@ -0,0 +1,60 @@
+// ray.hxx -- "RayWoodworth" chair protocol via CIO-DDA06/Jr driver
+//
+// Written by Alexander Perry, 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$
+
+
+#ifndef _FG_RAY_HXX
+#define _FG_RAY_HXX
+
+
+#include <simgear/compiler.h>
+
+#include STL_STRING
+
+#include "protocol.hxx"
+
+FG_USING_STD(string);
+
+
+class FGRAY : public FGProtocol {
+
+    char buf[ 20 ];
+    int length;
+
+    double chair_heading;
+    double chair_rising;
+    double chair_height;
+    double chair_vertical[2];
+
+public:
+
+    FGRAY();
+    ~FGRAY();
+
+    bool gen_message();
+    bool parse_message();
+ 
+    // process work for this port
+    bool process();
+};
+
+
+#endif // _FG_RAY_HXX