Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

More typing in message values

Browse Source
This commit is contained in:
Pascal Brisset
2005-03-11 17:58:17 +00:00
parent c581e6ad7b
commit 7e26f199af
5 changed files with 118 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/tmtc/listen.ml
+2 -2
View File
@@ -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 _ _ -> ());
sw/ground_segment/tmtc/receive.ml
+35 -23
View File
@@ -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 ->
sw/lib/ocaml/pprz.ml
+68 -51
View File
@@ -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
sw/lib/ocaml/pprz.mli
+12 -6
View File
@@ -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