diff --git a/conf/airframes/booz.xml b/conf/airframes/booz.xml
new file mode 100644
index 0000000000..0c2c6dc346
--- /dev/null
+++ b/conf/airframes/booz.xml
@@ -0,0 +1,68 @@
+
+
+
+
+ARCHI=arm7
+
+FLASH_MODE = IAP
+
+ctl.ARCHDIR = $(ARCHI)
+ctl.ARCH = arm7tdmi
+ctl.TARGET = ctl
+ctl.TARGETDIR = ctl
+
+ctl.CFLAGS += -DCONTROLLER -DCONFIG=\"conf_booz.h\"
+ctl.srcs = main_booz.c
+
+ctl.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(5e-3)'
+ctl.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+ctl.CFLAGS += -DLED
+
+ctl.srcs += $(SRC_ARCH)/armVIC.c
+
+ctl.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
+ctl.srcs += $(SRC_ARCH)/uart_hw.c
+
+ctl.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
+ctl.srcs += downlink.c pprz_transport.c
+
+ctl.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
+
+ctl.srcs += link_imu.c
+
+
+ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
+ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
+
+imu.ARCHDIR = $(ARCHI)
+imu.ARCH = arm7tdmi
+imu.TARGET = imu
+imu.TARGETDIR = imu
+
+imu.CFLAGS += -DIMU -DCONFIG=\"pprz_imu.h\"
+imu.srcs = main_imu.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
+
+imu.CFLAGS += -DLED
+
+imu.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
+imu.srcs += $(SRC_ARCH)/uart_hw.c
+
+imu.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
+imu.srcs += downlink.c pprz_transport.c
+
+imu.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
+imu.srcs += $(SRC_ARCH)/adc_hw.c
+
+imu.srcs += $(SRC_ARCH)/max1167.c
+imu.srcs += $(SRC_ARCH)/micromag.c
+
+imu.srcs += imu_v3.c
+
+imu.srcs += ahrs_new.c
+
+imu.srcs += link_imu.c
+
+
+
+
diff --git a/conf/airframes/motor_bench.xml b/conf/airframes/motor_bench.xml
index aeb3eb6cee..85c3b8bb59 100644
--- a/conf/airframes/motor_bench.xml
+++ b/conf/airframes/motor_bench.xml
@@ -1,4 +1,4 @@
-
+
diff --git a/conf/autopilot/conf_booz.h b/conf/autopilot/conf_booz.h
new file mode 100644
index 0000000000..3c13f9736c
--- /dev/null
+++ b/conf/autopilot/conf_booz.h
@@ -0,0 +1,50 @@
+#ifndef CONFIG_BOOZ_H
+#define CONFIG_BOOZ_H
+
+/* Master oscillator freq. */
+#define FOSC (12000000)
+/* PLL multiplier */
+#define PLL_MUL (5)
+/* CPU clock freq. */
+#define CCLK (FOSC * PLL_MUL)
+/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2 */
+#define PBSD_BITS 0x00
+#define PBSD_VAL 4
+/* Peripheral bus clock freq. */
+#define PCLK (CCLK / PBSD_VAL)
+
+
+/* PPM : rc rx on P0.6*/
+#define PPM_PINSEL PINSEL0
+#define PPM_PINSEL_VAL 0x02
+#define PPM_PINSEL_BIT 12
+
+
+
+#define LED_1_BANK 0
+#define LED_1_PIN 12
+
+#define LED_2_BANK 0
+#define LED_2_PIN 13
+
+/* ADC */
+#define ADC_0 AdcBank0(4)
+#ifdef USE_ADC_0
+#ifndef USE_AD0
+#define USE_AD0
+#endif
+#define USE_AD0_4
+#endif
+
+#define ADC_1 AdcBank0(1)
+#ifdef USE_ADC_1
+#ifndef USE_AD0
+#define USE_AD0
+#endif
+#define USE_AD0_1
+#endif
+
+
+
+
+#endif /* CONFIG_BOOZ_H */
diff --git a/conf/conf.xml.example b/conf/conf.xml.example
index faf80aa24a..4e580bc688 100644
--- a/conf/conf.xml.example
+++ b/conf/conf.xml.example
@@ -67,4 +67,31 @@
flight_plan="flight_plans/dummy.xml"
/>
+
+
+
+
+
+
diff --git a/conf/settings/booz.xml b/conf/settings/booz.xml
new file mode 100644
index 0000000000..e822de1389
--- /dev/null
+++ b/conf/settings/booz.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml
new file mode 100644
index 0000000000..b580f8a4bc
--- /dev/null
+++ b/conf/telemetry/booz.xml
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/booz_downlink.h b/sw/airborne/booz_downlink.h
new file mode 100644
index 0000000000..7e31878201
--- /dev/null
+++ b/sw/airborne/booz_downlink.h
@@ -0,0 +1,12 @@
+#ifndef BOOZ_DOWNLINK_H
+#define BOOZ_DOWNLINK_H
+
+#include "std.h"
+
+static inline void booz_downlink_periodic_task(void) {
+
+
+}
+
+
+#endif /* BOOZ_DOWNLINK_H */
diff --git a/sw/airborne/link_imu.c b/sw/airborne/link_imu.c
index 46e02f12ba..2013a6fab2 100644
--- a/sw/airborne/link_imu.c
+++ b/sw/airborne/link_imu.c
@@ -135,42 +135,61 @@ void SPI0_ISR(void) {
-#ifdef FBW /* mega128 control board code */
+#ifdef CONTROLLER /* lpc controller board */
-#include
-#if (__GNUC__ == 3)
-#include
-#endif
-#include
+#include "LPC21xx.h"
+#include "interrupt_hw.h"
-#include CONFIG
-#include "spi.h"
-#include "fbw_downlink.h"
+
+//#include CONFIG
+//#include "spi.h"
+//#include "fbw_downlink.h"
+
+/* DRDY connected pin to P0.16 EINT0 */
+#define IMU_DRDY_PINSEL PINSEL1
+#define IMU_DRDY_PINSEL_VAL 0x01
+#define IMU_DRDY_PINSEL_BIT 0
+#define IMU_DRDY_EINT 0
+
+static void EXTINT0_ISR(void) __attribute__((naked));
void link_imu_init ( void ) {
- spi_buffer_input = (uint8_t*)&link_imu_state;
- spi_buffer_output = (uint8_t*)&link_imu_state_foo;
- spi_buffer_length = sizeof(link_imu_state);
+
+ /* configure DRDY pin */
+ IMU_DRDY_PINSEL |= IMU_DRDY_PINSEL_VAL << IMU_DRDY_PINSEL_BIT;
+ SetBit(EXTMODE, IMU_DRDY_EINT); /* EINT is edge trigered */
+ ClearBit(EXTPOLAR,IMU_DRDY_EINT); /* EINT is trigered on falling edge */
+ SetBit(EXTINT,IMU_DRDY_EINT); /* clear pending EINT */
+
+ /* initialize interrupt vector */
+ VICIntSelect &= ~VIC_BIT( VIC_EINT0 ); /* select EINT0 as IRQ source */
+ VICIntEnable = VIC_BIT( VIC_EINT0 ); /* enable it */
+ VICVectCntl9 = VIC_ENABLE | VIC_EINT0;
+ VICVectAddr9 = (uint32_t)EXTINT0_ISR; // address of the ISR
+
+ // spi_buffer_input = (uint8_t*)&link_imu_state;
+ // spi_buffer_output = (uint8_t*)&link_imu_state_foo;
+ // spi_buffer_length = sizeof(link_imu_state);
/** Falling edge */
- SetBit(EICRB, ISC61);
+ // SetBit(EICRB, ISC61);
/** Clr pending interrupt */
- SetBit(EIFR, INTF6);
+ // SetBit(EIFR, INTF6);
/** Enable DTRDY interrupt */
- SetBit(EIMSK, INT6);
+ // SetBit(EIMSK, INT6);
}
static inline void link_imu_read( void ) {
- SpiSelectSlave0();
- SpiStart();
+ // SpiSelectSlave0();
+ // SpiStart();
}
-SIGNAL( SIG_INTERRUPT6 ) {
- link_imu_read();
-}
+//SIGNAL( SIG_INTERRUPT6 ) {
+ // link_imu_read();
+//}
void link_imu_event_task( void ) {
/* static uint8_t foo; */
@@ -181,4 +200,20 @@ void link_imu_event_task( void ) {
/* } */
}
-#endif /* FBW */
+void EXTINT0_ISR(void) {
+ ISR_ENTRY();
+ /* read dummy control byte reply */
+ // uint8_t foo __attribute__ ((unused)) = SSPDR;
+ /* trigger 2 bytes read */
+ // SSPDR = 0;
+ // SSPDR = 0;
+ /* enable timeout interrupt */
+ // SpiEnableRti();
+ /* clear EINT2 */
+ SetBit(EXTINT,IMU_DRDY_EINT); /* clear EINT0 */
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
+
+#endif /* CONTROLLER */
diff --git a/sw/airborne/link_imu.h b/sw/airborne/link_imu.h
index 579752b619..dfca9b9c51 100644
--- a/sw/airborne/link_imu.h
+++ b/sw/airborne/link_imu.h
@@ -33,12 +33,12 @@ extern void link_imu_send( void );
#endif /* IMU */
-#ifdef FBW
+#ifdef CONTROLLER
-void link_imu_event_task( void );
+extern void link_imu_event_task( void );
-#endif /* FBW */
+#endif /* CONTROLLER */
#endif /* LINK_IMU_H */
diff --git a/sw/airborne/main_booz.c b/sw/airborne/main_booz.c
new file mode 100644
index 0000000000..3123ba3b66
--- /dev/null
+++ b/sw/airborne/main_booz.c
@@ -0,0 +1,92 @@
+
+#include "std.h"
+#include "init_hw.h"
+#include "sys_time.h"
+#include "led.h"
+#include "interrupt_hw.h"
+
+#include "radio_control.h"
+
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include "booz_downlink.h"
+#include "link_imu.h"
+
+static inline void main_init( void );
+static inline void main_periodic_task( void );
+static inline void main_event_task( void );
+
+static inline void main_dl_parse_msg( void );
+
+int main( void ) {
+ main_init();
+ while(1) {
+ if (sys_time_periodic())
+ main_periodic_task();
+ main_event_task();
+ }
+ return 0;
+}
+
+
+
+static inline void main_init( void ) {
+ hw_init();
+ led_init();
+ sys_time_init();
+ radio_control_init();
+ link_imu_init();
+ uart1_init_tx();
+ int_enable();
+}
+
+static inline void main_periodic_task( void ) {
+ radio_control_periodic_task();
+ booz_downlink_periodic_task();
+
+
+ static uint8_t my_cnt = 0;
+ my_cnt++;
+ if (!(my_cnt%10)) {
+ LED_TOGGLE(1);
+ uint16_t foo;
+ DOWNLINK_SEND_BOOT(&foo);
+
+ }
+
+}
+
+static inline void main_event_task( void ) {
+ if (PprzBuffer()) {
+ ReadPprzBuffer();
+ if (pprz_msg_received) {
+ pprz_parse_payload();
+ pprz_msg_received = FALSE;
+ }
+ }
+ if (dl_msg_available) {
+ main_dl_parse_msg();
+ dl_msg_available = FALSE;
+ LED_TOGGLE(2);
+ }
+}
+
+bool_t dl_msg_available;
+
+#define MSG_SIZE 128
+uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned));
+
+#include "settings.h"
+
+#define IdOfMsg(x) (x[1])
+
+static inline void main_dl_parse_msg(void) {
+ uint8_t msg_id = IdOfMsg(dl_buffer);
+ if (msg_id == DL_SETTING) {
+ uint8_t i = DL_SETTING_index(dl_buffer);
+ float var = DL_SETTING_value(dl_buffer);
+ DlSetting(i, var);
+ DOWNLINK_SEND_DL_VALUE(&i, &var);
+ }
+}