diff --git a/Makefile b/Makefile index 5d7372c067..fac3ea6382 100644 --- a/Makefile +++ b/Makefile @@ -166,3 +166,17 @@ clean: find . -name '*~' -exec rm -f {} \; dist_clean : clean + + + +test_all_example_airframes: + make AIRCRAFT=Tux clean_ac hard_ac + make AIRCRAFT=Plaster clean_ac hard_ac + make AIRCRAFT=Twin1 clean_ac hard_ac + make AIRCRAFT=Twin2 clean_ac hard_ac + make AIRCRAFT=MJ1 clean_ac hard_ac + make AIRCRAFT=TS5 clean_ac fbw + make AIRCRAFT=TJ1 clean_ac ap + make AIRCRAFT=MJ4 clean_ac ap + make AIRCRAFT=GRZE3 clean_ac ap + make AIRCRAFT=Twin4 clean_ac hard_ac diff --git a/conf/airframes/flyingtux.xml b/conf/airframes/flyingtux.xml index d906bff56a..37ebc70e12 100644 --- a/conf/airframes/flyingtux.xml +++ b/conf/airframes/flyingtux.xml @@ -76,7 +76,7 @@ - +
@@ -104,11 +104,28 @@
+ +
+ +
+ include $(PAPARAZZI_SRC)/conf/autopilot/v1_2.makefile +fbw.srcs += commands.c + +fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" +fbw.srcs += $(SRC_ARCH)/servos_4017.c + +fbw.CFLAGS += -DRADIO_CONTROL +fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +fbw.CFLAGS += -DDOWNLINK -DUART0 +fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + ap.CFLAGS += -DINFRARED -DGPS -DUBX # -DRADIO_CONTROL_CALIB # ap.EXTRA_SRCS += if_calib.c + diff --git a/conf/airframes/gorrazoptere_esc_3DMG.xml b/conf/airframes/gorrazoptere_esc_3DMG.xml index 9c58768a84..58c1837466 100644 --- a/conf/airframes/gorrazoptere_esc_3DMG.xml +++ b/conf/airframes/gorrazoptere_esc_3DMG.xml @@ -133,9 +133,25 @@ + +
+ +
+ include $(PAPARAZZI_SRC)/conf/autopilot/disc_board.makefile +ap.srcs += commands.c + +ap.CFLAGS += -DACTUATORS=\"servos_esc_hw.h\" -DTIMER3 -DTIMER1_TOP=0x400 +ap.srcs += $(SRC_ARCH)/servos_esc_hw.c + +ap.CFLAGS += -DRADIO_CONTROL +ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +ap.CFLAGS += -DDOWNLINK -DUART0 +ap.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + # ap.CFLAGS += -DDEBUG_RC # LOCAL_CFLAGS += -DIMU_3DMG diff --git a/conf/airframes/microjet1.xml b/conf/airframes/microjet1.xml index e6d7528af2..b780fab2eb 100644 --- a/conf/airframes/microjet1.xml +++ b/conf/airframes/microjet1.xml @@ -1,13 +1,6 @@ -
- - - - - -
- + @@ -25,6 +18,11 @@ +
+ + +
+ @@ -33,12 +31,16 @@ -
- - + +
+ + + + +
-
+
@@ -77,7 +79,7 @@ - +
@@ -95,8 +97,23 @@
- + +
+ +
+ + include $(PAPARAZZI_SRC)/conf/autopilot/v1_2.makefile +fbw.srcs += commands.c + +fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" +fbw.srcs += $(SRC_ARCH)/servos_4017.c + +fbw.CFLAGS += -DRADIO_CONTROL +fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +fbw.CFLAGS += -DDOWNLINK -DUART0 +fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c ap.CFLAGS += -DGPS -DUBX -DINFRARED diff --git a/conf/airframes/microjet4.xml b/conf/airframes/microjet4.xml index fec10a1443..c1bcc62731 100644 --- a/conf/airframes/microjet4.xml +++ b/conf/airframes/microjet4.xml @@ -14,6 +14,12 @@ + + + + + +
@@ -27,17 +33,12 @@ - - - - - - - - +
+ +
-include $(PAPARAZZI_SRC)/conf/autopilot/tiny_test.makefile +include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile #ap.CFLAGS += -DTRACES=uart0 #ap.EXTRA_SRCS += $(SRC_ARCH)/traces.c $(SRC_ARCH)/uart.c #ap.CFLAGS += -DRADIO_CONTROL @@ -55,13 +56,18 @@ include $(PAPARAZZI_SRC)/conf/autopilot/tiny_test.makefile #ap.EXTRA_SRCS += $(SRC_ARCH)/modem_hw.c #ap.CFLAGS += -DDOWNLINK -ap.srcs += commands.c inter_mcu.c +ap.srcs += commands.c -ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -ap.srcs += $(SRC_ARCH)/servos_4017_hw.c +ap.CFLAGS += -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 +ap.srcs += $(SRC_ARCH)/servos_4015_hw.c ap.CFLAGS += -DRADIO_CONTROL -ap.srcs += $(SRC_ARCH)/ppm_hw.c +ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +ap.CFLAGS += -DDOWNLINK -DUSE_UART0 +ap.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + +ap.srcs += inter_mcu.c $(SRC_ARCH)/adc_hw.c diff --git a/conf/airframes/plaster1.xml b/conf/airframes/plaster1.xml index 1f8e91873c..1911afbcc7 100755 --- a/conf/airframes/plaster1.xml +++ b/conf/airframes/plaster1.xml @@ -106,10 +106,29 @@
+ +
+ +
+ include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile +fbw.srcs += commands.c + +fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017 +fbw.srcs += $(SRC_ARCH)/servos_4017.c + +fbw.CFLAGS += -DRADIO_CONTROL +fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +fbw.CFLAGS += -DDOWNLINK -DUART0 +fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + ap.CFLAGS += -DGPS -DUBX -DINFRARED -fbw.CFLAGS += -DDEBUG +fbw.CFLAGS += -DDEBUG + + + diff --git a/conf/airframes/twinjet1.xml b/conf/airframes/twinjet1.xml index 203134edd6..d5c34ede69 100644 --- a/conf/airframes/twinjet1.xml +++ b/conf/airframes/twinjet1.xml @@ -87,15 +87,36 @@
+ +
+ +
include $(PAPARAZZI_SRC)/conf/autopilot/robostix.makefile + +ap.srcs += commands.c + +ap.CFLAGS += -DACTUATORS=\"servos_direct_hw.h\" +ap.srcs += $(SRC_ARCH)/servos_direct_hw.c + +ap.CFLAGS += -DRADIO_CONTROL +ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +ap.CFLAGS += -DDOWNLINK -DUART0 +ap.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + #ap.CFLAGS += -DCONTROL -DAUTOPILOT #ap.EXTRA_SRCS += $(ACINCLUDE)/ap/control.c #ap.CFLAGS += -DADC #ap.EXTRA_SRCS += $(SRC_ARCH)/adc_hw.c #ap.CFLAGS += -DDATALINK #ap.EXTRA_SRCS += datalink.c nav.c estimator.c + + + + + diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml index 85b2631ff5..1997c71dd4 100644 --- a/conf/airframes/twinstar1.xml +++ b/conf/airframes/twinstar1.xml @@ -1,19 +1,11 @@ -
- -
-
- - -
- - - - + + + - - + + @@ -30,6 +22,11 @@ +
+ + +
+ @@ -39,10 +36,11 @@ -
- - +
+ +
+
@@ -104,9 +102,25 @@
- + +
+ +
+ + include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile +fbw.srcs += commands.c + +fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017 +fbw.srcs += $(SRC_ARCH)/servos_4017.c + +fbw.CFLAGS += -DRADIO_CONTROL +fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +fbw.CFLAGS += -DDOWNLINK -DUART0 +fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + ap.CFLAGS += -DGPS -DUBX -DINFRARED ap.CFLAGS += -Werror diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml index 46e0208748..b31af07255 100644 --- a/conf/airframes/twinstar2.xml +++ b/conf/airframes/twinstar2.xml @@ -1,9 +1,6 @@ -
- - -
- + + @@ -29,6 +26,11 @@ +
+ + +
+ @@ -37,10 +39,12 @@ -
- - + +
+ +
+
@@ -102,9 +106,27 @@
+ +
+ +
+ include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile +fbw.srcs += commands.c + +fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017 +fbw.srcs += $(SRC_ARCH)/servos_4017.c + +fbw.CFLAGS += -DRADIO_CONTROL +fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +fbw.CFLAGS += -DDOWNLINK -DUART0 +fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + +fbw.CFLAGS += -Werror + ap.CFLAGS += -DGPS -DUBX -DINFRARED ap.CFLAGS += -DDATALINK ap.EXTRA_SRCS += traffic_info.c datalink.c diff --git a/conf/airframes/twinstar4.xml b/conf/airframes/twinstar4.xml index 54d723e8ae..e11f620aea 100755 --- a/conf/airframes/twinstar4.xml +++ b/conf/airframes/twinstar4.xml @@ -1,9 +1,6 @@ -
- - -
- + + @@ -25,6 +22,17 @@ + + + + + + +
+ + +
+ @@ -36,10 +44,12 @@ -
- - +
+ +
+ +
@@ -81,7 +91,7 @@ - +
@@ -99,9 +109,27 @@
- + +
+ +
+ + include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile +fbw.srcs += commands.c + +fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017 +fbw.srcs += $(SRC_ARCH)/servos_4017.c + +fbw.CFLAGS += -DRADIO_CONTROL +fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +fbw.CFLAGS += -DDOWNLINK -DUART0 +fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + +fbw.CFLAGS += -DDEBUG + ap.CFLAGS += -DINFRARED -DGPS -DUBX -DRADIO_CONTROL_CALIB ap.CFLAGS += -DLED -DLED_1_BANK=D -DLED_1_PIN=0 -DLED_2_BANK=D -DLED_2_PIN=1 ap.EXTRA_SRCS += if_calib.c diff --git a/conf/airframes/twinstar5.xml b/conf/airframes/twinstar5.xml index 51126e99a1..fe79bee7a7 100644 --- a/conf/airframes/twinstar5.xml +++ b/conf/airframes/twinstar5.xml @@ -26,6 +26,11 @@ +
+ + +
+ @@ -37,10 +42,6 @@ -
- - -
@@ -109,6 +110,11 @@
+
+ +
+ + ap.CFLAGS += -DTIME_LED=2 include $(PAPARAZZI_SRC)/conf/autopilot/classix.makefile @@ -123,13 +129,16 @@ ap.srcs += $(SRC_ARCH)/modem_hw.c fbw.srcs += commands.c -fbw.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" +fbw.CFLAGS += -DDOWNLINK -DUSE_UART0 +fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c + +fbw.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING fbw.srcs += $(SRC_ARCH)/servos_4017_hw.c fbw.CFLAGS += -DRADIO_CONTROL fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c -fbw.srcs += inter_mcu.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/adc_hw.c +fbw.srcs += inter_mcu.c $(SRC_ARCH)/adc_hw.c diff --git a/conf/autopilot/disc_board.makefile b/conf/autopilot/disc_board.makefile index fca3e41173..38eff4967e 100644 --- a/conf/autopilot/disc_board.makefile +++ b/conf/autopilot/disc_board.makefile @@ -8,6 +8,6 @@ ap.LOW_FUSE = a0 ap.HIGH_FUSE = 99 ap.EXT_FUSE = ff ap.LOCK_FUSE = ff -ap.CFLAGS += -DFBW -DCONFIG=\"config_discboard.h\" -DTIMER3 -DTIMER1_TOP=0x400 -DRADIO_CONTROL -DACTUATORS=\"servos_esc_hw.h\" -DUART0 -DDOWNLINK -ap.srcs = sys_time.c radio_control.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/adc_hw.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_esc_hw.c commands.c main_fbw.c main.c -# inter_mcu.c pid.c estimator.c if_calib.c nav.c main_ap.c mainloop.c main.c +ap.CFLAGS += -DFBW -DCONFIG=\"config_discboard.h\" +ap.srcs = inter_mcu.c $(SRC_ARCH)/adc_hw.c sys_time.c main_fbw.c main.c +# pid.c estimator.c if_calib.c nav.c main_ap.c mainloop.c main.c diff --git a/conf/autopilot/robostix.makefile b/conf/autopilot/robostix.makefile index 2116e1a93e..7279ca676c 100644 --- a/conf/autopilot/robostix.makefile +++ b/conf/autopilot/robostix.makefile @@ -10,6 +10,6 @@ ap.LOW_FUSE = 0xbf ap.HIGH_FUSE = 0xc9 ap.EXT_FUSE = ff ap.LOCK_FUSE = ff -ap.CFLAGS += -DFBW -DCONFIG=\"config_robostix.h\" -DLED -DRADIO_CONTROL -DACTUATORS=\"servos_direct_hw.h\" -DUART0 -ap.srcs = sys_time.c radio_control.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/adc_hw.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_direct_hw.c commands.c main_fbw.c main.c +ap.CFLAGS += -DFBW -DCONFIG=\"config_robostix.h\" -DLED +ap.srcs = sys_time.c $(SRC_ARCH)/adc_hw.c main_fbw.c main.c # ap.srcs += main_ap.c diff --git a/conf/autopilot/tiny.makefile b/conf/autopilot/tiny.makefile index 2945ae5f83..ada4008b6e 100644 --- a/conf/autopilot/tiny.makefile +++ b/conf/autopilot/tiny.makefile @@ -6,6 +6,8 @@ ap.ARCHDIR = $(ARCHI) ap.ARCH = arm7tdmi ap.TARGET = autopilot ap.TARGETDIR = autopilot -ap.CFLAGS += -DAP -DFBW -DCONFIG=\"config_tiny.h\" -DLED -DRADIO_CONTROL -DACTUATORS -DGPS -DUBX -ap.srcs = estimator.c gps_ubx.c gps.c $(SRC_ARCH)/uart.c radio_control.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/servos_hw.c $(SRC_ARCH)/armVIC.c sys_time.c main_fbw_2.c main_ap_2.c main.c - +ap.CFLAGS += -DFBW -DCONFIG=\"config_tiny.h\" +ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main.c +#ap.CFLAGS += -DAP -DLED -DGPS -DUBX +#ap.srcs = main_ap_2.c +# estimator.c gps_ubx.c gps.c diff --git a/conf/autopilot/twin_avr.makefile b/conf/autopilot/twin_avr.makefile index 17a5536084..99fc6e449c 100644 --- a/conf/autopilot/twin_avr.makefile +++ b/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 -DRADIO_CONTROL -DINTER_MCU -DMCU_SPI_LINK -DACTUATORS=\"servos_4017.h\" -DUART0 -DDOWNLINK -fbw.srcs = $(SRC_ARCH)/ppm_hw.c commands.c radio_control.c inter_mcu.c $(SRC_ARCH)/servos_4017.c $(SRC_ARCH)/spi_fbw.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/adc_hw.c sys_time.c main_fbw.c main.c +fbw.CFLAGS += -DFBW -DINTER_MCU -DMCU_SPI_LINK +fbw.srcs = inter_mcu.c $(SRC_ARCH)/spi_fbw.c $(SRC_ARCH)/adc_hw.c sys_time.c main_fbw.c main.c diff --git a/conf/messages.xml b/conf/messages.xml index 9101fb8b33..1a9871c8a4 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -275,8 +275,6 @@ - - diff --git a/sw/airborne/actuators.h b/sw/airborne/actuators.h index c78e1f0e7d..cd16a07f2b 100644 --- a/sw/airborne/actuators.h +++ b/sw/airborne/actuators.h @@ -26,9 +26,8 @@ #if defined ACTUATORS #include "paparazzi.h" - -#include "airframe.h" /** Defines SetActuatorsFromCommands() macro */ +#include "airframe.h" extern void actuators_init( void ); diff --git a/sw/airborne/arm7/servos_4015_hw.c b/sw/airborne/arm7/servos_4015_hw.c index cbd33137d8..2979af53f8 100644 --- a/sw/airborne/arm7/servos_4015_hw.c +++ b/sw/airborne/arm7/servos_4015_hw.c @@ -1,16 +1,48 @@ -#include "servos_4015_hw.h" -#include "command.h" +#include "actuators.h" #include "armVIC.h" #include "airframe.h" #include "sys_time.h" -//#include "led.h" -const pprz_t failsafe_values[COMMANDS_NB] = COMMANDS_FAILSAFE; + + #define COMMAND(i) servos_values[i] #define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s) +void actuators_init ( void ) { + /* PWM selected as IRQ */ + VICIntSelect &= ~VIC_BIT(VIC_PWM); + /* PWM interrupt enabled */ + VICIntEnable = VIC_BIT(VIC_PWM); + VICVectCntl3 = VIC_ENABLE | VIC_PWM; + /* address of the ISR */ + VICVectAddr3 = (uint32_t)PWM_ISR; + /* PW5 pin (P0.21) used for PWM */ + IO0DIR |= _BV(SERV0_CLOCK_PIN); + IO1DIR |= _BV(SERV0_DATA_PIN) | _BV(SERV0_RESET_PIN); + SERV0_CLOCK_PINSEL |= SERV0_CLOCK_PINSEL_VAL << SERV0_CLOCK_PINSEL_BIT; + /* set match5 to go of a long time from now */ + PWMMR0 = 0XFFFFFF; + //PWMMR0 = CLOCK_OF_US(1500); + PWMMR5 = 0XFFF; + /* commit above change */ + PWMLER = PWMLER_LATCH0 | PWMLER_LATCH5; + /* interrupt on PWMMR5 match */ + PWMMCR = PWMMCR_MR0R | PWMMCR_MR5I; + /* enable PWM5 ouptput */ + PWMPCR = PWMPCR_ENA5; + /* enable PWM timer counter and PWM mode */ + PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE; + /* Load failsafe values */ + /* Set all servos at their midpoints */ + /* compulsory for unaffected servos */ + uint8_t i; + for( i=0 ; i < _4015_NB_CHANNELS ; i++ ) + servos_values[i] = SERVOS_TICS_OF_USEC(1500); +} + +uint16_t servos_values[_4015_NB_CHANNELS]; #define CLOCK_OF_US(us) ((us)*(PCLK/1000000)) @@ -20,14 +52,7 @@ const pprz_t failsafe_values[COMMANDS_NB] = COMMANDS_FAILSAFE; #define SERV7_START_POS 1600 -#define NB_SERVOS 4 #define SERVO_REFRESH_US 25000 -uint32_t servos_values[NB_SERVOS] = - { CLOCK_OF_US(SERV4_START_POS), - CLOCK_OF_US(SERV5_START_POS), - CLOCK_OF_US(SERV6_START_POS), - CLOCK_OF_US(SERV7_START_POS) - }; uint32_t servos_delay = CLOCK_OF_US(SERVO_REFRESH_US - SERV4_START_POS - SERV5_START_POS - SERV6_START_POS - SERV7_START_POS) / 2; uint8_t servos_idx = 0; @@ -44,7 +69,7 @@ void PWM_ISR ( void ) { PWMLER = PWMLER_LATCH0; servos_idx++; } - else if (servos_idx < NB_SERVOS) { + else if (servos_idx < _4015_NB_CHANNELS) { IO1CLR = _BV(SERV0_DATA_PIN); PWMMR0 = servos_values[servos_idx]; PWMLER = PWMLER_LATCH0; diff --git a/sw/airborne/arm7/servos_4015_hw.h b/sw/airborne/arm7/servos_4015_hw.h index 7700351993..3950439dc3 100644 --- a/sw/airborne/arm7/servos_4015_hw.h +++ b/sw/airborne/arm7/servos_4015_hw.h @@ -1,41 +1,22 @@ #ifndef SERVOS_4015_HW_H #define SERVOS_4015_HW_H +#include #include "std.h" + #include "LPC21xx.h" +#include "sys_time.h" + #include CONFIG +#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s) +#define ChopServo(x,a,b) Chop(x, a, b) +#define _4015_NB_CHANNELS 8 +extern uint16_t servos_values[_4015_NB_CHANNELS]; +#define Actuator(i) servos_values[i] void PWM_ISR ( void ) __attribute__((naked)); -static inline void command_init ( void ) { - /* PWM selected as IRQ */ - VICIntSelect &= ~VIC_BIT(VIC_PWM); - /* PWM interrupt enabled */ - VICIntEnable = VIC_BIT(VIC_PWM); - VICVectCntl3 = VIC_ENABLE | VIC_PWM; - /* address of the ISR */ - VICVectAddr3 = (uint32_t)PWM_ISR; - /* PW5 pin (P0.21) used for PWM */ - IO0DIR |= _BV(SERV0_CLOCK_PIN); - IO1DIR |= _BV(SERV0_DATA_PIN) | _BV(SERV0_RESET_PIN); - SERV0_CLOCK_PINSEL |= SERV0_CLOCK_PINSEL_VAL << SERV0_CLOCK_PINSEL_BIT; - - /* set match5 to go of a long time from now */ - PWMMR0 = 0XFFFFFF; - //PWMMR0 = CLOCK_OF_US(1500); - PWMMR5 = 0XFFF; - /* commit above change */ - PWMLER = PWMLER_LATCH0 | PWMLER_LATCH5; - /* interrupt on PWMMR5 match */ - PWMMCR = PWMMCR_MR0R | PWMMCR_MR5I; - /* enable PWM5 ouptput */ - PWMPCR = PWMPCR_ENA5; - /* enable PWM timer counter and PWM mode */ - PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE; - /* Load failsafe values */ - command_set(failsafe_values); -} #endif /* SERVOS_4015_HW_H */ diff --git a/sw/airborne/arm7/servos_4017_hw.c b/sw/airborne/arm7/servos_4017_hw.c index 31c5c74e1e..1a77a7e4dd 100644 --- a/sw/airborne/arm7/servos_4017_hw.c +++ b/sw/airborne/arm7/servos_4017_hw.c @@ -1,13 +1,7 @@ #include "actuators.h" -#include "LPC21xx.h" -#include CONFIG - -#include "std.h" #include "paparazzi.h" #include "airframe.h" -#include "actuators.h" - uint8_t servos_4017_idx; diff --git a/sw/airborne/arm7/servos_4017_hw.h b/sw/airborne/arm7/servos_4017_hw.h index ace7ded6b3..494252b394 100644 --- a/sw/airborne/arm7/servos_4017_hw.h +++ b/sw/airborne/arm7/servos_4017_hw.h @@ -2,12 +2,13 @@ #define SERVOS_4017_HW_H #include +#include "std.h" + +#include "LPC21xx.h" #include "sys_time.h" #include "led.h" -#define SERVOS_4017_CLOCK_FALLING 1 - #include CONFIG #define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s) diff --git a/sw/airborne/arm7/sys_time_hw.c b/sw/airborne/arm7/sys_time_hw.c index 58bbf286b3..cba34ef318 100644 --- a/sw/airborne/arm7/sys_time_hw.c +++ b/sw/airborne/arm7/sys_time_hw.c @@ -7,19 +7,24 @@ #warning "FIXMEEE - i fear that if two T0IR flags are up together I might discard an IT" #warning "And I've seen crashes: don't fly with that" + +#define TIMER0_IT_MASK (TIR_CR2I | TIR_MR1I) + void TIMER0_ISR ( void ) { ISR_ENTRY(); + if (T0IR&TIR_CR2I) { PPM_ISR(); /* clear interrupt */ T0IR = TIR_CR2I; } +#ifdef SERVOS_4017 else if (T0IR&TIR_MR1I) { SERVOS_4017_ISR(); /* clear interrupt */ T0IR = TIR_MR1I; } - +#endif VICVectAddr = 0x00000000; ISR_EXIT(); } diff --git a/sw/airborne/arm7/uart_hw.c b/sw/airborne/arm7/uart_hw.c index 435c1ecf56..a8524c3712 100644 --- a/sw/airborne/arm7/uart_hw.c +++ b/sw/airborne/arm7/uart_hw.c @@ -35,6 +35,13 @@ void uart0_init_rx( void ) { } +bool_t uart0_check_free_space( uint8_t len) { + int16_t space; + if ((space = (uart0_tx_extract_idx - uart0_tx_insert_idx)) <= 0) + space += UART0_TX_BUFFER_SIZE; + + return (uint16_t)(space - 1) >= len; +} void uart0_transmit( unsigned char data ) { #ifdef UART0_TX_INT_MODE diff --git a/sw/airborne/avr/adc_hw.c b/sw/airborne/avr/adc_hw.c index 6da48cf60f..688d20a026 100644 --- a/sw/airborne/avr/adc_hw.c +++ b/sw/airborne/avr/adc_hw.c @@ -26,7 +26,6 @@ //// ADC6 MVSERVO -#include #include #include #include "adc.h" diff --git a/sw/airborne/avr/link_mcu_ap.c b/sw/airborne/avr/link_mcu_ap.c index 14aea3c70e..9872a0111a 100644 --- a/sw/airborne/avr/link_mcu_ap.c +++ b/sw/airborne/avr/link_mcu_ap.c @@ -23,9 +23,8 @@ */ #include -#include #include -#include +#include #include "inter_mcu.h" #include "link_mcu_ap.h" diff --git a/sw/airborne/avr/modem_hw.c b/sw/airborne/avr/modem_hw.c index 86bb805d2f..647f40fc3b 100644 --- a/sw/airborne/avr/modem_hw.c +++ b/sw/airborne/avr/modem_hw.c @@ -28,7 +28,6 @@ #include #include -#include #include #include #include "modem.h" diff --git a/sw/airborne/avr/ppm_hw.c b/sw/airborne/avr/ppm_hw.c index 5d37afce60..b429821038 100644 --- a/sw/airborne/avr/ppm_hw.c +++ b/sw/airborne/avr/ppm_hw.c @@ -23,7 +23,7 @@ * */ -#include +#include #include "ppm.h" #include "sys_time.h" @@ -67,8 +67,6 @@ SIGNAL( SIG_INPUT_CAPTURE1 ) static uint8_t state = 0; static uint8_t sync_start; -#include "led.h" - this = ICR1; #ifdef TIMER1_TOP this += tmr1_ov_cnt; diff --git a/sw/airborne/avr/servos_4017.c b/sw/airborne/avr/servos_4017.c index 39b589b605..5d96b1340d 100644 --- a/sw/airborne/avr/servos_4017.c +++ b/sw/airborne/avr/servos_4017.c @@ -25,7 +25,7 @@ /** Implementation of actuators.h */ #include -#include +#include #include "servos_4017.h" #include "actuators.h" #include "sys_time.h" diff --git a/sw/airborne/avr/spi_ap.c b/sw/airborne/avr/spi_ap.c index 9bc6950a6d..76462a9ea8 100644 --- a/sw/airborne/avr/spi_ap.c +++ b/sw/airborne/avr/spi_ap.c @@ -24,7 +24,6 @@ #include #include -#include #include diff --git a/sw/airborne/avr/spi_fbw.c b/sw/airborne/avr/spi_fbw.c index 005131a268..f14fa46c03 100644 --- a/sw/airborne/avr/spi_fbw.c +++ b/sw/airborne/avr/spi_fbw.c @@ -26,9 +26,8 @@ #include #include -#include #include -#include +#include #include "inter_mcu.h" #include "spi_fbw.h" diff --git a/sw/airborne/avr/uart_hw.c b/sw/airborne/avr/uart_hw.c index 70b555dbc9..a1ad9dba21 100644 --- a/sw/airborne/avr/uart_hw.c +++ b/sw/airborne/avr/uart_hw.c @@ -22,7 +22,6 @@ * */ -#include #include #include @@ -62,6 +61,14 @@ void uart0_init_rx( void ) { sbi( UCSRB, RXCIE ); } +bool_t uart0_check_free_space( uint8_t len) { + int8_t space; + if ((space = (tx_tail - tx_head)) <= 0) + space += TX_BUF_SIZE; + + return (uint8_t)(space - 1) >= len; +} + void uart0_transmit( unsigned char data ) { if (UCSRB & _BV(TXCIE)) { /* we are waiting for the last char to be sent : buffering */ @@ -101,57 +108,6 @@ 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 uart0_transmit( unsigned char data ) { - if (UCSR0B & _BV(TXCIE)) { - /* we are waiting for the last char to be sent : buffering */ - if (tx_tail0 == tx_head0 + 1) { /* BUF_SIZE = 256 */ - /* Buffer is full (almost, but tx_head = tx_tail means "empty" */ - return; - } - tx_buf0[tx_head0] = data; - tx_head0++; /* BUF_SIZE = 256 */ - } else { /* Channel is free: just send */ - UDR0 = data; - sbi(UCSR0B, TXCIE); - } -} - -void uart1_transmit( unsigned char data ) { - if (UCSR1B & _BV(TXCIE)) { - /* we are waiting for the last char to be sent : buffering */ - if (tx_tail1 == tx_head1 + 1) { /* BUF_SIZE = 256 */ - /* Buffer is full (almost, but tx_head = tx_tail means "empty" */ - return; - } - tx_buf1[tx_head1] = data; - tx_head1++; /* BUF_SIZE = 256 */ - } else { /* Channel is free: just send */ - UDR1 = data; - sbi(UCSR1B, TXCIE); - } -} - - -SIGNAL(SIG_UART0_TRANS) { - if (tx_head0 == tx_tail0) { - /* Nothing more to send */ - cbi(UCSR0B, TXCIE); /* disable interrupt */ - } else { - UDR0 = tx_buf0[tx_tail0]; - tx_tail0++; /* warning tx_buf_len is 256 */ - } -} - -SIGNAL(SIG_UART1_TRANS) { - if (tx_head1 == tx_tail1) { - /* Nothing more to send */ - cbi(UCSR1B, TXCIE); /* disable interrupt */ - } else { - UDR1 = tx_buf1[tx_tail1]; - tx_tail1++; /* warning tx_buf_len is 256 */ - } -} - void uart0_init_tx( void ) { /* Baudrate is 38.4k */ UBRR0H = 0; @@ -177,6 +133,40 @@ void uart0_init_rx( void ) { sbi(UCSR0B, RXCIE ); } +bool_t uart0_check_free_space( uint8_t len) { + int8_t space; + if ((space = (tx_tail0 - tx_head0)) <= 0) + space += TX_BUF_SIZE; + + return (uint16_t)(space - 1) >= len; +} + +void uart0_transmit( unsigned char data ) { + if (UCSR0B & _BV(TXCIE)) { + /* we are waiting for the last char to be sent : buffering */ + if (tx_tail0 == tx_head0 + 1) { /* BUF_SIZE = 256 */ + /* Buffer is full (almost, but tx_head = tx_tail means "empty" */ + return; + } + tx_buf0[tx_head0] = data; + tx_head0++; /* BUF_SIZE = 256 */ + } else { /* Channel is free: just send */ + UDR0 = data; + sbi(UCSR0B, TXCIE); + } +} + +SIGNAL(SIG_UART0_TRANS) { + if (tx_head0 == tx_tail0) { + /* Nothing more to send */ + cbi(UCSR0B, TXCIE); /* disable interrupt */ + } else { + UDR0 = tx_buf0[tx_tail0]; + tx_tail0++; /* warning tx_buf_len is 256 */ + } +} + + void uart1_init_tx( void ) { /* Baudrate is 38.4k */ UBRR1H = 0; @@ -197,6 +187,31 @@ void uart1_init_rx( void ) { sbi(UCSR1B, RXCIE ); } +void uart1_transmit( unsigned char data ) { + if (UCSR1B & _BV(TXCIE)) { + /* we are waiting for the last char to be sent : buffering */ + if (tx_tail1 == tx_head1 + 1) { /* BUF_SIZE = 256 */ + /* Buffer is full (almost, but tx_head = tx_tail means "empty" */ + return; + } + tx_buf1[tx_head1] = data; + tx_head1++; /* BUF_SIZE = 256 */ + } else { /* Channel is free: just send */ + UDR1 = data; + sbi(UCSR1B, TXCIE); + } +} + + +SIGNAL(SIG_UART1_TRANS) { + if (tx_head1 == tx_tail1) { + /* Nothing more to send */ + cbi(UCSR1B, TXCIE); /* disable interrupt */ + } else { + UDR1 = tx_buf1[tx_tail1]; + tx_tail1++; /* warning tx_buf_len is 256 */ + } +} uint8_t uart1_char; bool_t uart1_char_available; diff --git a/sw/airborne/avr/uart_hw.h b/sw/airborne/avr/uart_hw.h index 0e892d80e9..bb826b8799 100644 --- a/sw/airborne/avr/uart_hw.h +++ b/sw/airborne/avr/uart_hw.h @@ -31,7 +31,6 @@ #define UART_HW_H #include -#include #include #include "std.h" diff --git a/sw/airborne/fbw_downlink.h b/sw/airborne/fbw_downlink.h index 93ba7f1cbb..dcc9138306 100644 --- a/sw/airborne/fbw_downlink.h +++ b/sw/airborne/fbw_downlink.h @@ -25,15 +25,57 @@ #ifndef FBW_DOWNLINK_H #define FBW_DOWNLINK_H +#include "inttypes.h" #include "messages_fbw.h" +#include "airframe.h" -#define PERIODIC_SEND_PPM() { Uart0PrintString("SEND_PPM\n"); } +#include "uart.h" +#include "main_fbw.h" +#include "radio_control.h" + +extern uint8_t ck_a, ck_b; +extern uint8_t downlink_nb_ovrn; +#define STX 0x05 + +#define __DownlinkCheckFreeSpace(dev, _x) dev ##_check_free_space(_x) +#define _DownlinkCheckFreeSpace(dev, _x) __DownlinkCheckFreeSpace(dev, _x) +#define DownlinkCheckFreeSpace(_x) _DownlinkCheckFreeSpace(DOWNLINK_FBW_DEVICE, _x) + +#define __DownlinkPut1Byte(dev, _x) dev ##_transmit(_x) +#define _DownlinkPut1Byte(dev, _x) __DownlinkPut1Byte(dev, _x) +#define DownlinkPut1Byte(_x) _DownlinkPut1Byte(DOWNLINK_FBW_DEVICE, _x) + +#define PERIODIC_SEND_PPM() {} +//#define PERIODIC_SEND_SERVOS() { Uart0PrintString("SERVOS\n");} #define PERIODIC_SEND_SERVOS() {} -#define PERIODIC_SEND_FBW_STATUS() {} +#define PERIODIC_SEND_FBW_STATUS() {DOWNLINK_SEND_FBW_STATUS(&fbw_mode, &rc_status, &fbw_mode)} #define PERIODIC_SEND_RC() {} + +/** 5 = STX + ac_id + msg_id + ck_a + ck_b */ +#define DownlinkSizeOf(_payload) (_payload+5) + +#define DownlinkPut1ByteUpdateCs(_byte) { \ + ck_a += _byte; \ + ck_b += ck_a; \ + DownlinkPut1Byte(_byte); \ +} + +#define DownlinkPut1ByteByAddr(_byte) { \ + uint8_t _x = *(_byte); \ + DownlinkPut1ByteUpdateCs(_x); \ +} + +#define DownlinkStartMessage(id) \ + { DownlinkPut1Byte(STX); DownlinkPut1Byte(id); ck_a = id; ck_b = id; DownlinkPut1ByteUpdateCs(AC_ID);} + +#define DownlinkEndMessage() \ + { DownlinkPut1Byte(ck_a); DownlinkPut1Byte(ck_b); } + + static inline void fbw_downlink_periodic_task(void) { PeriodicSend() -} + } + #endif /* FBW_DOWNLINK_H */ diff --git a/sw/airborne/main_ap_2.c b/sw/airborne/main_ap_2.c index f2ee0b1979..4c1d870681 100644 --- a/sw/airborne/main_ap_2.c +++ b/sw/airborne/main_ap_2.c @@ -1,7 +1,7 @@ #include "main_ap.h" -#include "low_level_hw.h" -#include "sys_time.h" +#include "init_hw.h" #include "int.h" +#include "sys_time.h" #include "led.h" #include "modem.h" #include "gps.h" diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c index 771eb211c2..0533deec4f 100644 --- a/sw/airborne/main_fbw.c +++ b/sw/airborne/main_fbw.c @@ -87,8 +87,9 @@ void init_fbw( void ) { #endif sys_time_init(); +#ifdef ACTUATORS actuators_init(); - +#endif /* Load the failsafe defaults */ SetCommands(commands_failsafe); @@ -134,7 +135,6 @@ void event_task_fbw( void) { if (ap_ok && fbw_mode == FBW_MODE_AUTO) { SetCommands(from_ap.from_ap.channels); } - #endif #ifdef IMU_3DMG diff --git a/sw/airborne/uart.h b/sw/airborne/uart.h index 71ddbb7e42..70bf981fa7 100644 --- a/sw/airborne/uart.h +++ b/sw/airborne/uart.h @@ -26,7 +26,7 @@ #define UART_H #include - +#include "std.h" #include "uart_hw.h" void uart0_init_tx( void ); @@ -35,6 +35,7 @@ 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); /** Not necessarily defined */ void uart1_init_tx( void ); diff --git a/sw/ground_segment/tmtc/maxstream.ml b/sw/ground_segment/tmtc/maxstream.ml index 6b52d8039a..6e35a03a87 100644 --- a/sw/ground_segment/tmtc/maxstream.ml +++ b/sw/ground_segment/tmtc/maxstream.ml @@ -25,7 +25,7 @@ open Latlong open Printf module W = Wavecard -module Tm_Pprz = Pprz.Protocol(struct let name = "telemetry_ap" end) +module Tm_Pprz = Pprz.Protocol(struct let name = "telemetry_fbw" end) module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) module Dl_Pprz = Pprz.Protocol(struct let name = "datalink" end) module PprzTransport = Serial.Transport(Tm_Pprz) @@ -182,7 +182,7 @@ let _ = Ivy.start !ivy_bus; try - let fd = Serial.opendev !port Serial.B9600 in + let fd = Serial.opendev !port Serial.B38400 in let ac = { fd=fd; } in (* Listening on wavecard *) let cb = fun _ ->