diff --git a/Makefile.ac b/Makefile.ac
index 68749690ed..994160de11 100644
--- a/Makefile.ac
+++ b/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
-AP_DOWNLINK_H=$(ACINCLUDE)/ap_downlink_gen.h
+PERIODIC_H=$(ACINCLUDE)/periodic.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 $@
diff --git a/Makefile.gen b/Makefile.gen
index 1ccc4c1c3e..5286d77d69 100644
--- a/Makefile.gen
+++ b/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 $@
diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml
index 0bd6030f57..1c09255178 100644
--- a/conf/airframes/twinstar1.xml
+++ b/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
diff --git a/conf/telemetry/default.xml b/conf/telemetry/default.xml
index 0604b3e61b..e07a1dd079 100644
--- a/conf/telemetry/default.xml
+++ b/conf/telemetry/default.xml
@@ -1,6 +1,7 @@
+
-
+
@@ -25,18 +26,18 @@
-
+
-
+
-
+
-
\ No newline at end of file
+
diff --git a/conf/telemetry/telemetry.dtd b/conf/telemetry/telemetry.dtd
new file mode 100644
index 0000000000..69b1ada59b
--- /dev/null
+++ b/conf/telemetry/telemetry.dtd
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 469b936a66..c269381b9d 100644
--- a/sw/airborne/ap_downlink.h
+++ b/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
diff --git a/sw/airborne/autopilot_fbw.c b/sw/airborne/autopilot_fbw.c
deleted file mode 100644
index 80d7c5939c..0000000000
--- a/sw/airborne/autopilot_fbw.c
+++ /dev/null
@@ -1,4 +0,0 @@
-#include "autopilot_fbw.h"
-
-
-uint8_t autopilot_mode = MODE_FAILSAFE;
diff --git a/sw/airborne/autopilot_fbw.h b/sw/airborne/autopilot_fbw.h
deleted file mode 100644
index e0379ea3b2..0000000000
--- a/sw/airborne/autopilot_fbw.h
+++ /dev/null
@@ -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
-#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 */
diff --git a/sw/airborne/downlink.c b/sw/airborne/downlink.c
index c406a22c5a..865e333801 100644
--- a/sw/airborne/downlink.c
+++ b/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;
diff --git a/sw/airborne/fbw_downlink.h b/sw/airborne/fbw_downlink.h
index a4539ea362..64dc6e90e7 100644
--- a/sw/airborne/fbw_downlink.h
+++ b/sw/airborne/fbw_downlink.h
@@ -26,7 +26,8 @@
#define FBW_DOWNLINK_H
#include
-#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()
}
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 9d3d879cc3..fd87d292ff 100644
--- a/sw/airborne/main_ap.c
+++ b/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;
}
}
diff --git a/sw/tools/Makefile b/sw/tools/Makefile
index 30205d4810..d8e8223545 100644
--- a/sw/tools/Makefile
+++ b/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/%)
diff --git a/sw/tools/gen_aircraft.ml b/sw/tools/gen_aircraft.ml
index 840bf263b9..798996cb88 100644
--- a/sw/tools/gen_aircraft.ml
+++ b/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)
diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml
index e43f7bfc32..5bdf6c59d2 100644
--- a/sw/tools/gen_messages.ml
+++ b/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";
- Gen_onboard.gen_periodic avr_h messages
+ Printf.fprintf avr_h "#endif // DOWNLINK\n"
diff --git a/sw/tools/gen_periodic.ml b/sw/tools/gen_periodic.ml
new file mode 100644
index 0000000000..f724f70ec0
--- /dev/null
+++ b/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 " 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)
+