diff --git a/conf/airframes/gorrazoptere_091.xml b/conf/airframes/gorrazoptere_091.xml index 9e2fb09b22..6382c99f11 100644 --- a/conf/airframes/gorrazoptere_091.xml +++ b/conf/airframes/gorrazoptere_091.xml @@ -8,7 +8,7 @@ - + @@ -36,41 +36,13 @@ -
- - - - - - - -
-
- - - - - - - - -
-
- - - -
-
- - - +
+ + - -
-
- -
+ + include $(PAPARAZZI_SRC)/conf/autopilot/gorrazoptere_091.makefile @@ -88,10 +60,17 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c ap.srcs += $(SRC_ARCH)/adc_hw.c -ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DUART0_BAUD=B38400 -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 +ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DUART1_BAUD=B2400 +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 +#ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DUART0_BAUD=B9600 +#ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c +ap.CFLAGS += -DLINK_IMU -DSPI_MASTER -DUSE_SPI +ap.srcs += link_imu.c spi.c $(SRC_ARCH)/spi_hw.c + +#ap.CFLAGS += -DAP +#ap.srcs += main_ap_heli.c diff --git a/conf/airframes/imu.xml b/conf/airframes/imu.xml index 562de2bf3f..da689971eb 100644 --- a/conf/airframes/imu.xml +++ b/conf/airframes/imu.xml @@ -14,7 +14,7 @@ LPC21ISP_XTAL = 12000 FLASH_MODE = IAP #FLASH_MODE = ISP -imu.CFLAGS += -DCONFIG=\"pprz_imu.h\" +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 += -DUSE_UART0 -DUART0_BAUD=B38400 @@ -33,6 +33,8 @@ imu.srcs += imu_v3.c imu.srcs += ahrs_new.c +imu.srcs += link_imu.c + diff --git a/conf/airframes/twinjet1.xml b/conf/airframes/twinjet1.xml index dc1a437d43..8abe126c4b 100644 --- a/conf/airframes/twinjet1.xml +++ b/conf/airframes/twinjet1.xml @@ -148,16 +148,19 @@
+ include $(PAPARAZZI_SRC)/conf/autopilot/classix.makefile +FLASH_MODE = ISP + fbw.CFLAGS += -DFBW -DCONFIG=\"classix.h\" -DLED -DTIME_LED=1 fbw.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main.c @@ -172,7 +175,7 @@ fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c fbw.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DUART0_BAUD=B9600 fbw.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c -fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK +fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI fbw.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c fbw.CFLAGS += -DADC -DUSE_AD0 @@ -182,8 +185,11 @@ fbw.srcs += $(SRC_ARCH)/adc_hw.c ap.CFLAGS += -DAP -DCONFIG=\"classix.h\" -DLED -DTIME_LED=2 ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_ap.c main.c -ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600 -ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c datalink.c traffic_info.c xbee.c +#ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600 +#ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c datalink.c traffic_info.c xbee.c + +ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0 -DDATALINK=Pprz -DUART0_BAUD=B9600 +ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c datalink.c traffic_info.c ap.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK ap.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c diff --git a/conf/autopilot/gorrazoptere_091.h b/conf/autopilot/gorrazoptere_091.h index 370dee71d6..1475de2c90 100644 --- a/conf/autopilot/gorrazoptere_091.h +++ b/conf/autopilot/gorrazoptere_091.h @@ -1,30 +1,6 @@ #ifndef CONFIG_GORRAZOPTERE_091_H #define CONFIG_GORRAZOPTERE_091_H -#define LED_1_BANK A -#define LED_1_PIN 7 - -#define LED_2_BANK A -#define LED_2_PIN 6 - -#define LED_3_BANK A -#define LED_3_PIN 5 - -#define LED_4_BANK A -#define LED_4_PIN 4 - -#define LED_5_BANK A -#define LED_5_PIN 3 - -#define LED_6_BANK A -#define LED_6_PIN 2 - -#define LED_7_BANK A -#define LED_7_PIN 1 - -#define LED_8_BANK A -#define LED_8_PIN 0 - /* clock in MHz */ #define CLOCK 8 @@ -33,4 +9,18 @@ #define PPM_PORT PORTD #define PPM_PIN PD4 +#define MASTER_SPI_SS0_PIN 0 +#define MASTER_SPI_SS0_PORT PORTB +#define MASTER_SPI_SS0_DDR DDRB +#define MASTER_SPI_IT0_PIN 6 +#define MASTER_SPI_IT0_PORT PORTE +#define MASTER_SPI_IT0_DDR DDRE +/** EINT6 */ + +#define MASTER_SPI_SCK_PIN 1 +#define MASTER_SPI_MOSI_PIN 2 +#define MASTER_SPI_MISO_PIN 3 +#define MASTER_SPI_PORT PORTB +#define MASTER_SPI_DDR DDRB + #endif /* CONFIG_GORRAZOPTERE_091_H */ diff --git a/conf/autopilot/twin_avr.h b/conf/autopilot/twin_avr.h new file mode 100644 index 0000000000..6a03193ff1 --- /dev/null +++ b/conf/autopilot/twin_avr.h @@ -0,0 +1,36 @@ +#define SLAVE_SPI_PORT PORTB +#define SLAVE_SPI_PIN PINB +#define SLAVE_SPI_SS_PIN 2 + +#define SLAVE_SPI_DDR DDRB +#define SLAVE_SPI_MOSI_PIN 3 +#define SLAVE_SPI_MISO_PIN 4 +#define SLAVE_SPI_SCK_PIN 5 + +#define MASTER_SPI_SS0_PIN 0 +#define MASTER_SPI_SS0_PORT PORTB +#define MASTER_SPI_SS0_DDR DDRB +#define MASTER_SPI_IT0_PIN 7 +#define MASTER_SPI_IT0_PORT PORTE +#define MASTER_SPI_IT0_DDR DDRE + +#define MASTER_SPI_SS1_PIN 3 +#define MASTER_SPI_SS1_PORT PORTE +#define MASTER_SPI_SS1_DDR DDRE +#define MASTER_SPI_IT1_PIN 6 +#define MASTER_SPI_IT1_PORT PORTE +#define MASTER_SPI_IT1_DDR DDRE + +#define MASTER_SPI_SS2_PIN 2 +#define MASTER_SPI_SS2_PORT PORTE +#define MASTER_SPI_SS2_DDR DDRE +#define MASTER_SPI_IT2_PIN 5 +#define MASTER_SPI_IT2_PORT PORTE +#define MASTER_SPI_IT2_DDR DDRE + + +#define MASTER_SPI_SCK_PIN 1 +#define MASTER_SPI_MOSI_PIN 2 +#define MASTER_SPI_MISO_PIN 3 +#define MASTER_SPI_PORT PORTB +#define MASTER_SPI_DDR DDRB diff --git a/conf/autopilot/twin_mcu.makefile b/conf/autopilot/twin_mcu.makefile index 7279a883c0..c1a9e320d6 100644 --- a/conf/autopilot/twin_mcu.makefile +++ b/conf/autopilot/twin_mcu.makefile @@ -1,6 +1,6 @@ ap.srcs += main_ap.c sys_time.c main.c inter_mcu.c link_mcu.c gps_ubx.c gps.c infrared.c pid.c nav.c estimator.c cam.c spi.c -ap.CFLAGS += -DMCU_SPI_LINK -DGPS -DUBX -DINFRARED -DRADIO_CONTROL -DINTER_MCU +ap.CFLAGS += -DMCU_SPI_LINK -DGPS -DUBX -DINFRARED -DRADIO_CONTROL -DINTER_MCU -DSPI_MASTER -DUSE_SPI fbw.srcs += sys_time.c main_fbw.c main.c commands.c radio_control.c pprz_transport.c downlink.c inter_mcu.c spi.c link_mcu.c -fbw.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DINTER_MCU -DMCU_SPI_LINK -DUART0_BAUD=B38400 +fbw.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DINTER_MCU -DMCU_SPI_LINK -DUART0_BAUD=B38400 -DSPI_SLAVE diff --git a/conf/autopilot/twin_mcu_avr.makefile b/conf/autopilot/twin_mcu_avr.makefile index ad3a1588ef..e0ffb7be1b 100644 --- a/conf/autopilot/twin_mcu_avr.makefile +++ b/conf/autopilot/twin_mcu_avr.makefile @@ -2,7 +2,7 @@ include $(PAPARAZZI_SRC)/conf/autopilot/twin_avr.makefile include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu.makefile ap.srcs += $(SRC_ARCH)/adc_hw.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/spi_hw.c -ap.CFLAGS += -DUSE_UART1 -DGPS_LINK=Uart1 +ap.CFLAGS += -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017 -DADC fbw.srcs += actuators.c $(SRC_ARCH)/adc_hw.c $(SRC_ARCH)/servos_4017.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/spi_hw.c diff --git a/conf/autopilot/v1_2.h b/conf/autopilot/v1_2.h index 7a45a4db89..7868158ca7 100644 --- a/conf/autopilot/v1_2.h +++ b/conf/autopilot/v1_2.h @@ -1,6 +1,8 @@ #ifndef CONFIG_V1_2_H #define CONFIG_V1_2_H +#include "twin_avr.h" + /* clock in MHz */ #define CLOCK 16 diff --git a/conf/autopilot/v1_2_1.h b/conf/autopilot/v1_2_1.h index e434f1e377..24c50c0418 100644 --- a/conf/autopilot/v1_2_1.h +++ b/conf/autopilot/v1_2_1.h @@ -1,6 +1,8 @@ #ifndef CONFIG_V1_2_1_H #define CONFIG_V1_2_1_H +#include "twin_avr.h" + /* clock in MHz */ #define CLOCK 16 diff --git a/conf/messages.xml b/conf/messages.xml index e857f0686a..61d748daac 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -240,17 +240,14 @@ - - - - - - - - - + + + + + + diff --git a/sw/airborne/arm7/include/LPC21xx.h b/sw/airborne/arm7/include/LPC21xx.h index 3780715da6..cfdc674d01 100644 --- a/sw/airborne/arm7/include/LPC21xx.h +++ b/sw/airborne/arm7/include/LPC21xx.h @@ -168,6 +168,10 @@ #define S0SPCCR SPI0->ccr /* Clock Counter Register */ #define S0SPINT SPI0->flag /* Interrupt Flag Register */ +/* S0SPINT bits definition */ +#define SPI0IF 0 + + /////////////////////////////////////////////////////////////////////////////// // Serial Peripheral Interface 1 (SPI1) #define SPI1 ((spiRegs_t *)0xE0030000) diff --git a/sw/airborne/arm7/servos_direct_hw.c b/sw/airborne/arm7/servos_direct_hw.c index f5d0f07c5e..4ccf65c1ba 100644 --- a/sw/airborne/arm7/servos_direct_hw.c +++ b/sw/airborne/arm7/servos_direct_hw.c @@ -74,14 +74,6 @@ void actuators_init ( void ) { /* commit PWMMRx changes */ PWMLER = PWMLER_LATCH0; - Actuator(0) = SERVOS_TICS_OF_USEC(1500); - Actuator(1) = SERVOS_TICS_OF_USEC(1500); - Actuator(2) = SERVOS_TICS_OF_USEC(1500); - Actuator(3) = SERVOS_TICS_OF_USEC(1500); - Actuator(4) = SERVOS_TICS_OF_USEC(1500); - Actuator(5) = SERVOS_TICS_OF_USEC(1500); - - /* enable PWM timer in PWM mode */ PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE; diff --git a/sw/airborne/avr/spi_hw.c b/sw/airborne/avr/spi_hw.c index 1541c09147..d1ef0e8a94 100644 --- a/sw/airborne/avr/spi_hw.c +++ b/sw/airborne/avr/spi_hw.c @@ -26,6 +26,7 @@ * \brief handling of hardware dependant SPI on AVR architecture */ +#include CONFIG #include "spi.h" #include @@ -39,29 +40,20 @@ volatile uint8_t spi_idx_buf; -#define HandleOneSpiByte() { \ - spi_idx_buf++; \ - if (spi_idx_buf < spi_buffer_length) { \ - SPDR = spi_buffer_output[spi_idx_buf]; \ - spi_buffer_input[spi_idx_buf-1] = SPDR; \ - } else if (spi_idx_buf == spi_buffer_length) { \ - spi_buffer_input[spi_idx_buf-1] = SPDR; \ - spi_message_received = TRUE; \ - SpiStop(); \ - } \ -} +#define HandleOneSpiByte() { \ + spi_idx_buf++; \ + if (spi_idx_buf < spi_buffer_length) { \ + SPDR = spi_buffer_output[spi_idx_buf]; \ + spi_buffer_input[spi_idx_buf-1] = SPDR; \ + } else if (spi_idx_buf == spi_buffer_length) { \ + spi_buffer_input[spi_idx_buf-1] = SPDR; \ + spi_message_received = TRUE; \ + SpiStop(); \ + } \ + } -#ifdef FBW - -#define IT_PORT PORTD -#define IT_DDR DDRD -#define IT_PIN 7 - -#define SPI_DDR DDRB -#define SPI_MOSI_PIN 3 -#define SPI_MISO_PIN 4 -#define SPI_SCK_PIN 5 +#ifdef SPI_SLAVE volatile bool_t spi_was_interrupted = FALSE; @@ -70,7 +62,7 @@ void spi_init(void) { // IT_DDR |= _BV(IT_PIN); /* set MISO pin output */ - SPI_DDR |= _BV(SPI_MISO_PIN); + SLAVE_SPI_DDR |= _BV(SLAVE_SPI_MISO_PIN); /* enable SPI, slave, MSB first, sck idle low */ SPCR = _BV(SPE); /* enable interrupt */ @@ -84,11 +76,11 @@ SIGNAL(SIG_SPI) { HandleOneSpiByte(); } -#endif /** FBW */ +#endif /** SPI_SLAVE */ /****************************************************************************/ -#ifdef AP +#ifdef SPI_MASTER #include "autopilot.h" @@ -103,22 +95,24 @@ uint8_t spi_nb_ovrn; void spi_init( void) { /* Set MOSI and SCK output, all others input */ - SPI_DDR |= _BV(SPI_MOSI_PIN)| _BV(SPI_SCK_PIN); + MASTER_SPI_DDR |= _BV(MASTER_SPI_MOSI_PIN)| _BV(MASTER_SPI_SCK_PIN); /* enable pull up for miso */ - // SPI_PORT |= _BV(SPI_MISO_PIN); + // SPI_PORT |= _BV(MASTER_SPI_MISO_PIN); /* Set SS0 output */ - SetBit( SPI_SS0_DDR, SPI_SS0_PIN); + SetBit( MASTER_SPI_SS0_DDR, MASTER_SPI_SS0_PIN); /* SS0 idles high (don't select slave yet)*/ +#if 0 /* Set SS1 output */ - SetBit( SPI_SS1_DDR, SPI_SS1_PIN); + SetBit( MASTER_SPI_SS1_DDR, MASTER_SPI_SS1_PIN); /* SS1 idles high (don't select slave yet)*/ /* Set SS2 output */ - SetBit( SPI_SS2_DDR, SPI_SS2_PIN); + SetBit( MASTER_SPI_SS2_DDR, MASTER_SPI_SS2_PIN); /* SS2 idles high (don't select slave yet)*/ +#endif SpiUnselectAllSlaves(); @@ -128,7 +122,7 @@ void spi_init( void) { /** SPI interrupt: starts a delay */ SIGNAL(SIG_SPI) { - if (spi_cur_slave == SPI_SLAVE0) { + /* if (spi_cur_slave == SPI_SLAVE0) { */ /* setup OCR1A to pop in 200 clock cycles */ /* this leaves time for the slave (fbw) */ /* to process the byte we've sent and to */ @@ -138,8 +132,9 @@ SIGNAL(SIG_SPI) { SetBit(TIFR, OCF1A); /* enable OC1A interrupt */ SetBit(TIMSK, OCIE1A); - } else + /* } else fatal_error_nb++; + */ } /** Send a byte */ @@ -150,4 +145,4 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) { HandleOneSpiByte(); } -#endif /* AP */ +#endif /* SPI_MASTER */ diff --git a/sw/airborne/avr/spi_hw.h b/sw/airborne/avr/spi_hw.h index 301635daa6..43eae1295c 100644 --- a/sw/airborne/avr/spi_hw.h +++ b/sw/airborne/avr/spi_hw.h @@ -33,49 +33,18 @@ extern volatile uint8_t spi_idx_buf; SPDR = spi_buffer_output[0]; \ spi_message_received = FALSE; \ } -#define SPI_IT1_PIN 6 -#define SPI_IT1_PORT PORTE -#define SPI_IT1_DDR DDRE -#ifdef FBW -#define SPI_PORT PORTB -#define SPI_PIN PINB -#define SPI_SS_PIN 2 +#ifdef SPI_SLAVE #define SpiStart() SpiInitBuf() -#endif /* FBW */ +#endif /* SPI_SLAVE */ -#ifdef AP - -#define SPI_SS0_PIN 0 -#define SPI_SS0_PORT PORTB -#define SPI_SS0_DDR DDRB -#define SPI_IT0_PIN 7 -#define SPI_IT0_PORT PORTE -#define SPI_IT0_DDR DDRE - -#define SPI_SS1_PIN 3 -#define SPI_SS1_PORT PORTE -#define SPI_SS1_DDR DDRE -#define SPI_IT1_PIN 6 -#define SPI_IT1_PORT PORTE -#define SPI_IT1_DDR DDRE - -#define SPI_SS2_PIN 2 -#define SPI_SS2_PORT PORTE -#define SPI_SS2_DDR DDRE -#define SPI_IT2_PIN 5 -#define SPI_IT2_PORT PORTE -#define SPI_IT2_DDR DDRE -#define SPI_SCK_PIN 1 -#define SPI_MOSI_PIN 2 -#define SPI_MISO_PIN 3 -#define SPI_PORT PORTB -#define SPI_DDR DDRB + +#ifdef SPI_MASTER /* Enable SPI, Master, clock fck/16, interrupt */ #define SpiStart() { \ @@ -89,27 +58,29 @@ extern volatile uint8_t spi_idx_buf; #define SpiUnselectAllSlaves() { \ spi_cur_slave = SPI_NONE; \ - SetBit( SPI_SS0_PORT, SPI_SS0_PIN );\ - SetBit( SPI_SS1_PORT, SPI_SS1_PIN );\ - SetBit( SPI_SS2_PORT, SPI_SS2_PIN );\ + SetBit( MASTER_SPI_SS0_PORT, MASTER_SPI_SS0_PIN );\ + /* \ + SetBit( MASTER_SPI_SS1_PORT, MASTER_SPI_SS1_PIN ); \ + SetBit( MASTER_SPI_SS2_PORT, MASTER_SPI_SS2_PIN );\ + */ \ } #define SpiSelectSlave0() { \ spi_cur_slave = SPI_SLAVE0; \ - ClearBit( SPI_SS0_PORT, SPI_SS0_PIN );\ + ClearBit( MASTER_SPI_SS0_PORT, MASTER_SPI_SS0_PIN );\ } #define SpiSelectSlave1() { \ spi_cur_slave = SPI_SLAVE1; \ - ClearBit( SPI_SS1_PORT, SPI_SS1_PIN );\ + ClearBit( MASTER_SPI_SS1_PORT, MASTER_SPI_SS1_PIN );\ } #define SpiSelectSlave2() { \ spi_cur_slave = SPI_SLAVE2; \ - ClearBit( SPI_SS2_PORT, SPI_SS2_PIN );\ + ClearBit( MASTER_SPI_SS2_PORT, MASTER_SPI_SS2_PIN );\ } -#endif /* AP */ +#endif /* SPI_MASTER */ #endif /* SPI_HW_H */ diff --git a/sw/airborne/avr/uart_hw.c b/sw/airborne/avr/uart_hw.c index c0d8b77cf2..2e26309bba 100644 --- a/sw/airborne/avr/uart_hw.c +++ b/sw/airborne/avr/uart_hw.c @@ -29,7 +29,8 @@ #include "uart.h" #include "sys_time.h" -#define B9600 9600UL +#define B2400 2400UL +#define B9600 9600UL #define B38400 38400UL #if defined (__AVR_ATmega8__) @@ -178,21 +179,26 @@ SIGNAL( SIG_UART0_RECV ) { #endif /** USE_UART0 */ +#ifdef USE_UART1 + static uint8_t tx_head1; /* next free in buf */ static volatile uint8_t tx_tail1; /* next char to send */ static uint8_t tx_buf1[ TX_BUF_SIZE ]; void uart1_init_tx( void ) { - /* Baudrate is 38.4k */ + /* set baud rate */ UBRR1H = 0; - UBRR1L = 25; // 38.4 + UBRR1L = F_CPU/(16*UART1_BAUD)-1; /* single speed */ UCSR1A = 0; - /* Enable receiver and transmitter */ + /* Enable transmitter */ UCSR1B = _BV(TXEN); /* Set frame format: 8data, 1stop bit */ UCSR1C = _BV(UCSZ1) | _BV(UCSZ0); + + tx_head1 = 0; + tx_tail1 = 0; } void uart1_init_rx( void ) { @@ -202,6 +208,14 @@ void uart1_init_rx( void ) { sbi(UCSR1B, RXCIE ); } +bool_t uart1_check_free_space( uint8_t len) { + int8_t space; + if ((space = (tx_tail1 - tx_head1)) <= 0) + space += TX_BUF_SIZE; + + return (uint16_t)(space - 1) >= len; +} + void uart1_transmit( unsigned char data ) { if (UCSR1B & _BV(TXCIE)) { /* we are waiting for the last char to be sent : buffering */ @@ -236,4 +250,8 @@ SIGNAL( SIG_UART1_RECV ) { uart1_char_available = TRUE; } +#endif /* USE_UART1 */ + #endif /* (__AVR_ATmega128__) */ + + diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h index 88ea53b6cd..9af5055e0e 100644 --- a/sw/airborne/downlink.h +++ b/sw/airborne/downlink.h @@ -74,6 +74,7 @@ extern uint8_t downlink_nb_ovrn; #define DownlinkPutUint32ByAddr(_x) Transport(PutUint32ByAddr(_x)) #define DownlinkPutFloatByAddr(_x) Transport(PutFloatByAddr(_x)) +#define DownlinkPutFloatArray(_n, _x) Transport(PutFloatArray(_n, _x)) #define DownlinkPutInt16Array(_n, _x) Transport(PutInt16Array(_n, _x)) #define DownlinkPutUint16Array(_n, _x) Transport(PutUint16Array(_n, _x)) #define DownlinkPutUint8Array(_n, _x) Transport(PutUint8Array(_n, _x)) diff --git a/sw/airborne/fbw_downlink.h b/sw/airborne/fbw_downlink.h index 8d23c578e5..d0ea4bfe33 100644 --- a/sw/airborne/fbw_downlink.h +++ b/sw/airborne/fbw_downlink.h @@ -39,6 +39,8 @@ #include "messages.h" #include "periodic.h" #include "airframe.h" +#include "commands.h" +#include "actuators.h" #include "uart.h" #include "main_fbw.h" diff --git a/sw/airborne/link_imu.c b/sw/airborne/link_imu.c new file mode 100644 index 0000000000..33fab25c63 --- /dev/null +++ b/sw/airborne/link_imu.c @@ -0,0 +1,153 @@ +#include "link_imu.h" + +#include "std.h" + +struct imu_state link_imu_state; +struct imu_state link_imu_state_foo; + + +#ifdef IMU /* IMU LPC code */ + +#include "LPC21xx.h" +#include "armVIC.h" + + + +volatile uint8_t spi0_data_available; +volatile uint8_t spi0_idx_buf; +uint8_t* spi0_buffer_output = (uint8_t*)&link_imu_state; +uint8_t* spi0_buffer_input = (uint8_t*)&link_imu_state_foo; + +void SPI0_ISR(void) __attribute__((naked)); + + + +#define Spi0InitBuf() { \ + spi0_idx_buf = 0; \ + IO1SET = _BV(SPI0_DRDY); \ + /* FIXME : test if last read done */ \ + S0SPDR = spi0_buffer_output[0]; \ + IO1CLR = _BV(SPI0_DRDY); \ + } + +#define Spi0OneByte() { \ + /* FIXME : do something usefull with the status register reading */ \ + uint8_t foo __attribute__((unused)) = S0SPSR; \ + spi0_buffer_input[spi0_idx_buf] = S0SPDR; \ + spi0_idx_buf++; \ + if (spi0_idx_buf < sizeof(link_imu_state)) { \ + S0SPDR = spi0_buffer_output[spi0_idx_buf]; \ + } \ + else { \ + spi0_data_available = TRUE; \ + IO1SET = _BV(SPI0_DRDY); \ + } \ + } + +/* + wiring on IMU_V3 + P0_4 SCK0 + P0_5 MISO0 + P0_6 MOSI0 + P0_7 SSEL0 + P1_24 DRDY +*/ + +#define SPI0_DRDY 24 + +/* */ +#define S0SPCR_SPIE 7 /* SPI enable */ + +void link_imu_init ( void ) { + /* init SPI0 */ + /* setup pins for sck, miso, mosi, SSEL */ + PINSEL0 |= 1<<8 | 1<<10| 1<<12 | 1<< 14; + /* setup P1_16 to P1_25 as GPIO */ + PINSEL2 &= ~(1<<3); + /* P1_24 is output */ + SetBit(IO1DIR, SPI0_DRDY); + /* DRDY idles high */ + SetBit(IO1SET, SPI0_DRDY); + /* configure SPI : 8 bits CPOL=0 CPHA=0 MSB_first slave */ + S0SPCR = 0; + /* setup SPI clock rate */ + S0SPCCR = 0x20; + + /* initialize interrupt vector */ + VICIntSelect &= ~VIC_BIT(VIC_SPI0); // SPI0 selected as IRQ + VICIntEnable = VIC_BIT(VIC_SPI0); // SPI0 interrupt enabled + VICVectCntl10 = VIC_ENABLE | VIC_SPI0; + VICVectAddr10 = (uint32_t)SPI0_ISR; // address of the ISR + + /* clear pending interrupt */ + SetBit(S0SPINT, SPI0IF); + /* enable SPI interrupt */ + SetBit(S0SPCR, S0SPCR_SPIE); + +} + +void link_imu_send ( void ) { + Spi0InitBuf(); +} + + +void SPI0_ISR(void) { + ISR_ENTRY(); + Spi0OneByte(); + /* clear the interrupt */ + S0SPINT = _BV(SPI0IF); + /* clear this interrupt from the VIC */ + VICVectAddr = 0x00000000; + ISR_EXIT(); +} + + +#endif /* IMU */ + + + + + +#ifdef FBW /* mega128 control board code */ + +#include +#if (__GNUC__ == 3) +#include +#endif +#include + +#include CONFIG +#include "spi.h" +#include "fbw_downlink.h" + +void link_imu_init ( void ) { + spi_init(); + 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); + + /** Clr pending interrupt */ + SetBit(EIFR, INTF6); + + /** Enable DTRDY interrupt */ + SetBit(EIMSK, INT6); + +} + +static inline void link_imu_read( void ) { + SpiSelectSlave0(); + SpiStart(); +} + +SIGNAL( SIG_INTERRUPT6 ) { + link_imu_read(); +} + +void link_imu_event_task( void ) { + DOWNLINK_SEND_IMU_SENSORS(3, link_imu_state.rates); +} + +#endif /* FBW */ diff --git a/sw/airborne/link_imu.h b/sw/airborne/link_imu.h new file mode 100644 index 0000000000..f7d92c159f --- /dev/null +++ b/sw/airborne/link_imu.h @@ -0,0 +1,32 @@ +#ifndef LINK_IMU_H +#define LINK_IMU_H + +#include + +#include "frames.h" + +struct imu_state { + float rates[AXIS_NB]; + float eulers[AXIS_NB]; +}; + + +extern struct imu_state link_imu_state; +extern void link_imu_init ( void ); + +#ifdef IMU + +extern volatile uint8_t spi0_data_available; +extern void link_imu_send( void ); + +#endif /* IMU */ + +#ifdef FBW + + +void link_imu_event_task( void ); + + +#endif /* FBW */ + +#endif /* LINK_IMU_H */ diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index 6a1b243bc4..bf57096c10 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -486,10 +486,6 @@ inline void periodic_task_ap( void ) { void init_ap( void ) { #ifdef LED led_init(); - - IO0DIR |= 1<<2; - IO0SET = 1<<2; - #endif #ifndef SINGLE_MCU /** Dual mcus : init done in main_fbw */ hw_init(); @@ -497,7 +493,7 @@ void init_ap( void ) { #ifdef ADC adc_init(); #endif -#endif +#endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #ifdef INFRARED @@ -565,9 +561,6 @@ void init_ap( void ) { } wc_configure(); #endif - - // LED_ON(3); - } diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c index ee25475f1d..681a3613b8 100644 --- a/sw/airborne/main_fbw.c +++ b/sw/airborne/main_fbw.c @@ -50,6 +50,8 @@ #ifdef MCU_SPI_LINK #include "link_mcu.h" #endif +#include "link_imu.h" + #ifdef ADC struct adc_buf vsupply_adc_buf; #endif @@ -70,6 +72,9 @@ void init_fbw( void ) { #ifdef USE_UART0 uart0_init_tx(); #endif +#ifdef USE_UART1 + uart1_init_tx(); +#endif #ifdef ADC adc_init(); adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE); @@ -89,8 +94,13 @@ void init_fbw( void ) { spi_init(); link_mcu_restart(); #endif - +#ifdef LINK_IMU + spi_init(); + link_imu_init(); +#endif +#ifndef SINGLE_MCU int_enable(); +#endif } @@ -146,6 +156,15 @@ void event_task_fbw( void) { } #endif /* MCU_SPI_LINK */ #endif /* INTER_MCU */ + +#ifdef LINK_IMU + if (spi_message_received) { + /* Got a message on SPI. */ + spi_message_received = FALSE; + link_imu_event_task(); + } +#endif /* LINK_IMU */ + } /************* PERIODIC ******************************************************/ diff --git a/sw/airborne/main_imu.c b/sw/airborne/main_imu.c index a29f18533f..88efe6dbd4 100644 --- a/sw/airborne/main_imu.c +++ b/sw/airborne/main_imu.c @@ -12,10 +12,11 @@ #include "micromag.h" #include "imu_v3.h" #include "ahrs_new.h" +#include "link_imu.h" -#include "airframe.h" -#include "messages.h" -#include "downlink.h" +//#include "airframe.h" +//#include "messages.h" +//#include "downlink.h" static inline void main_init( void ); static inline void main_periodic_task( void ); @@ -62,6 +63,8 @@ static inline void main_init( void ) { main_init_spi1(); max1167_init(); micromag_init(); + // spi0_init(); + link_imu_init(); int_enable(); } @@ -81,6 +84,10 @@ static inline void main_event_task( void ) { imu_print_gyro(); ImuUpdateAccels(); imu_print_accel(); + link_imu_state.rates[AXIS_X] = imu_gyro[AXIS_X]; + link_imu_state.rates[AXIS_Y] = imu_gyro[AXIS_Y]; + link_imu_state.rates[AXIS_Z] = imu_gyro[AXIS_Z]; + link_imu_send(); // DOWNLINK_SEND_IMU_SENSORS(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z], // &imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z], // &imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]); diff --git a/sw/airborne/pprz_transport.h b/sw/airborne/pprz_transport.h index e397a5bd9e..b1a27e5e68 100644 --- a/sw/airborne/pprz_transport.h +++ b/sw/airborne/pprz_transport.h @@ -98,6 +98,7 @@ extern uint8_t ck_a, ck_b; } \ } +#define PprzTransportPutFloatArray(_n, _x) PprzTransportPutArray(PprzTransportPutFloatByAddr, _n, _x) #define PprzTransportPutInt16Array(_n, _x) PprzTransportPutArray(PprzTransportPutInt16ByAddr, _n, _x) #define PprzTransportPutUint16Array(_n, _x) PprzTransportPutArray(PprzTransportPutUint16ByAddr, _n, _x) diff --git a/sw/airborne/spi.h b/sw/airborne/spi.h index 3b27713a84..d329864401 100644 --- a/sw/airborne/spi.h +++ b/sw/airborne/spi.h @@ -29,7 +29,7 @@ #ifndef SPI_H #define SPI_H -#ifdef MCU_SPI_LINK +#ifdef USE_SPI #include "std.h" #include "spi_hw.h" @@ -42,7 +42,7 @@ extern volatile bool_t spi_message_received; void spi_init(void); -#ifdef AP +#ifdef SPI_MASTER #define SPI_NONE 0 #define SPI_SLAVE0 1 @@ -55,9 +55,9 @@ extern uint8_t spi_nb_ovrn; #define SpiCheckAvailable() (spi_cur_slave == SPI_NONE) #define SpiOverRun() {spi_nb_ovrn++;} -#endif /* AP */ +#endif /* SPI_MASTER */ -#endif /* MCU_SPI_LINK */ +#endif /* USE_SPI */ #endif /* SPI_H */