new "JUMP_TO_BLOCK" message, fixes for AVR

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

conf/messages.xml

@@ -449,7 +449,7 @@
 
  <message name="JUMP_TO_BLOCK" ID="27">
   <field name="ac_id" type="string"/>
-  <field name="block_name" type="string"/>
+  <field name="block_id" type="uint8"/>
  </message>
 
 </class>
@@ -494,6 +494,9 @@
   <field name="value" type="float"/>
  </message>
 
+ <message name="BLOCK" ID="5">
+  <field name="block_id" type="uint8"/>
+ </message>
 
 </class>
 

sw/airborne/autopilot.h

@@ -33,11 +33,12 @@
 //#include "inter_mcu.h"
 #include <inttypes.h>
 #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;

sw/airborne/avr/servos_4017.h

@@ -0,0 +1 @@
+#define SERVOS_TICS_OF_USEC(_us) SYS_TICS_OF_USEC(_us)

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[];
 

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));
   }
 }

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"

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

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"
 
 

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 */

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

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

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) {

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 *)

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)