diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index 6a96dd7ff2..777114e62b 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -38,12 +38,12 @@ SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo fw_serv SERVERCMX = $(SERVERCMO:.cmo=.cmx) -all: link server messages settings ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge app_server ivy2nmea +all: link server messages settings ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy2serial ivy_serial_bridge app_server ivy2nmea opt: server.opt clean: - $(Q)rm -f link server messages settings *.bak *~ core *.o .depend *.opt *.out *.cm* ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge app_server gpsd2ivy c_ivy_client_example_1 c_ivy_client_example_2 c_ivy_client_example_3 ivy2nmea + $(Q)rm -f link server messages settings *.bak *~ core *.o .depend *.opt *.out *.cm* ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy2serial ivy_serial_bridge app_server gpsd2ivy c_ivy_client_example_1 c_ivy_client_example_2 c_ivy_client_example_3 ivy2nmea messages : messages.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ @@ -85,6 +85,10 @@ ivy2udp : ivy2udp.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< +ivy2serial : ivy2serial.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) + @echo OL $@ + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< + 150m : 150m.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ diff --git a/sw/ground_segment/tmtc/ivy2serial.ml b/sw/ground_segment/tmtc/ivy2serial.ml new file mode 100644 index 0000000000..8f8386ec34 --- /dev/null +++ b/sw/ground_segment/tmtc/ivy2serial.ml @@ -0,0 +1,112 @@ +(* + * Copyright (C) 2008 ENAC, Pascal Brisset, Antoine Drouin + * Copyright (C) 2016 Gautier Hattenberger + * + * 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. + * + *) + +(** + * Forward telemetry messages from the ivy bus to a serial connection + * a.k.a. a "reverse" link agent: + * ivy telemetry message -> serial out + * serial in -> ivy datalink message + * + * This tool can be used to rebuild the airborne modem input/output + * from the Ocaml simulator that directly outputs Ivy messages + * + * Only the PPRZ message format is supported + *) + + +module Tm_Pprz = PprzLink.Messages(struct let name = "telemetry" end) +module Dl_Pprz = PprzLink.Messages(struct let name = "datalink" end) +module PprzTransport = Protocol.Transport(Pprz_transport.Transport) + +open Printf + +let () = + + (* Options *) + let ivy_bus = ref Defivybus.default_ivy_bus in + let device = ref "/dev/ttyUSB0" + and baudrate = ref "57600" + and id = ref "1" in + + let options = [ + "-id", Arg.Set_string id , (sprintf " Default is %s" !id); + "-b", Arg.Set_string ivy_bus, (sprintf " Default is %s" !ivy_bus); + "-d", Arg.Set_string device, (sprintf " Default is %s" !device); + "-baudrate", Arg.Set_string baudrate, (sprintf " Default is %s" !baudrate); + ] in + Arg.parse + options + (fun x -> fprintf stderr "Warning: Discarding '%s'" x) + "Usage: "; + + (* Open serial device *) + let fd = Serial.opendev !device (Serial.speed_of_baudrate !baudrate) false in + + (* Start Ivy *) + Ivy.init "ivy2serial" "READY" (fun _ _ -> ()); + Ivy.start !ivy_bus; + + (* Bind to all Ivy message from AC (telemetry) and send over serial link *) + let get_ivy_message = fun _ args -> + try + let (msg_id, vs) = Tm_Pprz.values_of_string args.(0) in + let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) vs in + let buf = Pprz_transport.Transport.packet payload in + let o = Unix.out_channel_of_descr fd in + Printf.fprintf o "%s" buf; flush o + with _ -> () in + + let _b = Ivy.bind get_ivy_message (sprintf "^%s (.*)" !id) in + + (* The function to be called when data is available *) + let buffer_size = 256 in + let buffer = String.create buffer_size in + let get_datalink_message = fun _ -> + begin + try + let n = input (Unix.in_channel_of_descr fd) buffer 0 buffer_size in + let b = String.sub buffer 0 n in + Debug.trace 'x' (Debug.xprint b); + + let use_dl_message = fun payload -> + Debug.trace 'x' (Debug.xprint (Protocol.string_of_payload payload)); + let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in + let msg = Dl_Pprz.message_of_id msg_id in + Dl_Pprz.message_send "ground_dl" msg.PprzLink.name values in + + assert (PprzTransport.parse use_dl_message b = n) + with + exc -> + prerr_endline (Printexc.to_string exc) + end; + true in + + (* Connect IO watch *) + let hangup = fun _ -> + prerr_endline "Serial link disconnected"; + exit 1 in + ignore (Glib.Io.add_watch [`HUP] hangup (GMain.Io.channel_of_descr fd)); + ignore (Glib.Io.add_watch [`IN] get_datalink_message (GMain.Io.channel_of_descr fd)); + + (* Main Loop *) + GMain.main ()