From 8fce0c2e6e0453422f7ba0133f0925f5708b870b Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Thu, 4 Aug 2016 13:48:03 +0300 Subject: [PATCH] Add support for Colibri fc, used in Gemini racer hex --- src/main/drivers/flash_m25p16.c | 5 + src/main/target/COLIBRI/config.c | 99 +++++++++++++++++++ src/main/target/COLIBRI/target.c | 123 ++++++++++++++++++++++++ src/main/target/COLIBRI/target.h | 148 +++++++++++++++++++++++++++++ src/main/target/COLIBRI/target.mk | 11 +++ src/main/target/system_stm32f4xx.c | 4 + 6 files changed, 390 insertions(+) create mode 100644 src/main/target/COLIBRI/config.c create mode 100644 src/main/target/COLIBRI/target.c create mode 100644 src/main/target/COLIBRI/target.h create mode 100644 src/main/target/COLIBRI/target.mk diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 0ab3d6122b..a0afb64804 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -43,6 +43,7 @@ #define JEDEC_ID_MICRON_M25P16 0x202015 #define JEDEC_ID_MICRON_N25Q064 0x20BA17 #define JEDEC_ID_WINBOND_W25Q64 0xEF4017 +#define JEDEC_ID_MACRONIX_MX25L3206E 0xC22016 #define JEDEC_ID_MACRONIX_MX25L6406E 0xC22017 #define JEDEC_ID_MICRON_N25Q128 0x20ba18 #define JEDEC_ID_WINBOND_W25Q128 0xEF4018 @@ -161,6 +162,10 @@ static bool m25p16_readIdentification() geometry.sectors = 32; geometry.pagesPerSector = 256; break; + case JEDEC_ID_MACRONIX_MX25L3206E: + geometry.sectors = 64; + geometry.pagesPerSector = 256; + break; case JEDEC_ID_MICRON_N25Q064: case JEDEC_ID_WINBOND_W25Q64: case JEDEC_ID_MACRONIX_MX25L6406E: diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c new file mode 100644 index 0000000000..7d20ab1d90 --- /dev/null +++ b/src/main/target/COLIBRI/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 + +#include "platform.h" +#include "debug.h" + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.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/Gemini targets +void targetConfiguration(void) +{ + masterConfig.mixerMode = MIXER_HEX6X; + masterConfig.rxConfig.serialrx_provider = 2; + featureSet(FEATURE_RX_SERIAL); + + masterConfig.escAndServoConfig.minthrottle = 1070; + masterConfig.escAndServoConfig.maxthrottle = 2000; + + masterConfig.boardAlignment.pitchDegrees = 10; + //masterConfig.rcControlsConfig.deadband = 10; + //masterConfig.rcControlsConfig.yaw_deadband = 10; + masterConfig.mag_hardware = 1; + + masterConfig.profile[0].controlRateProfile[0].dynThrPID = 45; + masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint = 1700; + masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; +} diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c new file mode 100644 index 0000000000..dd87343119 --- /dev/null +++ b/src/main/target/COLIBRI/target.c @@ -0,0 +1,123 @@ +/* + * 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/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), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_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), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + 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), + 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), // Swap to servo if needed + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t airPPM[] = { + 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), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (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), + 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_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 + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM1 }, // S1_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S2_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM2 }, // S5_IN + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM2 }, // S6_IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM5 }, // S7_IN + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM5 }, // S8_IN + + + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S3_OUT + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12 }, // S4_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S5_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12 }, // S6_OUT + { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM10 }, // S7_OUT + { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM11 }, // S8_OUT +}; diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h new file mode 100644 index 0000000000..77cc0f2f57 --- /dev/null +++ b/src/main/target/COLIBRI/target.h @@ -0,0 +1,148 @@ +/* + * 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 PLL_M 16 +#define PLL_N 336 + +#define TARGET_BOARD_IDENTIFIER "COLI" + +#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) + +#define USBD_PRODUCT_STRING "Colibri" +#ifdef OPBL +#define USBD_SERIALNUMBER_STRING "0x8020000" +#endif + +#define LED0 PC14 +#define LED1 PC13 +#define BEEPER PC5 +#define INVERTER PB2 // PB2 used as inverter select GPIO +#define INVERTER_USART USART2 + +#define MPU6000_CS_PIN PC4 +#define MPU6000_SPI_INSTANCE SPI1 + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG_FLIP + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG_FLIP + +// MPU6000 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PC0 +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) + +#define MAG +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW270_DEG_FLIP + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH +#define MAG_INT_EXTI PC1 + +#define BARO +#define USE_BARO_MS5611 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + + +#define USE_VCP +#define VBUS_SENSING_PIN PA9 + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +//#define USE_UART4 +#define UART4_RX_PIN PC11 +#define UART4_TX_PIN PC10 + +//#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define SERIAL_PORT_COUNT 4 //VCP, UART1, UART2, UART3 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PC4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_3) +#define I2C3_SCL PA8 +#define I2C3_SDA PC9 + +#define SENSORS_SET (SENSOR_ACC) + +#define LED_STRIP + +#define WS2811_PIN PB7 // Shared UART1 +#define WS2811_TIMER TIM4 +#define WS2811_TIMER_CHANNEL TIM_Channel_2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST3_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream3 +#define WS2811_DMA_FLAG DMA_FLAG_TCIF3 +#define WS2811_DMA_IT DMA_IT_TCIF3 +#define WS2811_DMA_CHANNEL DMA_Channel_2 +#define WS2811_DMA_IRQ DMA1_Stream3_IRQn + +// alternative defaults for Colibri/Gemini target +#define TARGET_CONFIG + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) + +#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 USABLE_TIMER_CHANNEL_COUNT 16 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11)) diff --git a/src/main/target/COLIBRI/target.mk b/src/main/target/COLIBRI/target.mk new file mode 100644 index 0000000000..a3d0a413d8 --- /dev/null +++ b/src/main/target/COLIBRI/target.mk @@ -0,0 +1,11 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH +HSE_VALUE = 16000000 + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c \ + diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index 5a6d90059b..879acff784 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -371,7 +371,11 @@ uint32_t hse_value = HSE_VALUE; /************************* PLL Parameters *************************************/ #if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F469_479xx) /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ +#if defined(COLIBRI) + #define PLL_M 16 +#else #define PLL_M 8 +#endif #elif defined (STM32F446xx) #define PLL_M 8 #elif defined (STM32F410xx) || defined (STM32F411xE)