From 6d2e303633b454e3877dc0a00ea8c364944f84ae Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Mon, 10 Jun 2013 15:36:21 +0200
Subject: [PATCH] [ground_segment] add joystick hat to input2ivy

You can use the Hat<Position>(<hat_name>) function to trigger events,
where <Position> is one of
Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown

Only one hat can be used, but the interface will allow multiple ones in
the future.

closes #460
---
 conf/joystick/ms_sidewinder.xml           | 22 ++++---
 conf/joystick/xbox_gamepad.xml            | 67 +++++++++++++++++++++
 sw/ground_segment/joystick/input2ivy.ml   | 72 ++++++++++++++---------
 sw/ground_segment/joystick/ml_sdl_stick.c | 11 ++--
 sw/ground_segment/joystick/sdl_stick.c    | 49 ++++++++++-----
 sw/ground_segment/joystick/sdl_stick.h    |  1 +
 6 files changed, 166 insertions(+), 56 deletions(-)
 create mode 100644 conf/joystick/xbox_gamepad.xml

diff --git a/conf/joystick/ms_sidewinder.xml b/conf/joystick/ms_sidewinder.xml
index 29c61b7734..04ac84f305 100644
--- a/conf/joystick/ms_sidewinder.xml
+++ b/conf/joystick/ms_sidewinder.xml
@@ -4,8 +4,6 @@
   axis 1: pitch
   axis 2: yaw
   axis 3: throttle (reversed)
-  axis 4: hat switch left/right (right is positive)
-  axis 5: hat switch up/down (down is positive)
 
 It has 9 buttons.
  b0  - fire
@@ -18,6 +16,11 @@ It has 9 buttons.
  b7  - button D
  b8  - shift button
 
+and a POV hat.
+You can use the Hat<Position>(<hat_name>) function to trigger events,
+where <Position> is one of
+Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown
+so e.g. HatDown(myhat)
 -->
 
 <joystick>
@@ -26,8 +29,6 @@ It has 9 buttons.
     <axis index="1" name="pitch" limit="1.00" exponent="0.7" trim="0"/>
     <axis index="2" name="yaw" limit="1.00" exponent="0.7" trim="0"/>
     <axis index="3" name="throttle"/>
-    <axis index="4" name="hat_lr"/>
-    <axis index="5" name="hat_ud"/>
     <button index="0" name="fire"/>
     <button index="1" name="fire2"/>
     <button index="2" name="up"/>
@@ -37,23 +38,28 @@ It has 9 buttons.
     <button index="6" name="C"/>
     <button index="7" name="D"/>
     <button index="8" name="shift"/>
+    <hat index="0" name="hat"/>
   </input>
 
   <messages period="0.1">
 
-    <!--message class="datalink" name="RC_4CH" send_always="true">
+    <message class="datalink" name="RC_4CH" send_always="true">
       <field name="mode" value="1-fire+A"/>
       <field name="throttle" value="Fit(-throttle,-127,127,0,127)"/>
       <field name="roll" value="roll"/>
       <field name="pitch" value="pitch"/>
       <field name="yaw" value="yaw"/>
-    </message-->
+    </message>
 
-    <message class="datalink" name="RC_3CH" send_always="true">
+    <message class="ground" name="JUMP_TO_BLOCK" on_event="HatDown(hat)">
+      <field name="block_id" value="IndexOfBlock('land')"/>
+    </message>
+
+    <!--message class="datalink" name="RC_3CH" send_always="true">
       <field name="throttle_mode" value="0"/>
       <field name="roll" value="roll"/>
       <field name="pitch" value="pitch"/>
-    </message>
+    </message-->
 
   </messages>
 </joystick>
diff --git a/conf/joystick/xbox_gamepad.xml b/conf/joystick/xbox_gamepad.xml
new file mode 100644
index 0000000000..de8720a626
--- /dev/null
+++ b/conf/joystick/xbox_gamepad.xml
@@ -0,0 +1,67 @@
+<!-- Generic X-Box gamepad, e.g. Logitech wireless gamepad F710
+  Has six axes:
+  axis 0: LTS_H (left thumb stick horizontal) (or DPad horizontal in alternate mode)
+  axis 1: LTS_V (left thumb stick vertical) (or DPad vertical in alternate mode)
+  axis 2: LT (left trigger)
+  axis 3: RTS_H (right thumb stick horizontal)
+  axis 4: RTS_V (right thumb stick vertical)
+  axis 5: RT (right trigger)
+
+It has 11 buttons.
+ b0  - A
+ b1  - B
+ b2  - X
+ b3  - Y
+ b4  - LB (left button)
+ b5  - RB (right button)
+ b6  - back
+ b7  - start
+ b8  - ?
+ b9  - LSB (left stick button)
+ b10 - RSB (right stick button)
+
+and the DPad as a hat (in normal mode)
+You can use the Hat<Position>(<hat_name>) function to trigger events,
+where <Position> is one of
+Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown
+so e.g. HatDown(dpad)
+-->
+
+<joystick>
+  <input>
+    <axis index="0" name="yaw" limit="1.00" exponent="0.7" trim="0"/>
+    <axis index="1" name="throttle"/>
+    <axis index="2" name="LT" limit="1.00" trim="127"/>
+    <axis index="3" name="roll" limit="1.00" exponent="0.7" trim="0"/>
+    <axis index="4" name="pitch" limit="1.00" exponent="0.7" trim="0"/>
+    <axis index="5" name="RT" limit="1.00" trim="127"/>
+    <button index="0" name="A"/>
+    <button index="1" name="B"/>
+    <button index="2" name="X"/>
+    <button index="3" name="Y"/>
+    <button index="4" name="LB"/>
+    <button index="5" name="RB"/>
+    <button index="6" name="back"/>
+    <button index="7" name="start"/>
+    <button index="9" name="LSB"/>
+    <button index="10" name="RSB"/>
+    <hat index="0" name="dpad"/>
+  </input>
+
+  <messages period="0.1">
+
+    <message class="datalink" name="RC_4CH" send_always="true">
+      <!-- manual if you press both left and right shoulder buttons, only one: auto1, none: auto 2 -->
+      <field name="mode" value="2 - LB - RB"/>
+      <field name="throttle" value="Fit(-throttle,-127,127,0,127)"/>
+      <field name="roll" value="roll"/>
+      <field name="pitch" value="pitch"/>
+      <field name="yaw" value="yaw"/>
+    </message>
+
+    <message class="ground" name="JUMP_TO_BLOCK" on_event="HatDown(dpad)">
+      <field name="block_id" value="IndexOfBlock('land')"/>
+    </message>
+
+  </messages>
+</joystick>
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index b88c683bc5..ac2da4c10b 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -66,9 +66,9 @@ external stick_init : int -> int = "ml_stick_init"
 (** [stick_init device] Return 0 on success. Search for a device if [device]
     is the empty string *)
 
-external stick_read : unit -> int * int * int array = "ml_stick_read"
-(** Return the number of buttons, an integer of bits for the buttons values
-    and an array of signed integers for the axis *)
+external stick_read : unit -> int * int * int * int array = "ml_stick_read"
+(** Return the number of buttons, an integer of bits for the buttons values,
+    an integer for the hat and an array of signed integers for the axis *)
 
 (** Range for the input axis *)
 let max_input = 127
@@ -79,6 +79,7 @@ let trim_step = 0.2
 type input =
     Axis of int * int * float * float * float ref (* (index, deadband, limit, exponent, trim ) *)
   | Button of int
+  | Hat of int
 
 (** Description of a message *)
 type msg = {
@@ -207,6 +208,7 @@ let parse_input = fun input ->
             let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
             Axis (index, deadband, limit, exponent, ref trim)
         | "button" -> Button index
+        | "hat" -> Hat index
         | _ -> failwith "parse_input: unexepcted tag" in
     (name, value))
     (Xml.children input)
@@ -290,6 +292,7 @@ let trim_set = fun inputs value ->
   match input with
       Axis (i, deadband, limit, exponent, trim) -> trim := (second_of_two value)
     | Button i -> failwith "No trim for buttons"
+    | Hat _ -> failwith "No trim for hats"
 
 
 (** Input the trim file if it exists *)
@@ -341,10 +344,11 @@ let apply_trim = fun x trim ->
   if x_new > max_input then max_input else (if x_new < min_input then min_input else x_new)
 
 (** Access to an input value, button or axis *)
-let eval_input = fun buttons axis input ->
+let eval_input = fun buttons hat axis input ->
   match input with
       Axis (i, deadband, limit, exponent, trim) ->  (apply_trim (apply_limit (apply_exponent (apply_deadband axis.(i) deadband) exponent) limit) trim.contents)
     | Button i -> (buttons lsr i) land 0x1
+    | Hat _ -> hat
 
 (** Scale a value in the given bounds *)
 let scale = fun x min max ->
@@ -371,7 +375,7 @@ let pprz_mode = fun mode ->
   else 1
 
 (** Eval a function call (TO BE COMPLETED) *)
-let eval_call = fun f args ->
+let eval_call = fun f args hat->
   match f, args with
       "-", [a] -> - a
     | "-", [a1; a2] -> a1 - a2
@@ -382,6 +386,15 @@ let eval_call = fun f args ->
     | "||", [a1; a2] -> a1 lor a2
     | "<",  [a1; a2] -> if a1 < a2 then 1 else 0
     | ">",  [a1; a2] -> if a1 > a2 then 1 else 0
+    | "HatCentered",  [_] -> if hat = 0 then 1 else 0
+    | "HatUp",        [_] -> if hat = 1 then 1 else 0
+    | "HatRight",     [_] -> if hat = 2 then 1 else 0
+    | "HatRightUp",   [_] -> if hat = 3 then 1 else 0
+    | "HatDown",      [_] -> if hat = 4 then 1 else 0
+    | "HatRightDown", [_] -> if hat = 6 then 1 else 0
+    | "HatLeft",      [_] -> if hat = 8 then 1 else 0
+    | "HatLeftUp",    [_] -> if hat = 9 then 1 else 0
+    | "HatLeftDown",  [_] -> if hat = 12 then 1 else 0
     | "Scale", [x; min; max] -> scale (x) (min) (max)
     | "Fit", [x; min; max; min_input; max_input] -> fit (x) (min) (max) (min_input) (max_input)
     | "Bound", [x; min; max] -> bound (x) (min) (max)
@@ -390,22 +403,22 @@ let eval_call = fun f args ->
     | f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
 
 (** Eval an expression *)
-let eval_expr = fun buttons axis inputs variables expr ->
+let eval_expr = fun buttons hat axis inputs variables expr ->
   let rec eval = function
-  Syntax.Ident ident ->
-        (* try input first, then variables *)
-    let i = match (List.mem_assoc ident inputs, List.mem_assoc ident variables) with
-        (true, _) -> eval_input buttons axis (List.assoc ident inputs)
+    | Syntax.Ident ident ->
+      (* try input first, then variables and hat position *)
+      let i = match (List.mem_assoc ident inputs, List.mem_assoc ident variables) with
+      | (true, _) -> eval_input buttons hat axis (List.assoc ident inputs)
       | (false, true) ->
-        let v = List.assoc ident variables in
-        v.value
+          let v = List.assoc ident variables in
+          v.value
       | (false, false) -> failwith (sprintf "eval_expr: %s not found" ident)
-    in
-    i
+      in
+      i
     | Syntax.Int int -> int
     | Syntax.Float float -> failwith "eval_expr: float"
     | Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
-      eval_call ident (List.map eval exprs)
+      eval_call ident (List.map eval exprs) hat
     | Syntax.Index _ -> failwith "eval_expr: index"
     | Syntax.Field _ -> failwith "eval_expr: Field"
     | Syntax.Deref _ -> failwith "eval_expr: deref" in
@@ -433,6 +446,7 @@ let trim_save_add_leaf = fun x channel_pair ->
   match channel with
       Axis (i, deadband, limit, exponent, trim) -> x := x.contents ^ (Printf.sprintf "<trim axis='%s' value = '%f'/>" chan_name trim.contents)
     | Button i -> Printf.printf "%d" i
+    | Hat _ -> Printf.printf "hat"
 
 (** save trim settings to file *)
 let trim_save = fun inputs ->
@@ -450,13 +464,14 @@ let trim_adjust = fun axis_name adjustment inputs ->
   let input = my_assoc axis_name inputs in
   match input with
       Axis (i, deadband, limit, exponent, trim) -> trim := trim.contents +. adjustment
-    | Button i -> failwith "No trim for buttons"
+    | Button _ -> failwith "No trim for buttons"
+    | Hat _ -> failwith "No trim for hats"
 
 (** Update variables state *)
-let update_variables = fun inputs buttons axis variables ->
+let update_variables = fun inputs buttons hat axis variables ->
   List.iter (fun (_,var) ->
     List.iter (fun (value, expr) ->
-      let event = eval_expr buttons axis inputs variables expr in
+      let event = eval_expr buttons hat axis inputs variables expr in
       if event <> 0 then begin
         (* remove and add again ? *)
         var.value <- value
@@ -465,16 +480,16 @@ let update_variables = fun inputs buttons axis variables ->
   ) variables
 
 (** Send an ivy message if needed: new values and/or 'on_event' condition true*)
-let execute_action = fun ac_id inputs buttons axis variables message ->
+let execute_action = fun ac_id inputs buttons hat axis variables message ->
   let values =
     List.map
-      (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs variables expr)))
+      (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons hat axis inputs variables expr)))
       message.fields
 
   and on_event =
     match message.on_event with
         None -> true
-      | Some expr -> eval_expr buttons axis inputs variables expr <> 0 in
+      | Some expr -> eval_expr buttons hat axis inputs variables expr <> 0 in
 
   let previous_values = get_previous_values message.msg_name in
   (* FIXME ((value <> previous) && on_event) || send_always ??? *)
@@ -492,14 +507,15 @@ let execute_action = fun ac_id inputs buttons axis variables message ->
 
 
 (** Output on stderr the values from the input device *)
-let print_inputs = fun nb_buttons buttons axis ->
+let print_inputs = fun nb_buttons buttons hat axis ->
   print_string "buttons: ";
   for i = 0 to nb_buttons - 1 do
-    printf "%d:%d " i (eval_input buttons axis (Button i))
+    printf "%d:%d " i (eval_input buttons hat axis (Button i))
   done;
+  printf "\nhat: %d" (eval_input buttons hat axis (Hat 0));
   print_string "\naxis: ";
   for i = 0 to Array.length axis - 1 do
-    printf "%d:%d " i (eval_input buttons axis (Axis (i, 0, 1.0, 0.0, ref 0.0)))
+    printf "%d:%d " i (eval_input buttons hat axis (Axis (i, 0, 1.0, 0.0, ref 0.0)))
   done;
   ignore (print_newline ())
 
@@ -508,14 +524,14 @@ let print_inputs = fun nb_buttons buttons axis ->
     This is called at a rate programmed in the xml  *)
 let execute_actions = fun actions ac_id ->
   try
-    let (nb_buttons, buttons, axis) = stick_read () in
+    let (nb_buttons, buttons, hat, axis) = stick_read () in
 
     if !verbose then
-      print_inputs nb_buttons buttons axis;
+      print_inputs nb_buttons buttons hat axis;
 
     (* TODO update variables before msg *)
-    update_variables actions.inputs buttons axis actions.variables;
-    List.iter (execute_action ac_id actions.inputs buttons axis actions.variables) actions.messages
+    update_variables actions.inputs buttons hat axis actions.variables;
+    List.iter (execute_action ac_id actions.inputs buttons hat axis actions.variables) actions.messages
   with
       exc -> prerr_endline (Printexc.to_string exc)
 
diff --git a/sw/ground_segment/joystick/ml_sdl_stick.c b/sw/ground_segment/joystick/ml_sdl_stick.c
index 08b0aee696..6b76f8f0d8 100644
--- a/sw/ground_segment/joystick/ml_sdl_stick.c
+++ b/sw/ground_segment/joystick/ml_sdl_stick.c
@@ -48,9 +48,10 @@ ml_stick_init(value device_index_val) {
   return Val_int(opened);
 }
 
-/** Return a triple of
+/** Return a tuple with
  *  - the number of buttons
  *  - one integer with the buttons values
+ *  - one integer with the hat value
  *  - the array of axis values
  *
  * @param _unit  placeholder for ocaml equivalent of void
@@ -58,20 +59,22 @@ ml_stick_init(value device_index_val) {
 CAMLprim value
 ml_stick_read(value _unit) {
   CAMLparam0();
-  CAMLlocal3 (result, buttons, axis);
+  CAMLlocal4 (result, buttons, hat, axis);
 
   stick_read();
 
   buttons = Val_int(stick_button_values);
+  hat = Val_int(stick_hat_value);
 
   axis = caml_alloc_tuple(stick_axis_count);
   unsigned int i;
   for(i = 0; i < stick_axis_count; i++)
     Store_field(axis, i, Val_int(stick_axis_values[i]));
 
-  result = caml_alloc_tuple(3);
+  result = caml_alloc_tuple(4);
   Store_field(result, 0, Val_int(stick_button_count));
   Store_field(result, 1, buttons);
-  Store_field(result, 2, axis);
+  Store_field(result, 2, hat);
+  Store_field(result, 3, axis);
   CAMLreturn(result);
 }
diff --git a/sw/ground_segment/joystick/sdl_stick.c b/sw/ground_segment/joystick/sdl_stick.c
index 9218e69116..ff2a68359e 100644
--- a/sw/ground_segment/joystick/sdl_stick.c
+++ b/sw/ground_segment/joystick/sdl_stick.c
@@ -50,6 +50,7 @@ SDL_Joystick *sdl_joystick;
 SDL_Event sdl_event;
 
 int8_t stick_axis_values[AXIS_COUNT] = {0, 0, 0, 0, 0, 0};
+uint8_t stick_hat_value = 0;
 int32_t stick_button_values = 0;
 
 int stick_axis_count = 0;
@@ -84,6 +85,13 @@ int init_sdl_device(int device_index)
     dbgprintf(stderr,"ERROR: no suitable buttons found [%s:%d]\n",__FILE__,__LINE__);
   }
 
+  /* How many POV hats available */
+  int stick_hat_count = SDL_JoystickNumHats(sdl_joystick);
+  dbgprintf(stderr,"Available hats: %d (0x%x)\n",stick_hat_count,stick_hat_count);
+  if (stick_hat_count > 1) {
+    dbgprintf(stderr,"ERROR: only one POV hat supported [%s:%d]\n",__FILE__,__LINE__);
+  }
+
   /* How many axes available */
   stick_axis_count = min(SDL_JoystickNumAxes(sdl_joystick),AXIS_COUNT);
   dbgprintf(stderr,"Available axes: %d (0x%x)\n",stick_axis_count,stick_axis_count);
@@ -123,9 +131,8 @@ int stick_read( void ) {
     switch(sdl_event.type)
     {
       case SDL_JOYBUTTONDOWN:
-      //falls through to JOYBUTTONUP
+        //falls through to JOYBUTTONUP
       case SDL_JOYBUTTONUP:
-      {
         for (cnt = 0; cnt < stick_button_count; cnt++) {
           if (sdl_event.jbutton.button == cnt) {
             if (sdl_event.jbutton.state == SDL_PRESSED) {
@@ -134,8 +141,15 @@ int stick_read( void ) {
             break;
           }
         }
-      }
-      break;
+        break;
+
+      case SDL_JOYHATMOTION:
+        // only one hat (with index 0) supported
+        if (sdl_event.jhat.hat == 0) {
+          stick_hat_value = sdl_event.jhat.value;
+          break;
+        }
+        break;
 
       case SDL_JOYAXISMOTION:
         for (cnt = 0; cnt < stick_axis_count; cnt++) {
@@ -144,31 +158,34 @@ int stick_read( void ) {
             break;
           }
         }
-      break;
+        break;
 
       case SDL_QUIT:
-      printf("Quitting...\n");
-      exit(1);
-      break;
+        printf("Quitting...\n");
+        exit(1);
+        break;
 
       default:
-      //do nothing
-      //printf("unknown SDL event!!!\n");
-      break;
+        //do nothing
+        //printf("unknown SDL event!!!\n");
+        break;
     }
   }
 
-  dbgprintf(stderr,"buttons ");
+  dbgprintf(stderr, "buttons ");
   for (cnt = 0; cnt < stick_button_count; cnt++) {
-    dbgprintf(stderr,"%d ",(stick_button_values >> cnt) & 1 );
+    dbgprintf(stderr, "%d ", (stick_button_values >> cnt) & 1 );
   }
 
-  dbgprintf(stderr,"| axes ");
+  dbgprintf(stderr, "| hat ");
+  dbgprintf(stderr, "%d ", stick_hat_value);
+
+  dbgprintf(stderr, "| axes ");
   for (cnt = 0; cnt < stick_axis_count; cnt++) {
-    dbgprintf(stderr,"%d ",stick_axis_values[cnt]);
+    dbgprintf(stderr, "%d ", stick_axis_values[cnt]);
   }
 
-  dbgprintf(stderr,"\n");
+  dbgprintf(stderr, "\n");
 
   return 0;
 }
diff --git a/sw/ground_segment/joystick/sdl_stick.h b/sw/ground_segment/joystick/sdl_stick.h
index 6e5f52825d..28ecaa0a7f 100644
--- a/sw/ground_segment/joystick/sdl_stick.h
+++ b/sw/ground_segment/joystick/sdl_stick.h
@@ -40,6 +40,7 @@
 /* Global variables about the initialized device */
 extern int8_t stick_axis_values[STICK_AXIS_COUNT];
 extern int32_t stick_button_values;
+extern uint8_t stick_hat_value;
 extern int stick_axis_count, stick_button_count;
 
 /** Initialize a joystick with SDL.