diff --git a/conf/airframes/twinstar5.xml b/conf/airframes/twinstar5.xml
index daf68e9f4a..0a348e9202 100644
--- a/conf/airframes/twinstar5.xml
+++ b/conf/airframes/twinstar5.xml
@@ -160,7 +160,7 @@ ap.CFLAGS += -DDOWNLINK -DUSE_UART0
ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c
ap.CFLAGS += -DGPS -DUBX -DUSE_UART1
-ap.srcs += gps_ubx.c
+ap.srcs += gps_ubx.c gps.c
#ap.CFLAGS += -DMODEM -DDOWNLINK
#ap.srcs += $(SRC_ARCH)/modem_hw.c
diff --git a/conf/hardware.xml b/conf/hardware.xml
new file mode 100644
index 0000000000..a5619dcd6d
--- /dev/null
+++ b/conf/hardware.xml
@@ -0,0 +1,61 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/doc/architecture.fig b/doc/architecture.fig
new file mode 100644
index 0000000000..28d515f1f8
--- /dev/null
+++ b/doc/architecture.fig
@@ -0,0 +1,21 @@
+#FIG 3.2 Produced by xfig version 3.2.5-alpha5
+Landscape
+Center
+Metric
+A4
+100.00
+Single
+-2
+1200 2
+4 0 0 50 -1 0 16 0.0000 4 195 1005 855 8737 TIMERS\001
+4 0 0 50 -1 0 16 0.0000 4 195 600 2790 8737 ADC\001
+4 0 0 50 -1 0 16 0.0000 4 195 390 4590 8737 SPI\001
+4 0 0 50 -1 0 16 0.0000 4 195 780 6030 8737 UART\001
+4 0 0 50 -1 0 16 0.0000 4 195 1170 8325 5340 InterMCU\001
+4 0 0 50 -1 0 16 0.0000 4 195 1095 6435 5340 Downlink\001
+4 0 0 50 -1 0 16 0.0000 4 255 1275 4545 5310 Monitoring\001
+4 0 0 50 -1 0 16 0.0000 4 195 1515 2475 5340 RadioControl\001
+4 0 0 50 -1 0 16 0.0000 4 195 1125 675 5340 Actuators\001
+4 0 0 50 -1 0 16 0.0000 4 255 975 9990 5310 SysTime\001
+4 0 0 50 -1 0 16 0.0000 4 255 1140 2520 6075 PPM_HW\001
+4 0 0 50 -1 0 16 0.0000 4 255 2145 720 6705 SERVOS_XX_HW\001
diff --git a/sw/airborne/arm7/int.h b/sw/airborne/arm7/interrupt_hw.h
similarity index 97%
rename from sw/airborne/arm7/int.h
rename to sw/airborne/arm7/interrupt_hw.h
index d516510e83..061d538bdd 100644
--- a/sw/airborne/arm7/int.h
+++ b/sw/airborne/arm7/interrupt_hw.h
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*
*/
-/** \file int.h
+/** \file interrupt_hw.h
* \brief ARM Low level interrupt handling
*
*/
diff --git a/sw/airborne/arm7/uart_hw.c b/sw/airborne/arm7/uart_hw.c
index a8524c3712..5a2d87abaa 100644
--- a/sw/airborne/arm7/uart_hw.c
+++ b/sw/airborne/arm7/uart_hw.c
@@ -11,9 +11,8 @@
#if defined(UART0_TX_INT_MODE) || defined(UART0_RX_INT_MODE)
void uart0_ISR(void) __attribute__((naked));
-#endif // UART0_TX_INT_MODE || UART0_RX_INT_MODE
+#endif
-static void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
#ifdef UART0_RX_INT_MODE
uint8_t uart0_rx_buffer[UART0_RX_BUFFER_SIZE];
@@ -26,13 +25,55 @@ uint16_t uart0_tx_insert_idx, uart0_tx_extract_idx;
uint8_t uart0_tx_running;
#endif
+static void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
+
void uart0_init_tx( void ) {
uart0_init_param(UART_BAUD(38400), UART_8N1, UART_FIFO_8);
}
+void uart0_init_rx( void ) {}
-void uart0_init_rx( void ) {
+static void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
+ // set port pins for UART0
+ PINSEL0 = (PINSEL0 & ~U0_PINMASK) | U0_PINSEL;
+ U0IER = 0x00; // disable all interrupts
+ U0IIR; // clear interrupt ID
+ U0RBR; // clear receive register
+ U0LSR; // clear line status register
+
+ // set the baudrate
+ U0LCR = ULCR_DLAB_ENABLE; // select divisor latches
+ U0DLL = (uint8_t)baud; // set for baud low byte
+ U0DLM = (uint8_t)(baud >> 8); // set for baud high byte
+
+ // set the number of characters and other
+ // user specified operating parameters
+ U0LCR = (mode & ~ULCR_DLAB_ENABLE);
+ U0FCR = fmode;
+#if defined(UART0_TX_INT_MODE) || defined(UART0_RX_INT_MODE)
+ // initialize the interrupt vector
+ VICIntSelect &= ~VIC_BIT(VIC_UART0); // UART0 selected as IRQ
+ VICIntEnable = VIC_BIT(VIC_UART0); // UART0 interrupt enabled
+ VICVectCntl5 = VIC_ENABLE | VIC_UART0;
+ VICVectAddr5 = (uint32_t)uart0_ISR; // address of the ISR
+
+#ifdef UART0_TX_INT_MODE
+ // initialize the transmit data queue
+ uart0_tx_extract_idx = 0;
+ uart0_tx_insert_idx = 0;
+ uart0_tx_running = 0;
+#endif
+
+#ifdef UART0_RX_INT_MODE
+ // initialize the receive data queue
+ uart0_rx_extract_idx = 0;
+ uart0_rx_insert_idx = 0;
+
+ // enable receiver interrupts
+ U0IER = UIER_ERBFI;
+#endif
+#endif
}
bool_t uart0_check_free_space( uint8_t len) {
@@ -84,48 +125,6 @@ void uart0_transmit( unsigned char data ) {
// return (uint8_t)ch;
}
-static void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
- // set port pins for UART0
- PINSEL0 = (PINSEL0 & ~U0_PINMASK) | U0_PINSEL;
-
- U0IER = 0x00; // disable all interrupts
- U0IIR; // clear interrupt ID
- U0RBR; // clear receive register
- U0LSR; // clear line status register
-
- // set the baudrate
- U0LCR = ULCR_DLAB_ENABLE; // select divisor latches
- U0DLL = (uint8_t)baud; // set for baud low byte
- U0DLM = (uint8_t)(baud >> 8); // set for baud high byte
-
- // set the number of characters and other
- // user specified operating parameters
- U0LCR = (mode & ~ULCR_DLAB_ENABLE);
- U0FCR = fmode;
-#if defined(UART0_TX_INT_MODE) || defined(UART0_RX_INT_MODE)
- // initialize the interrupt vector
- VICIntSelect &= ~VIC_BIT(VIC_UART0); // UART0 selected as IRQ
- VICIntEnable = VIC_BIT(VIC_UART0); // UART0 interrupt enabled
- VICVectCntl5 = VIC_ENABLE | VIC_UART0;
- VICVectAddr5 = (uint32_t)uart0_ISR; // address of the ISR
-
-#ifdef UART0_TX_INT_MODE
- // initialize the transmit data queue
- uart0_tx_extract_idx = 0;
- uart0_tx_insert_idx = 0;
- uart0_tx_running = 0;
-#endif
-
-#ifdef UART0_RX_INT_MODE
- // initialize the receive data queue
- uart0_rx_extract_idx = 0;
- uart0_rx_insert_idx = 0;
-
- // enable receiver interrupts
- U0IER = UIER_ERBFI;
-#endif
-#endif
-}
void uart0_ISR(void)
{
@@ -195,3 +194,202 @@ void uart0_ISR(void)
VICVectAddr = 0x00000000; // clear this interrupt from the VIC
ISR_EXIT(); // recover registers and return
}
+
+/*
+ *
+ * UART1 handling functions - those are pale copies of UART0 ones
+ * We should probably find a better way to make the code configurable
+ * for both uarts
+ *
+ */
+#define UART1_TX_INT_MODE 1
+#define UART1_RX_INT_MODE 1
+
+#if defined(UART1_TX_INT_MODE) || defined(UART1_RX_INT_MODE)
+void uart1_ISR(void) __attribute__((naked));
+#endif
+
+#ifdef UART1_RX_INT_MODE
+uint8_t uart1_rx_buffer[UART1_RX_BUFFER_SIZE];
+uint16_t uart1_rx_insert_idx, uart1_rx_extract_idx;
+#endif
+
+#ifdef UART1_TX_INT_MODE
+uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE];
+uint16_t uart1_tx_insert_idx, uart1_tx_extract_idx;
+uint8_t uart1_tx_running;
+#endif
+
+static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
+
+void uart1_init_tx( void ) {
+ uart1_init_param(UART_BAUD(38400), UART_8N1, UART_FIFO_8);
+}
+
+void uart1_init_rx( void ) {}
+
+static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
+ // set port pins for UART1
+ PINSEL0 = (PINSEL0 & ~U1_PINMASK) | U1_PINSEL;
+
+ U1IER = 0x00; // disable all interrupts
+ U1IIR; // clear interrupt ID
+ U1RBR; // clear receive register
+ U1LSR; // clear line status register
+
+ // set the baudrate
+ U1LCR = ULCR_DLAB_ENABLE; // select divisor latches
+ U1DLL = (uint8_t)baud; // set for baud low byte
+ U1DLM = (uint8_t)(baud >> 8); // set for baud high byte
+
+ // set the number of characters and other
+ // user specified operating parameters
+ U1LCR = (mode & ~ULCR_DLAB_ENABLE);
+ U1FCR = fmode;
+#if defined(UART1_TX_INT_MODE) || defined(UART1_RX_INT_MODE)
+ // initialize the interrupt vector
+ VICIntSelect &= ~VIC_BIT(VIC_UART1); // UART0 selected as IRQ
+ VICIntEnable = VIC_BIT(VIC_UART1); // UART0 interrupt enabled
+ VICVectCntl6 = VIC_ENABLE | VIC_UART1;
+ VICVectAddr6 = (uint32_t)uart1_ISR; // address of the ISR
+
+#ifdef UART1_TX_INT_MODE
+ // initialize the transmit data queue
+ uart1_tx_extract_idx = 0;
+ uart1_tx_insert_idx = 0;
+ uart1_tx_running = 0;
+#endif
+
+#ifdef UART1_RX_INT_MODE
+ // initialize the receive data queue
+ uart1_rx_extract_idx = 0;
+ uart1_rx_insert_idx = 0;
+
+ // enable receiver interrupts
+ U1IER = UIER_ERBFI;
+#endif
+#endif
+}
+
+bool_t uart1_check_free_space( uint8_t len) {
+ int16_t space;
+ if ((space = (uart1_tx_extract_idx - uart1_tx_insert_idx)) <= 0)
+ space += UART1_TX_BUFFER_SIZE;
+
+ return (uint16_t)(space - 1) >= len;
+}
+
+void uart1_transmit( unsigned char data ) {
+#ifdef UART1_TX_INT_MODE
+ uint16_t temp;
+ unsigned cpsr;
+
+ temp = (uart1_tx_insert_idx + 1) % UART1_TX_BUFFER_SIZE;
+
+ if (temp == uart1_tx_extract_idx)
+ // return -1; // no room
+ return; // no room
+
+ cpsr = disableIRQ(); // disable global interrupts
+ U1IER &= ~UIER_ETBEI; // disable TX interrupts
+ restoreIRQ(cpsr); // restore global interrupts
+
+ // check if in process of sending data
+ if (uart1_tx_running)
+ {
+ // add to queue
+ uart1_tx_buffer[uart1_tx_insert_idx] = (uint8_t)data;
+ uart1_tx_insert_idx = temp;
+ }
+ else
+ {
+ // set running flag and write to output register
+ uart1_tx_running = 1;
+ U1THR = (uint8_t)data;
+ }
+
+ cpsr = disableIRQ(); // disable global interrupts
+ U1IER |= UIER_ETBEI; // enable TX interrupts
+ restoreIRQ(cpsr); // restore global interrupts
+#else
+ while (!(U1LSR & ULSR_THRE)) // wait for TX buffer to empty
+ continue; // also either WDOG() or swap()
+
+ U1THR = (uint8_t)ch;
+#endif
+ // return (uint8_t)ch;
+}
+
+void uart1_ISR(void)
+{
+ uint8_t iid;
+
+ // perform proper ISR entry so thumb-interwork works properly
+ ISR_ENTRY();
+
+ // loop until not more interrupt sources
+ while (((iid = U1IIR) & UIIR_NO_INT) == 0)
+ {
+ // identify & process the highest priority interrupt
+ switch (iid & UIIR_ID_MASK)
+ {
+ case UIIR_RLS_INT: // Receive Line Status
+ U1LSR; // read LSR to clear
+ break;
+
+#ifdef UART1_RX_INT_MODE
+ case UIIR_CTI_INT: // Character Timeout Indicator
+ case UIIR_RDA_INT: // Receive Data Available
+ do
+ {
+ uint16_t temp;
+
+ // calc next insert index & store character
+ temp = (uart1_rx_insert_idx + 1) % UART1_RX_BUFFER_SIZE;
+ uart1_rx_buffer[uart1_rx_insert_idx] = U1RBR;
+
+ // check for more room in queue
+ if (temp != uart1_rx_extract_idx)
+ uart1_rx_insert_idx = temp; // update insert index
+ }
+ while (U1LSR & ULSR_RDR);
+
+ break;
+#endif
+
+#ifdef UART1_TX_INT_MODE
+ case UIIR_THRE_INT: // Transmit Holding Register Empty
+ while (U1LSR & ULSR_THRE)
+ {
+ // check if more data to send
+ if (uart1_tx_insert_idx != uart1_tx_extract_idx)
+ {
+ U1THR = uart1_tx_buffer[uart1_tx_extract_idx++];
+ uart1_tx_extract_idx %= UART1_TX_BUFFER_SIZE;
+ }
+ else
+ {
+ // no
+ uart1_tx_running = 0; // clear running flag
+ break;
+ }
+ }
+
+ break;
+#endif // UART1_TX_INT_MODE
+
+ case UIIR_MS_INT: // MODEM Status
+ U1MSR; // read MSR to clear
+ break;
+
+ default: // Unknown
+ U1LSR;
+ U1RBR;
+ U1MSR;
+ break;
+ }
+ }
+
+ VICVectAddr = 0x00000000; // clear this interrupt from the VIC
+ ISR_EXIT(); // recover registers and return
+}
diff --git a/sw/airborne/avr/int.h b/sw/airborne/avr/interrupt_hw.h
similarity index 97%
rename from sw/airborne/avr/int.h
rename to sw/airborne/avr/interrupt_hw.h
index 08f9f5fefa..c6b732a754 100644
--- a/sw/airborne/avr/int.h
+++ b/sw/airborne/avr/interrupt_hw.h
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*
*/
-/** \file int.h
+/** \file interrupt_hw.h
* \brief AVR Low level interrupt handling
*
*/
diff --git a/sw/airborne/avr/low_level_hw.h b/sw/airborne/avr/low_level_hw.h
deleted file mode 100644
index dfd17852fe..0000000000
--- a/sw/airborne/avr/low_level_hw.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/*
- *\brief AVR low level hardware initialisation
- */
-
-#ifndef LOW_LEVEL_HW_H
-#define LOW_LEVEL_HW_H
-
-#define low_level_init() {}
-
-#endif /* LOW_LEVEL_HW_H */
diff --git a/sw/airborne/main_ap_2.c b/sw/airborne/main_ap_2.c
index 149749f341..ce27867363 100644
--- a/sw/airborne/main_ap_2.c
+++ b/sw/airborne/main_ap_2.c
@@ -1,6 +1,6 @@
#include "main_ap.h"
#include "init_hw.h"
-#include "int.h"
+#include "interrupt_hw.h"
#include "sys_time.h"
#include "led.h"
#include "modem.h"
@@ -29,6 +29,12 @@ int8_t nav_utm_zone0;
float estimator_phi;
float estimator_psi;
float estimator_theta;
+float estimator_x;
+float estimator_y;
+float estimator_z;
+float estimator_hspeed_mod;
+float estimator_hspeed_dir;
+float estimator_z_dot;
bool_t in_segment;
int16_t segment_x_1;
int16_t segment_y_1;
diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c
index f37c235dbb..3e8dd2f4bf 100644
--- a/sw/airborne/main_fbw.c
+++ b/sw/airborne/main_fbw.c
@@ -25,10 +25,11 @@
#include "main_fbw.h"
#include "init_hw.h"
-#include "int.h"
+#include "interrupt_hw.h"
+
#include "sys_time.h"
-#include "actuators.h"
#include "commands.h"
+#include "actuators.h"
#include "radio_control.h"
#include "fbw_downlink.h"
diff --git a/sw/airborne/main_fbw_2.c b/sw/airborne/main_fbw_2.c
deleted file mode 100644
index 299b096b24..0000000000
--- a/sw/airborne/main_fbw_2.c
+++ /dev/null
@@ -1,86 +0,0 @@
-#include "main_fbw.h"
-
-#include "low_level_hw.h"
-#include "int.h"
-#include "sys_time.h"
-#include "led.h"
-#include "ppm.h"
-#include "radio_control.h"
-#include "command.h"
-#include "control.h"
-#include "autopilot_fbw.h"
-
-#include "traces.h"
-
-void init_fbw( void ) {
- low_level_init();
- sys_time_init();
-#ifdef LED
- led_init();
-#endif /* LED */
-#ifdef ACTUATORS
- command_init();
-#endif /* ACTUATORS */
-#ifdef RADIO_CONTROL
- ppm_init();
- radio_control_init();
-#endif /* RADIO_CONTROL */
-#ifdef _3DMG
-
-#endif /* _3DMG */
-
-#ifdef TRACES
- TRACES_INIT();
- // uart1_init();
-#endif /* TRACES */
-
- /* if FBW is running in a separate MCU */
-#ifndef AP
-#endif /* not AP */
- int_enable();
-}
-
-void periodic_task_fbw( void ) {
-#ifdef RADIO_CONTROL
- if (radio_control_periodic_task()) {
- if (rc_status == RC_REALLY_LOST)
- command_set(failsafe_values);
- }
-#endif /* RADIO_CONTROL */
-}
-
-void event_task_fbw( void ) {
-#ifdef RADIO_CONTROL
- if (radio_control_ppm_event()) {
-#ifdef TRACES
- {
- static uint16_t foo;
- foo++;
- if (!(foo%8)) {
- PRINT_RADIO_CONTROL();
- }
- }
-#endif /* TRACES */
-#ifdef AUTOPILOT
- autopilot_process_radio_control();
-#ifdef CONTROL
- if (autopilot_mode == MODE_MANUAL) {
- control_process_radio_control_MANUAL();
- command_set(control_commands);
-#ifdef TRACES
- {
- static uint16_t foo;
- foo++;
- if (!(foo%8)) {
- PRINT_RADIO_CONTROL();
- PRINT_CONTROL_COMMANDS();
- }
- }
-#endif /* TRACES */
- }
-#endif /* CONTROL */
-#endif /* AUTOPILOT */
- }
-#endif /* RADIO_CONTROL */
-}
-
diff --git a/sw/airborne/mainloop.c b/sw/airborne/mainloop.c
index 86e4adbfb1..fe83b5e62d 100644
--- a/sw/airborne/mainloop.c
+++ b/sw/airborne/mainloop.c
@@ -25,8 +25,8 @@
*/
#include "main_ap.h"
-#include "int.h"
-#include "low_level_hw.h"
+#include "init_hw.h"
+#include "interrupt_hw.h"
#include "sys_time.h"
#include "adc.h"
#include "autopilot.h"
@@ -64,7 +64,7 @@ void init_ap( void ) {
led_init();
#endif
#ifndef FBW /** Dual mcus : init done in main_fbw */
- low_level_init();
+ hw_init();
sys_time_init();
#ifdef ADC
adc_init();
diff --git a/sw/airborne/uart.h b/sw/airborne/uart.h
index 70bf981fa7..a5f3f52069 100644
--- a/sw/airborne/uart.h
+++ b/sw/airborne/uart.h
@@ -31,8 +31,8 @@
void uart0_init_tx( void );
+/** uart0_init_rx() is optional but must be called after uart0_init_tx */
void uart0_init_rx( void );
-/** uart0_init_tx() must be called BEFORE */
void uart0_transmit( unsigned char data );
bool_t uart0_check_free_space( uint8_t len);
@@ -40,10 +40,11 @@ bool_t uart0_check_free_space( uint8_t len);
/** Not necessarily defined */
void uart1_init_tx( void );
+/** uart1_init_rx() is optional but must be called after uart1_init_tx */
void uart1_init_rx( void );
-/** uart1_init_tx() must be called BEFORE */
void uart1_transmit( unsigned char data );
+bool_t uart1_check_free_space( uint8_t len);
#define Uart0Init() { uart0_init_tx(); uart0_init_rx(); }
#define Uart1Init() { uart1_init_tx(); uart1_init_rx(); }