diff --git a/conf/autopilot/twin_avr.makefile b/conf/autopilot/twin_avr.makefile
index 70254d38e7..39430319fa 100644
--- a/conf/autopilot/twin_avr.makefile
+++ b/conf/autopilot/twin_avr.makefile
@@ -19,5 +19,5 @@ fbw.LOW_FUSE = 2e
fbw.HIGH_FUSE = cb
fbw.EXT_FUSE = ff
fbw.LOCK_FUSE = ff
-fbw.CFLAGS += -DFBW -DMCU_SPI_LINK
+fbw.CFLAGS += -DFBW -DMCU_SPI_LINK -DACTUATORS=\"servos_4017.h\"
fbw.srcs = $(SRC_ARCH)/ppm_hw.c inter_mcu.c $(SRC_ARCH)/servo.c $(SRC_ARCH)/spi_fbw.c $(SRC_ARCH)/uart_fbw.c $(SRC_ARCH)/adc_fbw.c sys_time.c main_fbw.c main.c
diff --git a/conf/messages.xml b/conf/messages.xml
index fffe70f672..bb29aa93ea 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -449,7 +449,7 @@
-
+
@@ -494,6 +494,9 @@
+
+
+
diff --git a/sw/airborne/autopilot.h b/sw/airborne/autopilot.h
index c86ffba376..ba7948b8bd 100644
--- a/sw/airborne/autopilot.h
+++ b/sw/airborne/autopilot.h
@@ -33,11 +33,12 @@
//#include "inter_mcu.h"
#include
#include "std.h"
+#include "sys_time.h"
#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
#define TRESHOLD1 TRESHOLD_MANUAL_PPRZ
-#define TRESHOLD2 CLOCK_OF_US(200)
+#define TRESHOLD2 SYS_TICS_OF_USEC(200)
#define PPRZ_MODE_MANUAL 0
@@ -48,7 +49,7 @@
#define PPRZ_MODE_NB 5
#define PPRZ_MODE_OF_PULSE(pprz, mega8_status) \
- (pprz > TRESHOLD2 ? PPRZ_MODE_AUTO2 : \
+ ((uint16_t)pprz > TRESHOLD2 ? PPRZ_MODE_AUTO2 : \
(pprz > TRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL))
extern uint8_t pprz_mode;
diff --git a/sw/airborne/avr/servos_4017.h b/sw/airborne/avr/servos_4017.h
new file mode 100644
index 0000000000..e678f2e76f
--- /dev/null
+++ b/sw/airborne/avr/servos_4017.h
@@ -0,0 +1 @@
+#define SERVOS_TICS_OF_USEC(_us) SYS_TICS_OF_USEC(_us)
diff --git a/sw/airborne/command.h b/sw/airborne/command.h
index 030604d038..f2459eaeb7 100644
--- a/sw/airborne/command.h
+++ b/sw/airborne/command.h
@@ -5,7 +5,7 @@
#include "paparazzi.h"
-//extern void command_init( void );
+extern void command_init( void );
extern void command_set(const pprz_t values[]);
extern const pprz_t failsafe_values[];
diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c
index e85318ae71..a233964ac4 100644
--- a/sw/airborne/datalink.c
+++ b/sw/airborne/datalink.c
@@ -65,5 +65,7 @@ void dl_parse_msg(void) {
}
} else if (msg_id == DL_SETTING_ID) {
DlSetting(DL_SETTING_index(dl_buffer), DL_SETTING_value(dl_buffer));
+ } else if (msg_id == DL_BLOCK_ID) {
+ nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
}
diff --git a/sw/airborne/mainloop.c b/sw/airborne/mainloop.c
index fbfe6edda0..31e44718c8 100644
--- a/sw/airborne/mainloop.c
+++ b/sw/airborne/mainloop.c
@@ -39,6 +39,7 @@
#include "wavecard.h"
#include "downlink.h"
#include "led.h"
+#include "inter_mcu.h"
#ifdef MCU_SPI_LINK /** ap alone, using SPI to communicate with fbw */
#include "spi_ap.h"
diff --git a/sw/airborne/modem.h b/sw/airborne/modem.h
index 04e428a356..98791a1891 100644
--- a/sw/airborne/modem.h
+++ b/sw/airborne/modem.h
@@ -1,4 +1,6 @@
/*
+ * $Id$
+ *
* Paparazzi mcu0 cmx469 modem functions
*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
@@ -27,7 +29,7 @@
#include "inttypes.h"
-//void modem_init( void );
+void modem_init( void );
extern uint8_t modem_nb_ovrn;
#ifdef MODEM
diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c
index 7c1a4e42e5..373ad30dfc 100644
--- a/sw/airborne/nav.c
+++ b/sw/airborne/nav.c
@@ -172,6 +172,10 @@ static float qdr;
fly_to_xy(ac->east - _distance*cos(alpha), ac->north - _distance*sin(alpha)); \
}
+void nav_goto_block(uint8_t b) {
+ GotoBlock(b);
+}
+
#include "flight_plan.h"
diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h
index 6f2f73b574..20415086b8 100644
--- a/sw/airborne/nav.h
+++ b/sw/airborne/nav.h
@@ -110,6 +110,6 @@ void nav_home(void);
void nav_init(void);
void nav_without_gps(void);
-
+extern void nav_goto_block(uint8_t block_id);
#endif /* NAV_H */
diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile
index c4c2322d3d..b29bc431b9 100644
--- a/sw/ground_segment/tmtc/Makefile
+++ b/sw/ground_segment/tmtc/Makefile
@@ -64,6 +64,9 @@ server : aircraft.cmo wind.cmo airprox.cmo server.cmo
wavecard : wavecard_connect.cmo
$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^
+dl_setting : dl_setting.cmo
+ $(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma gtkInit.cmo $^
+
modem.cmo : modem.cmi
%.cmo : %.ml
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index 3fa71db10c..b8fcceee15 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -39,7 +39,7 @@ SIMSA=sims.cma
OCAMLC = ocamlc
OCAMLOPT=ocamlopt -p
INCLUDES= -I +lablgtk2 -I ../lib/ocaml
-OCAMLCC = gcc -O2 -I /usr/include/glib-2.0 -I /usr/lib/glib-2.0/include -DUBX -DMOBILE_CAM -DAP -DDOWNLINK -DSITL -DINFRARED -DGPS -I $(OBJDIR) -I $(ACDIR)
+OCAMLCC = gcc -Werror -O2 -I /usr/include/glib-2.0 -I /usr/lib/glib-2.0/include -DUBX -DMOBILE_CAM -DAP -DDOWNLINK -DSITL -DINFRARED -DGPS -I $(OBJDIR) -I $(ACDIR)
AIRBORNE = ../airborne
VARINCLUDE=$(PAPARAZZI_HOME)/var/include
diff --git a/sw/simulator/sim_ap.c b/sw/simulator/sim_ap.c
index 224ad3ff12..fa56818530 100644
--- a/sw/simulator/sim_ap.c
+++ b/sw/simulator/sim_ap.c
@@ -137,6 +137,11 @@ value move_waypoint(value wp_id, value ux, value uy, value a) {
return Val_unit;
}
+value goto_block(value block_id) {
+ nav_goto_block(Int_val(block_id));
+ return Val_unit;
+}
+
value send_event(value event_id) {
uint8_t event = Int_val(event_id);
switch (event) {
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index b81ff2440c..c1609e3878 100644
--- a/sw/simulator/sitl.ml
+++ b/sw/simulator/sitl.ml
@@ -162,6 +162,13 @@ module Make(A:Data.MISSION) = struct
send_event (Pprz.int_assoc "event_id" vs)
+ external goto_block : int -> unit = "goto_block"
+ let get_jump_to_block = fun _sender vs ->
+ let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+ if ac_id = !my_id then
+ goto_block (Pprz.int_assoc "block_id" vs)
+
+
external dl_setting : int -> float -> unit = "dl_setting"
let get_dl_setting = fun _sender vs ->
let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
@@ -176,6 +183,7 @@ module Make(A:Data.MISSION) = struct
ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" get_flight_param);
ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" get_move_waypoint);
ignore (Ground_Pprz.message_bind "SEND_EVENT" get_send_event);
+ ignore (Ground_Pprz.message_bind "JUMP_TO_BLOCK" get_jump_to_block);
ignore (Ground_Pprz.message_bind "DL_SETTING" get_dl_setting)
(* Functions called by the simulator *)
diff --git a/sw/simulator/sys_time_hw.h b/sw/simulator/sys_time_hw.h
index 6983c693a2..a193503ec7 100644
--- a/sw/simulator/sys_time_hw.h
+++ b/sw/simulator/sys_time_hw.h
@@ -1,3 +1,3 @@
-#define CLOCK_OF_US(x) (x)
+#define SYS_TICS_OF_USEC(x) (x)
#define US_OF_CLOCK(x) (x)
-#define SERVOS_TICS_OF_USEC(s) CLOCK_OF_US(s)
+#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s)