*** empty log message ***

Makefile.ac

@@ -1,3 +1,4 @@
+# Hey Emacs, this is a -*- makefile -*-
 #   Paparazzi main $Id$
 #   Copyright (C) 2004 Pascal Brisset Antoine Drouin
 #
@@ -29,15 +30,14 @@ ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)
 AIRFRAME_H=$(ACINCLUDE)/airframe.h
 CONTROL_H=$(ACINCLUDE)/control.h
 CONTROL_C=$(ACINCLUDE)/ap/control.c
-FBW_DOWNLINK_H=$(ACINCLUDE)/fbw_downlink_gen.h
+PERIODIC_H=$(ACINCLUDE)/periodic.h
-AP_DOWNLINK_H=$(ACINCLUDE)/ap_downlink_gen.h
 RADIO_H=$(ACINCLUDE)/radio.h
 FLIGHT_PLAN_H=$(ACINCLUDE)/flight_plan.h
 FLIGHT_PLAN_XML=$(ACINCLUDE)/flight_plan.xml
 INFLIGHT_CALIB_H=$(ACINCLUDE)/inflight_calib.h
 MAKEFILE_AC=$(ACINCLUDE)/Makefile.ac
 
-all: $(AIRFRAME_H) $(RADIO_H) $(CONTROL_H) $(CONTROL_C) $(FLIGHT_PLAN_H) $(FLIGHT_PLAN_XML) $(INFLIGHT_CALIB_H) $(MAKEFILE_AC) $(FBW_DOWNLINK_H)
+all: $(AIRFRAME_H) $(RADIO_H) $(CONTROL_H) $(CONTROL_C) $(FLIGHT_PLAN_H) $(FLIGHT_PLAN_XML) $(INFLIGHT_CALIB_H) $(MAKEFILE_AC) $(PERIODIC_H)
 
 
 $(AIRFRAME_H) : $(CONF)/$(AIRFRAME) $(CONF_XML)
@@ -48,9 +48,9 @@ $(RADIO_H) : $(CONF)/$(RADIO) $(CONF_XML)
 	$(TOOLS)/gen_radio.out $< > /tmp/radio.h
 	mv /tmp/radio.h $@
 
-$(FBW_DOWNLINK_H) : $(MESSAGES_XML) $(CONF)/$(AIRFRAME) $(CONF_XML)
+$(PERIODIC_H) : $(MESSAGES_XML) $(CONF_XML) $(CONF)/$(TELEMETRY)
 	TMP_FILE=`mktemp`;\
-	$(TOOLS)/gen_messages.out $< telemetry_fbw > $$TMP_FILE;\
+	$(TOOLS)/gen_periodic.out $(MESSAGES_XML) $(CONF)/$(TELEMETRY) > $$TMP_FILE;\
 	mv  $$TMP_FILE $@
 	chmod a+r $@
 

Makefile.gen

@@ -36,7 +36,7 @@ DL_PROTOCOL_H=$(STATICINCLUDE)/dl_protocol.h
 MESSAGES_XML = $(CONF)/messages.xml
 UBX_XML = $(CONF)/ubx.xml
 
-static:  $(MESSAGES_H) $(MESSAGES_FBW_H) $(UBX_PROTOCOL_H) $(DL_PROTOCOL_H)
+static:  $(MESSAGES_H) $(UBX_PROTOCOL_H) $(DL_PROTOCOL_H)
 
 $(MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML)
 	test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE)
@@ -45,13 +45,6 @@ $(MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML)
 	mv  $$TMP_FILE $@
 	chmod a+r $@
 
-$(MESSAGES_FBW_H) : $(MESSAGES_XML) $(CONF_XML)
-	test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE)
-	TMP_FILE=`mktemp`;\
-	$(TOOLS)/gen_messages.out $< telemetry > $$TMP_FILE;\
-	mv  $$TMP_FILE $@
-	chmod a+r $@
-
 $(UBX_PROTOCOL_H) : $(UBX_XML)
 	$(TOOLS)/gen_ubx.out $< > /tmp/ubx.h
 	mv /tmp/ubx.h $@

conf/airframes/twinstar1.xml

@@ -120,7 +120,7 @@ fbw.CFLAGS += -DRADIO_CONTROL
 fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c 
 
 fbw.CFLAGS += -DDOWNLINK -DUART0 
-fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c
+fbw.srcs += downlink.c $(SRC_ARCH)/uart_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 
 ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c

conf/telemetry/default.xml

@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
+<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
 <telemetry>
-  <process name="ap">
+  <process name="Ap">
     <mode name="default">
       <message name="CALIB_START"    period="1"/> 
       <message name="CALIB_CONTRAST" period="1"/>
@@ -25,18 +26,18 @@
       <message name="ATTITUDE"       period="0.5"/>
     </mode>
   </process>
-  <process name="fbw">
+  <process name="Fbw">
     <mode name="default">
       <message name="PPM"            period="5"/>
       <message name="RC"             period="5"/>
       <message name="SERVOS"         period="5"/>
-      <message name="FBW_STATUS"     period="1">
+      <message name="FBW_STATUS"     period="1"/>
     </mode>
     <mode name="debug">
       <message name="PPM"            period="0.5"/>
       <message name="RC"             period="0.5"/>
       <message name="SERVOS"         period="0.5"/>
-      <message name="FBW_STATUS"     period="1">
+      <message name="FBW_STATUS"     period="1"/>
     </mode>
   </process>
-</telemetry>
+</telemetry>

conf/telemetry/telemetry.dtd

@@ -0,0 +1,16 @@
+<!ELEMENT telemetry (process+)>
+<!ELEMENT process (mode+)>
+<!ELEMENT mode (message+)>
+<!ELEMENT message EMPTY>
+
+<!ATTLIST telemetry>
+<!ATTLIST process
+  name CDATA #REQUIRED
+>
+<!ATTLIST mode
+  name CDATA #REQUIRED
+>
+<!ATTLIST message
+  name CDATA #REQUIRED
+  period CDATA #REQUIRED
+>

sw/airborne/ap_downlink.h

@@ -30,6 +30,7 @@
 #include "airframe.h"
 
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+extern uint8_t telemetry_mode_Ap;
 #include "downlink.h"
 
 #ifdef SITL
@@ -37,6 +38,7 @@
 #else
 #include "modem.h"
 #include "messages.h"
+#include "periodic.h"
 #endif
 
 #if DOWNLINK

sw/airborne/autopilot_fbw.c

@@ -1,4 +0,0 @@
-#include "autopilot_fbw.h"
-
-
-uint8_t autopilot_mode = MODE_FAILSAFE;

sw/airborne/autopilot_fbw.h

@@ -1,62 +0,0 @@
-/*
- * $Id$
- *  
- * 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. 
- *
- */
-
-/** \file autopilot_fbw.h
- *  \brief Autopilot modes
- *
- */
-
-#ifndef AUTOPILOT_FBW_H
-#define AUTOPILOT_FBW_H
-
-#include <inttypes.h>
-#include "std.h"
-#include "led.h"
-#include "paparazzi.h"
-
-#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
-
-#define MODE_MANUAL   0
-#define MODE_AUTO     1
-#define MODE_FAILSAFE 2
-#define MODE_NB       3
-
-#define MODE_OF_PPRZ(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? MODE_MANUAL : MODE_AUTO)
-
-extern uint8_t autopilot_mode;
-
-#ifdef RADIO_CONTROL
-#include "radio_control.h"
-static inline void autopilot_process_radio_control ( void ) {
-  autopilot_mode = MODE_OF_PPRZ(rc_values[RADIO_MODE]);
-#ifdef LED
-  if (autopilot_mode == MODE_MANUAL)
-    LED_ON(2);
-  else
-    LED_OFF(2);
-#endif /* LED */
-}
-#endif /* RADIO_CONTROL */
-
-#endif /* AUTOPILOT_FBW_H */

sw/airborne/downlink.c

@@ -2,3 +2,6 @@
 
 uint8_t ck_a, ck_b;
 uint8_t downlink_nb_ovrn;
+
+uint8_t telemetry_mode_Fbw;
+uint8_t telemetry_mode_Ap;

sw/airborne/fbw_downlink.h

@@ -26,7 +26,8 @@
 #define FBW_DOWNLINK_H
 
 #include <inttypes.h>
-#include "messages_fbw.h"
+#include "messages.h"
+#include "periodic.h"
 #include "airframe.h"
 
 #include "uart.h"
@@ -34,6 +35,7 @@
 #include "radio_control.h"
 
 #define DOWNLINK_DEVICE DOWNLINK_FBW_DEVICE
+extern uint8_t telemetry_mode_Fbw;
 #include "downlink.h"
 
 #define PERIODIC_SEND_PPM() {}
@@ -44,7 +46,7 @@
 
 
 static inline void fbw_downlink_periodic_task(void) {
-  PeriodicSend()
+  PeriodicSendFbw()
 }
 
 

sw/airborne/main_ap.c

@@ -214,14 +214,13 @@ static inline void reporting_task( void ) {
   /** initialisation phase during boot */
   if (boot) {
     DOWNLINK_SEND_BOOT(&version);
-    PERIODIC_SEND_IDENT();
     SEND_RAD_OF_IR();
     boot = FALSE;
   }
   /** then report periodicly */
   else {
 //    IO1CLR = LED_2_BIT;
-    PeriodicSend();
+    PeriodicSendAp();
 //    IO1SET = LED_2_BIT;
   }
 }

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 gen_dl.out extract_makefile.out gen_control.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 extract_makefile.out gen_control.out gen_periodic.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_aircraft.ml

@@ -42,6 +42,6 @@ let _ =
   mkdir (aircraft_dir // "autopilot");
   mkdir (aircraft_dir // "sim");
   
-  let c = sprintf "make -f Makefile.ac AIRCRAFT=%s AC_ID=%s AIRFRAME=%s RADIO=%s FLIGHT_PLAN=%s" aircraft (value "ac_id") (value "airframe") (value "radio") (value "flight_plan") in
+  let c = sprintf "make -f Makefile.ac AIRCRAFT=%s AC_ID=%s AIRFRAME=%s RADIO=%s FLIGHT_PLAN=%s TELEMETRY=%s" aircraft (value "ac_id") (value "airframe") (value "radio") (value "flight_plan") (value "telemetry") in
   prerr_endline c;
   exit (Sys.command c)

sw/tools/gen_messages.ml

@@ -26,8 +26,6 @@
 
 open Printf
 
-let divide = fun a b -> b mod a = 0
-
 
 module Syntax = struct
   type format = string
@@ -194,125 +192,6 @@ module Gen_onboard = struct
   let print_null_avr_macros = fun avr_h messages ->
     List.iter (print_null_avr_macro avr_h) messages
 
-  let freq = 10
-  let step = 1. /. float freq
-  let nb_steps = (256 / freq) * freq
-  let byte_rate = 480 (** byte/s *)
-  let step_rate = byte_rate / freq
-
-  let is_periodic = fun m -> m.period <> None
-  let period_of = fun m ->
-    match m.period with Some p -> p | None -> failwith "period_of"
-  let morefrequent = fun m1 m2 -> compare (period_of m1) (period_of m2)
-
-  type modulos = node list list (** Or list of Xor lists *)
-  and node = N of int * int * message list * modulos
-      (** [(p, s, msgs, c)] stands for messages [msg] to be send on a period [p]
-	 shifted [s] from the start of the [p] period (i.e. 
-	 when [time == s mod p]). Children [c] are messages for which the same
-	 time condition is needed *)
-      
-  let rec insert_in_or = fun ((s, p, m) as msg) t ->
-    match t with
-      [] -> [[N (s,p,[m], [])]]
-    | l::ls ->
-	let rec insert_in_xor = fun previous exclusive l ->
-	  match l with
-            [] -> 
-              if exclusive then
-		(N (s,p,[m], [])::previous)::ls
-              else
-		previous::insert_in_or msg ls
-	  | (N (s',p',ms',t') as x)::ls' ->
-              if s = s' && p == p' then
-		(previous@N (s', p', m::ms', t')::ls')::ls
-              else
-		let b = divide p' p in
-		if b && divide p' (abs (s-s')) then
-		  (previous@N (s', p', ms', insert_in_or msg t')::ls')::ls
-		else
-		  insert_in_xor (x::previous) (exclusive && b) ls' in
-	insert_in_xor [] true l;;
-
-let gen_periodic = fun avr_h messages ->
-  let indent = fun x -> fprintf avr_h "%s" (String.make x ' ') in
-
-  let periodic_messages = List.filter is_periodic messages in
-  let periodic_messages = List.sort morefrequent periodic_messages in
-  
-  let load = Array.create nb_steps 0 in
-  
-  let scheduled_messages = 
-    List.map
-      (fun m ->
-	let p = period_of m in
-	let period_steps = truncate (p /. step) in
-	let start_step = ref 0 in
-	for i = 1 to period_steps - 1 do
-	  if load.(i) < load.(!start_step) then start_step := i
-	done;
-	for j = 0 to nb_steps/period_steps - 1 do
-	  let s = ref (size_of_message m) in
-	  let i = ref (!start_step+j*period_steps) in
-	  while !s > 0 do
-	    let rest = step_rate - load.(!i) in
-	    let m = min rest !s in
-	    load.(!i) <- load.(!i) + m;
-	    incr i;
-	    s := !s - m
-	  done
-	done;
-	(!start_step, period_steps, m))
-      periodic_messages in
-  
-  fprintf avr_h "/* Periodic messages are sent over %d timeframes. A greedy algorithm is used to smooth the load according to the size of the messages. speed=%d byte/timeframe */\n" nb_steps step_rate;
-  for i = 0 to nb_steps - 1 do
-    fprintf avr_h "//%d:" i; indent load.(i); fprintf avr_h "%d\n" load.(i)
-  done;
-  fprintf avr_h "\n";
-  
-  fprintf avr_h "#define PeriodicSend() {  /* %dHz */ \\\n" freq;
-  fprintf avr_h "  static uint8_t periodic_i;\\\n";
-  fprintf avr_h "  periodic_i++; if (periodic_i == %d) periodic_i = 0;\\\n" nb_steps;
-
-  (** Sorting messages by increasing periods *)
-  let scheduled_messages = List.sort (fun (_,p,_) (_,p',_) -> compare p' p) scheduled_messages in
-
-  (** Hierarchize messages to minimize tests *)
-  let tree = List.fold_right insert_in_or scheduled_messages [] in
-  
-  let rec gen_or_code = fun tab modulos xors ->
-    List.iter
-      (fun xor ->
-	let required_modulos = List.map (fun (N (_s,p,_ms,_t)) -> p) xor in
-	let rec add_new_modulos = fun modulos l ->
-	  match l with
-	    [] -> modulos
-	  | p::ps ->
-	      if not (List.mem p modulos) then begin
-		indent tab;
-		fprintf avr_h "uint8_t i_mod_%d = periodic_i %% %d; \\\n" p p;
-		add_new_modulos (p::modulos) ps
-	      end else
-		add_new_modulos modulos ps in
-	let new_modulos = add_new_modulos modulos required_modulos in
-	List.iter 
-	  (fun (N (s,p,ms,t)) ->
-	    indent tab;
-	    fprintf avr_h "if (i_mod_%d == %d) { \\\n" p s;
-	    List.iter (fun m -> 
-	      indent tab;
-	      fprintf avr_h "  PERIODIC_SEND_%s(); \\\n" m.name) 
-	      ms;
-	    gen_or_code (tab+2) new_modulos t;
-	    indent tab;
-	    fprintf avr_h "} else")
-	  xor;
-	indent tab;
-	fprintf avr_h " {} \\\n")
-      xors in
-  gen_or_code 2 [] tree;
-  fprintf avr_h "}\n"
 end
 
 let _ =
@@ -331,5 +210,4 @@ let _ =
   Gen_onboard.print_avr_macros filename avr_h messages;
   Printf.fprintf avr_h "#else // DOWNLINK\n";
   Gen_onboard.print_null_avr_macros avr_h messages;
-  Printf.fprintf avr_h "#endif // DOWNLINK\n";
+  Printf.fprintf avr_h "#endif // DOWNLINK\n"
-  Gen_onboard.gen_periodic avr_h messages

sw/tools/gen_periodic.ml

@@ -0,0 +1,131 @@
+(*
+ * $Id$
+ *
+ * XML preprocessing for periodic messages
+ *  
+ * 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 margin = ref 0
+let step = 2
+
+let right () = margin := !margin + step
+let left () = margin := !margin - step
+
+let lprintf = fun c f ->
+  fprintf c "%s" (String.make !margin ' ');
+  fprintf c f
+
+let freq = 10
+let nb_steps = 100
+
+let remove_dup = fun l ->
+  let rec loop = fun l ->
+    match l with
+      [] | [_] -> l
+    | x::((x'::_) as xs) ->
+	if x = x' then loop xs else x::loop xs in
+  loop (List.sort compare l)
+
+let _ =
+  if Array.length Sys.argv <> 3 then begin
+    failwith (sprintf "Usage: %s <messages.xml> <telemetry.xml>" Sys.argv.(0)) 
+  end;
+
+  let messages_xml = Xml.parse_file Sys.argv.(1) in
+  let telemetry_xml = 
+    try
+      Xml.parse_file Sys.argv.(2)
+    with Dtd.Check_error e -> failwith (Dtd.check_error e)
+      
+  in
+
+  let avr_h = stdout in
+
+  fprintf avr_h "/* This file has been generated from %s and %s */\n" Sys.argv.(1) Sys.argv.(2);
+  fprintf avr_h "/* Please DO NOT EDIT */\n\n";
+  
+  fprintf avr_h "/* Periodic messages are sent over %d timeframes */\n" nb_steps;
+
+  (** For each process *)
+  List.iter
+    (fun process ->
+      let process_name = ExtXml.attrib process "name" in
+
+      fprintf avr_h "\n/* Macros for %s process */\n" process_name;
+
+      let modes = Xml.children process in
+      
+      let i = ref 0 in
+      List.iter (fun mode ->
+	let name = ExtXml.attrib mode "name" in
+	Xml2h.define (sprintf "TELEMETRY_MODE_%s_%s" process_name name) (string_of_int !i);
+	incr i)
+	modes;
+
+      lprintf avr_h "#define PeriodicSend%s() {  /* %dHz */ \\\n" process_name freq;
+      right ();
+      lprintf avr_h "static uint8_t periodic_i;\\\n";
+      lprintf avr_h "periodic_i++; if (periodic_i == %d) periodic_i = 0;\\\n" nb_steps;
+      
+      (** For each mode in this process *)
+      List.iter
+	(fun mode ->
+	  let mode_name = ExtXml.attrib mode "name" in
+	  lprintf avr_h "if (telemetry_mode_%s == TELEMETRY_MODE_%s_%s) {\\\n" process_name process_name mode_name;
+	  right ();
+
+	  (** Computes the required modulos *)
+	  let messages =
+	    List.map
+	      (fun x -> 
+		let p = float_of_string (ExtXml.attrib x "period") in
+		x, int_of_float (p*.float_of_int freq))
+	      (Xml.children mode) in
+	  let modulos = remove_dup (List.map snd messages) in
+	  List.iter
+	    (fun m ->
+	      lprintf avr_h "uint8_t i_mod_%d = periodic_i %% %d; \\\n" m m;
+	    )
+	    modulos;
+	  
+	  
+	  (** For each message in this mode *)
+	  List.iter
+	    (fun (message, p) ->
+	      assert(p < nb_steps);
+	      let message_name = ExtXml.attrib message "name" in
+	      lprintf avr_h "if (i_mod_%d == 0) {\\\n" p;
+	      right ();
+	      lprintf avr_h "PERIODIC_SEND_%s();\\\n" message_name;
+	      left ();
+	      lprintf avr_h "}\\\n")
+	    messages;
+	  left ();
+	  lprintf avr_h "}\\\n")
+	modes;
+      left ();
+      lprintf avr_h "}\n"
+    )
+    (Xml.children telemetry_xml)
+