More typing in message values

conf/messages.xml

@@ -138,7 +138,7 @@
   </message>
 
    <message name="IDENT" ID="0x14" period="10">
-     <field name="id" type="uint8" len="16" format="%s"/>
+     <field name="id" type="string" len="16" format="%s"/>
    </message>
  </class>
 

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
+    let s = Tele_Pprz.string_of_message msg values in
-    Ivy.send (sprintf "1234.567 %s %s" msg.Pprz.name s) 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

@@ -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_block <- ivalue "cur_block";
-      a.cur_stage <- ios (value "cur_stage")
+      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_mode <- ivalue "ap_mode";
-      a.ap_altitude <- ios (value "ap_altitude");
+      a.ap_altitude <- ivalue "ap_altitude";
-      a.if_calib_mode <- ios (value "if_calib_mode");
+      a.if_calib_mode <- ivalue "if_calib_mode";
-      a.mcu1_status <- ios (value "mcu1_status");
+      a.mcu1_status <- ivalue "mcu1_status";
-      a.lls_calib <- ios (value "lls_calib")
+      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);
+    let f = fun x -> Pprz.Float x in
-		  "pitch", sof (Geometry_2d.rad2deg a.pitch);
+    let values = ["roll", f (Geometry_2d.rad2deg a.roll);
-		  "east", sof a.east;
+		  "pitch", f (Geometry_2d.rad2deg a.pitch);
-		  "north", sof a.north;
+		  "east", f a.east;
-		  "speed", sof a.gspeed;
+		  "north", f a.north;
-		  "heading", sof (Geometry_2d.rad2deg a.course);
+		  "speed", f a.gspeed;
-		  "alt", sof a.alt;
+		  "heading", f (Geometry_2d.rad2deg a.course);
-		  "climb", sof a.climb] in
+		  "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

@@ -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;
@@ -68,6 +68,24 @@ let types = [
   ("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
 let default_value = fun x -> (List.assoc x types).value
@@ -85,7 +103,7 @@ 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
@@ -116,16 +134,16 @@ 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 value_field = fun buffer index (field:field) ->
   let format = field.fformat in
   match field._type with
-      "uint8" -> sprintf (magic format) (Char.code buffer.[index])
+    "uint8" -> Int (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)
+  | "int8" -> Int (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])
+  | "uint16" -> Int (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)
+  | "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" ->  sprintf (magic format) (float_of_bytes buffer index)
+  | "float" ->  Float (float_of_bytes buffer index)
-    | "int32"  | "uint32" -> sprintf (magic format) (int32_of_bytes buffer index)
+  | "int32"  | "uint32" -> Int32 (int32_of_bytes buffer index)
-    | _ -> failwith "format_field"
+  | _ -> failwith "value_field"
 
 module type CLASS = sig val name : string end
 
@@ -171,7 +189,7 @@ 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]+"
@@ -181,7 +199,7 @@ module Protocol(Class:CLASS) = struct
 	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
+	    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)
@@ -193,7 +211,6 @@ module Protocol(Class:CLASS) = struct
       (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

@@ -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