//
// 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.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// $Id$
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
-#include <simgear/math/fg_geodesy.hxx>
+#include <simgear/io/iochannel.hxx>
-#include <FDM/flight.hxx>
-#include <Time/fg_time.hxx>
-
-#include "iochannel.hxx"
+#include <FDM/flightProperties.hxx>
+#include <Environment/gravity.hxx>
#include "ray.hxx"
chair_heading = 0.0;
chair_vertical[0] = 0.0;
chair_vertical[1] = 0.0;
+// chair_FILE = stderr;
+ chair_FILE = 0;
}
}
-// Ray Woodworth's motion chair has between 3 and 5 axes installed.
+// 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, 15deg, 12in, 12in, 12in
+// +/- 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.
bool FGRAY::gen_message() {
// cout << "generating RayWoodworth message" << endl;
- FGInterface *f = cur_fdm_state;
+ FlightProperties f;
+
int axis, subaxis;
- const double fullscale[6] = { -0.8, -0.8, -0.25, /* radians */
+ 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;
+ double grav_acc = -Environment::Gravity::instance()->getGravity( f.getPosition() );
+ double vert_acc = f.get_A_Z_pilot() * 0.3;
if ( -3.0 < vert_acc )
vert_acc = -3.0;
/* Retrieve the desired components */
switch ( axis ) {
- case 0: ang_pos = f->get_Phi();
- lin_acc = f->get_A_Y_pilot() * 0.3;
+ 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;
+ case 1: ang_pos = f.get_Theta();
+ lin_acc = f.get_A_X_pilot() * 0.3;
break;
- case 2: ang_pos = f->get_Psi();
+ case 2: ang_pos = f.get_Psi();
lin_acc = grav_acc - vert_acc;
break;
default:
}
/* Make sure the angles are reasonable onscale */
- while ( ang_pos < -M_PI ) {
- ang_pos += 2 * M_PI;
+ /* We use an asymmetric mapping so that the chair behaves
+ reasonably when upside down. Otherwise it oscillates. */
+ while ( ang_pos < -SGD_2PI/3 ) {
+ ang_pos += SGD_2PI;
}
- while ( ang_pos > M_PI ) {
- ang_pos -= 2 * M_PI;
+ while ( ang_pos > 2*SGD_2PI/3 ) {
+ ang_pos -= SGD_2PI;
}
/* 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 );
+ 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 )
/* yaw */
/* Make sure that we walk through North cleanly */
- if ( fabs ( ang_pos - chair_heading ) > M_PI )
+ if ( fabs ( ang_pos - chair_heading ) > SGD_PI )
{ /* Need to swing chair by 360 degrees */
if ( ang_pos < chair_heading )
- chair_heading -= 2 * M_PI;
- else chair_heading += 2 * M_PI;
+ chair_heading -= SGD_2PI;
+ else chair_heading += SGD_2PI;
}
/* 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. */
+ 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) > M_PI / 2 )
+ if ( fabs(ang_pos) > SGD_PI_2 )
chair_heading += ang_pos / 3;
} else
}
/* Tell interested parties what we'll do */
- printf ( " %8.3f deg %8.3f cm.\n",
- ang_pos * 60.0, lin_pos * 100.0 );
+ 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 */
/* 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
+ if ( propose < -SGD_PI_2 ) *dac = 0x0000; else
+ if ( propose > SGD_PI_2 ) *dac = 0xFFFF; else
*dac = (unsigned short) ( 32767 *
( 1.0 + sin ( propose ) ) );
}
/* 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" );
+ SG_LOG( SG_IO, SG_ALERT, "RAY input not supported" );
return false;
}
// process work for this port
bool FGRAY::process() {
- FGIOChannel *io = get_io_channel();
+ SGIOChannel *io = get_io_channel();
- if ( get_direction() == out ) {
+ if ( get_direction() == SG_IO_OUT ) {
gen_message();
if ( ! io->write( buf, length ) ) {
- FG_LOG( FG_IO, FG_ALERT, "Error writing data." );
+ SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
return false;
}
- } else if ( get_direction() == in ) {
- FG_LOG( FG_IO, FG_ALERT, "in direction not supported for RAY." );
+ } else if ( get_direction() == SG_IO_IN ) {
+ SG_LOG( SG_IO, SG_ALERT, "in direction not supported for RAY." );
return false;
}
projects / flightgear.git / blobdiff