Autopilot-CVS: downlink/ground/src Joystick.cpp,1.6,1.7

[Dennis D'A. <den...@us...>]

Update of /cvsroot/autopilot/downlink/ground/src
In directory sc8-pr-cvs1:/tmp/cvs-serv8953

Modified Files:
	Joystick.cpp 
Log Message:
sends WIN32 joystick cmds to simulator, added a gui widget to select WIN32 joystick ID


Index: Joystick.cpp
===================================================================
RCS file: /cvsroot/autopilot/downlink/ground/src/Joystick.cpp,v
retrieving revision 1.6
retrieving revision 1.7
diff -u -d -r1.6 -r1.7
--- Joystick.cpp	3 Feb 2003 17:57:44 -0000	1.6
+++ Joystick.cpp	5 Feb 2003 17:31:26 -0000	1.7
@@ -32,6 +32,9 @@
 #include <cmath>
 #include <unistd.h>
 #include "macros.h"
+#include "state/state.h"
+#include "state/udp.h"
+ 
 
 #include "Ground.h"
 #include "Joystick.h"
@@ -47,14 +50,79 @@
 
 using namespace std;
 using namespace util;
+using namespace libstate;
 
 int			joy_pitch;
 int			joy_roll;
 int			joy_yaw;
 int			joy_throttle;
 int			joy_button[8];
+static const double	pi		= 3.141519;
+#ifdef WIN32
+static UINT joystickID = 0;
+#endif
+static joy_axis_t limits[8] =
+{
+
+	{ "roll", -32000, 32000, 0 },
+	{ "pitch", -32000, 32000, 0 },
+	{ "coll", 32000, -32000, 0 },
+	{ "yaw", -32000, 32000, 0 },
+	
+};
+
+static trim_t trims[ 8 ] =
+{
+	{  -4.00,   4.00,  0.58 },              /* roll */
+	{   4.00,  -4.00, -0.18 },              /* pitch */
+	{  3.00,   13.00,  0.00 },              /* coll */
+	{ -13.00,  13.00,  6.00 },              /* yaw */
+	
+	/* Rest are all zero */
+};
 
+/**
+ *  Scales a joystick reading into degrees of swashplate or
+ * tail rotor travel.
+ */
+static void
+handle_axis(
+	int			axis,
+	int			value
+)
+{
+	joy_axis_t *		values;
+	trim_t *		trim;
+	double			scaled = value;
 
+	if( axis < 0 || MAX_AXES < axis )
+	{
+	
+		return;
+	}
+
+	values	= &limits[axis];
+	trim	= &trims[axis];
+
+	if( !values->name || !values->name[0] )
+	{
+		return;
+	}
+
+	/* Convert the reading into a value from -1.0 to 1.0 */
+	scaled -= values->min;
+	scaled /= (values->max - values->min);
+
+	/* Convert the unit value to degrees */
+	scaled *= trim->max_deflection - trim->min_deflection;
+	scaled += trim->min_deflection;
+	scaled += trim->trim;
+
+	/* Convert degrees to radians */
+	scaled *= pi / 180.0;
+
+	values->last = scaled;
+}
 
 #ifndef WIN32
 static void
@@ -195,7 +263,7 @@
 	
 	joyinfo.dwSize=sizeof(joyinfo);
 	joyinfo.dwFlags=JOY_RETURNALL;
-	result=joyGetPosEx(0, &joyinfo);
+	result=joyGetPosEx(joystickID, &joyinfo);
 
 	if( result != JOYERR_NOERROR )
 	{
@@ -214,38 +282,43 @@
 		<< endl; */
 
 
-		if(0) {
-			cout<< "axis: "<< "X"<< "="<< joyinfo.dwXpos<< endl;
-			cout<<"axis: Y="<<joyinfo.dwYpos<<endl;
-			cout<<"axis: Z="<<joyinfo.dwZpos<<endl;
-			cout<<"axis: R="<<joyinfo.dwRpos<<endl;
-		}
-			
+	if(0) {
+		cout<< "axis: "<< "X"<< "="<< joyinfo.dwXpos<< endl;
+		cout<<"axis: Y="<<joyinfo.dwYpos<<endl;
+		cout<<"axis: Z="<<joyinfo.dwZpos<<endl;
+		cout<<"axis: R="<<joyinfo.dwRpos<<endl;
+	}
+		
 
-		Fl_Group *g = gui->axes;
+	Fl_Group *g = gui->axes;
 
 //		if( num >= g->children() )
 //			return;
 
-		joyX=(signed int) joyinfo.dwXpos - 32000;
-		joyY=(signed int) joyinfo.dwYpos - 32000;
-		joyZ=(signed int) joyinfo.dwZpos - 32000;
-		joyR=(signed int) joyinfo.dwRpos - 32000;
+	joyX=(signed int) joyinfo.dwXpos - 32000;
+	joyY=(signed int) joyinfo.dwYpos - 32000;
+	joyZ=(signed int) joyinfo.dwZpos - 32000;
+	joyR=(signed int) joyinfo.dwRpos - 32000;
+
+	handle_axis(0,joyX);
+	handle_axis(1,joyY);
+	handle_axis(2,joyZ);
+	handle_axis(3,joyR);
+
+	Fl_Valuator *a = (Fl_Valuator*) g->child( 0 );
+	a->value( joyX );
+	a=(Fl_Valuator*) g->child( 1 );
+	a->value( joyY );
+	a=(Fl_Valuator*) g->child( 2 );
+	a->value( joyZ );
+	a=(Fl_Valuator*) g->child( 3 );
+	a->value( joyR );
 	
-		Fl_Valuator *a = (Fl_Valuator*) g->child( 0 );
-		a->value( joyX );
-		a=(Fl_Valuator*) g->child( 1 );
-		a->value( joyY );
-		a=(Fl_Valuator*) g->child( 2 );
-		a->value( joyZ );
-		a=(Fl_Valuator*) g->child( 3 );
-		a->value( joyR );
-		
 
-		joy_roll	= joyX;
-		joy_pitch	= joyY;
-		joy_yaw		= joyR;
-		joy_throttle	= joyZ;
+	joy_roll	= joyX;
+	joy_pitch	= joyY;
+	joy_yaw		= joyR;
+	joy_throttle	= joyZ;
 
 	if( joyinfo.dwFlags & JOY_RETURNBUTTONS )
 	{
@@ -265,6 +338,46 @@
 	return;
 }
 
+static void
+null_handler(
+	void *			UNUSED( user_arg )
+)
+{
+	Fl::add_timeout(
+		.01,
+		null_handler
+	);
+	
+}
+
+/*
+ *  Read the state from the user
+ */
+static void
+send_jcmds(
+	void *			UNUSED( user_arg )
+)
+{
+	int			axis;
+
+	/* Send our commands */
+	for( axis=0 ; axis<8 ; axis++ )
+	{
+		joy_axis_t *values	= &limits[axis];
+
+		if( !values->name || !values->name[0] )
+			continue;
+
+		set_parameter( values->name, values->last );
+	}
+
+	Fl::add_timeout(
+		.03,
+		send_jcmds
+	);
+	
+}
+
 
 void
 reconnect_joy(
@@ -276,11 +389,17 @@
 	
 	joyinfo.dwSize = sizeof(joyinfo);
 	joyinfo.dwFlags = JOY_RETURNALL;
+
+	int tempJoyID = (int) gui->guiJoyID->value();
 	
-	result = joyGetPosEx(uJoyID, &joyinfo);
+	result = joyGetPosEx(tempJoyID, &joyinfo);
 	if ( result != JOYERR_NOERROR ) {
 		gui->joy_status->value( 0 );
-		perror( "uJoyID=0" );
+		Fl::add_timeout(
+			.05,
+			null_handler
+		);
+			
 		return;
 	}
 
@@ -289,6 +408,12 @@
 		joy_handler
 	);
 
+	Fl::add_timeout(
+		.02,
+		send_jcmds
+	);
+
+	joystickID=tempJoyID;
 	gui->joy_status->value( 1 );
 }