From 77e4b33172e80e824e73ba2f47296b41420ca580 Mon Sep 17 00:00:00 2001
From: Pascal Brisset <pascal.brisset@free.fr>
Date: Tue, 20 Oct 2009 11:00:34 +0000
Subject: [PATCH]  add generic tool to translate events from an input device
 into messages on the ivy bus

---
 sw/ground_segment/joystick/Makefile           |  29 +-
 .../{booz_fms.xml => attack3_booz_nav.xml}    |   2 +-
 sw/ground_segment/joystick/input2ivy.ml       | 352 ++++++++++++++++++
 sw/ground_segment/joystick/ml_usb_stick.c     |  69 ++++
 sw/ground_segment/joystick/xbox_booz_nav.xml  |  31 ++
 5 files changed, 476 insertions(+), 7 deletions(-)
 rename sw/ground_segment/joystick/{booz_fms.xml => attack3_booz_nav.xml} (93%)
 create mode 100644 sw/ground_segment/joystick/input2ivy.ml
 create mode 100644 sw/ground_segment/joystick/ml_usb_stick.c
 create mode 100644 sw/ground_segment/joystick/xbox_booz_nav.xml

diff --git a/sw/ground_segment/joystick/Makefile b/sw/ground_segment/joystick/Makefile
index dd609b9b3b..0bcc126367 100644
--- a/sw/ground_segment/joystick/Makefile
+++ b/sw/ground_segment/joystick/Makefile
@@ -23,23 +23,40 @@
 # Quiet compilation
 Q=@
 
+OCAMLC = ocamlc
+OCAMLLIB = ../../lib/ocaml
+TOOLSDIR = ../../tools
+OCAMLINCLUDES= -I $(OCAMLLIB) -I +lablgtk2 -I +xml-light -I $(TOOLSDIR)
+LIBPPRZCMA=$(OCAMLLIB)/lib-pprz.cma
+
 all: main_stick
 
-main_stick: main_stick.c usb_stick.c
+main_stick: main_stick.o usb_stick.o
 	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy
 
-main_stick_debug: main_stick.c usb_stick.c
+main_stick_debug: main_stick.o usb_stick.o
 	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy -DSTICK_DBG
 
-apm_stick: apm_stick.c usb_stick.c
+apm_stick: apm_stick.o usb_stick.o
 	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy
 
-xbox_stick: xbox_stick.c usb_stick.c
+xbox_stick: xbox_stick.o usb_stick.o
 	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy
 
-attack3_stick: attack3_stick.c usb_stick.c
+attack3_stick: attack3_stick.o usb_stick.o
 	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy
 
+input2ivy: usb_stick.o ml_usb_stick.o input2ivy.cmo
+	$(OCAMLC) $(OCAMLINCLUDES) -custom -o $@ unix.cma str.cma glibivy-ocaml.cma xml-light.cma lib-pprz.cma lablgtk.cma $(TOOLSDIR)/fp_syntax.cmo $(TOOLSDIR)/fp_lexer.cmo $(TOOLSDIR)/fp_parser.cmo $(TOOLSDIR)/fp_proc.cmo $^
+
+
+%.o : %.c
+	gcc -c -O2 -Wall `pkg-config glib-2.0 --cflags` $<
+
+%.cmo : %.ml
+	@echo OC $<
+	$(Q)$(OCAMLC) $(OCAMLINCLUDES) -c $<
+
 clean:
-	rm -f *~ core *.o *.bak .depend main_stick main_stick_debug apm_stick xbox_stick attack3_stick
+	rm -f *~ core *.o *.bak .depend main_stick main_stick_debug apm_stick xbox_stick attack3_stick *.cmo *.cmi input2ivy
 
diff --git a/sw/ground_segment/joystick/booz_fms.xml b/sw/ground_segment/joystick/attack3_booz_nav.xml
similarity index 93%
rename from sw/ground_segment/joystick/booz_fms.xml
rename to sw/ground_segment/joystick/attack3_booz_nav.xml
index 33f747fe8e..715f2c4293 100644
--- a/sw/ground_segment/joystick/booz_fms.xml
+++ b/sw/ground_segment/joystick/attack3_booz_nav.xml
@@ -11,7 +11,7 @@
   <button index="7" name="button8"/>
 </input>
 <messages period="0.1">
-  <message class="datalink" name="BOOZ2_FMS_COMMAND" min_period="1">
+  <message class="datalink" name="BOOZ2_FMS_COMMAND">
    <field name="h_mode" value="IndexOfEnum(NAV)"/>
    <field name="v_mode" value="IndexOfEnum(NAV)"/>
    <field name="v_sp"   value="(up-down)*42"/>
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
new file mode 100644
index 0000000000..2ced5fcbd3
--- /dev/null
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -0,0 +1,352 @@
+(*
+ * $Id$
+ *
+ * Forwarding events from an USB input device to the ivy bus
+ *  
+ * Copyright (C) 2009 ENAC, Pascal Brisset
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ *)
+
+open Printf
+
+let (//) = Filename.concat
+let conf_dir = Env.paparazzi_home // "conf"
+let verbose = ref false (* Command line option *)
+
+(** Messages libraries *)
+module DL = Pprz.Messages(struct let name = "datalink" end)
+module G = Pprz.Messages(struct let name = "ground" end)
+
+(** Syntax for expressions *)
+module Syntax = Fp_syntax (* From ../../tools/ *)
+
+(** Hash table of name-index associations for the settings of the A/C *)
+let index_of_settings = Hashtbl.create 13
+
+(**Hash table of name-index associations for the flightplan blocks of the A/C*)
+let index_of_blocks = Hashtbl.create 13
+
+(** External C functions to access the input device *)
+external stick_init : string -> 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 *)
+
+(** Range for the input axis *)
+let max_input = 127
+let min_input = -127
+
+(** Representation of an input value *)
+type input =
+    Axis of int
+  | Button of int
+
+(** Description of a message *)
+type msg = {
+    msg_name : string;
+    msg_class : string;
+    fields : (string * Syntax.expression) list;
+    on_event : Syntax.expression option
+  }
+
+(** Represenation of an input device and of the messages to send *)
+type actions = {
+    period_ms : int;
+    inputs : (string*input) list;
+    messages : msg list
+  }
+
+(** Get a message description from its name (and class name) *)
+let get_message = fun class_name msg_name ->
+  match class_name with
+    "datalink" -> snd (DL.message_of_name msg_name)
+  | "ground" -> snd (G.message_of_name msg_name)
+  | _ -> failwith class_name
+
+(** Get the A/C id from its name in conf/conf.xml *)
+let ac_id_of_name = fun ac_name ->
+  let conf_xml = Xml.parse_file (conf_dir // "conf.xml") in
+  try
+    let aircraft = ExtXml.child ~select:(fun x -> Xml.attrib x "name" = ac_name) conf_xml "aircraft" in
+    ExtXml.int_attrib aircraft "ac_id"
+  with
+    Not_found ->
+      failwith (sprintf "A/C '%s' not found" ac_name)
+
+(** Fill the index_of_settings table from var/AC/settings.xml *)
+let hash_index_of_settings = fun ac_name ->
+  let xml_file = Env.paparazzi_home // "var" // ac_name // "settings.xml" in
+  let xml = Xml.parse_file xml_file in
+  let index = ref 0 in
+  let rec loop = fun xml ->
+    if Xml.tag xml = "dl_settings" then
+      List.iter loop (Xml.children xml)
+    else begin (* dl_setting *)
+      Hashtbl.add index_of_settings (Xml.attrib xml "var") !index;
+      incr index
+    end in
+  loop (ExtXml.child xml "dl_settings")
+
+
+(** Fill the index_of_blocks table from var/AC/flight_plan.xml *)
+let hash_index_of_blocks = fun ac_name ->
+  let xml_file = Env.paparazzi_home // "var" // ac_name // "flight_plan.xml" in
+  let dump = Xml.parse_file xml_file in
+  let flight_plan = ExtXml.child dump "flight_plan" in
+  let blocks = ExtXml.child flight_plan "blocks" in
+  List.iter (fun block ->
+    Hashtbl.add
+      index_of_blocks
+      (Xml.attrib block "name") 
+      (ExtXml.int_attrib block "no"))
+    (Xml.children blocks)
+
+
+(* Return the rank of an element in a list, first is 0 *)
+let rank = fun x l ->
+  let rec loop i = function
+      [] -> raise Not_found
+    | y::ys -> if x = y then i else loop (i+1) ys in
+  loop 0 l
+
+(** Eval IndexOfEnum, IndexOfSetting and IndexOfBlock built-in functions
+   in an expression *)
+let eval_settings_and_blocks = fun field_descr expr ->
+  let rec loop = function
+    Syntax.Call ("IndexOfEnum", [Syntax.Ident enum]) -> begin
+      try Syntax.Int (rank enum field_descr.Pprz.enum) with
+	Not_found -> failwith (sprintf "IndexOfEnum: unknown value '%s'" enum)
+    end
+  | Syntax.Call ("IndexOfSetting", [Syntax.Ident var]) -> begin
+      try Syntax.Int (Hashtbl.find index_of_settings var) with
+	Not_found -> failwith (sprintf "IndexOfSetting: unknown var '%s'" var)
+    end
+  | Syntax.Call ("IndexOfBlock", [Syntax.Ident name]) -> begin
+      try Syntax.Int (Hashtbl.find index_of_blocks name) with
+	Not_found -> failwith (sprintf "IndexOfBlock: unknown block '%s'" name)
+    end
+  | Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
+      Syntax.Call (ident, List.map loop exprs)
+  | e -> e in
+  loop expr
+
+(** Parse an XML list of input channels *)
+let parse_input = fun input ->
+  List.map
+    (fun x ->
+      let name = Xml.attrib x "name"
+      and index = ExtXml.int_attrib x "index" in
+      let value =
+	match Xml.tag x with
+	  "axis" -> Axis index
+	| "button" -> Button index
+	| _ -> failwith "parse_input: unexepcted tag" in
+      (name, value))
+    (Xml.children input)
+
+(** Parse a 'à la C' expression *)
+let parse_value = fun s ->
+  Fp_proc.parse_expression s
+
+(** Parse a message field and eval *)
+let parse_msg_field = fun msg_descr field ->
+  let name = Xml.attrib field "name" in
+  let field_descr = List.assoc name msg_descr.Pprz.fields in
+
+  let value = eval_settings_and_blocks field_descr (parse_value (Xml.attrib field "value")) in
+  (name, value)
+
+(** Parse a complete message and build its representation *)
+let parse_msg = fun msg ->
+  let msg_name = Xml.attrib msg "name"
+  and msg_class = Xml.attrib msg "class" in
+
+  let msg_descr = get_message msg_class msg_name in
+
+  let fields = List.map (parse_msg_field msg_descr) (Xml.children msg) in
+
+  let on_event =
+    try Some (parse_value (Xml.attrib msg "on_event")) with _ -> None in
+
+  { msg_name = msg_name;
+    msg_class = msg_class;
+    fields = fields;
+    on_event = on_event
+  }
+
+(** Parse the complete (input and messages) XML desxription *)
+let parse_descr = fun xml_file ->
+  let xml = Xml.parse_file xml_file in
+
+  let inputs = parse_input (ExtXml.child xml "input")
+  and messages_xml = ExtXml.child xml "messages" in
+
+  let period_ms =truncate (1000.*.ExtXml.float_attrib messages_xml "period")
+  and messages = List.map parse_msg (Xml.children messages_xml) in
+
+  { period_ms = period_ms; inputs = inputs; messages = messages }
+
+(** Verbose List.assco *)
+let my_assoc = fun x l ->
+  try List.assoc x l with Not_found ->
+    failwith (sprintf "my_assoc: %s not found" x)
+
+(** Access to an input value, button or axis *)
+let eval_input = fun buttons axis input ->
+  match input with
+    Axis i -> axis.(i)
+  | Button i -> (buttons lsr i) land 0x1
+
+(** Scale a value in the given bounds *)
+let scale = fun x min max ->
+  min + ((x - min_input) * (max - min)) / (max_input - min_input)
+
+(** Eval a function call (TO BE COMPLETED) *)
+let eval_call = fun f args ->
+  match f, args with
+    "-", [a1; a2] -> a1 - a2
+  | "+", [a1; a2] -> a1 + a2
+  | "*", [a1; a2] -> a1 * a2
+  | "&&", [a1; a2] -> a1 land a2
+  | "Scale", [x; min; max] -> scale (x) (min) (max)
+  | f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
+
+(** Eval an expression *)
+let eval_expr = fun buttons axis inputs expr ->
+  let rec eval = function
+      Syntax.Ident ident ->
+	let input = my_assoc ident inputs in
+	eval_input buttons axis input
+    | 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)
+    | Syntax.Index _ -> failwith "eval_expr: index" in
+
+  eval expr
+  
+(** Store for the last sent values: msg_name->values *)
+let last_values = Hashtbl.create 5
+
+(** Get the previous sent values for a given message *)
+let get_previous_values = fun msg_name ->
+  try Hashtbl.find last_values msg_name with Not_found -> (false, [])
+
+(** Record the current values for a given message *)
+let record_values = fun msg_name values ->
+  Hashtbl.replace last_values msg_name values
+
+(**Send an ivy message if needed: new values and/or 'on_event' condition true*)
+let execute_action = fun ac_id inputs buttons axis message ->
+  let values =
+    List.map
+      (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs expr)))
+      message.fields
+
+  and on_event = 
+    match message.on_event with
+      None -> true
+    | Some expr -> eval_expr buttons axis inputs expr <> 0 in
+
+  let previous_values = get_previous_values message.msg_name in
+  if (on_event, values) <> previous_values && on_event then begin
+    let vs = ("ac_id", Pprz.Int ac_id) :: values in
+    match message.msg_class with
+      "datalink" -> DL.message_send "input2ivy" message.msg_name vs
+    | "ground" -> G.message_send "input2ivy" message.msg_name vs
+    | c -> failwith (sprintf "execute_action: unknown class '%s'" c)
+  end;
+  record_values message.msg_name (on_event, values)
+
+
+(** Output on stderr the values from the input device *)
+let print_inputs = fun nb_buttons buttons axis ->
+  fprintf stderr "buttons: ";
+  for i = 0 to nb_buttons - 1 do
+    fprintf stderr "%d:%d " i (eval_input buttons axis (Button i))
+  done;
+  fprintf stderr "\naxis: ";
+  for i = 0 to Array.length axis - 1 do
+    fprintf stderr "%d:%d " i (eval_input buttons axis (Axis i))
+  done;
+  fprintf stderr "\n%!"
+  
+
+(** Get the values from the input values and send messages *)
+let execute_actions = fun actions ac_id ->
+  try
+    let (nb_buttons, buttons, axis) = stick_read () in
+
+    if !verbose then
+      print_inputs nb_buttons buttons axis;
+    
+    List.iter (execute_action ac_id actions.inputs buttons axis) actions.messages
+  with
+    exc -> prerr_endline (Printexc.to_string exc)
+
+
+
+(************************************* MAIN **********************************)
+let () =
+  let ivy_bus = ref "127.255.255.255:2010" in
+  let device_name = ref ""
+  and ac_name = ref "MYAC"
+  and xml_descr = ref "" in
+
+  let anon_fun = (fun x -> xml_descr := x) in
+  let speclist =  
+    [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.255:2010";
+      "-ac",  Arg.Set_string ac_name, "<A/C name>";
+      "-d",  Arg.Set_string device_name, "<device name>";
+      "-v",  Arg.Set verbose, "Verbose mode (useful to identify the channels of an input device)";
+      "-", Arg.String anon_fun, "<xml file of actions>"
+    ]
+  and usage_msg = "Usage: " in
+
+  Arg.parse speclist anon_fun usage_msg;
+
+  if !xml_descr = "" then begin
+    Arg.usage speclist usage_msg;
+    exit 1
+  end;
+
+  let ac_id = ac_id_of_name !ac_name in
+
+  hash_index_of_settings !ac_name;
+  hash_index_of_blocks !ac_name;
+
+  let actions = parse_descr !xml_descr in
+
+  if stick_init !device_name <> 0 then
+    failwith (sprintf "Error: cannot open device %s\n" !device_name);
+
+  (** Connect to the Ivy bus *)
+  Ivy.init "Paparazzi joystick" "READY" (fun _ _ -> ());
+  Ivy.start !ivy_bus;
+
+  ignore (Glib.Timeout.add actions.period_ms (fun () -> execute_actions actions ac_id; true));
+  
+  (** Start the main loop *)
+  let loop = Glib.Main.create true in
+  while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done
+
diff --git a/sw/ground_segment/joystick/ml_usb_stick.c b/sw/ground_segment/joystick/ml_usb_stick.c
new file mode 100644
index 0000000000..533d744bb5
--- /dev/null
+++ b/sw/ground_segment/joystick/ml_usb_stick.c
@@ -0,0 +1,69 @@
+/* 
+   $Id$
+
+ Copyright (C) 2009 ENAC, Pascal Brisset, Gautier Hattenberger
+
+ Ocaml bindings for USB stick
+
+ This file is part of paparazzi.
+
+ paparazzi is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2, or (at your option)
+ any later version.
+
+ paparazzi is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with paparazzi; see the file COPYING.  If not, write to
+ the Free Software Foundation, 59 Temple Place - Suite 330,
+ Boston, MA 02111-1307, USA.  
+*/ 
+
+#include <stdio.h>
+#include <string.h>
+#include <caml/mlvalues.h>
+#include <caml/alloc.h>
+#include <caml/memory.h>
+#include <caml/callback.h>
+#include <caml/fail.h>
+#include "usb_stick.h"
+
+
+/** Open an input device. Try to discover it if name is empty */
+value
+ml_stick_init(value device_name_val) {
+  char * device_name = String_val(device_name_val);
+  if (strlen(device_name) == 0)
+    device_name = NULL;
+  int opened = stick_init(device_name);
+  return Val_int(opened);
+}
+
+/** Return a triple of 
+    - the number of buttons
+    - one integer with the buttons values
+    - the array of axis values */
+CAMLprim value
+ml_stick_read(value _unit) {
+  CAMLparam0();
+  CAMLlocal3 (result, buttons, axis);
+
+  stick_read();
+
+  buttons = Val_int(stick_button_values);
+
+  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);
+  Store_field(result, 0, Val_int(stick_button_count));
+  Store_field(result, 1, buttons);
+  Store_field(result, 2, axis);
+  CAMLreturn(result);
+}
diff --git a/sw/ground_segment/joystick/xbox_booz_nav.xml b/sw/ground_segment/joystick/xbox_booz_nav.xml
new file mode 100644
index 0000000000..cb6f81135d
--- /dev/null
+++ b/sw/ground_segment/joystick/xbox_booz_nav.xml
@@ -0,0 +1,31 @@
+<joystick>
+ <input>
+  <axis index="0" name="roll"/>
+  <axis index="1" name="pitch"/>
+  <axis index="3" name="yaw"/>
+  <axis index="4" name="tilt"/>
+  <button index="3" name="X"/>
+  <button index="0" name="down"/>
+  <button index="1" name="up"/>
+  <button index="4" name="Y"/>
+</input>
+<messages period="0.1">
+  <message class="datalink" name="BOOZ2_FMS_COMMAND">
+   <field name="h_mode" value="IndexOfEnum(NAV)"/>
+   <field name="v_mode" value="IndexOfEnum(NAV)"/>
+   <field name="v_sp"   value="(up-down)*42"/>
+   <field name="h_sp_1" value="roll"/>
+   <field name="h_sp_2" value="pitch"/>
+   <field name="h_sp_3" value="yaw"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING">
+    <field name="index" value="IndexOfSetting(booz2_cam_tilt)"/>
+    <field name="value" value="Scale(tilt, 1000, 2000)"/>
+  </message>
+
+  <message class="ground" name="JUMP_TO_BLOCK" on_event="X && Y">
+    <field name="block_id" value="IndexOfBlock('land here')"/>
+  </message>
+ </messages>
+</joystick>