From 7850f574be689740e1bfecbf58023164371e78b5 Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Wed, 19 Apr 2006 10:22:15 +0000 Subject: [PATCH] support for values array --- sw/airborne/downlink.h | 2 +- sw/ground_segment/cockpit/map2d.ml | 2 +- sw/ground_segment/tmtc/airprox.ml | 2 +- sw/ground_segment/tmtc/hw_modem_listen.ml | 14 +- sw/ground_segment/tmtc/maxstream.ml | 16 +- sw/ground_segment/tmtc/messages.ml | 2 +- sw/ground_segment/tmtc/modem.ml | 10 +- sw/ground_segment/tmtc/modem.mli | 4 +- sw/ground_segment/tmtc/server.ml | 6 +- sw/ground_segment/tmtc/wavecard_connect.ml | 8 +- sw/lib/ocaml/pprz.ml | 237 +++++++++++++-------- sw/lib/ocaml/pprz.mli | 28 +-- sw/lib/ocaml/serial.ml | 7 + sw/lib/ocaml/serial.mli | 13 +- sw/logalizer/play.ml | 2 +- sw/simulator/gaia.ml | 2 +- sw/simulator/sim.ml | 2 +- sw/simulator/sitl.ml | 2 +- 18 files changed, 227 insertions(+), 132 deletions(-) diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h index 72667cb0cf..c1ca22572d 100644 --- a/sw/airborne/downlink.h +++ b/sw/airborne/downlink.h @@ -39,7 +39,7 @@ extern uint8_t downlink_nb_ovrn; #define _DownlinkPut1Byte(dev, _x) __DownlinkPut1Byte(dev, _x) #define DownlinkPut1Byte(_x) _DownlinkPut1Byte(DOWNLINK_DEVICE, _x) -/** 5 = STX + len + ac_id + msg_id + ck_a + ck_b */ +/** 6 = STX + len + ac_id + msg_id + ck_a + ck_b */ #define DownlinkSizeOf(_payload) (_payload+6) #define DownlinkPut1ByteUpdateCs(_byte) { \ diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml index afc3a13c23..6955b1f52e 100644 --- a/sw/ground_segment/cockpit/map2d.ml +++ b/sw/ground_segment/cockpit/map2d.ml @@ -537,7 +537,7 @@ let button_press = fun (geomap:G.widget) ev -> (******* Real time handling of flying A/Cs ***********************************) module Live = struct - module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) + module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) type color = string type aircraft = { diff --git a/sw/ground_segment/tmtc/airprox.ml b/sw/ground_segment/tmtc/airprox.ml index b87368e65f..43f5b059dc 100644 --- a/sw/ground_segment/tmtc/airprox.ml +++ b/sw/ground_segment/tmtc/airprox.ml @@ -27,7 +27,7 @@ open Aircraft open Latlong -module Alerts_Pprz = Pprz.Protocol(struct let name = "alert" end) +module Alerts_Pprz = Pprz.Messages(struct let name = "alert" end) (** computes distance between 2d points *) let distance = fun (x1, y1) (x2, y2) -> diff --git a/sw/ground_segment/tmtc/hw_modem_listen.ml b/sw/ground_segment/tmtc/hw_modem_listen.ml index fe0e579464..2003cfadb6 100644 --- a/sw/ground_segment/tmtc/hw_modem_listen.ml +++ b/sw/ground_segment/tmtc/hw_modem_listen.ml @@ -33,8 +33,8 @@ let modem_msg_period = 1000 (** ms *) module ModemTransport = Serial.Transport(Modem.Protocol) module Tele_Class = struct let name = "telemetry_ap" end -module Tele_Pprz = Pprz.Protocol(Tele_Class) -module PprzTransport = Serial.Transport(Tele_Pprz) +module Tele_Pprz = Pprz.Messages(Tele_Class) +module PprzTransport = Serial.Transport(Pprz.Transport) (** Monitoring of the message reception *) type status = { @@ -60,14 +60,16 @@ let listen_pprz_modem = fun pprz_message_cb tty -> in (** Callback for a checksumed pprz message *) - let use_pprz_buf = fun buf -> + let use_pprz_payload = fun payload -> + let buf = Serial.string_of_payload payload in status.rx_byte <- status.rx_byte + String.length buf; Debug.call 'P' (fun f -> fprintf f "use_pprz: %s\n" (Debug.xprint buf)); - pprz_message_cb (Tele_Pprz.values_of_bin buf) in + pprz_message_cb (Tele_Pprz.values_of_payload payload) in (** Callback for a modem message *) let use_modem_message = let buffer = ref "" in - fun msg -> + fun payload -> + let msg = Serial.string_of_payload payload in Debug.call 'M' (fun f -> fprintf f "use_modem: %s\n" (Debug.xprint msg)); match Modem.parse msg with None -> () (* Only internal modem data *) @@ -76,7 +78,7 @@ let listen_pprz_modem = fun pprz_message_cb tty -> let b = !buffer ^ data in Debug.call 'M' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b)); (** Parse as pprz message and ... *) - let x = PprzTransport.parse use_pprz_buf b in + let x = PprzTransport.parse use_pprz_payload b in status.rx_err <- !PprzTransport.nb_err; (** ... remove from the buffer the chars which have been used *) buffer := String.sub b x (String.length b - x) diff --git a/sw/ground_segment/tmtc/maxstream.ml b/sw/ground_segment/tmtc/maxstream.ml index 0155ef1795..19ed477b3d 100644 --- a/sw/ground_segment/tmtc/maxstream.ml +++ b/sw/ground_segment/tmtc/maxstream.ml @@ -25,10 +25,10 @@ open Latlong open Printf module W = Wavecard -module Tm_Pprz = Pprz.Protocol(struct let name = "telemetry" end) -module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) -module Dl_Pprz = Pprz.Protocol(struct let name = "datalink" end) -module PprzTransport = Serial.Transport(Tm_Pprz) +module Tm_Pprz = Pprz.Messages(struct let name = "telemetry" end) +module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) +module Dl_Pprz = Pprz.Messages(struct let name = "datalink" end) +module PprzTransport = Serial.Transport(Pprz.Transport) let ground_id = 0 @@ -68,9 +68,9 @@ let maxstream_send = fun fd data -> Debug.call 'm' (fun f -> fprintf f "mm sending: "; for i = 0 to String.length buf - 1 do fprintf f "%x " (Char.code buf.[i]) done; fprintf f "\n"); flush o -let use_tele_message = fun buf -> - Debug.call 'm' (fun f -> fprintf f "mm receiving: "; for i = 0 to String.length buf - 1 do fprintf f "%x " (Char.code buf.[i]) done; fprintf f "\n"); - let (msg_id, ac_id, values) = Tm_Pprz.values_of_bin buf in +let use_tele_message = fun payload -> + Debug.call 'm' (fun f -> let buf = Serial.string_of_payload payload in fprintf f "mm receiving: "; for i = 0 to String.length buf - 1 do fprintf f "%x " (Char.code buf.[i]) done; fprintf f "\n"); + let (msg_id, ac_id, values) = Tm_Pprz.values_of_payload payload in let msg = Tm_Pprz.message_of_id msg_id in Tm_Pprz.message_send (string_of_int ac_id) msg.Pprz.name values @@ -88,7 +88,7 @@ let maxstream_receive = Serial.input (fun b -> maxstream_parse b) let send = fun ac s -> (* Wavecard.send_addressed ac.fd (W.REQ_SEND_MESSAGE,ac.addr,s) *) - maxstream_send ac.fd s + maxstream_send ac.fd (Serial.string_of_payload s) let send_dl_msg = fun ac a -> let (id, values) = Dl_Pprz.values_of_string a in diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index debcce25d8..e83daddf31 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -127,7 +127,7 @@ let _ = let rec one_class = fun (ident, xml_class, sender) -> let name = (Xml.attrib xml_class "name") in let messages = Xml.children xml_class in - let module P = Pprz.Protocol (struct let name = name end) in + let module P = Pprz.Messages (struct let name = name end) in let senders = Hashtbl.create 5 in match sender with | Some "*" -> diff --git a/sw/ground_segment/tmtc/modem.ml b/sw/ground_segment/tmtc/modem.ml index d9649f0bc2..74d6ad6281 100644 --- a/sw/ground_segment/tmtc/modem.ml +++ b/sw/ground_segment/tmtc/modem.ml @@ -29,7 +29,7 @@ open Printf module Protocol = struct (* Header: STX, length of (payload + checksum) *) (* Payload: tag, data *) -(* Tailer : checksum, ETX *) +(* Trailer : checksum, ETX *) let stx = Char.chr 0x02 let etx = 0x03 @@ -53,6 +53,14 @@ module Protocol = struct ck_a := Char.code msg.[i] lxor !ck_a done; !ck_a = Char.code msg.[l-2] && Char.code msg.[l-1] = etx + + let payload = fun msg -> + let l = String.length msg in + assert(l >= 4); + Serial.payload_of_string (String.sub msg 2 (l-4)) + + let packet = fun payload -> + failwith "Modem.Protocol.packet not implemented" end let msg_data = 0 diff --git a/sw/ground_segment/tmtc/modem.mli b/sw/ground_segment/tmtc/modem.mli index f2ada90d0f..33938aed65 100644 --- a/sw/ground_segment/tmtc/modem.mli +++ b/sw/ground_segment/tmtc/modem.mli @@ -27,12 +27,10 @@ module Protocol : sig + include Serial.PROTOCOL val stx : char val etx : int - val index_start : string -> int val payload_length : string -> int -> int - val length : string -> int -> int - val checksum : string -> bool end val parse : string -> string option diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index bbd79947bf..f5478d90bb 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -35,9 +35,9 @@ module U = Unix module Tele_Class = struct let name = "telemetry_ap" end module Ground = struct let name = "ground" end -module Tele_Pprz = Pprz.Protocol(Tele_Class) -module Ground_Pprz = Pprz.Protocol(Ground) -module Alerts_Pprz = Pprz.Protocol(struct let name = "alert" end) +module Tele_Pprz = Pprz.Messages(Tele_Class) +module Ground_Pprz = Pprz.Messages(Ground) +module Alerts_Pprz = Pprz.Messages(struct let name = "alert" end) let (//) = Filename.concat diff --git a/sw/ground_segment/tmtc/wavecard_connect.ml b/sw/ground_segment/tmtc/wavecard_connect.ml index eee8a5b580..5c0bc02674 100644 --- a/sw/ground_segment/tmtc/wavecard_connect.ml +++ b/sw/ground_segment/tmtc/wavecard_connect.ml @@ -27,9 +27,9 @@ open Latlong open Printf module W = Wavecard -module Tm_Pprz = Pprz.Protocol(struct let name = "telemetry_ap" end) -module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) -module Dl_Pprz = Pprz.Protocol(struct let name = "datalink" end) +module Tm_Pprz = Pprz.Messages(struct let name = "telemetry_ap" end) +module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) +module Dl_Pprz = Pprz.Messages(struct let name = "datalink" end) let ground_id = 0 @@ -39,7 +39,7 @@ let send_ack = fun delay fd () -> ignore (GMain.Timeout.add delay (fun _ -> W.send fd (W.ACK, ""); false)) let send = fun ac s -> - Wavecard.send_addressed ac.fd (W.REQ_SEND_MESSAGE,ac.addr,s) + Wavecard.send_addressed ac.fd (W.REQ_SEND_MESSAGE,ac.addr, Serial.string_of_payload s) let send_dl_msg = fun ac a -> let (id, values) = Dl_Pprz.values_of_string a in diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index a2a81d676a..bc9503f9b6 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -31,8 +31,12 @@ type message_id = int type ac_id = int type class_name = string type format = string -type _type = string -type value = Int of int | Float of float | String of string | Int32 of int32 +type _type = + Scalar of string + | ArrayType of string +type value = + Int of int | Float of float | String of string | Int32 of int32 + | Array of value array type field = { _type : _type; fformat : format; @@ -52,8 +56,12 @@ type type_descr = { type values = (string * value) list +type payload = string +let separator = "," +let regexp_separator = Str.regexp "," +let split_array = fun s -> Str.split regexp_separator s let (//) = Filename.concat let lazy_messages_xml = lazy (Xml.parse_file (Env.paparazzi_src // "conf" // "messages.xml")) @@ -77,23 +85,35 @@ let types = [ ("string", { format = "%s" ; glib_type = "gchar*"; size = max_int; value=String "42" }) ] +let is_array_type = fun s -> + let n = String.length s in + n >= 2 && String.sub s (n-2) 2 = "[]" + +let type_of_array_type = fun s -> + let n = String.length s in + String.sub s 0 (n-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 -> +let rec 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) + Scalar ("uint8" | "uint16" | "int8" | "int16") -> Int (int_of_string v) + | Scalar ("uint32" | "int32") -> Int32 (Int32.of_string v) + | Scalar "float" -> Float (float_of_string v) + | Scalar "string" -> String v + | ArrayType t' -> + Array (Array.map (value (Scalar t')) (Array.of_list (split_array v))) + | Scalar t -> failwith (sprintf "Pprz.value: Unexpected type: %s" t) -let string_of_value = function + +let rec 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 + | Array a -> String.concat separator (Array.to_list (Array.map string_of_value a)) let magic = fun x -> (Obj.magic x:('a,'b,'c) Pervasives.format) @@ -104,22 +124,28 @@ let formatted_string_of_value = fun format v -> Float x -> sprintf (magic format) x | v -> string_of_value v -let size_of_field = fun f -> (List.assoc f._type types).size -let default_format = fun x -> try (List.assoc x types).format with Not_found -> failwith (sprintf "Unknown format '%s'" x) -let default_value = fun x -> (List.assoc x types).value + +let sizeof = fun f -> + match f with + Scalar t -> (List.assoc t types).size + | ArrayType t -> failwith "sizeof: Array" +let size_of_field = fun f -> sizeof f._type +let default_format = fun x -> try (List.assoc x types).format with Not_found -> failwith (sprintf "Unknwon format '%s'" x) +let default_value = fun x -> + match x with + Scalar t -> (List.assoc t types).value + | ArrayType t -> failwith "default_value: Array" let payload_size_of_message = fun message -> List.fold_right (fun (_, f) s -> size_of_field f + s) message.fields - 3 (** message_len + message id + aircraft id *) - -let size_of_message = fun m -> - payload_size_of_message m + 3 (* STX, CK_A, CK_B *) + 2 (** + message id + aircraft id *) let field_of_xml = fun xml -> let t = ExtXml.attrib xml "type" in let f = try Xml.attrib xml "format" with _ -> default_format t in + let t = if is_array_type t then ArrayType (type_of_array_type t) else Scalar t in (ExtXml.attrib xml "name", { _type = t; fformat = f }) let string_of_values = fun vs -> @@ -177,34 +203,59 @@ let lazy_classes = let classes = fun () -> Lazy.force lazy_classes -let value_field = fun buffer index (field:field) -> - match field._type with - "uint8" -> Int (Char.code buffer.[index]) - | "int8" -> Int (int8_of_bytes buffer index) - | "uint16" -> Int (Char.code buffer.[index+1] lsl 8 + Char.code buffer.[index]) - | "int16" -> Int (int16_of_bytes buffer index) - | "float" -> Float (float_of_bytes buffer index) - | "int32" | "uint32" -> Int32 (int32_of_bytes buffer index) - | _ -> failwith "value_field" +(** Returns a value and its length *) +let rec value_of_bin = fun buffer index _type -> + match _type with + Scalar "uint8" -> Int (Char.code buffer.[index]), sizeof _type + | Scalar "int8" -> Int (int8_of_bytes buffer index), sizeof _type + | Scalar "uint16" -> Int (Char.code buffer.[index+1] lsl 8 + Char.code buffer.[index]), sizeof _type + | Scalar "int16" -> Int (int16_of_bytes buffer index), sizeof _type + | Scalar "float" -> Float (float_of_bytes buffer index), sizeof _type + | Scalar ("int32" | "uint32") -> Int32 (int32_of_bytes buffer index), sizeof _type + | ArrayType t -> + (** First get the number of values *) + let n = int8_of_bytes buffer index in + let type_of_elt = Scalar t in + let s = sizeof type_of_elt in + let size = 1 + n * s in + (Array (Array.init n + (fun i -> fst (value_of_bin buffer (index+1+i*s) type_of_elt))), size) + | _ -> failwith "value_of_bin" + +let value_field = fun buf index field -> + value_of_bin buf index field._type let byte = fun x -> Char.chr (x land 0xff) -let sprint_value = fun buf i field_type v -> - match field_type, v with - ("int8"|"uint8"), Int x -> buf.[i] <- Char.chr x - | "float", Float f -> sprint_float buf i f - | "int32", Int32 x -> sprint_int32 buf i x - | ("int32" | "uint32"), Int value -> - assert (field_type <> "uint32" || value >= 0); +(** Returns the size of outputed data *) +let rec sprint_value = fun buf i _type v -> + match _type, v with + Scalar ("int8"|"uint8"), Int x -> buf.[i] <- Char.chr x; sizeof _type + | Scalar "float", Float f -> sprint_float buf i f; sizeof _type + | Scalar "int32", Int32 x -> sprint_int32 buf i x; sizeof _type + | Scalar ("int32" | "uint32"), Int value -> + assert (_type <> Scalar "uint32" || value >= 0); buf.[i+3] <- byte (value asr 24); buf.[i+2] <- byte (value lsr 16); buf.[i+1] <- byte (value lsr 8); - buf.[i+0] <- byte value - | ("int16"|"uint16"), Int value -> - assert (field_type <> "uint16" || value >= 0); + buf.[i+0] <- byte value; + sizeof _type + | Scalar ("int16"|"uint16"), Int value -> + assert (_type <> Scalar "uint16" || value >= 0); buf.[i+1] <- byte (value lsr 8); - buf.[i+0] <- byte value - | x, _ -> failwith (sprintf "Pprz.sprint_value (%s)" x) + buf.[i+0] <- byte value; + sizeof _type + | ArrayType t, Array values -> + (** Put the size first, then the values *) + let n = Array.length values in + sprint_value buf i (Scalar "uint8") (Int n); + let type_of_elt = Scalar t in + let s = sizeof type_of_elt in + for j = 0 to n - 1 do + sprint_value buf (i+1+j*s) type_of_elt values.(j) + done; + 1 + n * s + | (Scalar x|ArrayType x), _ -> failwith (sprintf "Pprz.sprint_value (%s)" x) @@ -212,31 +263,18 @@ module type CLASS = sig val name : string end exception Unknown_msg_name of string * string -module Protocol(Class:CLASS) = struct - let stx = Char.chr 0x99 (** sw/airborne/modem.h *) +module Transport = struct + let stx = Char.chr 0x99 (** sw/airborne/downlink.h *) + let offset_length = 1 + let offset_payload = 2 + let index_start = fun buf -> String.index buf stx - let messages_by_id, messages_by_name = - try - Hashtbl.find (classes ()) Class.name - with - Not_found -> failwith (sprintf "Unknown message class: %s" Class.name) - let message_of_id = fun id -> Hashtbl.find messages_by_id id - let message_of_name = fun name -> - try - Hashtbl.find messages_by_name name - with - Not_found -> raise (Unknown_msg_name (name, Class.name)) - let length = fun buf start -> let len = String.length buf - start in - if len >= 2 then - (** let id = Char.code buf.[start+1] in - let msg = message_of_id id in - let l = size_of_message msg in **) - let l = Char.code buf.[start+1] in -(** Debug.call 'T' (fun f -> fprintf f "Pprz id=%d len=%d\n" id l); **) + if len > offset_length then + let l = Char.code buf.[start+offset_length] in Debug.call 'T' (fun f -> fprintf f "Pprz len=%d\n" l); l else @@ -258,51 +296,84 @@ module Protocol(Class:CLASS) = struct Debug.call 'T' (fun f -> fprintf f "Pprz cs: %d %d\n" ck_a (Char.code msg.[l-2])); ck_a = Char.code msg.[l-2] && ck_b = Char.code msg.[l-1] + let payload = fun msg -> + let l = String.length msg in + assert(Char.code msg.[offset_length] = l); + assert(l >= 4); + Serial.payload_of_string (String.sub msg 2 (l-4)) + + let packet = fun payload -> + let payload = Serial.string_of_payload payload in + let n = String.length payload in + let msg_length = n + 4 in (** + stx, len, ck_a and ck_b *) + let m = String.create msg_length in + String.blit payload 0 m offset_payload n; + m.[0] <- stx; + m.[offset_length] <- Char.chr msg_length; + let (ck_a, ck_b) = compute_checksum m in + m.[msg_length-2] <- Char.chr ck_a; + m.[msg_length-1] <- Char.chr ck_b; + m +end + + + +module Messages(Class:CLASS) = struct + let offset_ac_id = 0 + let offset_msg_id = 1 + let offset_fields = 2 + let max_length = 256 + let messages_by_id, messages_by_name = + try + Hashtbl.find (classes ()) Class.name + with + Not_found -> failwith (sprintf "Unknown message class: %s" Class.name) + let message_of_id = fun id -> Hashtbl.find messages_by_id id + let message_of_name = fun name -> + try + Hashtbl.find messages_by_name name + with + Not_found -> raise (Unknown_msg_name (name, Class.name)) + + let values_of_payload = fun buffer -> - let id = Char.code buffer.[2] in - let ac_id = Char.code buffer.[1] in + let buffer = Serial.string_of_payload buffer in + let id = Char.code buffer.[offset_msg_id] in + let ac_id = Char.code buffer.[offset_ac_id] in let message = message_of_id id in Debug.call 'T' (fun f -> fprintf f "Pprz.values id=%d\n" id); let rec loop = fun index fields -> match fields with [] -> [] | (field_name, field_descr)::fs -> - let n = size_of_field field_descr in - (field_name, value_field buffer index field_descr) :: loop (index+n) fs in - (id, ac_id, loop 3 message.fields) + let (value, n) = value_field buffer index field_descr in + (field_name, value) :: loop (index+n) fs in + (id, ac_id, loop offset_fields message.fields) - let values_of_bin = fun buffer -> - values_of_payload (String.sub buffer 1 (String.length buffer - 1)) -(** FIXME - this one is wrong since the message lenght introduction **) let payload_of_values = fun id ac_id values -> let message = message_of_id id in - let n = payload_size_of_message message in - let p = String.make n '#' in - p.[0] <- Char.chr id; - p.[1] <- Char.chr ac_id; - let i = ref 2 in + + (** The actual length is computed from the values *) + let p = String.make max_length '#' in + + p.[offset_msg_id] <- Char.chr id; + p.[offset_ac_id] <- Char.chr ac_id; + let i = ref offset_fields in List.iter (fun (field_name, field) -> let v = try List.assoc field_name values with Not_found -> default_value field._type in - sprint_value p !i field._type v; - i := !i + size_of_field field + let size = sprint_value p !i field._type v in + i := !i + size ) message.fields; - p - - let message_of_payload = fun p -> - let n = String.length p in - let m = String.create (n+3) in (** + stx, ck_a and ck_b *) - String.blit p 0 m 1 n; - m.[0] <- stx; - let (ck_a, ck_b) = compute_checksum m in - m.[n+3-2] <- Char.chr ck_a; - m.[n+3-1] <- Char.chr ck_b; - m + (** Cut to the actual length *) + let p = String.sub p 0 !i in + Serial.payload_of_string p + let space = Str.regexp "[ \t]+" let values_of_string = fun s -> diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index 6e0300b8bd..90bcde2f05 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -30,12 +30,19 @@ type class_name = string type message_id = int type ac_id = int type format = string -type _type = string -type value = Int of int | Float of float | String of string | Int32 of int32 +type _type = + Scalar of string + | ArrayType of string +type value = + Int of int | Float of float | String of string | Int32 of int32 + | Array of value array type field = { _type : _type; fformat : format; } type message = { name : string; fields : (string * field) list; } (** Message specification *) +val separator : string +(** Separator in array values *) + val size_of_field : field -> int val default_format : string -> string val string_of_value : value -> string @@ -60,26 +67,19 @@ exception Unknown_msg_name of string * string (** [Unknown_msg_name (name, class_name)] Raised if message [name] is not found in class [class_name]. *) +module Transport : Serial.PROTOCOL + module type CLASS = sig val name : string end -module Protocol : functor (Class : CLASS) -> sig - include Serial.PROTOCOL +module Messages : functor (Class : CLASS) -> sig val message_of_id : message_id -> message val message_of_name : string -> message_id * message - val values_of_payload : string -> message_id * ac_id * values + val values_of_payload : Serial.payload -> message_id * ac_id * values (** [values_of_bin payload] Parses a raw payload, returns the message id, the A/C id and the list of (field_name, value) *) - val values_of_bin : string -> message_id * ac_id * values - (** [values_of_bin raw_message] Same than previous but [raw_message] - includes header and checksum. *) - val payload_of_values : message_id -> ac_id -> values -> string + val payload_of_values : message_id -> ac_id -> values -> Serial.payload (** [payload_of_values id ac_id vs] Returns a payload *) - val message_of_payload : string -> string - (** [message_of_payload s] Returns a full message with header - and checksum *) - - val values_of_string : string -> message_id * values (** May raise [(Unknown_msg_name msg_name)] *) diff --git a/sw/lib/ocaml/serial.ml b/sw/lib/ocaml/serial.ml index ab1953c419..048f816f20 100644 --- a/sw/lib/ocaml/serial.ml +++ b/sw/lib/ocaml/serial.ml @@ -47,6 +47,11 @@ type speed = | B115200 | B230400 +type payload = string + +let string_of_payload = fun x -> x +let payload_of_string = fun x -> x + external init_serial : string -> speed -> Unix.file_descr = "c_init_serial";; @@ -89,6 +94,8 @@ module type PROTOCOL = sig val index_start : string -> int (* raise Not_found *) val length : string -> int -> int (* raise Not_enough *) val checksum : string -> bool + val payload : string -> payload + val packet : payload -> string end module Transport(Protocol:PROTOCOL) = struct diff --git a/sw/lib/ocaml/serial.mli b/sw/lib/ocaml/serial.mli index 6dba9f4216..dfc9531eaa 100644 --- a/sw/lib/ocaml/serial.mli +++ b/sw/lib/ocaml/serial.mli @@ -53,6 +53,11 @@ val input : (string -> int) -> Unix.file_descr -> unit time a new character arrives. [f] must return the number of consumed characters *) +type payload + +val string_of_payload : payload -> string +val payload_of_string : string -> payload + exception Not_enough module type PROTOCOL = sig @@ -63,8 +68,12 @@ module type PROTOCOL = val length : string -> int -> int (** [length buf start] Must return the length of the message starting at [start]. May raise Not_enough *) + val checksum : string -> bool (** [checksum message] *) + + val payload : string -> payload + val packet : payload -> string end module Transport : @@ -72,7 +81,7 @@ module Transport : sig val nb_err : int ref (* Errors on checksum *) val discarded_bytes : int ref - val parse : (string -> unit) -> string -> int + val parse : (payload -> unit) -> string -> int (** [parse f buf] Scans [buf] according to [Protocol] and applies [f] on - every recognised message. Returns the number of consumed bytes. *) + payload of every recognised message. Returns the number of consumed bytes. *) end diff --git a/sw/logalizer/play.ml b/sw/logalizer/play.ml index 46ff9294b0..9ce3ca21a5 100644 --- a/sw/logalizer/play.ml +++ b/sw/logalizer/play.ml @@ -24,7 +24,7 @@ * *) -module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) +module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) let log = ref [||] diff --git a/sw/simulator/gaia.ml b/sw/simulator/gaia.ml index 944400ee8d..b9793429c6 100644 --- a/sw/simulator/gaia.ml +++ b/sw/simulator/gaia.ml @@ -30,7 +30,7 @@ open Latlong let my_id = "gaia" let sending_period = 5000 (* ms *) -module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) +module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) let ivy_bus = ref "127.255.255.255:2010" diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml index bfe27638b4..17c8bb2301 100644 --- a/sw/simulator/sim.ml +++ b/sw/simulator/sim.ml @@ -27,7 +27,7 @@ open Printf open Stdlib -module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) +module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) let float_attrib xml a = float_of_string (ExtXml.attrib xml a) diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index e4694c60b2..78454bf228 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -26,7 +26,7 @@ open Printf -module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) +module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) let ios = int_of_string let fos = float_of_string