diff --git a/conf/airframes/booz2.xml b/conf/airframes/booz2.xml index 08762a3013..444fb696ab 100644 --- a/conf/airframes/booz2.xml +++ b/conf/airframes/booz2.xml @@ -37,7 +37,8 @@ - + + @@ -84,6 +80,51 @@ + +--> + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
diff --git a/conf/autopilot/conf_booz2.makefile b/conf/autopilot/conf_booz2.makefile index c0107aff80..e8244756cd 100644 --- a/conf/autopilot/conf_booz2.makefile +++ b/conf/autopilot/conf_booz2.makefile @@ -83,7 +83,7 @@ ap.ARCH = arm7tdmi ap.TARGET = ap ap.TARGETDIR = ap -ap.CFLAGS += -DKILL_MOTORS +#ap.CFLAGS += -DKILL_MOTORS ap.CFLAGS += -DCONFIG=\"booz2_board.h\" -I$(BOOZ_ARCH) -I$(BOOZ_PRIV) -I$(BOOZ_PRIV_ARCH) ap.srcs += $(BOOZ_PRIV)/booz2_main.c @@ -107,16 +107,24 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c ap.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC ap.srcs += $(BOOZ_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c -ap.CFLAGS += -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10 +ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c -ap.srcs += $(BOOZ_PRIV)/booz2_imu_v3.c $(BOOZ_PRIV_ARCH)/booz2_imu_v3_hw.c +#ap.srcs += $(BOOZ_PRIV)/booz2_imu_v3.c $(BOOZ_PRIV_ARCH)/booz2_imu_v3_hw.c +ap.srcs += $(BOOZ_PRIV)/booz2_imu_crista.c $(BOOZ_PRIV_ARCH)/booz2_imu_crista_hw.c ap.CFLAGS += -DFLOAT_T=float ap.srcs += $(BOOZ_PRIV)/booz2_imu.c ap.CFLAGS += -DBOOZ2_ANALOG_BARO_LED=5 -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))' ap.srcs += $(BOOZ_PRIV)/booz2_analog_baro.c + +ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 +#ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c +ap.CFLAGS += -DUSE_AMI601 +ap.srcs += AMI601.c + + ap.srcs += $(BOOZ_PRIV)/booz2_autopilot.c ap.srcs += $(BOOZ_PRIV)/booz2_stabilization.c @@ -129,7 +137,7 @@ ap.srcs += $(BOOZ_PRIV)/booz2_filter_aligner.c ap.srcs += $(BOOZ_PRIV)/booz2_filter_attitude_cmpl_euler.c ap.srcs += $(BOOZ_PRIV)/booz_trig_int.c -ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 +ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 -DUART1_VIC_SLOT=6 ap.CFLAGS += -DGPS_LINK=Uart1 -DGPS_LED=7 ap.srcs += $(BOOZ_PRIV)/booz2_gps.c ap.srcs += $(BOOZ_PRIV)/booz2_guidance.c @@ -231,7 +239,6 @@ tunnel.srcs += $(SRC_ARCH)/uart_hw.c # # test GPS # - test_gps.ARCHDIR = $(ARCHI) test_gps.ARCH = arm7tdmi test_gps.TARGET = test_gps @@ -266,8 +273,9 @@ test_led.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./1000.))' -DTIME_L test_led.CFLAGS += -DLED test_led.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c - +# # test modem +# test_modem.ARCHDIR = $(ARCHI) test_modem.ARCH = arm7tdmi test_modem.TARGET = test_modem @@ -289,23 +297,54 @@ test_modem.CFLAGS += -DBOOZ_ANALOG_BARO_LED=2 -DBOOZ_ANALOG_BARO_PERIOD='SYS_TIC test_modem.srcs += $(BOOZ_PRIV)/booz_analog_baro.c -# test imu -test_imu.ARCHDIR = $(ARCHI) -test_imu.ARCH = arm7tdmi -test_imu.TARGET = test_imu -test_imu.TARGETDIR = test_imu +# +# test AMI +# +test_ami.ARCHDIR = $(ARCHI) +test_ami.ARCH = arm7tdmi +test_ami.TARGET = test_ami +test_ami.TARGETDIR = test_ami -test_imu.CFLAGS += -DCONFIG=\"tiny_1_1.h\" -I$(BOOZ_PRIV_ARCH) -test_imu.srcs += $(BOOZ_PRIV)/test_imu.c -test_imu.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./1000.))' -DTIME_LED=1 -test_imu.CFLAGS += -DLED -test_imu.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +test_ami.CFLAGS += -DCONFIG=\"booz2_board.h\" -I$(BOOZ_PRIV_ARCH) +test_ami.srcs += $(BOOZ_PRIV_TEST)/booz2_test_ami.c +test_ami.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./50.))' -DTIME_LED=1 +test_ami.CFLAGS += -DLED +test_ami.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c -test_imu.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 -test_imu.srcs += $(SRC_ARCH)/uart_hw.c +test_ami.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 +test_ami.srcs += $(SRC_ARCH)/uart_hw.c + +test_ami.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 +test_ami.srcs += downlink.c pprz_transport.c + +test_ami.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 +test_ami.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c +test_ami.CFLAGS += -DUSE_AMI601 +test_ami.srcs += AMI601.c + + +# +# test crista +# +test_crista.ARCHDIR = $(ARCHI) +test_crista.ARCH = arm7tdmi +test_crista.TARGET = test_crista +test_crista.TARGETDIR = test_crista + +test_crista.CFLAGS += -DCONFIG=\"booz2_board.h\" -I$(BOOZ_PRIV) -I$(BOOZ_PRIV_ARCH) +test_crista.srcs += $(BOOZ_PRIV_TEST)/booz2_test_crista.c +test_crista.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./500.))' -DTIME_LED=1 +test_crista.CFLAGS += -DLED +test_crista.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c + +test_crista.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 +test_crista.srcs += $(SRC_ARCH)/uart_hw.c + +test_crista.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 +test_crista.srcs += downlink.c pprz_transport.c + +test_crista.CFLAGS += -DFLOAT_T=float +test_crista.srcs += $(BOOZ_PRIV)/booz2_imu.c +test_crista.srcs += $(BOOZ_PRIV)/booz2_imu_crista.c $(BOOZ_PRIV_ARCH)/booz2_imu_crista_hw.c -test_imu.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 -test_imu.srcs += downlink.c pprz_transport.c -test_imu.CFLAGS += -DFLOAT_T=float -test_imu.srcs += $(BOOZ_PRIV)/booz_imu_int.c $(BOOZ_PRIV_ARCH)/booz_imu_int_hw.c \ No newline at end of file diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index d082308924..bb9295870f 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -7,17 +7,24 @@ - - - - + + + + + - + - - - - + + + + + + + + + + @@ -33,9 +40,9 @@ - - - + + + diff --git a/sw/airborne/AMI601.c b/sw/airborne/AMI601.c index dfeeeac6cb..2eee8f3030 100644 --- a/sw/airborne/AMI601.c +++ b/sw/airborne/AMI601.c @@ -66,7 +66,7 @@ void ami601_read( void ) { ami601_i2c_done = FALSE; ami601_status = AMI601_SENDING_REQ; const uint8_t read_cmd[] = { 0x55, 0xAA, 0X14}; - memcpy((void*)i2c_buf, read_cmd, sizeof(read_cmd)); - i2c_transmit(AMI601_SLAVE_ADDR, sizeof(read_cmd), &ami601_i2c_done); + memcpy((void*)i2c1_buf, read_cmd, sizeof(read_cmd)); + i2c1_transmit(AMI601_SLAVE_ADDR, sizeof(read_cmd), &ami601_i2c_done); } } diff --git a/sw/airborne/AMI601.h b/sw/airborne/AMI601.h index f659ab87af..f3e6a860be 100644 --- a/sw/airborne/AMI601.h +++ b/sw/airborne/AMI601.h @@ -16,14 +16,6 @@ extern uint8_t ami601_foo1; extern uint8_t ami601_foo2; extern uint8_t ami601_foo3; -extern float ami601_ax; -extern float ami601_ay; -extern float ami601_az; - -extern float ami601_mx; -extern float ami601_my; -extern float ami601_mz; - #define AMI601_IDLE 0 #define AMI601_SENDING_REQ 1 #define AMI601_WAITING_MEASURE 2 @@ -36,7 +28,7 @@ extern volatile uint32_t ami601_nb_err; #define AMI601_SLAVE_ADDR 0x60 -#define AMI601EventCheckAndHandle() { \ +#define AMI601Event(_handler) { \ switch (ami601_status) { \ case AMI601_SENDING_REQ : \ if ( ami601_i2c_done ) { \ @@ -51,15 +43,16 @@ extern volatile uint32_t ami601_nb_err; break; \ case AMI601_READING_MEASURE : \ if ( ami601_i2c_done ) { \ - ami601_foo1 = i2c_buf[0]; /* AA ? */ \ - ami601_foo2 = i2c_buf[1]; /* 55 ? */ \ - ami601_foo3 = i2c_buf[2]; /* ERR ? */ \ + ami601_foo1 = i2c1_buf[0]; /* AA ? */ \ + ami601_foo2 = i2c1_buf[1]; /* 55 ? */ \ + ami601_foo3 = i2c1_buf[2]; /* ERR ? */ \ uint8_t i; \ for (i=0; i< AMI601_NB_CHAN; i++) { \ - ami601_val[i] = i2c_buf[3 + 2 * i]; \ - ami601_val[i] += i2c_buf[3 + 2 * i + 1] * 256; \ + ami601_val[i] = i2c1_buf[3 + 2 * i]; \ + ami601_val[i] += i2c1_buf[3 + 2 * i + 1] * 256; \ } \ ami601_status = AMI601_DATA_AVAILABLE; \ + _handler(); \ } \ break; \ } \ @@ -70,7 +63,7 @@ extern volatile uint32_t ami601_nb_err; /* T0MCR |= TMCR_MR1_I; */ \ ami601_i2c_done = FALSE; \ ami601_status = AMI601_READING_MEASURE; \ - i2c_receive(AMI601_SLAVE_ADDR, 15, &ami601_i2c_done); \ + i2c1_receive(AMI601_SLAVE_ADDR, 15, &ami601_i2c_done); \ } #endif /* AMI601_H */ diff --git a/sw/airborne/arm7/i2c_hw.c b/sw/airborne/arm7/i2c_hw.c index cfb8cf49cd..a0601f750a 100644 --- a/sw/airborne/arm7/i2c_hw.c +++ b/sw/airborne/arm7/i2c_hw.c @@ -32,16 +32,16 @@ #include "interrupt_hw.h" /* default clock speed 37.5KHz with our 15MHz PCLK */ -#ifndef I2C_SCLL -#define I2C_SCLL 200 +#ifndef I2C0_SCLL +#define I2C0_SCLL 200 #endif -#ifndef I2C_SCLH -#define I2C_SCLH 200 +#ifndef I2C0_SCLH +#define I2C0_SCLH 200 #endif -#ifndef I2C_VIC_SLOT -#define I2C_VIC_SLOT 9 +#ifndef I2C0_VIC_SLOT +#define I2C0_VIC_SLOT 9 #endif void i2c0_ISR(void) __attribute__((naked)); @@ -49,7 +49,7 @@ void i2c0_ISR(void) __attribute__((naked)); /* SDA0 on P0.3 */ /* SCL0 on P0.2 */ -void i2c_hw_init ( void ) { +void i2c0_hw_init ( void ) { /* set P0.2 and P0.3 to I2C0 */ PINSEL0 |= 1 << 4 | 1 << 6; @@ -58,14 +58,14 @@ void i2c_hw_init ( void ) { /* enable I2C */ I2C0CONSET = _BV(I2EN); /* set bitrate */ - I2C0SCLL = I2C_SCLL; - I2C0SCLH = I2C_SCLH; + I2C0SCLL = I2C0_SCLL; + I2C0SCLH = I2C0_SCLH; // initialize the interrupt vector VICIntSelect &= ~VIC_BIT(VIC_I2C0); // I2C0 selected as IRQ VICIntEnable = VIC_BIT(VIC_I2C0); // I2C0 interrupt enabled - _VIC_CNTL(I2C_VIC_SLOT) = VIC_ENABLE | VIC_I2C0; - _VIC_ADDR(I2C_VIC_SLOT) = (uint32_t)i2c0_ISR; // address of the ISR + _VIC_CNTL(I2C0_VIC_SLOT) = VIC_ENABLE | VIC_I2C0; + _VIC_ADDR(I2C0_VIC_SLOT) = (uint32_t)i2c0_ISR; // address of the ISR } #define I2C_DATA_REG I2C0DAT @@ -82,3 +82,47 @@ void i2c0_ISR(void) VICVectAddr = 0x00000000; // clear this interrupt from the VIC ISR_EXIT(); // recover registers and return } + +#ifdef USE_I2C1 + +#define I2C1_DATA_REG I2C1DAT +#define I2C1_STATUS_REG I2C1STAT + +void i2c1_ISR(void) __attribute__((naked)); + +/* SDA1 on P0.14 */ +/* SCL1 on P0.11 */ +void i2c1_hw_init ( void ) { + + /* set P0.11 and P0.14 to I2C1 */ + PINSEL0 |= 3 << 22 | 3 << 28; + /* clear all flags */ + I2C1CONCLR = _BV(AAC) | _BV(SIC) | _BV(STAC) | _BV(I2ENC); + /* enable I2C */ + I2C1CONSET = _BV(I2EN); + /* set bitrate */ + I2C1SCLL = I2C1_SCLL; + I2C1SCLH = I2C1_SCLH; + + // initialize the interrupt vector + VICIntSelect &= ~VIC_BIT(VIC_I2C1); // I2C0 selected as IRQ + VICIntEnable = VIC_BIT(VIC_I2C1); // I2C0 interrupt enabled + _VIC_CNTL(I2C1_VIC_SLOT) = VIC_ENABLE | VIC_I2C1; + _VIC_ADDR(I2C1_VIC_SLOT) = (uint32_t)i2c1_ISR; // address of the ISR +} + +void i2c1_ISR(void) +{ + ISR_ENTRY(); + + uint32_t state = I2C1_STATUS_REG; + I2c1Automaton(state); + I2c1ClearIT(); + + VICVectAddr = 0x00000000; // clear this interrupt from the VIC + ISR_EXIT(); // recover registers and return +} + + +#endif /* USE_I2C1 */ + diff --git a/sw/airborne/arm7/i2c_hw.h b/sw/airborne/arm7/i2c_hw.h index af8637ff8f..1aa284a394 100644 --- a/sw/airborne/arm7/i2c_hw.h +++ b/sw/airborne/arm7/i2c_hw.h @@ -11,7 +11,7 @@ #define I2cStopHandler() {} #endif -extern void i2c_hw_init(void); +extern void i2c0_hw_init(void); #define I2cSendAck() { I2C0CONSET = _BV(AA); } #define I2cSendStop() { \ @@ -32,4 +32,30 @@ extern void i2c_hw_init(void); #define I2cClearIT() { I2C0CONCLR = _BV(SIC); } +#ifdef USE_I2C1 + +extern void i2c1_hw_init(void); + +#define I2c1StopHandler() {} + +#define I2c1SendAck() { I2C1CONSET = _BV(AA); } +#define I2c1SendStop() { \ + I2C1CONSET = _BV(STO); \ + if (i2c1_finished) *i2c1_finished = TRUE; \ + i2c1_status = I2C_IDLE; \ + I2c1StopHandler(); \ + } +#define I2c1SendStart() { I2C1CONSET = _BV(STA); } +#define I2c1SendByte(b) { I2C1_DATA_REG = b; } + +#define I2c1Receive(_ack) { \ + if (_ack) I2C1CONSET = _BV(AA); \ + else I2C1CONCLR = _BV(AAC); \ + } + +#define I2c1ClearStart() { I2C1CONCLR = _BV(STAC); } +#define I2c1ClearIT() { I2C1CONCLR = _BV(SIC); } + +#endif /* USE_I2C1 */ + #endif /* I2C_HW_H */ diff --git a/sw/airborne/arm7/include/LPC21xx.h b/sw/airborne/arm7/include/LPC21xx.h index a65265c543..add6de9e20 100644 --- a/sw/airborne/arm7/include/LPC21xx.h +++ b/sw/airborne/arm7/include/LPC21xx.h @@ -157,6 +157,18 @@ #define I2C0SCLL I2C0->scll /* SCL Duty Cycle Register (low half word) */ #define I2C0CONCLR I2C0->conclr /* Control Clear Register */ + +#define I2C1 ((i2cRegs_t *)0xE005C000) +// I2C Registers +#define I2C1CONSET I2C1->conset /* Control Set Register */ +#define I2C1STAT I2C1->stat /* Status Register */ +#define I2C1DAT I2C1->dat /* Data Register */ +#define I2C1ADR I2C1->adr /* Slave Address Register */ +#define I2C1SCLH I2C1->sclh /* SCL Duty Cycle Register (high half word) */ +#define I2C1SCLL I2C1->scll /* SCL Duty Cycle Register (low half word) */ +#define I2C1CONCLR I2C1->conclr /* Control Clear Register */ + + // I2CONSET bit definition #define AA 2 diff --git a/sw/airborne/i2c.c b/sw/airborne/i2c.c index 6d38732526..e13ad6327b 100644 --- a/sw/airborne/i2c.c +++ b/sw/airborne/i2c.c @@ -13,7 +13,7 @@ volatile bool_t* i2c_finished; void i2c_init(void) { i2c_status = I2C_IDLE; - i2c_hw_init(); + i2c0_hw_init(); i2c_finished = NULL; } @@ -34,3 +34,42 @@ void i2c_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) { I2cSendStart(); } + + +#ifdef USE_I2C1 + +volatile uint8_t i2c1_status; +volatile uint8_t i2c1_buf[I2C1_BUF_LEN]; +volatile uint8_t i2c1_len; +volatile uint8_t i2c1_index; +volatile uint8_t i2c1_slave_addr; + +volatile bool_t* i2c1_finished; + +void i2c1_init(void) { + i2c1_status = I2C_IDLE; + i2c1_hw_init(); + i2c1_finished = NULL; +} + + +void i2c1_receive(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) { + i2c1_len = len; + i2c1_slave_addr = slave_addr | I2C_RECEIVE; + i2c1_finished = finished; + i2c1_status = I2C_BUSY; + I2c1SendStart(); +} + +void i2c1_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) { + i2c1_len = len; + i2c1_slave_addr = slave_addr & ~I2C_RECEIVE; + i2c1_finished = finished; + i2c1_status = I2C_BUSY; + I2c1SendStart(); +} + + + + +#endif /* USE_I2C1 */ diff --git a/sw/airborne/i2c.h b/sw/airborne/i2c.h index eccc28d3a0..de0ef55b9c 100644 --- a/sw/airborne/i2c.h +++ b/sw/airborne/i2c.h @@ -86,4 +86,73 @@ extern volatile bool_t* i2c_finished; } \ } \ + +#ifdef USE_I2C1 + +extern void i2c1_init(void); +extern void i2c1_receive(uint8_t slave_addr, uint8_t len, volatile bool_t* finished); +extern void i2c1_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished); + +extern volatile uint8_t i2c1_status; + +extern volatile uint8_t i2c1_buf[I2C1_BUF_LEN]; +extern volatile uint8_t i2c1_len; +extern volatile uint8_t i2c1_index; +extern volatile uint8_t i2c1_slave_addr; + +extern volatile bool_t* i2c1_finished; + +#define I2c1Automaton(state) { \ + switch (state) { \ + case I2C_START: \ + case I2C_RESTART: \ + I2c1SendByte(i2c1_slave_addr); \ + I2c1ClearStart(); \ + i2c1_index = 0; \ + break; \ + case I2C_MR_DATA_ACK: \ + if (i2c1_index < i2c1_len) { \ + i2c1_buf[i2c1_index] = I2C1_DATA_REG; \ + i2c1_index++; \ + I2c1Receive(i2c1_index < i2c1_len - 1); \ + } \ + else { \ + /* error , we should have got NACK */ \ + I2c1SendStop(); \ + } \ + break; \ + case I2C_MR_SLA_ACK: /* At least one char */ \ + /* Wait and reply with ACK or NACK */ \ + I2c1Receive(i2c1_index < i2c1_len - 1); \ + break; \ + case I2C_MR_SLA_NACK: \ + case I2C_MT_SLA_NACK: \ + I2c1SendStart(); \ + break; \ + case I2C_MT_SLA_ACK: \ + case I2C_MT_DATA_ACK: \ + if (i2c1_index < i2c1_len) { \ + I2c1SendByte(i2c1_buf[i2c1_index]); \ + i2c1_index++; \ + } else { \ + I2c1SendStop(); \ + } \ + break; \ + case I2C_MR_DATA_NACK: \ + if (i2c1_index < i2c1_len) { \ + i2c1_buf[i2c1_index] = I2C1_DATA_REG; \ + } \ + I2c1SendStop(); \ + break; \ + default: \ + I2c1SendStop(); \ + /* LED_ON(2); FIXME log error */ \ + } \ + } \ + + + +#endif /* USE_I2C1 */ + + #endif /* I2C_H */