New datalink messages

Makefile.gen

@@ -29,12 +29,13 @@ XML_GET=sw/lib/ocaml/xml_get.out
 STATICINCLUDE =$(PAPARAZZI_HOME)/var/include
 MESSAGES_H=$(STATICINCLUDE)/messages.h
 UBX_PROTOCOL_H=$(STATICINCLUDE)/ubx_protocol.h
+DL_PROTOCOL_H=$(STATICINCLUDE)/dl_protocol.h
 MESSAGES_XML = $(CONF)/messages.xml
 UBX_XML = $(CONF)/ubx.xml
 FBW_MESSAGES_H=$(STATICINCLUDE)/fbw_messages.h
 
 
-static:  $(MESSAGES_H) $(UBX_PROTOCOL_H) $(FBW_MESSAGES_H)
+static:  $(MESSAGES_H) $(UBX_PROTOCOL_H) $(FBW_MESSAGES_H) $(DL_PROTOCOL_H)
 
 $(MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML)
 	test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE)
@@ -49,9 +50,13 @@ $(FBW_MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML)
         mv $$TMP_FILE $@
 	chmod a+r $@
 
-$(UBX_PROTOCOL_H) : $(UBX_XML) $(CONF_XML)
+$(UBX_PROTOCOL_H) : $(UBX_XML)
 	$(TOOLS)/gen_ubx.out $< > /tmp/ubx.h
 	mv /tmp/ubx.h $@
 
+$(DL_PROTOCOL_H) : $(MESSAGES_XML)
+	$(TOOLS)/gen_dl.out $< > /tmp/dl.h
+	mv /tmp/dl.h $@
+
 clean :
 	rm -f $(H_OF_XML)

conf/messages.xml

@@ -407,6 +407,15 @@
       <field name="segment2_east" type="float" unit="m"/>
       <field name="segment2_north" type="float" unit="m"/>
   </message>
+
+ <message name="MOVE_WAYPOINT" ID="24">
+  <field name="ac_id" type="string"/>
+  <field name="wp_id" type="uint8"/>
+  <field name="utm_east"  type="float" unit="m"></field>
+  <field name="utm_north"  type="float" unit="m"></field>
+  <field name="alt"    type="float" unit="m"></field>
+ </message>
+
 </class>
 
 <class name="alert">
@@ -417,4 +426,24 @@
   </message>
 </class>
 
+<class name="datalink">
+ <message name="ACINFO" ID="1">
+  <field name="ac_id" type="uint8"/>
+  <field name="utm_east"  type="int32" unit="cm"></field>
+  <field name="utm_north"  type="int32" unit="cm"></field>
+  <field name="course" type="int16" unit="decideg"></field>
+  <field name="alt"    type="int32" unit="cm"></field>
+  <field name="speed"  type="uint16" unit="cm/s"></field>
+  <field name="climb"  type="int16" unit="cm/s"></field>
+  <field name="itow"  type="uint32" unit="ms"></field>
+ </message>
+ <message name="MOVE_WP" ID="2">
+  <field name="ac_id" type="uint8"/>
+  <field name="wp_id" type="uint8"/>
+  <field name="utm_east"  type="int32" unit="cm"></field>
+  <field name="utm_north"  type="int32" unit="cm"></field>
+  <field name="alt"    type="int32" unit="cm"></field>
+ </message>
+</class>
+
 </protocol>

sw/tools/Makefile

@@ -3,7 +3,7 @@ OCAMLC=ocamlc -I ../lib/ocaml
 OCAMLLEX=ocamllex
 OCAMLYACC=ocamlyacc
 
-all: gen_aircraft.out gen_airframe.out gen_calib.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out gen_sim_downlink.out
+all: gen_aircraft.out gen_airframe.out gen_calib.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out gen_sim_downlink.out gen_dl.out
 
 FP_CMO = fp_syntax.cmo fp_parser.cmo fp_lexer.cmo fp_proc.cmo gen_flight_plan.ml
 ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%)

sw/tools/gen_dl.ml

@@ -0,0 +1,102 @@
+(*
+ * $Id$
+ *
+ * XML preprocessing for datalink 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
+
+let sizeof = function
+    "int32" | "uint32" -> 4
+  | "int16" | "uint16" -> 2
+  | "int8" | "uint8" -> 1
+  | x -> failwith (sprintf "sizeof: unknown format '%s'" x)
+
+let (+=) = fun r x -> r := !r + x
+
+let get_at = fun offset format ->
+  let t = 
+    match format with
+      "int16" -> "int16_t"
+    | "int32" -> "int32_t"
+    | "uint16" -> "uint16_t"
+    | "uint32" -> "uint32_t"
+    | "uint8" -> "uint8_t"
+    | "int8" -> "int8_t"
+    | _ -> failwith (sprintf "get_at: unknown format '%s'" format) in
+  sprintf "(*((%s*)(_ubx_payload+%d)))" t offset
+
+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";
+  define (sprintf "DL_%s_ID" msg_name) (Xml.attrib m "ID");
+  
+  let offset = ref 0 in
+  let rec parse_field = fun f ->
+    match Xml.tag f with
+      "field" ->
+	let field_name = Xml.attrib f "name"
+	and format = Xml.attrib f "type" in
+	define (sprintf "DL_%s_%s(_ubx_payload)" msg_name field_name) (get_at !offset format);
+	offset += sizeof format
+    | x -> failwith ("Unexpected field: " ^ x)
+    in
+
+    List.iter parse_field (Xml.children m)
+  
+
+let _ =
+  if Array.length Sys.argv <> 2 then begin
+    failwith (sprintf "Usage: %s <.xml message file>" Sys.argv.(0)) 
+  end;
+  let xml_file = Sys.argv.(1) in
+  try
+    let xml = Xml.parse_file xml_file in
+    let xml_class = ExtXml.child xml ~select:(fun m -> ExtXml.attrib m "name" = "datalink") "class" in
+    fprintf out "/* Generated from %s */\n" xml_file;
+    fprintf out "/* Please DO NOT EDIT */\n\n";
+    
+    List.iter parse_message (Xml.children xml_class)
+  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.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 check error: %s\n\n" xml_file (Dtd.prove_error e)