[paparazzi-commits] [4557] nps: prevent joystick from overriding nps_radio_control during takeoff

[Felix Ruess]

Revision: 4557
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4557
Author:   flixr
Date:     2010-02-18 21:20:00 +0000 (Thu, 18 Feb 2010)
Log Message:
-----------
nps: prevent joystick from overriding nps_radio_control during takeoff

Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c
    paparazzi3/trunk/sw/simulator/nps/nps_radio_control.c
    paparazzi3/trunk/sw/simulator/nps/nps_radio_control_joystick.c
    paparazzi3/trunk/sw/simulator/nps/nps_radio_control_joystick.h

Modified: paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c      2010-02-18 
18:15:32 UTC (rev 4556)
+++ paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c      2010-02-18 
21:20:00 UTC (rev 4557)
@@ -63,8 +63,6 @@
   if (time < 8) {
     //    double hover = 0.25;
     double hover = 0.2493;
-    //   double hover = 0.23;
-    //  double hover = 0.;
     //  if (time > 20) hover = 0.25;
     double yaw = 0.000000;
     double pitch = 0.000;
@@ -79,16 +77,6 @@
     uint8_t i;
     for (i=0; i<ACTUATORS_MKK_NB; i++)
       autopilot.commands[i] = (double)supervision.commands[i] / 
SUPERVISION_MAX_MOTOR;
-#if 0
-    int32_t ut_front = supervision_commands[SERVO_FRONT] - TRIM_FRONT;
-    int32_t ut_back  = Actuator(SERVO_BACK)  - TRIM_BACK;
-    int32_t ut_right = Actuator(SERVO_RIGHT) - TRIM_RIGHT;
-    int32_t ut_left  = Actuator(SERVO_LEFT)  - TRIM_LEFT;
-    autopilot.commands[SERVO_FRONT] = (double)ut_front / SUPERVISION_MAX_MOTOR;
-    autopilot.commands[SERVO_BACK]  = (double)ut_back  / SUPERVISION_MAX_MOTOR;
-    autopilot.commands[SERVO_RIGHT] = (double)ut_right / SUPERVISION_MAX_MOTOR;
-    autopilot.commands[SERVO_LEFT]  = (double)ut_left  / SUPERVISION_MAX_MOTOR;
-#endif
   }
   //  printf("%f %f %f %f\n", autopilot.commands[SERVO_FRONT], 
autopilot.commands[SERVO_BACK],
   //                          autopilot.commands[SERVO_RIGHT], 
autopilot.commands[SERVO_LEFT]);

Modified: paparazzi3/trunk/sw/simulator/nps/nps_radio_control.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_radio_control.c       2010-02-18 
18:15:32 UTC (rev 4556)
+++ paparazzi3/trunk/sw/simulator/nps/nps_radio_control.c       2010-02-18 
21:20:00 UTC (rev 4557)
@@ -1,10 +1,9 @@
 #include "nps_radio_control.h"
 
-
-
 #include "nps_radio_control_spektrum.h"
 #include "nps_radio_control_joystick.h"
 
+#include <stdio.h>
 
 #define RADIO_CONTROL_DT (1./40.)
 
@@ -64,6 +63,14 @@
       radio_control_script_takeoff(time);
     else if (nps_radio_control.type == SCRIPT)
       scripts[nps_radio_control.num_script](time);
+    else if (nps_radio_control.type == JOYSTICK) {
+      nps_radio_control.throttle = nps_joystick.throttle;
+      nps_radio_control.roll = nps_joystick.roll;
+      nps_radio_control.pitch = nps_joystick.pitch;
+      nps_radio_control.yaw = nps_joystick.yaw;
+      nps_radio_control.mode = nps_joystick.mode;
+      //printf("throttle: %f, roll: %f, pitch: %f, yaw: %f\n", 
nps_joystick.throttle, nps_joystick.roll, nps_joystick.pitch, nps_joystick.yaw);
+    }
     return TRUE;
   }
   return FALSE;

Modified: paparazzi3/trunk/sw/simulator/nps/nps_radio_control_joystick.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_radio_control_joystick.c      
2010-02-18 18:15:32 UTC (rev 4556)
+++ paparazzi3/trunk/sw/simulator/nps/nps_radio_control_joystick.c      
2010-02-18 21:20:00 UTC (rev 4557)
@@ -1,4 +1,5 @@
 #include "nps_radio_control.h"
+#include "nps_radio_control_joystick.h"
 
 #include <glib.h>
 #include <stdio.h>
@@ -11,10 +12,18 @@
 #include <sys/ioctl.h>
 #include <linux/joystick.h>
 
+NpsJoystick nps_joystick;
+
 static gboolean on_js_data_received(GIOChannel *source, GIOCondition 
condition, gpointer data);
 
 int nps_radio_control_joystick_init(const char* device) {
 
+  nps_joystick.throttle = 0.5;
+  nps_joystick.roll = 0.;
+  nps_joystick.pitch = 0.;
+  nps_joystick.yaw = 0.;
+  nps_joystick.mode = 1.0;
+
   int fd = open(device, O_RDONLY | O_NONBLOCK);
   if (fd == -1) {
     printf("opening joystick device %s : %s\n", device, strerror(errno));
@@ -29,44 +38,46 @@
 
 #define JS_ROLL     0
 #define JS_PITCH    1
-#define JS_MODE     2
-#define JS_YAW      5
-#define JS_THROTTLE 6
+#define JS_YAW      2
+#define JS_THROTTLE 3
+#define JS_MODE     5
 #define JS_NB_AXIS  7
 
 
-static gboolean on_js_data_received(GIOChannel *source, 
-                                   GIOCondition condition __attribute__ 
((unused)), 
+static gboolean on_js_data_received(GIOChannel *source,
+                                   GIOCondition condition __attribute__ 
((unused)),
                                    gpointer data __attribute__ ((unused))) {
 
   struct js_event js;
   gsize len;
   GError *err = NULL;
   g_io_channel_read_chars(source, (gchar*)(&js), sizeof(struct js_event), 
&len, &err);
-  
+
   if (js.type == JS_EVENT_AXIS) {
     if (js.number < JS_NB_AXIS) {
       switch (js.number) {
       case JS_THROTTLE:
-       nps_radio_control.throttle = ((float)js.value + 28000.)/56000.; 
-       //printf("joystick throttle %d\n",js.value);
-       break;
-      case JS_ROLL: 
-       nps_radio_control.roll = (float)js.value/-28000.; 
-       //printf("joystick roll %d %f\n",js.value, nps_radio_control.roll);
-       break;
+        nps_joystick.throttle = ((float)js.value - 32767.)/-65534.;
+        //printf("joystick throttle %d\n",js.value);
+        break;
+      case JS_ROLL:
+        nps_joystick.roll = (float)js.value/-32767.;
+        //printf("joystick roll %d %f\n",js.value, nps_joystick.roll);
+        break;
       case JS_PITCH:
-       nps_radio_control.pitch = (float)js.value/-28000.; 
-       //printf("joystick pitch %d\n",js.value);
-       break;
+        nps_joystick.pitch = (float)js.value/32767.;
+        //printf("joystick pitch %d %f\n",js.value, nps_joystick.pitch);
+        break;
       case JS_YAW:
-       nps_radio_control.yaw = (float)js.value/-28000.; 
-       //printf("joystick yaw %d\n",js.value);
-       break;
+        //nps_joystick.yaw = 0.;
+        nps_joystick.yaw = (float)js.value/-32767.;
+        //printf("joystick yaw %d %f\n",js.value, nps_joystick.yaw);
+        break;
       case JS_MODE:
-       nps_radio_control.mode = (float)js.value/-32000.; 
-       //printf("joystick mode %d\n",js.value);
-       break;
+        nps_joystick.mode = 1.0;
+        //nps_joystick.mode = (float)js.value/-32767.;
+        //printf("joystick mode %d\n",js.value);
+        break;
       }
     }
   }

Modified: paparazzi3/trunk/sw/simulator/nps/nps_radio_control_joystick.h
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_radio_control_joystick.h      
2010-02-18 18:15:32 UTC (rev 4556)
+++ paparazzi3/trunk/sw/simulator/nps/nps_radio_control_joystick.h      
2010-02-18 21:20:00 UTC (rev 4557)
@@ -3,5 +3,15 @@
 
 extern int nps_radio_control_joystick_init(const char* device);
 
+struct NpsJoystick {
+  double throttle;
+  double roll;
+  double pitch;
+  double yaw;
+  double mode;
+};
 
+extern struct NpsJoystick nps_joystick;
+
+
 #endif /* NPS_RADIO_CONTROL_SPEKTRUM_H */