mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
add generic tool to translate events from an input device into messages on the ivy bus
This commit is contained in:
Pascal Brisset
@@ -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
|
||||
|
||||
|
||||
+1
-1
@@ -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"/>
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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>
|
||||
Reference in New Issue
Block a user