diff --git a/conf/airframes/ENAC/fixed-wing/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml index afe5f8a96f..d98be030f0 100644 --- a/conf/airframes/ENAC/fixed-wing/apogee.xml +++ b/conf/airframes/ENAC/fixed-wing/apogee.xml @@ -8,6 +8,9 @@ + + + @@ -21,6 +24,7 @@ + @@ -38,6 +42,7 @@ + diff --git a/conf/airframes/examples/MentorEnergy.xml b/conf/airframes/examples/MentorEnergy.xml index 39c98a1283..9416ac28b8 100644 --- a/conf/airframes/examples/MentorEnergy.xml +++ b/conf/airframes/examples/MentorEnergy.xml @@ -9,7 +9,6 @@ - @@ -27,7 +26,6 @@ - - + diff --git a/conf/airframes/examples/krooz_sd/fixedwing/krooz_sd_fw.xml b/conf/airframes/examples/krooz_sd/fixedwing/krooz_sd_fw.xml new file mode 100644 index 0000000000..7c0ab8835c --- /dev/null +++ b/conf/airframes/examples/krooz_sd/fixedwing/krooz_sd_fw.xml @@ -0,0 +1,190 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + +
+ +
+ + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml old mode 100755 new mode 100644 index 00c0fd5239..36e979ea40 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -17,7 +17,7 @@ - + @@ -91,23 +91,23 @@ - - - + + + - - - - - - + + + + + + - - - - - - + + + + + + @@ -123,7 +123,7 @@
- +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml old mode 100755 new mode 100644 index 916d281c06..40786c6218 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -17,7 +17,7 @@ - + @@ -94,23 +94,23 @@ - - - + + + - - - - - - + + + + + + - - - - - - + + + + + + @@ -126,7 +126,7 @@
- +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml old mode 100755 new mode 100644 index c018d28048..0b31cd058e --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -15,7 +15,7 @@ - + @@ -84,16 +84,16 @@ - - - + + + - - - + + + @@ -116,7 +116,7 @@
- +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml old mode 100755 new mode 100644 index 88b83d3cf3..9f3d371545 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -19,7 +19,7 @@ - + @@ -79,27 +79,27 @@ - - - + + + - - - - - - + + + + + + - - - - - - + + + + + + - +
@@ -111,7 +111,7 @@
- +
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index 8f583e899b..1c3262b131 100644 --- a/conf/airframes/examples/quadshot_asp21_spektrum.xml +++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml @@ -246,7 +246,7 @@ More information on the Quadshot can be found at transition-robotics.com --> - + diff --git a/conf/airframes/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml index c19b65dddd..84cc1f6305 100644 --- a/conf/airframes/flixr_discovery.xml +++ b/conf/airframes/flixr_discovery.xml @@ -67,7 +67,7 @@ diff --git a/conf/airframes/obsolete/mm/fixed-wing/slowfast2.xml b/conf/airframes/obsolete/mm/fixed-wing/slowfast2.xml index ce6d0b7f49..8a0c6e8042 100644 --- a/conf/airframes/obsolete/mm/fixed-wing/slowfast2.xml +++ b/conf/airframes/obsolete/mm/fixed-wing/slowfast2.xml @@ -36,7 +36,7 @@ - + diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index e3ae4a8c97..bd5519ef4c 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -201,6 +201,10 @@ ap.CFLAGS += -DUSE_SPI1 ap.srcs += peripherals/mcp355x.c ap.srcs += $(SRC_BOARD)/baro_board.c +# krooz baro +else ifeq ($(BOARD), krooz) +ap.srcs += $(SRC_BOARD)/baro_board.c + # apogee baro else ifeq ($(BOARD), apogee) ap.CFLAGS += -DUSE_I2C1 diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile deleted file mode 100644 index 446b84af59..0000000000 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile +++ /dev/null @@ -1,73 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- -# -# Aspirin IMU v2.1 -# -# -# required xml: -#
-# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -#
-# -# - - -# for fixedwing firmware and ap only -ifeq ($(TARGET), ap) - IMU_ASPIRIN_CFLAGS = -DUSE_IMU -endif - -IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\" -IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ - $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c - -include $(CFG_SHARED)/spi_master.makefile - -ifeq ($(ARCH), lpc21) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI1 -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0 -else ifeq ($(ARCH), stm32) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI2 -# Slave select configuration -# SLAVE2 is on PB12 (NSS) (MPU600 CS) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2 -endif - -IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1 - -# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets -# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example - -ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS) -ap.srcs += $(IMU_ASPIRIN_SRCS) - - -# -# NPS simulator -# -include $(CFG_SHARED)/imu_nps.makefile diff --git a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile new file mode 100644 index 0000000000..c2a7e83fe0 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile @@ -0,0 +1,29 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# KroozSD IMU +# + +IMU_KROOZ_CFLAGS = -DUSE_IMU +IMU_KROOZ_CFLAGS += -DIMU_TYPE_H=\"boards/krooz/imu_krooz.h\" + +IMU_KROOZ_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_BOARD)/imu_krooz.c \ + $(SRC_ARCH)/subsystems/imu/imu_krooz_sd_arch.c + +IMU_KROOZ_I2C_DEV=i2c2 +IMU_KROOZ_CFLAGS += -DUSE_I2C -DUSE_I2C2 -DI2C2_CLOCK_SPEED=400000 + +IMU_KROOZ_CFLAGS += -DIMU_KROOZ_I2C_DEV=$(IMU_KROOZ_I2C_DEV) +IMU_KROOZ_CFLAGS += -DMS5611_I2C_DEV=$(IMU_KROOZ_I2C_DEV) -DMS5611_SLAVE_ADDR=0xEC +IMU_KROOZ_SRCS += peripherals/mpu60x0.c +IMU_KROOZ_SRCS += peripherals/mpu60x0_i2c.c +IMU_KROOZ_SRCS += peripherals/hmc58xx.c +IMU_KROOZ_SRCS += modules/sensors/baro_ms5611_i2c.c + +AHRS_PROPAGATE_FREQUENCY ?= 512 +AHRS_CORRECT_FREQUENCY ?= 512 +ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) + +ap.CFLAGS += $(IMU_KROOZ_CFLAGS) +ap.srcs += $(IMU_KROOZ_SRCS) diff --git a/conf/modules/airspeed_ets.xml b/conf/modules/airspeed_ets.xml index de10d509a9..07cf003315 100644 --- a/conf/modules/airspeed_ets.xml +++ b/conf/modules/airspeed_ets.xml @@ -17,11 +17,12 @@ - Yellow wire: SDA - Brown wire: SCL - - - + + + + - +
diff --git a/conf/modules/baro_amsys.xml b/conf/modules/baro_amsys.xml index acd6f98b46..2b94bb226c 100644 --- a/conf/modules/baro_amsys.xml +++ b/conf/modules/baro_amsys.xml @@ -7,6 +7,10 @@ Module to read a Amsys AMS 5812-0150-A barometric sensor via I2C. + + + +
diff --git a/conf/modules/baro_ets.xml b/conf/modules/baro_ets.xml index 48b79418a0..0b59c2a45b 100644 --- a/conf/modules/baro_ets.xml +++ b/conf/modules/baro_ets.xml @@ -2,10 +2,26 @@ - Baro ETS (I2C) - - - + + Baro ETS (I2C). + Driver for the EagleTree Systems Baro Sensor. + Has only been tested with V3 of the sensor hardware. + + Notes: + Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together. + Sensor should be in the proprietary mode (default) and not in 3rd party mode. + + Sensor module wire assignments: + - Red wire: 5V + - White wire: Ground + - Yellow wire: SDA + - Brown wire: SCL + + + + + +
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c index 4384d15686..bb5d82876c 100644 --- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c @@ -84,15 +84,17 @@ struct spi_periph_dma { uint32_t spi; ///< SPI peripheral identifier uint32_t spidr; ///< SPI DataRegister address for DMA uint32_t dma; ///< DMA controller base address (DMA1 or DMA2) - uint8_t rx_chan; ///< receive DMA channel number - uint8_t tx_chan; ///< transmit DMA channel number + uint8_t rx_chan; ///< receive DMA channel (or stream on F4) number + uint8_t tx_chan; ///< transmit DMA channel (or stream on F4) number + uint32_t rx_chan_sel; ///< F4 only: actual receive DMA channel number + uint32_t tx_chan_sel; ///< F4 only: actual transmit DMA channel number uint8_t rx_nvic_irq; ///< receive interrupt uint8_t tx_nvic_irq; ///< transmit interrupt uint16_t tx_dummy_buf; ///< dummy tx buffer for receive only cases - bool_t tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len + bool_t tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len uint16_t rx_dummy_buf; ///< dummy rx buffer for receive only cases - bool_t rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len - struct locm3_spi_comm comm; ///< current communication paramters + bool_t rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len + struct locm3_spi_comm comm; ///< current communication paramters uint8_t comm_sig; ///< comm config signature used to check for changes }; @@ -439,12 +441,17 @@ static void set_comm_from_transaction(struct locm3_spi_comm* c, struct spi_trans static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr, uint16_t len, enum SPIDataSizeSelect dss, bool_t increment) { +#ifdef STM32F1 dma_channel_reset(dma, chan); +#elif defined STM32F4 + dma_stream_reset(dma, chan); +#endif dma_set_peripheral_address(dma, chan, periph_addr); dma_set_memory_address(dma, chan, buf_addr); dma_set_number_of_data(dma, chan, len); /* Set the dma transfer size based on SPI transaction DSS */ +#ifdef STM32F1 if (dss == SPIDss8bit) { dma_set_peripheral_size(dma, chan, DMA_CCR_PSIZE_8BIT); dma_set_memory_size(dma, chan, DMA_CCR_MSIZE_8BIT); @@ -452,6 +459,15 @@ static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, dma_set_peripheral_size(dma, chan, DMA_CCR_PSIZE_16BIT); dma_set_memory_size(dma, chan, DMA_CCR_MSIZE_16BIT); } +#elif defined STM32F4 + if (dss == SPIDss8bit) { + dma_set_peripheral_size(dma, chan, DMA_SxCR_PSIZE_8BIT); + dma_set_memory_size(dma, chan, DMA_SxCR_MSIZE_8BIT); + } else { + dma_set_peripheral_size(dma, chan, DMA_SxCR_PSIZE_16BIT); + dma_set_memory_size(dma, chan, DMA_SxCR_MSIZE_16BIT); + } +#endif if (increment) dma_enable_memory_increment_mode(dma, chan); @@ -566,8 +582,14 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran dma->rx_extra_dummy_dma = TRUE; } } +#ifdef STM32F1 dma_set_read_from_peripheral(dma->dma, dma->rx_chan); dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_VERY_HIGH); +#elif defined STM32F4 + dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel); + dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM); + dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_VERY_HIGH); +#endif /* @@ -591,17 +613,27 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran dma->tx_extra_dummy_dma = TRUE; } } +#ifdef STM32F1 dma_set_read_from_memory(dma->dma, dma->tx_chan); dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); - +#elif defined STM32F4 + dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel); + dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL); + dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM); +#endif /* Enable DMA transfer complete interrupts. */ dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan); dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan); /* Enable DMA channels */ +#ifdef STM32F1 dma_enable_channel(dma->dma, dma->rx_chan); dma_enable_channel(dma->dma, dma->tx_chan); +#elif defined STM32F4 + dma_enable_stream(dma->dma, dma->rx_chan); + dma_enable_stream(dma->dma, dma->tx_chan); +#endif /* Enable SPI transfers via DMA */ spi_enable_rx_dma((uint32_t)periph->reg_addr); @@ -620,11 +652,22 @@ void spi1_arch_init(void) { // set dma options spi1_dma.spidr = (uint32_t)&SPI1_DR; +#ifdef STM32F1 spi1_dma.dma = DMA1; spi1_dma.rx_chan = DMA_CHANNEL2; spi1_dma.tx_chan = DMA_CHANNEL3; spi1_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL2_IRQ; spi1_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL3_IRQ; +#elif defined STM32F4 + spi1_dma.dma = DMA2; + // TODO make a macro to configure this from board/airframe file ? + spi1_dma.rx_chan = DMA_STREAM0; + spi1_dma.tx_chan = DMA_STREAM3; + spi1_dma.rx_chan_sel = DMA_SxCR_CHSEL_3; + spi1_dma.tx_chan_sel = DMA_SxCR_CHSEL_3; + spi1_dma.rx_nvic_irq = NVIC_DMA2_STREAM0_IRQ; + spi1_dma.tx_nvic_irq = NVIC_DMA2_STREAM3_IRQ; +#endif spi1_dma.tx_dummy_buf = 0; spi1_dma.tx_extra_dummy_dma = FALSE; spi1_dma.rx_dummy_buf = 0; @@ -646,12 +689,22 @@ void spi1_arch_init(void) { rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_SPI1EN); // Configure GPIOs: SCK, MISO and MOSI +#ifdef STM32F1 + // TODO configure lisa board files to use gpio_setup_pin_af function gpio_set_mode(GPIO_BANK_SPI1_SCK, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_SPI1_SCK | GPIO_SPI1_MOSI); gpio_set_mode(GPIO_BANK_SPI1_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO_SPI1_MISO); +#elif defined STM32F4 + gpio_setup_pin_af(SPI1_GPIO_PORT_MISO, SPI1_GPIO_MISO, SPI1_GPIO_AF, FALSE); + gpio_setup_pin_af(SPI1_GPIO_PORT_MOSI, SPI1_GPIO_MOSI, SPI1_GPIO_AF, TRUE); + gpio_setup_pin_af(SPI1_GPIO_PORT_SCK, SPI1_GPIO_SCK, SPI1_GPIO_AF, TRUE); + + gpio_set_output_options(SPI1_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_MOSI); + gpio_set_output_options(SPI1_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_SCK); +#endif // reset SPI spi_reset(SPI1); @@ -677,7 +730,11 @@ void spi1_arch_init(void) { spi_set_nss_high(SPI1); // Enable SPI_1 DMA clock +#ifdef STM32F1 rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA1EN); +#elif defined STM32F4 + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA2EN); +#endif // Enable SPI1 periph. spi_enable(SPI1); @@ -692,10 +749,19 @@ void spi2_arch_init(void) { // set dma options spi2_dma.spidr = (uint32_t)&SPI2_DR; spi2_dma.dma = DMA1; +#ifdef STM32F1 spi2_dma.rx_chan = DMA_CHANNEL4; spi2_dma.tx_chan = DMA_CHANNEL5; spi2_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL4_IRQ; spi2_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL5_IRQ; +#elif defined STM32F4 + spi2_dma.rx_chan = DMA_STREAM3; + spi2_dma.tx_chan = DMA_STREAM4; + spi2_dma.rx_chan_sel = DMA_SxCR_CHSEL_0; + spi2_dma.tx_chan_sel = DMA_SxCR_CHSEL_0; + spi2_dma.rx_nvic_irq = NVIC_DMA1_STREAM3_IRQ; + spi2_dma.tx_nvic_irq = NVIC_DMA1_STREAM4_IRQ; +#endif spi2_dma.tx_dummy_buf = 0; spi2_dma.tx_extra_dummy_dma = FALSE; spi2_dma.rx_dummy_buf = 0; @@ -717,12 +783,22 @@ void spi2_arch_init(void) { rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_SPI2EN); // Configure GPIOs: SCK, MISO and MOSI +#ifdef STM32F1 + // TODO configure lisa board files to use gpio_setup_pin_af function gpio_set_mode(GPIO_BANK_SPI2_SCK, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_SPI2_SCK | GPIO_SPI2_MOSI); gpio_set_mode(GPIO_BANK_SPI2_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO_SPI2_MISO); +#elif defined STM32F4 + gpio_setup_pin_af(SPI2_GPIO_PORT_MISO, SPI2_GPIO_MISO, SPI2_GPIO_AF, FALSE); + gpio_setup_pin_af(SPI2_GPIO_PORT_MOSI, SPI2_GPIO_MOSI, SPI2_GPIO_AF, TRUE); + gpio_setup_pin_af(SPI2_GPIO_PORT_SCK, SPI2_GPIO_SCK, SPI2_GPIO_AF, TRUE); + + gpio_set_output_options(SPI2_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_MOSI); + gpio_set_output_options(SPI2_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_SCK); +#endif // reset SPI spi_reset(SPI2); @@ -747,7 +823,11 @@ void spi2_arch_init(void) { spi_set_nss_high(SPI2); // Enable SPI_2 DMA clock +#ifdef STM32F1 rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA1EN); +#elif defined STM32F4 + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA1EN); +#endif // Enable SPI2 periph. spi_enable(SPI2); @@ -761,11 +841,21 @@ void spi3_arch_init(void) { // set the default configuration spi3_dma.spidr = (uint32_t)&SPI3_DR; +#ifdef STM32F1 spi3_dma.dma = DMA2; spi3_dma.rx_chan = DMA_CHANNEL1; spi3_dma.tx_chan = DMA_CHANNEL2; spi3_dma.rx_nvic_irq = NVIC_DMA2_CHANNEL1_IRQ; spi3_dma.tx_nvic_irq = NVIC_DMA2_CHANNEL2_IRQ; +#elif defined STM32F4 + spi3_dma.dma = DMA1; + spi3_dma.rx_chan = DMA_STREAM0; + spi3_dma.tx_chan = DMA_STREAM5; + spi3_dma.rx_chan_sel = DMA_SxCR_CHSEL_0; + spi3_dma.tx_chan_sel = DMA_SxCR_CHSEL_0; + spi3_dma.rx_nvic_irq = NVIC_DMA1_STREAM0_IRQ; + spi3_dma.tx_nvic_irq = NVIC_DMA1_STREAM5_IRQ; +#endif spi3_dma.tx_dummy_buf = 0; spi3_dma.tx_extra_dummy_dma = FALSE; spi3_dma.rx_dummy_buf = 0; @@ -787,12 +877,22 @@ void spi3_arch_init(void) { rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_SPI3EN); // Configure GPIOs: SCK, MISO and MOSI +#ifdef STM32F1 + // TODO configure lisa board files to use gpio_setup_pin_af function gpio_set_mode(GPIO_BANK_SPI3_SCK, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_SPI3_SCK | GPIO_SPI3_MOSI); gpio_set_mode(GPIO_BANK_SPI3_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO_SPI3_MISO); +#elif defined STM32F4 + gpio_setup_pin_af(SPI3_GPIO_PORT_MISO, SPI3_GPIO_MISO, SPI3_GPIO_AF, FALSE); + gpio_setup_pin_af(SPI3_GPIO_PORT_MOSI, SPI3_GPIO_MOSI, SPI3_GPIO_AF, TRUE); + gpio_setup_pin_af(SPI3_GPIO_PORT_SCK, SPI3_GPIO_SCK, SPI3_GPIO_AF, TRUE); + + gpio_set_output_options(SPI3_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_MOSI); + gpio_set_output_options(SPI3_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_SCK); +#endif /// @todo disable JTAG so the pins can be used? @@ -819,7 +919,11 @@ void spi3_arch_init(void) { spi_set_nss_high(SPI3); // Enable SPI_3 DMA clock +#ifdef STM32F1 rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA2EN); +#elif defined STM32F4 + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA1EN); +#endif // Enable SPI3 periph. spi_enable(SPI3); @@ -838,22 +942,40 @@ void spi3_arch_init(void) { *****************************************************************************/ #ifdef USE_SPI1 /// receive transferred over DMA +#ifdef STM32F1 void dma1_channel2_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF2) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF2; } +#elif defined STM32F4 +void dma2_stream0_isr(void) +{ + if ((DMA2_LISR & DMA_LISR_TCIF0) != 0) { + // clear int pending bit + DMA2_LIFCR |= DMA_LIFCR_CTCIF0; + } +#endif process_rx_dma_interrupt(&spi1); } /// transmit transferred over DMA +#ifdef STM32F1 void dma1_channel3_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF3) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF3; } +#elif defined STM32F4 +void dma2_stream3_isr(void) +{ + if ((DMA2_LISR & DMA_LISR_TCIF3) != 0) { + // clear int pending bit + DMA2_LIFCR |= DMA_LIFCR_CTCIF3; + } +#endif process_tx_dma_interrupt(&spi1); } @@ -861,22 +983,40 @@ void dma1_channel3_isr(void) #ifdef USE_SPI2 /// receive transferred over DMA +#ifdef STM32F1 void dma1_channel4_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF4) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF4; } +#elif defined STM32F4 +void dma1_stream3_isr(void) +{ + if ((DMA1_LISR & DMA_LISR_TCIF3) != 0) { + // clear int pending bit + DMA1_LIFCR |= DMA_LIFCR_CTCIF3; + } +#endif process_rx_dma_interrupt(&spi2); } /// transmit transferred over DMA +#ifdef STM32F1 void dma1_channel5_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF5) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF5; } +#elif defined STM32F4 +void dma1_stream4_isr(void) +{ + if ((DMA1_HISR & DMA_HISR_TCIF4) != 0) { + // clear int pending bit + DMA1_HIFCR |= DMA_HIFCR_CTCIF4; + } +#endif process_tx_dma_interrupt(&spi2); } @@ -884,22 +1024,40 @@ void dma1_channel5_isr(void) #if USE_SPI3 /// receive transferred over DMA +#ifdef STM32F1 void dma2_channel1_isr(void) { if ((DMA2_ISR & DMA_ISR_TCIF1) != 0) { // clear int pending bit DMA2_IFCR |= DMA_IFCR_CTCIF1; } +#elif defined STM32F4 +void dma1_stream0_isr(void) +{ + if ((DMA1_LISR & DMA_LISR_TCIF0) != 0) { + // clear int pending bit + DMA1_LIFCR |= DMA_LIFCR_CTCIF0; + } +#endif process_rx_dma_interrupt(&spi3); } /// transmit transferred over DMA +#ifdef STM32F1 void dma2_channel2_isr(void) { if ((DMA2_ISR & DMA_ISR_TCIF2) != 0) { // clear int pending bit DMA2_IFCR |= DMA_IFCR_CTCIF2; } +#elif defined STM32F4 +void dma1_stream5_isr(void) +{ + if ((DMA1_HISR & DMA_HISR_TCIF5) != 0) { + // clear int pending bit + DMA1_HIFCR |= DMA_HIFCR_CTCIF5; + } +#endif process_tx_dma_interrupt(&spi3); } @@ -917,7 +1075,11 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { spi_disable_rx_dma((uint32_t)periph->reg_addr); /* Disable DMA rx channel */ +#ifdef STM32F1 dma_disable_channel(dma->dma, dma->rx_chan); +#elif defined STM32F4 + dma_disable_stream(dma->dma, dma->rx_chan); +#endif if (dma->rx_extra_dummy_dma) { @@ -935,13 +1097,23 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr, (uint32_t)&(dma->rx_dummy_buf), len_remaining, trans->dss, FALSE); +#ifdef STM32F1 dma_set_read_from_peripheral(dma->dma, dma->rx_chan); dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_HIGH); +#elif defined STM32F4 + dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel); + dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM); + dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_HIGH); +#endif /* Enable DMA transfer complete interrupts. */ dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan); /* Enable DMA channels */ +#ifdef STM32F1 dma_enable_channel(dma->dma, dma->rx_chan); +#elif defined STM32F4 + dma_enable_stream(dma->dma, dma->rx_chan); +#endif /* Enable SPI transfers via DMA */ spi_enable_rx_dma((uint32_t)periph->reg_addr); } @@ -979,7 +1151,11 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { spi_disable_tx_dma((uint32_t)periph->reg_addr); /* Disable DMA tx channel */ +#ifdef STM32F1 dma_disable_channel(dma->dma, dma->tx_chan); +#elif defined STM32F4 + dma_disable_stream(dma->dma, dma->tx_chan); +#endif if (dma->tx_extra_dummy_dma) { /* @@ -996,13 +1172,23 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr, (uint32_t)&(dma->tx_dummy_buf), len_remaining, trans->dss, FALSE); +#ifdef STM32F1 dma_set_read_from_memory(dma->dma, dma->tx_chan); dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); +#elif defined STM32F4 + dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel); + dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL); + dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM); +#endif /* Enable DMA transfer complete interrupts. */ dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan); /* Enable DMA channels */ +#ifdef STM32F1 dma_enable_channel(dma->dma, dma->tx_chan); +#elif defined STM32F4 + dma_enable_stream(dma->dma, dma->tx_chan); +#endif /* Enable SPI transfers via DMA */ spi_enable_tx_dma((uint32_t)periph->reg_addr); 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 4a56067c5a..6a4ba6bf7e 100644 --- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c @@ -208,6 +208,9 @@ void actuators_pwm_arch_init(void) { #ifdef PWM_SERVO_8 set_servo_gpio(PWM_SERVO_8_GPIO, PWM_SERVO_8_PIN, PWM_SERVO_8_AF, PWM_SERVO_8_RCC_IOP); #endif +#ifdef PWM_SERVO_9 + set_servo_gpio(PWM_SERVO_9_GPIO, PWM_SERVO_9_PIN, PWM_SERVO_9_AF, PWM_SERVO_9_RCC_IOP); +#endif #if PWM_USE_TIM1 @@ -262,5 +265,8 @@ void actuators_pwm_commit(void) { #ifdef PWM_SERVO_8 timer_set_oc_value(PWM_SERVO_8_TIMER, PWM_SERVO_8_OC, actuators_pwm_values[PWM_SERVO_8]); #endif +#ifdef PWM_SERVO_9 + timer_set_oc_value(PWM_SERVO_9_TIMER, PWM_SERVO_9_OC, actuators_pwm_values[PWM_SERVO_9]); +#endif } diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c new file mode 100644 index 0000000000..64ef448548 --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c @@ -0,0 +1,37 @@ +#include "subsystems/imu.h" + +#include +#include +#include +#include + +#include "subsystems/imu/imu_krooz_sd_arch.h" + +void imu_krooz_sd_arch_init(void) { + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_SYSCFGEN); + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN); + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPCEN); + gpio_mode_setup(GPIOB, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO5); + gpio_mode_setup(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO6); + + nvic_enable_irq(NVIC_EXTI9_5_IRQ); + exti_select_source(EXTI5, GPIOB); + exti_select_source(EXTI6, GPIOC); + exti_set_trigger(EXTI5, EXTI_TRIGGER_RISING); + exti_set_trigger(EXTI6, EXTI_TRIGGER_FALLING); + exti_enable_request(EXTI5); + exti_enable_request(EXTI6); + nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0F); +} + +void exti9_5_isr(void) { + /* clear EXTI */ + if(EXTI_PR & EXTI6) { + exti_reset_request(EXTI6); + hmc58xx_read(&imu_krooz.hmc); + } + if(EXTI_PR & EXTI5) { + exti_reset_request(EXTI5); + mpu60x0_i2c_read(&imu_krooz.mpu); + } +} diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.h b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.h new file mode 100644 index 0000000000..3917845d68 --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.h @@ -0,0 +1,8 @@ +#ifndef IMU_KROOZ_SD_ARCH_H +#define IMU_KROOZ_SD_ARCH_H + +#include "subsystems/imu.h" + +void imu_krooz_sd_arch_init(void); + +#endif /* IMU_KROOZ_SD_ARCH_H */ diff --git a/sw/airborne/boards/apogee_0.99.h b/sw/airborne/boards/apogee_0.99.h index b73511266b..8a79bdc24b 100644 --- a/sw/airborne/boards/apogee_0.99.h +++ b/sw/airborne/boards/apogee_0.99.h @@ -60,6 +60,18 @@ #define UART6_GPIO_PORT_TX GPIOC #define UART6_GPIO_TX GPIO6 +/* SPI */ +#define SPI1_GPIO_AF GPIO_AF5 +#define SPI1_GPIO_PORT_MISO GPIOA +#define SPI1_GPIO_MISO GPIO6 +#define SPI1_GPIO_PORT_MOSI GPIOA +#define SPI1_GPIO_MOSI GPIO7 +#define SPI1_GPIO_PORT_SCK GPIOA +#define SPI1_GPIO_SCK GPIO5 + +#define SPI_SELECT_SLAVE0_PORT GPIOB +#define SPI_SELECT_SLAVE0_PIN GPIO9 + /* Onboard ADCs */ #define USE_AD_TIM4 1 diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h index 431030eb31..f2b04185b2 100644 --- a/sw/airborne/boards/ardrone/at_com.h +++ b/sw/airborne/boards/ardrone/at_com.h @@ -178,7 +178,7 @@ typedef struct _navdata_gps_t { uint8_t unk_2[16]; struct{ uint8_t sat; - uint8_t unk; + uint8_t cn0; }channels[12]; int32_t gps_plugged; /*!< When the gps is plugged */ uint8_t unk_3[108]; diff --git a/sw/airborne/boards/krooz/baro_board.c b/sw/airborne/boards/krooz/baro_board.c new file mode 100644 index 0000000000..7b531ec97b --- /dev/null +++ b/sw/airborne/boards/krooz/baro_board.c @@ -0,0 +1,32 @@ + +#include "subsystems/sensors/baro.h" +#include "baro_board.h" +/* +#include "subsystems/datalink/downlink.h" +#include "mcu_periph/uart.h" +#include "mcu_periph/sys_time.h" +*/ + +struct Baro baro; + +void baro_init(void) { + baro_ms5611_init(); +} + +void baro_periodic(void) { + static uint8_t cnt; + switch(cnt) { + case 0: + baro_ms5611_periodic(); + cnt++; + break; + case 1: + baro_ms5611_d1(); + cnt++; + break; + case 2: + baro_ms5611_d2(); + cnt = 0; + break; + } +} diff --git a/sw/airborne/boards/krooz/baro_board.h b/sw/airborne/boards/krooz/baro_board.h new file mode 100644 index 0000000000..b73f33d90d --- /dev/null +++ b/sw/airborne/boards/krooz/baro_board.h @@ -0,0 +1,30 @@ + +/* + * board specific fonctions for the KroozSD board + * + */ + +#ifndef BOARDS_KROOZ_BARO_H +#define BOARDS_KROOZ_BARO_H + +#include "std.h" +#include "mcu_periph/i2c.h" +#include "modules/sensors/baro_ms5611_i2c.h" +#include "math/pprz_algebra_int.h" + +//#include "led.h" + +static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)) +{ + baro_ms5611_event(); + if(baro_ms5611_valid) { + baro.status = BS_RUNNING; + baro.absolute = (int32_t)baroms; + b_abs_handler(); + baro_ms5611_valid = FALSE; + } +} + +#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler) + +#endif /* BOARDS_KROOZ_SD_BARO_H */ diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c new file mode 100644 index 0000000000..3aad6a7803 --- /dev/null +++ b/sw/airborne/boards/krooz/imu_krooz.c @@ -0,0 +1,160 @@ +/* + * Copyright (C) 2013 Sergey Krukowski + * + * 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. + */ + +/** + * @file boards/krooz/imu_krooz.c + * + * Driver for the IMU on the KroozSD board. + * + * Invensense MPU-6050 + * Honeywell HMC-5883 + */ + +#include +#include "boards/krooz/imu_krooz.h" +#include "subsystems/imu/imu_krooz_sd_arch.h" +#include "mcu_periph/i2c.h" +#include "led.h" + +// Downlink +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV +#define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ +#define KROOZ_SMPLRT_DIV 1 +#endif +PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV) +PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER) + +#ifndef KROOZ_GYRO_RANGE +#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250 +#endif +PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE) + +#ifndef KROOZ_ACCEL_RANGE +#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G +#endif +PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE) + +struct ImuKrooz imu_krooz; + + +#if KROOZ_USE_MEDIAN_FILTER +#include "filters/median_filter.h" +struct MedianFilter3Int median_gyro, median_accel, median_mag; +#endif + +void imu_impl_init( void ) +{ + ///////////////////////////////////////////////////////////////////// + // MPU-60X0 + mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR); + // change the default configuration + imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV; + imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER; + imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE; + imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE; + imu_krooz.mpu.config.drdy_int_enable = TRUE; + + hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR); + +#if KROOZ_USE_MEDIAN_FILTER + // Init median filters + InitMedianFilterRatesInt(median_gyro); + InitMedianFilterVect3Int(median_accel); + InitMedianFilterVect3Int(median_mag); +#endif + + RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); + VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0); + imu_krooz.meas_nb = 0; + + imu_krooz.gyr_valid = FALSE; + imu_krooz.acc_valid = FALSE; + imu_krooz.mag_valid = FALSE; + + imu_krooz_sd_arch_init(); +} + +void imu_periodic( void ) +{ + // Start reading the latest gyroscope data + if (!imu_krooz.mpu.config.initialized) + mpu60x0_i2c_start_configure(&imu_krooz.mpu); + + if (!imu_krooz.hmc.initialized) + hmc58xx_start_configure(&imu_krooz.hmc); + + if (imu_krooz.meas_nb) { + RATES_ASSIGN(imu.gyro_unscaled, imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb); +#if KROOZ_USE_MEDIAN_FILTER + UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled); +#endif + VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb); +#if KROOZ_USE_MEDIAN_FILTER + UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); +#endif + RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); + VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0); + imu_krooz.meas_nb = 0; + + imu_krooz.gyr_valid = TRUE; + imu_krooz.acc_valid = TRUE; + } + + //RunOnceEvery(10,imu_krooz_downlink_raw()); +} + +void imu_krooz_downlink_raw( void ) +{ + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); +} + + +void imu_krooz_event( void ) +{ + // If the MPU6050 I2C transaction has succeeded: convert the data + mpu60x0_i2c_event(&imu_krooz.mpu); + if (imu_krooz.mpu.data_available) { + RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates); + VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect); + imu_krooz.meas_nb++; + imu_krooz.mpu.data_available = FALSE; + } + + // If the HMC5883 I2C transaction has succeeded: convert the data + hmc58xx_event(&imu_krooz.hmc); + if (imu_krooz.hmc.data_available) { + VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect); +#if KROOZ_USE_MEDIAN_FILTER + UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled); +#endif + imu_krooz.hmc.data_available = FALSE; + imu_krooz.mag_valid = TRUE; + } +} diff --git a/sw/airborne/boards/krooz/imu_krooz.h b/sw/airborne/boards/krooz/imu_krooz.h new file mode 100644 index 0000000000..74afaf00f3 --- /dev/null +++ b/sw/airborne/boards/krooz/imu_krooz.h @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2013 Sergey Krukowski + * + * 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. + */ + +/** + * @file boards/krooz/imu_krooz.h + * + * Driver for the IMU on the KroozSD board. + * + * Invensense MPU-6050 + */ + +#ifndef IMU_KROOZ_H +#define IMU_KROOZ_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "peripherals/mpu60x0_i2c.h" +#include "peripherals/hmc58xx.h" + +// Default configuration +#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN +#define IMU_GYRO_P_SIGN -1 +#define IMU_GYRO_Q_SIGN 1 +#define IMU_GYRO_R_SIGN 1 +#endif +#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN +#define IMU_ACCEL_X_SIGN -1 +#define IMU_ACCEL_Y_SIGN 1 +#define IMU_ACCEL_Z_SIGN 1 +#endif +#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN +#define IMU_MAG_X_SIGN 1 +#define IMU_MAG_Y_SIGN 1 +#define IMU_MAG_Z_SIGN 1 +#endif + +/** default gyro sensitivy and neutral from the datasheet + * MPU with 250 deg/s has 131.072 LSB/(deg/s) + * sens = 1/131.072 * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/131.072 * pi/180 * 4096 = 0.5454 + I*/ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +// FIXME +#define IMU_GYRO_P_SENS 0.5454 +#define IMU_GYRO_P_SENS_NUM 2727 +#define IMU_GYRO_P_SENS_DEN 5000 +#define IMU_GYRO_Q_SENS 0.5454 +#define IMU_GYRO_Q_SENS_NUM 2727 +#define IMU_GYRO_Q_SENS_DEN 5000 +#define IMU_GYRO_R_SENS 0.5454 +#define IMU_GYRO_R_SENS_NUM 2727 +#define IMU_GYRO_R_SENS_DEN 5000 +#endif +#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL +#define IMU_GYRO_P_NEUTRAL 0 +#define IMU_GYRO_Q_NEUTRAL 0 +#define IMU_GYRO_R_NEUTRAL 0 +#endif + + +/** default accel sensitivy from the datasheet + * MPU with 2g has 16384 LSB/g + * sens = 9.81 [m/s^2] / 16384 [LSB/g] * 2^INT32_ACCEL_FRAC = 0.6131 + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +// FIXME +#define IMU_ACCEL_X_SENS 0.6131 +#define IMU_ACCEL_X_SENS_NUM 6131 +#define IMU_ACCEL_X_SENS_DEN 10000 +#define IMU_ACCEL_Y_SENS 0.6131 +#define IMU_ACCEL_Y_SENS_NUM 6131 +#define IMU_ACCEL_Y_SENS_DEN 10000 +#define IMU_ACCEL_Z_SENS 0.6131 +#define IMU_ACCEL_Z_SENS_NUM 6131 +#define IMU_ACCEL_Z_SENS_DEN 10000 +#endif +#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL +#define IMU_ACCEL_X_NEUTRAL 0 +#define IMU_ACCEL_Y_NEUTRAL 0 +#define IMU_ACCEL_Z_NEUTRAL 0 +#endif + +struct ImuKrooz { + volatile bool_t gyr_valid; + volatile bool_t acc_valid; + volatile bool_t mag_valid; + struct Mpu60x0_I2c mpu; + struct Hmc58xx hmc; + struct Int32Rates rates_sum; + struct Int32Vect3 accel_sum; + volatile uint8_t meas_nb; +}; + +extern struct ImuKrooz imu_krooz; + + +/* must be defined in order to be IMU code: declared in imu.h +extern void imu_impl_init(void); +extern void imu_periodic(void); +*/ + +/* Own Extra Functions */ +extern void imu_krooz_event( void ); +extern void imu_krooz_downlink_raw( void ); + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) { + imu_krooz_event(); + if (imu_krooz.gyr_valid) { + imu_krooz.gyr_valid = FALSE; + _gyro_handler(); + } + if (imu_krooz.acc_valid) { + imu_krooz.acc_valid = FALSE; + _accel_handler(); + } + if (imu_krooz.mag_valid) { + imu_krooz.mag_valid = FALSE; + _mag_handler(); + } +} + +#endif // IMU_KROOZ_H diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h index d4fb6deab1..dc24cbda4a 100644 --- a/sw/airborne/boards/krooz_sd.h +++ b/sw/airborne/boards/krooz_sd.h @@ -1,5 +1,5 @@ -#ifndef CONFIG_KROOZ_1_0_H -#define CONFIG_KROOZ_1_0_H +#ifndef CONFIG_KROOZ_SD_H +#define CONFIG_KROOZ_SD_H #define BOARD_KROOZ @@ -99,6 +99,29 @@ #define UART5_GPIO_PORT_TX GPIOC #define UART5_GPIO_TX GPIO12 +/* SPI */ +#define SPI1_GPIO_AF GPIO_AF5 +#define SPI1_GPIO_PORT_MISO GPIOA +#define SPI1_GPIO_MISO GPIO6 +#define SPI1_GPIO_PORT_MOSI GPIOA +#define SPI1_GPIO_MOSI GPIO7 +#define SPI1_GPIO_PORT_SCK GPIOA +#define SPI1_GPIO_SCK GPIO5 + +#define SPI2_GPIO_AF GPIO_AF5 +#define SPI2_GPIO_PORT_MISO GPIOB +#define SPI2_GPIO_MISO GPIO14 +#define SPI2_GPIO_PORT_MOSI GPIOB +#define SPI2_GPIO_MOSI GPIO15 +#define SPI2_GPIO_PORT_SCK GPIOB +#define SPI2_GPIO_SCK GPIO13 + +#define SPI_SELECT_SLAVE0_PORT GPIOA +#define SPI_SELECT_SLAVE0_PIN GPIO4 +#define SPI_SELECT_SLAVE1_PORT GPIOB +#define SPI_SELECT_SLAVE1_PIN GPIO12 +#define SPI_SELECT_SLAVE2_PORT GPIOB +#define SPI_SELECT_SLAVE2_PIN GPIO2 /* I2C mapping */ #define I2C1_GPIO_PORT GPIOB @@ -109,6 +132,10 @@ #define I2C2_GPIO_SCL GPIO10 #define I2C2_GPIO_SDA GPIO11 +#define I2C3_GPIO_PORT_SCL GPIOA +#define I2C3_GPIO_PORT_SDA GPIOC +#define I2C3_GPIO_SCL GPIO8 +#define I2C3_GPIO_SDA GPIO9 /* Onboard ADCs */ #define USE_AD_TIM1 1 @@ -197,20 +224,10 @@ } #endif // USE_AD1 - -/* I2C mapping */ -#define GPIO_I2C1_SCL GPIO8 -#define GPIO_I2C1_SDA GPIO9 -#define GPIO_I2C2_SCL GPIO10 -#define GPIO_I2C2_SDA GPIO11 -#define GPIO_I2C3_SCL GPIO8 //PA8 -#define GPIO_I2C3_SDA GPIO9 //PC9 - /* Activate onboard baro */ #define BOARD_HAS_BARO 1 /* PWM */ -//#define PWM_USE_TIM2 1 #define PWM_USE_TIM3 1 #define PWM_USE_TIM4 1 #define PWM_USE_TIM5 1 @@ -227,18 +244,23 @@ #define USE_PWM9 1 //#define USE_PWM10 1 +#if USE_PWM10 +#define ACTUATORS_PWM_NB 11 +#define PWM_USE_TIM2 1 +#else #define ACTUATORS_PWM_NB 10 +#endif // PWM_SERVO_x is the index of the servo in the actuators_pwm_values array #if USE_PWM0 #define PWM_SERVO_0 0 #define PWM_SERVO_0_TIMER TIM3 -#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPCEN -#define PWM_SERVO_0_GPIO GPIOC -#define PWM_SERVO_0_PIN GPIO6 -#define PWM_SERVO_0_AF GPIO_AF1 -#define PWM_SERVO_0_OC TIM_OC1 -#define PWM_SERVO_0_OC_BIT (1<<0) +#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_0_GPIO GPIOB +#define PWM_SERVO_0_PIN GPIO1 +#define PWM_SERVO_0_AF GPIO_AF2 +#define PWM_SERVO_0_OC TIM_OC4 +#define PWM_SERVO_0_OC_BIT (1<<3) #else #define PWM_SERVO_0_OC_BIT 0 #endif @@ -248,10 +270,10 @@ #define PWM_SERVO_1_TIMER TIM3 #define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPCEN #define PWM_SERVO_1_GPIO GPIOC -#define PWM_SERVO_1_PIN GPIO7 -#define PWM_SERVO_1_AF GPIO_AF1 -#define PWM_SERVO_1_OC TIM_OC2 -#define PWM_SERVO_1_OC_BIT (1<<1) +#define PWM_SERVO_1_PIN GPIO8 +#define PWM_SERVO_1_AF GPIO_AF2 +#define PWM_SERVO_1_OC TIM_OC3 +#define PWM_SERVO_1_OC_BIT (1<<2) #else #define PWM_SERVO_1_OC_BIT 0 #endif @@ -261,23 +283,23 @@ #define PWM_SERVO_2_TIMER TIM3 #define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPCEN #define PWM_SERVO_2_GPIO GPIOC -#define PWM_SERVO_2_PIN GPIO8 -#define PWM_SERVO_2_AF GPIO_AF1 -#define PWM_SERVO_2_OC TIM_OC3 -#define PWM_SERVO_2_OC_BIT (1<<2) +#define PWM_SERVO_2_PIN GPIO7 +#define PWM_SERVO_2_AF GPIO_AF2 +#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_IDX 3 +#define PWM_SERVO_3 3 #define PWM_SERVO_3_TIMER TIM3 -#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPCEN -#define PWM_SERVO_3_GPIO GPIOC -#define PWM_SERVO_3_PIN GPIO9 -#define PWM_SERVO_3_AF GPIO_AF1 -#define PWM_SERVO_3_OC TIM_OC4 -#define PWM_SERVO_3_OC_BIT (1<<3) +#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO4 +#define PWM_SERVO_3_AF GPIO_AF2 +#define PWM_SERVO_3_OC TIM_OC1 +#define PWM_SERVO_3_OC_BIT (1<<0) #else #define PWM_SERVO_3_OC_BIT 0 #endif @@ -287,10 +309,10 @@ #define PWM_SERVO_4_TIMER TIM4 #define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPBEN #define PWM_SERVO_4_GPIO GPIOB -#define PWM_SERVO_4_PIN GPIO6 -#define PWM_SERVO_4_AF GPIO_AF1 -#define PWM_SERVO_4_OC TIM_OC1 -#define PWM_SERVO_4_OC_BIT (1<<0) +#define PWM_SERVO_4_PIN GPIO7 +#define PWM_SERVO_4_AF GPIO_AF2 +#define PWM_SERVO_4_OC TIM_OC2 +#define PWM_SERVO_4_OC_BIT (1<<1) #else #define PWM_SERVO_4_OC_BIT 0 #endif @@ -300,10 +322,10 @@ #define PWM_SERVO_5_TIMER TIM4 #define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPBEN #define PWM_SERVO_5_GPIO GPIOB -#define PWM_SERVO_5_PIN GPIO7 -#define PWM_SERVO_5_AF GPIO_AF1 -#define PWM_SERVO_5_OC TIM_OC2 -#define PWM_SERVO_5_OC_BIT (1<<1) +#define PWM_SERVO_5_PIN GPIO6 +#define PWM_SERVO_5_AF GPIO_AF2 +#define PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) #else #define PWM_SERVO_5_OC_BIT 0 #endif @@ -313,10 +335,10 @@ #define PWM_SERVO_6_TIMER TIM5 #define PWM_SERVO_6_RCC_IOP RCC_AHB1ENR_IOPAEN #define PWM_SERVO_6_GPIO GPIOA -#define PWM_SERVO_6_PIN GPIO0 -#define PWM_SERVO_6_AF GPIO_AF1 -#define PWM_SERVO_6_OC TIM_OC1 -#define PWM_SERVO_6_OC_BIT (1<<0) +#define PWM_SERVO_6_PIN GPIO3 +#define PWM_SERVO_6_AF GPIO_AF2 +#define PWM_SERVO_6_OC TIM_OC4 +#define PWM_SERVO_6_OC_BIT (1<<3) #else #define PWM_SERVO_6_OC_BIT 0 #endif @@ -326,10 +348,10 @@ #define PWM_SERVO_7_TIMER TIM5 #define PWM_SERVO_7_RCC_IOP RCC_AHB1ENR_IOPAEN #define PWM_SERVO_7_GPIO GPIOA -#define PWM_SERVO_7_PIN GPIO1 -#define PWM_SERVO_7_AF GPIO_AF1 -#define PWM_SERVO_7_OC TIM_OC2 -#define PWM_SERVO_7_OC_BIT (1<<1) +#define PWM_SERVO_7_PIN GPIO2 +#define PWM_SERVO_7_AF GPIO_AF2 +#define PWM_SERVO_7_OC TIM_OC3 +#define PWM_SERVO_7_OC_BIT (1<<2) #else #define PWM_SERVO_7_OC_BIT 0 #endif @@ -339,10 +361,10 @@ #define PWM_SERVO_8_TIMER TIM5 #define PWM_SERVO_8_RCC_IOP RCC_AHB1ENR_IOPAEN #define PWM_SERVO_8_GPIO GPIOA -#define PWM_SERVO_8_PIN GPIO2 -#define PWM_SERVO_8_AF GPIO_AF1 -#define PWM_SERVO_8_OC TIM_OC3 -#define PWM_SERVO_8_OC_BIT (1<<2) +#define PWM_SERVO_8_PIN GPIO1 +#define PWM_SERVO_8_AF GPIO_AF2 +#define PWM_SERVO_8_OC TIM_OC2 +#define PWM_SERVO_8_OC_BIT (1<<1) #else #define PWM_SERVO_8_OC_BIT 0 #endif @@ -352,10 +374,10 @@ #define PWM_SERVO_9_TIMER TIM5 #define PWM_SERVO_9_RCC_IOP RCC_AHB1ENR_IOPAEN #define PWM_SERVO_9_GPIO GPIOA -#define PWM_SERVO_9_PIN GPIO3 -#define PWM_SERVO_9_AF GPIO_AF1 -#define PWM_SERVO_9_OC TIM_OC4 -#define PWM_SERVO_9_OC_BIT (1<<3) +#define PWM_SERVO_9_PIN GPIO0 +#define PWM_SERVO_9_AF GPIO_AF2 +#define PWM_SERVO_9_OC TIM_OC1 +#define PWM_SERVO_9_OC_BIT (1<<0) #else #define PWM_SERVO_9_OC_BIT 0 #endif @@ -397,7 +419,7 @@ * Spektrum */ /* The line that is pulled low at power up to initiate the bind process */ -#define SPEKTRUM_BIND_PIN GPIO8 +#define SPEKTRUM_BIND_PIN GPIO9 #define SPEKTRUM_BIND_PIN_PORT GPIOA -#endif /* CONFIG_KROOZ_1_0_H */ +#endif /* CONFIG_KROOZ_SD_H */ diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 2b816e0625..ba54e696a4 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -65,6 +65,7 @@ static inline int ahrs_is_aligned(void) { return (ahrs.status == AHRS_RUNNING); } #else +PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL") static inline int ahrs_is_aligned(void) { return TRUE; } @@ -80,7 +81,7 @@ static inline int ahrs_is_aligned(void) { #ifndef MODE_STARTUP #define MODE_STARTUP AP_MODE_KILL -PRINT_CONFIG_MSG("Using AP_MODE_KILL as MODE_STARTUP") +PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP") #endif static void send_alive(void) { @@ -173,7 +174,8 @@ static void send_baro_raw(void) { } void autopilot_init(void) { - autopilot_mode = MODE_STARTUP; + /* mode is finally set at end of init if MODE_STARTUP is not KILL */ + autopilot_mode = AP_MODE_KILL; autopilot_motors_on = FALSE; kill_throttle = ! autopilot_motors_on; autopilot_in_flight = FALSE; @@ -187,8 +189,17 @@ void autopilot_init(void) { #ifdef POWER_SWITCH_LED LED_ON(POWER_SWITCH_LED); // POWER OFF #endif + autopilot_arming_init(); + nav_init(); + guidance_h_init(); + guidance_v_init(); + stabilization_init(); + + /* set startup mode, propagats through to guidance h/v */ + autopilot_set_mode(MODE_STARTUP); + register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp); diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index c0fe0297f8..a1ed2a3c64 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -140,11 +140,6 @@ STATIC_INLINE void main_init( void ) { baro_init(); imu_init(); - autopilot_init(); - nav_init(); - guidance_h_init(); - guidance_v_init(); - stabilization_init(); ahrs_aligner_init(); ahrs_init(); @@ -155,6 +150,8 @@ STATIC_INLINE void main_init( void ) { gps_init(); #endif + autopilot_init(); + modules_init(); settings_init(); @@ -227,6 +224,7 @@ STATIC_INLINE void failsafe_check( void ) { #if USE_GPS if (autopilot_mode == AP_MODE_NAV && + autopilot_motors_on && #if NO_GPS_LOST_WITH_RC_VALID radio_control.status != RC_OK && #endif diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.c b/sw/airborne/modules/digital_cam/led_cam_ctrl.c index 8928381f48..81b701f6af 100644 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.c @@ -52,6 +52,12 @@ void dc_send_command(uint8_t cmd) case DC_ON: DC_PUSH(DC_POWER_LED); break; +#endif +#ifdef DC_POWER_OFF_LED + case DC_OFF: + DC_PUSH(DC_POWER_OFF_LED); + dc_timer = DC_POWER_OFF_DELAY; + break; #endif default: break; diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.h b/sw/airborne/modules/digital_cam/led_cam_ctrl.h index 37eb0a7f17..fde5cb4aa7 100644 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.h @@ -35,6 +35,7 @@ * * * + * * @endverbatim * Related bank and pin must also be defined: * @verbatim @@ -68,6 +69,10 @@ extern uint8_t dc_timer; #define DC_SHUTTER_DELAY 2 /* 4Hz -> 0.5s */ #endif +#ifndef DC_POWER_OFF_DELAY +#define DC_POWER_OFF_DELAY 3 +#endif + #ifndef DC_SHUTTER_LED #error DC: Please specify at least a SHUTTER LED #endif @@ -90,6 +95,9 @@ static inline void led_cam_ctrl_init(void) #ifdef DC_POWER_LED DC_RELEASE(DC_POWER_LED); #endif +#ifdef DC_POWER_OFF_LED + DC_RELEASE(DC_POWER_OFF_LED); +#endif } @@ -114,6 +122,9 @@ static inline void led_cam_ctrl_periodic( void ) #endif #ifdef DC_POWER_LED DC_RELEASE(DC_POWER_LED); +#endif +#ifdef DC_POWER_OFF_LED + DC_RELEASE(DC_POWER_OFF_LED); #endif } diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index 706904d3bb..15eff2ce56 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -42,13 +42,14 @@ #include "state.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" +#include "mcu_periph/sys_time.h" #include "messages.h" #include "subsystems/datalink/downlink.h" #include #if !USE_AIRSPEED -#ifndef SENSOR_SYNC_SEND -#warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use ets_airspeed +#ifndef AIRSPEED_ETS_SYNC_SEND +#warning either set USE_AIRSPEED or AIRSPEED_ETS_SYNC_SEND to use ets_airspeed #endif #endif @@ -68,6 +69,13 @@ #ifndef AIRSPEED_ETS_I2C_DEV #define AIRSPEED_ETS_I2C_DEV i2c0 #endif +PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV) + +/** delay in seconds until sensor is read after startup */ +#ifndef AIRSPEED_ETS_START_DELAY +#define AIRSPEED_ETS_START_DELAY 0.2 +#endif +PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY) // Global variables @@ -85,6 +93,8 @@ volatile bool_t airspeed_ets_i2c_done; bool_t airspeed_ets_offset_init; uint32_t airspeed_ets_offset_tmp; uint16_t airspeed_ets_cnt; +uint32_t airspeed_ets_delay_time; +bool_t airspeed_ets_delay_done; void airspeed_ets_init( void ) { int n; @@ -93,7 +103,7 @@ void airspeed_ets_init( void ) { airspeed_ets_offset = 0; airspeed_ets_offset_tmp = 0; airspeed_ets_i2c_done = TRUE; - airspeed_ets_valid = TRUE; + airspeed_ets_valid = FALSE; airspeed_ets_offset_init = FALSE; airspeed_ets_cnt = AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT + AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG; @@ -102,10 +112,17 @@ void airspeed_ets_init( void ) { airspeed_ets_buffer[n] = 0.0; airspeed_ets_i2c_trans.status = I2CTransDone; + + airspeed_ets_delay_done = FALSE; + SysTimeTimerStart(airspeed_ets_delay_time); } void airspeed_ets_read_periodic( void ) { #ifndef SITL + if (!airspeed_ets_delay_done) { + if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) return; + else airspeed_ets_delay_done = TRUE; + } if (airspeed_ets_i2c_trans.status == I2CTransDone) i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2); #else // SITL @@ -170,7 +187,7 @@ void airspeed_ets_read_event( void ) { #if USE_AIRSPEED stateSetAirspeed_f(&airspeed_ets); #endif -#ifdef SENSOR_SYNC_SEND +#ifdef AIRSPEED_ETS_SYNC_SEND DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets); #endif } else { diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index a3e23c62c7..8160c89672 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -45,12 +45,21 @@ #define BARO_AMSYS_ADDR 0xE4 #define BARO_AMSYS_REG 0x07 -#define BARO_AMSYS_SCALE 0.32 +#ifndef BARO_AMSYS_SCALE +#define BARO_AMSYS_SCALE 1 +#endif +#ifndef BARO_AMSYS_MAX_PRESSURE #define BARO_AMSYS_MAX_PRESSURE 103400 // Pascal +#endif #define BARO_AMSYS_OFFSET_MAX 29491 #define BARO_AMSYS_OFFSET_MIN 3277 #define BARO_AMSYS_OFFSET_NBSAMPLES_INIT 40 #define BARO_AMSYS_OFFSET_NBSAMPLES_AVRG 60 +#ifndef BARO_AMSYS_FILTER +#define BARO_AMSYS_FILTER 0 +#endif +#define BARO_AMSYS_R 0.5 +#define BARO_AMSYS_SIGMA2 0.1 #ifdef MEASURE_AMSYS_TEMPERATURE #define TEMPERATURE_AMSYS_OFFSET_MAX 29491 @@ -59,8 +68,6 @@ #define TEMPERATURE_AMSYS_MIN -25 #endif -//#define BARO_AMSYS_R 0.5 -//#define BARO_AMSYS_SIGMA2 0.1 //#define BARO_ALT_CORRECTION 0.0 #ifndef BARO_AMSYS_I2C_DEV #define BARO_AMSYS_I2C_DEV i2c0 @@ -73,17 +80,18 @@ uint16_t baro_amsys_adc; float baro_amsys_offset; bool_t baro_amsys_valid; float baro_amsys_altitude; +bool_t baro_amsys_enabled; +float baro_amsys_r; +float baro_amsys_sigma2; float baro_amsys_temp; float baro_amsys_p; float baro_amsys_offset_altitude; float baro_amsys_abs_altitude; float ref_alt_init; //Altitude by initialising +float baro_scale; float baro_filter; float baro_old; -//float baro_amsys_r; -//float baro_amsys_sigma2; - struct i2c_transaction baro_amsys_i2c_trans; @@ -93,106 +101,112 @@ double baro_amsys_offset_tmp; uint16_t baro_amsys_cnt; void baro_amsys_init( void ) { - baro_filter=BARO_FILTER; - pBaroRaw = 0; - tBaroRaw = 0; - baro_amsys_altitude = 0.0; - baro_amsys_p=0.0; - baro_amsys_offset = 0; - baro_amsys_offset_tmp = 0; - baro_amsys_valid = TRUE; - baro_amsys_offset_init = FALSE; -// baro_amsys_enabled = TRUE; - baro_amsys_cnt = BARO_AMSYS_OFFSET_NBSAMPLES_INIT + BARO_AMSYS_OFFSET_NBSAMPLES_AVRG; -// baro_amsys_r = BARO_AMSYS_R; -// baro_amsys_sigma2 = BARO_AMSYS_SIGMA2; -// baro_head=0; - ref_alt_init = 0; - baro_amsys_i2c_trans.status = I2CTransDone; + baro_filter=BARO_AMSYS_FILTER; + pBaroRaw = 0; + tBaroRaw = 0; + baro_amsys_altitude = 0.0; + baro_amsys_p=0.0; + baro_amsys_offset = 0; + baro_amsys_offset_tmp = 0; + baro_amsys_valid = TRUE; + baro_amsys_offset_init = FALSE; + baro_amsys_enabled = TRUE; + baro_scale = BARO_AMSYS_SCALE; + baro_amsys_cnt = BARO_AMSYS_OFFSET_NBSAMPLES_INIT + BARO_AMSYS_OFFSET_NBSAMPLES_AVRG; + baro_amsys_r = BARO_AMSYS_R; + baro_amsys_sigma2 = BARO_AMSYS_SIGMA2; + // baro_head=0; + ref_alt_init = 0; + baro_amsys_i2c_trans.status = I2CTransDone; } void baro_amsys_read_periodic( void ) { // Initiate next read #ifndef SITL - if (baro_amsys_i2c_trans.status == I2CTransDone){ + if (baro_amsys_i2c_trans.status == I2CTransDone){ #ifndef MEASURE_AMSYS_TEMPERATURE - i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2); + i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2); #else - i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4); + i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4); #endif - } + } #else // SITL - pBaroRaw = 0; - baro_amsys_altitude = gps.hmsl / 1000.0; - baro_amsys_adc = baro_amsys_altitude / BARO_AMSYS_SCALE; - baro_amsys_valid = TRUE; + /* fake an offset so sim works for under hmsl as well */ + if (!baro_amsys_offset_init) { + baro_amsys_offset = BARO_AMSYS_OFFSET_MAX; + baro_amsys_offset_init = TRUE; + } + pBaroRaw = 0; + baro_amsys_altitude = gps.hmsl / 1000.0; + baro_amsys_adc = baro_amsys_offset - ((baro_amsys_altitude - ground_alt) / INS_BARO_SENS); + baro_amsys_valid = TRUE; +#endif + +#ifdef BARO_AMSYS_SYNC_SEND + DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp); +#else + RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)); #endif } void baro_amsys_read_event( void ) { - pBaroRaw = 0; - // Get raw altimeter from buffer - pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1]; + pBaroRaw = 0; + // Get raw altimeter from buffer + pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1]; #ifdef MEASURE_AMSYS_TEMPERATURE - tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3]; - baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN; + tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3]; + baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN; #endif - // Check if this is valid altimeter - if (pBaroRaw == 0) - baro_amsys_valid = FALSE; - else - baro_amsys_valid = TRUE; + // Check if this is valid altimeter + if (pBaroRaw == 0) + baro_amsys_valid = FALSE; + else + baro_amsys_valid = TRUE; baro_amsys_adc = pBaroRaw; - // Continue only if a new altimeter value was received - //if (baro_amsys_valid && GpsFixValid()) { - if (baro_amsys_valid) { - //Cut RAW Min and Max - if (pBaroRaw < BARO_AMSYS_OFFSET_MIN) - pBaroRaw = BARO_AMSYS_OFFSET_MIN; - if (pBaroRaw > BARO_AMSYS_OFFSET_MAX) - pBaroRaw = BARO_AMSYS_OFFSET_MAX; + // Continue only if a new altimeter value was received + //if (baro_amsys_valid && GpsFixValid()) { + if (baro_amsys_valid) { + //Cut RAW Min and Max + if (pBaroRaw < BARO_AMSYS_OFFSET_MIN) + pBaroRaw = BARO_AMSYS_OFFSET_MIN; + if (pBaroRaw > BARO_AMSYS_OFFSET_MAX) + pBaroRaw = BARO_AMSYS_OFFSET_MAX; - //Convert to pressure - baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN); - if (!baro_amsys_offset_init) { - --baro_amsys_cnt; - // Check if averaging completed - if (baro_amsys_cnt == 0) { - // Calculate average - baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG); - ref_alt_init = GROUND_ALT; - baro_amsys_offset_init = TRUE; + //Convert to pressure + baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN); + if (!baro_amsys_offset_init) { + --baro_amsys_cnt; + // Check if averaging completed + if (baro_amsys_cnt == 0) { + // Calculate average + baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG); + ref_alt_init = GROUND_ALT; + baro_amsys_offset_init = TRUE; - // hight over Sea level at init point - //baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255)); - } - // Check if averaging needs to continue - else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG) - baro_amsys_offset_tmp += baro_amsys_p; + // hight over Sea level at init point + //baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255)); + } + // Check if averaging needs to continue + else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG) + baro_amsys_offset_tmp += baro_amsys_p; - baro_amsys_altitude = 0.0; + baro_amsys_altitude = 0.0; - } - else { - // Lowpassfiltering and convert pressure to altitude - baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)/(1.2041*9.81); - baro_old = baro_amsys_altitude; - //New value available - } - baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init; - } /*else { - baro_amsys_abs_altitude = 0.0; - }*/ + } + else { + // Lowpassfiltering and convert pressure to altitude + baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)*baro_scale/(1.2041*9.81); + baro_old = baro_amsys_altitude; + //New value available + } + baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init; + } /*else { + baro_amsys_abs_altitude = 0.0; + }*/ - // Transaction has been read - baro_amsys_i2c_trans.status = I2CTransDone; -#ifdef SENSOR_SYNC_SEND - DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp) -#else - RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)); -#endif - + // Transaction has been read + baro_amsys_i2c_trans.status = I2CTransDone; } diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h index 98f028582f..94429a402e 100644 --- a/sw/airborne/modules/sensors/baro_amsys.h +++ b/sw/airborne/modules/sensors/baro_amsys.h @@ -33,10 +33,10 @@ extern uint16_t baro_amsys_adc; // extern float baro_amsys_offset; extern bool_t baro_amsys_valid; -// extern bool_t baro_amsys_enabled; +extern bool_t baro_amsys_enabled; extern float baro_amsys_altitude; -// extern float baro_amsys_r; -// extern float baro_amsys_sigma2; +extern float baro_amsys_r; +extern float baro_amsys_sigma2; extern float baro_filter; extern struct i2c_transaction baro_amsys_i2c_trans; diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index c3b89644d0..c4301495e5 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -43,6 +43,7 @@ #include "mcu_periph/i2c.h" #include "state.h" #include +#include "mcu_periph/sys_time.h" #include "subsystems/nav.h" @@ -50,12 +51,12 @@ #include "subsystems/gps.h" #endif -#ifdef BARO_ETS_TELEMETRY +#ifdef BARO_ETS_SYNC_SEND #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" -#endif //BARO_ETS_TELEMETRY +#endif //BARO_ETS_SYNC_SEND #define BARO_ETS_ADDR 0xE8 #define BARO_ETS_REG 0x07 @@ -72,6 +73,13 @@ #ifndef BARO_ETS_I2C_DEV #define BARO_ETS_I2C_DEV i2c0 #endif +PRINT_CONFIG_VAR(BARO_ETS_I2C_DEV) + +/** delay in seconds until sensor is read after startup */ +#ifndef BARO_ETS_START_DELAY +#define BARO_ETS_START_DELAY 0.2 +#endif +PRINT_CONFIG_VAR(BARO_ETS_START_DELAY) // Global variables uint16_t baro_ets_adc; @@ -85,16 +93,18 @@ float baro_ets_sigma2; struct i2c_transaction baro_ets_i2c_trans; // Local variables -bool_t baro_ets_offset_init; +bool_t baro_ets_offset_init; uint32_t baro_ets_offset_tmp; uint16_t baro_ets_cnt; +uint32_t baro_ets_delay_time; +bool_t baro_ets_delay_done; void baro_ets_init( void ) { baro_ets_adc = 0; baro_ets_altitude = 0.0; baro_ets_offset = 0; baro_ets_offset_tmp = 0; - baro_ets_valid = TRUE; + baro_ets_valid = FALSE; baro_ets_offset_init = FALSE; baro_ets_enabled = TRUE; baro_ets_cnt = BARO_ETS_OFFSET_NBSAMPLES_INIT + BARO_ETS_OFFSET_NBSAMPLES_AVRG; @@ -102,15 +112,22 @@ void baro_ets_init( void ) { baro_ets_sigma2 = BARO_ETS_SIGMA2; baro_ets_i2c_trans.status = I2CTransDone; + + baro_ets_delay_done = FALSE; + SysTimeTimerStart(baro_ets_delay_time); } void baro_ets_read_periodic( void ) { // Initiate next read #ifndef SITL + if (!baro_ets_delay_done) { + if (SysTimeTimer(baro_ets_delay_time) < USEC_OF_SEC(BARO_ETS_START_DELAY)) return; + else baro_ets_delay_done = TRUE; + } if (baro_ets_i2c_trans.status == I2CTransDone) i2c_receive(&BARO_ETS_I2C_DEV, &baro_ets_i2c_trans, BARO_ETS_ADDR, 2); #else // SITL - /* fake an offset so sim works for under hmsl as well */ + /* fake an offset so sim works as well */ if (!baro_ets_offset_init) { baro_ets_offset = 12400; baro_ets_offset_init = TRUE; @@ -120,7 +137,7 @@ void baro_ets_read_periodic( void ) { baro_ets_valid = TRUE; #endif -#ifdef BARO_ETS_TELEMETRY +#ifdef BARO_ETS_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); #endif } @@ -158,7 +175,7 @@ void baro_ets_read_event( void ) { if (baro_ets_offset_init) { baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); // New value available -#ifdef BARO_ETS_TELEMETRY +#ifdef BARO_ETS_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); #endif } else { diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index dbe127b4f8..91d2deecbc 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -52,6 +52,7 @@ uint8_t ms5611_status; uint16_t ms5611_c[PROM_NB]; uint32_t ms5611_d1, ms5611_d2; int32_t prom_cnt; +int64_t baroms; float fbaroms, ftempms; float baro_ms5611_alt; bool_t baro_ms5611_enabled; @@ -190,7 +191,7 @@ void baro_ms5611_event( void ) { break; case MS5611_ADC_D2: { - int64_t dt, baroms, tempms, off, sens, t2, off2, sens2; + int64_t dt, tempms, off, sens, t2, off2, sens2; /* read D2 (temperature) */ ms5611_d2 = (ms5611_trans.buf[0] << 16) | (ms5611_trans.buf[1] << 8) | diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h index aeef952cd6..cad37d60db 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.h +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -20,6 +20,7 @@ extern bool_t baro_ms5611_valid; extern bool_t baro_ms5611_enabled; extern float baro_ms5611_r; extern float baro_ms5611_sigma2; +extern int64_t baroms; enum ms5611_stat{ MS5611_UNINIT, @@ -35,11 +36,11 @@ enum ms5611_stat{ MS5611_ADC_D2 }; -void baro_ms5611_init(void); -void baro_ms5611_periodic(void); -void baro_ms5611_d1(void); -void baro_ms5611_d2(void); -void baro_ms5611_event(void); +extern void baro_ms5611_init(void); +extern void baro_ms5611_periodic(void); +extern void baro_ms5611_d1(void); +extern void baro_ms5611_d2(void); +extern void baro_ms5611_event(void); #define BaroMs5611Update(_b) { if (baro_ms5611_valid) { _b = baro_ms5611_alt; baro_ms5611_valid = FALSE; } } diff --git a/sw/airborne/subsystems/actuators/motor_mixing.c b/sw/airborne/subsystems/actuators/motor_mixing.c index 313ac9df90..e79daa50ed 100644 --- a/sw/airborne/subsystems/actuators/motor_mixing.c +++ b/sw/airborne/subsystems/actuators/motor_mixing.c @@ -172,13 +172,17 @@ void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) { if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR) motor_mixing.nb_failure++; - if (min_cmd < MOTOR_MIXING_MIN_MOTOR) { - int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd; + /* In case of both min and max saturation, only lower the throttle + * instead of applying both. This should prevent your quad shooting up, + * but it might loose altitude in case of such a saturation failure. + */ + if (max_cmd > MOTOR_MIXING_MAX_MOTOR) { + int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd; BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET); offset_commands(saturation_offset); } - if (max_cmd > MOTOR_MIXING_MAX_MOTOR) { - int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd; + else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) { + int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd; BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET); offset_commands(saturation_offset); } diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c index 60a1e5db78..0326f0dd64 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.c +++ b/sw/airborne/subsystems/gps/gps_ardrone2.c @@ -36,6 +36,7 @@ void gps_impl_init( void ) { } void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { + int i; // Set the lla double struct from the navdata struct LlaCoor_d gps_lla_d; gps_lla_d.lat = RadOfDeg(navdata_gps->lat); @@ -51,6 +52,12 @@ void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d); // TODO: parse other stuff + gps.nb_channels = GPS_NB_CHANNELS; + + for(i = 0; i < GPS_NB_CHANNELS; i++) { + gps.svinfos[i].svid = navdata_gps->channels[i].sat; + gps.svinfos[i].cno = navdata_gps->channels[i].cn0; + } // Check if we have a fix TODO: check if 2D or 3D fix? if (navdata_gps->gps_state == 1) diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h index 42e926ff34..2995d0f853 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.h +++ b/sw/airborne/subsystems/gps/gps_ardrone2.h @@ -30,7 +30,7 @@ #include "boards/ardrone/at_com.h" -//#define GPS_NB_CHANNELS 12 // TODO: Get channels out of packet +#define GPS_NB_CHANNELS 12 extern bool_t gps_ardrone2_available; /* diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c deleted file mode 100644 index b467e1bd35..0000000000 --- a/sw/airborne/subsystems/imu/imu_aspirin2.c +++ /dev/null @@ -1,346 +0,0 @@ -/* - * Copyright (C) 2012 Christophe DeWagter - * - * 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. - */ -#include "subsystems/imu.h" - -#include "led.h" -#include "mcu_periph/spi.h" - -// Peripherials -#include "peripherals/mpu60x0_regs.h" -#include "peripherals/hmc58xx_regs.h" -#include "peripherals/ms5611.h" - -#ifndef MPU6000_SLAVE_IDX -#define MPU6000_SLAVE_IDX SPI_SLAVE2 -#endif - -#ifndef MPU6000_SPI_DEV -#define MPU6000_SPI_DEV spi2 -#endif - -/* HMC58XX default conf */ -#ifndef HMC58XX_DO -#define HMC58XX_DO 0x6 // Data Output Rate (6 -> 50Hz with HMC5843, 75Hz with HMC5883) -#endif -#ifndef HMC58XX_MS -#define HMC58XX_MS 0x0 // Measurement configuration -#endif -#ifndef HMC58XX_GN -#define HMC58XX_GN 0x1 // Gain configuration (1 -> +- 1 Gauss) -#endif -#ifndef HMC58XX_MD -#define HMC58XX_MD 0x0 // Continious measurement mode -#endif - -#define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS)) -#define HMC58XX_CRB (HMC58XX_GN<<5) - -struct ImuAspirin2 imu_aspirin2; - -struct spi_transaction aspirin2_mpu60x0; - -// initialize peripherals -static void mpu_configure(void); -static void trans_cb( struct spi_transaction *trans ); - -void imu_impl_init(void) { - aspirin2_mpu60x0.select = SPISelectUnselect; - aspirin2_mpu60x0.cpol = SPICpolIdleHigh; - aspirin2_mpu60x0.cpha = SPICphaEdge2; - aspirin2_mpu60x0.dss = SPIDss8bit; - aspirin2_mpu60x0.bitorder = SPIMSBFirst; - aspirin2_mpu60x0.cdiv = SPIDiv64; - aspirin2_mpu60x0.slave_idx = MPU6000_SLAVE_IDX; - aspirin2_mpu60x0.output_length = IMU_ASPIRIN_BUFFER_LEN; - aspirin2_mpu60x0.input_length = IMU_ASPIRIN_BUFFER_LEN; - aspirin2_mpu60x0.after_cb = trans_cb; - - imu_aspirin2.status = Aspirin2StatusUninit; - imu_aspirin2.imu_available = FALSE; - aspirin2_mpu60x0.input_buf = &imu_aspirin2.input_buf_p[0]; - aspirin2_mpu60x0.output_buf = &imu_aspirin2.output_buf_p[0]; -} - - -void imu_periodic(void) -{ - - if (imu_aspirin2.status == Aspirin2StatusUninit) { - mpu_configure(); - imu_aspirin2.status = Aspirin2StatusIdle; - - aspirin2_mpu60x0.output_length = 22; - aspirin2_mpu60x0.input_length = 22; - aspirin2_mpu60x0.output_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ; - for (int i=1; i6) - (MPU_DIG_FILTER << 0) ); // Low-Pass Filter - - // MPU60X0_REG_SMPLRT_DIV - mpu_set( MPU60X0_REG_SMPLRT_DIV, MPU_SMPLRT_DIV); - - // MPU60X0_REG_GYRO_CONFIG - mpu_set( MPU60X0_REG_GYRO_CONFIG, - (3 << 3) ); // -2000deg/sec - - // MPU60X0_REG_ACCEL_CONFIG - mpu_set( MPU60X0_REG_ACCEL_CONFIG, - (0 << 0) | // No HPFL - (3 << 3) ); // Full Scale = 16g - -#ifndef MPU6000_NO_SLAVES -PRINT_CONFIG_MSG("Reading MPU slaves") - - ///////////////////////////////////// - // SPI Slave Configuration Section - - // Power the Aux I2C Circuit: - // MPU60X0_REG_AUX_VDDIO = 0 (good on startup): (0 << 7); // MPU6000: 0=Vdd. MPU6050 : 0=VLogic 1=Vdd - - // MPU60X0_REG_USER_CTRL: - mpu_set( MPU60X0_REG_USER_CTRL, - (1 << 5) | // I2C_MST_EN: Enable Aux I2C Master Mode - (1 << 4) | // I2C_IF_DIS: Disable I2C on primary interface - (0 << 1) ); // Trigger a I2C_MST_RESET - - // Enable the aux i2c - mpu_set( MPU60X0_REG_I2C_MST_CTRL, - (0 << 7) | // no multimaster - (0 << 6) | // do not delay IRQ waiting for all external slaves - (0 << 5) | // no slave 3 FIFO - (0 << 4) | // restart or stop/start from one slave to another: read -> write is always stop/start - (8 << 0) ); // 0=348kHz 8=256kHz, 9=500kHz - - mpu_set( MPU60X0_REG_I2C_MST_DELAY, - (0 << 2) | // No Delay Slave 2 - (1 << 3) ); // Delay Slave 3 - -#if defined IMU_ASPIRIN_VERSION_2_1 && USE_IMU_ASPIRIN2_BARO_SLAVE - - // MS5611 Send Reset - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, MS5611_REG_RESET); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Reg_Dis: do not write the register, just the data - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - // Wait at least 2.8ms - -#endif // read MS5611 as MPU slave - - // HMC5883 Magnetometer Configuration - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // From now on a delayed rate of 1/4 is defined... - - // HMC5883 Reading: - // a) write hmc-register to HMC - // b) read 6 bytes from HMC - - mpu_set( MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV0_CTRL, - (1 << 7) | // Slave 0 enable - (0 << 6) | // Byte Swap - (6 << 0) ); // Read 6 bytes - - // Slave 0 Control: - -#if defined IMU_ASPIRIN_VERSION_2_1 && USE_IMU_ASPIRIN2_BARO_SLAVE -PRINT_CONFIG_MSG("Reading the MS5611 as MPU slave") -/* - - - // Read MS5611 Calibration - mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV1_CTRL, - (1 << 7) | // Slave 1 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // Read 6 bytes - -*/ - - // Full Rate Request For Pressure - mpu_set( MPU60X0_REG_I2C_SLV2_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV2_DO, 0x48); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV2_CTRL, - (1 << 7) | // Slave 2 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Rig Dis: Write Only - (1 << 0) ); // Write 1 byte - - // Reduced rate request For Temperature: Overwrites the Pressure Request - mpu_set( MPU60X0_REG_I2C_SLV3_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV3_DO, 0x58); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV3_CTRL, - (1 << 7) | // Slave 2 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Rig Dis: Write Only - (1 << 0) ); // Write 1 byte - - mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV1_CTRL, - (1 << 7) | // Slave 1 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // Read 6 bytes - -#endif // read MS5611 as MPU slave - -#endif - -} - diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h deleted file mode 100644 index b55872f453..0000000000 --- a/sw/airborne/subsystems/imu/imu_aspirin2.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - * Copyright (C) 2012 Christophe DeWagter - * - * 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. - */ - -#ifndef IMU_ASPIRIN_2_H -#define IMU_ASPIRIN_2_H - -#include "generated/airframe.h" -#include "subsystems/imu.h" - - -#if defined IMU_ASPIRIN_VERSION_2_1 || defined IMU_ASPIRIN_VERSION_2_2 -#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN -#define IMU_MAG_X_SIGN 1 -#define IMU_MAG_Y_SIGN 1 -#define IMU_MAG_Z_SIGN 1 -#endif -#endif - -#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN -#define IMU_GYRO_P_SIGN 1 -#define IMU_GYRO_Q_SIGN 1 -#define IMU_GYRO_R_SIGN 1 -#endif -#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN -#define IMU_ACCEL_X_SIGN 1 -#define IMU_ACCEL_Y_SIGN 1 -#define IMU_ACCEL_Z_SIGN 1 -#endif - -/** default gyro sensitivy and neutral from the datasheet - * MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range - * sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC - * sens = 1/16.4 * pi/180 * 4096 = 4.359066229 - */ -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS 4.359 -#define IMU_GYRO_P_SENS_NUM 4359 -#define IMU_GYRO_P_SENS_DEN 1000 -#define IMU_GYRO_Q_SENS 4.359 -#define IMU_GYRO_Q_SENS_NUM 4359 -#define IMU_GYRO_Q_SENS_DEN 1000 -#define IMU_GYRO_R_SENS 4.359 -#define IMU_GYRO_R_SENS_NUM 4359 -#define IMU_GYRO_R_SENS_DEN 1000 -#endif -#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL -#define IMU_GYRO_P_NEUTRAL 0 -#define IMU_GYRO_Q_NEUTRAL 0 -#define IMU_GYRO_R_NEUTRAL 0 -#endif - -/** default accel sensitivy from the datasheet - * MPU60X0 has 2048 LSB/g - * fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC - * sens = 9.81 / 2048 * 1024 = 4.905 - */ -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS 4.905 -#define IMU_ACCEL_X_SENS_NUM 4905 -#define IMU_ACCEL_X_SENS_DEN 1000 -#define IMU_ACCEL_Y_SENS 4.905 -#define IMU_ACCEL_Y_SENS_NUM 4905 -#define IMU_ACCEL_Y_SENS_DEN 1000 -#define IMU_ACCEL_Z_SENS 4.905 -#define IMU_ACCEL_Z_SENS_NUM 4905 -#define IMU_ACCEL_Z_SENS_DEN 1000 -#endif -#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL -#define IMU_ACCEL_X_NEUTRAL 0 -#define IMU_ACCEL_Y_NEUTRAL 0 -#define IMU_ACCEL_Z_NEUTRAL 0 -#endif - - -enum Aspirin2Status - { Aspirin2StatusUninit, - Aspirin2StatusIdle, - Aspirin2StatusReading - }; - -#define IMU_ASPIRIN_BUFFER_LEN 32 - -struct ImuAspirin2 { - volatile enum Aspirin2Status status; - volatile uint8_t imu_available; - volatile uint8_t input_buf_p[IMU_ASPIRIN_BUFFER_LEN]; - volatile uint8_t output_buf_p[IMU_ASPIRIN_BUFFER_LEN]; -}; - -extern struct ImuAspirin2 imu_aspirin2; - - -static inline int imu_from_buff(volatile uint8_t *buf) -{ - int32_t x, y, z, p, q, r, Mx, My, Mz; - -#define MPU_OFFSET_STATUS 1 - if (!(buf[MPU_OFFSET_STATUS] & 0x01)) { - return 0; - } - -#define MPU_OFFSET_GYRO 10 - p = (int16_t) ((buf[0+MPU_OFFSET_GYRO] << 8) | buf[1+MPU_OFFSET_GYRO]); - q = (int16_t) ((buf[2+MPU_OFFSET_GYRO] << 8) | buf[3+MPU_OFFSET_GYRO]); - r = (int16_t) ((buf[4+MPU_OFFSET_GYRO] << 8) | buf[5+MPU_OFFSET_GYRO]); - -#define MPU_OFFSET_ACC 2 - x = (int16_t) ((buf[0+MPU_OFFSET_ACC] << 8) | buf[1+MPU_OFFSET_ACC]); - y = (int16_t) ((buf[2+MPU_OFFSET_ACC] << 8) | buf[3+MPU_OFFSET_ACC]); - z = (int16_t) ((buf[4+MPU_OFFSET_ACC] << 8) | buf[5+MPU_OFFSET_ACC]); - -#define MPU_OFFSET_MAG 16 - Mx = (int16_t) ((buf[0+MPU_OFFSET_MAG] << 8) | buf[1+MPU_OFFSET_MAG]); - My = (int16_t) ((buf[2+MPU_OFFSET_MAG] << 8) | buf[3+MPU_OFFSET_MAG]); - Mz = (int16_t) ((buf[4+MPU_OFFSET_MAG] << 8) | buf[5+MPU_OFFSET_MAG]); - -#ifdef LISA_M_LONGITUDINAL_X - RATES_ASSIGN(imu.gyro_unscaled, q, -p, r); - VECT3_ASSIGN(imu.accel_unscaled, y, -x, z); - VECT3_ASSIGN(imu.mag_unscaled, -Mx, -Mz, My); -#else - RATES_ASSIGN(imu.gyro_unscaled, p, q, r); - VECT3_ASSIGN(imu.accel_unscaled, x, y, z); - VECT3_ASSIGN(imu.mag_unscaled, Mz, -Mx, My); -#endif - - return 1; -} - - -static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) -{ - if (imu_aspirin2.status == Aspirin2StatusUninit) return; - - if (imu_aspirin2.imu_available) { - imu_aspirin2.imu_available = FALSE; - if (imu_from_buff(imu_aspirin2.input_buf_p)) { - _gyro_handler(); - _accel_handler(); - _mag_handler(); - } - } -} - -#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ - imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \ -} - -#endif /* IMU_ASPIRIN_2_H */ diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index 84918649e4..bd463158df 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -145,10 +145,11 @@ void imu_aspirin2_event(void) { mpu60x0_spi_event(&imu_aspirin2.mpu); if (imu_aspirin2.mpu.data_available) { + /* HMC5883 has xzy order of axes in returned data */ struct Int32Vect3 mag; mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0); - mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); - mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); + mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); + mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); #ifdef LISA_M_LONGITUDINAL_X RATES_ASSIGN(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates.q, @@ -158,11 +159,11 @@ void imu_aspirin2_event(void) imu_aspirin2.mpu.data_accel.vect.y, -imu_aspirin2.mpu.data_accel.vect.x, imu_aspirin2.mpu.data_accel.vect.z); - VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.z, mag.y); + VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z); #else RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); - VECT3_ASSIGN(imu.mag_unscaled, mag.z, -mag.x, mag.y) + VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z) #endif imu_aspirin2.mpu.data_available = FALSE; imu_aspirin2.gyro_valid = TRUE; diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index ea49a6d3e1..b2c78c6f2c 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -192,6 +192,12 @@ void alt_kalman(float z_meas) { R = baro_ms5611_r; SIGMA2 = baro_ms5611_sigma2; } else +#elif USE_BARO_AMSYS + if (baro_amsys_enabled) { + DT = BARO_AMSYS_DT; + R = baro_amsys_r; + SIGMA2 = baro_amsys_sigma2; + } else #elif USE_BARO_BMP if (baro_bmp_enabled) { DT = BARO_BMP_DT; diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index a3bf0ef210..2797e679c7 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -52,6 +52,10 @@ #include "modules/sensors/baro_ms5611_i2c.h" #endif +#if USE_BARO_AMSYS +#include "modules/sensors/baro_amsys.h" +#endif + extern int32_t ins_qfe; extern float ins_baro_alt; extern bool_t ins_baro_initialised; diff --git a/sw/extras/chdk/pictuav.lua b/sw/extras/chdk/pictuav.lua new file mode 100644 index 0000000000..07c56de78f --- /dev/null +++ b/sw/extras/chdk/pictuav.lua @@ -0,0 +1,168 @@ +--[[ +@title PictUAV +@param d Display off frame 0=never +@default d 0 +--]] + +-- Developed on IXUS 230 HS. Other cameras should work as well, but some errors or crashes are possible +-- also considering that some firmwares still have CHDK bugs. +-- +-- Other settings in the camera that you should try to manipulate to reduce the time between taking pictures: +-- CHDK menu +-- Extra Photo overrides +-- Disable overrides : Off +-- Do not override shutter speed ( I allow the camera to determine exposure time, but feel free to experiment) +-- ND filter state: Out (removes ND filter to allow more light to fall on the sensor ) +-- Override subj. dist. value: 65535. (good to keep it there) +-- Do not save RAW images, they take longer +-- +-- Camera menu +-- Use P mode, certainly not automatic +-- If you can hardset the focus, do this towards infinity +-- Play around with aperture. Smaller apertures are better for more sharpness, but not every camera allows you to. +-- (Some cameras have really bad behaviour on larger apertures that allow more light through, especially at edges). +-- Disable IS (plane vibrations mess up its function and increases the chance of blur) +-- Take Large images with Fine resolution. +-- Turn "Review" mode off. +-- Consider hard-setting ISO, but consider local weather. If set too high, shutter time goes up, which causes blur. +-- Blur can then also occur in areas where there is little light reflection from the earth. +-- +-- How to use the script: +-- Load this on the card under "CHDK/SCRIPTS" +-- Enter the CHDK menu through your "ALT" button +-- Under scripts, select the script and specify "Autostart: on" +-- +-- As the camera starts up, this also loads. with the shutter button pressed, you can interrupt the script +-- Then press the "ALT" button to disable the scripting actuator. +-- Press the shutter button to extend the lens +-- Press "ALT" again to bring up the scripting actuator. +-- Press the shutter button to reinitiate the script. +-- If you have a IXUS 230HS like me, the focus can't be set automatically. Point the camera at a distant object while the script +-- is starting. It should say "Focused", after which it's ready for use. +-- +-- Example paparazzi airframe configuration: +-- +--
+-- +-- +-- +-- +--
+-- +-- +-- +-- +-- +-- +-- +-- + +print( "PictUAV Started " ) + +function print_status (frame) + local free = get_jpg_count() + print("#" .. frame ) +end + +-- switch to autofocus mode, pre-focus, then go to manual focus mode (locking focus there). +-- this helps to reduce the delay between the signal and taking the picture. +function pre_focus() + local focused = false + local try = 1 + while not focused and try <= 5 do + print("Pre-focus attempt " .. try) + press("shoot_half") + sleep(2000) + if get_prop(18) > 0 then + print("Focused") + focused = true + set_aflock(1) + end + release("shoot_half") + sleep(500) + try = try + 1 + end + return focused +end + +-- set aperture/shooting mode to landscape +set_prop(6,3) + +ap = get_prop(6) +print ("AF(3=inf,4=MF) "..ap) + +-- Turn IS off +set_prop(145, 3) + +-- set P mode +set_capture_mode(2) +sleep(1000) + +-- Get focusing mode +p = get_prop(6) +if (p==3) then + print "Inf set." +else + -- on ixus230hs, no explicit MF. + -- so set to infinity (3) + while (p ~= 3) do + press "left" + release "left" + p = get_prop (6) + end + print "Focus set to infinity" +end + +-- on ixus230hs set focus doesn't fail, but doesn't do anything. +set_focus(100) +print "set_focus 100" +sleep(2000) +f = get_focus + +-- on ixus230hs set focus doesn't fail, but doesn't do anything. +set_focus( 65535 ) +print "set_focus 65535" +sleep( 2000) +g = get_focus + +if (f==g) then + print "set_focus inop" + -- if focusing until here didn't work, pre-focus using a different method. + pre_focus() +else + -- set_aflock(1) fails when the camera isn't knowingly focused. + print( "Setting aflock 1" ) + sleep(1000) + set_aflock( 1 ) +end + +-- measuring the pulse on CHDK isn't necessarily that accurate, they can easily vary by 40ms. +-- Since I'm using a 100ms wait here, the variance for a shoot is around 140ms. +-- If the pulse is 250ms, then I should allow for anything down to 110. +-- For Paparazzi, taking a single picture using the button may not work because the timing may be very off +-- considering the 4Hz loop (this requires a change in the cam module for paparazzi). + +print( "PictUAV loop " ) +a = -1 +repeat + a = get_usb_power(0) + -- a pulse is detected longer than ~610ms. + if (a>61) then + print( "shutting down " ) + shut_down() + sleep(1500) + break + end + -- a pulse longer than ~100ms is detected. + if (a>10) then + + frame = 1 + shoot() + + frame = frame + 1 + + end + sleep(100) +until ( false ) +print( "PictUAV ended " ) +