From d28bac2b89a6df07c526801d30ba3cc0dc0da392 Mon Sep 17 00:00:00 2001 From: night-ghost Date: Sun, 13 May 2018 19:51:19 +0500 Subject: [PATCH] AP_HAL_F4Light: new board: OmnibusV3 --- .../boards/f4light_OmnibusV3/1_read_me.md | 46 ++ .../boards/f4light_OmnibusV3/board.cpp | 211 ++++++++ .../boards/f4light_OmnibusV3/board.h | 217 ++++++++ .../f4light_OmnibusV3/ld/flash-10000.ld | 183 +++++++ .../boards/f4light_OmnibusV3/ld/flash.ld | 1 + .../f4light_OmnibusV3/ld/flash_8000000.ld | 210 ++++++++ .../boards/f4light_OmnibusV3/rules.mk | 56 ++ .../boards/f4light_OmnibusV3/stm32f4xx_conf.h | 82 +++ .../f4light_OmnibusV3/support/DEBUG-STLINK.sh | 5 + .../f4light_OmnibusV3/support/Rebuild.sh | 21 + .../f4light_OmnibusV3/support/UPLOAD-DFU.sh | 7 + .../support/UPLOAD-STLINK.sh | 9 + .../support/UPLOAD-plane-STLINK.sh | 9 + .../support/bl/UPLOAD-STLINK.sh | 12 + .../support/bl/UPLOAD-STLINK_res.sh | 10 + .../support/bl/UPLOAD-plane-STLINK.sh | 12 + .../f4light_OmnibusV3/system_stm32f4xx.c | 489 ++++++++++++++++++ .../boards/f4light_OmnibusV3/target-config.mk | 26 + libraries/AP_HAL_F4Light/support/MMM.sh | 3 + 19 files changed, 1609 insertions(+) create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/1_read_me.md create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/board.cpp create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/board.h create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash-10000.ld create mode 120000 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash.ld create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash_8000000.ld create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/rules.mk create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/stm32f4xx_conf.h create mode 100755 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/DEBUG-STLINK.sh create mode 100755 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/Rebuild.sh create mode 100755 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-DFU.sh create mode 100755 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-STLINK.sh create mode 100755 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-plane-STLINK.sh create mode 100755 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-STLINK.sh create mode 100755 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-STLINK_res.sh create mode 100755 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-plane-STLINK.sh create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/system_stm32f4xx.c create mode 100644 libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/target-config.mk diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/1_read_me.md b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/1_read_me.md new file mode 100644 index 0000000000..b06e0ff64a --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/1_read_me.md @@ -0,0 +1,46 @@ +this is for AirbotV2, AirbotV3, Also Known As Omnibus boards + +# Board connection + +Just see board's documentation. + + +## Default connection +| Function | Serial | +| ------ | ------ | +| USB | Serial0 in MP | +| Telemetry | UART1 (Serial1) | +| GPS | UART6 (Serial3) | +| Built-in OSD | Serial2 | + +This board REQUIRES external Compass via I2C bus. + +## OSD +Built-in OSD can be configured via files in root directory of SD card: +- eeprom.osd is configuration, exported from Configuration Tool. +- font.mcm is font (select one of https://github.com/night-ghost/minimosd-extra/tree/master/Released/FW_%2B_Char). This file will be deleted after flashing. + +Firmware supports connection to built-in OSD with CT from my MinimOSD (https://github.com/night-ghost/minimosd-extra). To do this: +- set BRD_CONNECT_COM parameter to OSD's Serial (usually 2), then reboot / power cycle +- USB will be connected to OSD after reboot, supported load/store/fonts in MAVLink mode + +OSD will work better when VSYNC out from MAX connected to PC3 (R8 to Vcc). + +## Voltage and current reading + +How to get voltage/current reading(tested on omnibus, should work on other targets to): +- BAT_MONITOR 4 +- BAT_VOLT_PIN 8 +- BAT_CURR_PIN 7 +- BAT_VOLT_MULT 11.0 (or 10.1 for apm power module) +- BAT_AMP_PERVOLT 38.0 (or 17 for apm power module) + +Don't try to configure Curr/Vol reading from Initial setup page of MP, because VOL/CURR variables will be reset. + +Attention! + +If you select PPM (both via jumper or removing 0 ohm resistor) UART1 is no more used for RC IN and can be +used for telemetry (Serial1 on MP settings). + +Once PPM is selected you can use this pin for RC IN with PPM/SBUS/DSM, the parser in the HAL is able to understand +which protocol are you using and to decode it properly. diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/board.cpp b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/board.cpp new file mode 100644 index 0000000000..2cc311cbc3 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/board.cpp @@ -0,0 +1,211 @@ +#ifndef BOARD_STM32V1F4 +#define BOARD_STM32V1F4 + +#include +#include "../../SPIDevice.h" +#include + +using namespace F4Light; + +extern const stm32_pin_info PIN_MAP[BOARD_NR_GPIO_PINS] __FLASH__ = { + + /* Top header */ +/* + const gpio_dev * const gpio_device; < Maple pin's GPIO device + const timer_dev * const timer_device; < Pin's timer device, if any. + const adc_dev * const adc_device; < ADC device, if any. + uint8_t gpio_bit; < Pin's GPIO port bit. + uint8_t timer_channel; < Timer channel, or 0 if none. + uint8_t adc_channel; < Pin ADC channel, or ADCx if none. +*/ + + {&gpiob, NULL, NULL, 10, NO_CH, nADC}, /* D0/PB10 0 USART3_TX/I2C2-SCL */ + {&gpiob, NULL, NULL, 2, NO_CH, nADC}, /* D1/PB2 1*/ + {&gpiob, NULL, NULL, 12, NO_CH, nADC}, /* D2/PB12 2 SDCARD CS pin */ + {&gpiob, NULL, NULL, 13, NO_CH, nADC}, /* D3/PB13 3 SPI2_SCK */ + {&gpiob,&timer12,NULL, 14, TIMER_CH1, nADC}, /* D4/PB14 4 SPI2_MOSI */ + {&gpiob,&timer12,NULL, 15, TIMER_CH2, nADC}, /* D5/PB15 5 SPI2_MISO */ + {&gpioc, NULL,&_adc1, 0, NO_CH, 10}, /* D6/PC0 6 NC */ + {&gpioc, NULL,&_adc1, 1, NO_CH, 11}, /* D7/PC1 7 AMP */ + {&gpioc, NULL,&_adc1, 2, NO_CH, 12}, /* D8/PC2 8 VOLT */ + {&gpioc, NULL,&_adc1, 3, NO_CH, 13}, /* D9/PC3 9 freq sense - resistor to VCC, used asd MAX7456 VSYNC */ + {&gpioc, NULL,&_adc1, 4, NO_CH, 14}, /* D10/PC4 10 EXTI_MPU6000 */ + {&gpioc, NULL,&_adc1, 5, NO_CH, 15}, /* D11/PC5 1 USB_SENSE */ + {&gpioc, &timer8,NULL, 6, TIMER_CH1, nADC}, /* D12/PC6 2 CH3_IN / UART6_TX */ + {&gpioc, &timer8,NULL, 7, TIMER_CH2, nADC}, /* D13/PC7 3 CH4_IN / UART6_RX */ + {&gpioc, &timer8,NULL, 8, TIMER_CH3, nADC}, /* D14/PC8 4 CH5_IN / S_scl */ + {&gpioc, &timer8,NULL, 9, TIMER_CH4, nADC}, /* D15/PC9 5 CH6_IN / S_sda */ + {&gpioc, NULL, NULL, 10, NO_CH, nADC}, /* D16/PC10 6 SPI3_SCLK */ + {&gpioc, NULL, NULL, 11, NO_CH, nADC}, /* D17/PC11 7 SPI3_MISO */ + {&gpioc, NULL, NULL, 12, NO_CH, nADC}, /* D18/PC12 8 SPI3_MOSI */ + {&gpioc, NULL, NULL, 13, NO_CH, nADC}, /* D19/PC13 9 NOT CONNECTED */ + {&gpioc, NULL, NULL, 14, NO_CH, nADC}, /* D20/PC14 20 NOT CONNECTED */ + {&gpioc, NULL, NULL, 15, NO_CH, nADC}, /* D21/PC15 1 NOT CONNECTED */ + {&gpioa, &timer1,NULL, 8, TIMER_CH1, nADC}, /* D22/PA8 2 SERVO6 */ + {&gpioa, &timer1,NULL, 9, TIMER_CH2, nADC}, /* D23/PA9 3 USART1_TX */ + {&gpioa, &timer1,NULL, 10, TIMER_CH3, nADC}, /* D24/PA10 4 USART1_RX */ + {&gpiob, &timer4,NULL, 9, TIMER_CH4, nADC}, /* D25/PB9 5 PPM_in2 */ + {&gpiod, NULL, NULL, 2, NO_CH, nADC}, /* D26/PD2 6 EXTI_RFM22B / UART5_RX */ + {&gpiod, NULL, NULL, 3, NO_CH, nADC}, /* D27/PD3 7*/ + {&gpiod, NULL, NULL, 6, NO_CH, nADC}, /* D28/PD6 8*/ + {&gpiog, NULL, NULL, 11, NO_CH, nADC}, /* D29/PG11 9*/ + {&gpiog, NULL, NULL, 12, NO_CH, nADC}, /* D30/PG12 30*/ + {&gpiog, NULL, NULL, 13, NO_CH, nADC}, /* D31/PG13 1*/ + {&gpiog, NULL, NULL, 14, NO_CH, nADC}, /* D32/PG14 2*/ + {&gpiog, NULL, NULL, 8, NO_CH, nADC}, /* D33/PG8 3*/ + {&gpiog, NULL, NULL, 7, NO_CH, nADC}, /* D34/PG7 4*/ + {&gpiog, NULL, NULL, 6, NO_CH, nADC}, /* D35/PG6 5*/ + {&gpiob, &timer3,NULL, 5, TIMER_CH2, nADC}, /* D36/PB5 6 LED_BLUE */ + {&gpiob, &timer4,NULL, 6, TIMER_CH1, nADC}, /* D37/PB6 7 RCD_CS on SPI_3*/ + {&gpiob, &timer4,NULL, 7, TIMER_CH2, nADC}, /* D38/PB7 8 SD_DET */ + {&gpiof, NULL,&_adc3, 6, NO_CH, 4}, /* D39/PF6 9*/ + {&gpiof, NULL,&_adc3, 7, NO_CH, 5}, /* D40/PF7 40*/ + {&gpiof, NULL,&_adc3, 8, NO_CH, 6}, /* D41/PF8 1*/ + {&gpiof, NULL,&_adc3, 9, NO_CH, 7}, /* D42/PF9 2*/ + {&gpiof, NULL,&_adc3,10, NO_CH, 8}, /* D43/PF10 3*/ + {&gpiof, NULL, NULL, 11, NO_CH, nADC}, /* D44/PF11 4*/ + {&gpiob, &timer3,&_adc1,1, TIMER_CH4, 9}, /* D45/PB1 5 SERVO2 */ + {&gpiob, &timer3,&_adc1,0, TIMER_CH3, 8}, /* D46/PB0 6 SERVO1 */ + {&gpioa, &timer2,&_adc1,0, TIMER_CH1, 0}, /* D47/PA0 7 RSSI input */ + {&gpioa, &timer2,&_adc1,1, TIMER_CH2, 1}, /* D48/PA1 8 SERVO5 / UART4_RX */ + {&gpioa, &timer2,&_adc1,2, TIMER_CH3, 2}, /* D49/PA2 9 SERVO4 */ + {&gpioa, &timer2,&_adc1,3, TIMER_CH4, 3}, /* D50/PA3 50 SERVO3 */ + {&gpioa, NULL, &_adc1,4, NO_CH, 4}, /* D51/PA4 1 CS_MPU6000 */ + {&gpioa, NULL, &_adc1,5, NO_CH, 5}, /* D52/PA5 2 SPI1_CLK */ + {&gpioa, &timer3,&_adc1,6, TIMER_CH1, 6}, /* D53/PA6 3 SPI1_MISO */ + {&gpioa, &timer3,&_adc1,7, TIMER_CH2, 7}, /* D54/PA7 4 SPI1_MOSI */ + {&gpiof, NULL, NULL, 0, NO_CH, nADC}, /* D55/PF0 5*/ + {&gpiod, NULL, NULL, 11, NO_CH, nADC}, /* D56/PD11 6*/ + {&gpiod, &timer4,NULL, 14, TIMER_CH3, nADC}, /* D57/PD14 7*/ + {&gpiof, NULL, NULL, 1, NO_CH, nADC}, /* D58/PF1 8*/ + {&gpiod, &timer4,NULL, 12, TIMER_CH1, nADC}, /* D59/PD12 9*/ + {&gpiod, &timer4,NULL, 15, TIMER_CH4, nADC}, /* D60/PD15 60*/ + {&gpiof, NULL, NULL, 2, NO_CH, nADC}, /* D61/PF2 1*/ + {&gpiod, &timer4,NULL, 13, TIMER_CH2, nADC}, /* D62/PD13 2*/ + {&gpiod, NULL, NULL, 0, NO_CH, nADC}, /* D63/PD0 3*/ + {&gpiof, NULL, NULL, 3, NO_CH, nADC}, /* D64/PF3 4*/ + {&gpioe, NULL, NULL, 3, NO_CH, nADC}, /* D65/PE3 5*/ + {&gpiod, NULL, NULL, 1, NO_CH, nADC}, /* D66/PD1 6*/ + {&gpiof, NULL, NULL, 4, NO_CH, nADC}, /* D67/PF4 7*/ + {&gpioe, NULL, NULL, 4, NO_CH, nADC}, /* D68/PE4 8*/ + {&gpioe, NULL, NULL, 7, NO_CH, nADC}, /* D69/PE7 9*/ + {&gpiof, NULL, NULL, 5, NO_CH, nADC}, /* D70/PF5 70*/ + {&gpioe, NULL, NULL, 5, NO_CH, nADC}, /* D71/PE5 1*/ + {&gpioe, NULL, NULL, 8, NO_CH, nADC}, /* D72/PE8 2*/ + {&gpiof, NULL, NULL, 12, NO_CH, nADC}, /* D73/PF12 3*/ + {&gpioe, NULL, NULL, 6, NO_CH, nADC}, /* D74/PE6 4*/ + {&gpioe, &timer1,NULL, 9, TIMER_CH1, nADC}, /* D75/PE9 */ + {&gpiof, NULL, NULL, 13, NO_CH, nADC}, /* D76/PF13 6*/ + {&gpioe, NULL, NULL, 10, NO_CH, nADC}, /* D77/PE10 7*/ + {&gpiof, NULL, NULL, 14, NO_CH, nADC}, /* D78/PF14 8*/ + {&gpiog, NULL, NULL, 9, NO_CH, nADC}, /* D79/PG9 9*/ + {&gpioe, &timer1,NULL, 11, TIMER_CH2, nADC}, /* D80/PE11 */ + {&gpiof, NULL, NULL, 15, NO_CH, nADC}, /* D81/PF15 1*/ + {&gpiog, NULL, NULL, 10, NO_CH, nADC}, /* D82/PG10 2*/ + {&gpioe, NULL, NULL, 12, NO_CH, nADC}, /* D83/PE12 3*/ + {&gpiog, NULL, NULL, 0, NO_CH, nADC}, /* D84/PG0 4*/ + {&gpiod, NULL, NULL, 5, NO_CH, nADC}, /* D85/PD5 5*/ + {&gpioe, &timer1,NULL, 13, TIMER_CH3, nADC}, /* D86/PE13 */ + {&gpiog, NULL, NULL, 1, NO_CH, nADC}, /* D87/PG1 7*/ + {&gpiod, NULL, NULL, 4, NO_CH, nADC}, /* D88/PD4 8*/ + {&gpioe, &timer1,NULL, 14, TIMER_CH4, nADC}, /* D89/PE14 */ + {&gpiog, NULL, NULL, 2, NO_CH, nADC}, /* D90/PG2 90*/ + {&gpioe, NULL, NULL, 1, NO_CH, nADC}, /* D91/PE1 1*/ + {&gpioe, NULL, NULL, 15, NO_CH, nADC}, /* D92/PE15 2*/ + {&gpiog, NULL, NULL, 3, NO_CH, nADC}, /* D93/PG3 3*/ + {&gpioe, NULL, NULL, 0, NO_CH, nADC}, /* D94/PE0 4*/ + {&gpiod, NULL, NULL, 8, NO_CH, nADC}, /* D95/PD8 5*/ + {&gpiog, NULL, NULL, 4, NO_CH, nADC}, /* D96/PG4 6*/ + {&gpiod, NULL, NULL, 9, NO_CH, nADC}, /* D97/PD9 7*/ + {&gpiog, NULL, NULL, 5, NO_CH, nADC}, /* D98/PG5 8*/ + {&gpiod, NULL, NULL, 10, NO_CH, nADC}, /* D99/PD10 9*/ + {&gpiob, NULL, NULL, 11, NO_CH, nADC}, /* D100/PB11 100 USART3_RX/I2C2-SDA */ + {&gpiob, &timer4,NULL, 8, TIMER_CH3, nADC}, /* D101/PB8 I2C1_SCL PPM_IN */ + {&gpioe, NULL, NULL, 2, NO_CH, nADC}, /* D102/PE2 */ + {&gpioa, NULL, NULL, 15, NO_CH, nADC}, /* D103/PA15 CS_OSD */ + {&gpiob, NULL, NULL, 3, NO_CH, nADC}, /* D104/PB3 CS_BARO */ + {&gpiob, NULL, NULL, 4, NO_CH, nADC}, /* D105/PB4 Buzzer */ + {&gpioa, NULL, NULL, 13, NO_CH, nADC}, /* D106/PA13 LED_MOTOR - SWDIO */ + {&gpioa, NULL, NULL, 14, NO_CH, nADC}, /* D107/PA14 */ + {&gpioa, NULL, NULL, 11, NO_CH, nADC}, /* D108/PA11 - USB D- */ + +}; + + +extern const struct TIM_Channel PWM_Channels[] __FLASH__ = { +//CH1 and CH2 also for PPMSUM / SBUS / DSM + { // 0 RC_IN1 + .pin = 101, + }, + { // 1 RC_IN2 + .pin = 25, + }, + { // 2 RC_IN3 + .pin = 12, + }, + { // 3 RC_IN4 + .pin = 13, + }, + { // 4 RC_IN5 + .pin = 14, + }, + { // 5 RC_IN6 + .pin = 15, + }, +}; + + +/* + DMA modes: + +0 - disable +1 - enable on large transfers +2 - enable alaways + +*/ +extern const SPIDesc spi_device_table[] = { // different SPI tables per board subtype +// name device bus mode cs_pin speed_low speed_high dma priority + { BOARD_INS_MPU60x0_NAME, _SPI1, 1, SPI_MODE_0, BOARD_MPU6000_CS_PIN, SPI_1_125MHZ, SPI_9MHZ, SPI_TRANSFER_DMA, DMA_Priority_VeryHigh, 1, 5 }, + { BOARD_SDCARD_NAME, _SPI2, 2, SPI_MODE_3, 255, SPI_1_125MHZ, SPI_18MHZ, SPI_TRANSFER_DMA, DMA_Priority_Medium, 0, 0 }, + { HAL_BARO_BMP280_NAME, _SPI3, 3, SPI_MODE_3, BOARD_BMP280_CS_PIN, SPI_1_125MHZ, SPI_9MHZ, SPI_TRANSFER_DMA, DMA_Priority_High, 1, 1 }, + { BOARD_OSD_NAME, _SPI3, 3, SPI_MODE_0, BOARD_OSD_CS_PIN, SPI_1_125MHZ, SPI_4_5MHZ, SPI_TRANSFER_DMA, DMA_Priority_Low, 2, 2 }, +}; + +extern const uint8_t F4Light_SPI_DEVICE_NUM_DEVICES = ARRAY_SIZE(spi_device_table); + +void boardInit(void) { + +/* we don't use RFM22! this pins are used for other needs so will be initialized in respective places + + // Init RFM22B SC pin and set to HI + gpio_set_mode( PIN_MAP[BOARD_RFM22B_CS_PIN].gpio_device, PIN_MAP[BOARD_RFM22B_CS_PIN].gpio_bit, GPIO_OUTPUT_PP); + gpio_write_bit(PIN_MAP[BOARD_RFM22B_CS_PIN].gpio_device, PIN_MAP[BOARD_RFM22B_CS_PIN].gpio_bit, 1); + + // Init RFM22B EXT_INT pin + gpio_set_mode(PIN_MAP[BOARD_RFM22B_INT_PIN].gpio_device, PIN_MAP[BOARD_RFM22B_INT_PIN].gpio_bit, GPIO_INPUT_PU); +*/ + +#ifdef BOARD_HMC5883_DRDY_PIN + // Init HMC5883 DRDY EXT_INT pin - but it not used by driver + gpio_set_mode(PIN_MAP[BOARD_HMC5883_DRDY_PIN].gpio_device, PIN_MAP[BOARD_HMC5883_DRDY_PIN].gpio_bit, GPIO_INPUT_PU); +#endif + +#ifdef BOARD_MPU6000_DRDY_PIN + // Init MPU6000 DRDY pin - but it not used by driver + gpio_set_mode(PIN_MAP[BOARD_MPU6000_DRDY_PIN].gpio_device, PIN_MAP[BOARD_MPU6000_DRDY_PIN].gpio_bit, GPIO_INPUT_PU); +#endif + +#ifdef BOARD_SBUS_INVERTER_RX + gpio_set_mode( PIN_MAP[BOARD_SBUS_INVERTER_RX].gpio_device, PIN_MAP[BOARD_SBUS_INVERTER_RX].gpio_bit, GPIO_OUTPUT_PP); + gpio_write_bit(PIN_MAP[BOARD_SBUS_INVERTER_RX].gpio_device, PIN_MAP[BOARD_SBUS_INVERTER_RX].gpio_bit, 0); // not inverted +#endif + +#ifdef BOARD_SBUS_INVERTER_TX + gpio_set_mode( PIN_MAP[BOARD_SBUS_INVERTER_TX].gpio_device, PIN_MAP[BOARD_SBUS_INVERTER_TX].gpio_bit, GPIO_OUTPUT_PP); + gpio_write_bit(PIN_MAP[BOARD_SBUS_INVERTER_TX].gpio_device, PIN_MAP[BOARD_SBUS_INVERTER_TX].gpio_bit, 0); // not inverted +#endif + +} + + +#endif diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/board.h b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/board.h new file mode 100644 index 0000000000..6774541cda --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/board.h @@ -0,0 +1,217 @@ +#ifndef _BOARD_STM32V1F4_H_ +#define _BOARD_STM32V1F4_H_ + + +/** + * @brief Configuration of the Cortex-M4 Processor and Core Peripherals + */ +#define __CM4_REV 0x0001 /*!< Core revision r0p1 */ +#define __MPU_PRESENT 1 /*!< STM32F4XX provides an MPU */ +#define __NVIC_PRIO_BITS 4 /*!< STM32F4XX uses 4 Bits for the Priority Levels */ +#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */ +#define __FPU_PRESENT 1 /*!< FPU present */ + +#define HSE_VALUE (8000000) + +#define CYCLES_PER_MICROSECOND (SystemCoreClock / 1000000) +#define SYSTICK_RELOAD_VAL (CYCLES_PER_MICROSECOND*1000-1) + +#undef STM32_PCLK1 +#undef STM32_PCLK2 +#define STM32_PCLK1 (CYCLES_PER_MICROSECOND*1000000/4) +#define STM32_PCLK2 (CYCLES_PER_MICROSECOND*1000000/2) + +#define BOARD_BUTTON_PIN 254 + +#ifndef LOW +# define LOW 0 +#endif +#ifndef HIGH +# define HIGH 1 +#endif + +#define BOARD_BUZZER_PIN 105 +#define HAL_BUZZER_ON 1 +#define HAL_BUZZER_OFF 0 + +#define BOARD_NR_USARTS 5 +#define BOARD_USART1_TX_PIN 23 +#define BOARD_USART1_RX_PIN 24 +#define BOARD_USART3_TX_PIN 0 +#define BOARD_USART3_RX_PIN 100 +#define BOARD_USART6_TX_PIN 12 +#define BOARD_USART6_RX_PIN 13 + +#define BOARD_USART4_RX_PIN 48 +#define BOARD_USART4_TX_PIN 47 + +//#define BOARD_USART5_RX_PIN 26 // PD2 EXTI_RFM22B / UART5_RX +//#define BOARD_BUTTON_PIN 103 // PA15 CS_RFM22B + +#define BOARD_DSM_USART (_USART1) + +#define BOARD_NR_SPI 3 +#define BOARD_SPI1_SCK_PIN 52 +#define BOARD_SPI1_MISO_PIN 53 +#define BOARD_SPI1_MOSI_PIN 54 +#define BOARD_SPI2_SCK_PIN 3 // PB13 +#define BOARD_SPI2_MISO_PIN 4 // PB14 +#define BOARD_SPI2_MOSI_PIN 5 // PB15 +#define BOARD_SPI3_MOSI_PIN 18 +#define BOARD_SPI3_MISO_PIN 17 +#define BOARD_SPI3_SCK_PIN 16 + +#define BOARD_MPU6000_CS_PIN 51 +#define BOARD_MPU6000_DRDY_PIN PC4 + +#define BOARD_SBUS_INVERTER_RX PC8 +#define BOARD_SBUS_INVERTER_TX PC9 + +// bus 2 (soft) pins +//#define BOARD_SOFT_SCL PC8 +//#define BOARD_SOFT_SDA PC9 + +// SoftSerial pins +//#define BOARD_SOFTSERIAL_TX PC8 +//#define BOARD_SOFTSERIAL_RX PC9 + +#define BOARD_USB_SENSE PC5 + + +# define BOARD_BLUE_LED_PIN 36 // BLUE +# define BOARD_GREEN_LED_PIN 6 // NC - PC0 pin 8 + +# define HAL_GPIO_A_LED_PIN BOARD_BLUE_LED_PIN +# define HAL_GPIO_B_LED_PIN BOARD_GREEN_LED_PIN + +# define HAL_GPIO_LED_ON LOW +# define HAL_GPIO_LED_OFF HIGH + + +#define BOARD_NR_GPIO_PINS 109 + +//TODO add #define BOARD_HAS_UART3 ? + +#define I2C1_SDA PB9 +#define I2C1_SCL PB8 + +#define I2C2_SDA PB11 +#define I2C2_SCL PB10 + + +//#define BOARD_I2C_BUS_INT 1 // hardware internal I2C +#define BOARD_I2C_BUS_EXT 1 // external I2C +#define BOARD_I2C_BUS_SLOW 1 // slow down bus with this number + +#define BOARD_I2C1_DISABLE // lots of drivers tries to scan all buses, spoiling device setup + +#define BOARD_BARO_DEFAULT HAL_BARO_BMP280_SPI +#define HAL_BARO_BMP280_NAME "bmp280" +#define BOARD_BMP280_CS_PIN 104 + +#define HAL_BARO_MS5611_I2C_BUS BOARD_I2C_BUS_EXT +#define HAL_BARO_MS5611_I2C_ADDR (0x77) + +#define BOARD_COMPASS_DEFAULT HAL_COMPASS_HMC5843 +#define BOARD_COMPASS_HMC5843_I2C_ADDR 0x1E +#define BOARD_COMPASS_HMC5843_ROTATION ROTATION_NONE + +#define HAL_COMPASS_HMC5843_I2C_BUS BOARD_I2C_BUS_EXT +#define HAL_COMPASS_HMC5843_I2C_ADDR BOARD_COMPASS_HMC5843_I2C_ADDR +#define HAL_COMPASS_HMC5843_ROTATION BOARD_COMPASS_HMC5843_ROTATION + + +#define BOARD_INS_DEFAULT HAL_INS_MPU60XX_SPI +#define BOARD_INS_ROTATION ROTATION_YAW_180 +#define BOARD_INS_MPU60x0_NAME "mpu6000" + +#define BOARD_STORAGE_SIZE 8192 // 4096 // EEPROM size + + +#define BOARD_SDCARD_NAME "sdcard" +#define BOARD_SDCARD_CS_PIN 2 +#define BOARD_SDCARD_DET_PIN 38 // PB7 + +#define BOARD_HAS_SDIO +#define HAL_BOARD_LOG_DIRECTORY "0:/APM/LOGS" +#define HAL_BOARD_TERRAIN_DIRECTORY "0:/APM/TERRAIN" +//#define HAL_PARAM_DEFAULTS_PATH "0:/APM/defaults.parm" +#define USB_MASSSTORAGE + +#define BOARD_OSD_NAME "osd" +#define BOARD_OSD_CS_PIN 103 +#define BOARD_OSD_VSYNC_PIN 9 // PC3, Frequency input +#define BOARD_OSD_RESET_PIN 6 // PC0, NC + + +#define BOARD_OWN_NAME "AirbotV2" + +# define BOARD_PUSHBUTTON_PIN 254 +# define BOARD_USB_MUX_PIN -1 +# define BOARD_BATTERY_VOLT_PIN 8 // Battery voltage on A0 (PC2) D8 +# define BOARD_BATTERY_CURR_PIN 7 // Battery current on A1 (PC1) D7 +# define BOARD_SONAR_SOURCE_ANALOG_PIN 254 + +# define HAL_BATT_VOLT_PIN 8 // ChibiOS compatible defines +# define HAL_BATT_CURR_PIN 8 +# define HAL_BATT_VOLT_SCALE 10.1 +# define HAL_BATT_CURR_SCALE 17 + + +#define BOARD_USB_DMINUS 108 + +#define BOARD_SBUS_UART 1 // can use some UART as hardware inverted input + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define BOARD_UARTS_LAYOUT 3 + +// use soft I2C driver instead hardware +//#define BOARD_SOFT_I2C + +#define SERVO_PIN_1 46 // PB0 +#define SERVO_PIN_2 45 // PB1 +#define SERVO_PIN_3 50 // PA3 +#define SERVO_PIN_4 49 // PA2 +#define SERVO_PIN_5 48 // PA1 +#define SERVO_PIN_6 22 // PA8 + +#define MOTOR_LAYOUT_DEFAULT 0 + +#if 1 + #define HAL_CONSOLE USB_Driver // console on USB + #define HAL_CONSOLE_PORT 0 +#else + #define HAL_CONSOLE uart1Driver // console on radio + #define HAL_CONSOLE_PORT 1 +#endif + +/* + // @Param: USB_STORAGE + // @DisplayName: allows access to SD card at next reboot + // @Description: Allows to read/write internal SD card via USB mass-storage protocol. Auto-reset. + // @Values: 0:normal, 1:work as USB flash drive + // @User: Advanced + AP_GROUPINFO("USB_STORAGE", 8, AP_Param_Helper, _usb_storage, 0), \ + + // @Param: SD_REFORMAT + // @DisplayName: Allows to re-format SD card in case of errors in FS + // @Description: Any FS errors that cause failure of logging will be corrected by SD card formatting + // @Values: 0: not allow, 1:allow + // @User: Advanced + AP_GROUPINFO("SD_REFORMAT", 7, AP_Param_Helper, _sd_format, 0), + +*/ + +#define BOARD_HAL_VARINFO \ + AP_GROUPINFO("USB_STORAGE", 30, AP_Param_Helper, _usb_storage, 0), \ + AP_GROUPINFO("SD_REFORMAT", 31, AP_Param_Helper, _sd_format, 0), + + +// parameters +#define BOARD_HAL_PARAMS \ + AP_Int8 _usb_storage; \ + AP_Int8 _sd_format; + +#endif + diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash-10000.ld b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash-10000.ld new file mode 100644 index 0000000000..8c6725a251 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash-10000.ld @@ -0,0 +1,183 @@ +/* +***************************************************************************** +** +** File : stm32_flash.ld +** +** Abstract : Linker script for STM32F407VG Device with +** 1024KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Environment : Atollic TrueSTUDIO(R) +** +** Distribution: The file is distributed “as is,” without any warranty +** of any kind. +** +** (c)Copyright Atollic AB. +** You may use this file as-is or modify it according to the needs of your +** project. Distribution of this file (unmodified or modified) is not +** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the +** rights to distribute the assembled, compiled & linked contents of this +** file as part of an application binary file, provided that it is built +** using the Atollic TrueSTUDIO(R) toolchain. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +/* _estack = 0x20020000; */ /* end of 128K RAM */ + _estack = 0x10010000; /* end of 64k CCM */ + +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0; /* required amount of heap */ +_Min_Stack_Size = 0x800; /* required amount of stack */ + +/* Specify the memory areas */ +MEMORY +{ + FLASH0 (rx) : ORIGIN = 0x08000000, LENGTH = 32K + FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 960K + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + CCM (rw) : ORIGIN = 0x10000000, LENGTH = 64K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + __isr_vector_start = .; + + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(.fini_array*)) + KEEP (*(SORT(.fini_array.*))) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = .; + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT> FLASH + + /* Uninitialized data section */ + . = ALIGN(4); + .bss (NOLOAD): + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + ._user_heap_stack (NOLOAD): + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; +/* . = . + _Min_Stack_Size; */ + . = ALIGN(4); + } >RAM + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + .ccm (NOLOAD): { + . = ALIGN(4); + _sccm = .; + *(.ccm) + . = ALIGN(4); + _eccm = .; + }>CCM + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash.ld b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash.ld new file mode 120000 index 0000000000..556e55212e --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash.ld @@ -0,0 +1 @@ +flash-10000.ld \ No newline at end of file diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash_8000000.ld b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash_8000000.ld new file mode 100644 index 0000000000..bd449964c1 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/ld/flash_8000000.ld @@ -0,0 +1,210 @@ +/* +***************************************************************************** +** +** File : stm32_flash.ld +** +** Abstract : Linker script for STM32F407VG Device with +** 1024KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Environment : Atollic TrueSTUDIO(R) +** +** Distribution: The file is distributed “as is,” without any warranty +** of any kind. +** +** (c)Copyright Atollic AB. +** You may use this file as-is or modify it according to the needs of your +** project. Distribution of this file (unmodified or modified) is not +** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the +** rights to distribute the assembled, compiled & linked contents of this +** file as part of an application binary file, provided that it is built +** using the Atollic TrueSTUDIO(R) toolchain. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +/* _estack = 0x20020000; */ /* end of 128K RAM */ + _estack = 0x10010000; /* end of 64k CCM */ + +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0; /* required amount of heap */ +_Min_Stack_Size = 0x800; /* required amount of stack */ + + +/* Specify the memory areas */ +MEMORY +{ + FLASH0 (rx) : ORIGIN = 0x08000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 960K + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + CCM (rw) : ORIGIN = 0x10000000, LENGTH = 64K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +/* Define output sections */ +SECTIONS +{ + + + /* The startup code goes first into FLASH */ + /*.isr_vector :*/ + .text0 : + { + . = ALIGN(4); + __isr_vector_start = .; + KEEP(*(.isr_vector)) /* Startup code */ + FILL(0xffff) + . = ALIGN(4); + } >FLASH0 + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + FILL(0xffff) + } >FLASH =0xFF + + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(.fini_array*)) + KEEP (*(SORT(.fini_array.*))) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = .; + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + FILL(0xffff) + } >RAM AT> FLASH + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + ._user_heap_stack : + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(4); + } >RAM + + + /* used by the startup to initialize data */ + _siccm = .; + +/* initialized CCM sections + .ccm (): { + . = ALIGN(4); + _sccm = .; + *(.ccm) + *(.ccm*) + . = ALIGN(4); + _eccm = .; + }>CCM AT> FLASH +*/ + + +/* empty CCM sections */ + .ccm (NOLOAD): { + . = ALIGN(4); + _sccm = .; + *(.ccm) + *(.ccm*) + . = ALIGN(4); + _eccm = .; + }>CCM + + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/rules.mk b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/rules.mk new file mode 100644 index 0000000000..25f7726328 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/rules.mk @@ -0,0 +1,56 @@ +# additional features for board + +# Standard things +sp := $(sp).x +dirstack_$(sp) := $(d) +d := $(dir) +BUILDDIRS += $(BUILD_PATH)/$(d) +#BUILDDIRS += $(BUILD_PATH)/$(d)/comm +BUILDDIRS += $(BUILD_PATH)/$(d)/../boards/$(BOARD) + +WIR := AP_HAL_F4Light/wirish +BRD := AP_HAL_F4Light/boards + +LIBRARY_INCLUDES += -I$(BRD)/$(BOARD) + +# Local flags +# always include board #defines +CFLAGS_$(d) := -Wall -Werror -include $(BRD)/$(BOARD)/board.h + + +# Local rules and targets +cSRCS_$(d) := +cSRCS_$(d) += $(BRD)/$(BOARD)/system_stm32f4xx.c # C startup code + +cppSRCS_$(d) := +cppSRCS_$(d) += $(BRD)/$(BOARD)/board.cpp +cppSRCS_$(d) += $(WIR)/boards.cpp + +sSRCS_$(d) := +sSRCS_$(d) += $(WIR)/startup.S # early startup code +sSRCS_$(d) += $(WIR)/exc.S # exception handling and task switching code + + +cFILES_$(d) := $(cSRCS_$(d):%=$(d)/%) +cppFILES_$(d) := $(cppSRCS_$(d):%=$(d)/%) +sFILES_$(d) := $(sSRCS_$(d):%=$(d)/%) + +OBJS_$(d) := $(cFILES_$(d):%.c=$(LIBRARIES_PATH)/%.o) +OBJS_$(d) += $(cppFILES_$(d):%.cpp=$(LIBRARIES_PATH)/%.o) +OBJS_$(d) += $(sFILES_$(d):%.S=$(LIBRARIES_PATH)/%.o) + +DEPS_$(d) := $(OBJS_$(d):%.o=%.d) + + +$(OBJS_$(d)): TGT_CFLAGS := $(CFLAGS_$(d)) + +TGT_BIN += $(OBJS_$(d)) + +# Standard things +-include $(DEPS_$(d)) +d := $(dirstack_$(sp)) +sp := $(basename $(sp)) + + +include $(HARDWARE_PATH)/osd/rules.mk +include $(HARDWARE_PATH)/massstorage/rules.mk diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/stm32f4xx_conf.h b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/stm32f4xx_conf.h new file mode 100644 index 0000000000..b6fc629997 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/stm32f4xx_conf.h @@ -0,0 +1,82 @@ +/** + ****************************************************************************** + * @file Project/STM32F4xx_StdPeriph_Templates/stm32f4xx_conf.h + * @author MCD Application Team + * @version V1.0.0 + * @date 30-September-2011 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F4xx_CONF_H +#define __STM32F4xx_CONF_H + +/* Uncomment the line below to expanse the "assert_param" macro in the + Standard Peripheral Library drivers code */ +/* #define USE_FULL_ASSERT 1 */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT + +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ + #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ + void assert(uint8_t* file, uint32_t line); +#else + #define assert_param(expr) ((void)0) +#endif /* USE_FULL_ASSERT */ + + +/* Includes ------------------------------------------------------------------*/ +/* Uncomment the line below to enable peripheral header file inclusion */ +#include "stm32f4xx_adc.h" +#include "stm32f4xx_can.h" +#include "stm32f4xx_dbgmcu.h" +#include "stm32f4xx_dma.h" +#include "stm32f4xx_exti.h" +#include "stm32f4xx_flash.h" +#include "stm32f4xx_gpio.h" +#include "stm32f4xx_i2c.h" +#include "stm32f4xx_iwdg.h" +#include "stm32f4xx_pwr.h" +#include "stm32f4xx_rcc.h" +#include "stm32f4xx_rtc.h" +#include "stm32f4xx_spi.h" +#include "stm32f4xx_syscfg.h" +#include "stm32f4xx_tim.h" +#include "stm32f4xx_usart.h" +#include "stm32f4xx_wwdg.h" +#include "stm32f4xx_nvic.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external + clock is used, keep this define commented */ +/*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ + + + +#endif /* __STM32F4xx_CONF_H */ + +/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/DEBUG-STLINK.sh b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/DEBUG-STLINK.sh new file mode 100755 index 0000000000..694c95d99e --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/DEBUG-STLINK.sh @@ -0,0 +1,5 @@ +#!/bin/sh + +/usr/local/stlink/st-util -m + + diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/Rebuild.sh b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/Rebuild.sh new file mode 100755 index 0000000000..5864578d3f --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/Rebuild.sh @@ -0,0 +1,21 @@ +#git submodule init && git submodule update +export TOOLCHAIN + +ROOT=`cd ../../../../..; pwd` + +export PATH=/usr/local/bin:$PATH + +echo $ROOT + + +( # AirBotF4 board + cd $ROOT/ArduCopter + make f4light-clean + make f4light VERBOSE=1 BOARD=f4light_OmnibusV3 +) && ( + cd $ROOT/ArduPlane + make f4light-clean + make f4light VERBOSE=1 BOARD=f4light_OmnibusV3 +) + +# at 4e017bf5b3da4f2a9ffc2e1cc0a37b94edac2bdc diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-DFU.sh b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-DFU.sh new file mode 100755 index 0000000000..ef44a8de0f --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-DFU.sh @@ -0,0 +1,7 @@ +#!/bin/sh + +#production binary for bootloader +dfu-util -a 0 --dfuse-address 0x08010000:unprotect:force -D ../../../../../ArduCopter/f4light_OmnibusV3.bin -R + + + diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-STLINK.sh b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-STLINK.sh new file mode 100755 index 0000000000..c43913c633 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-STLINK.sh @@ -0,0 +1,9 @@ +#!/bin/sh + +# production binary with bootloader +#/usr/local/stlink/st-flash --reset write /tmp/ArduCopter.build/f4light_Revolution.bin 0x08010000 + +#bare metal binary + +/usr/local/stlink/st-flash --reset write ../../../../../ArduCopter/f4light_OmnibusV3.bin 0x08010000 && \ +/usr/local/stlink/st-util -m diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-plane-STLINK.sh b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-plane-STLINK.sh new file mode 100755 index 0000000000..1653ead123 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/UPLOAD-plane-STLINK.sh @@ -0,0 +1,9 @@ +#!/bin/sh + +# production binary with bootloader +#/usr/local/stlink/st-flash --reset write /tmp/ArduCopter.build/f4light_Revolution.bin 0x08010000 + +#bare metal binary + +/usr/local/stlink/st-flash --reset write ../../../../../ArduPlane/f4light_OmnibusV3.bin 0x08010000 && \ +/usr/local/stlink/st-util -m \ No newline at end of file diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-STLINK.sh b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-STLINK.sh new file mode 100755 index 0000000000..82b3335c0f --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-STLINK.sh @@ -0,0 +1,12 @@ +#!/bin/sh + +# production binary without bootloader +#/usr/local/stlink/st-flash --reset write /tmp/ArduCopter.build/f4light_Revolution.bin 0x08010000 + +#bare metal binary or binary with bootloader +/usr/local/stlink/st-flash --reset read eeprom.bin 0x08004000 0xc000 && \ +/usr/local/stlink/st-flash --reset write ../../../../../../ArduCopter/f4light_OmnibusV3_bl.bin 0x08000000 && \ +/usr/local/stlink/st-flash --reset write eeprom.bin 0x08004000 && \ +/usr/local/stlink/st-util -m + + diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-STLINK_res.sh b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-STLINK_res.sh new file mode 100755 index 0000000000..46b2e7b2c2 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-STLINK_res.sh @@ -0,0 +1,10 @@ +#!/bin/sh + +# production binary without bootloader +#/usr/local/stlink/st-flash --reset write /tmp/ArduCopter.build/f4light_Revolution.bin 0x08010000 + +#bare metal binary or binary with bootloader +/usr/local/stlink/st-flash --reset write ../../../../../../ArduCopter/f4light_OmnibusV3_bl.bin 0x08000000 && \ +/usr/local/stlink/st-util -m + + diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-plane-STLINK.sh b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-plane-STLINK.sh new file mode 100755 index 0000000000..cf3bf91135 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/support/bl/UPLOAD-plane-STLINK.sh @@ -0,0 +1,12 @@ +#!/bin/sh + +# production binary with bootloader +#/usr/local/stlink/st-flash --reset write /tmp/ArduCopter.build/f4light_Revolution.bin 0x08010000 + +#bare metal binary +/usr/local/stlink/st-flash --reset read eeprom.bin 0x08004000 0xc000 && \ +/usr/local/stlink/st-flash --reset write ../../../../../../ArduPlane/f4light_OmnibusV3_bl.bin 0x08000000 +/usr/local/stlink/st-flash --reset write eeprom.bin 0x08004000 +/usr/local/stlink/st-util -m + + diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/system_stm32f4xx.c b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/system_stm32f4xx.c new file mode 100644 index 0000000000..97d299bc55 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/system_stm32f4xx.c @@ -0,0 +1,489 @@ +/** + ****************************************************************************** + * @file system_stm32f4xx.c + * @author MCD Application Team + * @version V1.0.0 + * @date 19-September-2011 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F4xx devices, + * and is generated by the clock configuration tool + * stm32f4xx_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - systemInit(oc): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f4xx.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (16 MHz) is used as system clock source. + * Then systemInit() function is called, in "startup_stm32f4xx.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the systemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 8 MHz, refer to "HSE_VALUE" define + * in "stm32f4xx.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + *============================================================================= + * Supported STM32F4xx device revision | Rev A + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 168000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 168000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 4 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *----------------------------------------------------------------------------- + * PLL_M | 8 + *----------------------------------------------------------------------------- + * PLL_N | 336 + *----------------------------------------------------------------------------- + * PLL_P | 2 + *----------------------------------------------------------------------------- + * PLL_Q | 7 + *----------------------------------------------------------------------------- + * PLLI2S_N | 192 + *----------------------------------------------------------------------------- + * PLLI2S_R | 5 + *----------------------------------------------------------------------------- + * I2S input clock(Hz) | 38400000 + *----------------------------------------------------------------------------- + * VDD(V) | 3.3 + *----------------------------------------------------------------------------- + * High Performance mode | Enabled + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 5 + *----------------------------------------------------------------------------- + * Prefetch Buffer | OFF + *----------------------------------------------------------------------------- + * Instruction cache | ON + *----------------------------------------------------------------------------- + * Data cache | ON + *----------------------------------------------------------------------------- + * Require 48MHz for USB OTG FS, | Enabled + * SDIO and RNG clock | + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ + +/* + M N Q P MHz + 4 168 7 2 168 + 4 360 15 4 180 + 4 192 8 2 192 + 4 216 9 2 216 + 4 240 10 2 240 + 4 264 11 2 264 + + +*/ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f4xx_system + * @{ + */ + +/** @addtogroup STM32F4xx_System_Private_Includes + * @{ + */ + +#include "stm32f4xx.h" + +void systemInit(uint8_t oc); + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Defines + * @{ + */ + + +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET FLASH_OFFSET /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ + + +/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ +#define PLL_M 4 +#define PLL_N 168 + +/* SYSCLK = PLL_VCO / PLL_P */ +#define PLL_P 2 + +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 7 + +/* PLLI2S_VCO = (HSE_VALUE Or HSI_VALUE / PLL_M) * PLLI2S_N + I2SCLK = PLLI2S_VCO / PLLI2S_R */ +#define PLLI2S_N 192 +#define PLLI2S_R 5 + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Variables + * @{ + */ + + uint32_t SystemCoreClock = 168000000; + + __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + + + + +/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes + * @{ + */ + +void SetSysClock(uint8_t oc); + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ +void systemInit(uint8_t oc) +{ + //SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR = 0x00000000; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset PLLCFGR register */ + RCC->PLLCFGR = 0x24003010; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + SetSysClock(oc); + + // Configure the Vector Table location add offset address ------------------ + extern unsigned __isr_vector_start; // from linker + SCB->VTOR = (uint32_t)&__isr_vector_start; +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value + * 16 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value + * 25 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate(void) +{ + uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock source */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock source */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock source */ + + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N + SYSCLK = PLL_VCO / PLL_P + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; + pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; + + if (pllsource != 0) + { + /* HSE used as PLL clock source */ + pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + else + { + /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + + pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; + SystemCoreClock = pllvco/pllp; + break; + default: + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK frequency --------------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK frequency */ + SystemCoreClock >>= tmp; +} + +extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag); + +/** + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @Note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in systemInit() function). + * @param None + * @retval None + */ +void SetSysClock(uint8_t oc) +{ +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + + /* Enable HSE */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Enable high performance mode, System frequency up to 168 MHz */ + RCC->APB1ENR |= RCC_APB1ENR_PWREN; + PWR->CR |= PWR_CR_PMODE; + + /* HCLK = SYSCLK / 1*/ + RCC->CFGR |= RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 2*/ + RCC->CFGR |= RCC_CFGR_PPRE2_DIV2; + + /* PCLK1 = HCLK / 4*/ + RCC->CFGR |= RCC_CFGR_PPRE1_DIV4; + +/* + M N Q P MHz +0 4 168 7 2 168 +1 4 360 15 4 180 +2 4 192 8 2 192 +3 4 216 9 2 216 +4 4 240 10 2 240 +5 4 264 11 2 264 + +*/ + uint8_t pll_m=4, pll_q, pll_p=2; + uint16_t pll_n; + uint8_t flash_latency; + uint32_t cr_flags = RCC_CR_CSSON; + switch(oc) { + case 0: + default: + pll_n=168; pll_q=7; + flash_latency = FLASH_ACR_LATENCY_5WS; + SystemCoreClock = 168000000; + break; + case 1: + pll_n=360; pll_q=15; pll_p=4; + flash_latency = FLASH_ACR_LATENCY_5WS; + SystemCoreClock = 180000000; +// cr_flags = 0; // CSS don't support this mode + break; + case 2: + pll_n=192; pll_q=8; + flash_latency = FLASH_ACR_LATENCY_6WS; + SystemCoreClock = 192000000; + break; + case 3: + pll_n=216; pll_q=9; + flash_latency = FLASH_ACR_LATENCY_6WS; + SystemCoreClock = 216000000; + break; + case 4: + pll_n=240; pll_q=10; + flash_latency = FLASH_ACR_LATENCY_7WS; + SystemCoreClock = 240000000; + break; + case 5: + pll_n=264; pll_q=11; + flash_latency = FLASH_ACR_LATENCY_7WS; + SystemCoreClock = 264000000; + break; + } + + /* Configure the main PLL */ +/* RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); */ + + RCC->PLLCFGR = pll_m | (pll_n << 6) | (((pll_p >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (pll_q << 24); + + /* Enable the main PLL */ + RCC->CR |= RCC_CR_PLLON | cr_flags; + + + /* Wait till the main PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) { } + + + /* Configure Flash no-prefetch, Instruction cache, Data cache and wait state */ + FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN | flash_latency; + + /* Select the main PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= RCC_CFGR_SW_PLL; + + /* Wait till the main PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL) {} + + FLASH->ACR |= FLASH_ACR_PRFTEN; // enable prefetch. this greatly increases both noice and speed + + // also see http://radiokot.ru/forum/viewtopic.php?f=59&t=117260 + + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + __error(12,0,0,0); + } + +/******************************************************************************/ +/* I2S clock configuration */ +/******************************************************************************/ + /* PLLI2S clock used as I2S clock source */ + RCC->CFGR &= ~RCC_CFGR_I2SSRC; + + /* Configure PLLI2S */ + RCC->PLLI2SCFGR = (PLLI2S_N << 6) | (PLLI2S_R << 28); +#if 0 // we don't use I2S + /* Enable PLLI2S */ + RCC->CR |= ((uint32_t)RCC_CR_PLLI2SON); + + /* Wait till PLLI2S is ready */ + while((RCC->CR & RCC_CR_PLLI2SRDY) == 0) { } +#endif +} + + diff --git a/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/target-config.mk b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/target-config.mk new file mode 100644 index 0000000000..1369547691 --- /dev/null +++ b/libraries/AP_HAL_F4Light/boards/f4light_OmnibusV3/target-config.mk @@ -0,0 +1,26 @@ +# Board-specific configuration values. Flash and SRAM sizes in bytes. + + MCU := STM32F405RG + PRODUCT_ID := 0003 + DENSITY := STM32_HIGH_DENSITY + FLASH_SIZE := 1048576 + SRAM_SIZE := 131072 + + BOARD_TYPE := 70 + BOARD_REV := 1 + BOOTLOADER := revo405_bl + +# Memory target-specific configuration values + +ifeq ($(MEMORY_TARGET), ram) + LDSCRIPT := ram.ld + VECT_BASE_ADDR := VECT_TAB_RAM +endif +ifeq ($(MEMORY_TARGET), flash) + LDSCRIPT := flash.ld + VECT_BASE_ADDR := VECT_TAB_FLASH +endif +ifeq ($(MEMORY_TARGET), jtag) + LDSCRIPT := jtag.ld + VECT_BASE_ADDR := VECT_TAB_BASE +endif diff --git a/libraries/AP_HAL_F4Light/support/MMM.sh b/libraries/AP_HAL_F4Light/support/MMM.sh index f74f7deda3..9c105cbf1e 100755 --- a/libraries/AP_HAL_F4Light/support/MMM.sh +++ b/libraries/AP_HAL_F4Light/support/MMM.sh @@ -54,6 +54,9 @@ make_plane(){ ) && ( # AirBotV2 board make_copter "f4light_AirbotV2" && \ make_plane "f4light_AirbotV2" +) && ( # OmnibusV3 board + make_copter "f4light_OmnibusV3" && \ + make_plane "f4light_OmnibusV3" ) && ( # RevoMini board with SD card make_copter "f4light_Revolution_SD" && \ make_plane "f4light_Revolution_SD"