From b52d30cbf65cf7a3397325f8417e87f1fd2f50d5 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 15 Apr 2008 09:31:30 +0000 Subject: [PATCH] generator of read and send macro for the xsens protocol --- Makefile | 9 +- conf/xsens.dtd | 21 ++-- conf/xsens_MTi-G.xml | 87 ++++++++------- sw/tools/Makefile | 2 +- sw/tools/gen_xsens.ml | 243 ++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 313 insertions(+), 49 deletions(-) create mode 100644 sw/tools/gen_xsens.ml diff --git a/Makefile b/Makefile index a01ead7751..56b3380f7b 100644 --- a/Makefile +++ b/Makefile @@ -45,9 +45,11 @@ STATICINCLUDE =$(PAPARAZZI_HOME)/var/include MESSAGES_H=$(STATICINCLUDE)/messages.h MESSAGES_FBW_H=$(STATICINCLUDE)/messages_fbw.h UBX_PROTOCOL_H=$(STATICINCLUDE)/ubx_protocol.h +XSENS_PROTOCOL_H=$(STATICINCLUDE)/xsens_protocol.h DL_PROTOCOL_H=$(STATICINCLUDE)/dl_protocol.h MESSAGES_XML = $(CONF)/messages.xml UBX_XML = $(CONF)/ubx.xml +XSENS_XML = $(CONF)/xsens_MTi-G.xml TOOLS=$(PAPARAZZI_SRC)/sw/tools ARMGCC=/usr/bin/arm-elf-gcc @@ -86,7 +88,7 @@ tmtc: lib multimon: cd $(MULTIMON); $(MAKE) -static_h: $(MESSAGES_H) $(UBX_PROTOCOL_H) $(DL_PROTOCOL_H) +static_h: $(MESSAGES_H) $(UBX_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) usb_lib: @[ -d sw/airborne/arm7/lpcusb ] && ((test -x $(ARMGCC) && (cd sw/airborne/arm7/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arm7/lpcusb directory missing" @@ -103,6 +105,11 @@ $(UBX_PROTOCOL_H) : $(UBX_XML) $(TOOLS)/gen_ubx.out $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) $(TOOLS)/gen_ubx.out $< > /tmp/ubx.h $(Q)mv /tmp/ubx.h $@ +$(XSENS_PROTOCOL_H) : $(XSENS_XML) $(TOOLS)/gen_xsens.out + @echo BUILD $@ + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) $(TOOLS)/gen_xsens.out $< > /tmp/xsens.h + $(Q)mv /tmp/xsens.h $@ + $(DL_PROTOCOL_H) : $(MESSAGES_XML) $(TOOLS)/gen_messages.out @echo BUILD $@ $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) $(TOOLS)/gen_messages.out $< datalink > /tmp/dl.h diff --git a/conf/xsens.dtd b/conf/xsens.dtd index 4ac2304323..ea95b4e3a0 100644 --- a/conf/xsens.dtd +++ b/conf/xsens.dtd @@ -1,21 +1,28 @@ - + + + + + + + - diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml index 628043db33..4ce37e7c7a 100644 --- a/conf/xsens_MTi-G.xml +++ b/conf/xsens_MTi-G.xml @@ -95,32 +95,39 @@ - + - + - - + + - + - + - + - + + + + + + + + @@ -158,44 +165,44 @@ - + - - - - - - - - - + + + + + + + + + - - - - + + + + - - - + + + - - - - - - - - - + + + + + + + + + @@ -204,15 +211,15 @@ - - - + + + - - - + + + diff --git a/sw/tools/Makefile b/sw/tools/Makefile index 0782c974bf..bc7351725e 100644 --- a/sw/tools/Makefile +++ b/sw/tools/Makefile @@ -28,7 +28,7 @@ INCLUDES=-I ../lib/ocaml -I +xml-light OCAMLLEX=ocamllex OCAMLYACC=ocamlyacc -all: gen_aircraft.out gen_airframe.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out extract_makefile.out gen_periodic.out gen_settings.out gen_tuning.out +all: gen_aircraft.out gen_airframe.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out extract_makefile.out gen_periodic.out gen_settings.out gen_tuning.out gen_xsens.out FP_CMO = fp_syntax.cmo fp_parser.cmo fp_lexer.cmo fp_proc.cmo gen_flight_plan.cmo ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%) diff --git a/sw/tools/gen_xsens.ml b/sw/tools/gen_xsens.ml new file mode 100644 index 0000000000..3e1c55103e --- /dev/null +++ b/sw/tools/gen_xsens.ml @@ -0,0 +1,243 @@ +(* + * $Id$ + * + * XML preprocessing for UBX protocol + * + * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + *) + +open Printf + +let out = stdout + +exception Variable_data + +let sizeof = function + "U32" -> 32 + | "U8" -> 8 + | "U4" | "I4" | "R4" -> 4 + | "U2" | "I2" -> 2 + | "U1" | "I1" -> 1 + | x -> failwith (sprintf "sizeof: unknown format '%s'" x) + +let (+=) = fun r x -> r := !r + x + +let test_type = function + "U32" | "U8" -> "skip" + | "V" -> "variable" + | _ -> "fixe" + +let c_type = fun format -> + match format with + "R4" -> "float" + | "I4" -> "int32_t" + | "I2" -> "int16_t" + | "I1" -> "int8_t" + | "U4" -> "uint32_t" + | "U2" -> "uint16_t" + | "U1" -> "uint8_t" + | _ -> failwith (sprintf "Gen_xsens.c_type: unknown format '%s'" format) + +(* format is BigEndian *) +let get_at = fun offset format block_size -> + let t = c_type format in + let block_offset = + if block_size = 0 then "" else sprintf "+%d*_xsens_block" block_size in + match format with + "U4" | "I4" | "R4" -> sprintf "(%s)(*((uint8_t*)_xsens_payload+3+%d%s)|*((uint8_t*)_xsens_payload+2+%d%s)<<8|((%s)*((uint8_t*)_xsens_payload+1+%d%s))<<16|((%s)*((uint8_t*)_xsens_payload+%d%s))<<24)" t offset block_offset offset block_offset t offset block_offset t offset block_offset + | "U2" | "I2" -> sprintf "(%s)(*((uint8_t*)_xsens_payload+1+%d%s)|*((uint8_t*)_xsens_payload+%d%s)<<8)" t offset block_offset offset block_offset + | "U1" | "I1" -> sprintf "(%s)(*((uint8_t*)_xsens_payload+%d%s))" t offset block_offset + | _ -> failwith (sprintf "Gen_xsens.c_type: unknown format '%s'" format) + +let define = fun x y -> + fprintf out "#define %s %s\n" x y + +exception Length_error of Xml.xml*int*int + + + +let parse_message = fun m -> + let msg_name = Xml.attrib m "name" in + + fprintf out "\n"; + let msg_id = sprintf "XSENS_%s_ID" msg_name in + define msg_id (Xml.attrib m "ID"); + + let field_name = fun f -> ExtXml.attrib f "name" in + let format = fun f -> Xml.attrib f "format" in + + let offset = ref 0 in + + (** Generating read function *) + let rec gen_read_macro = fun block_size f -> + match Xml.tag f with + "field" -> + let fn = field_name f + and fmt = format f in + begin + match test_type (format f) with + "fixe" -> + let block_no = if block_size = 0 then "" else ",_xsens_block" in + define (sprintf "XSENS_%s_%s(_xsens_payload%s)" msg_name fn block_no) (get_at !offset fmt block_size); + offset += sizeof fmt; + | "variable" -> fprintf out "/* XSENS_%s_%s: variable data size */\n" msg_name fn; + | _ -> offset += sizeof fmt; + end + | "block" -> + let s = int_of_string (Xml.attrib f "length") in + let o = !offset in + List.iter (gen_read_macro s) (Xml.children f); + let s' = !offset - o in + if s <> s' then raise (Length_error (f, s, s')) + | x -> failwith ("Unexpected field: " ^ x) + in + + (** Generating send function *) + let gen_send_macro = fun f -> + let param_name = fun f -> String.lowercase (field_name f) in + let rec param_names = fun f r -> + if Xml.tag f = "field" then + param_name f :: r + else + List.fold_right param_names (Xml.children f) r in + let param_type = fun f -> c_type (format f) in + let get_msg_length = fun f -> + try + Xml.attrib f "length" + with Xml.No_attribute("length") -> "0" in + fprintf out "\n#define XSENS_%s(" msg_name; + fprintf out "%s" (String.concat "," (param_names m [])); + fprintf out ") { \\\n"; + fprintf out " XsensHeader(UBX_%s_ID, %s);\\\n" msg_id (get_msg_length f); + let rec send_one_field = fun f -> + match Xml.tag f with + "field" -> + let s = sizeof (format f) in + let p = param_name f in + let t = param_type f in + fprintf out " %s _%s = %s; XsensSend%dByAddr((uint8_t*)&_%s);\\\n" t p p s p + | "block" -> + List.iter send_one_field (Xml.children f) + | _ -> assert (false) in + List.iter send_one_field (Xml.children m); + fprintf out " XsensTrailer();\\\n"; + fprintf out "}\n"; + offset += sizeof (format f) + in + + let gen_access_macro = fun m f -> + match Xml.attrib m "to" with + "MT" -> gen_send_macro f + | "host" -> gen_read_macro 0 f + | x -> failwith (sprintf "Unexpected direction: %s" x) + in + + List.iter (gen_access_macro m) (Xml.children m); + begin + try + let l = int_of_string (Xml.attrib m "length") in + if l <> !offset then raise (Length_error (m, l, !offset)) + with + Xml.No_attribute("length") -> () (** Undefined length authorized *) + end + + + +let parse_data = fun d -> + let data_name = Xml.attrib d "name" in + + fprintf out "\n"; + let data_length = sprintf "XSENS_DATA_%s_LENGTH" data_name in + define data_length (Xml.attrib d "length"); + + + let field_name = fun f -> ExtXml.attrib f "name" in + let format = fun f -> Xml.attrib f "format" in + + let offset = ref 0 in + + (** Generating read function *) + let gen_read_macro = fun f -> + match Xml.tag f with + "field" -> + let fn = field_name f + and fdt = format f in + begin + match test_type (format f) with + "fixe" -> + (* _xsens_block used as offset value *) + define (sprintf "XSENS_DATA_%s_%s(_xsens_payload,_xsens_block)" data_name fn) (get_at !offset fdt 1); + offset += sizeof fdt; + | "variable" -> failwith (sprintf "XSENS_%s_%s: variable data size" data_name fn); + | _ -> offset += sizeof fdt; + end + | x -> failwith ("Unexpected field: " ^ x) + in + + List.iter gen_read_macro (Xml.children d); + begin + try + let l = int_of_string (Xml.attrib d "length") in + if l <> !offset then raise (Length_error (d, l, !offset)) + with + Xml.No_attribute("length") -> () (** Undefined length authorized *) + end + + +let parse_message_and_data = fun m -> + match Xml.tag m with + "message" -> parse_message m + | "data" -> parse_data m + | x -> failwith (sprintf "Unexpected tag: %s" x) + + + +let _ = + if Array.length Sys.argv <> 2 then begin + failwith (sprintf "Usage: %s <.xml xsens protocol file>" Sys.argv.(0)) + end; + let xml_file = Sys.argv.(1) in + try + let xml = Xml.parse_file xml_file in + fprintf out "/* Generated from %s */\n" xml_file; + fprintf out "/* Please DO NOT EDIT */\n\n"; + + define "XSENS_START" "0xFA"; + define "XSENS_BID" "0xFF"; + fprintf out "\n"; + + List.iter (parse_message_and_data) (Xml.children xml) + with + Xml.Error (em, ep) -> + let l = Xml.line ep + and c1, c2 = Xml.range ep in + fprintf stderr "File \"%s\", line %d, characters %d-%d:\n" xml_file l c1 c2; + fprintf stderr "%s\n" (Xml.error_msg em); + exit 1 + | Length_error (m, l1, l2) -> + fprintf stderr "File \"%s\", inconsistent length: %d expected, %d found from fields in message:\n %s\n" xml_file l1 l2 (Xml.to_string_fmt m); + exit 1 + | Dtd.Parse_error e -> + fprintf stderr "File \"%s\", DTD parse error: %s\n" xml_file (Dtd.parse_error e) + | Dtd.Check_error e -> + fprintf stderr "File \"%s\", DTD check error: %s\n" xml_file (Dtd.check_error e) + | Dtd.Prove_error e -> + fprintf stderr "\nFile \"%s\", DTD prove error: %s\n\n" xml_file (Dtd.prove_error e)