From dda662fa861e682a765f3a13841b9c71a59cf8f5 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 23 Jun 2016 08:16:43 +0100 Subject: [PATCH 01/54] Fixed calling of updateRcCommands. --- src/main/mw.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/mw.c b/src/main/mw.c index 5a95f1c92..36dcad2a6 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -694,6 +694,8 @@ void subTaskMainSubprocesses(void) { #endif #if defined(BARO) || defined(SONAR) + // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(&masterConfig.airplaneConfig); @@ -874,8 +876,10 @@ void taskUpdateRxMain(void) processRx(); isRXDataNew = true; +#if !defined(BARO) && !defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); +#endif updateLEDs(); #ifdef BARO From 6f06be77f8574c50e54ab61095901ceb922de0ae Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 24 Jun 2016 21:06:40 +1200 Subject: [PATCH 02/54] Changed 'Makefile' to enable 'make' (without parameters to make use of multicore CPUs. --- Makefile | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index c083a6190..37b2dd292 100644 --- a/Makefile +++ b/Makefile @@ -664,7 +664,7 @@ all: $(VALID_TARGETS) $(VALID_TARGETS): echo "" && \ echo "Building $@" && \ - $(MAKE) -j binary hex TARGET=$@ && \ + $(MAKE) binary hex TARGET=$@ && \ echo "Building $@ succeeded." ## clean : clean up all temporary / machine-generated files @@ -700,8 +700,11 @@ st-flash_$(TARGET): $(TARGET_BIN) ## st-flash : flash firmware (.bin) onto flight controller st-flash: st-flash_$(TARGET) -binary: $(TARGET_BIN) -hex: $(TARGET_HEX) +binary: + $(MAKE) -j $(TARGET_BIN) + +hex: + $(MAKE) -j $(TARGET_HEX) unbrick_$(TARGET): $(TARGET_HEX) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon From ee860b122ba794dd01d5e5cae3645b9cd26f489e Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Sun, 26 Jun 2016 16:40:32 +1000 Subject: [PATCH 03/54] X_RACERSPI support for betaflight V3 --- fake_travis_build.sh | 3 +- src/main/target/X_RACERSPI/target.c | 106 ++++++++++++++++++ src/main/target/X_RACERSPI/target.h | 161 +++++++++++++++++++++++++++ src/main/target/X_RACERSPI/target.mk | 15 +++ 4 files changed, 284 insertions(+), 1 deletion(-) create mode 100644 src/main/target/X_RACERSPI/target.c create mode 100644 src/main/target/X_RACERSPI/target.h create mode 100644 src/main/target/X_RACERSPI/target.mk diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 5bae319ad..e9b25f9b2 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -20,7 +20,8 @@ targets=("PUBLISHMETA=True" \ "TARGET=ALIENFLIGHTF3" \ "TARGET=DOGE" \ "TARGET=SINGULARITY" \ - "TARGET=SIRINFPV") + "TARGET=SIRINFPV" \ + "TARGET=X_RACERSPI") #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c new file mode 100644 index 000000000..ff9c8044f --- /dev/null +++ b/src/main/target/X_RACERSPI/target.c @@ -0,0 +1,106 @@ + +#include +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // server #6 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h new file mode 100644 index 000000000..a655c93e2 --- /dev/null +++ b/src/main/target/X_RACERSPI/target.h @@ -0,0 +1,161 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "X_RACERSPI" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + + +#define LED0 PC14 +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 17 + + + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_INSTANCE SPI1 + + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG + +// MPU6000 interrupts +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define MPU_INT_EXTI PC13 +#define USE_EXTI + + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#ifndef UART1_GPIO +#define UART1_TX_PIN GPIO_Pin_9 // PA9 +#define UART1_RX_PIN GPIO_Pin_10 // PA10 +#define UART1_GPIO GPIOA +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource9 +#define UART1_RX_PINSOURCE GPIO_PinSource10 +#endif + +#define UART2_TX_PIN GPIO_Pin_2 // PA14 / SWCLK +#define UART2_RX_PIN GPIO_Pin_3 // PA15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource2 +#define UART2_RX_PINSOURCE GPIO_PinSource3 + +#ifndef UART3_GPIO +#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7) +#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7) +#define UART3_GPIO_AF GPIO_AF_7 +#define UART3_GPIO GPIOB +#define UART3_TX_PINSOURCE GPIO_PinSource10 +#define UART3_RX_PINSOURCE GPIO_PinSource11 +#endif + +#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 + + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +//GPIO_AF_1 + +#define SPI1_NSS_PIN PA15 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 + +#define LED_STRIP +#define LED_STRIP_TIMER TIM1 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_6 +#define WS2811_PIN GPIO_Pin_8 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM1 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) + + diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk new file mode 100644 index 000000000..2b09dc76d --- /dev/null +++ b/src/main/target/X_RACERSPI/target.mk @@ -0,0 +1,15 @@ +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c + From 016886fb3d04fc955bc29ece841ee58cc40096ba Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Sun, 26 Jun 2016 17:21:20 +1000 Subject: [PATCH 04/54] VARIANT of SPRACINGF3 --- src/main/target/X_RACERSPI/target.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index 2b09dc76d..71e749cc2 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -1,5 +1,6 @@ F3_TARGETS += $(TARGET) FEATURES = ONBOARDFLASH +TARGET_FLAGS = -DSPRACINGF3 TARGET_SRC = \ drivers/accgyro_mpu.c \ From 27e6034bb5c162c3f73874e2efb6b211d081b15c Mon Sep 17 00:00:00 2001 From: 4712betaflight <4712betaflight@outlook.de> Date: Sun, 26 Jun 2016 13:47:34 +0200 Subject: [PATCH 05/54] Add missing uint32_t tp #585 --- src/main/drivers/serial.c | 2 +- src/main/drivers/serial.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index bb81d3dd8..8a17ce679 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -56,7 +56,7 @@ void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count) } } -uint8_t serialRxBytesWaiting(serialPort_t *instance) +uint32_t serialRxBytesWaiting(serialPort_t *instance) { return instance->vTable->serialTotalRxWaiting(instance); } diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index d8f868ae7..1f75e931d 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -81,7 +81,7 @@ struct serialPortVTable { }; void serialWrite(serialPort_t *instance, uint8_t ch); -uint8_t serialRxBytesWaiting(serialPort_t *instance); +uint32_t serialRxBytesWaiting(serialPort_t *instance); uint8_t serialTxBytesFree(serialPort_t *instance); void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count); uint8_t serialRead(serialPort_t *instance); From c14af2506c5b7e85a71d9ffb7977e6ad7440e06c Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Sun, 26 Jun 2016 23:43:44 +1000 Subject: [PATCH 06/54] Fix whitespace to tab --- fake_travis_build.sh | 48 +++++++++++++++------------- src/main/target/X_RACERSPI/target.mk | 22 ++++++------- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/fake_travis_build.sh b/fake_travis_build.sh index e9b25f9b2..6c1077209 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -1,28 +1,30 @@ #!/bin/bash targets=("PUBLISHMETA=True" \ - "TARGET=CC3D" \ - "TARGET=CC3D_OPBL" \ - "TARGET=COLIBRI_RACE" \ - "TARGET=LUX_RACE" \ - "TARGET=SPRACINGF3" \ - "TARGET=SPRACINGF3EVO" \ - "TARGET=SPRACINGF3MINI" \ - "TARGET=OMNIBUS" \ - "TARGET=NAZE" \ - "TARGET=AFROMINI" \ - "TARGET=RMDO" \ - "TARGET=SPARKY" \ - "TARGET=MOTOLAB" \ - "TARGET=PIKOBLX" \ - "TARGET=IRCFUSIONF3" \ - "TARGET=ALIENFLIGHTF1" \ - "TARGET=ALIENFLIGHTF3" \ - "TARGET=DOGE" \ - "TARGET=SINGULARITY" \ - "TARGET=SIRINFPV" \ + "TARGET=CC3D" \ + "TARGET=CC3D_OPBL" \ + "TARGET=COLIBRI_RACE" \ + "TARGET=LUX_RACE" \ + "TARGET=SPRACINGF3" \ + "TARGET=SPRACINGF3EVO" \ + "TARGET=SPRACINGF3MINI" \ + "TARGET=OMNIBUS" \ + "TARGET=NAZE" \ + "TARGET=AFROMINI" \ + "TARGET=RMDO" \ + "TARGET=SPARKY" \ + "TARGET=MOTOLAB" \ + "TARGET=PIKOBLX" \ + "TARGET=IRCFUSIONF3" \ + "TARGET=ALIENFLIGHTF1" \ + "TARGET=ALIENFLIGHTF3" \ + "TARGET=DOGE" \ + "TARGET=SINGULARITY" \ + "TARGET=SIRINFPV" \ "TARGET=X_RACERSPI") + + #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) export BUILDNAME=${BUILDNAME:=fake_travis} @@ -31,9 +33,9 @@ export TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/simulated} for target in "${targets[@]}" do unset RUNTESTS PUBLISHMETA TARGET - echo - echo - echo "BUILDING '$target'" + echo + echo + echo "BUILDING '$target'" eval "export $target" make -f Makefile clean ./.travis.sh diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index 71e749cc2..97b440729 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -1,16 +1,16 @@ -F3_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH TARGET_FLAGS = -DSPRACINGF3 TARGET_SRC = \ drivers/accgyro_mpu.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/display_ug2864hsweg01.h \ - drivers/flash_m25p16.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c + drivers/accgyro_spi_mpu6000.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c From 4d238b27d5d33ed3145b9c5b0424e89ed7380409 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 08:41:24 +0100 Subject: [PATCH 07/54] Moved targetLooptime into gyro_t, tidied gyro_sync and gyro --- src/main/blackbox/blackbox.c | 5 +--- src/main/blackbox/blackbox_io.c | 1 - src/main/config/config.c | 1 - src/main/drivers/accgyro.h | 1 + src/main/drivers/accgyro_mpu.c | 10 ++++--- src/main/drivers/accgyro_mpu.h | 2 +- src/main/drivers/accgyro_mpu3050.c | 1 - src/main/drivers/accgyro_mpu6050.c | 2 +- src/main/drivers/gyro_sync.c | 44 +++++++++++++----------------- src/main/drivers/gyro_sync.h | 7 ++--- src/main/drivers/sensor.h | 2 +- src/main/flight/mixer.c | 1 - src/main/flight/pid.c | 3 +- src/main/io/osd.c | 1 - src/main/io/rc_controls.c | 1 - src/main/io/serial_cli.c | 1 - src/main/io/serial_msp.c | 7 ++--- src/main/main.c | 5 ++-- src/main/mw.c | 2 +- src/main/rx/rx.c | 1 - src/main/sensors/acceleration.c | 1 - src/main/sensors/gyro.c | 20 +++++++------- src/main/sensors/gyro.h | 2 -- src/main/sensors/initialisation.c | 3 +- 24 files changed, 52 insertions(+), 72 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ed88dd40b..46bae1958 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -325,9 +325,6 @@ extern uint32_t currentTime; //From rx.c: extern uint16_t rssi; -//From gyro.c -extern uint32_t targetLooptime; - //From rc_controls.c extern uint32_t rcModeActivationMask; @@ -1169,7 +1166,7 @@ static bool blackboxWriteSysinfo() } ); - BLACKBOX_PRINT_HEADER_LINE("looptime:%d", targetLooptime); + BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 82c105eb4..7f9b1ec5b 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -22,7 +22,6 @@ #include "drivers/accgyro.h" #include "drivers/light_led.h" #include "drivers/sound_beeper.h" -#include "drivers/gyro_sync.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index c371ad294..b7d404d3d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -38,7 +38,6 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/serial.h" -#include "drivers/gyro_sync.h" #include "drivers/pwm_output.h" #include "drivers/max7456.h" diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 385ea90b9..3d90de1bc 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -27,6 +27,7 @@ typedef struct gyro_s { sensorReadFuncPtr temperature; // read temperature if available sensorInterruptFuncPtr intStatus; float scale; // scalefactor + uint32_t targetLooptime; } gyro_t; typedef struct acc_s { diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 0882581d3..1dbe97609 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -32,7 +32,6 @@ #include "gpio.h" #include "exti.h" #include "bus_i2c.h" -#include "gyro_sync.h" #include "sensor.h" #include "accgyro.h" @@ -300,11 +299,14 @@ bool mpuGyroRead(int16_t *gyroADC) return true; } -void checkMPUDataReady(bool *mpuDataReadyPtr) { +bool checkMPUDataReady(void) +{ + bool ret; if (mpuDataReady) { - *mpuDataReadyPtr = true; + ret = true; mpuDataReady= false; } else { - *mpuDataReadyPtr = false; + ret = false; } + return ret; } diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 6dbfcebf6..20389147e 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -185,4 +185,4 @@ void mpuIntExtiInit(void); bool mpuAccRead(int16_t *accData); bool mpuGyroRead(int16_t *gyroADC); mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse); -void checkMPUDataReady(bool *mpuDataReadyPtr); +bool checkMPUDataReady(void); diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index c97472815..cefaf94b2 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -30,7 +30,6 @@ #include "accgyro.h" #include "accgyro_mpu.h" #include "accgyro_mpu3050.h" -#include "gyro_sync.h" // MPU3050, Standard address 0x68 #define MPU3050_ADDRESS 0x68 diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index d2717b135..d4fa0fb84 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -1,4 +1,4 @@ -/* +/* * This file is part of Cleanflight. * * Cleanflight is free software: you can redistribute it and/or modify diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 934708f37..61305e181 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -4,48 +4,40 @@ * Created on: 3 aug. 2015 * Author: borisb */ + #include #include -#include #include "platform.h" #include "build_config.h" -#include "common/axis.h" -#include "common/maths.h" - #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/gyro_sync.h" -#include "sensors/sensors.h" -#include "sensors/acceleration.h" - -#include "config/runtime_config.h" -#include "config/config.h" - -extern gyro_t gyro; - -uint32_t targetLooptime; static uint8_t mpuDividerDrops; -bool getMpuDataStatus(gyro_t *gyro) +bool gyroSyncCheckUpdate(const gyro_t *gyro) { - bool mpuDataStatus; if (!gyro->intStatus) - return false; - gyro->intStatus(&mpuDataStatus); - return mpuDataStatus; + return false; + return gyro->intStatus(); } -bool gyroSyncCheckUpdate(void) { - return getMpuDataStatus(&gyro); -} +#define GYRO_LPF_256HZ 0 +#define GYRO_LPF_188HZ 1 +#define GYRO_LPF_98HZ 2 +#define GYRO_LPF_42HZ 3 +#define GYRO_LPF_20HZ 4 +#define GYRO_LPF_10HZ 5 +#define GYRO_LPF_5HZ 6 +#define GYRO_LPF_NONE 7 -void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) { +uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) +{ int gyroSamplePeriod; - if (!lpf || lpf == 7) { + if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) { gyroSamplePeriod = 125; } else { gyroSamplePeriod = 1000; @@ -54,9 +46,11 @@ void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) { // calculate gyro divider and targetLooptime (expected cycleTime) mpuDividerDrops = gyroSyncDenominator - 1; - targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod; + const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod; + return targetLooptime; } -uint8_t gyroMPU6xxxGetDividerDrops(void) { +uint8_t gyroMPU6xxxGetDividerDrops(void) +{ return mpuDividerDrops; } diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index 5c229c689..f682e218c 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -5,8 +5,7 @@ * Author: borisb */ -extern uint32_t targetLooptime; - -bool gyroSyncCheckUpdate(void); +struct gyro_s; +bool gyroSyncCheckUpdate(const struct gyro_s *gyro); uint8_t gyroMPU6xxxGetDividerDrops(void); -void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator); +uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator); diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 6554f8267..7ae17f510 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -22,4 +22,4 @@ typedef void (*sensorInitFuncPtr)(void); // sensor init proto typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype -typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready +typedef bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e67723789..adc0145ba 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -36,7 +36,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/system.h" -#include "drivers/gyro_sync.h" #include "rx/rx.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e1ba58227..91ccbc2d8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -29,7 +29,6 @@ #include "common/filter.h" #include "drivers/sensor.h" -#include "drivers/gyro_sync.h" #include "drivers/accgyro.h" #include "sensors/sensors.h" @@ -74,7 +73,7 @@ pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we void setTargetPidLooptime(uint8_t pidProcessDenom) { - targetPidLooptime = targetLooptime * pidProcessDenom; + targetPidLooptime = gyro.targetLooptime * pidProcessDenom; } uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index db4c8ad4e..69f3f5691 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -52,7 +52,6 @@ #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" #include "drivers/sonar_hcsr04.h" -#include "drivers/gyro_sync.h" #include "drivers/usb_io.h" #include "drivers/transponder_ir.h" #include "drivers/sdcard.h" diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 23a87e764..7eec0de89 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -34,7 +34,6 @@ #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" #include "sensors/barometer.h" #include "sensors/battery.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 237a5310f..87df64634 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -48,7 +48,6 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/sdcard.h" -#include "drivers/gyro_sync.h" #include "drivers/buf_writer.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3bd8d1441..9f2609c0a 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -40,7 +40,6 @@ #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" -#include "drivers/gyro_sync.h" #include "drivers/sdcard.h" #include "drivers/buf_writer.h" #include "drivers/max7456.h" @@ -109,8 +108,8 @@ void setGyroSamplingSpeed(uint16_t looptime) { uint16_t gyroSampleRate = 1000; uint8_t maxDivider = 1; - if (looptime != targetLooptime || looptime == 0) { - if (looptime == 0) looptime = targetLooptime; // needed for pid controller changes + if (looptime != gyro.targetLooptime || looptime == 0) { + if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes #ifdef STM32F303xC if (looptime < 1000) { masterConfig.gyro_lpf = 0; @@ -854,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LOOP_TIME: headSerialReply(2); - serialize16((uint16_t)targetLooptime); + serialize16((uint16_t)gyro.targetLooptime); break; case MSP_RC_TUNING: headSerialReply(11); diff --git a/src/main/main.c b/src/main/main.c index dba36d3f4..686e39504 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -49,7 +49,6 @@ #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" #include "drivers/sonar_hcsr04.h" -#include "drivers/gyro_sync.h" #include "drivers/sdcard.h" #include "drivers/usb_io.h" #include "drivers/transponder_ir.h" @@ -694,12 +693,12 @@ void main_init(void) /* Setup scheduler */ schedulerInit(); - rescheduleTask(TASK_GYROPID, targetLooptime); + rescheduleTask(TASK_GYROPID, gyro.targetLooptime); setTaskEnabled(TASK_GYROPID, true); if(sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); - switch(targetLooptime) { // Switch statement kept in place to change acc rates in the future + switch(gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future case(500): case(375): case(250): diff --git a/src/main/mw.c b/src/main/mw.c index 5b282c3e8..31d6e22c7 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -800,7 +800,7 @@ void taskMainPidLoopCheck(void) const uint32_t startTime = micros(); while (true) { - if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) { + if (gyroSyncCheckUpdate(&gyro) || ((currentDeltaTime + (micros() - previousTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) { static uint8_t pidUpdateCountdown; if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 5d36956d4..255fb547e 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -41,7 +41,6 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/system.h" -#include "drivers/gyro_sync.h" #include "rx/pwm.h" #include "rx/sbus.h" #include "rx/spektrum.h" diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 320d00ddd..e4471fe30 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -28,7 +28,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/system.h" -#include "drivers/gyro_sync.h" #include "sensors/battery.h" #include "sensors/sensors.h" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index bce60a1fc..5cd741526 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -28,7 +28,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" #include "sensors/sensors.h" #include "io/beeper.h" #include "io/statusindicator.h" @@ -36,29 +35,30 @@ #include "sensors/gyro.h" -uint16_t calibratingG = 0; +gyro_t gyro; // gyro access functions +sensor_align_e gyroAlign = 0; + int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; -int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; +static uint16_t calibratingG = 0; +static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; static gyroConfig_t *gyroConfig; static biquad_t gyroFilterState[3]; static bool gyroFilterStateIsSet; static float gyroLpfCutFreq; -gyro_t gyro; // gyro access functions -sensor_align_e gyroAlign = 0; - void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz) { gyroConfig = gyroConfigToUse; gyroLpfCutFreq = gyro_lpf_hz; } -void initGyroFilterCoefficients(void) { +void initGyroFilterCoefficients(void) +{ int axis; - if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime); + if (gyroLpfCutFreq && gyro.targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], gyro.targetLooptime); gyroFilterStateIsSet = true; } } @@ -79,7 +79,7 @@ bool isOnFinalGyroCalibrationCycle(void) } uint16_t calculateCalibratingCycles(void) { - return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES; + return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } bool isOnFirstGyroCalibrationCycle(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 04a5188a0..075cccbe6 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -31,11 +31,9 @@ typedef enum { } gyroSensor_e; extern gyro_t gyro; -extern sensor_align_e gyroAlign; extern int32_t gyroADC[XYZ_AXIS_COUNT]; extern float gyroADCf[XYZ_AXIS_COUNT]; -extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT]; typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 00093d18e..df394c7a6 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -79,6 +79,7 @@ extern float magneticDeclination; extern gyro_t gyro; extern baro_t baro; extern acc_t acc; +extern sensor_align_e gyroAlign; uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; @@ -632,7 +633,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a acc.init(&acc); } // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro refresh rate before initialisation + gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation gyro.init(gyroLpf); detectMag(magHardwareToUse); From 75237dd2099351512368617ae4b9df9be78080a1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 16:14:17 +0100 Subject: [PATCH 08/54] Fixed up gyro init. --- src/main/config/config.c | 2 +- src/main/io/rc_controls.c | 2 +- src/main/main.c | 2 +- src/main/mw.c | 2 +- src/main/sensors/gyro.c | 53 +++++++++++++------------------ src/main/sensors/gyro.h | 5 +-- src/main/sensors/initialisation.c | 1 + 7 files changed, 30 insertions(+), 37 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b7d404d3d..2c0c9c17d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -750,7 +750,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); + gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 7eec0de89..0848670db 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -209,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/main.c b/src/main/main.c index 686e39504..c6cf2ffb2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -625,7 +625,7 @@ void init(void) if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 31d6e22c7..65abe6b48 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -377,7 +377,7 @@ void mwArm(void) static bool firstArmingCalibrationWasCompleted; if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); armingCalibrationWasInitialised = true; firstArmingCalibrationWasCompleted = true; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5cd741526..fc28705bf 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -45,21 +45,20 @@ static uint16_t calibratingG = 0; static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; static gyroConfig_t *gyroConfig; static biquad_t gyroFilterState[3]; -static bool gyroFilterStateIsSet; -static float gyroLpfCutFreq; +static uint8_t gyroLpfHz; -void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz) +void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz) { gyroConfig = gyroConfigToUse; - gyroLpfCutFreq = gyro_lpf_hz; + gyroLpfHz = gyro_lpf_hz; } -void initGyroFilterCoefficients(void) +void gyroInit(void) { - int axis; - if (gyroLpfCutFreq && gyro.targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], gyro.targetLooptime); - gyroFilterStateIsSet = true; + if (gyroLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known + for (int axis = 0; axis < 3; axis++) { + BiQuadNewLpf(gyroLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + } } } @@ -73,27 +72,27 @@ bool isGyroCalibrationComplete(void) return calibratingG == 0; } -bool isOnFinalGyroCalibrationCycle(void) +static bool isOnFinalGyroCalibrationCycle(void) { return calibratingG == 1; } -uint16_t calculateCalibratingCycles(void) { +uint16_t gyroCalculateCalibratingCycles(void) +{ return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } -bool isOnFirstGyroCalibrationCycle(void) +static bool isOnFirstGyroCalibrationCycle(void) { - return calibratingG == calculateCalibratingCycles(); + return calibratingG == gyroCalculateCalibratingCycles(); } static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) { - int8_t axis; static int32_t g[3]; static stdev_t var[3]; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { // Reset g[axis] at start of calibration if (isOnFirstGyroCalibrationCycle()) { @@ -113,10 +112,10 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho float dev = devStandardDeviation(&var[axis]); // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); return; } - gyroZero[axis] = (g[axis] + (calculateCalibratingCycles() / 2)) / calculateCalibratingCycles(); + gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); } } @@ -129,8 +128,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho static void applyGyroZero(void) { - int8_t axis; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { gyroADC[axis] -= gyroZero[axis]; } } @@ -138,14 +136,13 @@ static void applyGyroZero(void) void gyroUpdate(void) { int16_t gyroADCRaw[XYZ_AXIS_COUNT]; - int axis; // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { return; } - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; gyroADC[axis] = gyroADCRaw[axis]; } @@ -158,16 +155,10 @@ void gyroUpdate(void) applyGyroZero(); - if (gyroLpfCutFreq) { - if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */ - - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - if (gyroFilterStateIsSet) { - gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]); - gyroADC[axis] = lrintf(gyroADCf[axis]); - } else { - gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled - } + if (gyroLpfHz) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); + gyroADC[axis] = lrintf(gyroADCf[axis]); } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 075cccbe6..7a8214092 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -39,9 +39,10 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. } gyroConfig_t; -void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz); +void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); +void gyroInit(void); void gyroUpdate(void); bool isGyroCalibrationComplete(void); -uint16_t calculateCalibratingCycles(void); +uint16_t gyroCalculateCalibratingCycles(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index df394c7a6..80d616951 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -635,6 +635,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation gyro.init(gyroLpf); + gyroInit(); detectMag(magHardwareToUse); From f3eb19430149ba58c74e1e06dd6dcfc22c334b76 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 27 Jun 2016 06:16:23 +1000 Subject: [PATCH 09/54] Update README.md Logo change --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0239f5961..28d9c66a1 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Betaflight -![Betaflight](https://dl.dropboxusercontent.com/u/31537757/betaflight%20logo.jpg) +![Betaflight](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088900-228-bf_logo.jpg) Clean-code version of baseflight flight-controller - flight controllers are used to fly multi-rotor craft and fixed wing craft. From 67f7ae7206ad8cb1a9aed196216dfdc4bcee136e Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 27 Jun 2016 06:34:25 +1000 Subject: [PATCH 10/54] Using githubusercontent for logo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 28d9c66a1..0253334ef 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Betaflight -![Betaflight](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088900-228-bf_logo.jpg) +![Betaflight](https://camo.githubusercontent.com/8178215d6cb90842dc95c9d437b1bdf09b2d57a7/687474703a2f2f7374617469632e726367726f7570732e6e65742f666f72756d732f6174746163686d656e74732f362f312f302f332f372f362f61393038383930302d3232382d62665f6c6f676f2e6a7067) Clean-code version of baseflight flight-controller - flight controllers are used to fly multi-rotor craft and fixed wing craft. From deaeb23c84cd65d27afc12e134a8f0da8e2d3b76 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 27 Jun 2016 06:42:21 +1000 Subject: [PATCH 11/54] fixed incompatible pointer types in softserial --- src/main/drivers/serial_softserial.c | 2 +- src/main/drivers/serial_softserial.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index f5ed4d593..a2ad403b2 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -408,7 +408,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) } } -uint8_t softSerialRxBytesWaiting(serialPort_t *instance) +uint32_t softSerialRxBytesWaiting(serialPort_t *instance) { if ((instance->mode & MODE_RX) == 0) { return 0; diff --git a/src/main/drivers/serial_softserial.h b/src/main/drivers/serial_softserial.h index daa9c5c64..af80423b0 100644 --- a/src/main/drivers/serial_softserial.h +++ b/src/main/drivers/serial_softserial.h @@ -28,7 +28,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb // serialPort API void softSerialWriteByte(serialPort_t *instance, uint8_t ch); -uint8_t softSerialRxBytesWaiting(serialPort_t *instance); +uint32_t softSerialRxBytesWaiting(serialPort_t *instance); uint8_t softSerialTxBytesFree(serialPort_t *instance); uint8_t softSerialReadByte(serialPort_t *instance); void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate); From d069945f894eb7ac2848eb19d8e6abf7bb529c3b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 21:43:04 +0100 Subject: [PATCH 12/54] Improved gyroSetCalibrationCycles parameters. --- src/main/io/rc_controls.c | 2 +- src/main/main.c | 2 +- src/main/mw.c | 2 +- src/main/sensors/gyro.c | 32 ++++++++++++++++---------------- src/main/sensors/gyro.h | 5 ++--- 5 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 0848670db..fdea81003 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -209,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/main.c b/src/main/main.c index c6cf2ffb2..743f77178 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -625,7 +625,7 @@ void init(void) if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 65abe6b48..bf97c56a2 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -377,7 +377,7 @@ void mwArm(void) static bool firstArmingCalibrationWasCompleted; if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); armingCalibrationWasInitialised = true; firstArmingCalibrationWasCompleted = true; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index fc28705bf..48113e934 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -41,32 +41,27 @@ sensor_align_e gyroAlign = 0; int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; -static uint16_t calibratingG = 0; static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; -static gyroConfig_t *gyroConfig; -static biquad_t gyroFilterState[3]; -static uint8_t gyroLpfHz; +static const gyroConfig_t *gyroConfig; +static biquad_t gyroFilterState[XYZ_AXIS_COUNT]; +static uint8_t gyroSoftLpfHz; +static uint16_t calibratingG = 0; -void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz) { gyroConfig = gyroConfigToUse; - gyroLpfHz = gyro_lpf_hz; + gyroSoftLpfHz = gyro_soft_lpf_hz; } void gyroInit(void) { - if (gyroLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known + if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - BiQuadNewLpf(gyroLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime); } } } -void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired) -{ - calibratingG = calibrationCyclesRequired; -} - bool isGyroCalibrationComplete(void) { return calibratingG == 0; @@ -77,7 +72,7 @@ static bool isOnFinalGyroCalibrationCycle(void) return calibratingG == 1; } -uint16_t gyroCalculateCalibratingCycles(void) +static uint16_t gyroCalculateCalibratingCycles(void) { return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } @@ -87,6 +82,11 @@ static bool isOnFirstGyroCalibrationCycle(void) return calibratingG == gyroCalculateCalibratingCycles(); } +void gyroSetCalibrationCycles(void) +{ + calibratingG = gyroCalculateCalibratingCycles(); +} + static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) { static int32_t g[3]; @@ -112,7 +112,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho float dev = devStandardDeviation(&var[axis]); // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); return; } gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); @@ -155,7 +155,7 @@ void gyroUpdate(void) applyGyroZero(); - if (gyroLpfHz) { + if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7a8214092..54069b8a6 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -39,10 +39,9 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. } gyroConfig_t; -void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); -void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); +void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void); bool isGyroCalibrationComplete(void); -uint16_t gyroCalculateCalibratingCycles(void); From 6b5ffd14505a65e50561b61bd7f83490c375728b Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Mon, 27 Jun 2016 08:02:57 +1000 Subject: [PATCH 13/54] tab to space --- fake_travis_build.sh | 42 ++++++++++++++-------------- src/main/target/X_RACERSPI/target.mk | 24 ++++++++-------- 2 files changed, 33 insertions(+), 33 deletions(-) diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 6c1077209..1a2c9af90 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -1,27 +1,27 @@ #!/bin/bash targets=("PUBLISHMETA=True" \ - "TARGET=CC3D" \ - "TARGET=CC3D_OPBL" \ - "TARGET=COLIBRI_RACE" \ - "TARGET=LUX_RACE" \ - "TARGET=SPRACINGF3" \ - "TARGET=SPRACINGF3EVO" \ - "TARGET=SPRACINGF3MINI" \ - "TARGET=OMNIBUS" \ - "TARGET=NAZE" \ - "TARGET=AFROMINI" \ - "TARGET=RMDO" \ - "TARGET=SPARKY" \ - "TARGET=MOTOLAB" \ - "TARGET=PIKOBLX" \ - "TARGET=IRCFUSIONF3" \ - "TARGET=ALIENFLIGHTF1" \ - "TARGET=ALIENFLIGHTF3" \ - "TARGET=DOGE" \ - "TARGET=SINGULARITY" \ - "TARGET=SIRINFPV" \ - "TARGET=X_RACERSPI") + "TARGET=CC3D" \ + "TARGET=CC3D_OPBL" \ + "TARGET=COLIBRI_RACE" \ + "TARGET=LUX_RACE" \ + "TARGET=SPRACINGF3" \ + "TARGET=SPRACINGF3EVO" \ + "TARGET=SPRACINGF3MINI" \ + "TARGET=OMNIBUS" \ + "TARGET=NAZE" \ + "TARGET=AFROMINI" \ + "TARGET=RMDO" \ + "TARGET=SPARKY" \ + "TARGET=MOTOLAB" \ + "TARGET=PIKOBLX" \ + "TARGET=IRCFUSIONF3" \ + "TARGET=ALIENFLIGHTF1" \ + "TARGET=ALIENFLIGHTF3" \ + "TARGET=DOGE" \ + "TARGET=SINGULARITY" \ + "TARGET=SIRINFPV" \ + "TARGET=X_RACERSPI") diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index 97b440729..c0c219bd3 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -1,16 +1,16 @@ -F3_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH TARGET_FLAGS = -DSPRACINGF3 TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/display_ug2864hsweg01.h \ - drivers/flash_m25p16.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c From 16d9a952a2b7bad70a3e84336b6eb55377f16925 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 27 Jun 2016 14:12:02 +0200 Subject: [PATCH 14/54] Added I2C clock stretching. --- src/main/drivers/bus_i2c_stm32f10x.c | 3 +++ src/main/drivers/bus_i2c_stm32f30x.c | 2 ++ 2 files changed, 5 insertions(+) diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index c7e972b2d..72cf8769c 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -422,6 +422,9 @@ void i2cInit(I2CDevice device) I2C_Cmd(i2c->dev, ENABLE); I2C_Init(i2c->dev, &i2cInit); + + I2C_StretchClockCmd(i2c->dev, ENABLE); + // I2C ER Interrupt nvic.NVIC_IRQChannel = i2c->er_irq; diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index c324d03e7..8599d02f5 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -101,6 +101,8 @@ void i2cInit(I2CDevice device) I2C_Init(I2Cx, &i2cInit); + I2C_StretchClockCmd(I2Cx, ENABLE); + I2C_Cmd(I2Cx, ENABLE); } From afda764e45c4af82be7d242133da743616317500 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 27 Jun 2016 18:38:43 +0200 Subject: [PATCH 15/54] Tidy up the Makefile clean rules, same way main build/all rules was made. Single target clean now available as make goals. --- Makefile | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/Makefile b/Makefile index c083a6190..d97613b3e 100644 --- a/Makefile +++ b/Makefile @@ -667,24 +667,36 @@ $(VALID_TARGETS): $(MAKE) -j binary hex TARGET=$@ && \ echo "Building $@ succeeded." -## clean : clean up all temporary / machine-generated files + + +CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) ) +TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) ) + +## clean : clean up temporary / machine-generated files clean: + echo "Cleaning $(TARGET)" rm -f $(CLEAN_ARTIFACTS) rm -rf $(OBJECT_DIR)/$(TARGET) + echo "Cleaning $(TARGET) succeeded." -## clean_test : clean up all temporary / machine-generated files (tests) +## clean_test : clean up temporary / machine-generated files (tests) clean_test: cd src/test && $(MAKE) clean || true -## clean_all_targets : clean all valid target platforms -clean_all: - for clean_target in $(VALID_TARGETS); do \ - echo "" && \ - echo "Cleaning $$clean_target" && \ - $(MAKE) -j TARGET=$$clean_target clean || \ - break; \ - echo "Cleaning $$clean_target succeeded."; \ - done +## clean_ : clean up one specific target +$(CLEAN_TARGETS) : + $(MAKE) -j TARGET=$(subst clean_,,$@) clean + +## _clean : clean up one specific target (alias for above) +$(TARGETS_CLEAN) : + $(MAKE) -j TARGET=$(subst _clean,,$@) clean + +## clean_all : clean all valid targets +clean_all:$(CLEAN_TARGETS) + +## all_clean : clean all valid targets (alias for above) +all_clean:$(TARGETS_CLEAN) + flash_$(TARGET): $(TARGET_HEX) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon From 97fe5afd6c1e7f0c066c71ba3c3da0fb4f4ab2ec Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 19:26:02 +0100 Subject: [PATCH 16/54] Converted tabs to spaces. --- src/main/drivers/accgyro_bma280.c | 8 +- src/main/drivers/accgyro_l3g4200d.c | 8 +- src/main/drivers/accgyro_lsm303dlhc.c | 6 +- src/main/drivers/accgyro_mma845x.c | 8 +- src/main/drivers/accgyro_mpu.c | 36 +- src/main/drivers/accgyro_mpu.h | 12 +- src/main/drivers/accgyro_spi_mpu6000.c | 16 +- src/main/drivers/accgyro_spi_mpu6500.c | 2 +- src/main/drivers/accgyro_spi_mpu9250.c | 64 ++-- src/main/drivers/accgyro_spi_mpu9250.h | 2 +- src/main/drivers/adc.c | 10 +- src/main/drivers/adc_stm32f10x.c | 6 +- src/main/drivers/adc_stm32f30x.c | 16 +- src/main/drivers/adc_stm32f4xx.c | 26 +- src/main/drivers/barometer_bmp085.c | 18 +- src/main/drivers/barometer_bmp085.h | 4 +- src/main/drivers/barometer_bmp280.c | 10 +- src/main/drivers/barometer_ms5611.c | 12 +- src/main/drivers/bus_i2c_soft.c | 310 +++++++++--------- src/main/drivers/bus_spi.h | 28 +- src/main/drivers/compass_ak8975.c | 24 +- src/main/drivers/compass_hmc5883l.c | 20 +- src/main/drivers/dma_stm32f4xx.c | 2 +- src/main/drivers/inverter.c | 6 +- src/main/drivers/pwm_mapping.c | 2 +- src/main/drivers/pwm_mapping.h | 14 +- src/main/drivers/pwm_rx.c | 4 +- src/main/drivers/serial_softserial.c | 2 +- src/main/drivers/serial_uart.h | 12 +- src/main/drivers/serial_uart_stm32f4xx.c | 188 +++++------ src/main/drivers/serial_usb_vcp.c | 10 +- src/main/drivers/sonar_hcsr04.c | 6 +- src/main/drivers/sound_beeper.c | 22 +- src/main/drivers/sound_beeper.h | 6 +- src/main/drivers/system.h | 14 +- src/main/drivers/system_stm32f4xx.c | 34 +- src/main/drivers/timer.c | 6 +- src/main/main.c | 14 +- src/main/mw.c | 4 +- .../target/ALIENFLIGHTF3/hardware_revision.c | 4 +- src/main/target/CC3D/target.c | 52 +-- src/main/target/CJMCU/hardware_revision.c | 4 +- src/main/target/COLIBRI_RACE/target.h | 2 +- src/main/target/FURYF3/target.h | 6 +- src/main/target/KISSFC/target.h | 2 +- src/main/target/LUX_RACE/target.c | 12 +- src/main/target/REVO/target.h | 2 +- src/main/target/REVONANO/target.h | 2 +- src/main/target/SPRACINGF3/target.c | 14 +- src/main/target/X_RACERSPI/target.c | 14 +- src/main/target/X_RACERSPI/target.mk | 2 +- src/main/target/ZCOREF3/target.h | 6 +- src/main/telemetry/hott.c | 12 +- src/main/vcpf4/usb_bsp.c | 10 +- src/main/vcpf4/usb_conf.h | 6 +- src/main/vcpf4/usbd_cdc_vcp.h | 2 +- src/main/vcpf4/usbd_desc.c | 6 +- 57 files changed, 575 insertions(+), 575 deletions(-) diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c index 866d944e3..9d0ab9206 100644 --- a/src/main/drivers/accgyro_bma280.c +++ b/src/main/drivers/accgyro_bma280.c @@ -40,7 +40,7 @@ bool bma280Detect(acc_t *acc) bool ack = false; uint8_t sig = 0; - ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig); + ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig); if (!ack || sig != 0xFB) return false; @@ -51,8 +51,8 @@ bool bma280Detect(acc_t *acc) static void bma280Init(acc_t *acc) { - i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range - i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW + i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range + i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW acc->acc_1G = 512 * 8; } @@ -61,7 +61,7 @@ static bool bma280Read(int16_t *accelData) { uint8_t buf[6]; - if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) { + if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) { return false; } diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c index 27beb9d63..33bd2f1f8 100644 --- a/src/main/drivers/accgyro_l3g4200d.c +++ b/src/main/drivers/accgyro_l3g4200d.c @@ -63,7 +63,7 @@ bool l3g4200dDetect(gyro_t *gyro) delay(25); - i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid); + i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid); if (deviceid != L3G4200D_ID) return false; @@ -100,12 +100,12 @@ static void l3g4200dInit(uint8_t lpf) delay(100); - ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS); + ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS); if (!ack) failureMode(FAILURE_ACC_INIT); delay(5); - i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter); + i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter); } // Read 3 gyro values into user-provided buffer. No overrun checking is done. @@ -113,7 +113,7 @@ static bool l3g4200dRead(int16_t *gyroADC) { uint8_t buf[6]; - if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) { + if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) { return false; } diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 505764b29..7a7222d4d 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -115,15 +115,15 @@ int32_t accelSummedSamples500Hz[3]; void lsm303dlhcAccInit(acc_t *acc) { - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT); + i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT); delay(100); - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE); + i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE); delay(10); - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G); + i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G); delay(100); diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 2687c34b1..4adbe59ab 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -89,7 +89,7 @@ bool mma8452Detect(acc_t *acc) bool ack = false; uint8_t sig = 0; - ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig); + ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig); if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE)) return false; @@ -109,9 +109,9 @@ static inline void mma8451ConfigureInterrupt(void) IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? #endif - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver) - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver) + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 } static void mma8452Init(acc_t *acc) diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 0882581d3..63cb88ecf 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -228,43 +228,43 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb) void mpuIntExtiInit(void) { - static bool mpuExtiInitDone = false; + static bool mpuExtiInitDone = false; - if (mpuExtiInitDone || !mpuIntExtiConfig) { - return; - } + if (mpuExtiInitDone || !mpuIntExtiConfig) { + return; + } #if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI) - IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag); - + IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag); + #ifdef ENSURE_MPU_DATA_READY_IS_LOW - uint8_t status = IORead(mpuIntIO); - if (status) { - return; - } + uint8_t status = IORead(mpuIntIO); + if (status) { + return; + } #endif - IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI); - IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? + IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI); + IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? - EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler); - EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); - EXTIEnable(mpuIntIO, true); + EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler); + EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); + EXTIEnable(mpuIntIO, true); #endif - mpuExtiInitDone = true; + mpuExtiInitDone = true; } static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) { - bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data); + bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data); return ack; } static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data) { - bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); + bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); return ack; } diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 6dbfcebf6..15b6ee942 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -120,12 +120,12 @@ typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); typedef void(*mpuResetFuncPtr)(void); typedef struct mpuConfiguration_s { - uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each - mpuReadRegisterFunc read; - mpuWriteRegisterFunc write; - mpuReadRegisterFunc slowread; - mpuWriteRegisterFunc verifywrite; - mpuResetFuncPtr reset; + uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each + mpuReadRegisterFunc read; + mpuWriteRegisterFunc write; + mpuReadRegisterFunc slowread; + mpuWriteRegisterFunc verifywrite; + mpuResetFuncPtr reset; } mpuConfiguration_t; extern mpuConfiguration_t mpuConfiguration; diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 3d58913d0..cb78ffe10 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -49,12 +49,12 @@ static bool mpuSpi6000InitDone = false; // Bits -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define BITS_CLKSEL 0x07 -#define MPU_CLK_SEL_PLLGYROX 0x01 -#define MPU_CLK_SEL_PLLGYROZ 0x03 -#define MPU_EXT_SYNC_GYROX 0x02 +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define BITS_CLKSEL 0x07 +#define MPU_CLK_SEL_PLLGYROX 0x01 +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define MPU_EXT_SYNC_GYROX 0x02 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 @@ -74,9 +74,9 @@ static bool mpuSpi6000InitDone = false; #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 #define BITS_DLPF_CFG_MASK 0x07 #define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_RAW_RDY_EN 0x01 +#define BIT_RAW_RDY_EN 0x01 #define BIT_I2C_IF_DIS 0x10 -#define BIT_INT_STATUS_DATA 0x01 +#define BIT_INT_STATUS_DATA 0x01 #define BIT_GYRO 3 #define BIT_ACC 2 #define BIT_TEMP 1 diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 4df65c19c..273179725 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -72,7 +72,7 @@ static void mpu6500SpiInit(void) IOInit(mpuSpi6500CsPin, OWNER_SYSTEM, RESOURCE_SPI); IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG); - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); hardwareInitialised = true; } diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 2bad53bf3..95c0b3f50 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -64,7 +64,7 @@ void mpu9250ResetGyro(void) bool mpu9250WriteRegister(uint8_t reg, uint8_t data) { - ENABLE_MPU9250; + ENABLE_MPU9250; delayMicroseconds(1); spiTransferByte(MPU9250_SPI_INSTANCE, reg); spiTransferByte(MPU9250_SPI_INSTANCE, data); @@ -76,7 +76,7 @@ bool mpu9250WriteRegister(uint8_t reg, uint8_t data) bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_MPU9250; + ENABLE_MPU9250; spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); DISABLE_MPU9250; @@ -86,7 +86,7 @@ bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_MPU9250; + ENABLE_MPU9250; delayMicroseconds(1); spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); @@ -98,7 +98,7 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data) void mpu9250SpiGyroInit(uint8_t lpf) { - (void)(lpf); + (void)(lpf); mpuIntExtiInit(); @@ -126,55 +126,55 @@ void mpu9250SpiAccInit(acc_t *acc) bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) { - uint8_t in; - uint8_t attemptsRemaining = 20; + uint8_t in; + uint8_t attemptsRemaining = 20; - mpu9250WriteRegister(reg, data); - delayMicroseconds(100); + mpu9250WriteRegister(reg, data); + delayMicroseconds(100); do { - mpu9250SlowReadRegister(reg, 1, &in); - if (in == data) { - return true; - } else { - debug[3]++; - mpu9250WriteRegister(reg, data); - delayMicroseconds(100); - } + mpu9250SlowReadRegister(reg, 1, &in); + if (in == data) { + return true; + } else { + debug[3]++; + mpu9250WriteRegister(reg, data); + delayMicroseconds(100); + } } while (attemptsRemaining--); return false; } static void mpu9250AccAndGyroInit(uint8_t lpf) { - if (mpuSpi9250InitDone) { - return; - } + if (mpuSpi9250InitDone) { + return; + } spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); - delay(50); + delay(50); - verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); - verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11 + verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11 if (lpf == 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF + verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF } else if (lpf < 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF + verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF } else if (lpf > 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF + verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF } - verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops + verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops - verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); - verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #if defined(USE_MPU_DATA_READY_SIGNAL) - verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. + verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. #endif spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); @@ -191,10 +191,10 @@ bool mpu9250SpiDetect(void) #ifdef MPU9250_CS_PIN mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)); #endif - IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI); - IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG); + IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI); + IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); do { diff --git a/src/main/drivers/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro_spi_mpu9250.h index 521cefdf2..1af293288 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro_spi_mpu9250.h @@ -1,7 +1,7 @@ #pragma once -#define mpu9250_CONFIG 0x1A +#define mpu9250_CONFIG 0x1A /* We should probably use these. :) #define BITS_DLPF_CFG_256HZ 0x00 diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 46f987dfc..d214b49d3 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -35,11 +35,11 @@ volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; uint8_t adcChannelByTag(ioTag_t ioTag) { - for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) { - if (ioTag == adcTagMap[i].tag) - return adcTagMap[i].channel; - } - return 0; + for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) { + if (ioTag == adcTagMap[i].tag) + return adcTagMap[i].channel; + } + return 0; } uint16_t adcGetChannel(uint8_t channel) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index b082388fe..a810ac731 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -81,7 +81,7 @@ void adcInit(drv_adc_config_t *init) { #if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN) - UNUSED(init); + UNUSED(init); #endif uint8_t i; @@ -114,7 +114,7 @@ void adcInit(drv_adc_config_t *init) #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); + IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; @@ -125,7 +125,7 @@ void adcInit(drv_adc_config_t *init) #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); + IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index dc9c98ee5..8c2ccba5e 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -107,8 +107,8 @@ void adcInit(drv_adc_config_t *init) #ifdef VBAT_ADC_PIN if (init->enableVBat) { - IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; @@ -120,8 +120,8 @@ void adcInit(drv_adc_config_t *init) #ifdef RSSI_ADC_PIN if (init->enableRSSI) { - IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; @@ -133,8 +133,8 @@ void adcInit(drv_adc_config_t *init) #ifdef CURRENT_METER_ADC_GPIO if (init->enableCurrentMeter) { - IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; @@ -146,8 +146,8 @@ void adcInit(drv_adc_config_t *init) #ifdef EXTERNAL1_ADC_GPIO if (init->enableExternal1) { - IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 90ca22d12..e4c086a60 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -39,8 +39,8 @@ #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, - //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } }; /* note these could be packed up for saving space */ @@ -75,13 +75,13 @@ const adcTagMap_t adcTagMap[] = { ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { - if (instance == ADC1) - return ADCDEV_1; + if (instance == ADC1) + return ADCDEV_1; /* - if (instance == ADC2) // TODO add ADC2 and 3 - return ADCDEV_2; + if (instance == ADC2) // TODO add ADC2 and 3 + return ADCDEV_2; */ - return ADCINVALID; + return ADCINVALID; } void adcInit(drv_adc_config_t *init) @@ -112,7 +112,7 @@ void adcInit(drv_adc_config_t *init) #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; @@ -123,7 +123,7 @@ void adcInit(drv_adc_config_t *init) #ifdef RSSI_ADC_PIN if (init->enableRSSI) { IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; @@ -134,7 +134,7 @@ void adcInit(drv_adc_config_t *init) #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; @@ -184,11 +184,11 @@ void adcInit(drv_adc_config_t *init) ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; - ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; - ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; + ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; - ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group + ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group ADC_Init(adc.ADCx, &ADC_InitStructure); diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index a746500b5..34a01cd38 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -45,8 +45,8 @@ static bool isEOCConnected = true; // EXTI14 for BMP085 End of Conversion Interrupt void bmp085_extiHandler(extiCallbackRec_t* cb) { - UNUSED(cb); - isConversionComplete = true; + UNUSED(cb); + isConversionComplete = true; } bool bmp085TestEOCConnected(const bmp085Config_t *config); @@ -184,13 +184,13 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro) delay(20); // datasheet says 10ms, we'll be careful and do 20. - ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ + ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ if (ack) { bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID); bmp085.oversampling_setting = 3; if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */ - i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */ + i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */ bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */ bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */ bmp085_get_cal_param(); /* readout bmp085 calibparam structure */ @@ -277,7 +277,7 @@ static void bmp085_start_ut(void) #if defined(BARO_EOC_GPIO) isConversionComplete = false; #endif - i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); + i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); } static void bmp085_get_ut(void) @@ -291,7 +291,7 @@ static void bmp085_get_ut(void) } #endif - i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data); + i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data); bmp085_ut = (data[0] << 8) | data[1]; } @@ -305,7 +305,7 @@ static void bmp085_start_up(void) isConversionComplete = false; #endif - i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data); + i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data); } /** read out up for pressure conversion @@ -323,7 +323,7 @@ static void bmp085_get_up(void) } #endif - i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data); + i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data); bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting); } @@ -343,7 +343,7 @@ STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature static void bmp085_get_cal_param(void) { uint8_t data[22]; - i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data); + i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data); /*parameters AC1-AC6*/ bmp085.cal_param.ac1 = (data[0] << 8) | data[1]; diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h index dafb03a81..328bffb13 100644 --- a/src/main/drivers/barometer_bmp085.h +++ b/src/main/drivers/barometer_bmp085.h @@ -18,8 +18,8 @@ #pragma once typedef struct bmp085Config_s { - ioTag_t xclrIO; - ioTag_t eocIO; + ioTag_t xclrIO; + ioTag_t eocIO; } bmp085Config_t; bool bmp085Detect(const bmp085Config_t *config, baro_t *baro); diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c index cf7e60a81..6d4245848 100644 --- a/src/main/drivers/barometer_bmp280.c +++ b/src/main/drivers/barometer_bmp280.c @@ -83,14 +83,14 @@ bool bmp280Detect(baro_t *baro) // set oversampling + power mode (forced), and start sampling bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE); #else - i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */ + i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */ if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) return false; // read calibration - i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal); + i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal); // set oversampling + power mode (forced), and start sampling - i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); + i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); #endif bmp280InitDone = true; @@ -129,7 +129,7 @@ static void bmp280_start_up(void) { // start measurement // set oversampling + power mode (forced), and start sampling - i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); + i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); } static void bmp280_get_up(void) @@ -137,7 +137,7 @@ static void bmp280_get_up(void) uint8_t data[BMP280_DATA_FRAME_SIZE]; // read data from sensor - i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data); + i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data); bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4)); bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4)); } diff --git a/src/main/drivers/barometer_ms5611.c b/src/main/drivers/barometer_ms5611.c index 577146e2d..468f86046 100644 --- a/src/main/drivers/barometer_ms5611.c +++ b/src/main/drivers/barometer_ms5611.c @@ -67,7 +67,7 @@ bool ms5611Detect(baro_t *baro) delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms - ack = i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig); + ack = i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig); if (!ack) return false; @@ -93,14 +93,14 @@ bool ms5611Detect(baro_t *baro) static void ms5611_reset(void) { - i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1); + i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1); delayMicroseconds(2800); } static uint16_t ms5611_prom(int8_t coef_num) { uint8_t rxbuf[2] = { 0, 0 }; - i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command + i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command return rxbuf[0] << 8 | rxbuf[1]; } @@ -137,13 +137,13 @@ STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom) static uint32_t ms5611_read_adc(void) { uint8_t rxbuf[3]; - i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC + i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2]; } static void ms5611_start_ut(void) { - i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! + i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! } static void ms5611_get_ut(void) @@ -153,7 +153,7 @@ static void ms5611_get_ut(void) static void ms5611_start_up(void) { - i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! + i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! } static void ms5611_get_up(void) diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index edf9e6d72..cac5c6221 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -48,218 +48,218 @@ static volatile uint16_t i2cErrorCount = 0; static void I2C_delay(void) { - volatile int i = 7; - while (i) { - i--; - } + volatile int i = 7; + while (i) { + i--; + } } static bool I2C_Start(void) { - SDA_H; - SCL_H; - I2C_delay(); - if (!SDA_read) { - return false; - } - SDA_L; - I2C_delay(); - if (SDA_read) { - return false; - } - SDA_L; - I2C_delay(); - return true; + SDA_H; + SCL_H; + I2C_delay(); + if (!SDA_read) { + return false; + } + SDA_L; + I2C_delay(); + if (SDA_read) { + return false; + } + SDA_L; + I2C_delay(); + return true; } static void I2C_Stop(void) { - SCL_L; - I2C_delay(); - SDA_L; - I2C_delay(); - SCL_H; - I2C_delay(); - SDA_H; - I2C_delay(); + SCL_L; + I2C_delay(); + SDA_L; + I2C_delay(); + SCL_H; + I2C_delay(); + SDA_H; + I2C_delay(); } static void I2C_Ack(void) { - SCL_L; - I2C_delay(); - SDA_L; - I2C_delay(); - SCL_H; - I2C_delay(); - SCL_L; - I2C_delay(); + SCL_L; + I2C_delay(); + SDA_L; + I2C_delay(); + SCL_H; + I2C_delay(); + SCL_L; + I2C_delay(); } static void I2C_NoAck(void) { - SCL_L; - I2C_delay(); - SDA_H; - I2C_delay(); - SCL_H; - I2C_delay(); - SCL_L; - I2C_delay(); + SCL_L; + I2C_delay(); + SDA_H; + I2C_delay(); + SCL_H; + I2C_delay(); + SCL_L; + I2C_delay(); } static bool I2C_WaitAck(void) { - SCL_L; - I2C_delay(); - SDA_H; - I2C_delay(); - SCL_H; - I2C_delay(); - if (SDA_read) { - SCL_L; - return false; - } - SCL_L; - return true; + SCL_L; + I2C_delay(); + SDA_H; + I2C_delay(); + SCL_H; + I2C_delay(); + if (SDA_read) { + SCL_L; + return false; + } + SCL_L; + return true; } static void I2C_SendByte(uint8_t byte) { - uint8_t i = 8; - while (i--) { - SCL_L; - I2C_delay(); - if (byte & 0x80) { - SDA_H; - } - else { - SDA_L; - } - byte <<= 1; - I2C_delay(); - SCL_H; - I2C_delay(); - } - SCL_L; + uint8_t i = 8; + while (i--) { + SCL_L; + I2C_delay(); + if (byte & 0x80) { + SDA_H; + } + else { + SDA_L; + } + byte <<= 1; + I2C_delay(); + SCL_H; + I2C_delay(); + } + SCL_L; } static uint8_t I2C_ReceiveByte(void) { - uint8_t i = 8; - uint8_t byte = 0; + uint8_t i = 8; + uint8_t byte = 0; - SDA_H; - while (i--) { - byte <<= 1; - SCL_L; - I2C_delay(); - SCL_H; - I2C_delay(); - if (SDA_read) { - byte |= 0x01; - } - } - SCL_L; - return byte; + SDA_H; + while (i--) { + byte <<= 1; + SCL_L; + I2C_delay(); + SCL_H; + I2C_delay(); + if (SDA_read) { + byte |= 0x01; + } + } + SCL_L; + return byte; } void i2cInit(I2CDevice device) { - UNUSED(device); + UNUSED(device); - scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL)); - sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA)); + scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL)); + sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA)); - IOConfigGPIO(scl, IOCFG_OUT_OD); - IOConfigGPIO(sda, IOCFG_OUT_OD); + IOConfigGPIO(scl, IOCFG_OUT_OD); + IOConfigGPIO(sda, IOCFG_OUT_OD); } bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) { UNUSED(device); - int i; - if (!I2C_Start()) { - i2cErrorCount++; - return false; - } - I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); - if (!I2C_WaitAck()) { - I2C_Stop(); - return false; - } - I2C_SendByte(reg); - I2C_WaitAck(); - for (i = 0; i < len; i++) { - I2C_SendByte(data[i]); - if (!I2C_WaitAck()) { - I2C_Stop(); - i2cErrorCount++; - return false; - } - } - I2C_Stop(); - return true; + int i; + if (!I2C_Start()) { + i2cErrorCount++; + return false; + } + I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); + if (!I2C_WaitAck()) { + I2C_Stop(); + return false; + } + I2C_SendByte(reg); + I2C_WaitAck(); + for (i = 0; i < len; i++) { + I2C_SendByte(data[i]); + if (!I2C_WaitAck()) { + I2C_Stop(); + i2cErrorCount++; + return false; + } + } + I2C_Stop(); + return true; } bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data) { UNUSED(device); - if (!I2C_Start()) { - return false; - } - I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); - if (!I2C_WaitAck()) { - I2C_Stop(); - i2cErrorCount++; - return false; - } - I2C_SendByte(reg); - I2C_WaitAck(); - I2C_SendByte(data); - I2C_WaitAck(); - I2C_Stop(); - return true; + if (!I2C_Start()) { + return false; + } + I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); + if (!I2C_WaitAck()) { + I2C_Stop(); + i2cErrorCount++; + return false; + } + I2C_SendByte(reg); + I2C_WaitAck(); + I2C_SendByte(data); + I2C_WaitAck(); + I2C_Stop(); + return true; } bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { UNUSED(device); - if (!I2C_Start()) { - return false; - } - I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); - if (!I2C_WaitAck()) { - I2C_Stop(); - i2cErrorCount++; - return false; - } - I2C_SendByte(reg); - I2C_WaitAck(); - I2C_Start(); - I2C_SendByte(addr << 1 | I2C_Direction_Receiver); - I2C_WaitAck(); - while (len) { - *buf = I2C_ReceiveByte(); - if (len == 1) { - I2C_NoAck(); - } - else { - I2C_Ack(); - } - buf++; - len--; - } - I2C_Stop(); - return true; + if (!I2C_Start()) { + return false; + } + I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); + if (!I2C_WaitAck()) { + I2C_Stop(); + i2cErrorCount++; + return false; + } + I2C_SendByte(reg); + I2C_WaitAck(); + I2C_Start(); + I2C_SendByte(addr << 1 | I2C_Direction_Receiver); + I2C_WaitAck(); + while (len) { + *buf = I2C_ReceiveByte(); + if (len == 1) { + I2C_NoAck(); + } + else { + I2C_Ack(); + } + buf++; + len--; + } + I2C_Stop(); + return true; } uint16_t i2cGetErrorCounter(void) { - return i2cErrorCount; + return i2cErrorCount; } #endif diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index d408eac2f..019080d96 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -52,23 +52,23 @@ typedef enum { } SPIClockDivider_e; typedef enum SPIDevice { - SPIINVALID = -1, - SPIDEV_1 = 0, - SPIDEV_2, - SPIDEV_3, - SPIDEV_MAX = SPIDEV_3, + SPIINVALID = -1, + SPIDEV_1 = 0, + SPIDEV_2, + SPIDEV_3, + SPIDEV_MAX = SPIDEV_3, } SPIDevice; typedef struct SPIDevice_s { - SPI_TypeDef *dev; - ioTag_t nss; - ioTag_t sck; - ioTag_t mosi; - ioTag_t miso; - rccPeriphTag_t rcc; - uint8_t af; - volatile uint16_t errorCount; - bool sdcard; + SPI_TypeDef *dev; + ioTag_t nss; + ioTag_t sck; + ioTag_t mosi; + ioTag_t miso; + rccPeriphTag_t rcc; + uint8_t af; + volatile uint16_t errorCount; + bool sdcard; } spiDevice_t; bool spiInit(SPIDevice device); diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 386a06e7d..c35db1d1f 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -64,7 +64,7 @@ bool ak8975detect(mag_t *mag) bool ack = false; uint8_t sig = 0; - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig); if (!ack || sig != 'H') // 0x48 / 01001000 / 'H' return false; @@ -86,24 +86,24 @@ void ak8975Init() UNUSED(ack); - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode delay(20); - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode delay(10); - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values delay(10); - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading. + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading. delay(10); // Clear status registers - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); // Trigger first measurement - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); } #define BIT_STATUS1_REG_DATA_READY (1 << 0) @@ -118,13 +118,13 @@ bool ak8975Read(int16_t *magData) uint8_t status; uint8_t buf[6]; - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) { return false; } #if 1 // USE_I2C_SINGLE_BYTE_READS - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH #else for (uint8_t i = 0; i < 6; i++) { ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH @@ -134,7 +134,7 @@ bool ak8975Read(int16_t *magData) } #endif - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); if (!ack) { return false; } @@ -152,6 +152,6 @@ bool ak8975Read(int16_t *magData) magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4; - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again return true; } diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 4e59c375b..a018ab0f5 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -204,7 +204,7 @@ bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse) hmc5883Config = hmc5883ConfigToUse; - ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig); + ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig); if (!ack || sig != 'H') return false; @@ -241,15 +241,15 @@ void hmc5883lInit(void) } delay(50); - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias // Note that the very first measurement after a gain change maintains the same gain as the previous setting. // The new gain setting is effective from the second measurement and on. - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) delay(100); hmc5883lRead(magADC); for (i = 0; i < 10; i++) { // Collect 10 samples - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); delay(50); hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. @@ -267,9 +267,9 @@ void hmc5883lInit(void) } // Apply the negative bias. (Same gain) - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. for (i = 0; i < 10; i++) { - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); delay(50); hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. @@ -291,9 +291,9 @@ void hmc5883lInit(void) magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]); // leave test mode - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode delay(100); if (!bret) { // Something went wrong so get a best guess @@ -309,7 +309,7 @@ bool hmc5883lRead(int16_t *magData) { uint8_t buf[6]; - bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); + bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); if (!ack) { return false; } diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index 6b49fe629..fefbae280 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -32,7 +32,7 @@ static dmaHandlers_t dmaHandlers; void dmaNoOpHandler(DMA_Stream_TypeDef *stream) { - UNUSED(stream); + UNUSED(stream); } void DMA1_Stream2_IRQHandler(void) diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index 82ce8b099..e0231ddb7 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -31,15 +31,15 @@ static const IO_t pin = DEFIO_IO(INVERTER); void initInverter(void) { - IOInit(pin, OWNER_SYSTEM, RESOURCE_OUTPUT); - IOConfigGPIO(pin, IOCFG_OUT_PP); + IOInit(pin, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(pin, IOCFG_OUT_PP); inverterSet(false); } void inverterSet(bool on) { - IOWrite(pin, on); + IOWrite(pin, on); } #endif diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 50061fb79..8d332ba56 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -306,7 +306,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT) { #if defined(SPARKY) || defined(ALIENFLIGHTF3) - if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { + if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); } #endif diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 2212153cd..5d577af8e 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -78,19 +78,19 @@ typedef struct drv_pwm_config_s { } drv_pwm_config_t; enum { - MAP_TO_PPM_INPUT = 1, + MAP_TO_PPM_INPUT = 1, MAP_TO_PWM_INPUT, MAP_TO_MOTOR_OUTPUT, MAP_TO_SERVO_OUTPUT, }; typedef enum { - PWM_PF_NONE = 0, - PWM_PF_MOTOR = (1 << 0), - PWM_PF_SERVO = (1 << 1), - PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), - PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), - PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) + PWM_PF_NONE = 0, + PWM_PF_MOTOR = (1 << 0), + PWM_PF_SERVO = (1 << 1), + PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), + PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), + PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) } pwmPortFlags_e; enum {PWM_INVERTED = 1}; diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 9452a89ea..856613d2b 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -339,8 +339,8 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture static void pwmGPIOConfig(ioTag_t pin, ioConfig_t mode) { - IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT); - IOConfigGPIO(IOGetByTag(pin), mode); + IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT); + IOConfigGPIO(IOGetByTag(pin), mode); } void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index a2ad403b2..da2c03268 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -96,7 +96,7 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state) if (state) { IOHi(softSerial->txIO); } else { - IOLo(softSerial->txIO); + IOLo(softSerial->txIO); } } diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index a332edebf..33dde3216 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -40,13 +40,13 @@ typedef struct { serialPort_t port; #ifdef STM32F4 - DMA_Stream_TypeDef *rxDMAStream; - DMA_Stream_TypeDef *txDMAStream; - uint32_t rxDMAChannel; - uint32_t txDMAChannel; + DMA_Stream_TypeDef *rxDMAStream; + DMA_Stream_TypeDef *txDMAStream; + uint32_t rxDMAChannel; + uint32_t txDMAChannel; #else - DMA_Channel_TypeDef *rxDMAChannel; - DMA_Channel_TypeDef *txDMAChannel; + DMA_Channel_TypeDef *rxDMAChannel; + DMA_Channel_TypeDef *txDMAChannel; #endif uint32_t rxDMAIrq; diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 71b112e36..f5bba8469 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -365,22 +365,22 @@ void DMA2_Stream7_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7); - } } // USART1 Rx/Tx IRQ Handler @@ -402,30 +402,30 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t optio void DMA1_Stream6_IRQHandler(void) { uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port); - if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6)) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6); - } - handleUsartTxDma(s); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6); - } + if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6)) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6); + } } void USART2_IRQHandler(void) { uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port); - usartIrqHandler(s); + usartIrqHandler(s); } #endif @@ -442,22 +442,22 @@ void DMA1_Stream3_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_3]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF3)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF3); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF3); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF3)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF3); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF3); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF3); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF3)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF3); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF3)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF3); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF3)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF3); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF3)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF3); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF3)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF3); - } } void USART3_IRQHandler(void) @@ -480,22 +480,22 @@ void DMA1_Stream4_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_4]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF4)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF4); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF4); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF4)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF4); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF4); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF4); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF4)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF4); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF4)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF4); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF4)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF4); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF4)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF4); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF4)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF4); - } } void UART4_IRQHandler(void) @@ -518,22 +518,22 @@ void DMA1_Stream7_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_5]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7); - } } void UART5_IRQHandler(void) @@ -556,22 +556,22 @@ void DMA2_Stream6_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_6]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6); - } } void USART6_IRQHandler(void) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index e9ccef82f..bb74b36b3 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -183,12 +183,12 @@ serialPort_t *usbVcpOpen(void) #ifdef STM32F4 IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, RESOURCE_IO); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, RESOURCE_IO); - USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); + USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); #else - Set_System(); - Set_USBClock(); - USB_Interrupts_Config(); - USB_Init(); + Set_System(); + Set_USBClock(); + USB_Interrupts_Config(); + USB_Init(); #endif s = &vcpPort; diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 6374f81d3..3b8ec91f7 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -92,9 +92,9 @@ void hcsr04_init(sonarRange_t *sonarRange) IOConfigGPIO(echoIO, IOCFG_IN_FLOATING); #ifdef USE_EXTI - EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); - EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! - EXTIEnable(echoIO, true); + EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); + EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! + EXTIEnable(echoIO, true); #endif lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance() diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 1e71845d2..cba9a2a5c 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -39,31 +39,31 @@ static bool beeperInverted = false; void systemBeep(bool onoff) { #ifndef BEEPER - UNUSED(onoff); + UNUSED(onoff); #else - IOWrite(beeperIO, beeperInverted ? onoff : !onoff); + IOWrite(beeperIO, beeperInverted ? onoff : !onoff); #endif } void systemBeepToggle(void) { #ifdef BEEPER - IOToggle(beeperIO); + IOToggle(beeperIO); #endif } void beeperInit(const beeperConfig_t *config) { #ifndef BEEPER - UNUSED(config); + UNUSED(config); #else - beeperIO = IOGetByTag(config->ioTag); - beeperInverted = config->isInverted; + beeperIO = IOGetByTag(config->ioTag); + beeperInverted = config->isInverted; - if (beeperIO) { - IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT); - IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); - } - systemBeep(false); + if (beeperIO) { + IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT); + IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); + } + systemBeep(false); #endif } diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index bdd17b745..ab7a7c3df 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -30,9 +30,9 @@ #endif typedef struct beeperConfig_s { - ioTag_t ioTag; - unsigned isInverted : 1; - unsigned isOD : 1; + ioTag_t ioTag; + unsigned isInverted : 1; + unsigned isOD : 1; } beeperConfig_t; void systemBeep(bool on); diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 6f8bf23d6..92e17f8a8 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -25,13 +25,13 @@ uint32_t micros(void); uint32_t millis(void); typedef enum { - FAILURE_DEVELOPER = 0, - FAILURE_MISSING_ACC, - FAILURE_ACC_INIT, - FAILURE_ACC_INCOMPATIBLE, - FAILURE_INVALID_EEPROM_CONTENTS, - FAILURE_FLASH_WRITE_FAILED, - FAILURE_GYRO_INIT_FAILED + FAILURE_DEVELOPER = 0, + FAILURE_MISSING_ACC, + FAILURE_ACC_INIT, + FAILURE_ACC_INCOMPATIBLE, + FAILURE_INVALID_EEPROM_CONTENTS, + FAILURE_FLASH_WRITE_FAILED, + FAILURE_GYRO_INIT_FAILED } failureMode_e; // failure diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index bcaf71c9d..f0042026f 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -45,8 +45,8 @@ void systemReset(void) if (mpuConfiguration.reset) mpuConfiguration.reset(); - __disable_irq(); - NVIC_SystemReset(); + __disable_irq(); + NVIC_SystemReset(); } void systemResetToBootloader(void) @@ -54,10 +54,10 @@ void systemResetToBootloader(void) if (mpuConfiguration.reset) mpuConfiguration.reset(); - *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX + *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); + __disable_irq(); + NVIC_SystemReset(); } void enableGPIOPowerUsageAndNoiseReductions(void) @@ -82,7 +82,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void) RCC_AHB1Periph_BKPSRAM | RCC_AHB1Periph_DMA1 | RCC_AHB1Periph_DMA2 | - 0, ENABLE + 0, ENABLE ); RCC_AHB2PeriphClockCmd(0, ENABLE); @@ -172,25 +172,25 @@ void systemInit(void) SetSysClock(); // Configure NVIC preempt/priority groups - NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); + NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); // cache RCC->CSR value to use it in isMPUSoftreset() and others - cachedRccCsrValue = RCC->CSR; + cachedRccCsrValue = RCC->CSR; /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ - extern void *isr_vector_table_base; - NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0); - RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE); + extern void *isr_vector_table_base; + NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0); + RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE); - RCC_ClearFlag(); + RCC_ClearFlag(); - enableGPIOPowerUsageAndNoiseReductions(); + enableGPIOPowerUsageAndNoiseReductions(); // Init cycle counter - cycleCounterInit(); + cycleCounterInit(); - memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); - // SysTick - SysTick_Config(SystemCoreClock / 1000); + memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); + // SysTick + SysTick_Config(SystemCoreClock / 1000); } diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 673265f54..732fa5f4a 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -148,7 +148,7 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { return timerDefinitions[i].rcc; - } + } } return 0; } @@ -190,7 +190,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) #else TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; #endif - + TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure); @@ -660,7 +660,7 @@ void timerInit(void) IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); } #endif - + // initialize timer channel structures for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { timerChannelInfo[i].type = TYPE_FREE; diff --git a/src/main/main.c b/src/main/main.c index dba36d3f4..b1a157c4d 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -488,12 +488,12 @@ void init(void) #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, - masterConfig.acc_hardware, - masterConfig.mag_hardware, - masterConfig.baro_hardware, - masterConfig.mag_declination, - masterConfig.gyro_lpf, - masterConfig.gyro_sync_denom)) { + masterConfig.acc_hardware, + masterConfig.mag_hardware, + masterConfig.baro_hardware, + masterConfig.mag_declination, + masterConfig.gyro_lpf, + masterConfig.gyro_sync_denom)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } @@ -688,7 +688,7 @@ void processLoopback(void) { #define processLoopback() #endif -void main_init(void) +void main_init(void) { init(); diff --git a/src/main/mw.c b/src/main/mw.c index e2819b722..f77c42673 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -185,7 +185,7 @@ float calculateRate(int axis, int16_t rc) { } - return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection } void processRcCommand(void) @@ -778,7 +778,7 @@ void subTaskMotorUpdate(void) uint8_t setPidUpdateCountDown(void) { if (masterConfig.gyro_soft_lpf_hz) { - return masterConfig.pid_process_denom - 1; + return masterConfig.pid_process_denom - 1; } else { return 1; } diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.c b/src/main/target/ALIENFLIGHTF3/hardware_revision.c index 210836995..b51d38658 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.c +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.c @@ -39,7 +39,7 @@ static IO_t HWDetectPin = IO_NONE; void detectHardwareRevision(void) { - HWDetectPin = IOGetByTag(IO_TAG(HW_PIN)); + HWDetectPin = IOGetByTag(IO_TAG(HW_PIN)); IOInit(HWDetectPin, OWNER_SYSTEM, RESOURCE_INPUT); IOConfigGPIO(HWDetectPin, IOCFG_IPU); @@ -74,4 +74,4 @@ const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) else { return &alienFlightF3V2MPUIntExtiConfig; } -} \ No newline at end of file +} diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index b146e6a72..abfe089aa 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -28,12 +28,12 @@ const uint16_t multiPWM[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 0xFFFF }; @@ -59,26 +59,26 @@ const uint16_t airPWM[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 - PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 0xFFFF }; const uint16_t multiPPM_BP6[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; @@ -89,9 +89,9 @@ const uint16_t multiPWM_BP6[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -118,11 +118,11 @@ const uint16_t airPWM_BP6[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 0xFFFF }; diff --git a/src/main/target/CJMCU/hardware_revision.c b/src/main/target/CJMCU/hardware_revision.c index 29071314b..558e19a76 100755 --- a/src/main/target/CJMCU/hardware_revision.c +++ b/src/main/target/CJMCU/hardware_revision.c @@ -55,5 +55,5 @@ void updateHardwareRevision(void) const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) { - return NULL; -} \ No newline at end of file + return NULL; +} diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 68bf28448..976b287a0 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -34,7 +34,7 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN PA4 +#define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 #define USE_SPI diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 6201b8b3f..f849070b7 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -25,9 +25,9 @@ #define USE_EXTI #define CONFIG_PREFER_ACC_ON -#define LED0 PC14 +#define LED0 PC14 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect @@ -188,7 +188,7 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTF (BIT(4)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 03c3cf31f..6d79046a7 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -86,6 +86,6 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTF (BIT(4)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 30804c575..b665072ac 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -26,12 +26,12 @@ const uint16_t multiPWM[] = { PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index cda67742e..d2124e5b2 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -23,7 +23,7 @@ #define USBD_PRODUCT_STRING "Revolution" #ifdef OPBL - #define USBD_SERIALNUMBER_STRING "0x8020000" +#define USBD_SERIALNUMBER_STRING "0x8020000" #endif #define LED0 PB5 diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 3c0287f2f..6f4a2c431 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -22,7 +22,7 @@ #define USBD_PRODUCT_STRING "Revo Nano" #ifdef OPBL - #define USBD_SERIALNUMBER_STRING "0x8010000" +#define USBD_SERIALNUMBER_STRING "0x8010000" #endif #define LED0 PC14 diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ff9c8044f..ddebee117 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -92,15 +92,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index ff9c8044f..ddebee117 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -92,15 +92,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index c0c219bd3..3d831edc4 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -12,5 +12,5 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c + drivers/sonar_hcsr04.c diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index c02b4d3ec..d8ac143e8 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB8 +#define LED0 PB8 #define BEEPER PC15 #define BEEPER_INVERTED @@ -45,8 +45,8 @@ #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG #define BARO #define USE_BARO_BMP280 diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 1eec8b9c0..6fb0bc1a9 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -424,8 +424,8 @@ static void hottCheckSerialData(uint32_t currentMicros) } static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) { - closeSerialPort(hottPort); - hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED); + closeSerialPort(hottPort); + hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED); } static void hottSendTelemetryData(void) { @@ -433,9 +433,9 @@ static void hottSendTelemetryData(void) { hottIsSending = true; // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3)) - workAroundForHottTelemetryOnUsart(hottPort, MODE_TX); + workAroundForHottTelemetryOnUsart(hottPort, MODE_TX); else - serialSetMode(hottPort, MODE_TX); + serialSetMode(hottPort, MODE_TX); hottMsgCrc = 0; return; } @@ -445,9 +445,9 @@ static void hottSendTelemetryData(void) { hottIsSending = false; // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3)) - workAroundForHottTelemetryOnUsart(hottPort, MODE_RX); + workAroundForHottTelemetryOnUsart(hottPort, MODE_RX); else - serialSetMode(hottPort, MODE_RX); + serialSetMode(hottPort, MODE_RX); flushHottRxBuffer(); return; } diff --git a/src/main/vcpf4/usb_bsp.c b/src/main/vcpf4/usb_bsp.c index ba3fee767..1160c47d8 100644 --- a/src/main/vcpf4/usb_bsp.c +++ b/src/main/vcpf4/usb_bsp.c @@ -28,12 +28,12 @@ #include "../drivers/io.h" void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; } void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) { - (void)pdev; - (void)state; + (void)pdev; + (void)state; } @@ -46,7 +46,7 @@ void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) { void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; GPIO_InitTypeDef GPIO_InitStructure; #ifndef USE_ULPI_PHY @@ -100,7 +100,7 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev) */ void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; NVIC_InitTypeDef NVIC_InitStructure; NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); diff --git a/src/main/vcpf4/usb_conf.h b/src/main/vcpf4/usb_conf.h index 6caf29888..60c803620 100644 --- a/src/main/vcpf4/usb_conf.h +++ b/src/main/vcpf4/usb_conf.h @@ -233,9 +233,9 @@ #elif defined (__ICCARM__) /* IAR Compiler */ #define __packed __packed #elif defined ( __GNUC__ ) /* GNU Compiler */ - #ifndef __packed - #define __packed __attribute__ ((__packed__)) - #endif + #ifndef __packed + #define __packed __attribute__ ((__packed__)) + #endif #elif defined (__TASKING__) /* TASKING Compiler */ #define __packed __unaligned #endif /* __CC_ARM */ diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 3c89b5b9e..3ca22aa4e 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,7 +34,7 @@ #include "usbd_usr.h" #include "usbd_desc.h" -#define USB_RX_BUFSIZE 1024 +#define USB_RX_BUFSIZE 1024 __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index 41282dab2..f9478327e 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -197,7 +197,7 @@ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END = */ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; *length = sizeof(USBD_DeviceDesc); return USBD_DeviceDesc; } @@ -211,7 +211,7 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; *length = sizeof(USBD_LangIDDesc); return USBD_LangIDDesc; } @@ -245,7 +245,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; USBD_GetString ((uint8_t*)USBD_MANUFACTURER_STRING, USBD_StrDesc, length); return USBD_StrDesc; } From 7d5c8de5520b5d177f683a415be42318e4600878 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 19:32:41 +0100 Subject: [PATCH 17/54] Changed tabs to spaces --- src/main/drivers/accgyro_l3gd20.c | 2 +- src/main/drivers/accgyro_spi_mpu6000.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index dd28aceb1..aade7445b 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -62,7 +62,7 @@ #define BLOCK_DATA_UPDATE_CONTINUOUS ((uint8_t)0x00) -#define BLE_MSB ((uint8_t)0x40) +#define BLE_MSB ((uint8_t)0x40) #define BOOT ((uint8_t)0x80) diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h index c717a7bb1..0081bddeb 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro_spi_mpu6000.h @@ -1,7 +1,7 @@ #pragma once -#define MPU6000_CONFIG 0x1A +#define MPU6000_CONFIG 0x1A #define BITS_DLPF_CFG_256HZ 0x00 #define BITS_DLPF_CFG_188HZ 0x01 From a2d1af04aafa5c23b105aad894deb3e6cc787da4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 20:04:21 +0100 Subject: [PATCH 18/54] Minor cosmetic tidying --- src/main/drivers/timer.c | 2 -- src/main/main.c | 9 ++++----- src/main/sensors/acceleration.c | 2 +- src/main/sensors/barometer.c | 7 ++++--- src/main/sensors/compass.c | 9 +++++---- src/main/target/X_RACERSPI/target.c | 1 - src/main/target/X_RACERSPI/target.h | 2 +- 7 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 732fa5f4a..6999a9798 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -17,7 +17,6 @@ #include #include -#include #include #include "platform.h" @@ -26,7 +25,6 @@ #include "nvic.h" -#include "gpio.h" #include "gpio.h" #include "rcc.h" #include "system.h" diff --git a/src/main/main.c b/src/main/main.c index b1a157c4d..e31c6933d 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -17,11 +17,11 @@ #include #include -#include #include #include #include "platform.h" + #include "common/axis.h" #include "common/color.h" #include "common/maths.h" @@ -153,9 +153,6 @@ static uint8_t systemState = SYSTEM_STATE_INITIALISING; void init(void) { - uint8_t i; - drv_pwm_config_t pwm_params; - printfSupportInit(); initEEPROM(); @@ -260,6 +257,7 @@ void init(void) mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif + drv_pwm_config_t pwm_params; memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR @@ -333,6 +331,7 @@ void init(void) #endif pwmRxInit(masterConfig.inputFilteringMode); + // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm); @@ -504,7 +503,7 @@ void init(void) LED0_OFF; LED2_OFF; - for (i = 0; i < 10; i++) { + for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 320d00ddd..1a4f6ef21 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -45,7 +45,7 @@ acc_t acc; // acc access functions sensor_align_e accAlign = 0; uint32_t accTargetLooptime; -uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. +static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. extern uint16_t InflightcalibratingA; extern bool AccInflightCalibrationArmed; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 2a011d8e2..1793e3794 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -20,6 +20,10 @@ #include #include "platform.h" + +int32_t BaroAlt = 0; + +#ifdef BARO #include "common/maths.h" #include "drivers/barometer.h" @@ -32,9 +36,6 @@ baro_t baro; // barometer access functions uint16_t calibratingB = 0; // baro calibration = get new ground pressure value int32_t baroPressure = 0; int32_t baroTemperature = 0; -int32_t BaroAlt = 0; - -#ifdef BARO static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 7ee14da4c..de075bfe2 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,13 +40,14 @@ #endif mag_t mag; // mag access functions +int32_t magADC[XYZ_AXIS_COUNT]; +sensor_align_e magAlign = 0; + +#ifdef MAG extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead. -int16_t magADCRaw[XYZ_AXIS_COUNT]; -int32_t magADC[XYZ_AXIS_COUNT]; -sensor_align_e magAlign = 0; -#ifdef MAG +static int16_t magADCRaw[XYZ_AXIS_COUNT]; static uint8_t magInit = 0; void compassInit(void) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index ddebee117..9c0bd0403 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -84,7 +84,6 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index a655c93e2..39c2e6c20 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -17,7 +17,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "X_RACERSPI" +#define TARGET_BOARD_IDENTIFIER "XRC3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE From abd6b1e0f67637c64c053e415920640009a8b6b0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 28 Jun 2016 07:25:02 +1000 Subject: [PATCH 19/54] Enable pull ups for i2c for F3 targets by default. --- src/main/drivers/bus_i2c_stm32f30x.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index c324d03e7..adb424390 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -83,8 +83,8 @@ void i2cInit(I2CDevice device) RCC_ClockCmd(i2c->rcc, ENABLE); RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK); - IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4); - IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4); + IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4); + IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4); I2C_InitTypeDef i2cInit = { .I2C_Mode = I2C_Mode_I2C, From 5a10e75551f56597c1ac4aa0ec28d10cf8d127fe Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 28 Jun 2016 10:27:26 +1000 Subject: [PATCH 20/54] Update hmc5883l to use new IO --- src/main/drivers/compass_hmc5883l.c | 81 ++++++----------------------- src/main/drivers/compass_hmc5883l.h | 16 ++---- src/main/sensors/initialisation.c | 31 ++--------- src/main/target/NAZE/target.h | 3 +- src/main/target/SPRACINGF3/target.h | 9 ++-- 5 files changed, 31 insertions(+), 109 deletions(-) diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index a018ab0f5..c8c398fc3 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -29,7 +29,8 @@ #include "system.h" #include "nvic.h" -#include "gpio.h" +#include "io.h" +#include "exti.h" #include "bus_i2c.h" #include "light_led.h" @@ -120,14 +121,14 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f }; static const hmc5883Config_t *hmc5883Config = NULL; -void MAG_DATA_READY_EXTI_Handler(void) +#ifdef USE_MAG_DATA_READY_SIGNAL + +static IO_t intIO; +static extiCallbackRec_t hmc5883_extiCallbackRec; + +void hmc5883_extiHandler(extiCallbackRec_t* cb) { - if (EXTI_GetITStatus(hmc5883Config->exti_line) == RESET) { - return; - } - - EXTI_ClearITPendingBit(hmc5883Config->exti_line); - + UNUSED(cb); #ifdef DEBUG_MAG_DATA_READY_INTERRUPT // Measure the delta between calls to the interrupt handler // currently should be around 65/66 milli seconds / 15hz output rate @@ -143,57 +144,26 @@ void MAG_DATA_READY_EXTI_Handler(void) lastCalledAt = now; #endif } +#endif static void hmc5883lConfigureDataReadyInterruptHandling(void) { #ifdef USE_MAG_DATA_READY_SIGNAL - if (!(hmc5883Config->exti_port_source && hmc5883Config->exti_pin_source)) { + if (!(hmc5883Config->intTag)) { return; } -#ifdef STM32F10X - // enable AFIO for EXTI support - RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); -#endif - -#ifdef STM32F303xC - /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); -#endif - -#ifdef STM32F10X - gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source); -#endif - -#ifdef STM32F303xC - gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source); -#endif - + intIO = IOGetByTag(hmc5883Config->intTag); #ifdef ENSURE_MAG_DATA_READY_IS_HIGH - uint8_t status = GPIO_ReadInputDataBit(hmc5883Config->gpioPort, hmc5883Config->gpioPin); + uint8_t status = IORead(intIO); if (!status) { return; } #endif - registerExtiCallbackHandler(hmc5883Config->exti_irqn, MAG_DATA_READY_EXTI_Handler); - - EXTI_ClearITPendingBit(hmc5883Config->exti_line); - - EXTI_InitTypeDef EXTIInit; - EXTIInit.EXTI_Line = hmc5883Config->exti_line; - EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt; - EXTIInit.EXTI_Trigger = EXTI_Trigger_Falling; - EXTIInit.EXTI_LineCmd = ENABLE; - EXTI_Init(&EXTIInit); - - NVIC_InitTypeDef NVIC_InitStructure; - - NVIC_InitStructure.NVIC_IRQChannel = hmc5883Config->exti_irqn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MAG_DATA_READY); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MAG_DATA_READY); - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler); + EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising); + EXTIEnable(intIO, true); #endif } @@ -221,25 +191,6 @@ void hmc5883lInit(void) int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. bool bret = true; // Error indicator - gpio_config_t gpio; - - if (hmc5883Config) { -#ifdef STM32F303 - if (hmc5883Config->gpioAHBPeripherals) { - RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE); - } -#endif -#ifdef STM32F10X - if (hmc5883Config->gpioAPB2Peripherals) { - RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE); - } -#endif - gpio.pin = hmc5883Config->gpioPin; - gpio.speed = Speed_2MHz; - gpio.mode = Mode_IN_FLOATING; - gpioInit(hmc5883Config->gpioPort, &gpio); - } - delay(50); i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias // Note that the very first measurement after a gain change maintains the same gain as the previous setting. diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 53c4c9f3e..035a2c936 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -17,20 +17,10 @@ #pragma once -typedef struct hmc5883Config_s { -#ifdef STM32F303 - uint32_t gpioAHBPeripherals; -#endif -#ifdef STM32F10X - uint32_t gpioAPB2Peripherals; -#endif - uint16_t gpioPin; - GPIO_TypeDef *gpioPort; +#include "io.h" - uint8_t exti_port_source; - uint32_t exti_line; - uint8_t exti_pin_source; - IRQn_Type exti_irqn; +typedef struct hmc5883Config_s { + ioTag_t intTag; } hmc5883Config_t; bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 80d616951..2d96a0130 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -484,27 +484,12 @@ static void detectMag(magSensor_e magHardwareToUse) #ifdef USE_MAG_HMC5883 const hmc5883Config_t *hmc5883Config = 0; -#ifdef NAZE +#ifdef NAZE // TODO remove this target specific define static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { - .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, - .gpioPin = Pin_12, - .gpioPort = GPIOB, - - /* Disabled for v4 needs more work. - .exti_port_source = GPIO_PortSourceGPIOB, - .exti_pin_source = GPIO_PinSource12, - .exti_line = EXTI_Line12, - .exti_irqn = EXTI15_10_IRQn - */ + .intTag = IO_TAG(PB12) /* perhaps disabled? */ }; static const hmc5883Config_t nazeHmc5883Config_v5 = { - .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, - .gpioPin = Pin_14, - .gpioPort = GPIOC, - .exti_port_source = GPIO_PortSourceGPIOC, - .exti_line = EXTI_Line14, - .exti_pin_source = GPIO_PinSource14, - .exti_irqn = EXTI15_10_IRQn + .intTag = IO_TAG(MAG_INT_EXTI) }; if (hardwareRevision < NAZE32_REV5) { hmc5883Config = &nazeHmc5883Config_v1_v4; @@ -513,15 +498,9 @@ static void detectMag(magSensor_e magHardwareToUse) } #endif -#ifdef SPRACINGF3 +#ifdef MAG_INT_EXTI static const hmc5883Config_t spRacingF3Hmc5883Config = { - .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, - .gpioPin = Pin_14, - .gpioPort = GPIOC, - .exti_port_source = EXTI_PortSourceGPIOC, - .exti_pin_source = EXTI_PinSource14, - .exti_line = EXTI_Line14, - .exti_irqn = EXTI15_10_IRQn + .intTag = IO_TAG(MAG_INT_EXTI) }; hmc5883Config = &spRacingF3Hmc5883Config; diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 9f6e3d2b2..95f531faf 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -74,7 +74,8 @@ //#define DEBUG_MAG_DATA_READY_INTERRUPT #define USE_MAG_DATA_READY_SIGNAL - +#define MAG_INT_EXTI PC14 + #define GYRO #define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 68413bbe0..a4f44762d 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -35,10 +35,6 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH - - #define GYRO #define USE_GYRO_MPU6050 #define GYRO_MPU6050_ALIGN CW270_DEG @@ -57,6 +53,11 @@ #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW270_DEG +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH +#define MAG_INT_EXTI PC14 + + #define USE_FLASHFS #define USE_FLASH_M25P16 From e04c0a3d66ad5e44b06b23ca8a1b1b2f6cd2ee9f Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 28 Jun 2016 15:13:33 +1000 Subject: [PATCH 21/54] Additional IO cleanup for HMC5883 fix --- src/main/drivers/accgyro_lsm303dlhc.c | 2 +- src/main/drivers/accgyro_lsm303dlhc.h | 57 ++++++--------------------- src/main/drivers/accgyro_mpu.h | 2 + src/main/drivers/compass_ak8963.c | 2 - 4 files changed, 14 insertions(+), 49 deletions(-) diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 7a7222d4d..4caed3091 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -26,7 +26,7 @@ #include "common/axis.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "bus_i2c.h" #include "sensor.h" diff --git a/src/main/drivers/accgyro_lsm303dlhc.h b/src/main/drivers/accgyro_lsm303dlhc.h index 6cc79066c..1f5c56f68 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.h +++ b/src/main/drivers/accgyro_lsm303dlhc.h @@ -22,8 +22,7 @@ */ /* LSM303DLHC ACC struct */ -typedef struct -{ +typedef struct { uint8_t Power_Mode; /* Power-down/Normal Mode */ uint8_t AccOutput_DataRate; /* OUT data rate */ uint8_t Axes_Enable; /* Axes enable */ @@ -31,25 +30,23 @@ typedef struct uint8_t BlockData_Update; /* Block Data Update */ uint8_t Endianness; /* Endian Data selection */ uint8_t AccFull_Scale; /* Full Scale selection */ -}LSM303DLHCAcc_InitTypeDef; +} LSM303DLHCAcc_InitTypeDef; /* LSM303DLHC Acc High Pass Filter struct */ -typedef struct -{ +typedef struct { uint8_t HighPassFilter_Mode_Selection; /* Internal filter mode */ uint8_t HighPassFilter_CutOff_Frequency; /* High pass filter cut-off frequency */ uint8_t HighPassFilter_AOI1; /* HPF_enabling/disabling for AOI function on interrupt 1 */ uint8_t HighPassFilter_AOI2; /* HPF_enabling/disabling for AOI function on interrupt 2 */ -}LSM303DLHCAcc_FilterConfigTypeDef; +} LSM303DLHCAcc_FilterConfigTypeDef; /* LSM303DLHC Mag struct */ -typedef struct -{ +typedef struct { uint8_t Temperature_Sensor; /* Temperature sensor enable/disable */ uint8_t MagOutput_DataRate; /* OUT data rate */ uint8_t Working_Mode; /* operating mode */ uint8_t MagFull_Scale; /* Full Scale selection */ -}LSM303DLHCMag_InitTypeDef; +} LSM303DLHCMag_InitTypeDef; /** * @} */ @@ -78,43 +75,11 @@ typedef struct * @brief LSM303DLHC I2C Interface pins */ #define LSM303DLHC_I2C I2C1 -#define LSM303DLHC_I2C_CLK RCC_APB1Periph_I2C1 - -#define LSM303DLHC_I2C_SCK_PIN GPIO_Pin_6 /* PB.06 */ -#define LSM303DLHC_I2C_SCK_GPIO_PORT GPIOB /* GPIOB */ -#define LSM303DLHC_I2C_SCK_GPIO_CLK RCC_AHBPeriph_GPIOB -#define LSM303DLHC_I2C_SCK_SOURCE GPIO_PinSource6 -#define LSM303DLHC_I2C_SCK_AF GPIO_AF_4 - -#define LSM303DLHC_I2C_SDA_PIN GPIO_Pin_7 /* PB.7 */ -#define LSM303DLHC_I2C_SDA_GPIO_PORT GPIOB /* GPIOB */ -#define LSM303DLHC_I2C_SDA_GPIO_CLK RCC_AHBPeriph_GPIOB -#define LSM303DLHC_I2C_SDA_SOURCE GPIO_PinSource7 -#define LSM303DLHC_I2C_SDA_AF GPIO_AF_4 - -#define LSM303DLHC_DRDY_PIN GPIO_Pin_2 /* PE.02 */ -#define LSM303DLHC_DRDY_GPIO_PORT GPIOE /* GPIOE */ -#define LSM303DLHC_DRDY_GPIO_CLK RCC_AHBPeriph_GPIOE -#define LSM303DLHC_DRDY_EXTI_LINE EXTI_Line2 -#define LSM303DLHC_DRDY_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE -#define LSM303DLHC_DRDY_EXTI_PIN_SOURCE EXTI_PinSource2 -#define LSM303DLHC_DRDY_EXTI_IRQn EXTI2_TS_IRQn - -#define LSM303DLHC_I2C_INT1_PIN GPIO_Pin_4 /* PE.04 */ -#define LSM303DLHC_I2C_INT1_GPIO_PORT GPIOE /* GPIOE */ -#define LSM303DLHC_I2C_INT1_GPIO_CLK RCC_AHBPeriph_GPIOE -#define LSM303DLHC_I2C_INT1_EXTI_LINE EXTI_Line4 -#define LSM303DLHC_I2C_INT1_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE -#define LSM303DLHC_I2C_INT1_EXTI_PIN_SOURCE EXTI_PinSource4 -#define LSM303DLHC_I2C_INT1_EXTI_IRQn EXTI4_IRQn - -#define LSM303DLHC_I2C_INT2_PIN GPIO_Pin_5 /* PE.05 */ -#define LSM303DLHC_I2C_INT2_GPIO_PORT GPIOE /* GPIOE */ -#define LSM303DLHC_I2C_INT2_GPIO_CLK RCC_AHBPeriph_GPIOE -#define LSM303DLHC_I2C_INT2_EXTI_LINE EXTI_Line5 -#define LSM303DLHC_I2C_INT2_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE -#define LSM303DLHC_I2C_INT2_EXTI_PIN_SOURCE EXTI_PinSource5ss -#define LSM303DLHC_I2C_INT2_EXTI_IRQn EXTI9_5_IRQn +#define LSM303DLHC_I2C_SCK_PIN PB6 /* PB.06 */ +#define LSM303DLHC_I2C_SDA_PIN PB7 /* PB.7 */ +#define LSM303DLHC_DRDY_PIN PE2 /* PE.02 */ +#define LSM303DLHC_I2C_INT1_PIN PE4 /* PE.04 */ +#define LSM303DLHC_I2C_INT2_PIN PE5 /* PE.05 */ /******************************************************************************/ /*************************** START REGISTER MAPPING **************************/ diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 4ce6b552d..917c06f1a 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -17,6 +17,8 @@ #pragma once +#include "exti.h" + // MPU6050 #define MPU_RA_WHO_AM_I 0x75 #define MPU_RA_WHO_AM_I_LEGACY 0x00 diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 7b60fe5c4..0668fcdd7 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -30,8 +30,6 @@ #include "common/maths.h" #include "system.h" -#include "gpio.h" -#include "exti.h" #include "bus_i2c.h" #include "bus_spi.h" From 657564efa7907c34fa2e733e47379170189846c4 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 28 Jun 2016 19:47:00 +1000 Subject: [PATCH 22/54] Small tidy --- src/main/drivers/bus_i2c_stm32f30x.c | 8 ++++---- src/main/sensors/compass.c | 2 +- src/main/sensors/initialisation.c | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index adb424390..145731667 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -31,6 +31,9 @@ #ifndef SOFT_I2C +#define I2C_HIGHSPEED_TIMING 0x00500E30 // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. +#define I2C_STANDARD_TIMING 0x00E0257A // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + #define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_GPIO_AF GPIO_AF_4 @@ -93,10 +96,7 @@ void i2cInit(I2CDevice device) .I2C_OwnAddress1 = 0x00, .I2C_Ack = I2C_Ack_Enable, .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_Timing = i2c->overClock ? - 0x00500E30 : // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. - 0x00E0257A, // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. - //.I2C_Timing = 0x8000050B; + .I2C_Timing = (i2c->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING) }; I2C_Init(I2Cx, &i2cInit); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index de075bfe2..cca2328c0 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -35,7 +35,7 @@ #include "sensors/sensors.h" #include "sensors/compass.h" -#ifdef NAZE +#ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 2d96a0130..399ad7b84 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -499,11 +499,11 @@ static void detectMag(magSensor_e magHardwareToUse) #endif #ifdef MAG_INT_EXTI - static const hmc5883Config_t spRacingF3Hmc5883Config = { + static const hmc5883Config_t extiHmc5883Config = { .intTag = IO_TAG(MAG_INT_EXTI) }; - hmc5883Config = &spRacingF3Hmc5883Config; + hmc5883Config = &extiHmc5883Config; #endif #endif From c5071fcdad567ee7f20c98c2294e1af8a7b3e3ff Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 28 Jun 2016 21:07:04 +1000 Subject: [PATCH 23/54] USE_I2C_PULLUP now a potential define in target.h --- src/main/drivers/bus_i2c_stm32f30x.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 145731667..cc78da8d6 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -31,6 +31,12 @@ #ifndef SOFT_I2C +#if defined(USE_I2C_PULLUP) +#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP) +#else +#define IOCFG_I2C IOCFG_AF_OD +#endif + #define I2C_HIGHSPEED_TIMING 0x00500E30 // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. #define I2C_STANDARD_TIMING 0x00E0257A // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. @@ -86,8 +92,8 @@ void i2cInit(I2CDevice device) RCC_ClockCmd(i2c->rcc, ENABLE); RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK); - IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4); - IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4); + IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_4); + IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_4); I2C_InitTypeDef i2cInit = { .I2C_Mode = I2C_Mode_I2C, From 32d5a664d5a7c5ecd87aa70115e8fb25446fdddd Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 28 Jun 2016 20:46:47 +0100 Subject: [PATCH 24/54] Fixed KISS VBAT and motor order --- src/main/target/KISSFC/target.c | 20 ++++++++++---------- src/main/target/KISSFC/target.h | 10 +++++----- src/main/target/KISSFC/target.mk | 2 +- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 30d03b9f6..db129454a 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -7,33 +7,33 @@ const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), PWM9 | (MAP_TO_PWM_INPUT << 8), - PWM10 | (MAP_TO_PWM_INPUT << 8), - PWM11 | (MAP_TO_PWM_INPUT << 8), - PWM12 | (MAP_TO_PWM_INPUT << 8), + PWM10 | (MAP_TO_PWM_INPUT << 8), + PWM11 | (MAP_TO_PWM_INPUT << 8), + PWM12 | (MAP_TO_PWM_INPUT << 8), 0xFFFF }; diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 6d79046a7..f45fbe059 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -72,11 +72,11 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA -//#define USE_ADC -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +//#define CURRENT_METER_ADC_PIN PA5 +//#define RSSI_ADC_PIN PB2 #define SPEKTRUM_BIND diff --git a/src/main/target/KISSFC/target.mk b/src/main/target/KISSFC/target.mk index 56c635c92..6ed94fd46 100644 --- a/src/main/target/KISSFC/target.mk +++ b/src/main/target/KISSFC/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP +FEATURES = VCP TARGET_SRC = \ drivers/accgyro_mpu.c \ From a4788fa2ab033415e4fb9f603a1877b1ebb8e7d6 Mon Sep 17 00:00:00 2001 From: nathan Date: Mon, 27 Jun 2016 22:22:29 -0700 Subject: [PATCH 25/54] its betaflight, not raceflight :) --- src/main/vcpf4/usbd_desc.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index f9478327e..f3f9a3af9 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -57,7 +57,7 @@ * @{ */ #define USBD_LANGID_STRING 0x409 -#define USBD_MANUFACTURER_STRING "RaceFlight" +#define USBD_MANUFACTURER_STRING "BetaFlight" #ifdef USBD_PRODUCT_STRING #define USBD_PRODUCT_HS_STRING USBD_PRODUCT_STRING From e70d15114b013da8d0bda2150d406ab7392f2f81 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 29 Jun 2016 19:00:18 +1000 Subject: [PATCH 26/54] Fixed ADC issue with NAZE (and other F1 targets) --- src/main/drivers/adc.h | 3 ++ src/main/drivers/adc_stm32f10x.c | 44 +++++++++------------- src/main/drivers/adc_stm32f30x.c | 48 ++++++++---------------- src/main/drivers/adc_stm32f4xx.c | 64 ++++++++++++++------------------ src/main/drivers/rcc.c | 8 ++-- 5 files changed, 68 insertions(+), 99 deletions(-) diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index f090c8fa8..55914a336 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -17,6 +17,8 @@ #pragma once +#include "io.h" + typedef enum { ADC_BATTERY = 0, ADC_RSSI = 1, @@ -28,6 +30,7 @@ typedef enum { #define ADC_CHANNEL_COUNT (ADC_CHANNEL_MAX + 1) typedef struct adc_config_s { + ioTag_t tag; uint8_t adcChannel; // ADC1_INxx channel number uint8_t dmaIndex; // index into DMA buffer in case of sparse channels bool enabled; diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index a810ac731..9d9474246 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -91,60 +91,50 @@ void adcInit(drv_adc_config_t *init) #ifdef VBAT_ADC_PIN if (init->enableVBat) { - IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); - adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_BATTERY].enabled = true; - adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { - IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); - adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_RSSI].enabled = true; - adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); } #endif #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { - IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); - adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_EXTERNAL1].enabled = true; - adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); } #endif #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { - IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); - adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_CURRENT].enabled = true; - adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); } #endif ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); if (device == ADCINVALID) return; - + adcDevice_t adc = adcHardware[device]; + + for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcConfig[i].tag) + continue; + + IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AIN, 0)); + adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); + adcConfig[i].dmaIndex = configuredAdcChannels++; + adcConfig[i].sampleTime = ADC_SampleTime_239Cycles5; + adcConfig[i].enabled = true; + } RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) RCC_ClockCmd(adc.rccADC, ENABLE); RCC_ClockCmd(adc.rccDMA, ENABLE); - // FIXME ADC driver assumes all the GPIO was already placed in 'AIN' mode - DMA_DeInit(adc.DMAy_Channelx); DMA_InitTypeDef DMA_InitStructure; DMA_StructInit(&DMA_InitStructure); diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 8c2ccba5e..3fc7427b5 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -107,53 +107,25 @@ void adcInit(drv_adc_config_t *init) #ifdef VBAT_ADC_PIN if (init->enableVBat) { - IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - - adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); - adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; - adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5; - adcConfig[ADC_BATTERY].enabled = true; - adcChannelCount++; + adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { - IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - - adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); - adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; - adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5; - adcConfig[ADC_RSSI].enabled = true; - adcChannelCount++; + adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); } #endif #ifdef CURRENT_METER_ADC_GPIO if (init->enableCurrentMeter) { - IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - - adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); - adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; - adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5; - adcConfig[ADC_CURRENT].enabled = true; - adcChannelCount++; + adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); } #endif #ifdef EXTERNAL1_ADC_GPIO if (init->enableExternal1) { - IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); - adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; - adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5; - adcConfig[ADC_EXTERNAL1].enabled = true; - adcChannelCount++; + adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); } #endif @@ -163,6 +135,18 @@ void adcInit(drv_adc_config_t *init) adcDevice_t adc = adcHardware[device]; + for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcConfig[i].tag) + continue; + + IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); + adcConfig[i].dmaIndex = adcChannelCount++; + adcConfig[i].sampleTime = ADC_SampleTime_601Cycles5; + adcConfig[i].enabled = true; + } + RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz RCC_ClockCmd(adc.rccADC, ENABLE); RCC_ClockCmd(adc.rccDMA, ENABLE); diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index e4c086a60..e0b36a5f3 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -100,45 +100,25 @@ void adcInit(drv_adc_config_t *init) #ifdef VBAT_ADC_PIN if (init->enableVBat) { - IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); //VBAT_ADC_CHANNEL; - adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_BATTERY].enabled = true; - adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_480Cycles; - } -#endif - -#ifdef EXTERNAL1_ADC_PIN - if (init->enableExternal1) { - IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL; - adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_EXTERNAL1].enabled = true; - adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_480Cycles; + adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); //VBAT_ADC_CHANNEL; } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { - IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL; - adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_RSSI].enabled = true; - adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_480Cycles; + adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); //RSSI_ADC_CHANNEL; + } +#endif + +#ifdef EXTERNAL1_ADC_PIN + if (init->enableExternal1) { + adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); //EXTERNAL1_ADC_CHANNEL; } #endif #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { - IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL; - adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_CURRENT].enabled = true; - adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_480Cycles; + adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); //CURRENT_METER_ADC_CHANNEL; } #endif @@ -150,6 +130,18 @@ void adcInit(drv_adc_config_t *init) adcDevice_t adc = adcHardware[device]; + for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcConfig[i].tag) + continue; + + IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); + adcConfig[i].dmaIndex = configuredAdcChannels++; + adcConfig[i].sampleTime = ADC_SampleTime_480Cycles; + adcConfig[i].enabled = true; + } + RCC_ClockCmd(adc.rccDMA, ENABLE); RCC_ClockCmd(adc.rccADC, ENABLE); @@ -174,20 +166,20 @@ void adcInit(drv_adc_config_t *init) ADC_CommonInitTypeDef ADC_CommonInitStructure; ADC_CommonStructInit(&ADC_CommonInitStructure); - ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; - ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; - ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; + ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; + ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; ADC_CommonInit(&ADC_CommonInitStructure); ADC_StructInit(&ADC_InitStructure); - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group ADC_Init(adc.ADCx, &ADC_InitStructure); diff --git a/src/main/drivers/rcc.c b/src/main/drivers/rcc.c index f8cc88b72..b1e759f97 100644 --- a/src/main/drivers/rcc.c +++ b/src/main/drivers/rcc.c @@ -7,7 +7,7 @@ void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState) int tag = periphTag >> 5; uint32_t mask = 1 << (periphTag & 0x1f); switch (tag) { -#if defined(STM32F303xC) +#if defined(STM32F3) || defined(STM32F1) case RCC_AHB: RCC_AHBPeriphClockCmd(mask, NewState); break; @@ -18,7 +18,7 @@ void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState) case RCC_APB1: RCC_APB1PeriphClockCmd(mask, NewState); break; -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#if defined(STM32F4) case RCC_AHB1: RCC_AHB1PeriphClockCmd(mask, NewState); break; @@ -31,7 +31,7 @@ void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState) int tag = periphTag >> 5; uint32_t mask = 1 << (periphTag & 0x1f); switch (tag) { -#if defined(STM32F303xC) +#if defined(STM32F3) || defined(STM32F10X_CL) case RCC_AHB: RCC_AHBPeriphResetCmd(mask, NewState); break; @@ -42,7 +42,7 @@ void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState) case RCC_APB1: RCC_APB1PeriphResetCmd(mask, NewState); break; -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#if defined(STM32F4) case RCC_AHB1: RCC_AHB1PeriphResetCmd(mask, NewState); break; From 72934e2be286e069b1ad9d971c34df4cb195b54d Mon Sep 17 00:00:00 2001 From: nathan Date: Wed, 29 Jun 2016 02:02:57 -0700 Subject: [PATCH 27/54] betaflight font mapping accounts for logo at the end of the font file, starting at 0xA0 --- src/main/drivers/max7456_symbols.h | 21 +++++++-------------- 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index 68de42893..2030cb956 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -125,16 +125,9 @@ #define SYM_MAG11 0xB6 // AH Center screen Graphics -//#define SYM_AH_CENTER 0x01 -#ifdef ALT_CENTER - #define SYM_AH_CENTER_LINE 0xB0 - #define SYM_AH_CENTER 0xB1 - #define SYM_AH_CENTER_LINE_RIGHT 0xB2 -#else - #define SYM_AH_CENTER_LINE 0x26 - #define SYM_AH_CENTER 0x7E - #define SYM_AH_CENTER_LINE_RIGHT 0xBC -#endif +#define SYM_AH_CENTER_LINE 0x26 +#define SYM_AH_CENTER_LINE_RIGHT 0x27 +#define SYM_AH_CENTER 0x7E #define SYM_AH_RIGHT 0x02 #define SYM_AH_LEFT 0x03 #define SYM_AH_DECORATION_UP 0xC9 @@ -183,7 +176,7 @@ #define SYM_FT 0x0F // Voltage and amperage -#define SYM_VOLT 0xA9 +#define SYM_VOLT 0x00 #define SYM_AMP 0x9A #define SYM_MAH 0xA4 #define SYM_WATT 0x57 @@ -214,11 +207,11 @@ #define SYM_FLY_H 0x71 // Throttle Position (%) -#define SYM_THR 0xC8 -#define SYM_THR1 0xC9 +#define SYM_THR 0x04 +#define SYM_THR1 0x05 // RSSI -#define SYM_RSSI 0xBA +#define SYM_RSSI 0x01 // Menu cursor #define SYM_CURSOR SYM_AH_LEFT From 7bf60be27f99713d4431674279102861b96b8e59 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 29 Jun 2016 15:01:45 +0100 Subject: [PATCH 28/54] Configuable default VBAT settings --- src/main/config/config.c | 3 --- src/main/sensors/battery.h | 2 ++ src/main/target/ALIENFLIGHTF3/target.h | 5 ++--- src/main/target/KISSFC/target.h | 3 +++ src/main/target/SINGULARITY/target.h | 1 + 5 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 2c0c9c17d..284c59097 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -659,7 +659,6 @@ static void resetConf(void) masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; #endif #ifdef ALIENFLIGHTF3 - masterConfig.batteryConfig.vbatscale = 20; masterConfig.mag_hardware = MAG_NONE; // disabled by default #endif masterConfig.rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048; @@ -687,8 +686,6 @@ static void resetConf(void) #if defined(SINGULARITY) // alternative defaults settings for SINGULARITY target - masterConfig.batteryConfig.vbatscale = 77; - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; #endif diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 26e54eb4e..21197e07f 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -20,7 +20,9 @@ #include "rx/rx.h" #include "common/maths.h" +#ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 +#endif #define VBAT_RESDIVVAL_DEFAULT 10 #define VBAT_RESDIVMULTIPLIER_DEFAULT 1 #define VBAT_SCALE_MIN 0 diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 89e9c1a9c..ccef02d34 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -18,6 +18,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF3" // AlienFlight F3. +#define ALIENFLIGHT #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT @@ -118,9 +119,7 @@ #define ADC_INSTANCE ADC2 //#define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PA4 - -// alternative defaults for AlienFlight F3 target -#define ALIENFLIGHT +#define VBAT_SCALE_DEFAULT 20 #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index f45fbe059..538dc653b 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -73,11 +73,14 @@ #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_ADC +#define VBAT_SCALE_DEFAULT 164 #define ADC_INSTANCE ADC1 #define VBAT_ADC_PIN PA0 //#define CURRENT_METER_ADC_PIN PA5 //#define RSSI_ADC_PIN PB2 +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_VBAT #define SPEKTRUM_BIND #define BIND_PIN PB4 diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 192971fd1..f58dcbe96 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -95,6 +95,7 @@ #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PB2 +#define VBAT_SCALE_DEFAULT 77 #define LED_STRIP #define LED_STRIP_TIMER TIM1 From 9059254db6ae6d0b527066f1e0e274e61e3f0bd7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 28 Jun 2016 22:00:23 +0100 Subject: [PATCH 29/54] Minor tidy of sensor code --- src/main/drivers/accgyro_mpu6050.c | 2 -- src/main/sensors/acceleration.h | 3 +-- src/main/sensors/barometer.h | 6 ++---- src/main/sensors/compass.h | 10 ++++------ src/main/sensors/gyro.c | 6 +++++- src/main/sensors/gyro.h | 3 ++- src/main/sensors/sensors.h | 2 +- 7 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index d4fa0fb84..3cc3ac8f6 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -38,8 +38,6 @@ #include "accgyro_mpu.h" #include "accgyro_mpu6050.h" -extern uint8_t mpuLowPassFilter; - //#define DEBUG_MPU_DATA_READY_INTERRUPT // MPU6050, Standard address 0x68 diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 9e56c2abe..dc98ae78e 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -29,10 +29,9 @@ typedef enum { ACC_MPU6000 = 7, ACC_MPU6500 = 8, ACC_FAKE = 9, + ACC_MAX = ACC_FAKE } accelerationSensor_e; -#define ACC_MAX ACC_FAKE - extern sensor_align_e accAlign; extern acc_t acc; extern uint32_t accTargetLooptime; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 7ea2c1e5b..0ef2638ad 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -22,11 +22,11 @@ typedef enum { BARO_NONE = 1, BARO_BMP085 = 2, BARO_MS5611 = 3, - BARO_BMP280 = 4 + BARO_BMP280 = 4, + BARO_MAX = BARO_BMP280 } baroSensor_e; #define BARO_SAMPLE_COUNT_MAX 48 -#define BARO_MAX BARO_MS5611 typedef struct barometerConfig_s { uint8_t baro_sample_count; // size of baro filter array @@ -38,7 +38,6 @@ typedef struct barometerConfig_s { extern int32_t BaroAlt; extern int32_t baroTemperature; // Use temperature for telemetry -#ifdef BARO void useBarometerConfig(barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); @@ -46,4 +45,3 @@ uint32_t baroUpdate(void); bool isBaroReady(void); int32_t baroCalculateAltitude(void); void performBaroCalibrationCycle(void); -#endif diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 0807ba410..2f5ae7d21 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -23,15 +23,13 @@ typedef enum { MAG_NONE = 1, MAG_HMC5883 = 2, MAG_AK8975 = 3, - MAG_AK8963 = 4 + MAG_AK8963 = 4, + MAG_MAX = MAG_AK8963 } magSensor_e; -#define MAG_MAX MAG_AK8963 - -#ifdef MAG void compassInit(void); -void updateCompass(flightDynamicsTrims_t *magZero); -#endif +union flightDynamicsTrims_u; +void updateCompass(union flightDynamicsTrims_u *magZero); extern int32_t magADC[XYZ_AXIS_COUNT]; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 48113e934..c79d29013 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -41,7 +41,7 @@ sensor_align_e gyroAlign = 0; int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; -static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; +static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; static biquad_t gyroFilterState[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfHz; @@ -160,5 +160,9 @@ void gyroUpdate(void) gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } + } else { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroADCf[axis] = gyroADC[axis]; + } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 54069b8a6..2239eb681 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -27,7 +27,8 @@ typedef enum { GYRO_MPU6000, GYRO_MPU6500, GYRO_MPU9250, - GYRO_FAKE + GYRO_FAKE, + GYRO_MAX = GYRO_FAKE } gyroSensor_e; extern gyro_t gyro; diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 92e6907ee..f47b96d0b 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -34,7 +34,7 @@ typedef struct int16_flightDynamicsTrims_s { int16_t yaw; } flightDynamicsTrims_def_t; -typedef union { +typedef union flightDynamicsTrims_u { int16_t raw[3]; flightDynamicsTrims_def_t values; } flightDynamicsTrims_t; From d3b51f2360dd23155b00d5d54d22502924506599 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 29 Jun 2016 18:06:08 +0100 Subject: [PATCH 30/54] Added count to sensor index enums --- src/main/sensors/initialisation.c | 2 +- src/main/sensors/sensors.h | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 399ad7b84..0e75ad15f 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -81,7 +81,7 @@ extern baro_t baro; extern acc_t acc; extern sensor_align_e gyroAlign; -uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; +uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; const extiConfig_t *selectMPUIntExtiConfig(void) diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index f47b96d0b..9e6fdda92 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -21,12 +21,11 @@ typedef enum { SENSOR_INDEX_GYRO = 0, SENSOR_INDEX_ACC, SENSOR_INDEX_BARO, - SENSOR_INDEX_MAG + SENSOR_INDEX_MAG, + SENSOR_INDEX_COUNT } sensorIndex_e; -#define MAX_SENSORS_TO_DETECT (SENSOR_INDEX_MAG + 1) - -extern uint8_t detectedSensors[MAX_SENSORS_TO_DETECT]; +extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; typedef struct int16_flightDynamicsTrims_s { int16_t roll; From c10129bc507a2cbc27af063e633ae2360b5557ad Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 29 Jun 2016 20:12:52 +0100 Subject: [PATCH 31/54] Combined timer output and inverted fields --- src/main/drivers/pwm_mapping.h | 2 -- src/main/drivers/pwm_output.c | 4 +-- src/main/drivers/timer.c | 2 +- src/main/drivers/timer.h | 7 +++-- src/main/target/ALIENFLIGHTF3/target.c | 28 +++++++++--------- src/main/target/ALIENFLIGHTF4/target.c | 26 ++++++++-------- src/main/target/BLUEJAYF4/target.c | 14 ++++----- src/main/target/CC3D/target.c | 24 +++++++-------- src/main/target/CHEBUZZF3/target.c | 36 +++++++++++------------ src/main/target/CJMCU/target.c | 28 +++++++++--------- src/main/target/COLIBRI_RACE/target.c | 22 +++++++------- src/main/target/DOGE/target.c | 18 ++++++------ src/main/target/EUSTM32F103RC/target.c | 28 +++++++++--------- src/main/target/FURYF3/target.c | 14 ++++----- src/main/target/FURYF4/target.c | 12 ++++---- src/main/target/IRCFUSIONF3/target.c | 34 ++++++++++----------- src/main/target/KISSFC/target.c | 24 +++++++-------- src/main/target/LUX_RACE/target.c | 22 +++++++------- src/main/target/MOTOLAB/target.c | 18 ++++++------ src/main/target/NAZE/target.c | 28 +++++++++--------- src/main/target/OLIMEXINO/target.c | 28 +++++++++--------- src/main/target/OMNIBUS/target.c | 24 +++++++-------- src/main/target/PIKOBLX/target.c | 18 ++++++------ src/main/target/PORT103R/target.c | 28 +++++++++--------- src/main/target/REVO/target.c | 24 +++++++-------- src/main/target/REVONANO/target.c | 24 +++++++-------- src/main/target/RMDO/target.c | 34 ++++++++++----------- src/main/target/SINGULARITY/target.c | 20 ++++++------- src/main/target/SIRINFPV/target.c | 12 ++++---- src/main/target/SPARKY/target.c | 22 +++++++------- src/main/target/SPRACINGF3/target.c | 34 ++++++++++----------- src/main/target/SPRACINGF3EVO/target.c | 24 +++++++-------- src/main/target/SPRACINGF3MINI/target.c | 26 ++++++++-------- src/main/target/STM32F3DISCOVERY/target.c | 28 +++++++++--------- src/main/target/X_RACERSPI/target.c | 34 ++++++++++----------- src/main/target/ZCOREF3/target.c | 34 ++++++++++----------- 36 files changed, 402 insertions(+), 403 deletions(-) diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 5d577af8e..40aa5d4d2 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -93,8 +93,6 @@ typedef enum { PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) } pwmPortFlags_e; -enum {PWM_INVERTED = 1}; - typedef struct pwmPortConfiguration_s { uint8_t index; pwmPortFlags_e flags; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index b44594938..277b47736 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -97,9 +97,9 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 configTimeBase(timerHardware->tim, period, mhz); pwmGPIOConfig(timerHardware->tag, IOCFG_AF_PP); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->outputInverted); + pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED); - if (timerHardware->outputEnable) { + if (timerHardware->output & TIMER_OUTPUT_ENABLED) { TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE); } TIM_Cmd(timerHardware->tim, ENABLE); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 6999a9798..37067ae25 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -456,7 +456,7 @@ void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHig if(outEnable) { TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - if (timHw->outputInverted) { + if (timHw->output & TIMER_OUTPUT_INVERTED) { stateHigh = !stateHigh; } TIM_OCInitStructure.TIM_OCPolarity = stateHigh ? TIM_OCPolarity_High : TIM_OCPolarity_Low; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 0cd5f684e..cdc461c69 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -76,13 +76,14 @@ typedef struct timerHardware_s { ioTag_t tag; uint8_t channel; uint8_t irq; - uint8_t outputEnable; + uint8_t output; ioConfig_t ioMode; #if defined(STM32F3) || defined(STM32F4) uint8_t alternateFunction; #endif - uint8_t outputInverted; } timerHardware_t; +enum {TIMER_OUTPUT_ENABLED = 0x01, TIMER_OUTPUT_INVERTED = 0x02}; + #ifdef STM32F1 #if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) @@ -147,4 +148,4 @@ void timerForceOverflow(TIM_TypeDef *tim); void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration -rccPeriphTag_t timerRCC(TIM_TypeDef *tim); \ No newline at end of file +rccPeriphTag_t timerRCC(TIM_TypeDef *tim); diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index 7b7e6c163..ad85d7b9b 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -52,33 +52,33 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // 6 x 3 pin headers // - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 // // 6 pin header // // PWM7-10 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // // PPM PORT - Also USART2 RX (AF5) // - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_1, 0} // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 - //{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_IPD, GPIO_PinSource3, GPIO_AF_9, 0} // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + //{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_IPD, GPIO_PinSource3, GPIO_AF_9 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 // USART3 RX/TX // RX conflicts with PPM port - //{ TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_PinSource11, GPIO_AF_1, 0} // RX - PB11 - *TIM2_CH4, USART3_RX (AF7) - PWM11 - //{ TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_PinSource10, GPIO_AF_1, 0} // TX - PB10 - *TIM2_CH3, USART3_TX (AF7) - PWM12 + //{ TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_PinSource11, GPIO_AF_1 } // RX - PB11 - *TIM2_CH4, USART3_RX (AF7) - PWM11 + //{ TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_PinSource10, GPIO_AF_1 } // TX - PB10 - *TIM2_CH3, USART3_TX (AF7) - PWM12 }; diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index 054b27e81..e6024280f 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -73,19 +73,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM1 - PA8 RC1 - { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM2 - PB0 RC2 - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM3 - PB1 RC3 - { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM4 - PA14 RC4 - { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM5 - PA15 RC5 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 + { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 + { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 + { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 + { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4, 0}, // PWM6 - PB8 OUT1 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4, 0}, // PWM7 - PB9 OUT2 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, 0}, // PWM8 - PA0 OUT3 - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, 0}, // PWM9 - PA1 OUT4 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // PWM10 - PC6 OUT5 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // PWM11 - PC7 OUT6 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // PWM13 - PC8 OUT7 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // PWM13 - PC9 OUT8 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 }; diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index ff191e822..24df5a739 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -51,12 +51,12 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8, 0 }, // PPM IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, 0 }, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, 0 }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, 0 }, // S3_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9, 0 }, // S4_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, 0 }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, 0 }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // PPM IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S1_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S2_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S4_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT }; diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index abfe089aa..c685f7e35 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -128,17 +128,17 @@ const uint16_t airPWM_BP6[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD, 0}, // S1_IN - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD, 0}, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD, 0}, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD, 0}, // S4_IN - Current - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, 0}, // S5_IN - Vbattery - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, 0}, // S6_IN - PPM IN - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, 0}, // S1_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, 0}, // S2_OUT - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, 0}, // S3_OUT - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, 0}, // S4_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, 0}, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, 0} // S6_OUT + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // S1_IN + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // S4_IN - Current + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // S5_IN - Vbattery + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // S6_IN - PPM IN + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP }, // S1_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP }, // S2_OUT + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP }, // S3_OUT + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP }, // S4_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP } // S6_OUT }; diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 68e163dbf..132ff783f 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -73,23 +73,23 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // INPUTS CH1-8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PWM1 - PA8 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PWM2 - PB8 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PWM3 - PB9 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, 0}, // PWM4 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, 0}, // PWM5 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, 0}, // PWM6 - PC8 - { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3, 0}, // PWM7 - PF9 - { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3, 0}, // PWM8 - PF10 - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM9 - PD12 - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM10 - PD13 - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM11 - PD14 - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM12 - PD15 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM13 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM14 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM15 - PA3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM16 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM17 - PB1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0} // PWM18 - PA4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM2 - PB8 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM3 - PB9 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM4 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM5 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM6 - PC8 + { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3 }, // PWM7 - PF9 + { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3 }, // PWM8 - PF10 + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PD12 + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM10 - PD13 + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM11 - PD14 + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM12 - PD15 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM13 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM14 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM15 - PA3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM16 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM17 - PB1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 } // PWM18 - PA4 }; diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index b84a10454..926f3efa0 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -38,19 +38,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD, 0}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD, 0}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD, 0} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 }; diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 323a1e74d..675067e06 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -67,16 +67,16 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PC6 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PC7 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PMW4 - PC8 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM11 - PB15 }; diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 958339b1c..ebd5e6f1f 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -58,17 +58,17 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PWM1 - PA8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - PB9 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB9 - { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PMW4 - PA10 - { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM5 - PA9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM7 - PA1 + { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PMW4 - PA10 + { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM5 - PA9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PWM8 - PB1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PWM9 - PB0 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM8 - PB1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM9 - PB0 }; diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index 2527bc389..6e1b4ae6b 100644 --- a/src/main/target/EUSTM32F103RC/target.c +++ b/src/main/target/EUSTM32F103RC/target.c @@ -72,19 +72,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, 0 }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, 0 }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD, 0 }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, 0 }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD, 0 }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD, 0 }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD, 0 }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD, 0 }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD, 0 }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD, 0 }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD, 0 }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD, 0 }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD, 0 }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD, 0 } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 }; diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 656c75e6d..4dc593bb3 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -52,15 +52,15 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PPM IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PPM IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - S1 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - S2 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - S1 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - S2 { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM6 - S3 - { TIM16, IO_TAG(PB4), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM7 - S4 + { TIM16, IO_TAG(PB4), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - S4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO TIMER - LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO TIMER - LED_STRIP }; diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index b6dbf5fe7..c5bb0486c 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -42,13 +42,13 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PPM_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM_IN - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9, 0}, // S1_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, 0}, // S4_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S1_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT -// { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5, 0}, // LED Strip +// { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5 }, // LED Strip }; diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index 7728a6f4f..7207468fa 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -81,22 +81,22 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index db129454a..60f2a0736 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -48,18 +48,18 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, PWM_INVERTED}, - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, PWM_INVERTED}, - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, PWM_INVERTED}, - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, PWM_INVERTED}, - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, PWM_INVERTED}, - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, PWM_INVERTED}, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_6}, + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_2}, + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, - { TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_10,0}, - { TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_5, 0}, + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, + { TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_10}, + { TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_5}, }; diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index b665072ac..e26bb2950 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -66,16 +66,16 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PC6 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PC7 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PMW4 - PC8 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM11 - PB15 }; diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index 47789fd4f..b624db7cc 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -42,14 +42,14 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 9e35b3a17..110ce3140 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -72,19 +72,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD, 0}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD, 0}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD, 0} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 }; diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c index 35bf464de..1b75af422 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -72,19 +72,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD, 0}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD, 0}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD, 0} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 }; diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 1784dcf51..94250435a 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -66,24 +66,24 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent // Used by SPI1, MAX7456 - //{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - //{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 + //{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 + //{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 - PA1 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM5 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM6 - PA3 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA1 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM9 - PB10 - TIM2_CH3 / USART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM10 - PB11 - TIM2_CH4 / USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / USART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / USART3_RX (AF7) // LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index 47789fd4f..b624db7cc 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -42,14 +42,14 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 7e71ab1d6..42ce07bec 100644 --- a/src/main/target/PORT103R/target.c +++ b/src/main/target/PORT103R/target.c @@ -72,19 +72,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, 0}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD, 0}, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD, 0}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD, 0}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD, 0}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD, 0} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 }; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index be834daf3..b32c5175f 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -71,19 +71,19 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12, 0}, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12, 0}, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8, 0}, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8, 0}, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8, 0}, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8, 0}, // S6_IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3, 0}, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3, 0}, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM9, 0}, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM2, 0}, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5, 0}, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5, 0}, // S6_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 8e5db9def..53d404217 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -86,18 +86,18 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2, 0}, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // S2_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // S3_IN - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // S4_IN - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3, 0}, // S5_IN - { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2, 0}, // S6_IN - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // S1_OUT - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, 0}, // S2_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4, 0}, // S3_OUT - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4, 0}, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, 0}, // S5_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, 0}, // S6_OUT + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S4_IN + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_IN + { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S6_IN + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S1_OUT + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S2_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // S3_OUT + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index 7728a6f4f..eaeb18fca 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -81,22 +81,22 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index fe6106ec9..645bd323f 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -60,15 +60,15 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM6 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // SOFTSERIAL1 RX (NC) - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // SOFTSERIAL1 TX - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // SOFTSERIAL1 RX (NC) + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // SOFTSERIAL1 TX + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // LED_STRIP }; diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index 4ac409d39..dfe1517fb 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -49,13 +49,13 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 - PB6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - PB6 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - PB9 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB6 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB0 - *TIM3_CH3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB1 - *TIM3_CH4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 }; diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 41fe8f2b8..d5ecdcc96 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -48,18 +48,18 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // 6 3-pin headers - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 // PWM7 - PMW10 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, 0} // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ddebee117..122f07bb0 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -82,25 +82,25 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index a4b8890d9..1ac9743d3 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -67,17 +67,17 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PPM - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM3 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index 6a0262020..bc0d06cee 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -68,27 +68,27 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PPM - PB5 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 - PA1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM5 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM6 - PA3 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA1 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM9 - PB10 - TIM2_CH3 / USART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM10 - PB11 - TIM2_CH4 / USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / USART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / USART3_RX (AF7) // LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 1d2e615ea..dcf2426ce 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -73,19 +73,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PWM1 - PA8 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PWM2 - PB8 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, 0}, // PWM3 - PB9 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, 0}, // PWM4 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, 0}, // PWM5 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, 0}, // PWM6 - PC8 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PWM7 - PB1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PWM8 - PA2 - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM9 - PD12 - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM10 - PD13 - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM11 - PD14 - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM12 - PD15 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM13 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0} // PWM14 - PA2 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM2 - PB8 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM3 - PB9 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM4 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM5 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM6 - PC8 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM7 - PB1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM8 - PA2 + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PD12 + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM10 - PD13 + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM11 - PD14 + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM12 - PD15 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM13 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 } // PWM14 - PA2 }; diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index 9c0bd0403..f2ad959f8 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -82,24 +82,24 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index e3b40ee63..a3cfdc733 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -81,24 +81,24 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; From ceb9ef2db277098d721df482e6937ea7e589e413 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 30 Jun 2016 08:32:40 +0100 Subject: [PATCH 32/54] Added facility to set rx serial provider in for target. --- src/main/config/config.c | 30 +++++++++++++++----------- src/main/target/ALIENFLIGHTF3/target.h | 5 +++-- src/main/target/ALIENFLIGHTF4/target.h | 5 +++-- src/main/target/FURYF3/target.h | 4 ++-- src/main/target/FURYF4/target.h | 7 +++--- src/main/target/KISSFC/target.h | 5 +++-- src/main/target/NAZE/target.h | 16 ++++++++------ src/main/target/SPRACINGF3/target.h | 4 ++-- 8 files changed, 42 insertions(+), 34 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 284c59097..5bcd1e923 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -268,8 +268,13 @@ void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) { +#ifdef BRUSHED_MOTORS + escAndServoConfig->minthrottle = 1000; + escAndServoConfig->maxthrottle = 2000; +#else escAndServoConfig->minthrottle = 1150; escAndServoConfig->maxthrottle = 1850; +#endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; escAndServoConfig->escDesyncProtection = 0; @@ -415,8 +420,6 @@ uint16_t getCurrentMinthrottle(void) // Default settings static void resetConf(void) { - int i; - // Clear all configuration memset(&masterConfig, 0, sizeof(master_t)); setProfile(0); @@ -477,7 +480,11 @@ static void resetConf(void) resetTelemetryConfig(&masterConfig.telemetryConfig); +#ifdef SERIALRX_PROVIDER + masterConfig.rxConfig.serialrx_provider = SERIALRX_PROVIDER; +#else masterConfig.rxConfig.serialrx_provider = 0; +#endif masterConfig.rxConfig.sbus_inversion = 1; masterConfig.rxConfig.spektrum_sat_bind = 0; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; @@ -487,7 +494,7 @@ static void resetConf(void) masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection - for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { + for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i]; channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); @@ -549,8 +556,7 @@ static void resetConf(void) resetPidProfile(¤tProfile->pidProfile); - uint8_t rI; - for (rI = 0; rI Date: Fri, 1 Jul 2016 01:30:04 -0700 Subject: [PATCH 33/54] 0x00 is reserved for string null byte termination, move the voltage symbol to 0x06 --- src/main/drivers/max7456_symbols.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index 2030cb956..b2e8da72b 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -176,7 +176,7 @@ #define SYM_FT 0x0F // Voltage and amperage -#define SYM_VOLT 0x00 +#define SYM_VOLT 0x06 #define SYM_AMP 0x9A #define SYM_MAH 0xA4 #define SYM_WATT 0x57 From d0f060175cda9cf3ca047fbf5f986cec3d827366 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 1 Jul 2016 12:10:45 +0200 Subject: [PATCH 34/54] Ignore linker wchar size warnings. --- Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Makefile b/Makefile index b8c891ef6..e41d2d19e 100644 --- a/Makefile +++ b/Makefile @@ -600,6 +600,7 @@ LDFLAGS = -lm \ -Wl,-gc-sections,-Map,$(TARGET_MAP) \ -Wl,-L$(LINKER_DIR) \ -Wl,--cref \ + -Wl,--no-wchar-size-warning \ -T$(LD_SCRIPT) ############################################################################### From 924c891bf1f26000b60b6411882aeb74062b9706 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 2 Jul 2016 14:38:04 +1000 Subject: [PATCH 35/54] Update to LED STRIP to use new IO --- src/main/config/config.c | 4 +- src/main/drivers/light_ws2811strip.c | 16 +- .../drivers/light_ws2811strip_stm32f10x.c | 52 +++--- .../drivers/light_ws2811strip_stm32f30x.c | 61 +++---- .../drivers/light_ws2811strip_stm32f4xx.c | 150 ++++++++++++------ src/main/drivers/pwm_mapping.c | 18 +-- src/main/drivers/timer.c | 12 ++ src/main/drivers/timer.h | 12 +- src/main/drivers/timer_stm32f30x.c | 20 +-- src/main/drivers/timer_stm32f4xx.c | 28 ++-- src/main/io/ledstrip.c | 8 +- src/main/io/ledstrip.h | 2 +- src/main/io/serial_msp.c | 2 +- src/main/scheduler/scheduler_tasks.c | 4 +- src/main/target/ALIENFLIGHTF4/target.h | 8 +- src/main/target/BLUEJAYF4/target.h | 13 ++ src/main/target/BLUEJAYF4/target.mk | 4 +- src/main/target/CC3D/target.h | 9 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- src/main/target/COLIBRI_RACE/target.h | 9 +- src/main/target/DOGE/target.h | 8 +- src/main/target/EUSTM32F103RC/target.h | 3 - src/main/target/FURYF3/target.h | 8 +- src/main/target/LUX_RACE/target.h | 9 +- src/main/target/MOTOLAB/target.h | 8 +- src/main/target/NAZE/target.h | 3 +- src/main/target/OLIMEXINO/target.h | 6 - src/main/target/OMNIBUS/target.h | 8 +- src/main/target/PIKOBLX/target.h | 8 +- src/main/target/PORT103R/target.h | 3 - src/main/target/REVO/target.h | 2 - src/main/target/REVONANO/target.h | 6 - src/main/target/RMDO/target.h | 8 +- src/main/target/SINGULARITY/target.h | 8 +- src/main/target/SPARKY/target.h | 9 +- src/main/target/SPRACINGF3/target.h | 8 +- src/main/target/SPRACINGF3EVO/target.h | 9 +- src/main/target/SPRACINGF3MINI/target.h | 9 +- src/main/target/STM32F3DISCOVERY/target.h | 8 +- src/main/target/X_RACERSPI/target.h | 8 +- 40 files changed, 277 insertions(+), 296 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 5bcd1e923..43820350d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -848,10 +848,10 @@ void validateAndFixConfig(void) if (featureConfigured(FEATURE_SOFTSERIAL) && ( 0 #ifdef USE_SOFTSERIAL1 - || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER) + || (WS2811_TIMER == SOFTSERIAL_1_TIMER) #endif #ifdef USE_SOFTSERIAL2 - || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER) + || (WS2811_TIMER == SOFTSERIAL_2_TIMER) #endif )) { // led strip needs the same timer as softserial diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 3821350f5..5bd5857b5 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -80,18 +80,9 @@ void setStripColors(const hsvColor_t *colors) } } -void ws2811DMAHandler(DMA_Channel_TypeDef *channel) { - if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) { - ws2811LedDataTransferInProgress = 0; - DMA_Cmd(channel, DISABLE); - DMA_ClearFlag(WS2811_DMA_TC_FLAG); - } -} - void ws2811LedStripInit(void) { memset(&ledStripDMABuffer, 0, WS2811_DMA_BUFFER_SIZE); - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler); ws2811LedStripHardwareInit(); ws2811UpdateStrip(); } @@ -141,12 +132,11 @@ STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue) */ void ws2811UpdateStrip(void) { - static uint32_t waitCounter = 0; static rgbColor24bpp_t *rgb24; - // wait until previous transfer completes - while(ws2811LedDataTransferInProgress) { - waitCounter++; + // don't wait - risk of infinite block, just get an update next time round + if (ws2811LedDataTransferInProgress) { + return; } dmaBufferOffset = 0; // reset buffer memory index diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index b7f1bd413..40003663d 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -23,35 +23,41 @@ #include "common/color.h" #include "drivers/light_ws2811strip.h" #include "nvic.h" +#include "io.h" +#include "dma.h" +#include "timer.h" + +#ifdef LED_STRIP + +static IO_t ws2811IO = IO_NONE; +bool ws2811Initialised = false; + +void ws2811DMAHandler(DMA_Channel_TypeDef *channel) +{ + if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) { + ws2811LedDataTransferInProgress = 0; + DMA_Cmd(channel, DISABLE); + DMA_ClearFlag(WS2811_DMA_TC_FLAG); + } +} void ws2811LedStripHardwareInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; - GPIO_InitTypeDef GPIO_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint16_t prescalerValue; -#ifdef CC3D - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOB, &GPIO_InitStructure); -#else - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - - /* GPIOA Configuration: TIM3 Channel 1 as alternate function push-pull */ - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOA, &GPIO_InitStructure); -#endif - - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler); + + ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); +/* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(ws2811IO, IO_CONFIG(GPIO_Speed_50MHz, GPIO_Mode_AF_PP)); + + RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + /* Compute the prescaler value */ prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; /* Time base configuration */ @@ -108,16 +114,20 @@ void ws2811LedStripHardwareInit(void) NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); + ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) { + if (!ws2811Initialised) + return; + DMA_SetCurrDataCounter(DMA1_Channel6, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred TIM_SetCounter(TIM3, 0); TIM_Cmd(TIM3, ENABLE); DMA_Cmd(DMA1_Channel6, ENABLE); } - +#endif \ No newline at end of file diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index fbcc8d3fb..567fd0b8e 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -20,51 +20,54 @@ #include -#include "gpio.h" +#include "io.h" #include "nvic.h" #include "common/color.h" #include "drivers/light_ws2811strip.h" +#include "dma.h" +#include "rcc.h" +#include "timer.h" -#ifndef WS2811_GPIO -#define WS2811_GPIO GPIOB -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#ifdef LED_STRIP + +#ifndef WS2811_PIN +#define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER - #endif +static IO_t ws2811IO = IO_NONE; +bool ws2811Initialised = false; + +void ws2811DMAHandler(DMA_Channel_TypeDef *channel) +{ + if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) { + ws2811LedDataTransferInProgress = 0; + DMA_Cmd(channel, DISABLE); + DMA_ClearFlag(WS2811_DMA_TC_FLAG); + } +} + void ws2811LedStripHardwareInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; - GPIO_InitTypeDef GPIO_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint16_t prescalerValue; - - RCC_AHBPeriphClockCmd(WS2811_GPIO_AHB_PERIPHERAL, ENABLE); - - GPIO_PinAFConfig(WS2811_GPIO, WS2811_PIN_SOURCE, WS2811_GPIO_AF); - - /* Configuration alternate function push-pull */ - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = WS2811_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(WS2811_GPIO, &GPIO_InitStructure); - - - RCC_APB2PeriphClockCmd(WS2811_TIMER_APB2_PERIPHERAL, ENABLE); + + dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler); + + ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerGPIOAF(WS2811_TIMER)); + + RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); /* Compute the prescaler value */ prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; @@ -122,16 +125,20 @@ void ws2811LedStripHardwareInit(void) NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); + ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) { + if (!ws2811Initialised) + return; + DMA_SetCurrDataCounter(WS2811_DMA_CHANNEL, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred TIM_SetCounter(WS2811_TIMER, 0); TIM_Cmd(WS2811_TIMER, ENABLE); DMA_Cmd(WS2811_DMA_CHANNEL, ENABLE); } - +#endif \ No newline at end of file diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index d85f1c575..7bc139c32 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -22,64 +22,122 @@ #include "common/color.h" #include "light_ws2811strip.h" +#include "dma.h" #include "nvic.h" +#include "io.h" +#include "system.h" +#include "rcc.h" +#include "timer.h" #ifdef LED_STRIP + +#if !defined(WS2811_PIN) +#define WS2811_PIN PA0 +#define WS2811_TIMER TIM5 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream2 +#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 +#define WS2811_DMA_IT DMA_IT_TCIF2 +#define WS2811_DMA_CHANNEL DMA_Channel_6 +#define WS2811_DMA_IRQ DMA1_Stream2_IRQn +#define WS2811_TIMER_CHANNEL TIM_Channel_1 +#endif + +static IO_t ws2811IO = IO_NONE; +static uint16_t timDMASource = 0; +bool ws2811Initialised = false; + +void ws2811DMAHandler(DMA_Stream_TypeDef *stream) +{ + if (DMA_GetFlagStatus(stream, WS2811_DMA_FLAG)) { + ws2811LedDataTransferInProgress = 0; + DMA_ClearITPendingBit(stream, WS2811_DMA_IT); + DMA_Cmd(stream, DISABLE); + TIM_DMACmd(WS2811_TIMER, timDMASource, DISABLE); + } +} + void ws2811LedStripHardwareInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; - GPIO_InitTypeDef GPIO_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint16_t prescalerValue; - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE); - + RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; - GPIO_Init(GPIOA, &GPIO_InitStructure); - - GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM5); - - + IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerGPIOAF(WS2811_TIMER)); + // Stop timer - TIM_Cmd(TIM5, DISABLE); + TIM_Cmd(WS2811_TIMER, DISABLE); /* Compute the prescaler value */ - prescalerValue = (uint16_t) (SystemCoreClock / 2 / 84000000) - 1; + prescalerValue = (uint16_t)(SystemCoreClock / 2 / 84000000) - 1; + /* Time base configuration */ TIM_TimeBaseStructure.TIM_Period = 104; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; TIM_OCInitStructure.TIM_Pulse = 0; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - TIM_OC1Init(TIM5, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable); + + uint32_t channelAddress = 0; + switch (WS2811_TIMER_CHANNEL) + { + case TIM_Channel_1: + TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC1; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR1); + TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC2; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR2); + TIM_OC2PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC3; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR3); + TIM_OC3PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC4; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR4); + TIM_OC4PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + } + + TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE); + TIM_ARRPreloadConfig(WS2811_TIMER, ENABLE); - TIM_Cmd(TIM5, ENABLE); + TIM_CCxCmd(WS2811_TIMER, WS2811_TIMER_CHANNEL, TIM_CCx_Enable); + TIM_Cmd(WS2811_TIMER, ENABLE); + dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler); /* configure DMA */ - /* DMA1 Channel Config */ - DMA_Cmd(DMA1_Stream2, DISABLE); // disable DMA channel 6 - DMA_DeInit(DMA1_Stream2); + DMA_Cmd(WS2811_DMA_STREAM, DISABLE); + DMA_DeInit(WS2811_DMA_STREAM); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_Channel = DMA_Channel_6; - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(TIM5->CCR1); + DMA_InitStructure.DMA_Channel = WS2811_DMA_CHANNEL; + DMA_InitStructure.DMA_PeripheralBaseAddr = channelAddress; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; @@ -93,38 +151,36 @@ void ws2811LedStripHardwareInit(void) DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull; DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(DMA1_Stream2, &DMA_InitStructure); - DMA_ITConfig(DMA1_Stream2, DMA_IT_TC, ENABLE); - DMA_ClearITPendingBit(DMA1_Stream2, DMA_IT_TCIF2); // clear DMA1 Channel 6 transfer complete flag + + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE); + DMA_Init(WS2811_DMA_STREAM, &DMA_InitStructure); + DMA_ITConfig(WS2811_DMA_STREAM, DMA_IT_TC, ENABLE); + DMA_ClearITPendingBit(WS2811_DMA_STREAM, WS2811_DMA_IT); + NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream2_IRQn; + NVIC_InitStructure.NVIC_IRQChannel = WS2811_DMA_IRQ; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_WS2811_DMA); NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_WS2811_DMA); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); + ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); } -void DMA1_Stream2_IRQHandler(void) -{ - if (DMA_GetFlagStatus(DMA1_Stream2, DMA_FLAG_TCIF2)) { - ws2811LedDataTransferInProgress = 0; - DMA_Cmd(DMA1_Stream2, DISABLE); - TIM_DMACmd(TIM5, TIM_DMA_CC1, DISABLE); - DMA_ClearITPendingBit(DMA1_Stream2, DMA_IT_TCIF2); - } -} - void ws2811LedStripDMAEnable(void) { - DMA_SetCurrDataCounter(DMA1_Stream2, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(TIM5, 0); - DMA_Cmd(DMA1_Stream2, ENABLE); - TIM_DMACmd(TIM5, TIM_DMA_CC1, ENABLE); + if (!ws2811Initialised) + return; + + DMA_SetCurrDataCounter(WS2811_DMA_STREAM, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(WS2811_TIMER, 0); + DMA_Cmd(WS2811_DMA_STREAM, ENABLE); + TIM_DMACmd(WS2811_TIMER, timDMASource, ENABLE); } -#endif + +#endif \ No newline at end of file diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 8d332ba56..5e797ea2f 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -157,33 +157,33 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; #endif -#ifdef LED_STRIP_TIMER +#ifdef WS2811_TIMER // skip LED Strip output if (init->useLEDStrip) { - if (timerHardwarePtr->tim == LED_STRIP_TIMER) + if (timerHardwarePtr->tim == WS2811_TIMER) continue; -#if defined(STM32F303xC) && defined(WS2811_GPIO) && defined(WS2811_PIN_SOURCE) - if (CheckGPIOPinSource(timerHardwarePtr->tag, WS2811_GPIO, WS2811_PIN_SOURCE)) +#if defined(STM32F303xC) && defined(WS2811_PIN) + if (timerHardwarePtr->tag == IO_TAG(WS2811_PIN)) continue; #endif } #endif -#ifdef VBAT_ADC_GPIO - if (init->useVbat && CheckGPIOPin(timerHardwarePtr->tag, VBAT_ADC_GPIO, VBAT_ADC_GPIO_PIN)) { +#ifdef VBAT_ADC_PIN + if (init->useVbat && timerHardwarePtr->tag == IO_TAG(VBAT_ADC_PIN)) { continue; } #endif #ifdef RSSI_ADC_GPIO - if (init->useRSSIADC && CheckGPIOPin(timerHardwarePtr->tag, RSSI_ADC_GPIO, RSSI_ADC_GPIO_PIN)) { + if (init->useRSSIADC && timerHardwarePtr->tag == IO_TAG(RSSI_ADC_PIN)) { continue; } #endif #ifdef CURRENT_METER_ADC_GPIO - if (init->useCurrentMeterADC && CheckGPIOPin(timerHardwarePtr->tag, CURRENT_METER_ADC_GPIO, CURRENT_METER_ADC_GPIO_PIN)) { + if (init->useCurrentMeterADC && timerHardwarePtr->tag == IO_TAG(CURRENT_METER_ADC_PIN)) { continue; } #endif @@ -274,7 +274,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } if (init->useChannelForwarding && !init->airplane) { -#if defined(NAZE) && defined(LED_STRIP_TIMER) +#if defined(NAZE) && defined(WS2811_TIMER) // if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14 if (init->useLEDStrip) { if (timerIndex >= PWM13 && timerIndex <= PWM14) { diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 37067ae25..e168a46df 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -151,6 +151,18 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) return 0; } +#if defined(STM32F3) || defined(STM32F4) +uint8_t timerGPIOAF(TIM_TypeDef *tim) +{ + for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].alternateFunction; + } + } + return 0; +} +#endif + void timerNVICConfigure(uint8_t irq) { NVIC_InitTypeDef NVIC_InitStructure; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index cdc461c69..26078d451 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -69,6 +69,9 @@ typedef struct timerOvrHandlerRec_s { typedef struct timerDef_s { TIM_TypeDef *TIMx; rccPeriphTag_t rcc; +#if defined(STM32F3) || defined(STM32F4) + uint8_t alternateFunction; +#endif } timerDef_t; typedef struct timerHardware_s { @@ -82,8 +85,11 @@ typedef struct timerHardware_s { uint8_t alternateFunction; #endif } timerHardware_t; -enum {TIMER_OUTPUT_ENABLED = 0x01, TIMER_OUTPUT_INVERTED = 0x02}; +enum { + TIMER_OUTPUT_ENABLED = 0x01, + TIMER_OUTPUT_INVERTED = 0x02 +}; #ifdef STM32F1 #if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) @@ -149,3 +155,7 @@ void timerForceOverflow(TIM_TypeDef *tim); void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration rccPeriphTag_t timerRCC(TIM_TypeDef *tim); + +#if defined(STM32F3) || defined(STM32F4) +uint8_t timerGPIOAF(TIM_TypeDef *tim); +#endif diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 4bce8a70f..baad75634 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -10,16 +10,16 @@ #include "timer.h" const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1) }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2) }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3) }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4) }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) }, - { .TIMx = TIM15, .rcc = RCC_APB2(TIM15) }, - { .TIMx = TIM16, .rcc = RCC_APB2(TIM16) }, - { .TIMx = TIM17, .rcc = RCC_APB2(TIM17) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_6 }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_1 }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_2 }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_10 }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_5 }, + { .TIMx = TIM15, .rcc = RCC_APB2(TIM15), GPIO_AF_9 }, + { .TIMx = TIM16, .rcc = RCC_APB2(TIM16), GPIO_AF_1 }, + { .TIMx = TIM17, .rcc = RCC_APB2(TIM17), GPIO_AF_1 }, }; diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index 8916a526c..374e9164b 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -35,20 +35,20 @@ #define CCMR_Offset ((uint16_t)0x0018) const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1) }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2) }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3) }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4) }, - { .TIMx = TIM5, .rcc = RCC_APB1(TIM5) }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) }, - { .TIMx = TIM9, .rcc = RCC_APB2(TIM9) }, - { .TIMx = TIM10, .rcc = RCC_APB2(TIM10) }, - { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, - { .TIMx = TIM12, .rcc = RCC_APB1(TIM12) }, - { .TIMx = TIM13, .rcc = RCC_APB1(TIM13) }, - { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_TIM1 }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_TIM2 }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_TIM3 }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_TIM4 }, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), GPIO_AF_TIM5 }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_TIM8 }, + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), GPIO_AF_TIM9 }, + { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), GPIO_AF_TIM10 }, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), GPIO_AF_TIM11 }, + { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), GPIO_AF_TIM12 }, + { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), GPIO_AF_TIM13 }, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), GPIO_AF_TIM14 }, }; void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 061f0d798..863f3a8b6 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -440,7 +440,7 @@ void updateLedCount(void) } } -void reevalulateLedConfig(void) +void reevaluateLedConfig(void) { updateLedCount(); determineLedStripDimensions(); @@ -534,7 +534,7 @@ bool parseLedStripConfig(uint8_t ledIndex, const char *config) memset(ledConfig, 0, sizeof(ledConfig_t)); } - reevalulateLedConfig(); + reevaluateLedConfig(); return ok; } @@ -1095,7 +1095,7 @@ void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t)); memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); - reevalulateLedConfig(); + reevaluateLedConfig(); } void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) @@ -1107,7 +1107,7 @@ void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) void ledStripEnable(void) { - reevalulateLedConfig(); + reevaluateLedConfig(); ledStripInitialised = true; ws2811LedStripInit(); diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 5f901e46c..18915229b 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -94,4 +94,4 @@ bool parseColor(uint8_t index, const char *colorConfig); void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); void ledStripEnable(void); -void reevalulateLedConfig(void); +void reevaluateLedConfig(void); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9f2609c0a..b616605ac 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1807,7 +1807,7 @@ static bool processInCommand(void) ledConfig->color = read8(); - reevalulateLedConfig(); + reevaluateLedConfig(); } break; #endif diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 721ad27eb..18299e490 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -168,8 +168,8 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_LEDSTRIP] = { .taskName = "LEDSTRIP", .taskFunc = taskLedStrip, - .desiredPeriod = 1000000 / 100, // 100 Hz - .staticPriority = TASK_PRIORITY_IDLE, + .desiredPeriod = 1000000 / 10, // 10 Hz + .staticPriority = TASK_PRIORITY_LOW, }, #endif diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index d5122a486..5c5e7795f 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -146,16 +146,10 @@ // LED strip configuration using RC5 pin. //#define LED_STRIP -//#define LED_STRIP_TIMER TIM8 //#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -//#define WS2811_GPIO GPIOB -//#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -//#define WS2811_GPIO_AF GPIO_AF_3 -//#define WS2811_PIN GPIO_Pin_15 // TIM8_CH3 -//#define WS2811_PIN_SOURCE GPIO_PinSource15 +//#define WS2811_PIN PB15 // TIM8_CH3 //#define WS2811_TIMER TIM8 -//#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM8 //#define WS2811_DMA_CHANNEL DMA1_Channel3 //#define WS2811_IRQ DMA1_Channel3_IRQn diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 7160da98f..8d9d1c633 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -134,6 +134,19 @@ #define USE_ADC #define VBAT_ADC_PIN PC3 +#define LED_STRIP +// LED Strip can run off Pin 6 (PB1) of the ESC outputs. +#define WS2811_PIN PB1 +#define WS2811_TIMER TIM3 +#define WS2811_TIMER_CHANNEL TIM_Channel_4 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream2 +#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 +#define WS2811_DMA_IT DMA_IT_TCIF2 +#define WS2811_DMA_CHANNEL DMA_Channel_5 +#define WS2811_DMA_IRQ DMA1_Stream2_IRQn + + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index 9f2308666..f52396562 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -4,5 +4,7 @@ FEATURES += SDCARD VCP TARGET_SRC = \ drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_mpu6500.c \ - drivers/barometer_ms5611.c + drivers/barometer_ms5611.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index a0415670d..8d6e6afbc 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -92,12 +92,13 @@ #define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA0 -#define RSSI_ADC_PIN PB0 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA0 +#define RSSI_ADC_PIN PB0 #define LED_STRIP -#define LED_STRIP_TIMER TIM3 +#define WS2811_PIN PB4 +#define WS2811_TIMER TIM3 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 10c99ccc0..be0764a03 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1403,7 +1403,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ledConfig->color = bstRead8(); - reevalulateLedConfig(); + reevaluateLedConfig(); } break; #endif diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 976b287a0..2822227df 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -122,15 +122,8 @@ #define LED_STRIP #define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG -#define LED_STRIP_TIMER TIM16 - -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 44a16782b..7ca509ea2 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -140,15 +140,9 @@ #define LED_STRIP // tqfp48 pin 16 -#define LED_STRIP_TIMER TIM16 #define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index d6d916bb2..0869dd46b 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -102,9 +102,6 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 -//#define LED_STRIP -#define LED_STRIP_TIMER TIM3 - #define SPEKTRUM_BIND // USART2, PA3 #define BIND_PIN PA3 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 9075271c1..db4ddd745 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -156,16 +156,10 @@ #define CURRENT_METER_ADC_PIN PA2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 6292d2435..cef9b31b0 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -95,15 +95,8 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define LED_STRIP_TIMER TIM16 - -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 27bf98691..183e1326d 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -126,16 +126,10 @@ #define LED_STRIP #if 1 -#define LED_STRIP_TIMER TIM16 #define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_GPIO GPIOB -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index df73f8e4f..6de152cb8 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -155,7 +155,8 @@ #define LED_STRIP -#define LED_STRIP_TIMER TIM3 +#define WS2811_TIMER TIM3 +#define WS2811_PIN PA6 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index b7972ad77..3ff23fd8d 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -78,18 +78,12 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) -// #define SOFT_I2C // enable to test software i2c -// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) -// #define SOFT_I2C_PB67 - #define USE_ADC #define CURRENT_METER_ADC_PIN PB1 #define VBAT_ADC_PIN PA4 #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM3 // IO - assuming all IOs on smt32f103rb LQFP64 package #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index c9eb4a59d..76d995fce 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -166,15 +166,9 @@ #define LED_STRIP -#define LED_STRIP_TIMER TIM1 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 8b14570c9..36ee21772 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -94,16 +94,10 @@ #define LED_STRIP #if 1 -#define LED_STRIP_TIMER TIM16 #define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_GPIO GPIOB -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 64e0b6455..d5d0850d9 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -115,9 +115,6 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM3 - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f103RCT6 in 64pin package diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index d2124e5b2..3a67944b9 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -110,8 +110,6 @@ #define SENSORS_SET (SENSOR_ACC) -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM5 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 6f4a2c431..14912fffc 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -85,12 +85,6 @@ #define VBAT_ADC_PIN PA6 #define RSSI_ADC_PIN PA5 - -//#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG) - -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM5 - #define GPS #define BLACKBOX #define TELEMETRY diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 779b8ab6c..602609862 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -112,16 +112,10 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index f58dcbe96..4d047843e 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -98,16 +98,10 @@ #define VBAT_SCALE_DEFAULT 77 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 58101f0b8..0913d0ec1 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -97,16 +97,9 @@ #define LED_STRIP #if 1 // LED strip configuration using PWM motor output pin 5. -#define LED_STRIP_TIMER TIM16 - #define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 7b2a365a5..840dda225 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -121,16 +121,10 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index c4a9a0484..4adbe0d56 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -142,16 +142,9 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 - #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 93e220420..61b4bd9b5 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -150,15 +150,8 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 - -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 947ad4913..7664b47e0 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -146,14 +146,8 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define LED_STRIP_TIMER TIM16 -#define WS2811_GPIO GPIOB -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 39c2e6c20..217765adc 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -124,16 +124,10 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 From 9ed87c0d9acbe3556460516999fc4c10ee121b41 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 2 Jul 2016 14:38:46 +1000 Subject: [PATCH 36/54] FuryF3 target.c extra item in initialise --- src/main/target/FURYF3/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 4dc593bb3..cfbced9c1 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -58,7 +58,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - S1 { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - S2 - { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM6 - S3 + { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10}, // PWM6 - S3 { TIM16, IO_TAG(PB4), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - S4 { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO TIMER - LED_STRIP From 6df2b6db8d4259576f5cd77726af0f31f2514444 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 2 Jul 2016 17:23:58 +1000 Subject: [PATCH 37/54] Cleaned up USB VCP buffers --- src/main/drivers/io.h | 4 ++++ src/main/io/serial_4way.c | 27 ++++++++++++++------------- src/main/vcpf4/usbd_cdc_vcp.c | 34 ++++++++++++++-------------------- src/main/vcpf4/usbd_cdc_vcp.h | 6 +++--- 4 files changed, 35 insertions(+), 36 deletions(-) diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index debc78d95..121e7a0e1 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -40,18 +40,21 @@ typedef uint8_t ioConfig_t; // packed IO configuration #define IO_CONFIG(mode, speed) ((mode) | (speed)) #define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_2MHz) +#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_25MHz) #define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_Out_OD, GPIO_Speed_2MHz) #define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF_PP, GPIO_Speed_2MHz) #define IOCFG_AF_OD IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_2MHz) #define IOCFG_IPD IO_CONFIG(GPIO_Mode_IPD, GPIO_Speed_2MHz) #define IOCFG_IPU IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_2MHz) #define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_2MHz) +#define IOCFG_IPU_25 IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_25MHz) #elif defined(STM32F3) || defined(STM32F4) #define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5)) #define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO +#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP_PD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_DOWN) @@ -60,6 +63,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration #define IOCFG_IPD IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_DOWN) #define IOCFG_IPU IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_UP) #define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_NOPULL) +#define IOCFG_IPU_25 IO_CONFIG(GPIO_Mode_IN, GPIO_Speed_25MHz, 0, GPIO_PuPd_UP) #elif defined(UNIT_TEST) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 6d96a36ec..81742c96f 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -44,15 +44,15 @@ #define USE_TXRX_LED #if defined(USE_TXRX_LED) && defined(LED0) -# define RX_LED_OFF LED0_OFF -# define RX_LED_ON LED0_ON -# ifdef LED1 -# define TX_LED_OFF LED1_OFF -# define TX_LED_ON LED1_ON -# else -# define TX_LED_OFF LED0_OFF -# define TX_LED_ON LED0_ON -# endif +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON +#ifdef LED1 +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON +#else +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON +#endif #else # define RX_LED_OFF do {} while(0) # define RX_LED_ON do {} while(0) @@ -120,12 +120,12 @@ void setEscLo(uint8_t selEsc) void setEscInput(uint8_t selEsc) { - IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU); + IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU_25); } void setEscOutput(uint8_t selEsc) { - IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP); + IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP_25); } // Initialize 4way ESC interface @@ -330,6 +330,7 @@ void esc4wayProcess(serialPort_t *serial) while(!esc4wayExitRequested) { // restart looking for new sequence from host crcIn = 0; + uint8_t esc = readByteCrc(); if(esc != cmd_Local_Escape) continue; // wait for sync character @@ -348,7 +349,6 @@ void esc4wayProcess(serialPort_t *serial) paramBuf[i] = readByteCrc(); readByteCrc(); readByteCrc(); // update input CRC - RX_LED_OFF; outLen = 0; // output handling code will send single zero byte if necessary replyAck = esc4wayAck_OK; @@ -356,6 +356,7 @@ void esc4wayProcess(serialPort_t *serial) if(crcIn != 0) // CRC of correct message == 0 replyAck = esc4wayAck_I_INVALID_CRC; + TX_LED_ON; if (replyAck == esc4wayAck_OK) replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen); @@ -365,8 +366,8 @@ void esc4wayProcess(serialPort_t *serial) outLen = 1; } + RX_LED_OFF; crcOut = 0; - TX_LED_ON; serialBeginWrite(port); writeByteCrc(cmd_Remote_Escape); writeByteCrc(command); diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 93ac1aaa5..e4681418b 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -170,20 +170,16 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) */ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) { - uint16_t ptr = APP_Rx_ptr_in; - uint32_t i; - - for (i = 0; i < Len; i++) - APP_Rx_Buffer[ptr++ & (APP_RX_DATA_SIZE-1)] = Buf[i]; - - APP_Rx_ptr_in = ptr % APP_RX_DATA_SIZE; - + for (uint32_t i = 0; i < Len; i++) { + APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; + APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; + } return USBD_OK; } uint8_t usbAvailable(void) { - return (usbData.rxBufHead != usbData.rxBufTail); + return (usbData.bufferInPosition != usbData.bufferOutPosition); } /******************************************************************************* @@ -198,8 +194,8 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) uint32_t ch = 0; while (usbAvailable() && ch < len) { - recvBuf[ch] = usbData.rxBuf[usbData.rxBufTail]; - usbData.rxBufTail = (usbData.rxBufTail + 1) % USB_RX_BUFSIZE; + recvBuf[ch] = usbData.buffer[usbData.bufferOutPosition]; + usbData.bufferOutPosition = (usbData.bufferOutPosition + 1) % USB_RX_BUFSIZE; ch++; receiveLength--; } @@ -224,18 +220,16 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) */ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) { - uint16_t ptr = usbData.rxBufHead; - uint32_t i; + __disable_irq(); - for (i = 0; i < Len; i++) - usbData.rxBuf[ptr++ & (USB_RX_BUFSIZE-1)] = Buf[i]; + for (uint32_t i = 0; i < Len; i++) { + usbData.buffer[usbData.bufferInPosition] = Buf[i]; + usbData.bufferInPosition = (usbData.bufferInPosition + 1) % USB_RX_BUFSIZE; + receiveLength++; + } - usbData.rxBufHead = ptr % USB_RX_BUFSIZE; + __enable_irq(); - receiveLength = ((usbData.rxBufHead - usbData.rxBufTail) > 0 ? - (usbData.rxBufHead - usbData.rxBufTail) : - (usbData.rxBufHead + USB_RX_BUFSIZE - usbData.rxBufTail)) % USB_RX_BUFSIZE; - if(receiveLength > (USB_RX_BUFSIZE-1)) return USBD_FAIL; diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 3ca22aa4e..7cc0f8feb 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -71,9 +71,9 @@ typedef struct } LINE_CODING; typedef struct { - uint8_t rxBuf[USB_RX_BUFSIZE]; - uint16_t rxBufHead; - uint16_t rxBufTail; + uint8_t buffer[USB_RX_BUFSIZE]; + uint16_t bufferInPosition; + uint16_t bufferOutPosition; } usbStruct_t; From 7d38b510a4929efff1d380569431caccc04dff59 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 2 Jul 2016 17:30:05 +1000 Subject: [PATCH 38/54] Tabs to spaces --- src/main/drivers/timer_stm32f4xx.c | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index 374e9164b..9b81d936a 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -35,20 +35,20 @@ #define CCMR_Offset ((uint16_t)0x0018) const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_TIM1 }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_TIM2 }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_TIM3 }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_TIM4 }, - { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), GPIO_AF_TIM5 }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_TIM8 }, - { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), GPIO_AF_TIM9 }, - { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), GPIO_AF_TIM10 }, - { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), GPIO_AF_TIM11 }, - { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), GPIO_AF_TIM12 }, - { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), GPIO_AF_TIM13 }, - { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), GPIO_AF_TIM14 }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_TIM1 }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_TIM2 }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_TIM3 }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_TIM4 }, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), GPIO_AF_TIM5 }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_TIM8 }, + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), GPIO_AF_TIM9 }, + { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), GPIO_AF_TIM10 }, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), GPIO_AF_TIM11 }, + { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), GPIO_AF_TIM12 }, + { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), GPIO_AF_TIM13 }, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), GPIO_AF_TIM14 }, }; void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode) From afea1d6123f087b8cb87dc69010144e12abad66d Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 2 Jul 2016 19:37:39 +1000 Subject: [PATCH 39/54] Formatting based on feedback --- .../drivers/light_ws2811strip_stm32f4xx.c | 51 ++++++++-------- src/main/drivers/timer_stm32f4xx.c | 61 +++++++++---------- 2 files changed, 54 insertions(+), 58 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 7bc139c32..f1d04dfbe 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -96,32 +96,31 @@ void ws2811LedStripHardwareInit(void) TIM_OCInitStructure.TIM_Pulse = 0; uint32_t channelAddress = 0; - switch (WS2811_TIMER_CHANNEL) - { - case TIM_Channel_1: - TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure); - timDMASource = TIM_DMA_CC1; - channelAddress = (uint32_t)(&WS2811_TIMER->CCR1); - TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(WS2811_TIMER, &TIM_OCInitStructure); - timDMASource = TIM_DMA_CC2; - channelAddress = (uint32_t)(&WS2811_TIMER->CCR2); - TIM_OC2PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(WS2811_TIMER, &TIM_OCInitStructure); - timDMASource = TIM_DMA_CC3; - channelAddress = (uint32_t)(&WS2811_TIMER->CCR3); - TIM_OC3PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(WS2811_TIMER, &TIM_OCInitStructure); - timDMASource = TIM_DMA_CC4; - channelAddress = (uint32_t)(&WS2811_TIMER->CCR4); - TIM_OC4PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); - break; + switch (WS2811_TIMER_CHANNEL) { + case TIM_Channel_1: + TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC1; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR1); + TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC2; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR2); + TIM_OC2PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC3; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR3); + TIM_OC3PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC4; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR4); + TIM_OC4PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; } TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE); diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index 9b81d936a..af09f85e1 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -35,15 +35,15 @@ #define CCMR_Offset ((uint16_t)0x0018) const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_TIM1 }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_TIM2 }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_TIM3 }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_TIM4 }, - { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), GPIO_AF_TIM5 }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_TIM8 }, - { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), GPIO_AF_TIM9 }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_TIM1 }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_TIM2 }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_TIM3 }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_TIM4 }, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), GPIO_AF_TIM5 }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_TIM8 }, + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), GPIO_AF_TIM9 }, { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), GPIO_AF_TIM10 }, { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), GPIO_AF_TIM11 }, { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), GPIO_AF_TIM12 }, @@ -53,35 +53,32 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode) { - uint32_t tmp = 0; + uint32_t tmp = 0; - /* Check the parameters */ - assert_param(IS_TIM_LIST8_PERIPH(TIMx)); - assert_param(IS_TIM_CHANNEL(TIM_Channel)); - assert_param(IS_TIM_OCM(TIM_OCMode)); + /* Check the parameters */ + assert_param(IS_TIM_LIST8_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_OCM(TIM_OCMode)); - tmp = (uint32_t) TIMx; - tmp += CCMR_Offset; + tmp = (uint32_t) TIMx; + tmp += CCMR_Offset; - if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) - { - tmp += (TIM_Channel>>1); + if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) { + tmp += (TIM_Channel>>1); - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= TIM_OCMode; - } - else - { - tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1; + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= TIM_OCMode; + } else { + tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1; - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); - } + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); + } } From a66820382c7f9a6238de3a4a2993813d306bfb6c Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 2 Jul 2016 19:47:37 +1000 Subject: [PATCH 40/54] More formatting (minor cleanup) --- src/main/drivers/timer_stm32f10x.c | 43 ++++++++++----------- src/main/drivers/timer_stm32f30x.c | 60 +++++++++++++++--------------- 2 files changed, 50 insertions(+), 53 deletions(-) diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 27e9df381..09ef0efc8 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -57,34 +57,31 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode) { - uint32_t tmp = 0; + uint32_t tmp = 0; - /* Check the parameters */ - assert_param(IS_TIM_LIST8_PERIPH(TIMx)); - assert_param(IS_TIM_CHANNEL(TIM_Channel)); - assert_param(IS_TIM_OCM(TIM_OCMode)); + /* Check the parameters */ + assert_param(IS_TIM_LIST8_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_OCM(TIM_OCMode)); - tmp = (uint32_t) TIMx; - tmp += CCMR_Offset; + tmp = (uint32_t) TIMx; + tmp += CCMR_Offset; - if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) - { - tmp += (TIM_Channel>>1); + if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) { + tmp += (TIM_Channel>>1); - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= TIM_OCMode; - } - else - { - tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1; + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= TIM_OCMode; + } else { + tmp += (uint16_t)(TIM_Channel - (uint16_t)4) >> (uint16_t)1; - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); - } + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); + } } diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index baad75634..033aa316c 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -10,16 +10,16 @@ #include "timer.h" const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_6 }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_1 }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_2 }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_10 }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_5 }, - { .TIMx = TIM15, .rcc = RCC_APB2(TIM15), GPIO_AF_9 }, - { .TIMx = TIM16, .rcc = RCC_APB2(TIM16), GPIO_AF_1 }, - { .TIMx = TIM17, .rcc = RCC_APB2(TIM17), GPIO_AF_1 }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_6 }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_1 }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_2 }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_10 }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_5 }, + { .TIMx = TIM15, .rcc = RCC_APB2(TIM15), GPIO_AF_9 }, + { .TIMx = TIM16, .rcc = RCC_APB2(TIM16), GPIO_AF_1 }, + { .TIMx = TIM17, .rcc = RCC_APB2(TIM17), GPIO_AF_1 }, }; @@ -58,31 +58,31 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode) { - uint32_t tmp = 0; + uint32_t tmp = 0; - /* Check the parameters */ - assert_param(IS_TIM_LIST1_PERIPH(TIMx)); - assert_param(IS_TIM_CHANNEL(TIM_Channel)); - assert_param(IS_TIM_OCM(TIM_OCMode)); + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_OCM(TIM_OCMode)); - tmp = (uint32_t) TIMx; - tmp += CCMR_OFFSET; + tmp = (uint32_t) TIMx; + tmp += CCMR_OFFSET; - if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) { - tmp += (TIM_Channel>>1); + if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) { + tmp += (TIM_Channel>>1); - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= CCMR_OC13M_MASK; + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= CCMR_OC13M_MASK; - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= TIM_OCMode; - } else { - tmp += (uint32_t)(TIM_Channel - (uint32_t)4)>> (uint32_t)1; + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= TIM_OCMode; + } else { + tmp += (uint32_t)(TIM_Channel - (uint32_t)4) >> (uint32_t)1; - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= CCMR_OC24M_MASK; + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= CCMR_OC24M_MASK; - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8); - } + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8); + } } From 2a340c137a189cc410ecd13671724a63ec43d075 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 21 Jun 2016 23:45:37 +0100 Subject: [PATCH 41/54] Made headers in target.h files consistent. --- src/main/target/ALIENFLIGHTF3/target.c | 17 ++++++++++++++++- src/main/target/ALIENFLIGHTF4/target.c | 19 ++++++++++++++++++- src/main/target/BLUEJAYF4/target.c | 17 ++++++++++++++++- src/main/target/CC3D/target.c | 17 ++++++++++++++++- src/main/target/CHEBUZZF3/target.c | 18 +++++++++++++++++- src/main/target/CJMCU/target.c | 17 ++++++++++++++++- src/main/target/COLIBRI_RACE/target.c | 17 ++++++++++++++++- src/main/target/DOGE/target.c | 18 +++++++++++++++++- src/main/target/EUSTM32F103RC/target.c | 18 +++++++++++++++++- src/main/target/FURYF3/target.c | 18 +++++++++++++++++- src/main/target/FURYF4/target.c | 18 +++++++++++++++++- src/main/target/IRCFUSIONF3/target.c | 18 +++++++++++++++++- src/main/target/KISSFC/target.c | 18 +++++++++++++++++- src/main/target/LUX_RACE/target.c | 18 +++++++++++++++++- src/main/target/MOTOLAB/target.c | 18 +++++++++++++++++- src/main/target/NAZE/target.c | 18 +++++++++++++++++- src/main/target/OMNIBUS/target.c | 18 +++++++++++++++++- src/main/target/PIKOBLX/target.c | 18 +++++++++++++++++- src/main/target/PORT103R/target.c | 18 +++++++++++++++++- src/main/target/REVO/target.c | 18 +++++++++++++++++- src/main/target/REVONANO/target.c | 2 +- src/main/target/RMDO/target.c | 18 +++++++++++++++++- src/main/target/SINGULARITY/target.c | 18 +++++++++++++++++- src/main/target/SIRINFPV/target.c | 18 +++++++++++++++++- src/main/target/SPARKY/target.c | 18 +++++++++++++++++- src/main/target/SPRACINGF3/target.c | 17 ++++++++++++++++- src/main/target/SPRACINGF3EVO/target.c | 17 ++++++++++++++++- src/main/target/SPRACINGF3MINI/target.c | 17 ++++++++++++++++- src/main/target/STM32F3DISCOVERY/target.c | 17 ++++++++++++++++- 29 files changed, 469 insertions(+), 29 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index ad85d7b9b..437bbbf0a 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index e6024280f..1aef43a59 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -1,5 +1,22 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include -#include #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index 24df5a739..f005e5d1a 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index c685f7e35..c657054d6 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 132ff783f..631d83e6a 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index 926f3efa0..0f8228559 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 675067e06..91b21c8b3 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index ebd5e6f1f..c07a92541 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index 6e1b4ae6b..b2a0db9a1 100644 --- a/src/main/target/EUSTM32F103RC/target.c +++ b/src/main/target/EUSTM32F103RC/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 4dc593bb3..c2e230b9a 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index c5bb0486c..ac600ea43 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index 7207468fa..e31aa3a7b 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 60f2a0736..07450ad50 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index e26bb2950..adee79183 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index b624db7cc..a89063cc9 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 110ce3140..b7c13f56b 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 94250435a..d895b571c 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index b624db7cc..a89063cc9 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 42ce07bec..8f21b7cbb 100644 --- a/src/main/target/PORT103R/target.c +++ b/src/main/target/PORT103R/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index b32c5175f..9a30df53b 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 53d404217..9a579f2bc 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -15,10 +15,10 @@ * along with Cleanflight. If not, see . */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index eaeb18fca..1bea16f50 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 645bd323f..813157951 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index dfe1517fb..0630b527d 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index d5ecdcc96..389617278 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index 122f07bb0..bf2a06da4 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 1ac9743d3..171f6be27 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index bc0d06cee..214dab656 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index dcf2426ce..583c10f66 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include From 405d9f051aad299691a60e1a7905d757e8877d7f Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 2 Jul 2016 21:35:52 +1000 Subject: [PATCH 42/54] Added debugging counters --- src/main/io/serial_4way.c | 4 ++-- src/main/io/serial_4way_avrootloader.c | 2 +- src/main/vcpf4/usbd_cdc_vcp.c | 8 +++++--- src/main/vcpf4/usbd_cdc_vcp.h | 2 +- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 81742c96f..49c580541 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -359,6 +359,7 @@ void esc4wayProcess(serialPort_t *serial) TX_LED_ON; if (replyAck == esc4wayAck_OK) replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen); + RX_LED_OFF; // send single '\0' byte is output when length is zero (len ==0 -> 256 bytes) if(!outLen) { @@ -366,7 +367,6 @@ void esc4wayProcess(serialPort_t *serial) outLen = 1; } - RX_LED_OFF; crcOut = 0; serialBeginWrite(port); writeByteCrc(cmd_Remote_Escape); @@ -374,7 +374,7 @@ void esc4wayProcess(serialPort_t *serial) writeByteCrc(addr >> 8); writeByteCrc(addr & 0xff); writeByteCrc(outLen & 0xff); // only low byte is send, 0x00 -> 256B - for(int i = 0; i < outLen; i++) + for(int i = 0; i < outLen % 0x100; i++) writeByteCrc(paramBuf[i]); writeByteCrc(replyAck); writeByte(crcOut >> 8); diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index efb461026..01a598e8b 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -238,7 +238,7 @@ void BL_SendCMDRunRestartBootloader(void) static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr { // skip if adr == 0xFFFF - if((pMem->addr == 0xffff)) + if(pMem->addr == 0xffff) return 1; uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->addr >> 8, pMem->addr & 0xff }; BL_SendBuf(sCMD, sizeof(sCMD), true); diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index e4681418b..8b789fa48 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -218,18 +218,20 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) * @param Len: Number of data received (in bytes) * @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL */ +static uint32_t rxTotalBytes = 0; +static uint32_t rxPackets = 0; + static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) { - __disable_irq(); + rxPackets++; for (uint32_t i = 0; i < Len; i++) { usbData.buffer[usbData.bufferInPosition] = Buf[i]; usbData.bufferInPosition = (usbData.bufferInPosition + 1) % USB_RX_BUFSIZE; receiveLength++; + rxTotalBytes++; } - __enable_irq(); - if(receiveLength > (USB_RX_BUFSIZE-1)) return USBD_FAIL; diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 7cc0f8feb..df1e29001 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,7 +34,7 @@ #include "usbd_usr.h" #include "usbd_desc.h" -#define USB_RX_BUFSIZE 1024 +#define USB_RX_BUFSIZE 2048 __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; From f2122e88d9e308ff778bcf333407b1ec1ab1cbd6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 2 Jul 2016 12:53:47 +0100 Subject: [PATCH 43/54] Tidied whitespace in target files --- src/main/target/ALIENFLIGHTF3/target.c | 29 +++--- src/main/target/ALIENFLIGHTF4/target.c | 104 +++++++++++----------- src/main/target/CC3D/target.c | 14 +-- src/main/target/LUX_RACE/target.c | 12 +-- src/main/target/NAZE/target.c | 12 +-- src/main/target/REVO/target.c | 14 +-- src/main/target/STM32F3DISCOVERY/target.c | 40 ++++----- 7 files changed, 108 insertions(+), 117 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index 437bbbf0a..57fc41763 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -24,16 +24,16 @@ const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM15 - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM15 - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1 - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM2 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM2 + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM15 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM15 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1 + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM2 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM2 0xFFFF }; @@ -63,10 +63,7 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - // // 6 x 3 pin headers - // - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR @@ -74,20 +71,14 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - // // 6 pin header - // - // PWM7-10 { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N - // // PPM PORT - Also USART2 RX (AF5) - // - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 //{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_IPD, GPIO_PinSource3, GPIO_AF_9 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index 1aef43a59..efedb5668 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -22,70 +22,70 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 or #1 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #2 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #5 or #3 - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #6 or #4 - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #7 or #5 - PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #8 or #6 - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 or #1 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #2 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #5 or #3 + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #6 or #4 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #7 or #5 + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #8 or #6 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 - PWM2 | (MAP_TO_PWM_INPUT << 8), // input #2 - PWM3 | (MAP_TO_PWM_INPUT << 8), // input #3 - PWM4 | (MAP_TO_PWM_INPUT << 8), // input #4 - PWM5 | (MAP_TO_PWM_INPUT << 8), // input #5 - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 or #1 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #2 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #5 or #3 - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #6 or #4 - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #7 or #5 - PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #8 or #6 + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), // input #2 + PWM3 | (MAP_TO_PWM_INPUT << 8), // input #3 + PWM4 | (MAP_TO_PWM_INPUT << 8), // input #4 + PWM5 | (MAP_TO_PWM_INPUT << 8), // input #5 + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 or #1 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #2 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #5 or #3 + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #6 or #4 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #7 or #5 + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #8 or #6 0xFFFF }; const uint16_t airPPM[] = { - PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 - PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 - PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #6 - PWM2 | (MAP_TO_SERVO_OUTPUT << 8), // servo #7 - PWM3 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 - PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #9 - PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #6 + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), // servo #7 + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #9 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 0xFFFF }; const uint16_t airPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 - PWM2 | (MAP_TO_PWM_INPUT << 8), // input #2 - PWM3 | (MAP_TO_PWM_INPUT << 8), // input #3 - PWM4 | (MAP_TO_PWM_INPUT << 8), // input #4 - PWM5 | (MAP_TO_PWM_INPUT << 8), // input #5 - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 - PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 - PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #6 + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), // input #2 + PWM3 | (MAP_TO_PWM_INPUT << 8), // input #3 + PWM4 | (MAP_TO_PWM_INPUT << 8), // input #4 + PWM5 | (MAP_TO_PWM_INPUT << 8), // input #5 + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #6 0xFFFF }; diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index c657054d6..a870de9e2 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -23,16 +23,16 @@ const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index adee79183..91b21c8b3 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -72,12 +72,12 @@ const uint16_t airPWM[] = { PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index b7c13f56b..7116b5355 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -23,16 +23,16 @@ const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 9a30df53b..03f60516d 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -23,17 +23,17 @@ const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 583c10f66..1712bebae 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -23,16 +23,16 @@ const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; @@ -45,27 +45,27 @@ const uint16_t multiPWM[] = { PWM6 | (MAP_TO_PWM_INPUT << 8), PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 0xFFFF }; const uint16_t airPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), - PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 - PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 PWM6 | (MAP_TO_SERVO_OUTPUT << 8), PWM7 | (MAP_TO_SERVO_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 0xFFFF }; @@ -78,12 +78,12 @@ const uint16_t airPWM[] = { PWM6 | (MAP_TO_PWM_INPUT << 8), PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), - PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 0xFFFF }; From 2123a758e7e55223ac7b306f1c276b100d81ad7c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 2 Jul 2016 15:23:19 +0100 Subject: [PATCH 44/54] Allowed setting of default serial rx in target.h --- src/main/config/config.c | 24 ++++++++---------------- src/main/target/ALIENFLIGHTF3/target.h | 2 +- src/main/target/COLIBRI_RACE/target.h | 6 ++++-- src/main/target/KISSFC/target.h | 3 ++- src/main/target/NAZE/target.h | 1 + src/main/target/OLIMEXINO/target.c | 18 +++++++++++++++++- src/main/target/SINGULARITY/target.h | 6 +++--- 7 files changed, 36 insertions(+), 24 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 5bcd1e923..755598e4d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -645,7 +645,13 @@ static void resetConf(void) masterConfig.blackbox_rate_denom = 1; #endif // BLACKBOX - + +#ifdef SERIALRX_UART + if (featureConfigured(FEATURE_RX_SERIAL)) { + masterConfig.serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; + } +#endif + // alternative defaults settings for COLIBRI RACE targets #if defined(COLIBRI_RACE) masterConfig.escAndServoConfig.minthrottle = 1025; @@ -660,11 +666,6 @@ static void resetConf(void) #if defined(ALIENFLIGHT) featureClear(FEATURE_ONESHOT125); -#ifdef ALIENFLIGHTF1 - masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; -#else - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; -#endif #ifdef ALIENFLIGHTF3 masterConfig.mag_hardware = MAG_NONE; // disabled by default #endif @@ -688,11 +689,6 @@ static void resetConf(void) masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif #endif -#if defined(SINGULARITY) - // alternative defaults settings for SINGULARITY target - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; -#endif - // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); @@ -901,15 +897,11 @@ void validateAndFixConfig(void) #if defined(COLIBRI_RACE) masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP; - if(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) { + if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) { featureClear(FEATURE_RX_PARALLEL_PWM); featureClear(FEATURE_RX_MSP); featureSet(FEATURE_RX_PPM); } - if(featureConfigured(FEATURE_RX_SERIAL)) { - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; - //masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS; - } #endif useRxConfig(&masterConfig.rxConfig); diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index a5d1b042f..4cd3d955c 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -117,7 +117,6 @@ #define USE_ADC #define ADC_INSTANCE ADC2 -//#define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PA4 #define VBAT_SCALE_DEFAULT 20 @@ -133,6 +132,7 @@ #define DEFAULT_FEATURES FEATURE_MOTOR_STOP #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 976b287a0..449a65e26 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -113,7 +113,6 @@ #define USE_ADC #define ADC_INSTANCE ADC1 -#define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PC0 #define CURRENT_METER_ADC_PIN PC1 #define RSSI_ADC_PIN PC2 @@ -144,7 +143,10 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_VBAT +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 9095ef701..d5679d6f2 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -81,7 +81,8 @@ #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS; +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 #define SPEKTRUM_BIND #define BIND_PIN PB4 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index df73f8e4f..1bc1ded34 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -180,6 +180,7 @@ #define DEFAULT_FEATURES FEATURE_MOTOR_STOP #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART2 #define HARDWARE_BIND_PLUG // Hardware bind plug at PB5 (Pin 41) diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c index 1b75af422..ea20267b3 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index f58dcbe96..814b72420 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -91,7 +91,6 @@ #define M25P16_SPI_INSTANCE SPI2 #define USE_ADC -#define BOARD_HAS_VOLTAGE_DIVIDER #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PB2 @@ -115,8 +114,9 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART3 #define SPEKTRUM_BIND // USART2, PA15 From 8ee2e4f739b005f5850b06ff93942a8f3f984a13 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 2 Jul 2016 15:31:00 +0100 Subject: [PATCH 45/54] Updated VBAT scale --- src/main/target/KISSFC/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index d5679d6f2..260a2bc57 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -73,7 +73,7 @@ #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_ADC -#define VBAT_SCALE_DEFAULT 164 +#define VBAT_SCALE_DEFAULT 160 #define ADC_INSTANCE ADC1 #define VBAT_ADC_PIN PA0 //#define CURRENT_METER_ADC_PIN PA5 From 816c5d1b8b060bf7b62b931f2b71a637a9392fe6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 2 Jul 2016 21:14:56 +0100 Subject: [PATCH 46/54] Fixed Alien F4 serial RX UART. --- src/main/target/ALIENFLIGHTF4/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index d5122a486..2481ea697 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -176,6 +176,7 @@ #define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE From 72a6e701eb2bb0af06931807d50a9081c85ab4a6 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 3 Jul 2016 07:44:35 +1000 Subject: [PATCH 47/54] Fixed build issue for F1, and added dfu CLI command (for restart in DFU mode) --- src/main/drivers/io.h | 2 -- src/main/drivers/system.h | 1 + src/main/drivers/system_stm32f10x.c | 8 +++++++- src/main/drivers/system_stm32f30x.c | 9 ++++++++- src/main/drivers/system_stm32f4xx.c | 15 +++++++++++++++ src/main/io/serial_4way.c | 4 ++-- src/main/io/serial_cli.c | 24 +++++++++++++++++++++--- 7 files changed, 54 insertions(+), 9 deletions(-) diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index 121e7a0e1..315560e43 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -40,14 +40,12 @@ typedef uint8_t ioConfig_t; // packed IO configuration #define IO_CONFIG(mode, speed) ((mode) | (speed)) #define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_2MHz) -#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_25MHz) #define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_Out_OD, GPIO_Speed_2MHz) #define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF_PP, GPIO_Speed_2MHz) #define IOCFG_AF_OD IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_2MHz) #define IOCFG_IPD IO_CONFIG(GPIO_Mode_IPD, GPIO_Speed_2MHz) #define IOCFG_IPU IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_2MHz) #define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_2MHz) -#define IOCFG_IPU_25 IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_25MHz) #elif defined(STM32F3) || defined(STM32F4) diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 92e17f8a8..c6d58aa74 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -42,6 +42,7 @@ void systemReset(void); void systemResetToBootloader(void); bool isMPUSoftReset(void); void cycleCounterInit(void); +void checkForBootLoaderRequest(void); void enableGPIOPowerUsageAndNoiseReductions(void); // current crystal frequency - 8 or 12MHz diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index 4ad526850..3e59fe734 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -37,7 +37,8 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) { +void systemResetToBootloader(void) +{ // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC @@ -68,6 +69,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + SetSysClock(false); #ifdef CC3D @@ -110,3 +113,6 @@ void systemInit(void) SysTick_Config(SystemCoreClock / 1000); } +void checkForBootLoaderRequest(void) +{ +} \ No newline at end of file diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index 7e58ab061..ee8aef1a0 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -35,7 +35,8 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) { +void systemResetToBootloader(void) +{ // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC @@ -82,6 +83,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + // Enable FPU SCB->CPACR = (0x3 << (10 * 2)) | (0x3 << (11 * 2)); SetSysClock(); @@ -102,3 +105,7 @@ void systemInit(void) // SysTick SysTick_Config(SystemCoreClock / 1000); } + +void checkForBootLoaderRequest(void) +{ +} diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index f0042026f..697b2b743 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -169,6 +169,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + SetSysClock(); // Configure NVIC preempt/priority groups @@ -194,3 +196,16 @@ void systemInit(void) SysTick_Config(SystemCoreClock / 1000); } +void(*bootJump)(void); +void checkForBootLoaderRequest(void) +{ + __enable_irq(); + if (*((uint32_t *)0x2001FFFC) == 0xDEADBEEF) { + *((uint32_t *)0x2001FFFC) = 0x0; + __set_MSP(0x20001000); + + bootJump = (void(*)(void))(*((uint32_t *) 0x1fff0004)); + bootJump(); + while (1); + } +} \ No newline at end of file diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 49c580541..bcea2f8a1 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -120,12 +120,12 @@ void setEscLo(uint8_t selEsc) void setEscInput(uint8_t selEsc) { - IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU_25); + IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU); } void setEscOutput(uint8_t selEsc) { - IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP_25); + IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP); } // Initialize 4way ESC interface diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a51f828ed..e160869df 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -112,6 +112,7 @@ static void cliRxFail(char *cmdline); static void cliAdjustmentRange(char *cmdline); static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); +void cliDfu(char *cmdLine); static void cliDump(char *cmdLine); void cliDumpProfile(uint8_t profileIndex); void cliDumpRateProfile(uint8_t rateProfileIndex) ; @@ -122,6 +123,7 @@ static void cliPlaySound(char *cmdline); static void cliProfile(char *cmdline); static void cliRateProfile(char *cmdline); static void cliReboot(void); +static void cliRebootEx(bool bootLoader); static void cliSave(char *cmdline); static void cliSerial(char *cmdline); #ifndef SKIP_SERIAL_PASSTHROUGH @@ -263,8 +265,8 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), - CLI_COMMAND_DEF("dump", "dump configuration", - "[master|profile]", cliDump), + CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), + CLI_COMMAND_DEF("dump", "dump configuration", "[master|profile]", cliDump), CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), CLI_COMMAND_DEF("feature", "configure features", "list\r\n" @@ -2564,10 +2566,19 @@ static void cliRateProfile(char *cmdline) { static void cliReboot(void) { - cliPrint("\r\nRebooting"); + cliRebootEx(false); +} + +static void cliRebootEx(bool bootLoader) +{ + cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); waitForSerialPortToFinishTransmitting(cliPort); stopMotors(); + if (bootLoader) { + systemResetToBootloader(); + return; + } systemReset(); } @@ -3107,6 +3118,13 @@ static void cliResource(char *cmdline) } } +void cliDfu(char *cmdLine) +{ + UNUSED(cmdLine); + cliPrint("\r\nRestarting in DFU mode"); + cliRebootEx(true); +} + void cliInit(serialConfig_t *serialConfig) { UNUSED(serialConfig); From f499041c8e16476459a911902c8c046e6c17f355 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 3 Jul 2016 08:00:21 +1000 Subject: [PATCH 48/54] STM32F4: execute extra enable irq on startup only if rebooting into DFU --- src/main/drivers/system_stm32f4xx.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 697b2b743..53f3767a5 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -199,9 +199,11 @@ void systemInit(void) void(*bootJump)(void); void checkForBootLoaderRequest(void) { - __enable_irq(); if (*((uint32_t *)0x2001FFFC) == 0xDEADBEEF) { + *((uint32_t *)0x2001FFFC) = 0x0; + + __enable_irq(); __set_MSP(0x20001000); bootJump = (void(*)(void))(*((uint32_t *) 0x1fff0004)); From 74d20a276f7156538bbecba65375447a1ed4bf78 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 29 Jun 2016 15:02:29 +0100 Subject: [PATCH 49/54] Made filter naming, parameters and state consistent --- src/main/common/filter.c | 91 +++++++++++++++++---------------- src/main/common/filter.h | 26 ++++++---- src/main/flight/imu.c | 1 - src/main/flight/mixer.c | 6 +-- src/main/flight/pid.c | 12 ++--- src/main/sensors/acceleration.c | 19 +++---- src/main/sensors/battery.c | 15 +++--- src/main/sensors/gyro.c | 6 +-- 8 files changed, 93 insertions(+), 83 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 21fd2948f..d059fe882 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -17,76 +17,81 @@ #include #include -#include #include -#include "common/axis.h" #include "common/filter.h" #include "common/maths.h" -#define M_LN2_FLOAT 0.69314718055994530942f -#define M_PI_FLOAT 3.14159265358979323846f +#define M_LN2_FLOAT 0.69314718055994530942f +#define M_PI_FLOAT 3.14159265358979323846f #define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ -// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime) -float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) { +// PT1 Low Pass filter - // Pre calculate and store RC - if (!filter->RC) { - filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); - } +void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT) +{ + filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); + filter->dT = dT; +} - filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state); +float pt1FilterApply(pt1Filter_t *filter, float input) +{ + filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); + return filter->state; +} + +float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) +{ + // Pre calculate and store RC + if (!filter->RC) { + filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); + filter->dT = dT; + } + + filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); return filter->state; } /* sets up a biquad Filter */ -void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate) +void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate) { - float sampleRate; + const float sampleRate = 1 / ((float)refreshRate * 0.000001f); - sampleRate = 1 / ((float)refreshRate * 0.000001f); - - float omega, sn, cs, alpha; - float a0, a1, a2, b0, b1, b2; - - /* setup variables */ - omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate; - sn = sinf(omega); - cs = cosf(omega); + // setup variables + const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate; + const float sn = sinf(omega); + const float cs = cosf(omega); //this is wrong, should be hyperbolic sine //alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn); - alpha = sn / (2 * BIQUAD_Q); + const float alpha = sn / (2 * BIQUAD_Q); - b0 = (1 - cs) / 2; - b1 = 1 - cs; - b2 = (1 - cs) / 2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + const float b0 = (1 - cs) / 2; + const float b1 = 1 - cs; + const float b2 = (1 - cs) / 2; + const float a0 = 1 + alpha; + const float a1 = -2 * cs; + const float a2 = 1 - alpha; - /* precompute the coefficients */ - newState->b0 = b0 / a0; - newState->b1 = b1 / a0; - newState->b2 = b2 / a0; - newState->a1 = a1 / a0; - newState->a2 = a2 / a0; + // precompute the coefficients + filter->b0 = b0 / a0; + filter->b1 = b1 / a0; + filter->b2 = b2 / a0; + filter->a1 = a1 / a0; + filter->a2 = a2 / a0; - /* zero initial samples */ - newState->d1 = newState->d2 = 1; + // zero initial samples + filter->d1 = filter->d2 = 0; } /* Computes a biquad_t filter on a sample */ -float applyBiQuadFilter(float sample, biquad_t *state) //direct form 2 transposed +float biquadFilterApply(biquadFilter_t *filter, float input) { - float result; - - result = state->b0 * sample + state->d1; - state->d1 = state->b1 * sample - state->a1 * result + state->d2; - state->d2 = state->b2 * sample - state->a2 * result; + const float result = filter->b0 * input + filter->d1; + filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; + filter->d2 = filter->b2 * input - filter->a2 * result; return result; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 809063f4b..66fcb53bb 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -17,20 +17,26 @@ #define DELTA_MAX_SAMPLES 12 -typedef struct filterStatePt1_s { - float state; - float RC; - float constdT; -} filterStatePt1_t; +typedef struct pt1Filter_s { + float state; + float RC; + float dT; +} pt1Filter_t; /* this holds the data required to update samples thru a filter */ -typedef struct biquad_s { +typedef struct biquadFilter_s { float b0, b1, b2, a1, a2; float d1, d2; -} biquad_t; +} biquadFilter_t; + + +void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate); +float biquadFilterApply(biquadFilter_t *filter, float input); + +void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); +float pt1FilterApply(pt1Filter_t *filter, float input); +float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT); -float applyBiQuadFilter(float sample, biquad_t *state); -float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt); int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]); float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]); -void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate); + diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 2e200cff1..d885d30ea 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -28,7 +28,6 @@ #include "debug.h" #include "common/axis.h" -#include "common/filter.h" #include "drivers/system.h" #include "drivers/sensor.h" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index adc0145ba..b5da4a017 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -946,7 +946,7 @@ void filterServos(void) #ifdef USE_SERVOS static int16_t servoIdx; static bool servoFilterIsSet; - static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS]; + static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; #if defined(MIXER_DEBUG) uint32_t startTime = micros(); @@ -955,11 +955,11 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetPidLooptime); + biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } - servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx])); + servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 91ccbc2d8..b0dee8749 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -112,8 +112,8 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static filterStatePt1_t deltaFilterState[3]; -static filterStatePt1_t yawFilterState; +static pt1Filter_t deltaFilter[3]; +static pt1Filter_t yawFilter; #ifndef SKIP_PID_LUXFLOAT static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, @@ -205,7 +205,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat //-----calculate D-term if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); axisPID[axis] = lrintf(PTerm + ITerm); @@ -230,7 +230,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat delta *= (1.0f / getdT()); // Filter delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); @@ -344,7 +344,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin //-----calculate D-term if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); axisPID[axis] = PTerm + ITerm; @@ -369,7 +369,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; // Filter delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT()); DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d79eb2e14..da86d37eb 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -47,7 +47,6 @@ uint32_t accTargetLooptime; static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. extern uint16_t InflightcalibratingA; -extern bool AccInflightCalibrationArmed; extern bool AccInflightCalibrationMeasurementDone; extern bool AccInflightCalibrationSavetoEEProm; extern bool AccInflightCalibrationActive; @@ -55,7 +54,7 @@ extern bool AccInflightCalibrationActive; static flightDynamicsTrims_t *accelerationTrims; static float accLpfCutHz = 0; -static biquad_t accFilterState[XYZ_AXIS_COUNT]; +static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; static bool accFilterInitialised = false; void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) @@ -87,9 +86,8 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { static int32_t a[3]; - int axis; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { // Reset a[axis] at start of calibration if (isOnFirstAccelerationCalibrationCycle()) @@ -179,14 +177,13 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { - int16_t accADCRaw[XYZ_AXIS_COUNT]; - int axis; + int16_t accADCRaw[XYZ_AXIS_COUNT]; if (!acc.read(accADCRaw)) { return; } - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis]; accSmooth[axis] = accADCRaw[axis]; } @@ -194,13 +191,17 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) if (accLpfCutHz) { if (!accFilterInitialised) { if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(accLpfCutHz, &accFilterState[axis], accTargetLooptime); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + } accFilterInitialised = true; } } if (accFilterInitialised) { - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis])); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis])); + } } } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 5bcc265f1..23159d87c 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -63,20 +63,19 @@ uint16_t batteryAdcToVoltage(uint16_t src) static void updateBatteryVoltage(void) { - uint16_t vbatSample; - static biquad_t vbatFilterState; - static bool vbatFilterStateIsSet; + static biquadFilter_t vbatFilter; + static bool vbatFilterIsInitialised; // store the battery voltage with some other recent battery voltage readings - vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); + uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample; - if (!vbatFilterStateIsSet) { - BiQuadNewLpf(VBATT_LPF_FREQ, &vbatFilterState, 50000); //50HZ Update - vbatFilterStateIsSet = true; + if (!vbatFilterIsInitialised) { + biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update + vbatFilterIsInitialised = true; } - vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState); + vbatSample = biquadFilterApply(&vbatFilter, vbatSample); vbat = batteryAdcToVoltage(vbatSample); if (debugMode == DEBUG_BATTERY) debug[1] = vbat; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c79d29013..4f425dc1d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -43,7 +43,7 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static biquad_t gyroFilterState[XYZ_AXIS_COUNT]; +static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; @@ -57,7 +57,7 @@ void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); } } } @@ -157,7 +157,7 @@ void gyroUpdate(void) if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); + gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { From 39ec4a128ab3fe67d094b42e0693fb993f22c84b Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Mon, 4 Jul 2016 02:16:44 -0500 Subject: [PATCH 50/54] FuryF4 Target Changes --- src/main/target/FURYF4/target.h | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 397206c9b..2559e2d27 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -17,13 +17,10 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "FYF4" //Call it a revo for now so it connects to RFC for testing. +#define TARGET_BOARD_IDENTIFIER "FYF4" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH -#define CONFIG_MSP_PORT 2 -#define CONFIG_RX_SERIAL_PORT 1 -#define USBD_PRODUCT_STRING "FURYF4" +#define USBD_PRODUCT_STRING "FuryF4" #define LED0 PB5 #define LED1 PB4 @@ -67,8 +64,8 @@ #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PD2 -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PB3 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN PB12 /* #define SDCARD_DETECT_PIN PD2 @@ -86,11 +83,18 @@ // Divide to under 25MHz for normal operation: #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 + +//#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 +//#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 +//#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +//#define SDCARD_DMA_CHANNEL DMA_Channel_0 + +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CHANNEL DMA_Channel_0 + #define USE_FLASHFS #define USE_FLASH_M25P16 #define M25P16_CS_PIN PB3 @@ -149,10 +153,18 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define SPEKTRUM_BIND +// USART3 Rx, PB11 +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTD (BIT(2)) #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) From c7c459d122fd4f55cae47680ddba1dee3dbce2ac Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 5 Jul 2016 01:13:31 +0200 Subject: [PATCH 51/54] Add GYRO INT to KISS target --- src/main/target/KISSFC/target.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 03c3cf31f..04e319cf3 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -28,9 +28,10 @@ #define USABLE_TIMER_CHANNEL_COUNT 12 -#define EXTI2_TS_CALLBACK_HANDLER_COUNT 1 // MPU data ready +#define USE_EXTI +#define MPU_INT_EXTI PB2 #define USE_MPU_DATA_READY_SIGNAL - +#define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO #define USE_GYRO_MPU6050 From 67a48ca4c9790d05a5f71700226ff1922a306df2 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 4 Jul 2016 07:46:49 +0200 Subject: [PATCH 52/54] Move custom configuration for AlienFlight and Colibri Race to target directory --- src/main/config/config.c | 36 +-------- src/main/target/ALIENFLIGHTF3/config.c | 100 +++++++++++++++++++++++++ src/main/target/ALIENFLIGHTF3/target.h | 2 +- src/main/target/ALIENFLIGHTF4/config.c | 100 +++++++++++++++++++++++++ src/main/target/ALIENFLIGHTF4/target.h | 4 +- src/main/target/COLIBRI_RACE/config.c | 84 +++++++++++++++++++++ src/main/target/COLIBRI_RACE/target.h | 1 + src/main/target/NAZE/config.c | 99 ++++++++++++++++++++++++ src/main/target/NAZE/target.h | 2 +- 9 files changed, 389 insertions(+), 39 deletions(-) create mode 100644 src/main/target/ALIENFLIGHTF3/config.c create mode 100644 src/main/target/ALIENFLIGHTF4/config.c create mode 100644 src/main/target/COLIBRI_RACE/config.c create mode 100644 src/main/target/NAZE/config.c diff --git a/src/main/config/config.c b/src/main/config/config.c index bc8d830a5..003b7c32a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -89,6 +89,7 @@ #endif void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); +void targetConfiguration(void); #if !defined(FLASH_SIZE) #error "Flash size not defined for target. (specify in KB)" @@ -652,41 +653,8 @@ static void resetConf(void) } #endif - // alternative defaults settings for COLIBRI RACE targets -#if defined(COLIBRI_RACE) - masterConfig.escAndServoConfig.minthrottle = 1025; - masterConfig.escAndServoConfig.maxthrottle = 1980; - masterConfig.batteryConfig.vbatmaxcellvoltage = 45; - masterConfig.batteryConfig.vbatmincellvoltage = 30; -#endif - #if defined(TARGET_CONFIG) - targetConfiguration(&masterConfig); -#endif - -#if defined(ALIENFLIGHT) - featureClear(FEATURE_ONESHOT125); -#ifdef ALIENFLIGHTF3 - masterConfig.mag_hardware = MAG_NONE; // disabled by default -#endif - masterConfig.rxConfig.spektrum_sat_bind = 5; - masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; - masterConfig.motor_pwm_rate = 32000; - masterConfig.failsafeConfig.failsafe_delay = 2; - masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentControlRateProfile->rates[FD_PITCH] = 40; - currentControlRateProfile->rates[FD_ROLL] = 40; - currentControlRateProfile->rates[FD_YAW] = 40; - parseRcChannels("TAER1234", &masterConfig.rxConfig); - - masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif + targetConfiguration(); #endif // copy first profile into remaining profile diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c new file mode 100644 index 000000000..9defa6223 --- /dev/null +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -0,0 +1,100 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// alternative defaults settings for AlienFlight targets +void targetConfiguration(void) { + featureClear(FEATURE_ONESHOT125); + masterConfig.mag_hardware = MAG_NONE; // disabled by default + masterConfig.rxConfig.spektrum_sat_bind = 5; + masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; + masterConfig.motor_pwm_rate = 32000; + masterConfig.failsafeConfig.failsafe_delay = 2; + masterConfig.failsafeConfig.failsafe_off_delay = 0; + currentControlRateProfile->rates[FD_PITCH] = 40; + currentControlRateProfile->rates[FD_ROLL] = 40; + currentControlRateProfile->rates[FD_YAW] = 40; + parseRcChannels("TAER1234", &masterConfig.rxConfig); + + masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif +} diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 4cd3d955c..1df1d3c4f 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -18,7 +18,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF3" // AlienFlight F3. -#define ALIENFLIGHT +#define TARGET_CONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c new file mode 100644 index 000000000..9defa6223 --- /dev/null +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -0,0 +1,100 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// alternative defaults settings for AlienFlight targets +void targetConfiguration(void) { + featureClear(FEATURE_ONESHOT125); + masterConfig.mag_hardware = MAG_NONE; // disabled by default + masterConfig.rxConfig.spektrum_sat_bind = 5; + masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; + masterConfig.motor_pwm_rate = 32000; + masterConfig.failsafeConfig.failsafe_delay = 2; + masterConfig.failsafeConfig.failsafe_off_delay = 0; + currentControlRateProfile->rates[FD_PITCH] = 40; + currentControlRateProfile->rates[FD_ROLL] = 40; + currentControlRateProfile->rates[FD_YAW] = 40; + parseRcChannels("TAER1234", &masterConfig.rxConfig); + + masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif +} diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 98387ee23..54983b3d2 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -17,6 +17,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF4" +#define TARGET_CONFIG #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) @@ -153,9 +154,6 @@ //#define WS2811_DMA_CHANNEL DMA1_Channel3 //#define WS2811_IRQ DMA1_Channel3_IRQn -// alternative defaults for AlienFlight F4 target -#define ALIENFLIGHT - #define SPEKTRUM_BIND // USART2, PA3 #define BIND_PIN PA3 diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c new file mode 100644 index 000000000..ff958beb5 --- /dev/null +++ b/src/main/target/COLIBRI_RACE/config.c @@ -0,0 +1,84 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// alternative defaults settings for COLIBRI RACE targets +void targetConfiguration(void) { + masterConfig.escAndServoConfig.minthrottle = 1025; + masterConfig.escAndServoConfig.maxthrottle = 1980; + masterConfig.batteryConfig.vbatmaxcellvoltage = 45; + masterConfig.batteryConfig.vbatmincellvoltage = 30; +} diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 2139ebb37..0492926e5 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -20,6 +20,7 @@ #define TARGET_BOARD_IDENTIFIER "CLBR" #define BST_DEVICE_NAME "COLIBRI RACE" #define BST_DEVICE_NAME_LENGTH 12 +#define TARGET_CONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c new file mode 100644 index 000000000..e0592bbf9 --- /dev/null +++ b/src/main/target/NAZE/config.c @@ -0,0 +1,99 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// alternative defaults settings for AlienFlight targets +void targetConfiguration(void) { + featureClear(FEATURE_ONESHOT125); + masterConfig.rxConfig.spektrum_sat_bind = 5; + masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; + masterConfig.motor_pwm_rate = 32000; + masterConfig.failsafeConfig.failsafe_delay = 2; + masterConfig.failsafeConfig.failsafe_off_delay = 0; + currentControlRateProfile->rates[FD_PITCH] = 40; + currentControlRateProfile->rates[FD_ROLL] = 40; + currentControlRateProfile->rates[FD_YAW] = 40; + parseRcChannels("TAER1234", &masterConfig.rxConfig); + + masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif +} diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index cb2f57539..cc8b93c09 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -172,7 +172,7 @@ // alternative defaults for AlienFlight F1 target #undef TARGET_BOARD_IDENTIFIER #define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1. -#define ALIENFLIGHT +#define TARGET_CONFIG #undef BOARD_HAS_VOLTAGE_DIVIDER #undef USE_SERIAL_4WAY_BLHELI_INTERFACE From 0b48943a3cb376aebfa22098a8452ecd782e0a2b Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 5 Jul 2016 22:03:18 +1000 Subject: [PATCH 53/54] Fix ADC STM32F3 define --- src/main/drivers/adc_stm32f30x.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 3fc7427b5..0116a6734 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -117,13 +117,13 @@ void adcInit(drv_adc_config_t *init) } #endif -#ifdef CURRENT_METER_ADC_GPIO +#ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); } #endif -#ifdef EXTERNAL1_ADC_GPIO +#ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); } From b274289f0c8233be35dbf857026cf3a4409937b7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 5 Jul 2016 16:48:31 +0200 Subject: [PATCH 54/54] Move PPM pin for KISS FC to PITCH --- src/main/target/KISSFC/target.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 07450ad50..cf260d6d9 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -22,7 +22,7 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM7 | (MAP_TO_PPM_INPUT << 8), + PWM9 | (MAP_TO_PPM_INPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -44,7 +44,7 @@ const uint16_t multiPWM[] = { PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), PWM9 | (MAP_TO_PWM_INPUT << 8), PWM10 | (MAP_TO_PWM_INPUT << 8), @@ -71,7 +71,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1},