generator of read and send macro for the xsens protocol

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

conf/xsens.dtd

@@ -1,21 +1,28 @@
 <!-- XSENS MTi protocol DTD -->
 
 <!ELEMENT xsens (message|data)+>
-<!ELEMENT message (field*)>
+<!ELEMENT message (field|block)*>
+<!ELEMENT block (field*)>
 <!ELEMENT field EMPTY>
+<!ELEMENT data (field)+>
 
 <!ATTLIST message
   name CDATA #REQUIRED
   ID CDATA #REQUIRED
-  to (MT,host) #REQUIRED
+  to (MT|host) #REQUIRED
   length CDATA #IMPLIED>
 
+<!ATTLIST block
+  times CDATA #IMPLIED
+  length CDATA #REQUIRED>
+
+<!ATTLIST field
+  name CDATA #REQUIRED
+  format CDATA #REQUIRED
+  scaling CDATA #IMPLIED
+  unit CDATA #IMPLIED>
+
 <!ATTLIST data
   name CDATA #REQUIRED
   length CDATA #REQUIRED>
 
-<!ATTLIST field
-  name CDATA #REQUIRED
-  format CDATA #IMPLIED
-  scaling CDATA #IMPLIED
-  unit CDATA #IMPLIED>

conf/xsens_MTi-G.xml

@@ -95,32 +95,39 @@
 
   <message name="ReqData" ID="0x34" to="MT"/>
   <message name="MTData" ID="0x32" to="host">
-    <field name="data"/>
+    <field name="data" format="V"/>
   </message>
 
   <message name="ReqHeading" ID="Ox82" to="MT"/>
   <message name="ReqHeadingAck" ID="0x83" to="host">
-    <field name="heading" format="F4" unit="rad"/>
+    <field name="heading" format="R4" unit="rad"/>
   </message>
 
-  <message name="SetHeading" ID="0x82" to="MT" length="1">
-    <field name="heading" format="F4" unit="rad"/>
+  <message name="SetHeading" ID="0x82" to="MT" length="4">
+    <field name="heading" format="R4" unit="rad"/>
   </message>
-  <message name="SetHeading" ID="0x83" to="host"/>
+  <message name="SetHeadingAck" ID="0x83" to="host"/>
 
   <message name="ReqMagneticDeclination" ID="0x6A" to="MT"/>
   <message name="ReqMagneticDeclinationAck" ID="0x6B" to="host">
-    <field name="declination" format="F4" unit="rad"/>
+    <field name="declination" format="R4" unit="rad"/>
   </message>
 
   <message name="SetMagneticDeclination" ID="0x6A" to="MT" length="4">
-    <field name="declination" format="F4" unit="rad"/>
+    <field name="declination" format="R4" unit="rad"/>
   </message>
   <message name="SetMagneticDeclinationAck" ID="0x6B" to="host"/>
 
   <message name="ReqGPSStatus" ID="0xA6" to="MT"/>
   <message name="GPSStatus" ID="0xA7" to="host">
-    <field name="status"/>
+    <field name="nch" format="U1"/>
+    <block times="nch" length="5">
+      <field name="chn" format="U1"/>
+      <field name="svid" format="U1"/>
+      <field name="bitmask" format="U1"/>
+      <field name="qi" format="I1"/>
+      <field name="cnr" format="I1"/>
+    </block>
   </message>
 
   <message name="ReqUTCTime" ID="0x60" to="MT"/>
@@ -158,44 +165,44 @@
   </data>
 
   <data name="Temp" length="4">
-    <field name="temp" format="F4"/>
+    <field name="temp" format="R4"/>
   </data>
 
   <data name="Calibrated" length="36">
-    <field name="accX" format="F4" unit="m/s/s"/>
-    <field name="accY" format="F4" unit="m/s/s"/>
-    <field name="accZ" format="F4" unit="m/s/s"/>
-    <field name="gyrX" format="F4" unit="rad/s"/>
-    <field name="gyrY" format="F4" unit="rad/s"/>
-    <field name="gyrZ" format="F4" unit="rad/s"/>
-    <field name="magX" format="F4"/>
-    <field name="magY" format="F4"/>
-    <field name="magZ" format="F4"/>
+    <field name="accX" format="R4" unit="m/s/s"/>
+    <field name="accY" format="R4" unit="m/s/s"/>
+    <field name="accZ" format="R4" unit="m/s/s"/>
+    <field name="gyrX" format="R4" unit="rad/s"/>
+    <field name="gyrY" format="R4" unit="rad/s"/>
+    <field name="gyrZ" format="R4" unit="rad/s"/>
+    <field name="magX" format="R4"/>
+    <field name="magY" format="R4"/>
+    <field name="magZ" format="R4"/>
   </data>
 
   <data name="Quaternion" length="16">
-    <field name="q0" format="F4"/>
-    <field name="q1" format="F4"/>
-    <field name="q2" format="F4"/>
-    <field name="q3" format="F4"/>
+    <field name="q0" format="R4"/>
+    <field name="q1" format="R4"/>
+    <field name="q2" format="R4"/>
+    <field name="q3" format="R4"/>
   </data>
 
   <data name="Euler" length="12">
-    <field name="roll" format="F4" unit="deg"/>
-    <field name="pitch" format="F4" unit="deg"/>
-    <field name="yaw" format="F4" unit="deg"/>
+    <field name="roll" format="R4" unit="deg"/>
+    <field name="pitch" format="R4" unit="deg"/>
+    <field name="yaw" format="R4" unit="deg"/>
   </data>
 
   <data name="Matrix" length="36">
-    <field name="a" format="F4"/>
-    <field name="b" format="F4"/>
-    <field name="c" format="F4"/>
-    <field name="d" format="F4"/>
-    <field name="e" format="F4"/>
-    <field name="f" format="F4"/>
-    <field name="g" format="F4"/>
-    <field name="h" format="F4"/>
-    <field name="i" format="F4"/>
+    <field name="a" format="R4"/>
+    <field name="b" format="R4"/>
+    <field name="c" format="R4"/>
+    <field name="d" format="R4"/>
+    <field name="e" format="R4"/>
+    <field name="f" format="R4"/>
+    <field name="g" format="R4"/>
+    <field name="h" format="R4"/>
+    <field name="i" format="R4"/>
   </data>
 
   <data name="Auxiliary" length="4">
@@ -204,15 +211,15 @@
   </data>
 
   <data name="Position" length="12">
-    <field name="lat" format="F4" unit="deg"/>
-    <field name="lon" format="F4" unit="deg"/>
-    <field name="alt" format="F4" unit="m"/>
+    <field name="lat" format="R4" unit="deg"/>
+    <field name="lon" format="R4" unit="deg"/>
+    <field name="alt" format="R4" unit="m"/>
   </data>
 
   <data name="Velocity" length="12">
-    <field name="vx" format="F4" unit="m/s"/>
-    <field name="vy" format="F4" unit="m/s"/>
-    <field name="vz" format="F4" unit="m/s"/>
+    <field name="vx" format="R4" unit="m/s"/>
+    <field name="vy" format="R4" unit="m/s"/>
+    <field name="vz" format="R4" unit="m/s"/>
   </data>
 
   <data name="Status" length="1">

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/%)

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)