diff --git a/conf/Makefile.stm32-upload b/conf/Makefile.stm32-upload index e4bc065d22..b43459ef7e 100644 --- a/conf/Makefile.stm32-upload +++ b/conf/Makefile.stm32-upload @@ -154,6 +154,13 @@ upload: $(OBJDIR)/$(TARGET).bin @echo "Using ST-LINK with SWD at $(STLINK_ADDR)" $(Q)st-flash write $^ $(STLINK_ADDR) +else ifeq ($(FLASH_MODE),PX4_BOOTLOADER) + # Program the device and start it. +upload: $(OBJDIR)/$(TARGET).bin + $(PAPARAZZI_SRC)/sw/tools/px4/px_mkfw.py --prototype $(PX4_PROTOTYPE) --image $(OBJDIR)/$(TARGET).bin > $(OBJDIR)/$(TARGET).px4 + $(PAPARAZZI_SRC)/sw/tools/px4/print_message.py + $(PAPARAZZI_SRC)/sw/tools/px4/px_uploader.py --port $(PX4_BL_PORT) $(OBJDIR)/$(TARGET).px4 + # # no known flash mode else diff --git a/conf/airframes/TUDELFT/tudelft_conf.xml b/conf/airframes/TUDELFT/tudelft_conf.xml index 99ffb7ac23..6b7ff13814 100644 --- a/conf/airframes/TUDELFT/tudelft_conf.xml +++ b/conf/airframes/TUDELFT/tudelft_conf.xml @@ -109,6 +109,17 @@ settings_modules="modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml" gui_color="#9b69e5ce9e7a" /> + + + + + + + + + + + diff --git a/conf/airframes/TUDELFT/tudelft_iris_indi.xml b/conf/airframes/TUDELFT/tudelft_iris_indi.xml new file mode 100644 index 0000000000..d151b9df6c --- /dev/null +++ b/conf/airframes/TUDELFT/tudelft_iris_indi.xml @@ -0,0 +1,250 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+
+ + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + +
+ + + + + +
+ + + + + + + +
+ + + +
+ + +
+ + + + + + + +
+
+ + +
+
+ + + + + + + +
+
+ + + + + + + + + + + + + +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + + + + +
+
+ + + + + + + + +
+
+ + +
+
+ + + +
+
+ + + + + +
+
+ + + + + +
+ diff --git a/conf/airframes/TUDELFT/tudelft_iris_pid.xml b/conf/airframes/TUDELFT/tudelft_iris_pid.xml new file mode 100644 index 0000000000..0833fd7434 --- /dev/null +++ b/conf/airframes/TUDELFT/tudelft_iris_pid.xml @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+
+ + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + +
+ + + + + +
+ + + + + + + +
+ + + +
+ + +
+ + + + + + + +
+
+ + +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + + + + +
+
+ + + + + + + + +
+
+ + +
+
+ + + +
+
+ + + + + +
+
+ + + + + +
+ diff --git a/conf/boards/px4fmu_2.4.makefile b/conf/boards/px4fmu_2.4.makefile index 9301b7424a..e1452f66f5 100644 --- a/conf/boards/px4fmu_2.4.makefile +++ b/conf/boards/px4fmu_2.4.makefile @@ -2,7 +2,7 @@ # # px4fmu_2.4.makefile # -# This is for the main MCU (STM32F427) on the pixhawk board +# This is for the main MCU (STM32F427) on the PX4 board # See https://pixhawk.org/modules/pixhawk for details # @@ -19,10 +19,12 @@ $(TARGET).LDSCRIPT=$(SRC_ARCH)/px4fmu_2.4.ld HARD_FLOAT=yes -# default flash mode is via usb dfu bootloader -# possibilities: DFU, SWD -FLASH_MODE ?= SWD - +# default flash mode is the PX4 bootloader +# possibilities: DFU, SWD, PX4 bootloader +FLASH_MODE ?= PX4_BOOTLOADER +PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/px4fmu-v2.prototype" +PX4_BL_PORT ?= "/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/pci-3D_Robotics*" +$(TARGET).MAKEFILE = stm32 # # default LED configuration @@ -40,12 +42,10 @@ SYS_TIME_LED ?= 1 MODEM_PORT ?= UART2 MODEM_BAUD ?= B57600 +#The GPS serial on px4 is called serial 3, but connected to uart4 on the f4 GPS_PORT ?= UART4 GPS_BAUD ?= B38400 -RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 - - # # default actuator configuration # diff --git a/conf/boards/px4io_2.4.makefile b/conf/boards/px4io_2.4.makefile new file mode 100644 index 0000000000..42406c6d1d --- /dev/null +++ b/conf/boards/px4io_2.4.makefile @@ -0,0 +1,48 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# px4io_2.4.makefile +# +# This is for the main MCU (STM32F427) on the PX4 board +# See https://pixhawk.org/modules/pixhawk for details +# + +BOARD=px4io +BOARD_VERSION=2.4 +BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" + +ARCH=stm32 +$(TARGET).ARCHDIR = $(ARCH) +$(TARGET).LDSCRIPT=$(SRC_ARCH)/px4io_2.4.ld + +# default flash mode is via usb dfu bootloader +# possibilities: DFU, SWD, PX4_BOOTLOADER +PX4_BL_PORT ?= "/dev/serial/by-id/usb-FTDI_*" +PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/px4io-v2.prototype" + +FLASH_MODE ?= PX4_BOOTLOADER +$(TARGET).MAKEFILE = stm32 + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= none +GPS_LED ?= none +SYS_TIME_LED ?= 1 +FBW_MODE_LED ?= 3 + +# +# default UART configuration (modem, gps, spektrum) +# +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART1 + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm diff --git a/conf/firmwares/subsystems/fixedwing/intermcu_uart.makefile b/conf/firmwares/subsystems/fixedwing/intermcu_uart.makefile index 7bb2ce981b..2c220b3971 100644 --- a/conf/firmwares/subsystems/fixedwing/intermcu_uart.makefile +++ b/conf/firmwares/subsystems/fixedwing/intermcu_uart.makefile @@ -11,6 +11,7 @@ ifeq ($(TARGET),fbw) INTERMCU_PORT ?= UART2 INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z) fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B57600 + fbw.CFLAGS += -DFBW_MODE_LED=$(FBW_MODE_LED) else INTERMCU_PORT ?= UART5 INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z) diff --git a/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile b/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile index 3199bbd3fd..8b2feea188 100644 --- a/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile +++ b/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile @@ -4,18 +4,24 @@ ifeq ($(TARGET),fbw) INTERMCU_PORT ?= UART3 + INTERMCU_BAUD ?= B230400 INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z) - fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400 + fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=$(INTERMCU_BAUD) fbw.CFLAGS += -DINTER_MCU_FBW -DDOWNLINK + fbw.CFLAGS += -DFBW_MODE_LED=$(FBW_MODE_LED) fbw.srcs += pprzlink/src/pprz_transport.c fbw.srcs += subsystems/intermcu/intermcu_fbw.c else INTERMCU_PORT ?= UART3 + INTERMCU_BAUD ?= B230400 INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z) ap.CFLAGS += -DINTER_MCU_AP -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) - ap.CFLAGS += -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400 + ap.CFLAGS += -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=$(INTERMCU_BAUD) $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/intermcu/intermcu_ap.h\" -DRADIO_CONTROL - $(TARGET).CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) + RADIO_CONTROL_LED ?= none +ifneq ($(RADIO_CONTROL_LED),none) + $(TARGET).CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) +endif ap.srcs += subsystems/intermcu/intermcu_ap.c ap.srcs += pprzlink/src/pprz_transport.c diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index a6f9839cbd..096a079e4f 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -167,8 +167,8 @@ else ifeq ($(BOARD), krooz) BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c # PX4FMU -else ifeq ($(BOARD), px4fmu) - BARO_BOARD_CFLAGS += -DUSE_I2C2 +else ifeq ($(BOARD),$(filter $(BOARD),px4fmu)) + BARO_BOARD_CFLAGS += -DUSE_I2C2 BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 BARO_BOARD_SRCS += peripherals/ms5611.c BARO_BOARD_SRCS += peripherals/ms5611_i2c.c diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile index 46b00af392..1813b07a92 100644 --- a/conf/firmwares/test_progs.makefile +++ b/conf/firmwares/test_progs.makefile @@ -229,6 +229,23 @@ test_telemetry.CFLAGS += $(COMMON_TELEMETRY_CFLAGS) test_telemetry.srcs += $(COMMON_TELEMETRY_SRCS) test_telemetry.srcs += test/test_telemetry.c +# +# test_datalink : Sends ALIVE and pong telemetry messages +# +# configuration +# MODEM_PORT : +# MODEM_BAUD : +# +test_datalink.ARCHDIR = $(ARCH) +test_datalink.CFLAGS += $(COMMON_TEST_CFLAGS) +test_datalink.srcs += $(COMMON_TEST_SRCS) +test_datalink.CFLAGS += $(COMMON_DATALINK_CFLAGS) +test_datalink.CFLAGS += $(COMMON_TELEMETRY_CFLAGS) +test_datalink.srcs += $(COMMON_DATALINK_SRCS) +test_datalink.srcs += $(COMMON_TELEMETRY_SRCS) +test_datalink.srcs += test/test_datalink.c + + # # test_math_trig_compressed: Test math trigonometric using compressed data # diff --git a/conf/flash_modes.xml b/conf/flash_modes.xml index d3685e3044..826a198844 100644 --- a/conf/flash_modes.xml +++ b/conf/flash_modes.xml @@ -96,6 +96,13 @@ + + + + + + + diff --git a/conf/modules/px4io_flash.xml b/conf/modules/px4io_flash.xml new file mode 100644 index 0000000000..f16af5254a --- /dev/null +++ b/conf/modules/px4io_flash.xml @@ -0,0 +1,31 @@ + + + + + Flashes the px4io f1 through the px4 bootloader. + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/spektrum_soft_bind.xml b/conf/modules/spektrum_soft_bind.xml new file mode 100644 index 0000000000..9920ada163 --- /dev/null +++ b/conf/modules/spektrum_soft_bind.xml @@ -0,0 +1,25 @@ + + + + + Puts Spektrum in binding mode through software + + + + + + + + +
+ +
+ + + + + + + + + diff --git a/conf/radios/FrSky3dr.xml b/conf/radios/FrSky3dr.xml new file mode 100644 index 0000000000..53287a7781 --- /dev/null +++ b/conf/radios/FrSky3dr.xml @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/system/udev/rules/50-paparazzi.rules b/conf/system/udev/rules/50-paparazzi.rules index 4cef376c31..893b0849e0 100644 --- a/conf/system/udev/rules/50-paparazzi.rules +++ b/conf/system/udev/rules/50-paparazzi.rules @@ -20,6 +20,9 @@ SUBSYSTEM=="tty", ATTRS{product}=="FT232R USB UART", SYMLINK+="ftdi-serial", GRO # Paparazzi STM32 Autopilot board USB CDC serial device SUBSYSTEM=="tty", ATTRS{manufacturer}=="Paparazzi UAV", ATTRS{product}=="CDC Serial STM32", SYMLINK+="paparazzi/stm32-usb-serial" +# Paparazzi 3dr radio +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", SYMLINK+="paparazzi/link", GROUP="plugdev" + # MaxStream xbee pro box SUBSYSTEM=="tty", ATTRS{product}=="MaxStream PKG-U", SYMLINK+="paparazzi/xbee", GROUP="plugdev" diff --git a/conf/telemetry/TUDELFT/tudelft_xbee868.xml b/conf/telemetry/TUDELFT/tudelft_xbee868.xml index 9cee6119de..b7cdd56cc8 100644 --- a/conf/telemetry/TUDELFT/tudelft_xbee868.xml +++ b/conf/telemetry/TUDELFT/tudelft_xbee868.xml @@ -1,5 +1,5 @@ - + diff --git a/conf/telemetry/default_rotorcraft_slow.xml b/conf/telemetry/default_rotorcraft_slow.xml index 88a3995b1c..935598dcc0 100644 --- a/conf/telemetry/default_rotorcraft_slow.xml +++ b/conf/telemetry/default_rotorcraft_slow.xml @@ -27,7 +27,8 @@ - + + diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index e9c4683dd3..30a810dadd 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -42,50 +42,115 @@ #if defined(STM32F4) /* untested, should go into libopencm3 */ const clock_scale_t hse_24mhz_3v3[CLOCK_3V3_END] = { - { /* 48MHz */ - .pllm = 24, - .plln = 96, - .pllp = 2, - .pllq = 2, - .hpre = RCC_CFGR_HPRE_DIV_NONE, - .ppre1 = RCC_CFGR_PPRE_DIV_4, - .ppre2 = RCC_CFGR_PPRE_DIV_2, - .power_save = 1, - .flash_config = FLASH_ACR_ICE | FLASH_ACR_DCE | - FLASH_ACR_LATENCY_3WS, - .apb1_frequency = 12000000, - .apb2_frequency = 24000000, - }, - { /* 120MHz */ - .pllm = 24, - .plln = 240, - .pllp = 2, - .pllq = 5, - .hpre = RCC_CFGR_HPRE_DIV_NONE, - .ppre1 = RCC_CFGR_PPRE_DIV_4, - .ppre2 = RCC_CFGR_PPRE_DIV_2, - .power_save = 1, - .flash_config = FLASH_ACR_ICE | FLASH_ACR_DCE | - FLASH_ACR_LATENCY_3WS, - .apb1_frequency = 30000000, - .apb2_frequency = 60000000, - }, - { /* 168MHz */ - .pllm = 24, - .plln = 336, - .pllp = 2, - .pllq = 7, - .hpre = RCC_CFGR_HPRE_DIV_NONE, - .ppre1 = RCC_CFGR_PPRE_DIV_4, - .ppre2 = RCC_CFGR_PPRE_DIV_2, - .flash_config = FLASH_ACR_ICE | FLASH_ACR_DCE | - FLASH_ACR_LATENCY_5WS, - .apb1_frequency = 42000000, - .apb2_frequency = 84000000, - }, + { /* 48MHz */ + .pllm = 24, + .plln = 96, + .pllp = 2, + .pllq = 2, + .hpre = RCC_CFGR_HPRE_DIV_NONE, + .ppre1 = RCC_CFGR_PPRE_DIV_4, + .ppre2 = RCC_CFGR_PPRE_DIV_2, + .power_save = 1, + .flash_config = FLASH_ACR_ICE | FLASH_ACR_DCE | + FLASH_ACR_LATENCY_3WS, + .apb1_frequency = 12000000, + .apb2_frequency = 24000000, + }, + { /* 120MHz */ + .pllm = 24, + .plln = 240, + .pllp = 2, + .pllq = 5, + .hpre = RCC_CFGR_HPRE_DIV_NONE, + .ppre1 = RCC_CFGR_PPRE_DIV_4, + .ppre2 = RCC_CFGR_PPRE_DIV_2, + .power_save = 1, + .flash_config = FLASH_ACR_ICE | FLASH_ACR_DCE | + FLASH_ACR_LATENCY_3WS, + .apb1_frequency = 30000000, + .apb2_frequency = 60000000, + }, + { /* 168MHz */ + .pllm = 24, + .plln = 336, + .pllp = 2, + .pllq = 7, + .hpre = RCC_CFGR_HPRE_DIV_NONE, + .ppre1 = RCC_CFGR_PPRE_DIV_4, + .ppre2 = RCC_CFGR_PPRE_DIV_2, + .flash_config = FLASH_ACR_ICE | FLASH_ACR_DCE | + FLASH_ACR_LATENCY_5WS, + .apb1_frequency = 42000000, + .apb2_frequency = 84000000, + }, }; #endif +#if defined(STM32F1) +/*---------------------------------------------------------------------------*/ +/** @brief RCC Set System Clock HSE at 24MHz from HSE at 24MHz + +*/ +void rcc_clock_setup_in_hse_24mhz_out_24mhz_pprz(void); +void rcc_clock_setup_in_hse_24mhz_out_24mhz_pprz(void) +{ + /* Enable internal high-speed oscillator. */ + rcc_osc_on(HSI); + rcc_wait_for_osc_ready(HSI); + + /* Select HSI as SYSCLK source. */ + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_HSICLK); + + /* Enable external high-speed oscillator 24MHz. */ + rcc_osc_on(HSE); + rcc_wait_for_osc_ready(HSE); + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_HSECLK); + + /* + * Set prescalers for AHB, ADC, ABP1, ABP2. + * Do this before touching the PLL (TODO: why?). + */ + rcc_set_hpre(RCC_CFGR_HPRE_SYSCLK_NODIV); /* Set. 24MHz Max. 72MHz */ + rcc_set_adcpre(RCC_CFGR_ADCPRE_PCLK2_DIV2); /* Set. 12MHz Max. 14MHz */ + rcc_set_ppre1(RCC_CFGR_PPRE1_HCLK_NODIV); /* Set. 24MHz Max. 36MHz */ + rcc_set_ppre2(RCC_CFGR_PPRE2_HCLK_NODIV); /* Set. 24MHz Max. 72MHz */ + + /* + * Sysclk runs with 24MHz -> 0 waitstates. + * 0WS from 0-24MHz + * 1WS from 24-48MHz + * 2WS from 48-72MHz + */ + flash_set_ws(FLASH_ACR_LATENCY_0WS); + + /* + * Set the PLL multiplication factor to 2. + * 24MHz (external) * 2 (multiplier) / 2 (RCC_CFGR_PLLXTPRE_HSE_CLK_DIV2) = 24MHz + */ + rcc_set_pll_multiplication_factor(RCC_CFGR_PLLMUL_PLL_CLK_MUL2); + + /* Select HSE as PLL source. */ + rcc_set_pll_source(RCC_CFGR_PLLSRC_HSE_CLK); + + /* + * External frequency divide by 2 before entering PLL + * (only valid/needed for HSE). + */ + rcc_set_pllxtpre(RCC_CFGR_PLLXTPRE_HSE_CLK_DIV2); + + rcc_osc_on(PLL); + rcc_wait_for_osc_ready(PLL); + + /* Select PLL as SYSCLK source. */ + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_PLLCLK); + + /* Set the peripheral clock frequencies used */ + rcc_ahb_frequency = 24000000; + rcc_apb1_frequency = 24000000; + rcc_apb2_frequency = 24000000; +} +#endif + void mcu_arch_init(void) { #if LUFTBOOT @@ -121,6 +186,8 @@ void mcu_arch_init(void) #if defined(STM32F4) PRINT_CONFIG_MSG("Using 24MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&hse_24mhz_3v3[CLOCK_3V3_168MHZ]); +#elif defined(STM32F1) + rcc_clock_setup_in_hse_24mhz_out_24mhz_pprz(); #endif #elif EXT_CLK == 25000000 #if defined(STM32F4) @@ -142,6 +209,7 @@ void mcu_arch_init(void) } #if defined(STM32F1) + #define RCC_CFGR_PPRE2_SHIFT 11 #define RCC_CFGR_PPRE2 (7 << RCC_CFGR_PPRE2_SHIFT) @@ -191,8 +259,7 @@ uint32_t timer_get_frequency(uint32_t timer_peripheral) #ifdef TIM11 case TIM11: #endif - if (!rcc_get_ppre2()) - { + if (!rcc_get_ppre2()) { /* without APB2 prescaler, runs at APB2 freq */ return rcc_apb2_frequency; } else { @@ -216,8 +283,7 @@ uint32_t timer_get_frequency(uint32_t timer_peripheral) #ifdef TIM14 case TIM14: #endif - if (!rcc_get_ppre1()) - { + if (!rcc_get_ppre1()) { /* without APB1 prescaler, runs at APB1 freq */ return rcc_apb1_frequency; } else { diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c index 839dfdbfbe..75ec2b1c60 100644 --- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c @@ -147,7 +147,7 @@ PRINT_CONFIG_MSG("Analog to Digital Coverter 2 active") #if USE_AD3 PRINT_CONFIG_MSG("Analog to Digital Coverter 3 active") #endif -#if !USE_AD1 && !USE_AD2 && !USE_AD3 +#if !USE_AD1 && !USE_AD2 && !USE_AD3 && !defined FBW #warning ALL ADC CONVERTERS INACTIVE #endif @@ -222,11 +222,12 @@ static struct { void adc_init(void) { - +#if USE_AD1 || USE_AD2 || USE_AD3 uint8_t x = 0; // ADC channel mapping uint8_t adc_channel_map[4]; +#endif /* Init GPIO ports for ADC operation */ @@ -582,9 +583,11 @@ void adc1_2_isr(void) void adc_isr(void) #endif { +#if USE_AD1 || USE_AD2 || USE_AD3 uint8_t channel = 0; uint16_t value = 0; struct adc_buf *buf; +#endif #if USE_ADC_WATCHDOG /* diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h index 910523b66c..79a9dd16ee 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h @@ -40,6 +40,7 @@ #define B115200 115200 #define B230400 230400 #define B921600 921600 +#define B1500000 1500000 #define UART_SPEED(_def) _def #endif /* STM32_UART_ARCH_H */ diff --git a/sw/airborne/arch/stm32/px4fmu_2.4.ld b/sw/airborne/arch/stm32/px4fmu_2.4.ld index c75ae6f361..e7158dbb88 100644 --- a/sw/airborne/arch/stm32/px4fmu_2.4.ld +++ b/sw/airborne/arch/stm32/px4fmu_2.4.ld @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 Felix Ruess + * Copyright (C) 2016 The Paparazzi Team> * * This file is part of Paparazzi. * @@ -19,15 +19,14 @@ * Boston, MA 02111-1307, USA. */ -/* Linker script for Pixhawk PX4FMU v2.4 (STM32F427, 2048K flash, 256K RAM). */ +/* Linker script for Pixhawk PX4FMU v2.4 (STM32F427, 1024K flash, 192K RAM). */ /* Define memory regions. */ MEMORY { - /* only 192K (SRAM1 and SRAM2) are accessible by all AHB masters. */ - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K - /* Reserving 128kb flash for persistent settings. */ - rom (rx) : ORIGIN = 0x08000000, LENGTH = 1920K + ram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + /* Pixhawk seems to have had a bad badge of f4's, so only use the first 1mb */ + rom (rx) : ORIGIN = 0x08004000, LENGTH = 1008K } /* Include the common ld script. */ diff --git a/sw/airborne/arch/stm32/px4io_2.4.ld b/sw/airborne/arch/stm32/px4io_2.4.ld new file mode 100644 index 0000000000..b560cf8996 --- /dev/null +++ b/sw/airborne/arch/stm32/px4io_2.4.ld @@ -0,0 +1,36 @@ +/* + * Hey Emacs, this is a -*- makefile -*- + * + * Copyright (C) 2016 Kevin van Hecke + * + * 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. + */ + +/* Linker script for the PX4IO (STM32F103c8t6 medium density, 64 Kbytes Flash). */ + +/* Define memory regions. */ +MEMORY +{ + ram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K + /* Leaving 2k of space at the end of rom for stored settings */ + rom (rx) : ORIGIN = 0x08001000, LENGTH = 60K +} + +/* Include the common ld script. */ +INCLUDE libopencm3_stm32f1.ld + diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c index 0955809c76..4d136d0019 100644 --- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c @@ -43,7 +43,6 @@ int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; */ void actuators_pwm_arch_init(void) { - /*----------------------------------- * Configure timer peripheral clocks *-----------------------------------*/ diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c index 0234b33c41..65fddebf4c 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c @@ -56,14 +56,55 @@ INFO("Radio-Control now follows PPRZ sign convention: this means you might need #define MIN_FRAME_SPACE 70 // 7ms #define MAX_BYTE_SPACE 3 // .3ms + +//not all f1's have a timer 6, so, some redefines have to happen +#define PASTER3(x,y,z) x ## y ## z +#define EVALUATOR3(x,y,z) PASTER3(x,y,z) +#define NVIC_TIMx_IRQ EVALUATOR3(NVIC_TIM, SPEKTRUM_TIMER,_IRQ) +#define NVIC_TIMx_DAC_IRQ EVALUATOR3(NVIC_TIM, SPEKTRUM_TIMER,_DAC_IRQ) // not really necessary, only for f4 which probably always has a timer 4 +#define TIMx_ISR EVALUATOR3(tim, SPEKTRUM_TIMER,_isr) +#define TIMx_DAC_ISR EVALUATOR3(tim, SPEKTRUM_TIMER,_dac_isr) + +#define PASTER2(x,y) x ## y +#define EVALUATOR2(x,y) PASTER2(x,y) +#define TIMx EVALUATOR2(TIM, SPEKTRUM_TIMER) +#define RCC_TIMx EVALUATOR2(RCC_TIM, SPEKTRUM_TIMER) + +#ifndef SPEKTRUM_TIMER +#define SPEKTRUM_TIMER 6 +#endif + +#if (SPEKTRUM_TIMER == 6) #ifndef NVIC_TIM6_IRQ_PRIO #define NVIC_TIM6_IRQ_PRIO 2 +#define NVIC_TIMx_IRQ_PRIO 2 +#else +#define NVIC_TIMx_IRQ_PRIO NVIC_TIM6_IRQ_PRIO #endif - #ifndef NVIC_TIM6_DAC_IRQ_PRIO #define NVIC_TIM6_DAC_IRQ_PRIO 2 +#define NVIC_TIMx_DAC_IRQ_PRIO 2 +#else +#define NVIC_TIMx_DAC_IRQ_PRIO NVIC_TIM6_DAC_IRQ_PRIO +#endif #endif +#if (SPEKTRUM_TIMER == 3) +#ifndef NVIC_TIM3_IRQ_PRIO +#define NVIC_TIM3_IRQ_PRIO 2 +#define NVIC_TIMx_IRQ_PRIO 2 +#else +#define NVIC_TIMx_IRQ_PRIO NVIC_TIM6_IRQ_PRIO +#endif +#ifndef NVIC_TIM3_DAC_IRQ_PRIO +#define NVIC_TIM3_DAC_IRQ_PRIO 2 +#define NVIC_TIMx_DAC_IRQ_PRIO 2 +#else +#define NVIC_TIMx_DAC_IRQ_PRIO NVIC_TIM6_DAC_IRQ_PRIO +#endif +#endif + +PRINT_CONFIG_MSG_VALUE("SPEKTRUM_TIMER: " , SPEKTRUM_TIMER) #ifdef NVIC_UART_IRQ_PRIO #define NVIC_PRIMARY_UART_PRIO NVIC_UART_IRQ_PRIO @@ -136,13 +177,10 @@ int8_t SpektrumSigns[] = RADIO_CONTROL_SPEKTRUM_SIGNS; /* Parser state variables */ static uint8_t EncodingType = 0; static uint8_t ExpectedFrames = 0; -/* initialise the uarts used by the parser */ -void SpektrumUartInit(void); + /* initialise the timer used by the parser to ensure sync */ void SpektrumTimerInit(void); -void tim6_irq_handler(void); - /** Set polarity using RC_POLARITY_GPIO. * SBUS signal has a reversed polarity compared to normal UART * this allows to using hardware UART peripheral by changing @@ -289,6 +327,8 @@ void radio_control_impl_init(void) static inline void SpektrumParser(uint8_t _c, SpektrumStateType *spektrum_state, bool_t secondary_receiver) { + + uint16_t ChannelData; uint8_t TimedOut; static uint8_t TmpEncType = 0; /* 0 = 10bit, 1 = 11 bit */ @@ -299,7 +339,7 @@ static inline void SpektrumParser(uint8_t _c, SpektrumStateType *spektrum_state, /* If we have just started the resync process or */ /* if we have recieved a character before our */ /* 7ms wait has finished */ - if ((spektrum_state->ReSync == 1) || + if ((spektrum_state->ReSync == 1) || ((spektrum_state->Sync == 0) && (!TimedOut))) { spektrum_state->ReSync = 0; @@ -310,6 +350,7 @@ static inline void SpektrumParser(uint8_t _c, SpektrumStateType *spektrum_state, spektrum_state->SecondFrame = 0; return; } +//LED_OFF(1); /* the first byte of a new frame. It was received */ /* more than 7ms after the last received byte. */ @@ -503,55 +544,56 @@ void RadioControlEventImp(void (*frame_handler)(void)) /***************************************************************************** * - * Initialise TIM6 to fire an interrupt every 100 microseconds to provide + * Initialise TIMx to fire an interrupt every 100 microseconds to provide * timebase for SpektrumParser * *****************************************************************************/ void SpektrumTimerInit(void) { - /* enable TIM6 clock */ - rcc_periph_clock_enable(RCC_TIM6); + /* enable TIMx clock */ + rcc_periph_clock_enable(RCC_TIMx); - /* TIM6 configuration, always counts up */ - timer_set_mode(TIM6, TIM_CR1_CKD_CK_INT, 0, 0); + timer_reset(TIMx); + /* TIMx configuration, always counts up */ + timer_set_mode(TIMx, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); // used to be 0 0 /* 100 microseconds ie 0.1 millisecond */ - timer_set_period(TIM6, TIM_TICS_FOR_100us - 1); - uint32_t tim6_clk = timer_get_frequency(TIM6); + timer_set_period(TIMx, TIM_TICS_FOR_100us - 1); + uint32_t TIMx_clk = timer_get_frequency(TIMx); /* timer ticks with 1us */ - timer_set_prescaler(TIM6, ((tim6_clk / ONE_MHZ) - 1)); + timer_set_prescaler(TIMx, ((TIMx_clk / ONE_MHZ) - 1)); - /* Enable TIM6 interrupts */ + /* Enable TIMx interrupts */ #ifdef STM32F1 - nvic_set_priority(NVIC_TIM6_IRQ, NVIC_TIM6_IRQ_PRIO); - nvic_enable_irq(NVIC_TIM6_IRQ); + nvic_set_priority(NVIC_TIMx_IRQ, NVIC_TIMx_IRQ_PRIO); + nvic_enable_irq(NVIC_TIMx_IRQ); #elif defined STM32F4 - /* the define says DAC IRQ, but it is also the global TIM6 IRQ*/ - nvic_set_priority(NVIC_TIM6_DAC_IRQ, NVIC_TIM6_DAC_IRQ_PRIO); - nvic_enable_irq(NVIC_TIM6_DAC_IRQ); + /* the define says DAC IRQ, but it is also the global TIMx IRQ*/ + nvic_set_priority(NVIC_TIMx_DAC_IRQ, NVIC_TIMx_DAC_IRQ_PRIO); + nvic_enable_irq(NVIC_TIMx_DAC_IRQ); #endif - /* Enable TIM6 Update interrupt */ - timer_enable_irq(TIM6, TIM_DIER_UIE); - timer_clear_flag(TIM6, TIM_SR_UIF); + /* Enable TIMx Update interrupt */ + timer_enable_irq(TIMx, TIM_DIER_UIE); + timer_clear_flag(TIMx, TIM_SR_UIF); - /* TIM6 enable counter */ - timer_enable_counter(TIM6); + /* TIMx enable counter */ + timer_enable_counter(TIMx); } /***************************************************************************** * - * TIM6 interrupt request handler updates times used by SpektrumParser + * TIMx interrupt request handler updates times used by SpektrumParser * *****************************************************************************/ #ifdef STM32F1 -void tim6_isr(void) +void TIMx_ISR(void) { #elif defined STM32F4 -void tim6_dac_isr(void) { +void TIMx_DAC_ISR(void) { #endif - timer_clear_flag(TIM6, TIM_SR_UIF); + timer_clear_flag(TIMx, TIM_SR_UIF); if (PrimarySpektrumState.SpektrumTimer) { --PrimarySpektrumState.SpektrumTimer; @@ -582,7 +624,7 @@ void SpektrumUartInit(void) { gpio_setup_pin_af(PrimaryUart(_BANK), PrimaryUart(_PIN), PrimaryUart(_AF), FALSE); /* Configure Primary UART */ - usart_set_baudrate(PrimaryUart(_DEV), 115200); + usart_set_baudrate(PrimaryUart(_DEV), B115200); usart_set_databits(PrimaryUart(_DEV), 8); usart_set_stopbits(PrimaryUart(_DEV), USART_STOPBITS_1); usart_set_parity(PrimaryUart(_DEV), USART_PARITY_NONE); @@ -609,7 +651,7 @@ void SpektrumUartInit(void) { gpio_setup_pin_af(SecondaryUart(_BANK), SecondaryUart(_PIN), SecondaryUart(_AF), FALSE); /* Configure secondary UART */ - usart_set_baudrate(SecondaryUart(_DEV), 115200); + usart_set_baudrate(SecondaryUart(_DEV), B115200); usart_set_databits(SecondaryUart(_DEV), 8); usart_set_stopbits(SecondaryUart(_DEV), USART_STOPBITS_1); usart_set_parity(SecondaryUart(_DEV), USART_PARITY_NONE); @@ -701,7 +743,7 @@ void SecondaryUart(_ISR)(void) { * *****************************************************************************/ void radio_control_spektrum_try_bind(void) { - +#ifdef SPEKTRUM_BIND_PIN_PORT #ifdef SPEKTRUM_BIND_PIN_HIGH /* Init GPIO for the bind pin, we enable the pulldown resistor. * (esden) As far as I can tell only navstick is using the PIN LOW version of @@ -727,6 +769,7 @@ void radio_control_spektrum_try_bind(void) { if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) != 0) { return; } +#endif #endif /* Master receiver Rx push-pull */ diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.h b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.h index caf21ae9d2..99971b0bda 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.h +++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.h @@ -79,5 +79,7 @@ #endif extern void RadioControlEventImp(void (*_received_frame_handler)(void)); +/* initialise the uarts used by the parser */ +void SpektrumUartInit(void); #endif /* RADIO_CONTROL_SPEKTRUM_ARCH_H */ diff --git a/sw/airborne/boards/px4fmu_2.4.h b/sw/airborne/boards/px4fmu_2.4.h index 4e9cd7853f..8c491502dd 100644 --- a/sw/airborne/boards/px4fmu_2.4.h +++ b/sw/airborne/boards/px4fmu_2.4.h @@ -4,14 +4,14 @@ #define BOARD_PX4FMU_v2 /* Pixhawk board (PX4FMUv2 has a 24MHz external clock and 168MHz internal. */ +//STM32F4 #define EXT_CLK 24000000 #define AHB_CLK 168000000 - /* * Onboard LEDs */ -/* red/amber , on PE12 */ +/* red, on PE12 */ #ifndef USE_LED_1 #define USE_LED_1 1 #endif @@ -23,13 +23,6 @@ /* * UART - */ -/* -#define UART1_GPIO_AF GPIO_AF7 -#define UART1_GPIO_PORT_RX GPIOA -#define UART1_GPIO_RX GPIO9 -#define UART1_GPIO_PORT_TX GPIOA -#define UART1_GPIO_TX GPIO10 */ //OK // conector telem1 @@ -156,24 +149,19 @@ // SDIO_CMD pd2 /* Onboard ADCs */ -#define USE_AD_TIM4 1 - -#define BOARD_ADC_CHANNEL_1 11 -#define BOARD_ADC_CHANNEL_2 12 -#define BOARD_ADC_CHANNEL_3 13 -#define BOARD_ADC_CHANNEL_4 10 +#define USE_AD_TIM5 1 /* provide defines that can be used to access the ADC_x in the code or airframe file * these directly map to the index number of the 4 adc channels defined above * 4th (index 3) is used for bat monitoring by default */ -#if USE_ADC_1 -#define AD1_1_CHANNEL 11 -#define ADC_1 AD1_1 -#define ADC_1_GPIO_PORT GPIOC -#define ADC_1_GPIO_PIN GPIO1 -#endif +//#if USE_ADC_1 +//#define AD1_1_CHANNEL 11 +//#define ADC_1 AD1_1 +//#define ADC_1_GPIO_PORT GPIOC +//#define ADC_1_GPIO_PIN GPIO1 +//#endif /* #if USE_ADC_2 #define AD1_2_CHANNEL 12 @@ -185,18 +173,19 @@ //OK current sens #if USE_ADC_3 -#define AD1_3_CHANNEL 13 +#define AD1_3_CHANNEL 3 #define ADC_3 AD1_3 #define ADC_3_GPIO_PORT GPIOA #define ADC_3_GPIO_PIN GPIO3 #endif +#define MilliAmpereOfAdc(adc)((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f) // Internal ADC for battery enabled by default #ifndef USE_ADC_4 #define USE_ADC_4 1 #endif #if USE_ADC_4 -#define AD1_4_CHANNEL 10 +#define AD1_4_CHANNEL 2 #define ADC_4 AD1_4 #define ADC_4_GPIO_PORT GPIOA #define ADC_4_GPIO_PIN GPIO2 @@ -206,7 +195,7 @@ #ifndef ADC_CHANNEL_VSUPPLY #define ADC_CHANNEL_VSUPPLY ADC_4 #endif -#define DefaultVoltageOfAdc(adc) (0.006185*adc) +#define DefaultVoltageOfAdc(adc) (0.00975f*adc) // value comes from px4 code sensors.cpp _parameters.battery_voltage_scaling = 0.0082f; Manual calib on iris = 0.0096... /* @@ -229,45 +218,6 @@ #define I2C3_GPIO_SDA GPIO9 */ -/* - * PPM - */ - - -#define USE_PPM_TIM1 1 - -#define PPM_CHANNEL TIM_IC1 -#define PPM_TIMER_INPUT TIM_IC_IN_TI1 -#define PPM_IRQ NVIC_TIM1_CC_IRQ -#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ -// Capture/Compare InteruptEnable and InterruptFlag -#define PPM_CC_IE TIM_DIER_CC1IE -#define PPM_CC_IF TIM_SR_CC1IF -#define PPM_GPIO_PORT GPIOA -#define PPM_GPIO_PIN GPIO10 -#define PPM_GPIO_AF GPIO_AF1 - -/* - * Spektrum - */ -/* The line that is pulled low at power up to initiate the bind process */ -/* GPIO_EXT1 on PX4FMU */ - -/* -#define SPEKTRUM_BIND_PIN GPIO4 -#define SPEKTRUM_BIND_PIN_PORT GPIOC -*/ -/* -#define SPEKTRUM_UART2_RCC RCC_USART2 -#define SPEKTRUM_UART2_BANK GPIOA -#define SPEKTRUM_UART2_PIN GPIO3 -#define SPEKTRUM_UART2_AF GPIO_AF7 -#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ -#define SPEKTRUM_UART2_ISR usart2_isr -#define SPEKTRUM_UART2_DEV USART2 -*/ - - /* Activate onboard baro by default */ #ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 @@ -290,6 +240,7 @@ #define USE_PWM4 1 #define USE_PWM5 1 #define USE_PWM6 1 +//#define USE_BUZZER 1 // Servo numbering on the PX4 starts with 1 // PWM_SERVO_x is the index of the servo in the actuators_pwm_values array @@ -372,8 +323,7 @@ #define PWM_SERVO_6_OC_BIT 0 #endif -//Buzzer -/* +//Buzzer (alarm) #if USE_BUZZER #define PWM_BUZZER #define PWM_BUZZER_TIMER TIM2 @@ -385,11 +335,9 @@ #else #define PWM_BUZZER_OC_BIT 0 #endif -*/ - #define PWM_TIM1_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) -//#define PWM_TIM2_CHAN_MASK (PWM_BUZZER_OC_BIT) +#define PWM_TIM2_CHAN_MASK (PWM_BUZZER_OC_BIT) #define PWM_TIM4_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT) #endif /* CONFIG_PX4FMU_2_4_H */ diff --git a/sw/airborne/boards/px4io_2.4.h b/sw/airborne/boards/px4io_2.4.h new file mode 100644 index 0000000000..c205e033bb --- /dev/null +++ b/sw/airborne/boards/px4io_2.4.h @@ -0,0 +1,243 @@ +#ifndef CONFIG_PX4IO_2_4_H +#define CONFIG_PX4IO_2_4_H + +#define BOARD_PX4IO +//STM32F103c8t6 (medium density!) + +/* Pixhawk board (PX4FIOv2 has a 24MHz external clock and 24MHz internal. */ +#define EXT_CLK 24000000 //this osc is actually outside of the specs (max 16MHz) +#define AHB_CLK 24000000 + + +/* + * LEDs + */ +/* blue led, a.k.a. ACT */ +#ifndef USE_LED_1 +#define USE_LED_1 1 +#endif +#define LED_1_GPIO GPIOB +#define LED_1_GPIO_PIN GPIO14 +#define LED_1_GPIO_ON gpio_clear +#define LED_1_GPIO_OFF gpio_set +#define LED_1_AFIO_REMAP ((void)0) + +//led Amber a.k.a b/e led +#ifndef USE_LED_2 +#define USE_LED_2 1 +#endif +#define LED_2_GPIO GPIOB +#define LED_2_GPIO_PIN GPIO15 +#define LED_2_GPIO_ON gpio_clear +#define LED_2_GPIO_OFF gpio_set +#define LED_2_AFIO_REMAP ((void)0) + +//safety led in the switch, red +#ifndef USE_LED_3 +#define USE_LED_3 1 +#endif +#define LED_3_GPIO GPIOB +#define LED_3_GPIO_PIN GPIO13 +#define LED_3_GPIO_ON gpio_clear +#define LED_3_GPIO_OFF gpio_set +#define LED_3_AFIO_REMAP ((void)0) + +//TODO: safety switch is on PB5! + +/* + * UART +*/ + +// fmu debug / spektrum receiver (only rx) +#define UART1_GPIO_AF 0 +#define UART1_GPIO_PORT_RX GPIOA +#define UART1_GPIO_RX GPIO10 +#define UART1_GPIO_PORT_TX GPIOA +#define UART1_GPIO_TX GPIO9 +// intermcu fmu +#define UART2_GPIO_AF 0 +#define UART2_GPIO_PORT_RX GPIOA +#define UART2_GPIO_RX GPIO3 +#define UART2_GPIO_PORT_TX GPIOA +#define UART2_GPIO_TX GPIO2 + + +/* + * Spektrum + */ +/* The line that is pulled low at power up to initiate the bind process */ +#define SPEKTRUM_POWER_PIN_PORT GPIOC +#define SPEKTRUM_POWER_PIN GPIO13 + +#define SPEKTRUM_TIMER 3 + +#define SPEKTRUM_UART1_RCC RCC_USART1 +#define SPEKTRUM_UART1_BANK GPIOA +#define SPEKTRUM_UART1_PIN GPIO10 +#define SPEKTRUM_UART1_AF 0 +#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ +#define SPEKTRUM_UART1_ISR usart1_isr +#define SPEKTRUM_UART1_DEV USART1 + + +/* + * PPM input + */ +#define USE_PPM_TIM1 1 +#define PPM_CHANNEL TIM_IC1 +#define PPM_TIMER_INPUT TIM_IC_IN_TI1 +#define PPM_IRQ NVIC_TIM1_UP_IRQ +#define PPM_IRQ2 NVIC_TIM1_CC_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC1IE +#define PPM_CC_IF TIM_SR_CC1IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO8 +#define PPM_GPIO_AF 0 + +//#define USE_AD_TIM1 1 +#ifndef USE_ADC_1 +#define USE_ADC_1 0 +#endif +#if USE_ADC_1 // VDD servo ADC12_IN4, untested +#define AD1_1_CHANNEL 12 +#define ADC_1 AD1_4 +#define ADC_1_GPIO_PORT GPIOA +#define ADC_1_GPIO_PIN GPIO4 +#endif + +/* + * PWM + * + */ +//sevo outputs on px4io f1: +//chn: 1 2 3 4 5 6 7 8 +//pin: A0 A1 B8 B9 A6 A7 B0 B1 +//timer/channel: 2/1 2/2 4/3 4/4 3/1 3/2 3/3 3/4 +#define PWM_USE_TIM2 1 +//#define PWM_USE_TIM3 1 // spektrum already uses tim3 +#define PWM_USE_TIM4 1 + +//#define ACTUATORS_PWM_NB 4 + +#define USE_PWM1 1 +#define USE_PWM2 1 +#define USE_PWM3 1 +#define USE_PWM4 1 +//#define USE_PWM5 1 +//#define USE_PWM6 1 +//#define USE_PWM7 1 +//#define USE_PWM8 1 + +// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array +#if USE_PWM1 +#define PWM_SERVO_1 0 +#define PWM_SERVO_1_TIMER TIM2 +#define PWM_SERVO_1_GPIO GPIOA +#define PWM_SERVO_1_PIN GPIO0 +#define PWM_SERVO_1_AF 0 +#define PWM_SERVO_1_OC TIM_OC1 +#define PWM_SERVO_1_OC_BIT (1<<0) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 1 +#define PWM_SERVO_2_TIMER TIM2 +#define PWM_SERVO_2_GPIO GPIOA +#define PWM_SERVO_2_PIN GPIO1 +#define PWM_SERVO_2_AF 0 +#define PWM_SERVO_2_OC TIM_OC2 +#define PWM_SERVO_2_OC_BIT (1<<1) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +#if USE_PWM3 +#define PWM_SERVO_3 2 +#define PWM_SERVO_3_TIMER TIM4 +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO8 +#define PWM_SERVO_3_AF 0 +#define PWM_SERVO_3_OC TIM_OC3 +#define PWM_SERVO_3_OC_BIT (1<<2) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 3 +#define PWM_SERVO_4_TIMER TIM4 +#define PWM_SERVO_4_GPIO GPIOB +#define PWM_SERVO_4_PIN GPIO9 +#define PWM_SERVO_4_AF 0 +#define PWM_SERVO_4_OC TIM_OC4 +#define PWM_SERVO_4_OC_BIT (1<<3) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + +#if USE_PWM5 +#define PWM_SERVO_5 4 +#define PWM_SERVO_5_TIMER TIM3 +#define PWM_SERVO_5_GPIO GPIOA +#define PWM_SERVO_5_PIN GPIO6 +#define PWM_SERVO_5_AF 0 +#define PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) +#else +#define PWM_SERVO_5_OC_BIT 0 +#endif + +#if USE_PWM6 +#define PWM_SERVO_6 5 +#define PWM_SERVO_6_TIMER TIM3 +#define PWM_SERVO_6_GPIO GPIOA +#define PWM_SERVO_6_PIN GPIO7 +#define PWM_SERVO_6_AF 0 +#define PWM_SERVO_6_OC TIM_OC2 +#define PWM_SERVO_6_OC_BIT (1<<1) +#else +#define PWM_SERVO_6_OC_BIT 0 +#endif + +#if USE_PWM7 +#define PWM_SERVO_7 6 +#define PWM_SERVO_7_TIMER TIM3 +#define PWM_SERVO_7_GPIO GPIOB +#define PWM_SERVO_7_PIN GPIO0 +#define PWM_SERVO_7_AF 0 +#define PWM_SERVO_7_OC TIM_OC3 +#define PWM_SERVO_7_OC_BIT (1<<2) +#else +#define PWM_SERVO_7_OC_BIT 0 +#endif + +#if USE_PWM8 +#define PWM_SERVO_8 7 +#define PWM_SERVO_8_TIMER TIM3 +#define PWM_SERVO_8_GPIO GPIOB +#define PWM_SERVO_8_PIN GPIO1 +#define PWM_SERVO_8_AF 0 +#define PWM_SERVO_8_OC TIM_OC4 +#define PWM_SERVO_8_OC_BIT (1<<3) +#else +#define PWM_SERVO_8_OC_BIT 0 +#endif + +/* servos 1-2 on TIM2 */ +#define PWM_TIM2_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT) +/* servos 3-4 on TIM4 */ +#define PWM_TIM4_CHAN_MASK (PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) +/* servos 5-8 on TIM3 */ +//#define PWM_TIM3_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT|PWM_SERVO_7_OC_BIT|PWM_SERVO_8_OC_BIT) + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" +#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsPwmInit() +#define ActuatorsDefaultCommit() ActuatorsPwmCommit() + + +#endif /* CONFIG_PX4IO_2_4_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.c b/sw/airborne/firmwares/rotorcraft/main_fbw.c index 7b528073d4..94ecee3a95 100644 --- a/sw/airborne/firmwares/rotorcraft/main_fbw.c +++ b/sw/airborne/firmwares/rotorcraft/main_fbw.c @@ -47,7 +47,8 @@ #include "firmwares/rotorcraft/main_fbw.h" #include "firmwares/rotorcraft/autopilot_rc_helpers.h" -//#include "generated/modules.h" +#define MODULES_C +#include "generated/modules.h" /** Fly by wire modes */ typedef enum {FBW_MODE_MANUAL = 0, FBW_MODE_AUTO = 1, FBW_MODE_FAILSAFE = 2} fbw_mode_enum; @@ -59,7 +60,7 @@ fbw_mode_enum fbw_mode; //PRINT_CONFIG_VAR(MODULES_FREQUENCY) tid_t main_periodic_tid; ///< id for main_periodic() timer -//tid_t modules_tid; ///< id for modules_periodic_task() timer +tid_t modules_tid; ///< id for modules_periodic_task() timer tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer tid_t electrical_tid; ///< id for electrical_periodic() timer tid_t telemetry_tid; ///< id for telemetry_periodic() timer @@ -85,17 +86,17 @@ STATIC_INLINE void main_init(void) mcu_init(); + actuators_init(); + electrical_init(); - actuators_init(); #if USE_MOTOR_MIXING motor_mixing_init(); #endif radio_control_init(); - // TODO - //modules_init(); + modules_init(); mcu_int_enable(); @@ -103,7 +104,7 @@ STATIC_INLINE void main_init(void) // register the timers for the periodic functions main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); -// modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); + modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); radio_control_tid = sys_time_register_timer((1. / 60.), NULL); electrical_tid = sys_time_register_timer(0.1, NULL); telemetry_tid = sys_time_register_timer((1. / 20.), NULL); @@ -115,13 +116,15 @@ STATIC_INLINE void main_init(void) STATIC_INLINE void handle_periodic_tasks(void) { + + if (sys_time_check_and_ack_timer(main_periodic_tid)) { main_periodic(); } - //if (sys_time_check_and_ack_timer(modules_tid)) { - // TODO - //modules_periodic_task(); - //} + if (sys_time_check_and_ack_timer(modules_tid)) { + + modules_periodic_task(); + } if (sys_time_check_and_ack_timer(radio_control_tid)) { radio_control_periodic_task(); } @@ -175,9 +178,17 @@ STATIC_INLINE void main_periodic(void) } } + + static uint16_t dv = 0; + // TODO make module out of led blink? /* set failsafe commands */ if (fbw_mode == FBW_MODE_FAILSAFE) { SetCommands(commands_failsafe); + if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);} + } else if (fbw_mode == FBW_MODE_MANUAL) { + if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);} + } else if (fbw_mode == FBW_MODE_AUTO) { + LED_TOGGLE(FBW_MODE_LED); // toggle instead of on, because then it is still visible when fbw_mode switches very fast } /* set actuators */ @@ -208,6 +219,7 @@ static void autopilot_on_rc_frame(void) /* if manual */ if (fbw_mode == FBW_MODE_MANUAL) { + autopilot_motors_on = TRUE; #ifdef SetCommandsFromRC SetCommandsFromRC(commands, radio_control.values); #else @@ -216,13 +228,15 @@ static void autopilot_on_rc_frame(void) } /* Forward radiocontrol to AP */ - intermcu_on_rc_frame(); + intermcu_on_rc_frame(fbw_mode); } static void autopilot_on_ap_command(void) { if (fbw_mode != FBW_MODE_MANUAL) { SetCommands(intermcu_commands); + } else { + autopilot_motors_on = TRUE; } } @@ -237,6 +251,6 @@ STATIC_INLINE void main_event(void) // InterMCU InterMcuEvent(autopilot_on_ap_command); - // TODO Modules - //modules_event_task(); + //Modules + modules_event_task(); } diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c index 8dc94e6733..6136658415 100644 --- a/sw/airborne/mcu.c +++ b/sw/airborne/mcu.c @@ -83,7 +83,7 @@ void mcu_init(void) led_init(); #endif /* for now this means using spektrum */ -#if defined RADIO_CONTROL & defined RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT & defined RADIO_CONTROL_BIND_IMPL_FUNC +#if defined RADIO_CONTROL & defined RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT & defined RADIO_CONTROL_BIND_IMPL_FUNC & defined SPEKTRUM_BIND_PIN_PORT RADIO_CONTROL_BIND_IMPL_FUNC(); #endif #if USE_UART0 diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index 22be5f5800..615906af87 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -66,7 +66,7 @@ struct uart_periph { uint8_t tx_buf[UART_TX_BUFFER_SIZE]; uint16_t tx_insert_idx; uint16_t tx_extract_idx; - uint8_t tx_running; + volatile uint8_t tx_running; /** UART Register */ void *reg_addr; /** UART Baudrate */ diff --git a/sw/airborne/modules/px4io_flash/protocol.h b/sw/airborne/modules/px4io_flash/protocol.h new file mode 100644 index 0000000000..0317d7211e --- /dev/null +++ b/sw/airborne/modules/px4io_flash/protocol.h @@ -0,0 +1,390 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#define __STDC_FORMAT_MACROS +#include "std.h" + +/** + * @file protocol.h + * + * PX4IO interface protocol. + * + * Communication is performed via writes to and reads from 16-bit virtual + * registers organised into pages of 255 registers each. + * + * The first two bytes of each write select a page and offset address + * respectively. Subsequent reads and writes increment the offset within + * the page. + * + * Some pages are read- or write-only. + * + * Note that some pages may permit offset values greater than 255, which + * can only be achieved by long writes. The offset does not wrap. + * + * Writes to unimplemented registers are ignored. Reads from unimplemented + * registers return undefined values. + * + * As convention, values that would be floating point in other parts of + * the PX4 system are expressed as signed integer values scaled by 10000, + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. + * + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. + */ + +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) + +#define PX4IO_PROTOCOL_VERSION 4 + +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + +/* static configuration page */ +#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ +#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ +#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ +#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT 8 /**< hardcoded # of control groups*/ + +/* dynamic status page */ +#define PX4IO_PAGE_STATUS 1 +#define PX4IO_P_STATUS_FREEMEM 0 +#define PX4IO_P_STATUS_CPULOAD 1 + +#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */ + +#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ + +#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */ +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ + +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ +#define PX4IO_P_STATUS_MIXER_LOWER_LIMIT (1 << 0) /**< at least one actuator output has reached lower limit */ +#define PX4IO_P_STATUS_MIXER_UPPER_LIMIT (1 << 1) /**< at least one actuator output has reached upper limit */ +#define PX4IO_P_STATUS_MIXER_YAW_LIMIT (1 << 2) /**< yaw control is limited because it causes output clipping */ + +/* array of post-mix actuator outputs, -10000..10000 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of PWM servo output values, microseconds */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of raw RC input values, microseconds */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ +#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 50 +#define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ + +#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ + +#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ +#else +#define PX4IO_P_SETUP_RELAYS_PAD 5 +#endif + +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; +/* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +/* storage space of 12 occupied by CRC */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state - this is a non-data write and + hence index 12 can safely be used. */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ + +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ + +#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 52 + +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ + +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +#pragma pack(push, 1) +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; +#pragma pack(pop) + +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } + + return c; +} diff --git a/sw/airborne/modules/px4io_flash/px4io_flash.c b/sw/airborne/modules/px4io_flash/px4io_flash.c new file mode 100644 index 0000000000..cba0de8b51 --- /dev/null +++ b/sw/airborne/modules/px4io_flash/px4io_flash.c @@ -0,0 +1,258 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/px4io_flash/px4io_flash.c" + * @author Kevin van Hecke + * Flashes the px4io f1 through the px4 bootloader. + * Assumes the telem2 port on the Pixhawk is connected to a ttyACM device (blackmagic probe) + */ + +#include "modules/px4io_flash/px4io_flash.h" +//#include "subsystems/datalink/downlink.h" +#include "modules/px4io_flash/protocol.h" +#include "mcu_periph/sys_time_arch.h" +#include "subsystems/intermcu/intermcu_ap.h" + +// Serial Port +#include "mcu_periph/uart.h" + +// define coms link for px4io f1 +#define PX4IO_PORT (&((PX4IO_UART).device)) +#define TELEM2_PORT (&((TELEM2_UART).device)) + +// weird that these below are not in protocol.h, which is from the firmware px4 repo +// below is copied from qgroundcontrol: +#define PROTO_INSYNC 0x12 ///< 'in sync' byte sent before status +#define PROTO_EOC 0x20 ///< end of command +// Reply bytes +#define PROTO_OK 0x10 ///< INSYNC/OK - 'ok' response +#define PROTO_FAILED 0x11 ///< INSYNC/FAILED - 'fail' response +#define PROTO_INVALID 0x13 ///< INSYNC/INVALID - 'invalid' response for bad commands +// Command bytes +#define PROTO_GET_SYNC 0x21 ///< NOP for re-establishing sync +#define PROTO_GET_DEVICE 0x22 ///< get device ID bytes +#define PROTO_CHIP_ERASE 0x23 ///< erase program area and reset program address +#define PROTO_LOAD_ADDRESS 0x24 ///< set next programming address +#define PROTO_PROG_MULTI 0x27 ///< write bytes at program address and increment +#define PROTO_GET_CRC 0x29 ///< compute & return a CRC +#define PROTO_BOOT 0x30 ///< boot the application + +bool_t setToBootloaderMode; + +void px4ioflash_init(void) +{ + setToBootloaderMode = FALSE; +} + +void px4ioflash_event(void) +{ + // setToBootloaderMode=true; + if (PX4IO_PORT->char_available(PX4IO_PORT->periph)) { + if (!setToBootloaderMode) { + //ignore anything coming from IO if not in bootloader mode (which should be nothing) + } else { + //relay everything from IO to the laptop + unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph); + TELEM2_PORT->put_byte(TELEM2_PORT->periph, b); + } + } + + //TODO: check if timeout was surpassed + if (TELEM2_PORT->char_available(TELEM2_PORT->periph) && !setToBootloaderMode) { + //data was received on the pc uart, so + //stop all intermcu comminication: + disable_inter_comm(true); + //send the reboot to bootloader command: + + /* + * The progdieshit define is very usefull, if for whatever reason the (normal, not bootloader) firmware on the IO chip became disfunct. + * In that case: + * 1. enable this define + * 2. build and upload the fmu f4 chip (ap target in pprz center) + * 3. build the io code, and convert the firmware using the following command: + * /home/houjebek/paparazzi/sw/tools/pixhawk/px_mkfw.py --prototype "/home/houjebek/px4/Firmware/Images/px4io-v2.prototype" --image /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.bin > /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4 + * 4. Start the following command: + * /home/houjebek/paparazzi/sw/tools/pixhawk/px_uploader.py --port "/dev/serial/by-id/usb-FTDI_TTL232R-3V3_FT906KBO-if00-port0" /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4 + * 5a. Either, boot the Pixhawk (reconnect usb) holding the IO reset button until the FMU led stops blinking fast (i.e. exits its own bootloader) + * 5b Or, press the IO reset button on the pixhawk + * 6. Watch the output of the command of step 4, it should recognize the IO bootloader and start flashing. If not try repeating step 5a. + * 7. Don forget to disable the define and upload the ap again :) + */ + //#define progdieshit + +#ifndef progdieshit + static struct IOPacket dma_packet; + dma_packet.count_code = 0x40 + 0x01; + dma_packet.crc = 0; + dma_packet.page = PX4IO_PAGE_SETUP; + dma_packet.offset = PX4IO_P_SETUP_REBOOT_BL; + dma_packet.regs[0] = PX4IO_REBOOT_BL_MAGIC; + dma_packet.crc = crc_packet(&dma_packet); + struct IOPacket *pkt = &dma_packet; + uint8_t *p = (uint8_t *)pkt; + PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[0]); + PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[1]); + PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[2]); + PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[3]); + PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[4]); + PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[5]); + + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'E'); + // for (int i=0;i<6;i++) { + // unsigned char tmp[3]; + // itoa(p[i],tmp,16); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,tmp[0]); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,tmp[1]); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); + // } + + sys_time_usleep(5000); // this seems to be close to the minimum delay necessary to process this packet at the IO side + //the pixhawk IO chip should respond with: + // 0x00 ( PKT_CODE_SUCCESS ) + // 0xe5 + // 0x32 + // 0x0a + //After that, the IO chips reboots into bootloader mode, in which it will stay for a short period + //The baudrate in bootloader mode ic changed to 115200 (normal operating baud is 1500000, at least for original pixhawk fmu firmware) + + //state machine + int state = 0; + while (state < 4 && PX4IO_PORT->char_available(PX4IO_PORT->periph)) { + + unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph); + switch (state) { + case (0) : + if (b == PKT_CODE_SUCCESS) { state++; } else { state = 0; } + break; + case (1) : + if (b == 0xe5) { state++; } else { state = 0; } + break; + case (2) : + if (b == 0x32) { state++; } else { state = 0; } + break; + case (3) : + if (b == 0x0a) { state++; } else { state = 0; } + break; + default : + TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'b'); + break; + } + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,state+48); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); + } +#else + int state = 4; +#endif + if (state == 4) { +#ifndef progdieshit + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'6'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); +#endif + uart_periph_set_baudrate(PX4IO_PORT->periph, B115200); + /* look for the bootloader for 150 ms */ + int ret = 0; + for (int i = 0; i < 15 && !ret ; i++) { + sys_time_usleep(10000); + + + //send a get_sync command in order to keep the io in bootloader mode + PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_GET_SYNC); + PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_EOC); + + +#ifndef progdieshit + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'6'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'a'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); +#endif + + //get_sync should be replied with, so check if that happens and + //all other bytes are discarded, hopefully those were not important + //(they may be caused by sending multiple syncs) + while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) { + unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph); + +#ifndef progdieshit + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'6'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'b'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); +#endif + + if (b == PROTO_INSYNC) { +#ifndef progdieshit + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'7'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); +#endif + + setToBootloaderMode = true; + ret = 1; + break; + } + } + } + if (setToBootloaderMode) { + +#ifndef progdieshit + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'8'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); +#endif + + //if successfully entered bootloader mode, clear any remaining bytes (which may have a function, but I did not check) + while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {PX4IO_PORT->get_byte(PX4IO_PORT->periph);} + } + +#ifndef progdieshit + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'9'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); +#endif + + } else { + TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'E'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'r'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'r'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'o'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'r'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n'); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r'); + } + } else if (TELEM2_PORT->char_available(TELEM2_PORT->periph)) { + //already in bootloader mode, just directly relay data + unsigned char b = TELEM2_PORT->get_byte(TELEM2_PORT->periph); + PX4IO_PORT->put_byte(PX4IO_PORT->periph, b); + // TELEM2_PORT->put_byte(TELEM2_PORT->periph,b); + } + +} + diff --git a/sw/airborne/modules/px4io_flash/px4io_flash.h b/sw/airborne/modules/px4io_flash/px4io_flash.h new file mode 100644 index 0000000000..6e315e4f33 --- /dev/null +++ b/sw/airborne/modules/px4io_flash/px4io_flash.h @@ -0,0 +1,33 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/px4io_flash/px4io_flash.h" + * @author Kevin van Hecke + * Flashes the px4io f1 through the px4 bootloader. + */ + +#ifndef PX4IO_FLASH_H +#define PX4IO_FLASH_H + +extern void px4ioflash_init(void); +extern void px4ioflash_event(void); + +#endif + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.c b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.c new file mode 100644 index 0000000000..82213df990 --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.c @@ -0,0 +1,30 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind.c" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" + +void spektrum_soft_bind_init() {} + + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.h b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.h new file mode 100644 index 0000000000..0b1289d0a3 --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind.h" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#ifndef SPEKTRUM_SOFT_BIND_H +#define SPEKTRUM_SOFT_BIND_H + +#if defined FBW +#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" +#else +#include "modules/spektrum_soft_bind/spektrum_soft_bind_ap.h" +#endif + +#endif + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.c b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.c new file mode 100644 index 0000000000..e87f977742 --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.c @@ -0,0 +1,38 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind_ap.c" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#include "modules/spektrum_soft_bind/spektrum_soft_bind_ap.h" +#include "subsystems/intermcu/intermcu_ap.h" +#include "led.h" + +void spektrum_soft_bind_init(void) {} + +uint8_t bind_soft_value; +void spektrum_soft_bind_click(uint8_t val __attribute__((unused))) +{ + intermcu_send_spektrum_bind(); +} + + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.h b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.h new file mode 100644 index 0000000000..6cfcbde3ff --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind_ap.h" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#ifndef SPEKTRUM_AP_SOFT_BIND_H +#define SPEKTRUM_AP_SOFT_BIND_H + +#include "std.h" + +extern void spektrum_soft_bind_init(void); +extern void spektrum_soft_bind_click(uint8_t val); + +extern uint8_t bind_soft_value; + +#endif + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c new file mode 100644 index 0000000000..1496f27fad --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c @@ -0,0 +1,54 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" +#include "subsystems/intermcu/intermcu_fbw.h" +#include "mcu.h" +#include "subsystems/radio_control.h" +#include "mcu_periph/sys_time_arch.h" + +#include "led.h" +#include "mcu_periph/gpio.h" + +void spektrum_soft_bind_init(void) +{ + gpio_setup_output(SPEKTRUM_POWER_PIN_PORT, SPEKTRUM_POWER_PIN); + gpio_set(SPEKTRUM_POWER_PIN_PORT, SPEKTRUM_POWER_PIN); +} + +void received_spektrum_soft_bind(void) +{ + + //power cycle the spektrum + gpio_clear(SPEKTRUM_POWER_PIN_PORT, SPEKTRUM_POWER_PIN); + sys_time_usleep(100000); + gpio_set(SPEKTRUM_POWER_PIN_PORT, SPEKTRUM_POWER_PIN); + + //put to bind mode + RADIO_CONTROL_BIND_IMPL_FUNC(); //basically = radio_control_spektrum_try_bind() + + SpektrumUartInit(); + +} diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h new file mode 100644 index 0000000000..dcb262b003 --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h @@ -0,0 +1,34 @@ +/* + * Copyright (C) Kevin van Hecke + * + * 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, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#ifndef SPEKTRUM_FBW_SOFT_BIND_H +#define SPEKTRUM_FBW_SOFT_BIND_H +#include "std.h" + +extern void spektrum_soft_bind_init(void); +extern void received_spektrum_soft_bind(void); + +#endif + diff --git a/sw/airborne/subsystems/datalink/w5100.h b/sw/airborne/subsystems/datalink/w5100.h index 33cb150a13..360cb695cf 100644 --- a/sw/airborne/subsystems/datalink/w5100.h +++ b/sw/airborne/subsystems/datalink/w5100.h @@ -55,7 +55,7 @@ struct w5100_periph { volatile uint16_t tx_extract_idx[W5100_BUFFER_NUM]; volatile uint8_t work_tx[4]; volatile uint8_t work_rx[4]; - uint8_t tx_running; + volatile uint8_t tx_running; /** Generic device interface */ struct link_device device; }; @@ -90,7 +90,8 @@ static inline void w5100_read_buffer(struct pprz_transport *t) #define W5100CheckAndParse(_dev, _trans) w5100_check_and_parse(&(_dev).device, &(_trans)) -static inline void w5100_check_and_parse(struct link_device *dev, struct pprz_transport *trans) { +static inline void w5100_check_and_parse(struct link_device *dev, struct pprz_transport *trans) +{ if (dev->char_available(dev->periph)) { w5100_read_buffer(trans); if (trans->trans_rx.msg_received) { diff --git a/sw/airborne/subsystems/intermcu.h b/sw/airborne/subsystems/intermcu.h index d57047293a..eff1a9fcee 100644 --- a/sw/airborne/subsystems/intermcu.h +++ b/sw/airborne/subsystems/intermcu.h @@ -33,7 +33,9 @@ #define INTERMCU_AP 0 #define INTERMCU_FBW 1 +#ifndef INTERMCU_LOST_CNT #define INTERMCU_LOST_CNT 25 /* 50ms with a 512Hz timer TODO fixed value */ +#endif enum intermcu_status { INTERMCU_OK, diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c index e9862cef18..7f4b5c68f2 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.c +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -30,6 +30,10 @@ #include "pprzlink/pprz_transport.h" #include "mcu_periph/uart.h" +#include "subsystems/datalink/telemetry.h" +#include "subsystems/electrical.h" +#include "autopilot.h" + #if COMMANDS_NB > 8 #error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED" #endif @@ -41,9 +45,21 @@ static struct pprz_transport intermcu_transport; struct intermcu_t inter_mcu; static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void)); + + +static void send_status(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, + &(radio_control.status), &(radio_control.frame_rate), &(inter_mcu.status), &electrical.vsupply, + &electrical.current); // due to limitation of GCS, send the electrical from ap as if it comes from fbw... +} + void intermcu_init(void) { pprz_transport_init(&intermcu_transport); + + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status); + } void intermcu_periodic(void) @@ -56,12 +72,28 @@ void intermcu_periodic(void) } } +static bool_t disable_comm; +void disable_inter_comm(bool_t value) +{ + disable_comm = value; +} + void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused))) { - pprz_msg_send_IMCU_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device, - INTERMCU_AP, 0, COMMANDS_NB, command_values); //TODO: Fix status + if (!disable_comm) { + pprz_msg_send_IMCU_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device, + INTERMCU_AP, &autopilot_motors_on, COMMANDS_NB, command_values); //TODO: Append more status + } } +void intermcu_send_spektrum_bind(void) +{ + if (!disable_comm) { + pprz_msg_send_IMCU_SPEKTRUM_SOFT_BIND(&(intermcu_transport.trans_tx), intermcu_device, INTERMCU_AP); + } +} + + static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void)) { /* Parse the Inter MCU message */ @@ -70,6 +102,7 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_fram case DL_IMCU_RADIO_COMMANDS: { uint8_t i; uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(trans->payload); + inter_mcu.status = DL_IMCU_RADIO_COMMANDS_status(trans->payload); int16_t *rc_values = DL_IMCU_RADIO_COMMANDS_values(trans->payload); for (i = 0; i < size; i++) { radio_control.values[i] = rc_values[i]; @@ -92,14 +125,16 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_fram void RadioControlEvent(void (*frame_handler)(void)) { - /* Parse incoming bytes */ - if (intermcu_device->char_available(intermcu_device->periph)) { - while (intermcu_device->char_available(intermcu_device->periph) && !intermcu_transport.trans_rx.msg_received) { - parse_pprz(&intermcu_transport, intermcu_device->get_byte(intermcu_device->periph)); - } + if (!disable_comm) { + /* Parse incoming bytes */ + if (intermcu_device->char_available(intermcu_device->periph)) { + while (intermcu_device->char_available(intermcu_device->periph) && !intermcu_transport.trans_rx.msg_received) { + parse_pprz(&intermcu_transport, intermcu_device->get_byte(intermcu_device->periph)); + } - if (intermcu_transport.trans_rx.msg_received) { - intermcu_parse_msg(&(intermcu_transport.trans_rx), frame_handler); + if (intermcu_transport.trans_rx.msg_received) { + intermcu_parse_msg(&(intermcu_transport.trans_rx), frame_handler); + } } } } diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.h b/sw/airborne/subsystems/intermcu/intermcu_ap.h index 4898978fbc..6cee9f703c 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.h +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.h @@ -32,24 +32,20 @@ void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode); void RadioControlEvent(void (*frame_handler)(void)); +void intermcu_send_spektrum_bind(void); +void disable_inter_comm(bool_t value); /* We need radio defines for the Autopilot */ #define RADIO_THROTTLE 0 #define RADIO_ROLL 1 #define RADIO_PITCH 2 #define RADIO_YAW 3 -#define RADIO_GEAR 4 -#define RADIO_FLAP 5 +#define RADIO_MODE 4 +#define RADIO_KILL_SWITCH 5 #define RADIO_AUX1 5 #define RADIO_AUX2 6 #define RADIO_AUX3 7 #define RADIO_CONTROL_NB_CHANNEL 8 -#ifndef RADIO_MODE -#define RADIO_MODE RADIO_GEAR -#endif -//#ifndef RADIO_KILL_SWITCH -//#define RADIO_KILL_SWITCH RADIO_FLAP -//#endif #endif /* INTERMCU_AP_ROTORCRAFT_H */ diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c index f97a34badb..068e5df276 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c @@ -29,6 +29,18 @@ #include "subsystems/radio_control.h" #include "mcu_periph/uart.h" #include "pprzlink/pprz_transport.h" +#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" + +#ifdef BOARD_PX4IO +#include "libopencm3/cm3/scb.h" +#include "led.h" +#include "mcu_periph/sys_time.h" +static uint8_t px4RebootSequence[] = {0x41, 0xd7, 0x32, 0x0a, 0x46, 0x39}; +static uint8_t px4RebootSequenceCount = 0; +static bool_t px4RebootTimeout = FALSE; +uint8_t autopilot_motors_on = FALSE; +tid_t px4bl_tid; ///< id for time out of the px4 bootloader reset +#endif #if RADIO_CONTROL_NB_CHANNEL > 8 #undef RADIO_CONTROL_NB_CHANNEL @@ -43,10 +55,15 @@ static struct pprz_transport intermcu_transport; struct intermcu_t inter_mcu; pprz_t intermcu_commands[COMMANDS_NB]; static inline void intermcu_parse_msg(struct transport_rx *trans, void (*commands_frame_handler)(void)); +static inline void checkPx4RebootCommand(unsigned char b); void intermcu_init(void) { pprz_transport_init(&intermcu_transport); +#ifdef BOARD_PX4IO + px4bl_tid = sys_time_register_timer(20.0, NULL); +#endif + } void intermcu_periodic(void) @@ -59,10 +76,38 @@ void intermcu_periodic(void) } } -void intermcu_on_rc_frame(void) +void intermcu_on_rc_frame(uint8_t fbw_mode) { + + pprz_t values[8]; + + values[INTERMCU_RADIO_THROTTLE] = radio_control.values[RADIO_THROTTLE]; + values[INTERMCU_RADIO_ROLL] = radio_control.values[RADIO_ROLL]; + values[INTERMCU_RADIO_PITCH] = radio_control.values[RADIO_PITCH]; + values[INTERMCU_RADIO_YAW] = radio_control.values[RADIO_YAW]; +#ifdef RADIO_MODE + values[INTERMCU_RADIO_MODE] = radio_control.values[RADIO_MODE]; +#endif +#ifdef RADIO_KILL_SWITCH + values[INTERMCU_RADIO_KILL_SWITCH] = radio_control.values[RADIO_KILL]; +#endif + +#if defined (RADIO_AUX1) && defined (RADIO_KILL_SWITCH) +#warning "RC AUX1 and KILL_SWITCH are on the same channel." +#endif + +#ifdef RADIO_AUX1 + values[INTERMCU_RADIO_AUX1] = radio_control.values[RADIO_AUX1]; +#endif +#ifdef RADIO_AUX2 + values[INTERMCU_RADIO_AUX2] = radio_control.values[RADIO_AUX2]; +#endif +#ifdef RADIO_AUX3 + values[INTERMCU_RADIO_AUX2] = radio_control.values[RADIO_AUX2]; +#endif + pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device, - INTERMCU_FBW, 0, RADIO_CONTROL_NB_CHANNEL, radio_control.values); //TODO: Fix status + INTERMCU_FBW, &fbw_mode, RADIO_CONTROL_NB_CHANNEL, values); } void intermcu_send_status(uint8_t mode) @@ -81,6 +126,8 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*command uint8_t i; uint8_t size = DL_IMCU_COMMANDS_values_length(trans->payload); int16_t *new_commands = DL_IMCU_COMMANDS_values(trans->payload); + uint8_t status = DL_IMCU_COMMANDS_status(trans->payload); + autopilot_motors_on = status & 0x1; for (i = 0; i < size; i++) { intermcu_commands[i] = new_commands[i]; } @@ -91,6 +138,11 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*command break; } +#if defined(SPEKTRUM_HAS_SOFT_BIND_PIN) //TODO: make subscribable module parser + case DL_IMCU_SPEKTRUM_SOFT_BIND: + received_spektrum_soft_bind(); + break; +#endif default: break; } @@ -101,10 +153,15 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*command void InterMcuEvent(void (*frame_handler)(void)) { + /* Parse incoming bytes */ if (intermcu_device->char_available(intermcu_device->periph)) { while (intermcu_device->char_available(intermcu_device->periph) && !intermcu_transport.trans_rx.msg_received) { - parse_pprz(&intermcu_transport, intermcu_device->get_byte(intermcu_device->periph)); + unsigned char b = intermcu_device->get_byte(intermcu_device->periph); +#ifdef BOARD_PX4IO + checkPx4RebootCommand(b); +#endif + parse_pprz(&intermcu_transport, b); } if (intermcu_transport.trans_rx.msg_received) { @@ -112,3 +169,44 @@ void InterMcuEvent(void (*frame_handler)(void)) } } } +#ifdef BOARD_PX4IO +static inline void checkPx4RebootCommand(unsigned char b) +{ + if (!px4RebootTimeout) { + + + if (sys_time_check_and_ack_timer(px4bl_tid)) { + //time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight + px4RebootTimeout = true; + sys_time_cancel_timer(px4bl_tid); + return; + } + + LED_ON(SYS_TIME_LED); + + if (b == px4RebootSequence[px4RebootSequenceCount]) { + px4RebootSequenceCount++; + } else { + px4RebootSequenceCount = 0; + } + + if (px4RebootSequenceCount >= 6) { // 6 = length of rebootSequence + 1 + px4RebootSequenceCount = 0; // should not be necessary... + + //send some magic back + //this is the same as the Pixhawk IO code would send + intermcu_device->put_byte(intermcu_device->periph, 0x00); + intermcu_device->put_byte(intermcu_device->periph, 0xe5); + intermcu_device->put_byte(intermcu_device->periph, 0x32); + intermcu_device->put_byte(intermcu_device->periph, 0x0a); + intermcu_device->put_byte(intermcu_device->periph, + 0x66); // dummy byte, seems to be necessary otherwise one byte is missing at the fmu side... + + while (((struct uart_periph *)(intermcu_device->periph))->tx_running) {LED_TOGGLE(SYS_TIME_LED);} // tx_running is volatile now, so LED_TOGGLE not necessary anymore + + LED_OFF(SYS_TIME_LED); + scb_reset_system(); + } + } +} +#endif diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.h b/sw/airborne/subsystems/intermcu/intermcu_fbw.h index 82a23664b9..88d065bd00 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_fbw.h +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.h @@ -29,9 +29,23 @@ #include "subsystems/intermcu.h" +extern uint8_t autopilot_motors_on; extern pprz_t intermcu_commands[COMMANDS_NB]; -void intermcu_on_rc_frame(void); +void intermcu_on_rc_frame(uint8_t fbw_mode); void intermcu_send_status(uint8_t mode); void InterMcuEvent(void (*frame_handler)(void)); + +/* We need radio defines for the Autopilot */ +#define INTERMCU_RADIO_THROTTLE 0 +#define INTERMCU_RADIO_ROLL 1 +#define INTERMCU_RADIO_PITCH 2 +#define INTERMCU_RADIO_YAW 3 +#define INTERMCU_RADIO_MODE 4 +#define INTERMCU_RADIO_KILL_SWITCH 5 +#define INTERMCU_RADIO_AUX1 5 +#define INTERMCU_RADIO_AUX2 6 +#define INTERMCU_RADIO_AUX3 7 +#define INTERMCU_RADIO_CONTROL_NB_CHANNEL 8 + #endif /* INTERMCU_FBW_ROTORCRAFT_H */ diff --git a/sw/airborne/subsystems/radio_control.c b/sw/airborne/subsystems/radio_control.c index 72a0c8a20d..dbbb4b3fdb 100644 --- a/sw/airborne/subsystems/radio_control.c +++ b/sw/airborne/subsystems/radio_control.c @@ -64,7 +64,7 @@ void radio_control_periodic_task(void) radio_control.time_since_last_frame++; } -#if defined RADIO_CONTROL_LED +#if defined(RADIO_CONTROL_LED) if (radio_control.status == RC_OK) { LED_ON(RADIO_CONTROL_LED); } else { diff --git a/sw/airborne/test/test_datalink.c b/sw/airborne/test/test_datalink.c index 6ff8575b4a..58beb110e0 100644 --- a/sw/airborne/test/test_datalink.c +++ b/sw/airborne/test/test_datalink.c @@ -19,6 +19,11 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file test_datalink.c + * + * Periodically sends ALIVE (10Hz) and ping/pong (every 5s) telemetry messages. + */ #define DATALINK_C #include BOARD_CONFIG @@ -50,6 +55,7 @@ static inline void main_init(void) { mcu_init(); sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); + downlink_init(); } static inline void main_periodic(void) @@ -74,3 +80,4 @@ void dl_parse_msg(void) break; } } + diff --git a/sw/tools/px4/print_message.py b/sw/tools/px4/print_message.py new file mode 100755 index 0000000000..d9593ab7ed --- /dev/null +++ b/sw/tools/px4/print_message.py @@ -0,0 +1,6 @@ +#!/usr/bin/python +# temporary solution to print a message to reconnect the usb cable +#At some point hopefully this can be automated (without replugging) +print("\n ") +print("**** Please reconnect the usb cable now! *****") +print(" \n") diff --git a/sw/tools/px4/px4fmu-v2.prototype b/sw/tools/px4/px4fmu-v2.prototype new file mode 100644 index 0000000000..5109b77d1e --- /dev/null +++ b/sw/tools/px4/px4fmu-v2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 9, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv2 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv2", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/sw/tools/px4/px4io-v2.prototype b/sw/tools/px4/px4io-v2.prototype new file mode 100644 index 0000000000..af87924e90 --- /dev/null +++ b/sw/tools/px4/px4io-v2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 10, + "magic": "PX4FWv2", + "description": "Firmware for the PX4IOv2 board", + "image": "", + "build_time": 0, + "summary": "PX4IOv2", + "version": "2.0", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/sw/tools/px4/px_mkfw.py b/sw/tools/px4/px_mkfw.py new file mode 100755 index 0000000000..2f07fa1e73 --- /dev/null +++ b/sw/tools/px4/px_mkfw.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# PX4 firmware image generator +# +# The PX4 firmware file is a JSON-encoded Python object, containing +# metadata fields and a zlib-compressed base64-encoded firmware image. +# + +import sys +import argparse +import json +import base64 +import zlib +import time +import subprocess + +# +# Construct a basic firmware description +# +def mkdesc(): + proto = {} + proto['magic'] = "PX4FWv1" + proto['board_id'] = 0 + proto['board_revision'] = 0 + proto['version'] = "" + proto['summary'] = "" + proto['description'] = "" + proto['git_identity'] = "" + proto['build_time'] = 0 + proto['image'] = bytes() + proto['image_size'] = 0 + return proto + +# Parse commandline +parser = argparse.ArgumentParser(description="Firmware generator for the PX autopilot system.") +parser.add_argument("--prototype", action="store", help="read a prototype description from a file") +parser.add_argument("--board_id", action="store", help="set the board ID required") +parser.add_argument("--board_revision", action="store", help="set the board revision required") +parser.add_argument("--version", action="store", help="set a version string") +parser.add_argument("--summary", action="store", help="set a brief description") +parser.add_argument("--description", action="store", help="set a longer description") +parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") +parser.add_argument("--parameter_xml", action="store", help="the parameters.xml file") +parser.add_argument("--airframe_xml", action="store", help="the airframes.xml file") +parser.add_argument("--image", action="store", help="the firmware image") +args = parser.parse_args() + +# Fetch the firmware descriptor prototype if specified +if args.prototype != None: + f = open(args.prototype,"r") + desc = json.load(f) + f.close() +else: + desc = mkdesc() + +desc['build_time'] = int(time.time()) + +if args.board_id != None: + desc['board_id'] = int(args.board_id) +if args.board_revision != None: + desc['board_revision'] = int(args.board_revision) +if args.version != None: + desc['version'] = str(args.version) +if args.summary != None: + desc['summary'] = str(args.summary) +if args.description != None: + desc['description'] = str(args.description) +if args.git_identity != None: + cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"]) + p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout + desc['git_identity'] = str(p.read().strip()) + p.close() +if args.parameter_xml != None: + f = open(args.parameter_xml, "rb") + bytes = f.read() + desc['parameter_xml_size'] = len(bytes) + desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') +if args.airframe_xml != None: + f = open(args.airframe_xml, "rb") + bytes = f.read() + desc['airframe_xml_size'] = len(bytes) + desc['airframe_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') +if args.image != None: + f = open(args.image, "rb") + bytes = f.read() + desc['image_size'] = len(bytes) + desc['image'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') + +print(json.dumps(desc, indent=4)) diff --git a/sw/tools/px4/px_uploader.py b/sw/tools/px4/px_uploader.py new file mode 100755 index 0000000000..ea83273bfc --- /dev/null +++ b/sw/tools/px4/px_uploader.py @@ -0,0 +1,636 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Serial firmware uploader for the PX4FMU bootloader +# +# The PX4 firmware file is a JSON-encoded Python object, containing +# metadata fields and a zlib-compressed base64-encoded firmware image. +# +# The uploader uses the following fields from the firmware file: +# +# image +# The firmware that will be uploaded. +# image_size +# The size of the firmware in bytes. +# board_id +# The board for which the firmware is intended. +# board_revision +# Currently only used for informational purposes. +# + +# for python2.7 compatibility +from __future__ import print_function + +import sys +import argparse +import binascii +import serial +import struct +import json +import zlib +import base64 +import time +import array +import os + +from sys import platform as _platform + + +class firmware(object): + '''Loads a firmware file''' + + desc = {} + image = bytes() + crctab = array.array('I', [ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d]) + crcpad = bytearray(b'\xff\xff\xff\xff') + + def __init__(self, path): + + # read the file + f = open(path, "r") + self.desc = json.load(f) + f.close() + + self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) + + # pad image to 4-byte length + while ((len(self.image) % 4) != 0): + self.image.append('\xff') + + def property(self, propname): + return self.desc[propname] + + def __crc32(self, bytes, state): + for byte in bytes: + index = (state ^ byte) & 0xff + state = self.crctab[index] ^ (state >> 8) + return state + + def crc(self, padlen): + state = self.__crc32(self.image, int(0)) + for i in range(len(self.image), (padlen - 1), 4): + state = self.__crc32(self.crcpad, state) + return state + + +class uploader(object): + '''Uploads a firmware file to the PX FMU bootloader''' + + # protocol bytes + INSYNC = b'\x12' + EOC = b'\x20' + + # reply bytes + OK = b'\x10' + FAILED = b'\x11' + INVALID = b'\x13' # rev3+ + BAD_SILICON_REV = b'\x14' # rev5+ + + # command bytes + NOP = b'\x00' # guaranteed to be discarded by the bootloader + GET_SYNC = b'\x21' + GET_DEVICE = b'\x22' + CHIP_ERASE = b'\x23' + CHIP_VERIFY = b'\x24' # rev2 only + PROG_MULTI = b'\x27' + READ_MULTI = b'\x28' # rev2 only + GET_CRC = b'\x29' # rev3+ + GET_OTP = b'\x2a' # rev4+ , get a word from OTP area + GET_SN = b'\x2b' # rev4+ , get a word from SN area + GET_CHIP = b'\x2c' # rev5+ , get chip version + SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay + GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII + MAX_DES_LENGTH = 20 + + REBOOT = b'\x30' + + INFO_BL_REV = b'\x01' # bootloader protocol revision + BL_REV_MIN = 2 # minimum supported bootloader protocol + BL_REV_MAX = 5 # maximum supported bootloader protocol + INFO_BOARD_ID = b'\x02' # board type + INFO_BOARD_REV = b'\x03' # board revision + INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes + + PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4 + READ_MULTI_MAX = 252 # protocol max is 255 + + NSH_INIT = bytearray(b'\x0d\x0d\x0d') + NSH_REBOOT_BL = b"reboot -b\n" + NSH_REBOOT = b"reboot\n" + MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') + MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') + + def __init__(self, portname, baudrate): + # open the port, keep the default timeout short so we can poll quickly + self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.otp = b'' + self.sn = b'' + + def close(self): + if self.port is not None: + self.port.close() + + def __send(self, c): + #print("send " + binascii.hexlify(c)) + self.port.write(c) + + + def __recv(self, count=1): + c = self.port.read(count) + if len(c) < 1: + #print("Timeout") + raise RuntimeError("timeout waiting for data (%u bytes)" % count) + #print("recv " + binascii.hexlify(c)) + return c + + def __recv_int(self): + raw = self.__recv(4) + val = struct.unpack("= 9.0: + self.__drawProgressBar(label, 20.0-estimatedTimeRemaining, 9.0) + else: + self.__drawProgressBar(label, 10.0, 10.0) + sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) ) + sys.stdout.flush() + + if self.__trySync(): + self.__drawProgressBar(label, 10.0, 10.0) + return; + + raise RuntimeError("timed out waiting for erase") + + # send a PROG_MULTI command to write a collection of bytes + def __program_multi(self, data): + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.PROG_MULTI) + self.__send(length) + self.__send(data) + self.__send(uploader.EOC) + self.__getSync() + + # verify multiple bytes in flash + def __verify_multi(self, data): + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.READ_MULTI) + self.__send(length) + self.__send(uploader.EOC) + self.port.flush() + programmed = self.__recv(len(data)) + if programmed != data: + print("got " + binascii.hexlify(programmed)) + print("expect " + binascii.hexlify(data)) + return False + self.__getSync() + return True + + # send the reboot command + def __reboot(self): + self.__send(uploader.REBOOT + + uploader.EOC) + self.port.flush() + + # v3+ can report failure if the first word flash fails + if self.bl_rev >= 3: + self.__getSync() + + # split a sequence into a list of size-constrained pieces + def __split_len(self, seq, length): + return [seq[i:i+length] for i in range(0, len(seq), length)] + + # upload code + def __program(self, label, fw): + print("\n", end='') + code = fw.image + groups = self.__split_len(code, uploader.PROG_MULTI_MAX) + + uploadProgress = 0 + for bytes in groups: + self.__program_multi(bytes) + + #Print upload progress (throttled, so it does not delay upload progress) + uploadProgress += 1 + if uploadProgress % 256 == 0: + self.__drawProgressBar(label, uploadProgress, len(groups)) + self.__drawProgressBar(label, 100, 100) + + # verify code + def __verify_v2(self, label, fw): + print("\n", end='') + self.__send(uploader.CHIP_VERIFY + + uploader.EOC) + self.__getSync() + code = fw.image + groups = self.__split_len(code, uploader.READ_MULTI_MAX) + verifyProgress = 0 + for bytes in groups: + verifyProgress += 1 + if verifyProgress % 256 == 0: + self.__drawProgressBar(label, verifyProgress, len(groups)) + if (not self.__verify_multi(bytes)): + raise RuntimeError("Verification failed") + self.__drawProgressBar(label, 100, 100) + + def __verify_v3(self, label, fw): + print("\n", end='') + self.__drawProgressBar(label, 1, 100) + expect_crc = fw.crc(self.fw_maxsize) + self.__send(uploader.GET_CRC + + uploader.EOC) + report_crc = self.__recv_int() + self.__getSync() + verifyProgress = 0 + if report_crc != expect_crc: + print("Expected 0x%x" % expect_crc) + print("Got 0x%x" % report_crc) + raise RuntimeError("Program CRC failed") + self.__drawProgressBar(label, 100, 100) + + def __set_boot_delay(self, boot_delay): + self.__send(uploader.SET_BOOT_DELAY + + struct.pack("b", boot_delay) + + uploader.EOC) + self.__getSync() + + # get basic data about the board + def identify(self): + # make sure we are in sync before starting + self.__sync() + + # get the bootloader protocol ID first + self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) + if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): + print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) + raise RuntimeError("Bootloader protocol mismatch") + + self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) + self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) + self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) + + # upload the firmware + def upload(self, fw): + # Make sure we are doing the right thing + if self.board_type != fw.property('board_id'): + msg = "Firmware not suitable for this board (board_type=%u board_id=%u)" % ( + self.board_type, fw.property('board_id')) + if args.force: + print("WARNING: %s" % msg) + else: + raise IOError(msg) + if self.fw_maxsize < fw.property('image_size'): + raise RuntimeError("Firmware image is too large for this board") + + # OTP added in v4: + if self.bl_rev > 3: + for byte in range(0,32*6,4): + x = self.__getOTP(byte) + self.otp = self.otp + x + print(binascii.hexlify(x).decode('Latin-1') + ' ', end='') + # see src/modules/systemlib/otp.h in px4 code: + self.otp_id = self.otp[0:4] + self.otp_idtype = self.otp[4:5] + self.otp_vid = self.otp[8:4:-1] + self.otp_pid = self.otp[12:8:-1] + self.otp_coa = self.otp[32:160] + # show user: + try: + print("type: " + self.otp_id.decode('Latin-1')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('Latin-1'), end='') # show user + print('') + print("chip: %08x" % self.__getCHIP()) + if (self.bl_rev >= 5): + des = self.__getCHIPDes() + if (len(des) == 2): + print("family: %s" % des[0]) + print("revision: %s" % des[1]) + print("flash %d" % self.fw_maxsize) + except Exception: + # ignore bad character encodings + pass + + self.__erase("Erase ") + self.__program("Program", fw) + + if self.bl_rev == 2: + self.__verify_v2("Verify ", fw) + else: + self.__verify_v3("Verify ", fw) + + if args.boot_delay is not None: + self.__set_boot_delay(args.boot_delay) + + print("\nRebooting.\n") + self.__reboot() + self.port.close() + + def send_reboot(self): + try: + # try reboot via NSH first + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT_BL) + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT) + # then try MAVLINK command + self.__send(uploader.MAVLINK_REBOOT_ID1) + self.__send(uploader.MAVLINK_REBOOT_ID0) + except: + return + +# Detect python version +if sys.version_info[0] < 3: + runningPython3 = False +else: + runningPython3 = True + +# Parse commandline arguments +parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") +parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") +parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") +parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading') +parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') +parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") +args = parser.parse_args() + +# warn people about ModemManager which interferes badly with Pixhawk +if os.path.exists("/usr/sbin/ModemManager"): + print("==========================================================================================================") + print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)") + print("==========================================================================================================") + +# Load the firmware file +fw = firmware(args.firmware) +print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'))) +print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") + +# Spin waiting for a device to show up +try: + while True: + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: + + #print("Trying %s" % port) + + # create an uploader attached to the port + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) + except Exception: + # open failed, rate-limit our attempts + time.sleep(0.05) + + # and loop to the next port + continue + # port is open, try talking to it + try: + # identify the bootloader + up.identify() + print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) + + except Exception: + # most probably a timeout talking to the port, no bootloader, try to reboot the board + print("attempting reboot on %s..." % port) + print("if the board does not respond, unplug and re-plug the USB connector.") + up.send_reboot() + + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(0.5) + + # always close the port + up.close() + continue + + try: + # ok, we have a bootloader, try flashing it + up.upload(fw) + + except RuntimeError as ex: + # print the error + print("\nERROR: %s" % ex.args) + + except IOError as e: + up.close(); + continue + + finally: + # always close the port + up.close() + + # we could loop here if we wanted to wait for more boards... + sys.exit(0) + + # Delay retries to < 20 Hz to prevent spin-lock from hogging the CPU + time.sleep(0.05) + +# CTRL+C aborts the upload/spin-lock by interrupt mechanics +except KeyboardInterrupt: + print("\n Upload aborted by user.") + sys.exit(0)