diff --git a/conf/messages/custom_downlink.xml b/conf/messages/custom_downlink.xml index ccb979a3c02..1aaf0dc1fbd 100644 --- a/conf/messages/custom_downlink.xml +++ b/conf/messages/custom_downlink.xml @@ -24,8 +24,9 @@ + @@ -319,7 +320,7 @@ - + @@ -349,19 +350,14 @@ - - - - - - + - + @@ -373,7 +369,7 @@ - + @@ -385,25 +381,17 @@ - - - - - - - - - + - + - + @@ -486,7 +474,7 @@ - + @@ -513,14 +501,7 @@ - - - - - - - - + diff --git a/conf/messages/downlink.xml b/conf/messages/downlink.xml index 23d03c6304b..38eecd16e1c 100644 --- a/conf/messages/downlink.xml +++ b/conf/messages/downlink.xml @@ -111,13 +111,7 @@ - - - - - - - + @@ -270,18 +264,12 @@ - - - - - - - + - + @@ -359,17 +347,11 @@ - - - - - - - + - + @@ -567,7 +549,7 @@ - + @@ -888,7 +870,7 @@ - - + - - - - - - @@ -1125,11 +1101,7 @@ - - - - - + @@ -1145,25 +1117,4 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/messages/mavlink_downlink.xml b/conf/messages/mavlink_downlink.xml index d7c161d3343..55c56a8e66d 100644 --- a/conf/messages/mavlink_downlink.xml +++ b/conf/messages/mavlink_downlink.xml @@ -1,14 +1,14 @@ - + - + @@ -97,17 +97,17 @@ - + - + - + - + - + - @@ -143,20 +143,20 @@ - + - + - + - + - + - + - - + @@ -911,41 +911,41 @@ The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). - + - + - + - + - + - - + - + - + - - + - + - + - diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index d19464839ab..92bb7e0b84c 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -64,21 +64,21 @@ int32_t pos_z = (int32_t)(estimator_z*256);\ int32_t speed_z = (int32_t)(estimator_z_dot*526316);\ int32_t ref = 0;\ - DOWNLINK_SEND_POSITION_SPEED_ACCEL(_trans, _dev, &pos_x, &pos_y, &pos_z, &speed_x, &speed_y, &speed_z, &ref, &ref, &ref, &ref, &ref, &ref);\ + DOWNLINK_SEND_POSITION_SPEED_ACCEL(_trans, _dev, &pos_x, &pos_y, &pos_z, &speed_x, &speed_y, &speed_z, &ref, &ref, &ref, &ref, &ref, &ref);\ }) #define PERIODIC_SEND_ENERGY(_trans, _dev) Downlink({ \ - uint16_t vsup = vsupply; \ - int16_t amps = (int16_t) (current/10); \ - int16_t pwr = (int16_t) (vsup*amps); \ - int16_t e = energy; \ - DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &s, &pwr, &e, &v_ctl_throttle_slewed);\ + uint16_t vsup = vsupply; \ + int16_t amps = (int16_t) (current/10); \ + int16_t pwr = (int16_t) (vsup*amps); \ + int16_t e = energy; \ + DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &s, &pwr, &e, &v_ctl_throttle_slewed);\ }) #define SEND_MISSION_STATUS(_trans, _dev) Downlink({ \ - uint8_t _circle_count = NavCircleCount(); \ - uint32_t nb_sec = sys_time.nb_sec; \ - DOWNLINK_SEND_MISSION_STATUS(_trans, _dev, &estimator_flight_time, &nav_block, &block_time, &nav_stage, &stage_time, &nb_sec, &gps.fix, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count, &horizontal_mode);\ + uint8_t _circle_count = NavCircleCount(); \ + uint32_t nb_sec = sys_time.nb_sec; \ + DOWNLINK_SEND_MISSION_STATUS(_trans, _dev, &estimator_flight_time, &nav_block, &block_time, &nav_stage, &stage_time, &nb_sec, &gps.fix, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count, &horizontal_mode);\ }) #define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) Downlink({ \ diff --git a/sw/ground_segment/tmtc/ac_server.ml b/sw/ground_segment/tmtc/ac_server.ml index 11fc45843b1..c405cd8eb8d 100644 --- a/sw/ground_segment/tmtc/ac_server.ml +++ b/sw/ground_segment/tmtc/ac_server.ml @@ -125,9 +125,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> let fvalue = fun x -> let f = fvalue (value x) in match classify_float f with - FP_infinite | FP_nan -> - let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.Pprz.name (string_of_values values) in - raise (Telemetry_error (ac_name, format_string_field msg)) + FP_infinite | FP_nan -> + let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.Pprz.name (string_of_values values) in + raise (Telemetry_error (ac_name, format_string_field msg)) | _ -> f and ivalue = fun x -> ivalue (value x) @@ -135,7 +135,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> if not (msg.Pprz.name = "DOWNLINK_STATUS") then a.last_msg_date <- U.gettimeofday (); match msg.Pprz.name with - "ROTORCRAFT_FP" -> + "ROTORCRAFT_FP" -> begin match a.nav_ref with None -> (); (* No nav_ref yet *) | Some nav_ref -> @@ -181,7 +181,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.heading <- a.course; a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0); if a.gspeed > 3. && a.ap_mode = _AUTO2 then - Wind.update ac_name a.gspeed a.course + Wind.update ac_name a.gspeed a.course end | "GPS_LLA" -> let lat = ivalue "lat" @@ -197,7 +197,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0); a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE"; if a.gspeed > 3. && a.ap_mode = _AUTO2 then - Wind.update ac_name a.gspeed a.course + Wind.update ac_name a.gspeed a.course | "GPS_SOL" -> a.gps_Pacc <- ivalue "Pacc" | "GPS_INT" -> @@ -218,7 +218,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> end; a.desired_altitude <- (try fvalue "altitude" with _ -> fvalue "desired_altitude"); a.desired_climb <- (try fvalue "climb" with _ -> fvalue "desired_climb"); - begin try a.desired_course <- norm_course (fvalue "course") with _ -> () end + begin try a.desired_course <- norm_course (fvalue "course") with _ -> () end | "NAVIGATION_REF" -> a.nav_ref <- Some (Utm { utm_x = fvalue "utm_east"; utm_y = fvalue "utm_north"; utm_zone = ivalue "utm_zone" }) | "NAVIGATION_REF_LLA" -> @@ -240,7 +240,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> end | "ENERGY" -> a.throttle <- fvalue "throttle" /. 9600. *. 100.; - a.rpm <- a.throttle *. 100.; + a.rpm <- a.throttle *. 100.; (*a.kill_mode <- ivalue "kill_auto_throttle" <> 0;*) (*a.flight_time <- ivalue "flight_time";*) a.bat <- fvalue "voltage" /. 10.; @@ -269,7 +269,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.vehicle_type <- FixedWing; a.gaz_mode <- check_index (ivalue "ap_gaz") gaz_modes "AP_GAZ"; a.lateral_mode <- check_index (ivalue "ap_lateral") lat_modes "AP_LAT"; - a.kill_mode <- ivalue "kill_auto_throttle" <> 0; + a.kill_mode <- ivalue "kill_auto_throttle" <> 0; (*a.inflight_calib.if_mode <- check_index (ivalue "if_calib_mode") if_modes "IF_MODE"; let mcu1_status = ivalue "mcu1_status" in @@ -291,7 +291,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> then "AUTO" else "MANUAL"; end ; - *) + *) if a.fbw.rc_mode = "FAILSAFE" then a.ap_mode <- 5 (* Override and set FAIL(Safe) Mode *) @@ -305,13 +305,13 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.kill_mode <- ivalue "ap_motors_on" == 0; | "MISSION_STATUS" -> a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE"; - a.flight_time <- ivalue "flight_time"; - a.stage_time <- ivalue "stage_time"; + a.flight_time <- ivalue "flight_time"; + a.stage_time <- ivalue "stage_time"; a.block_time <- ivalue "block_time"; - a.cur_block <- ivalue "cur_block"; + a.cur_block <- ivalue "cur_block"; a.cur_stage <- ivalue "cur_stage"; a.horizontal_mode <- check_index (ivalue "horizontal_mode") horiz_modes "AP_HORIZ"; - a.dist_to_wp <- sqrt (fvalue "dist2_wp") + a.dist_to_wp <- sqrt (fvalue "dist2_wp") | "CAM" -> a.cam.phi <- (Deg>>Rad) (fvalue "phi"); a.cam.theta <- (Deg>>Rad) (fvalue "theta"); @@ -364,11 +364,11 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> | "DL_VALUE" -> let i = ivalue "index" in if i < max_nb_dl_setting_values then begin - a.dl_setting_values.(i) <- fvalue "value"; - a.nb_dl_setting_values <- max a.nb_dl_setting_values (i+1) + a.dl_setting_values.(i) <- fvalue "value"; + a.nb_dl_setting_values <- max a.nb_dl_setting_values (i+1) end else - failwith "Too much dl_setting values !!!" - | "INS_REF" -> + failwith "Too much dl_setting values !!!" + | "INS_REF" -> let x = foi32value "ecef_x0" /. 100. and y = foi32value "ecef_y0" /. 100. and z = foi32value "ecef_z0" /. 100. @@ -404,7 +404,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> and lon = ivalue "lon" and alt = ivalue "alt" in let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in - update_waypoint a (ivalue "wp_id") geo (float alt /. 100.) + update_waypoint a (ivalue "wp_id") geo (float alt /. 100.) | "GENERIC_COM" -> let flight_time = ivalue "flight_time" in if flight_time >= a.flight_time then begin diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index 59b366e9810..f77704e1883 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -162,20 +162,20 @@ let rec one_class = fun (notebook:GPack.notebook) (ident, xml_class, sender) -> | Some "*" -> (* Waiting for a new sender in this class *) let get_one = fun sender _vs -> - if not (Hashtbl.mem senders sender) then begin - Hashtbl.add senders sender (); - one_class notebook (ident, xml_class, Some sender) - end in + if not (Hashtbl.mem senders sender) then begin + Hashtbl.add senders sender (); + one_class notebook (ident, xml_class, Some sender) + end in List.iter - (fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one)) - messages + (fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one)) + messages | _ -> let class_notebook = GPack.notebook ~tab_border:0 ~tab_pos:`LEFT () in let l = match sender with None -> "" | Some s -> ":"^s in let label = GMisc.label ~text:(ident^l) () in ignore (notebook#append_page ~tab_label:label#coerce class_notebook#coerce); let bind, sender_name = match sender with - None -> (fun m cb -> (P.message_bind m cb)), "*" + None -> (fun m cb -> (P.message_bind m cb)), "*" | Some sender -> (fun m cb -> (P.message_bind ~sender m cb)), sender in (** Forall messages in the class *) @@ -188,14 +188,14 @@ let rec one_class = fun (notebook:GPack.notebook) (ident, xml_class, sender) -> (*********************** Main ************************************************) let _ = let ivy_bus = ref Defivybus.default_ivy_bus in - let class_names = List.map (fun _class -> if ("downlink" = ExtXml.attrib _class "type")||("datalink" = ExtXml.attrib _class "type") then ExtXml.attrib _class "name" else "" ) (Xml.children (Pprz.messages_xml ())) in - let classes = ref [] in - ignore (List.map (fun c_name -> match c_name with - | "" -> () - | name -> classes := List.append !classes [name^":*"] - ) class_names); - - Arg.parse + let class_names = List.map (fun _class -> if ("downlink" = ExtXml.attrib _class "type")||("datalink" = ExtXml.attrib _class "type") then ExtXml.attrib _class "name" else "" ) (Xml.children (Pprz.messages_xml ())) in + let classes = ref [] in + ignore (List.map (fun c_name -> match c_name with + | "" -> () + | name -> classes := List.append !classes [name^":*"] + ) class_names); + + Arg.parse [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf " Default is %s" !ivy_bus); "-c", Arg.String (fun x -> classes := x :: !classes), "class name"] (fun x -> prerr_endline ("WARNING: don't do anything with "^x)) @@ -218,12 +218,12 @@ let _ = let xml = Pprz.messages_xml () in let class_of = fun n -> try - List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml) + List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml) with Not_found -> failwith (sprintf "Unknown messages class: %s" n) in List.map (fun x -> match Str.split (Str.regexp ":") x with - [cl; s] -> (cl, class_of cl, Some s) + [cl; s] -> (cl, class_of cl, Some s) | [cl] -> (x, class_of cl, None) | _ -> failwith (sprintf "Wrong class '%s', class[:sender] expected" x)) !classes in diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 44b205cfee0..87168fbda9b 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -37,7 +37,7 @@ type format = string type _type = Scalar of string | ArrayType of string - | FixedArrayType of string * int + | FixedArrayType of string * int type value = Int of int | Float of float | String of string | Int32 of int32 | Char of char | Int64 of int64 | Array of value array @@ -116,28 +116,28 @@ let type_of_array_type = fun s -> String.sub s 0 (n-2) let is_fixed_array_type = fun s -> - let type_parts = Str.full_split (Str.regexp "[][]") s in - match type_parts with - | [Str.Text _; Str.Delim "["; Str.Text _ ; Str.Delim "]"] -> true - | _ -> false + let type_parts = Str.full_split (Str.regexp "[][]") s in + match type_parts with + | [Str.Text _; Str.Delim "["; Str.Text _ ; Str.Delim "]"] -> true + | _ -> false let type_of_fixed_array_type = fun s -> - try - let type_parts = Str.full_split (Str.regexp "[][]") s in - match type_parts with - | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> begin ignore( int_of_string (len)); ty end - | _ -> failwith("Pprz.type_of_fixed_array_type is not a fixed array type") - with - | Failure str -> failwith(sprintf "Pprz.type_of_fixed_array_type: length is not an integer") + try + let type_parts = Str.full_split (Str.regexp "[][]") s in + match type_parts with + | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> begin ignore( int_of_string (len)); ty end + | _ -> failwith("Pprz.type_of_fixed_array_type is not a fixed array type") + with + | Failure str -> failwith(sprintf "Pprz.type_of_fixed_array_type: length is not an integer") let length_of_fixed_array_type = fun s -> - try - let type_parts = Str.full_split (Str.regexp "[][]") s in - match type_parts with - | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> begin ignore( int_of_string (len)); len end - | _ -> failwith("Pprz.type_of_fixed_array_type is not a fixed array type") - with - | Failure str -> failwith(sprintf "Pprz.type_of_fixed_array_type: length is not an integer") + try + let type_parts = Str.full_split (Str.regexp "[][]") s in + match type_parts with + | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> begin ignore( int_of_string (len)); len end + | _ -> failwith("Pprz.type_of_fixed_array_type is not a fixed array type") + with + | Failure str -> failwith(sprintf "Pprz.type_of_fixed_array_type: length is not an integer") let int_of_string = fun x -> try int_of_string x with @@ -181,18 +181,18 @@ let sizeof = fun f -> match f with Scalar t -> (List.assoc t types).size | ArrayType t -> failwith "sizeof: Array" - | FixedArrayType (t,l) -> failwith "sizeof: Array" + | FixedArrayType (t,l) -> failwith "sizeof: Array" let size_of_field = fun f -> sizeof f._type let default_format = function Scalar x | ArrayType x | FixedArrayType (x,_) -> try (List.assoc x types).format with - Not_found -> failwith (sprintf "Unknown format '%s'" x) + Not_found -> failwith (sprintf "Unknown format '%s'" x) let default_value = fun x -> match x with Scalar t -> (List.assoc t types).value | ArrayType t -> failwith "default_value: Array" - | FixedArrayType (t,l) -> failwith "default_value: Array" + | FixedArrayType (t,l) -> failwith "default_value: Array" let payload_size_of_message = fun message -> List.fold_right @@ -270,19 +270,19 @@ let int_of_value = fun value -> | Int32 x -> let i = Int32.to_int x in if Int32.compare x (Int32.of_int i) <> 0 then - failwith "Pprz.int_assoc: Int32 too large to be converted into an int"; + failwith "Pprz.int_assoc: Int32 too large to be converted into an int"; i - | Int64 x -> + | Int64 x -> let i = Int64.to_int x in if Int64.compare x (Int64.of_int i) <> 0 then - failwith "Pprz.int_assoc: Int64 too large to be converted into an int"; + failwith "Pprz.int_assoc: Int64 too large to be converted into an int"; i | _ -> invalid_arg "Pprz.int_assoc" let int_assoc = fun (a:string) vs -> int_of_value (assoc a vs) -let int32_assoc = fun (a:string) vs -> +let int32_assoc = fun (a:string) vs -> match assoc a vs with Int32 x -> x | _ -> invalid_arg "Pprz.int_assoc" @@ -306,14 +306,14 @@ let link_mode_of_string = function let rec value_of_bin = fun buffer index _type -> match _type with Scalar "uint8" -> Int (Char.code buffer.[index]), sizeof _type - | Scalar "char" -> Char (buffer.[index]), sizeof _type + | Scalar "char" -> Char (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 "double" -> Float (double_of_bytes buffer index), sizeof _type | Scalar ("int32" | "uint32") -> Int32 (int32_of_bytes buffer index), sizeof _type - | Scalar ("int64" | "uint64") -> Int64 (int64_of_bytes buffer index), sizeof _type + | Scalar ("int64" | "uint64") -> Int64 (int64_of_bytes buffer index), sizeof _type | ArrayType t -> (** First get the number of values *) let n = int8_of_bytes buffer index in @@ -321,15 +321,15 @@ let rec value_of_bin = fun buffer index _type -> 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) - | FixedArrayType (t,l) -> + (fun i -> fst (value_of_bin buffer (index+1+i*s) type_of_elt))), size) + | FixedArrayType (t,l) -> (** First get the number of values *) - let n = l in + let n = l in let type_of_elt = Scalar t in let s = sizeof type_of_elt in let size = 0 + n * s in (Array (Array.init n - (fun i -> fst (value_of_bin buffer (index+0+i*s) type_of_elt))), size) + (fun i -> fst (value_of_bin buffer (index+0+i*s) type_of_elt))), size) | Scalar "string" -> let n = Char.code buffer.[index] in (String (String.sub buffer (index+1) n), (1+n)) @@ -345,11 +345,11 @@ let rec sprint_value = fun buf i _type v -> match _type, v with Scalar "uint8", Int x -> if x < 0 || x > 0xff then - failwith (sprintf "Value too large to fit in a uint8: %d" x); + failwith (sprintf "Value too large to fit in a uint8: %d" x); buf.[i] <- Char.chr x; sizeof _type | Scalar "int8", Int x -> if x < -0x7f || x > 0x7f then - failwith (sprintf "Value too large to fit in a int8: %d" x); + failwith (sprintf "Value too large to fit in a int8: %d" x); sprint_int8 buf i x; sizeof _type | Scalar "float", Float f -> sprint_float buf i f; sizeof _type | Scalar "double", Float f -> sprint_double buf i f; sizeof _type @@ -363,13 +363,13 @@ let rec sprint_value = fun buf i _type v -> buf.[i+1] <- byte (value lsr 8); buf.[i+0] <- byte value; sizeof _type - | Scalar ("int64" | "uint64"), Int value -> + | Scalar ("int64" | "uint64"), Int value -> assert (_type <> Scalar "uint64" || value >= 0); buf.[i+7] <- byte (value asr 56); - buf.[i+6] <- byte (value lsr 48); - buf.[i+5] <- byte (value lsr 40); - buf.[i+4] <- byte (value lsr 32); - buf.[i+3] <- byte (value lsr 24); + buf.[i+6] <- byte (value lsr 48); + buf.[i+5] <- byte (value lsr 40); + buf.[i+4] <- byte (value lsr 32); + buf.[i+3] <- byte (value lsr 24); buf.[i+2] <- byte (value lsr 16); buf.[i+1] <- byte (value lsr 8); buf.[i+0] <- byte value; @@ -386,16 +386,16 @@ let rec sprint_value = fun buf i _type v -> let type_of_elt = Scalar t in let s = sizeof type_of_elt in for j = 0 to n - 1 do - ignore (sprint_value buf (i+1+j*s) type_of_elt values.(j)) + ignore (sprint_value buf (i+1+j*s) type_of_elt values.(j)) done; 1 + n * s - | FixedArrayType (t,l), Array values -> + | FixedArrayType (t,l), Array values -> (** Put the size first, then the values *) let n = Array.length values in let type_of_elt = Scalar t in let s = sizeof type_of_elt in for j = 0 to n - 1 do - ignore (sprint_value buf (i+0+j*s) type_of_elt values.(j)) + ignore (sprint_value buf (i+0+j*s) type_of_elt values.(j)) done; 0 + n * s | Scalar "string", String s -> @@ -404,13 +404,13 @@ let rec sprint_value = fun buf i _type v -> (** Put the length first, then the bytes *) buf.[i] <- Char.chr n; if (i + n >= String.length buf) then - failwith "Error in sprint_value: message too long"; + failwith "Error in sprint_value: message too long"; String.blit s 0 buf (i+1) n; 1 + n - | Scalar "char", Char c -> - buf.[i] <- c; sizeof _type + | Scalar "char", Char c -> + buf.[i] <- c; sizeof _type | (Scalar x|ArrayType x), _ -> failwith (sprintf "Pprz.sprint_value (%s)" x) - | FixedArrayType (x,l), _ -> failwith (sprintf "Pprz.sprint_value (%s)" x) + | FixedArrayType (x,l), _ -> failwith (sprintf "Pprz.sprint_value (%s)" x) @@ -420,21 +420,21 @@ let hex_of_int_array = function (* One integer -> 2 chars *) let s = String.create (2*n) in Array.iteri - (fun i dec -> - let x = int_of_value array.(i) in - assert (0 <= x && x <= 0xff); - let hex = sprintf "%02x" x in - String.blit hex 0 s (2*i) 2) - array; + (fun i dec -> + let x = int_of_value array.(i) in + assert (0 <= x && x <= 0xff); + let hex = sprintf "%02x" x in + String.blit hex 0 s (2*i) 2) + array; s | value -> failwith (sprintf "Error: expecting array in Pprz.hex_of_int_array, found %s" (string_of_value value)) type _class = { - class_id : int; - class_name : string; - class_type : string; - class_xml : Xml.xml; + class_id : int; + class_name : string; + class_type : string; + class_xml : Xml.xml; } let join_messages_of_type = fun xml _type final_class -> @@ -457,10 +457,10 @@ let join_messages_of_type = fun xml _type final_class -> | Xml.No_attribute atr -> prerr_endline ("Info: Using old messages.xml version"); xml let get_downlink_messages_in_one_class = fun xml -> - join_messages_of_type xml "downlink" "telemetry" + join_messages_of_type xml "downlink" "telemetry" let get_uplink_messages_in_one_class = fun xml -> - join_messages_of_type xml "uplink" "datalink" + join_messages_of_type xml "uplink" "datalink" exception Unknown_msg_name of string * string @@ -553,21 +553,21 @@ type messages_mode = Type | Name module type CLASS_Xml = sig val xml : Xml.xml val selection : string - val mode : messages_mode - val sel_class_id : int option + val mode : messages_mode + val sel_class_id : int option end module type CLASS_NAME = sig - val class_name : string + val class_name : string end module type CLASS_TYPE = sig - val class_type : string + val class_type : string end type msg_and_class_id = { - msg_id : int; - cls_id : int; + msg_id : int; + cls_id : int; } let space = Str.regexp "[ \t]+" @@ -575,21 +575,21 @@ let semicolon = Str.regexp "[;\t]+" let coma = Str.regexp "[,\t]+" module type MESSAGES = sig - val xml_version : string - val formated_xml : Xml.xml + val xml_version : string + val formated_xml : Xml.xml val messages : (msg_and_class_id, message) Hashtbl.t - val message_of_id : ?class_id:int -> message_id -> message + val message_of_id : ?class_id:int -> message_id -> message val message_of_name : string -> message_id * message - val class_id_of_msg : message_name -> class_id - (** [class_id_of_msg msg_name] returns the class id containing the given message *) + val class_id_of_msg : message_name -> class_id + (** [class_id_of_msg msg_name] returns the class id containing the given message *) - val class_id_of_msg_args : string -> class_id - (** [class_id_of_msg_args args.(0)] returns the class id containing the given message when args.(0) is the parameter *) + val class_id_of_msg_args : string -> class_id + (** [class_id_of_msg_args args.(0)] returns the class id containing the given message when args.(0) is the parameter *) - val class_id_of_msg_args_unsorted : string -> class_id - (** [class_id_of_msg_args_unsorted args.(0)] returns the class id containing the given message when string with semicolons is the parameter *) + val class_id_of_msg_args_unsorted : string -> class_id + (** [class_id_of_msg_args_unsorted args.(0)] returns the class id containing the given message when string with semicolons is the parameter *) val values_of_payload : Serial.payload -> packet_seq * sender_id * class_id * message_id * values (** [values_of_bin payload] Parses a raw payload, returns the @@ -628,121 +628,121 @@ end module MessagesOfXml(Class:CLASS_Xml) = struct - (* ________________________________ XML VERSION CONTROL ____________________________________________ *) - let old_classes_info = [("telemetry",["common_telemetry";"0";"downlink"]);("datalink",["common_commands";"1";"uplink"]);("alert",["alert";"2";"ground"]);("ground",["ground";"3";"ground"]);("DIA",["DIA";"4";"ground"])] - - let convert_to_new_xml = fun xml -> - try - let classes = List.map (fun _class -> - let new_info = List.assoc (ExtXml.attrib _class "name") old_classes_info in - {class_id = int_of_string(List.nth new_info 1) ; class_name = List.nth new_info 0 ; class_type = List.nth new_info 2 ; class_xml = _class }) (Xml.children xml) in - let class_nodes = List.map (fun cls -> - let param_list = ["id",string_of_int(cls.class_id);"name",cls.class_name;"type",cls.class_type] in - Xml.Element("class",param_list,Xml.children cls.class_xml) - ) classes in - let node_protocol = Xml.Element("protocol",["version","1.0"],class_nodes) in - node_protocol - with - | Not_found -> failwith ("Pprz.convert_to_new_xml New info for old class not defined") - - let to_new_xml_format = fun xml -> - try - let version = Xml.attrib xml "version" in - match version with - | "2.0" -> ("2.0",xml) - | v -> failwith (sprintf "Pprz.to_new_xml_format Version not handled (Version: %s)" v) - with Xml.No_attribute atr -> prerr_endline ("Info: Using old messages.xml version"); ("1.0",convert_to_new_xml xml) - - (** Stores the original version of the xml file and converts the xml file to the 2.0 version format *) - let (xml_version,formated_xml) = to_new_xml_format Class.xml - - (* _____________________ FROM HERE THE XML FILE IS 2.0 VERSION (formated_xml) _______________________ *) + (* ________________________________ XML VERSION CONTROL ____________________________________________ *) + let old_classes_info = [("telemetry",["common_telemetry";"0";"downlink"]);("datalink",["common_commands";"1";"uplink"]);("alert",["alert";"2";"ground"]);("ground",["ground";"3";"ground"]);("DIA",["DIA";"4";"ground"])] + + let convert_to_new_xml = fun xml -> + try + let classes = List.map (fun _class -> + let new_info = List.assoc (ExtXml.attrib _class "name") old_classes_info in + {class_id = int_of_string(List.nth new_info 1) ; class_name = List.nth new_info 0 ; class_type = List.nth new_info 2 ; class_xml = _class }) (Xml.children xml) in + let class_nodes = List.map (fun cls -> + let param_list = ["id",string_of_int(cls.class_id);"name",cls.class_name;"type",cls.class_type] in + Xml.Element("class",param_list,Xml.children cls.class_xml) + ) classes in + let node_protocol = Xml.Element("protocol",["version","1.0"],class_nodes) in + node_protocol + with + | Not_found -> failwith ("Pprz.convert_to_new_xml New info for old class not defined") + + let to_new_xml_format = fun xml -> + try + let version = Xml.attrib xml "version" in + match version with + | "2.0" -> ("2.0",xml) + | v -> failwith (sprintf "Pprz.to_new_xml_format Version not handled (Version: %s)" v) + with Xml.No_attribute atr -> prerr_endline ("Info: Using old messages.xml version"); ("1.0",convert_to_new_xml xml) + + (** Stores the original version of the xml file and converts the xml file to the 2.0 version format *) + let (xml_version,formated_xml) = to_new_xml_format Class.xml + + (* _____________________ FROM HERE THE XML FILE IS 2.0 VERSION (formated_xml) _______________________ *) let max_length = 256 - type _class = { - class_id : int; - class_name : string; - class_type : string; - class_xml : Xml.xml; - } - (** classes_by_id: Hastbl{class_id,Hashtbl{message_id,message}} *) - let classes_by_id = Hashtbl.create 1 - (** messages_by_name: Hastbl{message_name,message} *) - let messages_by_name = Hashtbl.create 256 - (** messages_by_name: Hastbl{(message_id,class_id),message} *) - let messages = Hashtbl.create 256 - - let parse_class = fun struc -> - let xml_class = struc.class_xml in - let by_id = Hashtbl.create 13 in - List.iter - (fun xml_msg -> - let name = ExtXml.attrib xml_msg "name" - and link = - try - Some (link_mode_of_string (Xml.attrib xml_msg "link")) - with - Xml.No_attribute("link") -> None - in - let msg = { - name = name; - fields = List.map field_of_xml (Xml.children xml_msg); - link = link - } in - let id = int_of_string (ExtXml.attrib xml_msg "id") in - if Hashtbl.mem by_id id then failwith (sprintf "Duplicated id in messages.xml: %d" id); - Hashtbl.add by_id id msg; - let ids_for_messages = { msg_id = id; cls_id = struc.class_id} in - Hashtbl.add messages ids_for_messages msg; - Hashtbl.add messages_by_name name (id, msg)) - (Xml.children xml_class); - by_id + type _class = { + class_id : int; + class_name : string; + class_type : string; + class_xml : Xml.xml; + } + (** classes_by_id: Hastbl{class_id,Hashtbl{message_id,message}} *) + let classes_by_id = Hashtbl.create 1 + (** messages_by_name: Hastbl{message_name,message} *) + let messages_by_name = Hashtbl.create 256 + (** messages_by_name: Hastbl{(message_id,class_id),message} *) + let messages = Hashtbl.create 256 + + let parse_class = fun struc -> + let xml_class = struc.class_xml in + let by_id = Hashtbl.create 13 in + List.iter + (fun xml_msg -> + let name = ExtXml.attrib xml_msg "name" + and link = + try + Some (link_mode_of_string (Xml.attrib xml_msg "link")) + with + Xml.No_attribute("link") -> None + in + let msg = { + name = name; + fields = List.map field_of_xml (Xml.children xml_msg); + link = link + } in + let id = int_of_string (ExtXml.attrib xml_msg "id") in + if Hashtbl.mem by_id id then failwith (sprintf "Duplicated id in messages.xml: %d" id); + Hashtbl.add by_id id msg; + let ids_for_messages = { msg_id = id; cls_id = struc.class_id} in + Hashtbl.add messages ids_for_messages msg; + Hashtbl.add messages_by_name name (id, msg)) + (Xml.children xml_class); + by_id let messages_loaded = try - match Class.mode with - | Type -> - let class_structs = List.map (fun _class -> - {class_id = int_of_string (ExtXml.attrib _class "id") ; class_name = ExtXml.attrib _class "name" ; class_type = ExtXml.attrib _class "type" ; class_xml = _class } - ) (Xml.children formated_xml) in - let selected_classes = ref [] in - ignore (List.map (fun struc -> match struc.class_type with - | "datalink" -> if (Class.selection = "uplink"||Class.selection = "downlink") then selected_classes := List.append !selected_classes [struc]; - | other_type -> if Class.selection = other_type then selected_classes := List.append !selected_classes [struc]; - ) class_structs); - ignore (List.map (fun sel -> - let by_id = parse_class sel in - Hashtbl.add classes_by_id sel.class_id by_id - ) !selected_classes); - true - | Name -> - let class_structs = List.map (fun _class -> - {class_id = int_of_string (ExtXml.attrib _class "id") ; class_name = ExtXml.attrib _class "name" ; class_type = ExtXml.attrib _class "type" ; class_xml = _class } - ) (Xml.children formated_xml) in - let selected_classes = ref [] in - ignore (List.map (fun struc -> if struc.class_name = Class.selection then selected_classes := List.append !selected_classes [struc]; - ) class_structs); - ignore (List.map (fun sel -> - let by_id = parse_class sel in - Hashtbl.add classes_by_id sel.class_id by_id - ) !selected_classes); - true + match Class.mode with + | Type -> + let class_structs = List.map (fun _class -> + {class_id = int_of_string (ExtXml.attrib _class "id") ; class_name = ExtXml.attrib _class "name" ; class_type = ExtXml.attrib _class "type" ; class_xml = _class } + ) (Xml.children formated_xml) in + let selected_classes = ref [] in + ignore (List.map (fun struc -> match struc.class_type with + | "datalink" -> if (Class.selection = "uplink"||Class.selection = "downlink") then selected_classes := List.append !selected_classes [struc]; + | other_type -> if Class.selection = other_type then selected_classes := List.append !selected_classes [struc]; + ) class_structs); + ignore (List.map (fun sel -> + let by_id = parse_class sel in + Hashtbl.add classes_by_id sel.class_id by_id + ) !selected_classes); + true + | Name -> + let class_structs = List.map (fun _class -> + {class_id = int_of_string (ExtXml.attrib _class "id") ; class_name = ExtXml.attrib _class "name" ; class_type = ExtXml.attrib _class "type" ; class_xml = _class } + ) (Xml.children formated_xml) in + let selected_classes = ref [] in + ignore (List.map (fun struc -> if struc.class_name = Class.selection then selected_classes := List.append !selected_classes [struc]; + ) class_structs); + ignore (List.map (fun sel -> + let by_id = parse_class sel in + Hashtbl.add classes_by_id sel.class_id by_id + ) !selected_classes); + true with | Not_found -> failwith ("Pprz->messages: error loading classes and messages (Not_found) ") - | e -> failwith (sprintf "Pprz->messages: error loading classes and messages (Exception: %s) " (Printexc.to_string e)) + | e -> failwith (sprintf "Pprz->messages: error loading classes and messages (Exception: %s) " (Printexc.to_string e)) - let check_mode = fun class_id -> - match class_id with - | None -> if (Class.mode = Name) then match Class.sel_class_id with None -> failwith("") | Some cls_id-> cls_id else failwith ("Pprz.Messages class_id not defined ") - | Some cls_id -> cls_id + let check_mode = fun class_id -> + match class_id with + | None -> if (Class.mode = Name) then match Class.sel_class_id with None -> failwith("") | Some cls_id-> cls_id else failwith ("Pprz.Messages class_id not defined ") + | Some cls_id -> cls_id let message_of_id = fun ?class_id id -> - try - let cls_id = check_mode class_id in - let sel_class = Hashtbl.find classes_by_id cls_id in - Hashtbl.find sel_class id - with Not_found -> failwith ("Pprz.Messages message not found") + try + let cls_id = check_mode class_id in + let sel_class = Hashtbl.find classes_by_id cls_id in + Hashtbl.find sel_class id + with Not_found -> failwith ("Pprz.Messages message not found") let message_of_name = fun name -> @@ -751,45 +751,45 @@ module MessagesOfXml(Class:CLASS_Xml) = struct with Not_found -> raise (Unknown_msg_name (name, Class.selection)) - let is_message_inside = fun pattern message -> - let msg_name = Xml.attrib message "name" in - if msg_name = pattern then true else false - - let get_id_and_messages = fun msg clas -> - let id = ExtXml.attrib clas "id" - and messages = Xml.children clas in - let contains = List.map (is_message_inside msg) messages in - let is_inside = List.mem true contains in - if is_inside = true then int_of_string(id) else -1 - - exception Msg_Duplicated of string - exception Msg_Not_found of string - - let class_id_of_msg = fun msg -> - try - let classes = Xml.children formated_xml in - let classes_with = List.map (get_id_and_messages msg) classes in - let result = List.filter (fun x -> if(x>(-1))then true else false) classes_with in - match result with - | [] -> raise (Msg_Not_found msg) - | [x] -> x - | _ -> raise (Msg_Duplicated msg) - with - | Msg_Duplicated s -> failwith (sprintf "No class containing message %s" s) - | Msg_Not_found s -> failwith (sprintf "More than one class containing message %s" s) - | Not_found -> failwith (sprintf "No class attribute found for message '%s'" msg) - | e -> failwith (sprintf "Unhandled exception (Exception: %s)" (Printexc.to_string e)) - - let class_id_of_msg_args = fun s -> - match Str.split space s with - | [] -> failwith (sprintf "Cannot obtain msg_name from arguments string '%s'" s) - | msg_name::args -> class_id_of_msg msg_name - - (** For raw_datalink messages *) - let class_id_of_msg_args_unsorted = fun s -> - match Str.split semicolon s with - | [] -> failwith (sprintf "Cannot obtain msg_name from unsorted arguments string '%s'" s) - | msg_name::args -> class_id_of_msg msg_name + let is_message_inside = fun pattern message -> + let msg_name = Xml.attrib message "name" in + if msg_name = pattern then true else false + + let get_id_and_messages = fun msg clas -> + let id = ExtXml.attrib clas "id" + and messages = Xml.children clas in + let contains = List.map (is_message_inside msg) messages in + let is_inside = List.mem true contains in + if is_inside = true then int_of_string(id) else -1 + + exception Msg_Duplicated of string + exception Msg_Not_found of string + + let class_id_of_msg = fun msg -> + try + let classes = Xml.children formated_xml in + let classes_with = List.map (get_id_and_messages msg) classes in + let result = List.filter (fun x -> if(x>(-1))then true else false) classes_with in + match result with + | [] -> raise (Msg_Not_found msg) + | [x] -> x + | _ -> raise (Msg_Duplicated msg) + with + | Msg_Duplicated s -> failwith (sprintf "No class containing message %s" s) + | Msg_Not_found s -> failwith (sprintf "More than one class containing message %s" s) + | Not_found -> failwith (sprintf "No class attribute found for message '%s'" msg) + | e -> failwith (sprintf "Unhandled exception (Exception: %s)" (Printexc.to_string e)) + + let class_id_of_msg_args = fun s -> + match Str.split space s with + | [] -> failwith (sprintf "Cannot obtain msg_name from arguments string '%s'" s) + | msg_name::args -> class_id_of_msg msg_name + + (** For raw_datalink messages *) + let class_id_of_msg_args_unsorted = fun s -> + match Str.split semicolon s with + | [] -> failwith (sprintf "Cannot obtain msg_name from unsorted arguments string '%s'" s) + | msg_name::args -> class_id_of_msg msg_name let values_of_payload = fun buffer -> @@ -797,45 +797,45 @@ module MessagesOfXml(Class:CLASS_Xml) = struct try let id = Char.code buffer.[offset_msg_id] in let sender_id = Char.code buffer.[offset_sender_id] in - let packet_seq = Char.code buffer.[offset_packet_seq] in - let class_id = Char.code buffer.[offset_class_id] in + let packet_seq = Char.code buffer.[offset_packet_seq] in + let class_id = Char.code buffer.[offset_class_id] in let message = message_of_id ~class_id:class_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 - [] -> - if index = String.length buffer then - [] - else - failwith (sprintf "Pprz.values_of_payload, too many bytes: %s" (Debug.xprint buffer)) - | (field_name, field_descr)::fs -> - let (value, n) = value_field buffer index field_descr in - (field_name, value) :: loop (index+n) fs in + match fields with + [] -> + if index = String.length buffer then + [] + else + failwith (sprintf "Pprz.values_of_payload, too many bytes: %s" (Debug.xprint buffer)) + | (field_name, field_descr)::fs -> + let (value, n) = value_field buffer index field_descr in + (field_name, value) :: loop (index+n) fs in (packet_seq, sender_id, class_id, id, loop offset_fields message.fields) with Invalid_argument("index out of bounds") -> - failwith (sprintf "Pprz.values_of_payload, wrong argument: %s" (Debug.xprint buffer)) + failwith (sprintf "Pprz.values_of_payload, wrong argument: %s" (Debug.xprint buffer)) let payload_of_values = fun ?gen_packet_seq sender_id ?class_id id values -> - let cls_id = check_mode class_id in - let message = message_of_id ~class_id:cls_id id in + let cls_id = check_mode class_id in + let message = message_of_id ~class_id:cls_id id 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_sender_id] <- Char.chr sender_id; - p.[offset_packet_seq] <- Char.chr (match gen_packet_seq with None -> 0 | Some some -> some); - p.[offset_class_id] <- Char.chr cls_id; + p.[offset_packet_seq] <- Char.chr (match gen_packet_seq with None -> 0 | Some some -> some); + p.[offset_class_id] <- Char.chr cls_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 - let size = sprint_value p !i field._type v in - i := !i + size - ) + let v = + try List.assoc field_name values with + Not_found -> default_value field._type in + let size = sprint_value p !i field._type v in + i := !i + size + ) message.fields; (** Cut to the actual length *) @@ -845,64 +845,64 @@ module MessagesOfXml(Class:CLASS_Xml) = struct 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, field) v -> (field_name, value field._type v)) msg.fields args in - (msg_id, values) - with - Invalid_argument "List.map2" -> failwith (sprintf "Pprz.values_of_string: incorrect number of fields in '%s'" s) - end + 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 + Invalid_argument "List.map2" -> failwith (sprintf "Pprz.values_of_string: incorrect number of fields in '%s'" s) + end | [] -> invalid_arg (sprintf "Pprz.values_of_string: %s" s) - let values_of_string_unsorted = fun s -> + let values_of_string_unsorted = fun s -> match Str.split semicolon s with msg_name::args -> - begin - try - let msg_id, msg = message_of_name msg_name in - - let assoc_args = ref [] in - for i = 0 to List.length args -1 do - let parts = Str.split coma (List.nth args i) in - assoc_args := List.append !assoc_args [((List.nth parts 0),(List.nth parts 1))] - done; - - let values = List.map (fun (field_name, field) -> - let v = List.assoc field_name !assoc_args in - (field_name, value field._type v) ) msg.fields in - (msg_id, values) - with - | Not_found -> failwith (sprintf "Pprz.values_of_string_unsorted: field name not found in args list") - | e -> failwith (sprintf "Pprz.values_of_string_unsorted: Unhandled exception (Exception: %s)" (Printexc.to_string e)) - end + begin + try + let msg_id, msg = message_of_name msg_name in + + let assoc_args = ref [] in + for i = 0 to List.length args -1 do + let parts = Str.split coma (List.nth args i) in + assoc_args := List.append !assoc_args [((List.nth parts 0),(List.nth parts 1))] + done; + + let values = List.map (fun (field_name, field) -> + let v = List.assoc field_name !assoc_args in + (field_name, value field._type v) ) msg.fields in + (msg_id, values) + with + | Not_found -> failwith (sprintf "Pprz.values_of_string_unsorted: field name not found in args list") + | e -> failwith (sprintf "Pprz.values_of_string_unsorted: Unhandled exception (Exception: %s)" (Printexc.to_string e)) + end | [] -> invalid_arg (sprintf "Pprz.values_of_string_unsorted: %s" s) let string_of_message = fun ?(sep=" ") msg values -> (** Check that the values are compatible with this message *) List.iter (fun (k, _) -> - if not (List.mem_assoc k msg.fields) - then invalid_arg (sprintf "Pprz.string_of_message: unknown field '%s' in message '%s'" k msg.name)) + if not (List.mem_assoc k msg.fields) + then invalid_arg (sprintf "Pprz.string_of_message: unknown field '%s' in message '%s'" k msg.name)) values; String.concat sep (msg.name:: List.map - (fun (field_name, field) -> - let v = - try List.assoc field_name values with - Not_found -> - default_value field._type in - formatted_string_of_value field.fformat v) - msg.fields) + (fun (field_name, field) -> + let v = + try List.assoc field_name values with + Not_found -> + default_value field._type in + formatted_string_of_value field.fformat v) + msg.fields) let message_send = fun ?timestamp sender msg_name values -> let m = snd (message_of_name msg_name) in let s = string_of_message m values in let timestamp_string = match timestamp with - None -> "" + None -> "" | Some x -> sprintf "%f " x in let msg = sprintf "%s%s %s" timestamp_string sender s in let n = String.length msg in @@ -914,28 +914,28 @@ module MessagesOfXml(Class:CLASS_Xml) = struct let message_bind = fun ?sender msg_name cb -> match sender with None -> - Ivy.bind - (fun _ args -> - let values = try snd (values_of_string args.(2)) with exc -> prerr_endline (Printexc.to_string exc); [] in - cb args.(1) values) - (sprintf "^([0-9]+\\.[0-9]+ )?([^ ]*) +(%s( .*|$))" msg_name) + Ivy.bind + (fun _ args -> + let values = try snd (values_of_string args.(2)) with exc -> prerr_endline (Printexc.to_string exc); [] in + cb args.(1) values) + (sprintf "^([0-9]+\\.[0-9]+ )?([^ ]*) +(%s( .*|$))" msg_name) | Some s -> - Ivy.bind - (fun _ args -> - let values = try snd (values_of_string args.(1)) with exc -> prerr_endline (Printexc.to_string exc); [] in - cb s values) - (sprintf "^([0-9]+\\.[0-9]+ )?%s +(%s( .*|$))" s msg_name) + Ivy.bind + (fun _ args -> + let values = try snd (values_of_string args.(1)) with exc -> prerr_endline (Printexc.to_string exc); [] in + cb s values) + (sprintf "^([0-9]+\\.[0-9]+ )?%s +(%s( .*|$))" s msg_name) let message_answerer = fun sender msg_name cb -> let ivy_cb = fun _ args -> let asker = args.(0) and asker_id = args.(1) in try (** Against [cb] exceptions *) - let values = cb asker (snd (values_of_string args.(2))) in - let m = string_of_message (snd (message_of_name msg_name)) values in - Ivy.send (sprintf "%s %s %s" asker_id sender m) + let values = cb asker (snd (values_of_string args.(2))) in + let m = string_of_message (snd (message_of_name msg_name)) values in + Ivy.send (sprintf "%s %s %s" asker_id sender m) with - exc -> fprintf stderr "Pprz.answerer %s:%s: %s\n%!" sender msg_name (Printexc.to_string exc) + exc -> fprintf stderr "Pprz.answerer %s:%s: %s\n%!" sender msg_name (Printexc.to_string exc) in Ivy.bind ivy_cb (sprintf "^([^ ]*) +([^ ]*) +(%s_REQ.*)" msg_name) @@ -958,30 +958,30 @@ module Messages_of_type(Class:CLASS_TYPE) = struct include MessagesOfXml(struct let xml = messages_xml () let selection = Class.class_type - let mode = Type - let sel_class_id = None + let mode = Type + let sel_class_id = None end) end type class_id_name = { - class_id : int; - class_name : string; + class_id : int; + class_name : string; } let class_id_of_name = fun name -> - try - let messages_xml = Xml.parse_file messages_file in - let classes = List.map (fun i_class -> {class_id = int_of_string (ExtXml.attrib i_class "id") ; class_name = ExtXml.attrib i_class "name"}) (Xml.children messages_xml) in - let sel_class = List.find (fun x -> if x.class_name = name then true else false ) classes in - sel_class.class_id - with - | Not_found -> failwith (sprintf "Pprz.Messages_of_name initialization error. No class with name %s" name) + try + let messages_xml = Xml.parse_file messages_file in + let classes = List.map (fun i_class -> {class_id = int_of_string (ExtXml.attrib i_class "id") ; class_name = ExtXml.attrib i_class "name"}) (Xml.children messages_xml) in + let sel_class = List.find (fun x -> if x.class_name = name then true else false ) classes in + sel_class.class_id + with + | Not_found -> failwith (sprintf "Pprz.Messages_of_name initialization error. No class with name %s" name) module Messages_of_name(Class:CLASS_NAME) = struct include MessagesOfXml(struct let xml = messages_xml () let selection = Class.class_name - let mode = Name - let sel_class_id = Some (class_id_of_name Class.class_name) + let mode = Name + let sel_class_id = Some (class_id_of_name Class.class_name) end) end diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index 591ca11d5cf..c73a5ec9785 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -36,7 +36,7 @@ type format = string type _type = Scalar of string | ArrayType of string - | FixedArrayType of string * int + | FixedArrayType of string * int type value = Int of int | Float of float | String of string | Int32 of int32 | Char of char | Int64 of int64 | Array of value array @@ -151,11 +151,11 @@ module TransportExtended : Serial.PROTOCOL val offset_fields : int module type CLASS_NAME = sig - val class_name : string + val class_name : string end module type CLASS_TYPE = sig - val class_type : string + val class_type : string end type messages_mode = Type | Name @@ -163,31 +163,31 @@ type messages_mode = Type | Name module type CLASS_Xml = sig val xml : Xml.xml val selection : string - val mode : messages_mode - val sel_class_id : int option + val mode : messages_mode + val sel_class_id : int option end type msg_and_class_id = { - msg_id : int; - cls_id : int; + msg_id : int; + cls_id : int; } module type MESSAGES = sig - val xml_version : string - val formated_xml : Xml.xml + val xml_version : string + val formated_xml : Xml.xml val messages : (msg_and_class_id, message) Hashtbl.t - val message_of_id : ?class_id:int -> message_id -> message + val message_of_id : ?class_id:int -> message_id -> message val message_of_name : string -> message_id * message - val class_id_of_msg : message_name -> class_id - (** [class_id_of_msg msg_name] returns the class id containing the given message *) + val class_id_of_msg : message_name -> class_id + (** [class_id_of_msg msg_name] returns the class id containing the given message *) - val class_id_of_msg_args : string -> class_id - (** [class_id_of_msg_args args.(0)] returns the class id containing the given message when args.(0) is the parameter *) + val class_id_of_msg_args : string -> class_id + (** [class_id_of_msg_args args.(0)] returns the class id containing the given message when args.(0) is the parameter *) - val class_id_of_msg_args_unsorted : string -> class_id - (** [class_id_of_msg_args_unsorted args.(0)] returns the class id containing the given message when string with semicolons is the parameter *) + val class_id_of_msg_args_unsorted : string -> class_id + (** [class_id_of_msg_args_unsorted args.(0)] returns the class id containing the given message when string with semicolons is the parameter *) val values_of_payload : Serial.payload -> packet_seq * sender_id * class_id * message_id * values (** [values_of_bin payload] Parses a raw payload, returns the diff --git a/sw/logalizer/plot.ml b/sw/logalizer/plot.ml index 2488cb86777..c749b2199ed 100644 --- a/sw/logalizer/plot.ml +++ b/sw/logalizer/plot.ml @@ -751,7 +751,7 @@ let load_log = fun ?export ?factor (plot:plot) (menubar:GMenu.menu_shell GMenu.f (* First sort by message id *) let l = ref [] in - Hashtbl.iter (fun msg_ids fields -> l := (P.message_of_id ~class_id:msg_ids.cls_id msg_ids.msg_id, fields):: !l) msgs; + Hashtbl.iter (fun msg_ids fields -> l := (P.message_of_id ~class_id:msg_ids.cls_id msg_ids.msg_id, fields):: !l) msgs; let msgs = List.sort (fun (a,_) (b,_) -> compare a b) !l in let msgs = diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml index 56359cdd455..71071413c20 100644 --- a/sw/tools/gen_messages.ml +++ b/sw/tools/gen_messages.ml @@ -33,7 +33,7 @@ type format = string type _type = Basic of string | Array of string * string - | FixedArray of string * string * int + | FixedArray of string * string * int type field = _type * string * format option @@ -49,7 +49,7 @@ type message = { module Syntax = struct (** Parse a type name and returns a _type value *) let parse_type = fun t varname -> - try + try let type_parts = Str.full_split (Str.regexp "[][]") t in match type_parts with | [Str.Text ty] -> Basic ty @@ -66,17 +66,17 @@ module Syntax = struct List.assoc t Pprz.types with Not_found -> - failwith (sprintf "Error: '%s' unknown type" t) + failwith (sprintf "Error: '%s' unknown type" t) let rec sizeof = function Basic t -> string_of_int (assoc_types t).Pprz.size | Array (t, varname) -> sprintf "1+%s*%s" (length_name varname) (sizeof (Basic t)) - | FixedArray (t, varname, len) -> sprintf "0+%d*%s" len (sizeof (Basic t)) + | FixedArray (t, varname, len) -> sprintf "0+%d*%s" len (sizeof (Basic t)) let rec nameof = function Basic t -> String.capitalize t | Array _ -> failwith "nameof" - | FixedArray _ -> failwith "nameof" + | FixedArray _ -> failwith "nameof" (** Translates a "message" XML element into a value of the 'message' type *) let struct_of_xml = fun xml -> @@ -85,13 +85,13 @@ module Syntax = struct and period = try Some (ExtXml.float_attrib xml "period") with _ -> None and fields = List.map - (fun field -> - let id = ExtXml.attrib field "name" - and type_name = ExtXml.attrib field "type" - and fmt = try Some (Xml.attrib field "format") with _ -> None in - let _type = parse_type type_name id in - (_type, id, fmt)) - (Xml.children xml) in + (fun field -> + let id = ExtXml.attrib field "name" + and type_name = ExtXml.attrib field "type" + and fmt = try Some (Xml.attrib field "format") with _ -> None in + let _type = parse_type type_name id in + (_type, id, fmt)) + (Xml.children xml) in { id=id; name = name; period = period; fields = fields } let check_single_ids = fun msgs -> @@ -124,24 +124,24 @@ module Gen_onboard = struct let print_field = fun h (t, name, (_f: format option)) -> match t with Basic _ -> - fprintf h "\t DownlinkPut%sByAddr(_trans, _dev, (%s)); \\\n" (Syntax.nameof t) name + fprintf h "\t DownlinkPut%sByAddr(_trans, _dev, (%s)); \\\n" (Syntax.nameof t) name | Array (t, varname) -> - let _s = Syntax.sizeof (Basic t) in - fprintf h "\t DownlinkPut%sArray(_trans, _dev, %s, %s); \\\n" (Syntax.nameof (Basic t)) (Syntax.length_name varname) name - | FixedArray (t, varname, len) -> - let _s = Syntax.sizeof (Basic t) in - fprintf h "\t DownlinkPut%sFixedArray(_trans, _dev, %d, %s); \\\n" (Syntax.nameof (Basic t)) len name + let _s = Syntax.sizeof (Basic t) in + fprintf h "\t DownlinkPut%sArray(_trans, _dev, %s, %s); \\\n" (Syntax.nameof (Basic t)) (Syntax.length_name varname) name + | FixedArray (t, varname, len) -> + let _s = Syntax.sizeof (Basic t) in + fprintf h "\t DownlinkPut%sFixedArray(_trans, _dev, %d, %s); \\\n" (Syntax.nameof (Basic t)) len name let print_parameter h = function (Array _, s, _) -> fprintf h "%s, %s" (Syntax.length_name s) s - | (FixedArray _, s, _) -> fprintf h "%s" s + | (FixedArray _, s, _) -> fprintf h "%s" s | (_, s, _) -> fprintf h "%s" s let print_macro_parameters h = function [] -> () | f::fields -> - print_parameter h f; - List.iter (fun f -> fprintf h ", "; print_parameter h f) fields + print_parameter h f; + List.iter (fun f -> fprintf h ", "; print_parameter h f) fields let rec size_fields = fun fields size -> match fields with @@ -153,9 +153,9 @@ module Gen_onboard = struct let estimated_size_of_message = fun m -> try List.fold_right - (fun (t, _, _) r -> int_of_string (Syntax.sizeof t)+r) - m.fields - 0 + (fun (t, _, _) r -> int_of_string (Syntax.sizeof t)+r) + m.fields + 0 with Failure "int_of_string" -> 0 @@ -202,8 +202,8 @@ module Gen_onboard = struct let sizes = List.map - (fun m -> (estimated_size_of_message m, m.name)) - messages in + (fun m -> (estimated_size_of_message m, m.name)) + messages in let sizes = List.sort (fun (s1,_) (s2,_) -> compare s2 s1) sizes in List.iter @@ -227,63 +227,63 @@ module Gen_onboard = struct if !offset < 0 then failwith "FIXME: No field allowed after an array field (print_get_macros)"; (** Converts bytes into the required type *) let typed = fun o pprz_type -> (* o for offset *) - let size = pprz_type.Pprz.size in - if check_alignment && o mod (min size 4) <> 0 then failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); - - match size with - 1 -> sprintf "(%s)(*((uint8_t*)_payload+%d))" pprz_type.Pprz.inttype o - | 2 -> sprintf "(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8)" pprz_type.Pprz.inttype o o - | 4 when pprz_type.Pprz.inttype = "float" -> - sprintf "({ union { uint32_t u; float f; } _f; _f.u = (uint32_t)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24); _f.f; })" o o o o - | 8 when pprz_type.Pprz.inttype = "double" -> - let s = ref (sprintf "*((uint8_t*)_payload+%d)" o) in - for i = 1 to 7 do - s := !s ^ sprintf "|((uint64_t)*((uint8_t*)_payload+%d+%d))<<%d" o i (8*i) - done; - sprintf "({ union { uint64_t u; double f; } _f; _f.u = (uint64_t)(%s); Swap32IfBigEndian(_f.u); _f.f; })" !s - | 4 -> - sprintf "(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24)" pprz_type.Pprz.inttype o o o o - | 8 -> - let s = ref (sprintf "(%s)(*((uint8_t*)_payload+%d)" pprz_type.Pprz.inttype o) in - for i = 1 to 7 do - s := !s ^ sprintf "|((uint64_t)*((uint8_t*)_payload+%d+%d))<<%d" o i (8*i) - done; - sprintf "%s)" !s - | _ -> failwith "unexpected size in Gen_messages.print_get_macros" in - - (** To be an array or not to be an array: *) - match _type with - Basic t -> - let pprz_type = Syntax.assoc_types t in - fprintf h "#define DL_%s_%s(_payload) (%s)\n" msg_name field_name (typed !offset pprz_type); - offset := !offset + pprz_type.Pprz.size - - | Array (t, _varname) -> - (** The macro to access to the length of the array *) - fprintf h "#define DL_%s_%s_length(_payload) (%s)\n" msg_name field_name (typed !offset (Syntax.assoc_types "uint8")); - incr offset; - (** The macro to access to the array itself *) - let pprz_type = Syntax.assoc_types t in - if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then - failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); - - fprintf h "#define DL_%s_%s(_payload) ((%s*)(_payload+%d))\n" msg_name field_name pprz_type.Pprz.inttype !offset; - offset := -1 (** Mark for no more fields *) - | FixedArray (t, _varname, len) -> - (** The macro to access to the length of the array *) - fprintf h "#define DL_%s_%s_length(_payload) (%d)\n" msg_name field_name len; - (** The macro to access to the array itself *) - let pprz_type = Syntax.assoc_types t in - if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then - failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); - - fprintf h "#define DL_%s_%s(_payload) ((%s*)(_payload+%d))\n" msg_name field_name pprz_type.Pprz.inttype !offset; - offset := !offset + (pprz_type.Pprz.size*len) - in - - fprintf h "\n"; - (** Do it for all the fields of the message *) - List.iter parse_field message.fields + let size = pprz_type.Pprz.size in + if check_alignment && o mod (min size 4) <> 0 then failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); + + match size with + 1 -> sprintf "(%s)(*((uint8_t*)_payload+%d))" pprz_type.Pprz.inttype o + | 2 -> sprintf "(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8)" pprz_type.Pprz.inttype o o + | 4 when pprz_type.Pprz.inttype = "float" -> + sprintf "({ union { uint32_t u; float f; } _f; _f.u = (uint32_t)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24); _f.f; })" o o o o + | 8 when pprz_type.Pprz.inttype = "double" -> + let s = ref (sprintf "*((uint8_t*)_payload+%d)" o) in + for i = 1 to 7 do + s := !s ^ sprintf "|((uint64_t)*((uint8_t*)_payload+%d+%d))<<%d" o i (8*i) + done; + sprintf "({ union { uint64_t u; double f; } _f; _f.u = (uint64_t)(%s); Swap32IfBigEndian(_f.u); _f.f; })" !s + | 4 -> + sprintf "(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24)" pprz_type.Pprz.inttype o o o o + | 8 -> + let s = ref (sprintf "(%s)(*((uint8_t*)_payload+%d)" pprz_type.Pprz.inttype o) in + for i = 1 to 7 do + s := !s ^ sprintf "|((uint64_t)*((uint8_t*)_payload+%d+%d))<<%d" o i (8*i) + done; + sprintf "%s)" !s + | _ -> failwith "unexpected size in Gen_messages.print_get_macros" in + + (** To be an array or not to be an array: *) + match _type with + Basic t -> + let pprz_type = Syntax.assoc_types t in + fprintf h "#define DL_%s_%s(_payload) (%s)\n" msg_name field_name (typed !offset pprz_type); + offset := !offset + pprz_type.Pprz.size + + | Array (t, _varname) -> + (** The macro to access to the length of the array *) + fprintf h "#define DL_%s_%s_length(_payload) (%s)\n" msg_name field_name (typed !offset (Syntax.assoc_types "uint8")); + incr offset; + (** The macro to access to the array itself *) + let pprz_type = Syntax.assoc_types t in + if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then + failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); + + fprintf h "#define DL_%s_%s(_payload) ((%s*)(_payload+%d))\n" msg_name field_name pprz_type.Pprz.inttype !offset; + offset := -1 (** Mark for no more fields *) + | FixedArray (t, _varname, len) -> + (** The macro to access to the length of the array *) + fprintf h "#define DL_%s_%s_length(_payload) (%d)\n" msg_name field_name len; + (** The macro to access to the array itself *) + let pprz_type = Syntax.assoc_types t in + if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then + failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); + + fprintf h "#define DL_%s_%s(_payload) ((%s*)(_payload+%d))\n" msg_name field_name pprz_type.Pprz.inttype !offset; + offset := !offset + (pprz_type.Pprz.size*len) + in + + fprintf h "\n"; + (** Do it for all the fields of the message *) + List.iter parse_field message.fields end (* module Gen_onboard *) @@ -296,29 +296,29 @@ let () = let filename = Sys.argv.(1) and class_name = Sys.argv.(2) - and class_id = Sys.argv.(3) - and check_align = Sys.argv.(4) in + and class_id = Sys.argv.(3) + and check_align = Sys.argv.(4) in try let messages = Syntax.read filename class_name in - let h = stdout in + let h = stdout in Printf.fprintf h "/* Automatically generated from %s */\n" filename; Printf.fprintf h "/* Please DO NOT EDIT */\n"; Printf.fprintf h "/* Macros to send and receive messages of class %s */\n" class_name; (** Macros for airborne downlink (sending) *) - let u_class_name = String.uppercase class_name in - Printf.fprintf h "#ifndef MSG_%s_H\n" u_class_name; - Printf.fprintf h "#define MSG_%s_H\n" u_class_name; + let u_class_name = String.uppercase class_name in + Printf.fprintf h "#ifndef MSG_%s_H\n" u_class_name; + Printf.fprintf h "#define MSG_%s_H\n" u_class_name; Gen_onboard.print_downlink_macros h class_name class_id messages; (** Macros for airborne datalink (receiving) *) - match check_align with - | "0" -> List.iter (Gen_onboard.print_get_macros h false) messages; Printf.fprintf h "#endif // MSG_%s_H\n" u_class_name - | "1" -> List.iter (Gen_onboard.print_get_macros h true) messages; Printf.fprintf h "#endif // MSG_%s_H\n" u_class_name - | er -> failwith (sprintf "Parameter has value different than 0 or 1 (Value = %s)" er ) + match check_align with + | "0" -> List.iter (Gen_onboard.print_get_macros h false) messages; Printf.fprintf h "#endif // MSG_%s_H\n" u_class_name + | "1" -> List.iter (Gen_onboard.print_get_macros h true) messages; Printf.fprintf h "#endif // MSG_%s_H\n" u_class_name + | er -> failwith (sprintf "Parameter has value different than 0 or 1 (Value = %s)" er ) with Xml.Error (msg, pos) -> failwith (sprintf "%s:%d : %s\n" filename (Xml.line pos) (Xml.error_msg msg)) diff --git a/sw/tools/gen_messages_xml.ml b/sw/tools/gen_messages_xml.ml index 5680b334243..0612cc61fbb 100644 --- a/sw/tools/gen_messages_xml.ml +++ b/sw/tools/gen_messages_xml.ml @@ -44,272 +44,272 @@ let downlink_msg_h = Sys.argv.(6) (** Type include_xml: the structure of a XML include element *) type include_xml = { - file : string; - class_id : int; - class_name : string; + file : string; + class_id : int; + class_name : string; } type generated_class = { - g_id : string; - g_name : string; - g_type : string; + g_id : string; + g_name : string; + g_type : string; } (** Module to obtain the desired messages files to include in messages.xml *) module Includes = struct - exception Invalid_include_structure of string - exception Duplicated_class_id of string - exception Duplicated_class_name of string - exception Repeated_element of string - - (** Translates an "include" XML element into a value of the 'include' type *) - let struct_of_xml_include = fun conf_xml -> - try - let file = ExtXml.attrib conf_xml "file" - and class_id = ExtXml.int_attrib conf_xml "class_id" - and class_name = ExtXml.attrib conf_xml "class_name" in - { file = file; class_id = class_id; class_name = class_name } - with e -> raise (Invalid_include_structure (Printexc.to_string e)) - - let check_repeated_elements = fun list -> - let rec loop = fun list -> - match list with - | [] | [_] -> list - | x::((x'::_) as xs) -> if x = x' then raise (Repeated_element x) else x::loop xs in - loop (List.sort compare list) - - let check_single_names = fun xml_includes -> - let names = List.map (fun inc -> inc.class_name) xml_includes in - try - check_repeated_elements names - with - | Repeated_element el -> raise (Duplicated_class_name el) - | e -> raise e - - let check_single_ids = fun xml_includes -> - let ids = List.map (fun inc -> string_of_int inc.class_id) xml_includes in - try - check_repeated_elements ids; - with - | Repeated_element el -> raise (Duplicated_class_id el) - | e -> raise e - - (** Returns a list of all the includes in the messages_conf.xml file *) - let get_includes = fun () -> - let conf_xml = Xml.parse_file includes_file in - try - let xml_includes = List.map struct_of_xml_include (Xml.children conf_xml) in - ignore (check_single_ids xml_includes); - ignore (check_single_names xml_includes); - xml_includes - with - | Duplicated_class_id ii -> raise (Duplicated_class_id ii) - | Duplicated_class_name n -> raise (Duplicated_class_name n) - | Invalid_include_structure exc -> raise (Invalid_include_structure exc) - | e -> raise e + exception Invalid_include_structure of string + exception Duplicated_class_id of string + exception Duplicated_class_name of string + exception Repeated_element of string + + (** Translates an "include" XML element into a value of the 'include' type *) + let struct_of_xml_include = fun conf_xml -> + try + let file = ExtXml.attrib conf_xml "file" + and class_id = ExtXml.int_attrib conf_xml "class_id" + and class_name = ExtXml.attrib conf_xml "class_name" in + { file = file; class_id = class_id; class_name = class_name } + with e -> raise (Invalid_include_structure (Printexc.to_string e)) + + let check_repeated_elements = fun list -> + let rec loop = fun list -> + match list with + | [] | [_] -> list + | x::((x'::_) as xs) -> if x = x' then raise (Repeated_element x) else x::loop xs in + loop (List.sort compare list) + + let check_single_names = fun xml_includes -> + let names = List.map (fun inc -> inc.class_name) xml_includes in + try + check_repeated_elements names + with + | Repeated_element el -> raise (Duplicated_class_name el) + | e -> raise e + + let check_single_ids = fun xml_includes -> + let ids = List.map (fun inc -> string_of_int inc.class_id) xml_includes in + try + check_repeated_elements ids; + with + | Repeated_element el -> raise (Duplicated_class_id el) + | e -> raise e + + (** Returns a list of all the includes in the messages_conf.xml file *) + let get_includes = fun () -> + let conf_xml = Xml.parse_file includes_file in + try + let xml_includes = List.map struct_of_xml_include (Xml.children conf_xml) in + ignore (check_single_ids xml_includes); + ignore (check_single_names xml_includes); + xml_includes + with + | Duplicated_class_id ii -> raise (Duplicated_class_id ii) + | Duplicated_class_name n -> raise (Duplicated_class_name n) + | Invalid_include_structure exc -> raise (Invalid_include_structure exc) + | e -> raise e end (** Module to obtain the classes and their messages from the included files *) module Classes = struct - exception Invalid_class_structure of string - exception Invalid_class_type of string - exception XML_parsing_error of string * string * string - exception Invalid_class_xml_node of string - exception Class_id_out_of_range of int + exception Invalid_class_structure of string + exception Invalid_class_type of string + exception XML_parsing_error of string * string * string + exception Invalid_class_xml_node of string + exception Class_id_out_of_range of int - (** Checks if the class type is one of the allowed ones *) - let check_class_type = fun attribute -> - match attribute with - | "datalink" | "uplink" | "downlink" | "airborne" | "ground" -> attribute - | t -> raise (Invalid_class_type t) + (** Checks if the class type is one of the allowed ones *) + let check_class_type = fun attribute -> + match attribute with + | "datalink" | "uplink" | "downlink" | "airborne" | "ground" -> attribute + | t -> raise (Invalid_class_type t) - (** Translates a "class" XML element into a value of the 'class' type *) - let struct_of_xml_class = fun xml -> - try - let _type = ExtXml.attrib xml "type" in - check_class_type(_type) - with - | Invalid_class_type t -> raise (Invalid_class_type t) - | e -> raise (Invalid_class_structure (Printexc.to_string e)) + (** Translates a "class" XML element into a value of the 'class' type *) + let struct_of_xml_class = fun xml -> + try + let _type = ExtXml.attrib xml "type" in + check_class_type(_type) + with + | Invalid_class_type t -> raise (Invalid_class_type t) + | e -> raise (Invalid_class_structure (Printexc.to_string e)) - type parameters_list = (string * string) list + type parameters_list = (string * string) list (** Builds a XML Node with the class and its messages from a given file *) - let get_class_from_include = fun _include -> - let file = spread_messages_path // _include.file - and class_id = _include.class_id - and class_name = _include.class_name in - try - (** Get XML code from file *) - let spread_xml = Xml.parse_file file in - (** Get class type *) - let class_type = struct_of_xml_class spread_xml in - if((class_type<>"ground")&&(class_id > 31 || class_id < 0)) then raise (Class_id_out_of_range class_id) else - (** Get messages piece of xml *) - let xml_piece_messages = Xml.children spread_xml in - (** Build the parameters list for the XML node *) - let param_list = ["id",string_of_int class_id;"name",class_name;"type",class_type] in - (** Buid XML node with class tag & params + child (all messages) *) - Xml.Element("class",param_list,xml_piece_messages) - with - | Invalid_class_type t -> raise (Invalid_class_type t) - | Invalid_class_structure exc -> raise (Invalid_class_structure exc) - | Class_id_out_of_range i -> raise (Class_id_out_of_range i) - | Xml.Error (msg, pos) -> raise (XML_parsing_error (file,(Xml.error_msg msg),(string_of_int (Xml.line pos)))) - | e -> raise e - - (** Extracts the class parameters from a XML class node *) - let extract_name_type = fun xml_class -> - try - { g_id = ExtXml.attrib xml_class "id"; g_name = ExtXml.attrib xml_class "name"; g_type = ExtXml.attrib xml_class "type" } - with - | e -> raise (Invalid_class_xml_node (Printexc.to_string e)) + let get_class_from_include = fun _include -> + let file = spread_messages_path // _include.file + and class_id = _include.class_id + and class_name = _include.class_name in + try + (** Get XML code from file *) + let spread_xml = Xml.parse_file file in + (** Get class type *) + let class_type = struct_of_xml_class spread_xml in + if((class_type<>"ground")&&(class_id > 31 || class_id < 0)) then raise (Class_id_out_of_range class_id) else + (** Get messages piece of xml *) + let xml_piece_messages = Xml.children spread_xml in + (** Build the parameters list for the XML node *) + let param_list = ["id",string_of_int class_id;"name",class_name;"type",class_type] in + (** Buid XML node with class tag & params + child (all messages) *) + Xml.Element("class",param_list,xml_piece_messages) + with + | Invalid_class_type t -> raise (Invalid_class_type t) + | Invalid_class_structure exc -> raise (Invalid_class_structure exc) + | Class_id_out_of_range i -> raise (Class_id_out_of_range i) + | Xml.Error (msg, pos) -> raise (XML_parsing_error (file,(Xml.error_msg msg),(string_of_int (Xml.line pos)))) + | e -> raise e + + (** Extracts the class parameters from a XML class node *) + let extract_name_type = fun xml_class -> + try + { g_id = ExtXml.attrib xml_class "id"; g_name = ExtXml.attrib xml_class "name"; g_type = ExtXml.attrib xml_class "type" } + with + | e -> raise (Invalid_class_xml_node (Printexc.to_string e)) end (** Module to save the generated messages file *) module SaveXml = struct - exception Cannot_open_file of string * string - exception Cannot_move_file of string - - (** Saved the generated file with some header *) - let save = fun xml_code -> - try - let (temp_filename,h) = Filename.open_temp_file "TEMP" "TEMP" in - (*let h = open_out (generated_file) in*) - (*let h = stdout in*) - Printf.fprintf h "\n"; - Printf.fprintf h "\n"; - Printf.fprintf h "\n"; - Printf.fprintf h "\n"; - Printf.fprintf h "\n"; - Printf.fprintf h "%s" xml_code; - close_out h; - let c = sprintf "mv %s %s " temp_filename generated_file in - let returned_code = Sys.command c in - if returned_code <> 0 then raise ( Cannot_move_file (string_of_int returned_code)) - with - | e -> raise (Cannot_open_file ("messages.xml",(Printexc.to_string e))) + exception Cannot_open_file of string * string + exception Cannot_move_file of string + + (** Saved the generated file with some header *) + let save = fun xml_code -> + try + let (temp_filename,h) = Filename.open_temp_file "TEMP" "TEMP" in + (*let h = open_out (generated_file) in*) + (*let h = stdout in*) + Printf.fprintf h "\n"; + Printf.fprintf h "\n"; + Printf.fprintf h "\n"; + Printf.fprintf h "\n"; + Printf.fprintf h "\n"; + Printf.fprintf h "%s" xml_code; + close_out h; + let c = sprintf "mv %s %s " temp_filename generated_file in + let returned_code = Sys.command c in + if returned_code <> 0 then raise ( Cannot_move_file (string_of_int returned_code)) + with + | e -> raise (Cannot_open_file ("messages.xml",(Printexc.to_string e))) end (** MAIN MODULE: DOES ALL THE PROCEDURE TO GENERATE THE messages.xml FROM THE messages_conf.xml INCLUDES *) module SpreadMessages = struct - (** Generates a XML file called messages.xml with all the included classes in messages_conf.xml and their messages *) - let generate_messages_xml = fun () -> - try - (** Get list of inluded files *) - let includes = Includes.get_includes () in - (** Get list of included classes as XML Elements *) - let classes = List.map Classes.get_class_from_include includes in - (** Create the complete XML element *) - let final_xml = Xml.Element("protocol",["version","2.0"],classes) in - (** User-readable xml formating *) - let formated_xml = Xml.to_string_fmt final_xml in - (** Save the xml code into a xml file *) - SaveXml.save formated_xml; - (** Prepare and return data for the next function (List of class_ids, class_names and class_types) *) - let class_ids_names_types = List.map Classes.extract_name_type classes in - class_ids_names_types - with - | Includes.Invalid_include_structure exc -> failwith (sprintf "Invalid structure (Exception: %s)" exc) - | Includes.Duplicated_class_id ii -> failwith (sprintf "Duplicated class id (%s) at includes file" ii) - | Classes.Class_id_out_of_range i -> failwith (sprintf "Class id (%d) out of range [0->31] at includes file. Only ground type classes can exceed 31" i) - | Includes.Duplicated_class_name n -> failwith (sprintf "Duplicated class name (%s) at includes file" n) - | Classes.Invalid_class_structure exc -> failwith (sprintf "Invalid structure (Exception: %s)" exc) - | Classes.Invalid_class_type typ -> failwith (sprintf "Invalid class type: %s" typ) - | Classes.XML_parsing_error (file,msg,pos) -> failwith (sprintf "Error parsing XML file: %s (Error: %s Line: %s)" (file) (msg) (pos)) - | SaveXml.Cannot_open_file (file,exc) -> failwith (sprintf "Cannot open the file to generate: %s (Exception: %s)" file exc) - | SaveXml.Cannot_move_file err -> failwith (sprintf "Cannot move the generated file to the final destination (Error code for command MV: %s)" err) - | Classes.Invalid_class_xml_node exc -> failwith (sprintf "Invalid xml node in generated file (Exception: %s)" exc) - | e -> failwith (sprintf "Unhandled exception raised: %s" (Printexc.to_string e)) + (** Generates a XML file called messages.xml with all the included classes in messages_conf.xml and their messages *) + let generate_messages_xml = fun () -> + try + (** Get list of inluded files *) + let includes = Includes.get_includes () in + (** Get list of included classes as XML Elements *) + let classes = List.map Classes.get_class_from_include includes in + (** Create the complete XML element *) + let final_xml = Xml.Element("protocol",["version","2.0"],classes) in + (** User-readable xml formating *) + let formated_xml = Xml.to_string_fmt final_xml in + (** Save the xml code into a xml file *) + SaveXml.save formated_xml; + (** Prepare and return data for the next function (List of class_ids, class_names and class_types) *) + let class_ids_names_types = List.map Classes.extract_name_type classes in + class_ids_names_types + with + | Includes.Invalid_include_structure exc -> failwith (sprintf "Invalid structure (Exception: %s)" exc) + | Includes.Duplicated_class_id ii -> failwith (sprintf "Duplicated class id (%s) at includes file" ii) + | Classes.Class_id_out_of_range i -> failwith (sprintf "Class id (%d) out of range [0->31] at includes file. Only ground type classes can exceed 31" i) + | Includes.Duplicated_class_name n -> failwith (sprintf "Duplicated class name (%s) at includes file" n) + | Classes.Invalid_class_structure exc -> failwith (sprintf "Invalid structure (Exception: %s)" exc) + | Classes.Invalid_class_type typ -> failwith (sprintf "Invalid class type: %s" typ) + | Classes.XML_parsing_error (file,msg,pos) -> failwith (sprintf "Error parsing XML file: %s (Error: %s Line: %s)" (file) (msg) (pos)) + | SaveXml.Cannot_open_file (file,exc) -> failwith (sprintf "Cannot open the file to generate: %s (Exception: %s)" file exc) + | SaveXml.Cannot_move_file err -> failwith (sprintf "Cannot move the generated file to the final destination (Error code for command MV: %s)" err) + | Classes.Invalid_class_xml_node exc -> failwith (sprintf "Invalid xml node in generated file (Exception: %s)" exc) + | e -> failwith (sprintf "Unhandled exception raised: %s" (Printexc.to_string e)) end module MakeCalls = struct - let make_target = "gen_messages_macros" - let make_options = "" + let make_target = "gen_messages_macros" + let make_options = "" - let make = fun class_name class_id check_alignment -> - let file = Env.paparazzi_home // "Makefile" in - let macros_target = var_include_path // ("messages_"^(String.lowercase class_name)^".h") in - let c = sprintf "make -f %s MACROS_TARGET=%s MACROS_CLASS=%s MACROS_CLASS_ID=%s MACROS_ALIGN=%u %s %s" file macros_target class_name class_id check_alignment make_options make_target in + let make = fun class_name class_id check_alignment -> + let file = Env.paparazzi_home // "Makefile" in + let macros_target = var_include_path // ("messages_"^(String.lowercase class_name)^".h") in + let c = sprintf "make -f %s MACROS_TARGET=%s MACROS_CLASS=%s MACROS_CLASS_ID=%s MACROS_ALIGN=%u %s %s" file macros_target class_name class_id check_alignment make_options make_target in let returned_code = Sys.command c in - if returned_code <> 0 then failwith (sprintf "Make command error (Error code: %d)" returned_code) + if returned_code <> 0 then failwith (sprintf "Make command error (Error code: %d)" returned_code) - let generate_macros = fun classes -> - List.map (fun clas -> match (clas.g_type) with - | "datalink" -> prerr_endline ("\t Datalink Class -> Generate macros ("^clas.g_name^") [Check Alignment]"); make clas.g_name clas.g_id 1 - | "uplink" -> prerr_endline ("\t Uplink Class -> Generate macros ("^clas.g_name^") [Check Alignment]"); make clas.g_name clas.g_id 1 - | "downlink" -> prerr_endline ("\t Downlink Class -> Generate macros ("^clas.g_name^")"); make clas.g_name clas.g_id 0 - | "ground" -> prerr_endline ("\t Ground Class -> Do nothing ("^clas.g_name^")") - | "airborne" -> prerr_endline ("\t Airborne Class -> Generate macros ("^clas.g_name^") FIXME!") - | t -> failwith (sprintf "Invalid class type in generated file: %s" t) - ) classes; + let generate_macros = fun classes -> + List.map (fun clas -> match (clas.g_type) with + | "datalink" -> prerr_endline ("\t Datalink Class -> Generate macros ("^clas.g_name^") [Check Alignment]"); make clas.g_name clas.g_id 1 + | "uplink" -> prerr_endline ("\t Uplink Class -> Generate macros ("^clas.g_name^") [Check Alignment]"); make clas.g_name clas.g_id 1 + | "downlink" -> prerr_endline ("\t Downlink Class -> Generate macros ("^clas.g_name^")"); make clas.g_name clas.g_id 0 + | "ground" -> prerr_endline ("\t Ground Class -> Do nothing ("^clas.g_name^")") + | "airborne" -> prerr_endline ("\t Airborne Class -> Generate macros ("^clas.g_name^") FIXME!") + | t -> failwith (sprintf "Invalid class type in generated file: %s" t) + ) classes; end module FinalMacros = struct - exception Cannot_open_file of string - exception Cannot_move_file of string * string - - let create_both_files = fun () -> - try - let (temp_filename_up,h_up) = Filename.open_temp_file "TEMP" "TEMP" in - let (temp_filename_down,h_down) = Filename.open_temp_file "TEMP" "TEMP" in - (temp_filename_up, h_up, temp_filename_down, h_down) - with - | e -> raise (Cannot_open_file (Printexc.to_string e)) - - let close_both_files = fun temp_filename_up h_up temp_filename_down h_down -> - close_out h_up; - close_out h_down; - let cu = sprintf "mv %s %s " temp_filename_up uplink_msg_h in - let returned_code_u = Sys.command cu in - prerr_endline ("\t Uplink -> "^uplink_msg_h); - let cd = sprintf "mv %s %s " temp_filename_down downlink_msg_h in - let returned_code_d = Sys.command cd in - prerr_endline ("\t Downlink -> "^downlink_msg_h); - if returned_code_u <> 0 then raise ( Cannot_move_file ("uplink",string_of_int returned_code_u)); - if returned_code_d <> 0 then raise ( Cannot_move_file ("downlink",string_of_int returned_code_d)) + exception Cannot_open_file of string + exception Cannot_move_file of string * string + + let create_both_files = fun () -> + try + let (temp_filename_up,h_up) = Filename.open_temp_file "TEMP" "TEMP" in + let (temp_filename_down,h_down) = Filename.open_temp_file "TEMP" "TEMP" in + (temp_filename_up, h_up, temp_filename_down, h_down) + with + | e -> raise (Cannot_open_file (Printexc.to_string e)) + + let close_both_files = fun temp_filename_up h_up temp_filename_down h_down -> + close_out h_up; + close_out h_down; + let cu = sprintf "mv %s %s " temp_filename_up uplink_msg_h in + let returned_code_u = Sys.command cu in + prerr_endline ("\t Uplink -> "^uplink_msg_h); + let cd = sprintf "mv %s %s " temp_filename_down downlink_msg_h in + let returned_code_d = Sys.command cd in + prerr_endline ("\t Downlink -> "^downlink_msg_h); + if returned_code_u <> 0 then raise ( Cannot_move_file ("uplink",string_of_int returned_code_u)); + if returned_code_d <> 0 then raise ( Cannot_move_file ("downlink",string_of_int returned_code_d)) let include_in_file = fun clas h -> - Printf.fprintf h "#include \"messages_%s.h\" \n" clas.g_name - - let generate_files = fun classes -> - try - let (temp_filename_up, h_up, temp_filename_down, h_down) = create_both_files () in - ignore( - List.map (fun clas -> match (clas.g_type) with - | "uplink" -> include_in_file clas h_up - | "downlink" -> include_in_file clas h_down - | "datalink" -> include_in_file clas h_up; include_in_file clas h_down - | _ -> () - ) classes); - close_both_files temp_filename_up h_up temp_filename_down h_down - with - | Cannot_open_file s -> failwith (sprintf "Cannot open file to inlcude macro files (Exception: %s)" s) - | Cannot_move_file (f,s) -> failwith (sprintf "Cannot move file of %s macros to it's final destination (Error code for command MV: %s)" f s ) + Printf.fprintf h "#include \"messages_%s.h\" \n" clas.g_name + + let generate_files = fun classes -> + try + let (temp_filename_up, h_up, temp_filename_down, h_down) = create_both_files () in + ignore( + List.map (fun clas -> match (clas.g_type) with + | "uplink" -> include_in_file clas h_up + | "downlink" -> include_in_file clas h_down + | "datalink" -> include_in_file clas h_up; include_in_file clas h_down + | _ -> () + ) classes); + close_both_files temp_filename_up h_up temp_filename_down h_down + with + | Cannot_open_file s -> failwith (sprintf "Cannot open file to inlcude macro files (Exception: %s)" s) + | Cannot_move_file (f,s) -> failwith (sprintf "Cannot move file of %s macros to it's final destination (Error code for command MV: %s)" f s ) end (********************* Main **************************************************) let () = - if Array.length Sys.argv <> 7 then - failwith (sprintf "Usage: %s " Sys.argv.(0)); - let classes_info = SpreadMessages.generate_messages_xml () in + if Array.length Sys.argv <> 7 then + failwith (sprintf "Usage: %s " Sys.argv.(0)); + let classes_info = SpreadMessages.generate_messages_xml () in - prerr_endline ("----------- GENERATING MESSAGES MACROS (Spread) -----------"); - ignore (MakeCalls.generate_macros classes_info); - prerr_endline ("-----------------------------------------------------------"); + prerr_endline ("----------- GENERATING MESSAGES MACROS (Spread) -----------"); + ignore (MakeCalls.generate_macros classes_info); + prerr_endline ("-----------------------------------------------------------"); - prerr_endline ("----------- GENERATING MESSAGES MACROS (Global) -----------"); - ignore (FinalMacros.generate_files classes_info); - prerr_endline ("-----------------------------------------------------------"); + prerr_endline ("----------- GENERATING MESSAGES MACROS (Global) -----------"); + ignore (FinalMacros.generate_files classes_info); + prerr_endline ("-----------------------------------------------------------");