diff --git a/conf/messages.xml b/conf/messages.xml index bc654b3a93..7e1441877a 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -138,7 +138,7 @@ - + diff --git a/sw/ground_segment/tmtc/listen.ml b/sw/ground_segment/tmtc/listen.ml index 33623eb243..218ef9f9fc 100644 --- a/sw/ground_segment/tmtc/listen.ml +++ b/sw/ground_segment/tmtc/listen.ml @@ -62,8 +62,8 @@ let _ = let handle_pprz_message = fun (msg_id, values) -> let msg = Tele_Pprz.message_of_id msg_id in - let s = String.concat " " (List.map snd values) in - Ivy.send (sprintf "1234.567 %s %s" msg.Pprz.name s) in + let s = Tele_Pprz.string_of_message msg values in + Ivy.send (sprintf "1234.567 %s" s) in listen_tty handle_pprz_message !serial_dev; Ivy.init "Paparazzi listen" "READY" (fun _ _ -> ()); diff --git a/sw/ground_segment/tmtc/receive.ml b/sw/ground_segment/tmtc/receive.ml index 93af00e9ef..ad22c15b0c 100644 --- a/sw/ground_segment/tmtc/receive.ml +++ b/sw/ground_segment/tmtc/receive.ml @@ -65,8 +65,6 @@ let listen_pprz_modem = fun use_pprz_message tty -> ignore (Glib.Io.add_watch [`IN] cb (Glib.Io.channel_of_descr fd)) -let fos = float_of_string -let ios = int_of_string let space = Str.regexp "[ \t]+" let (//) = Filename.concat @@ -123,14 +121,27 @@ let logger = fun () -> end; open_out (logs_path // name) +let fvalue = fun x -> + match x with + Pprz.Float x -> x + | Pprz.Int32 x -> Int32.to_float x + | Pprz.Int x -> float_of_int x + | _ -> failwith (sprintf "Receive.log_and_parse: float expected, got '%s'" (Pprz.string_of_value x)) +let ivalue = fun x -> + match x with + Pprz.Int x -> x + | _ -> failwith "Receive.log_and_parse: int expected" + + let log_and_parse = fun log ac_name a msg values -> let t = U.gettimeofday () in - let s = String.concat " " (List.map snd values) in + let s = String.concat " " (List.map (fun (_, v) -> Pprz.string_of_value v) values) in fprintf log "%.2f %s %s %s\n" t ac_name msg.Pprz.name s; flush log; Ivy.send (sprintf "%s RAW %.2f %s %s" ac_name t msg.Pprz.name s); let value = fun x -> try List.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in - let fvalue = fun x -> fos (value x) in + let fvalue = fun x -> fvalue (value x) + and ivalue = fun x -> ivalue (value x) in match msg.Pprz.name with "GPS" -> a.east <- fvalue "east" /. 100.; @@ -143,19 +154,19 @@ let log_and_parse = fun log ac_name a msg values -> a.roll <- fvalue "phi"; a.pitch <- fvalue "theta" | "NAVIGATION" -> - a.cur_block <- ios (value "cur_block"); - a.cur_stage <- ios (value "cur_stage") + a.cur_block <- ivalue "cur_block"; + a.cur_stage <- ivalue "cur_stage" | "CLIMB_PID" -> a.throttle <- fvalue "gaz" /. 9600. *. 100.; a.rpm <- a.throttle *. 100. | "BAT" -> a.bat <- fvalue "voltage" /. 10. | "PPRZ_MODE" -> - a.ap_mode <- ios (value "ap_mode"); - a.ap_altitude <- ios (value "ap_altitude"); - a.if_calib_mode <- ios (value "if_calib_mode"); - a.mcu1_status <- ios (value "mcu1_status"); - a.lls_calib <- ios (value "lls_calib") + a.ap_mode <- ivalue "ap_mode"; + a.ap_altitude <- ivalue "ap_altitude"; + a.if_calib_mode <- ivalue "if_calib_mode"; + a.mcu1_status <- ivalue "mcu1_status"; + a.lls_calib <- ivalue "lls_calib" | _ -> () @@ -175,26 +186,27 @@ let send_aircraft_msg = fun ac -> try let sof = fun f -> sprintf "%.1f" f in let a = Hashtbl.find aircrafts ac in - let values = ["roll", sof (Geometry_2d.rad2deg a.roll); - "pitch", sof (Geometry_2d.rad2deg a.pitch); - "east", sof a.east; - "north", sof a.north; - "speed", sof a.gspeed; - "heading", sof (Geometry_2d.rad2deg a.course); - "alt", sof a.alt; - "climb", sof a.climb] in + let f = fun x -> Pprz.Float x in + let values = ["roll", f (Geometry_2d.rad2deg a.roll); + "pitch", f (Geometry_2d.rad2deg a.pitch); + "east", f a.east; + "north", f a.north; + "speed", f a.gspeed; + "heading", f (Geometry_2d.rad2deg a.course); + "alt", f a.alt; + "climb", f a.climb] in let _, fp_msg = AcInfo_Pprz.message_of_name "FLIGHT_PARAM" in Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message fp_msg values)); - let values = ["cur_block", soi a.cur_block;"cur_stage", soi a.cur_stage] + let values = ["cur_block", Pprz.Int a.cur_block;"cur_stage", Pprz.Int a.cur_stage] and _, ns_msg = AcInfo_Pprz.message_of_name "NAV_STATUS" in Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message ns_msg values)); - let values = ["throttle", sof a.throttle;"rpm", sof a.rpm;"temp", sof a.temp;"bat", sof a.bat;"amp", sof a.amp;"energy", sof a.energy] + let values = ["throttle", f a.throttle;"rpm", f a.rpm;"temp", f a.temp;"bat", f a.bat;"amp", f a.amp;"energy", f a.energy] and _, es_msg = AcInfo_Pprz.message_of_name "ENGINE_STATUS" in Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message es_msg values)); - let values = ["mode", soi a.ap_mode; "v_mode", soi a.ap_altitude] + let values = ["mode", Pprz.Int a.ap_mode; "v_mode", Pprz.Int a.ap_altitude] and _, as_msg = AcInfo_Pprz.message_of_name "AP_STATUS" in Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message as_msg values)) with @@ -257,7 +269,7 @@ let handle_pprz_message = fun log a -> match !name with None -> if msg.Pprz.name = "IDENT" then - let n = List.assoc "id" values in + let n = Pprz.string_of_value (List.assoc "id" values) in name := Some n; register_aircraft n a | Some ac_name -> diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 2495e679e9..12f91c135c 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -31,7 +31,7 @@ type message_id = int type class_name = string type format = string type _type = string -type value = string +type value = Int of int | Float of float | String of string | Int32 of int32 type field = { _type : _type; fformat : format; @@ -66,7 +66,25 @@ let types = [ ("int16", { format = "%d"; glib_type = "gint16"; size = 2; value="42" }); ("int32", { format = "%ld" ; glib_type = "gint32"; size = 4; value="42" }); ("float", { format = "%f" ; glib_type = "gfloat"; size = 4; value="4.2" }) -] +] + +let int_of_string = fun x -> + try int_of_string x with + _ -> failwith (sprintf "Pprz.int_of_string: %s" x) + +let value = fun t v -> + match t with + "uint8" | "uint16" | "int8" | "int16" -> Int (int_of_string v) + | "uint32" | "int32" -> Int32 (Int32.of_string v) + | "float" -> Float (float_of_string v) + | "string" -> String v + | _ -> failwith (sprintf "Pprz.value: Unexpected type: %s" t) + +let string_of_value = function + Int x -> string_of_int x + | Float x -> string_of_float x + | Int32 x -> Int32.to_string x + | String s -> s let size_of_field = fun f -> (List.assoc f._type types).size let default_format = fun x -> (List.assoc x types).format @@ -85,47 +103,47 @@ let field_of_xml = fun xml -> (** Table of msg classes indexed by name. Each class is a table of messages -indexed by ids *) + indexed by ids *) let lazy_classes = lazy - (let h = Hashtbl.create 13 in - List.iter - (fun xml_class -> - let by_id = Hashtbl.create 13 - and by_name = Hashtbl.create 13 in - List.iter - (fun xml_msg -> - try - let name = ExtXml.attrib xml_msg "name" in - let msg = { - name = name; - fields = List.map field_of_xml (Xml.children xml_msg) - } in - let id = int_of_string (ExtXml.attrib xml_msg "id") (* - 1 !!!!*) in - Hashtbl.add by_id id msg; - Hashtbl.add by_name name (id, msg) - with _ -> - fprintf stderr "Warning: Ignoring '%s'\n" (Xml.to_string xml_msg)) - (Xml.children xml_class); - Hashtbl.add h (ExtXml.attrib xml_class "name") (by_id, by_name) - ) - (Xml.children (messages_xml ())); - h) + (let h = Hashtbl.create 13 in + List.iter + (fun xml_class -> + let by_id = Hashtbl.create 13 + and by_name = Hashtbl.create 13 in + List.iter + (fun xml_msg -> + try + let name = ExtXml.attrib xml_msg "name" in + let msg = { + name = name; + fields = List.map field_of_xml (Xml.children xml_msg) + } in + let id = int_of_string (ExtXml.attrib xml_msg "id") (* - 1 !!!!*) in + Hashtbl.add by_id id msg; + Hashtbl.add by_name name (id, msg) + with _ -> + fprintf stderr "Warning: Ignoring '%s'\n" (Xml.to_string xml_msg)) + (Xml.children xml_class); + Hashtbl.add h (ExtXml.attrib xml_class "name") (by_id, by_name) + ) + (Xml.children (messages_xml ())); + h) let classes = fun () -> Lazy.force lazy_classes - + let magic = fun x -> (Obj.magic x:('a,'b,'c) Pervasives.format) - let format_field = fun buffer index (field:field) -> - let format = field.fformat in - match field._type with - "uint8" -> sprintf (magic format) (Char.code buffer.[index]) - | "int8" -> sprintf (magic format) (if Char.code buffer.[index] <= 128 then Char.code buffer.[index] else Char.code buffer.[index] - 256) - | "uint16" -> sprintf (magic format) (Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1]) - | "int16" -> sprintf (magic format) (if Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1] <= 32768 then Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1] else Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1] - 65536) - | "float" -> sprintf (magic format) (float_of_bytes buffer index) - | "int32" | "uint32" -> sprintf (magic format) (int32_of_bytes buffer index) - | _ -> failwith "format_field" +let value_field = fun buffer index (field:field) -> + let format = field.fformat in + match field._type with + "uint8" -> Int (Char.code buffer.[index]) + | "int8" -> Int (if Char.code buffer.[index] <= 128 then Char.code buffer.[index] else Char.code buffer.[index] - 256) + | "uint16" -> Int (Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1]) + | "int16" -> Int (if Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1] <= 32768 then Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1] else Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1] - 65536) + | "float" -> Float (float_of_bytes buffer index) + | "int32" | "uint32" -> Int32 (int32_of_bytes buffer index) + | _ -> failwith "value_field" module type CLASS = sig val name : string end @@ -171,29 +189,28 @@ module Protocol(Class:CLASS) = struct [] -> [] | (field_name, field_descr)::fs -> let n = size_of_field field_descr in - (field_name, format_field buffer index field_descr) :: loop (index+n) fs in + (field_name, value_field buffer index field_descr) :: loop (index+n) fs in (id, loop 2 message.fields) let space = Str.regexp "[ \t]+" let values_of_string = fun s -> - match Str.split space s with - msg_name::args -> - begin - try - let msg_id, msg = message_of_name msg_name in - let values = List.map2 (fun (field_name, _) v -> (field_name, v)) msg.fields args in - (msg_id, values) - with - Not_found -> raise (Unknown_msg_name msg_name) - end - | [] -> invalid_arg "Pprz.values_of_string" + match Str.split space s with + msg_name::args -> + begin + try + let msg_id, msg = message_of_name msg_name in + let values = List.map2 (fun (field_name, field) v -> (field_name, value field._type v)) msg.fields args in + (msg_id, values) + with + Not_found -> raise (Unknown_msg_name msg_name) + end + | [] -> invalid_arg "Pprz.values_of_string" let string_of_message = fun msg values -> String.concat " " (msg.name:: List.map (fun (field_name, field) -> - try List.assoc field_name values with Not_found -> default_value field._type) + try string_of_value (List.assoc field_name values) with Not_found -> default_value field._type) msg.fields) end - diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index d2439a7f90..77fd7bb20b 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -28,13 +28,19 @@ type class_name = string type message_id = int type format = string type _type = string -type value = string +type value = Int of int | Float of float | String of string | Int32 of int32 type field = { _type : _type; fformat : format; } type message = { name : string; fields : (string * field) list; } -type type_descr = { format : format; glib_type : string; size:int; value:string} -val types : (string * type_descr) list val size_of_field : field -> int val default_format : string -> string +val string_of_value : value -> string +type type_descr = { + format : string ; + glib_type : string; + size : int; + value : string + } +val types : (string * type_descr) list exception Unknown_msg_name of string @@ -43,14 +49,14 @@ module Protocol : functor (Class : CLASS) -> sig include Serial.PROTOCOL val message_of_id : message_id -> message val message_of_name : string -> message_id * message - val values_of_bin : string -> message_id * (string * string) list + val values_of_bin : string -> message_id * (string * value) list (** [values raw_message] Parses a raw message, returns the message id and the liste of (field_name, value) *) - val values_of_string : string -> message_id * (string * string) list + val values_of_string : string -> message_id * (string * value) list (** May raise [(Unknown_msg_name msg_name)] *) - val string_of_message : message -> (string * string) list -> string + val string_of_message : message -> (string * value) list -> string (** [string_of_message msg values] *) end