[paparazzi-commits] [4269] adding position mode for joystick attack3 with booz

[Gautier Hattenberger]

Revision: 4269
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4269
Author:   gautier
Date:     2009-10-19 09:28:40 +0000 (Mon, 19 Oct 2009)
Log Message:
-----------
adding position mode for joystick attack3 with booz

Modified Paths:
--------------
    paparazzi3/trunk/sw/ground_segment/joystick/attack3_stick.c

Modified: paparazzi3/trunk/sw/ground_segment/joystick/attack3_stick.c
===================================================================
--- paparazzi3/trunk/sw/ground_segment/joystick/attack3_stick.c 2009-10-19 
09:26:51 UTC (rev 4268)
+++ paparazzi3/trunk/sw/ground_segment/joystick/attack3_stick.c 2009-10-19 
09:28:40 UTC (rev 4269)
@@ -35,6 +35,7 @@
 #define UPDATE_FREQ     (1./TIMEOUT_PERIOD)
 
 #define DEFAULT_AC_ID   1
+#define DEFAULT_MODE    0
 
 #define CAM_TILT_MIN    1000
 #define CAM_TILT_MAX    2000
@@ -47,6 +48,7 @@
 /* Options */
 char * device_name    = NULL;
 int aircraft_id       = DEFAULT_AC_ID;
+int mode              = DEFAULT_MODE;
 
 void parse_args(int argc, char * argv[])
 {
@@ -55,6 +57,7 @@
   for (i = 1; i < argc; i++) {
     if      (!strcmp(argv[i],"-d") && i<argc-1) device_name = argv[++i];
     else if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
+    else if (!strcmp(argv[i],"-m") && i<argc-1) mode = atoi(argv[++i]);
     else if (!strcmp(argv[i],"-h")) goto l_help;
   }
   return;
@@ -65,6 +68,7 @@
   printf("Options:\n");
   printf("  -d <string>  device name\n");
   printf("  -a <int>     aircraft id (default: %d)\n",DEFAULT_AC_ID);
+  printf("  -m <int>     mode (0 attitude, 1 speed, default: 
%d)\n",DEFAULT_MODE);
   printf("  -h           display this help\n");
   exit(1);
 }
@@ -144,6 +148,56 @@
        return 1;
 }
 
+#define SPEED_MAX_VAL 127
+// SPEED
+static gboolean joystick_speed_periodic(gpointer data __attribute__ 
((unused))) {
+
+       stick_read();
+
+       int vx = -STICK_APPLY_DEADBAND(stick_axis_values[1]); // vx > 0 front
+       int vy = STICK_APPLY_DEADBAND(stick_axis_values[0]); // vy > 0 right
+  int tilt = stick_axis_values[2];
+
+  int right = bit_is_set(stick_button_values,4);
+  int left  = bit_is_set(stick_button_values,3);
+  int up    = bit_is_set(stick_button_values,2);
+  int down  = bit_is_set(stick_button_values,1);
+
+  int yaw_rate = 0; // rate > 0 clockwise
+  if (left != right) {
+    if (right) yaw_rate = SPEED_MAX_VAL;
+    else yaw_rate = -SPEED_MAX_VAL;
+  }
+
+  int climb = 0; // climb > 0 up
+  if (up != down) {
+    if (up) climb = SPEED_MAX_VAL;
+    else climb = -SPEED_MAX_VAL;
+  }
+
+  static int last_tilt = 0;
+  cam_tilt_sp = CAM_TILT_MIN + (CAM_TILT_MAX - CAM_TILT_MIN) * (tilt + 
127.)/254.;
+  if (cam_tilt_sp < CAM_TILT_MIN) cam_tilt_sp = CAM_TILT_MIN;
+  if (cam_tilt_sp > CAM_TILT_MAX) cam_tilt_sp = CAM_TILT_MAX;
+
+  // NAV h = 4, v = 5
+  IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d", 4, 5, climb, vx, vy, 
yaw_rate, aircraft_id);
+  //printf("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d\n", 4, 5, climb, vx, vy, 
yaw_rate, aircraft_id);
+  if (tilt != last_tilt) {
+    IvySendMsg("dl DL_SETTING %d %d %f",
+        aircraft_id,
+        1, //CAM_TILT FIXME
+        cam_tilt_sp);
+//    printf("dl DL_SETTING %d %d %f\n",
+//        aircraft_id,
+//        1, //CAM_TILT FIXME
+//        cam_tilt_sp);
+  }
+  last_tilt = tilt;
+
+       return 1;
+}
+
 int main ( int argc, char** argv) {
 
   GMainLoop *ml =  g_main_loop_new(NULL, FALSE);
@@ -155,8 +209,16 @@
 
   if (stick_init(device_name) != 0) return 0;
 
-  g_timeout_add(TIMEOUT_PERIOD, joystick_attitude_periodic, NULL);
+  if (mode == 0) 
+    g_timeout_add(TIMEOUT_PERIOD, joystick_attitude_periodic, NULL);
+  else if (mode == 1)
+    g_timeout_add(TIMEOUT_PERIOD, joystick_speed_periodic, NULL);
+  else {
+    fprintf(stderr,"Unknown mode : %d\n",mode);
+    exit(1);
+  }
 
+
   g_main_loop_run(ml);
 
   return 0;