From 8a93041ea1204b354c5befaf62d6720d7b42d653 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Fri, 11 Mar 2016 20:55:27 -0800 Subject: [PATCH 01/38] SIRINFPV target initial code --- Makefile | 23 +- src/main/config/config.c | 10 + src/main/config/config.h | 1 + src/main/config/config_master.h | 4 + src/main/drivers/bus_spi.c | 4 +- src/main/drivers/max7456.c | 168 +++++ src/main/drivers/max7456.h | 130 ++++ src/main/drivers/pwm_mapping.c | 30 + src/main/drivers/rtc6705.c | 151 +++++ src/main/drivers/rtc6705.h | 25 + src/main/drivers/timer.c | 20 + src/main/flight/mixer.h | 3 +- src/main/io/osd.c | 671 ++++++++++++++++++++ src/main/io/osd.h | 42 ++ src/main/io/serial_cli.c | 3 + src/main/io/serial_msp.c | 29 +- src/main/io/serial_msp.h | 9 + src/main/main.c | 10 + src/main/mw.c | 10 + src/main/scheduler.h | 4 +- src/main/scheduler_tasks.c | 12 +- src/main/sensors/initialisation.c | 13 + src/main/target/FPVRACER/system_stm32f30x.c | 371 +++++++++++ src/main/target/FPVRACER/system_stm32f30x.h | 76 +++ src/main/target/FPVRACER/target.h | 206 ++++++ 25 files changed, 2016 insertions(+), 9 deletions(-) create mode 100644 src/main/drivers/max7456.c create mode 100644 src/main/drivers/max7456.h create mode 100644 src/main/drivers/rtc6705.c create mode 100644 src/main/drivers/rtc6705.h create mode 100644 src/main/io/osd.c create mode 100644 src/main/io/osd.h create mode 100644 src/main/target/FPVRACER/system_stm32f30x.c create mode 100644 src/main/target/FPVRACER/system_stm32f30x.h create mode 100644 src/main/target/FPVRACER/target.h diff --git a/Makefile b/Makefile index 7ee64c6fea..a91fdf7f43 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,7 @@ FORKNAME = betaflight CC3D_TARGETS = CC3D CC3D_OPBL -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SIRINFPV # Valid targets for OP VCP support VCP_VALID_TARGETS = $(CC3D_TARGETS) @@ -52,9 +52,9 @@ OPBL_VALID_TARGETS = CC3D_OPBL 64K_TARGETS = CJMCU 128K_TARGETS = ALIENFLIGHTF1 $(CC3D_TARGETS) NAZE OLIMEXINO RMDO AFROMINI -256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI +256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SIRINFPV -F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI +F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SIRINFPV # Configure default flash sizes for the targets @@ -749,6 +749,23 @@ SPRACINGF3MINI_SRC = \ $(VCP_SRC) # $(FATFS_SRC) +SIRINFPV_SRC = \ + $(STM32F30x_COMMON_SRC) \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/serial_usb_vcp.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + drivers/max7456.c \ + drivers/rtc6705.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ + io/osd.c \ + $(HIGHEND_SRC) \ + $(COMMON_SRC) \ + $(VCP_SRC) +# $(FATFS_SRC) + # Search path and source files for the ST stdperiph library VPATH := $(VPATH):$(STDPERIPH_DIR)/src diff --git a/src/main/config/config.c b/src/main/config/config.c index 194a9af5cc..04a611c597 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -395,6 +395,16 @@ static void resetConf(void) // featureSet(FEATURE_DISPLAY); //#endif +#ifdef SIRINFPV + featureSet(FEATURE_OSD); + featureSet(FEATURE_RX_SERIAL); + masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; + //masterConfig.batteryConfig.vbatscale = 20; + masterConfig.mag_hardware = MAG_NONE; // disabled by default + masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS; + masterConfig.vtx_channel = 0; +#endif + #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. diff --git a/src/main/config/config.h b/src/main/config/config.h index 7cb16dc900..ff43e5a14d 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -44,6 +44,7 @@ typedef enum { FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, + FEATURE_OSD = 1 << 22, } features_e; void handleOneshotFeatureChangeOnRestart(void); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index b9ec1d33a6..fde12ac993 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -125,6 +125,10 @@ typedef struct master_t { uint8_t transponderData[6]; #endif +#ifdef OSD + uint8_t vtx_channel; +#endif + profile_t profile[MAX_PROFILE_COUNT]; uint8_t current_profile_index; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index a3a2a8c5d8..5bcd784826 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -345,8 +345,8 @@ void initSpi3(void) GPIO_PinAFConfig(SPI3_GPIO, SPI3_MOSI_PIN_SOURCE, GPIO_AF_6); #ifdef SPI2_NSS_PIN_SOURCE - RCC_AHBPeriphClockCmd(SPI3_NNS_PERIPHERAL, ENABLE); - GPIO_PinAFConfig(SPI3_NNS_GPIO, SPI3_NSS_PIN_SOURCE, GPIO_AF_6); + RCC_AHBPeriphClockCmd(SPI3_NSS_PERIPHERAL, ENABLE); + GPIO_PinAFConfig(SPI3_NSS_GPIO, SPI3_NSS_PIN_SOURCE, GPIO_AF_6); #endif // Init pins diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c new file mode 100644 index 0000000000..c1a7623a59 --- /dev/null +++ b/src/main/drivers/max7456.c @@ -0,0 +1,168 @@ +/* + * 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" + +#ifdef USE_MAX7456 + +#include "drivers/bus_spi.h" +#include "drivers/system.h" + +#include "max7456.h" + +#define DISABLE_MAX7456 GPIO_SetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) +#define ENABLE_MAX7456 GPIO_ResetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) + + +uint16_t max_screen_size; +uint8_t video_signal_type = 0; +uint8_t max7456_lock = 0; +char screen[480]; + + +uint8_t max7456_send(uint8_t add, uint8_t data) { + spiTransferByte(MAX7456_SPI_INSTANCE, add); + return spiTransferByte(MAX7456_SPI_INSTANCE, data); +} + + +// ============================================================ WRITE TO SCREEN + +void max7456_init(void) { + uint8_t max7456_reset=0x02; + uint8_t max_screen_rows; + uint8_t srdata = 0; + uint16_t x; + + //Minimum spi clock period for max7456 is 100ns (10Mhz) + spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); + + // force soft reset on Max7456 + ENABLE_MAX7456; + max7456_send(VM0_reg, max7456_reset); + delay(100); + + srdata = max7456_send(0xA0, 0xFF); + if ((0x01 & srdata) == 0x01){ //PAL + video_signal_type = VIDEO_MODE_PAL; + } + else if((0x02 & srdata) == 0x02){ //NTSC + video_signal_type = VIDEO_MODE_NTSC; + } + + max7456_reset |= video_signal_type; + + if (video_signal_type) { //PAL + max_screen_size = 480; + max_screen_rows = 16; + } else { // NTSC + max_screen_size = 390; + max_screen_rows = 13; + } + + // set all rows to same charactor black/white level + for(x = 0; x < max_screen_rows; x++) { + max7456_send(MAX7456ADD_RB0 + x, BWBRIGHTNESS); + } + + // make sure the Max7456 is enabled + max7456_send(VM0_reg, OSD_ENABLE | video_signal_type); + + DISABLE_MAX7456; + delay(100); + for (x=0; x<256; x++) + screen[x] = (char)x; + max7456_draw_screen(); +} + +// Copy string from ram into screen buffer +void max7456_write_string(const char *string, int address) { + char *dest = screen + address; + + while(*string) + *dest++ = *string++; +} + +void max7456_draw_screen(void) { + uint16_t xx; + if (!max7456_lock) { + ENABLE_MAX7456; + for (xx = 0; xx < max_screen_size; ++xx) { + max7456_send(MAX7456ADD_DMAH, xx>>8); + max7456_send(MAX7456ADD_DMAL, xx); + max7456_send(MAX7456ADD_DMDI, screen[xx]); + screen[xx] = ' '; + } + DISABLE_MAX7456; + } +} + +void max7456_draw_screen_fast(void) { + uint16_t xx; + if (!max7456_lock) { + ENABLE_MAX7456; + max7456_send(MAX7456ADD_DMAH, 0); + max7456_send(MAX7456ADD_DMAL, 0); + max7456_send(MAX7456ADD_DMM, 1); + for (xx = 0; xx < max_screen_size; ++xx) { + max7456_send(MAX7456ADD_DMDI, screen[xx]); + screen[xx] = ' '; + } + max7456_send(MAX7456ADD_DMDI, 0xFF); + max7456_send(MAX7456ADD_DMM, 0); + DISABLE_MAX7456; + } +} + +#define NVM_RAM_SIZE 54 +#define WRITE_NVR 0xA0 +#define STATUS_REG_NVR_BUSY 0x20 + + +void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { + uint8_t x; + + max7456_lock = 1; + ENABLE_MAX7456; + + // disable display + max7456_send(VM0_reg, video_signal_type); + + max7456_send(MAX7456ADD_CMAH, char_address); // set start address high + + for(x = 0; x < 54; x++) { + max7456_send(MAX7456ADD_CMAL, x); //set start address low + max7456_send(MAX7456ADD_CMDI, font_data[x]); + } + + // transfer 54 bytes from shadow ram to NVM + max7456_send(MAX7456ADD_CMM, WRITE_NVR); + + // wait until bit 5 in the status register returns to 0 (12ms) + while ((spiTransferByte(MAX7456_SPI_INSTANCE, MAX7456ADD_STAT) & STATUS_REG_NVR_BUSY) != 0); + + max7456_send(VM0_reg, video_signal_type | 0x0C); + DISABLE_MAX7456; + max7456_lock = 0; +} + + +#endif diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h new file mode 100644 index 0000000000..c8e34a8a5e --- /dev/null +++ b/src/main/drivers/max7456.h @@ -0,0 +1,130 @@ +/* + * 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 DATAOUT 11 // MOSI +#define DATAIN 12 // MISO +#define SPICLOCK 13 // sck +#define VSYNC 2 // INT0 + +#ifndef WHITEBRIGHTNESS + #define WHITEBRIGHTNESS 0x01 +#endif +#ifndef BLACKBRIGHTNESS + #define BLACKBRIGHTNESS 0x00 +#endif + +#define BWBRIGHTNESS ((BLACKBRIGHTNESS << 2) | WHITEBRIGHTNESS) + +//MAX7456 opcodes +#define DMM_reg 0x04 +#define DMAH_reg 0x05 +#define DMAL_reg 0x06 +#define DMDI_reg 0x07 +#define VM0_reg 0x00 +#define VM1_reg 0x01 + +// video mode register 0 bits +#define VIDEO_BUFFER_DISABLE 0x01 +//#define MAX7456_RESET 0x02 +#define VERTICAL_SYNC_NEXT_VSYNC 0x04 +#define OSD_ENABLE 0x08 +#define SYNC_MODE_AUTO 0x00 +#define SYNC_MODE_INTERNAL 0x30 +#define SYNC_MODE_EXTERNAL 0x20 +#define VIDEO_MODE_PAL 0x40 +#define VIDEO_MODE_NTSC 0x00 + +// video mode register 1 bits + + +// duty cycle is on_off +#define BLINK_DUTY_CYCLE_50_50 0x00 +#define BLINK_DUTY_CYCLE_33_66 0x01 +#define BLINK_DUTY_CYCLE_25_75 0x02 +#define BLINK_DUTY_CYCLE_75_25 0x03 + +// blinking time +#define BLINK_TIME_0 0x00 +#define BLINK_TIME_1 0x04 +#define BLINK_TIME_2 0x08 +#define BLINK_TIME_3 0x0C + +// background mode brightness (percent) +#define BACKGROUND_BRIGHTNESS_0 0x00 +#define BACKGROUND_BRIGHTNESS_7 0x01 +#define BACKGROUND_BRIGHTNESS_14 0x02 +#define BACKGROUND_BRIGHTNESS_21 0x03 +#define BACKGROUND_BRIGHTNESS_28 0x04 +#define BACKGROUND_BRIGHTNESS_35 0x05 +#define BACKGROUND_BRIGHTNESS_42 0x06 +#define BACKGROUND_BRIGHTNESS_49 0x07 + +#define BACKGROUND_MODE_GRAY 0x40 + +//MAX7456 commands +#define CLEAR_display 0x04 +#define CLEAR_display_vert 0x06 +#define END_string 0xff + + +#define MAX7456ADD_VM0 0x00 //0b0011100// 00 // 00 ,0011100 +#define MAX7456ADD_VM1 0x01 +#define MAX7456ADD_HOS 0x02 +#define MAX7456ADD_VOS 0x03 +#define MAX7456ADD_DMM 0x04 +#define MAX7456ADD_DMAH 0x05 +#define MAX7456ADD_DMAL 0x06 +#define MAX7456ADD_DMDI 0x07 +#define MAX7456ADD_CMM 0x08 +#define MAX7456ADD_CMAH 0x09 +#define MAX7456ADD_CMAL 0x0a +#define MAX7456ADD_CMDI 0x0b +#define MAX7456ADD_OSDM 0x0c +#define MAX7456ADD_RB0 0x10 +#define MAX7456ADD_RB1 0x11 +#define MAX7456ADD_RB2 0x12 +#define MAX7456ADD_RB3 0x13 +#define MAX7456ADD_RB4 0x14 +#define MAX7456ADD_RB5 0x15 +#define MAX7456ADD_RB6 0x16 +#define MAX7456ADD_RB7 0x17 +#define MAX7456ADD_RB8 0x18 +#define MAX7456ADD_RB9 0x19 +#define MAX7456ADD_RB10 0x1a +#define MAX7456ADD_RB11 0x1b +#define MAX7456ADD_RB12 0x1c +#define MAX7456ADD_RB13 0x1d +#define MAX7456ADD_RB14 0x1e +#define MAX7456ADD_RB15 0x1f +#define MAX7456ADD_OSDBL 0x6c +#define MAX7456ADD_STAT 0xA0 + +// Selectable by video mode +//uint8_t ENABLE_display; +//uint8_t ENABLE_display_vert; +//uint8_t DISABLE_display; +uint16_t MAX_screen_size; +char screen[480]; + + +void max7456_init(void); +void max7456_draw_screen(void); +void max7456_draw_screen_fast(void); +void max7456_write_string(const char *string, int address); +void max7456_write_nvm(uint8_t char_address, uint8_t *font_data); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index d250fcff96..1a069dba0e 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -562,6 +562,36 @@ static const uint16_t airPWM[] = { }; #endif +#if defined(SIRINFPV) +static const uint16_t multiPPM[] = { + PWM1 | (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_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t multiPWM[] = { + PWM1 | (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_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPPM[] = { + 0xFFFF +}; + +static const uint16_t airPWM[] = { + 0xFFFF +}; +#endif + static const uint16_t * const hardwareMaps[] = { multiPWM, multiPPM, diff --git a/src/main/drivers/rtc6705.c b/src/main/drivers/rtc6705.c new file mode 100644 index 0000000000..ceb525fae7 --- /dev/null +++ b/src/main/drivers/rtc6705.c @@ -0,0 +1,151 @@ +/* + * 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" + +#ifdef USE_RTC6705 + +#include "drivers/bus_spi.h" +#include "drivers/system.h" +#include "drivers/gpio.h" + +#include "rtc6705.h" + +#define RTC6705_SPICLK_ON GPIO_SetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) +#define RTC6705_SPICLK_OFF GPIO_ResetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) + +#define RTC6705_SPIDATA_ON GPIO_SetBits(RTC6705_SPIDATA_GPIO, RTC6705_SPIDATA_PIN) +#define RTC6705_SPIDATA_OFF GPIO_ResetBits(RTC6705_SPIDATA_GPIO, RTC6705_SPIDATA_PIN) + +#define RTC6705_SPILE_ON GPIO_SetBits(RTC6705_SPILE_GPIO, RTC6705_SPILE_PIN) +#define RTC6705_SPILE_OFF GPIO_ResetBits(RTC6705_SPILE_GPIO, RTC6705_SPILE_PIN) + +char *vtx_bands[] = { + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +uint16_t vtx_freq[] = +{ + 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, + 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, + 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, + 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, + 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, +}; + +uint16_t current_vtx_channel; + +void rtc6705_init(void) { + gpio_config_t gpio; + +#ifdef STM32F303 + #ifdef RTC6705_SPICLK_PERIPHERAL + RCC_AHBPeriphClockCmd(RTC6705_SPICLK_PERIPHERAL, ENABLE); + #endif + #ifdef RTC6705_SPILE_PERIPHERAL + RCC_AHBPeriphClockCmd(RTC6705_SPILE_PERIPHERAL, ENABLE); + #endif + #ifdef RTC6705_SPIDATA_PERIPHERAL + RCC_AHBPeriphClockCmd(RTC6705_SPIDATA_PERIPHERAL, ENABLE); + #endif +#endif +#ifdef STM32F10X + #ifdef RTC6705_SPICLK_PERIPHERAL + RCC_APB2PeriphClockCmd(RTC6705_SPICLK_PERIPHERAL, ENABLE); + #endif + #ifdef RTC6705_SPILE_PERIPHERAL + RCC_APB2PeriphClockCmd(RTC6705_SPILE_PERIPHERAL, ENABLE); + #endif + #ifdef RTC6705_SPIDATA_PERIPHERAL + RCC_APB2PeriphClockCmd(RTC6705_SPIDATA_PERIPHERAL, ENABLE); + #endif +#endif + + gpio.pin = RTC6705_SPICLK_PIN; + gpio.speed = Speed_50MHz; + gpio.mode = Mode_Out_PP; + gpioInit(RTC6705_SPICLK_GPIO, &gpio); + + gpio.pin = RTC6705_SPILE_PIN; + gpio.speed = Speed_50MHz; + gpio.mode = Mode_Out_PP; + gpioInit(RTC6705_SPILE_GPIO, &gpio); + + gpio.pin = RTC6705_SPIDATA_PIN; + gpio.speed = Speed_50MHz; + gpio.mode = Mode_Out_PP; + gpioInit(RTC6705_SPIDATA_GPIO, &gpio); +} + +void rtc6705_write_register(uint8_t addr, uint32_t data) { + uint8_t i; + + RTC6705_SPILE_OFF; + delay(1); + // send address + for (i=0; i<4; i++) { + if ((addr >> i) & 1) + RTC6705_SPIDATA_ON; + else + RTC6705_SPIDATA_OFF; + + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + } + // Write bit + + RTC6705_SPIDATA_ON; + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + for (i=0; i<20; i++) { + if ((data >> i) & 1) + RTC6705_SPIDATA_ON; + else + RTC6705_SPIDATA_OFF; + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + } + RTC6705_SPILE_ON; +} + + +void rtc6705_set_channel(uint16_t channel_freq) { + + uint32_t freq = (uint32_t)channel_freq * 1000; + uint32_t N, A; + + freq /= 40; // TODO check all channels for the R value + N = freq / 64; + A = freq % 64; + rtc6705_write_register(0, 400); + rtc6705_write_register(1, (N << 7) | A); +} +#endif diff --git a/src/main/drivers/rtc6705.h b/src/main/drivers/rtc6705.h new file mode 100644 index 0000000000..511d090878 --- /dev/null +++ b/src/main/drivers/rtc6705.h @@ -0,0 +1,25 @@ +/* + * 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 + +extern char* vtx_bands[]; +extern uint16_t vtx_freq[]; +extern uint16_t current_vtx_channel; + +void rtc6705_init(void); +void rtc6705_set_channel(uint16_t channel_freq); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 19f3bae6a0..32cfea6b60 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -278,6 +278,26 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif +#if defined(SIRINFPV) +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + + { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2}, // PWM1 - PB6 + { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2}, // PWM2 - PB6 + { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2}, // PWM3 - PB8 + { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2}, // PWM4 - PB9 + + { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // PWM5 - PB0 - *TIM3_CH3 + { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // PWM6 - PB1 - *TIM3_CH4 + +}; + +#define USED_TIMERS (TIM_N(3) | TIM_N(4)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOB) + +#endif + #if defined(MOTOLAB) const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, GPIOA, Pin_4, TIM_Channel_2, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // PWM1 - PA4 - *TIM3_CH2 diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index eb324c6768..f27c2909ca 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -189,9 +189,10 @@ extern int16_t servo[MAX_SUPPORTED_SERVOS]; bool isMixerUsingServos(void); void writeServos(void); void filterServos(void); -bool motorLimitReached; #endif +bool motorLimitReached; + extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; diff --git a/src/main/io/osd.c b/src/main/io/osd.c new file mode 100644 index 0000000000..9a2e67b7f1 --- /dev/null +++ b/src/main/io/osd.c @@ -0,0 +1,671 @@ +/* + * 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 +#include + +#include "platform.h" +#include "scheduler.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/atomic.h" +#include "common/maths.h" +#include "common/typeconversion.h" + +#include "drivers/nvic.h" + +#include "drivers/sensor.h" +#include "drivers/system.h" +#include "drivers/gpio.h" +#include "drivers/light_led.h" +#include "drivers/sound_beeper.h" +#include "drivers/timer.h" +#include "drivers/serial.h" +#include "drivers/serial_softserial.h" +#include "drivers/serial_uart.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/pwm_mapping.h" +#include "drivers/pwm_rx.h" +#include "drivers/adc.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_bst.h" +#include "drivers/bus_spi.h" +#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" + +#include "rx/rx.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/flashfs.h" +#include "io/gps.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/gimbal.h" +#include "io/ledstrip.h" +#include "io/display.h" +#include "io/asyncfatfs/asyncfatfs.h" +#include "io/transponder_ir.h" + +#include "sensors/sensors.h" +#include "sensors/sonar.h" +#include "sensors/barometer.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/initialisation.h" + +#include "telemetry/telemetry.h" +#include "blackbox/blackbox.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/failsafe.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#ifdef USE_HARDWARE_REVISION_DETECTION +#include "hardware_revision.h" +#endif + +#include "build_config.h" +#include "debug.h" + +#ifdef OSD + +#include "io/osd.h" +#include "drivers/max7456.h" +#include "drivers/rtc6705.h" +#include "scheduler.h" +#include "common/printf.h" + +#define MICROSECONDS_IN_A_SECOND (1000 * 1000) + +#define OSD_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5) +#define OSD_LINE_LENGTH 30 + +static uint32_t next_osd_update_at = 0; +static uint32_t armed_seconds = 0; +static uint32_t armed_at = 0; +static bool armed = false; + +static uint8_t current_page = 0; +static uint8_t sticks[] = {0,0,0,0}; +char string_buffer[30]; +uint8_t cursor_row = 255; +uint8_t cursor_col = 0; +bool in_menu = false; + +#ifdef USE_RTC6705 +void update_vtx_band(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (current_vtx_channel < 32) + current_vtx_channel += 8; + } else { + if (current_vtx_channel > 7) + current_vtx_channel -= 8; + } +} + +void print_vtx_band(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%s", vtx_bands[current_vtx_channel / 8]); + max7456_write_string(string_buffer, pos); +} + +void update_vtx_channel(bool increase, uint8_t col) { + (void)col; + if (increase) { + if ((current_vtx_channel % 8) < 7) + current_vtx_channel++; + } else { + if ((current_vtx_channel % 8) > 0) + current_vtx_channel--; + } +} + +void print_vtx_channel(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%d", current_vtx_channel % 8 + 1); + max7456_write_string(string_buffer, pos); +} + +void print_vtx_freq(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%d M", vtx_freq[current_vtx_channel]); + max7456_write_string(string_buffer, pos); +} +#endif + +void print_pid(uint16_t pos, uint8_t col, int pid_term) { + switch(col) { + case 0: + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) + sprintf(string_buffer, "%d", (int)(currentProfile->pidProfile.P_f[pid_term] * 10.0)); + else + sprintf(string_buffer, "%d", currentProfile->pidProfile.P8[pid_term]); + break; + case 1: + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) + sprintf(string_buffer, "%d", (int)(currentProfile->pidProfile.I_f[pid_term] * 100.0)); + else + sprintf(string_buffer, "%d", currentProfile->pidProfile.I8[pid_term]); + break; + case 2: + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) + sprintf(string_buffer, "%d", (int)(currentProfile->pidProfile.D_f[pid_term] * 1000.0)); + else + sprintf(string_buffer, "%d", currentProfile->pidProfile.D8[pid_term]); + break; + default: + return; + } + max7456_write_string(string_buffer, pos); +} + +void print_roll_pid(uint16_t pos, uint8_t col) { + print_pid(pos, col, ROLL); +} + +void print_pitch_pid(uint16_t pos, uint8_t col) { + print_pid(pos, col, PITCH); +} + +void print_yaw_pid(uint16_t pos, uint8_t col) { + print_pid(pos, col, YAW); +} + +void print_roll_rate(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_ROLL]); + max7456_write_string(string_buffer, pos); + } +} + +void print_pitch_rate(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_PITCH]); + max7456_write_string(string_buffer, pos); + } +} + +void print_yaw_rate(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_YAW]); + max7456_write_string(string_buffer, pos); + } +} + +void update_int_pid(bool inc, uint8_t col, int pid_term) { + void* ptr; + switch(col) { + case 0: + ptr = ¤tProfile->pidProfile.P8[pid_term]; + break; + case 1: + ptr = ¤tProfile->pidProfile.I8[pid_term]; + break; + case 2: + ptr = ¤tProfile->pidProfile.D8[pid_term]; + break; + default: + return; + } + + if (inc) { + if (*(uint8_t*)ptr < 200) + *(uint8_t*)ptr += 1; + } else { + if (*(uint8_t*)ptr > 0) + *(uint8_t*)ptr -= 1; + } +} + +void update_float_pid(bool inc, uint8_t col, int pid_term) { + void* ptr; + float diff; + + switch(col) { + case 0: + ptr = ¤tProfile->pidProfile.P_f[pid_term]; + diff = 0.1; + break; + case 1: + ptr = ¤tProfile->pidProfile.I_f[pid_term]; + diff = 0.01; + break; + case 2: + ptr = ¤tProfile->pidProfile.D_f[pid_term]; + diff = 0.001; + break; + } + + if (inc) { + if (*(float*)ptr < 100.0) + *(float*)ptr += diff; + } else { + if (*(float*)ptr > 0.0) + *(float*)ptr -= diff; + } +} + +void update_roll_pid(bool inc, uint8_t col) { + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) + update_float_pid(inc, col, ROLL); + else + update_int_pid(inc, col, ROLL); +} + +void update_pitch_pid(bool inc, uint8_t col) { + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) + update_float_pid(inc, col, PITCH); + else + update_int_pid(inc, col, PITCH); +} + +void update_yaw_pid(bool inc, uint8_t col) { + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) + update_float_pid(inc, col, YAW); + else + update_int_pid(inc, col, YAW); +} + +void update_roll_rate(bool inc, uint8_t col) { + (void)col; + if (inc) { + if (currentControlRateProfile->rates[FD_ROLL] < CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX) + currentControlRateProfile->rates[FD_ROLL]++; + } else { + if (currentControlRateProfile->rates[FD_ROLL] > 0) + currentControlRateProfile->rates[FD_ROLL]--; + } +} + +void update_pitch_rate(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (currentControlRateProfile->rates[FD_PITCH] < CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX) + currentControlRateProfile->rates[FD_PITCH]++; + } else { + if (currentControlRateProfile->rates[FD_PITCH] > 0) + currentControlRateProfile->rates[FD_PITCH]--; + } +} + +void update_yaw_rate(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (currentControlRateProfile->rates[FD_YAW] < CONTROL_RATE_CONFIG_YAW_RATE_MAX) + currentControlRateProfile->rates[FD_YAW]++; + } else { + if (currentControlRateProfile->rates[FD_YAW] > 0) + currentControlRateProfile->rates[FD_YAW]--; + } +} + +void print_average_system_load(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%d", averageSystemLoadPercent); + max7456_write_string(string_buffer, pos); +} + +void print_batt_voltage(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%d.%1d", vbat / 10, vbat % 10); + max7456_write_string(string_buffer, pos); +} + +/* + TODO: add this to menu + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } }, + { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, + { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, + { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, + { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, + { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, + { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, + { "acro_plus_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } }, + { "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } }, +*/ + +page_t menu_pages[] = { + { + .title = "STATUS", + .cols_number = 1, + .rows_number = 2, + .cols = { + { + .title = NULL, + .x_pos = 15 + } + }, + .rows = { + { + .title = "AVG LOAD", + .y_pos = 0, + .update = NULL, + .print = print_average_system_load + }, + { + .title = "BATT", + .y_pos = 1, + .update = NULL, + .print = print_batt_voltage + }, + } + }, +#ifdef USE_RTC6705 + { + .title = "VTX SETTINGS", + .cols_number = 1, + .rows_number = 3, + .cols = { + { + .title = NULL, + .x_pos = 15 + } + }, + .rows = { + { + .title = "BAND", + .y_pos = 0, + .update = update_vtx_band, + .print = print_vtx_band + }, + { + .title = "CHANNEL", + .y_pos = 1, + .update = update_vtx_channel, + .print = print_vtx_channel + }, + { + .title = "FREQUENCY", + .y_pos = 2, + .update = NULL, + .print = print_vtx_freq + } + } + }, +#endif + { + .title = "PID SETTINGS", + .cols_number = 3, + .rows_number = 6, + .cols = { + { + .title = "P", + .x_pos = 13 + }, + { + .title = "I", + .x_pos = 19 + }, + { + .title = "D", + .x_pos = 25 + } + }, + .rows = { + { + .title = "ROLL", + .y_pos = 0, + .update = update_roll_pid, + .print = print_roll_pid + }, + { + .title = "PITCH", + .y_pos = 1, + .update = update_pitch_pid, + .print = print_pitch_pid + }, + { + .title = "YAW", + .y_pos = 2, + .update = update_yaw_pid, + .print = print_yaw_pid + }, + { + .title = "ROLL_RATE", + .y_pos = 3, + .update = update_roll_rate, + .print = print_roll_rate + }, + { + .title = "PITCH_RATE", + .y_pos = 4, + .update = update_pitch_rate, + .print = print_pitch_rate + }, + { + .title = "YAW_RATE", + .y_pos = 5, + .update = update_yaw_rate, + .print = print_yaw_rate + }, + } + }, +}; + +void show_menu(void) { + uint8_t line = 1; + uint16_t pos; + col_t *col; + row_t *row; + uint16_t cursor_x, cursor_y; + + sprintf(string_buffer, "EXIT SAVE+EXIT PAGE"); + max7456_write_string(string_buffer, 12 * OSD_LINE_LENGTH + 1); + + pos = (OSD_LINE_LENGTH - strlen(menu_pages[current_page].title)) / 2 + line * OSD_LINE_LENGTH; + sprintf(string_buffer, "%s", menu_pages[current_page].title); + max7456_write_string(string_buffer, pos); + + line += 2; + + for (int i = 0; i < menu_pages[current_page].cols_number; i++){ + col = &menu_pages[current_page].cols[i]; + if (cursor_col == i) + cursor_x = col->x_pos - 1; + + if (col->title) { + sprintf(string_buffer, "%s", col->title); + max7456_write_string(string_buffer, line * OSD_LINE_LENGTH + col->x_pos); + } + } + + line++; + for (int i = 0; i < menu_pages[current_page].rows_number; i++) { + row = &menu_pages[current_page].rows[i]; + if (cursor_row == i) + cursor_y = line + row->y_pos; + + sprintf(string_buffer, "%s", row->title); + max7456_write_string(string_buffer, (line + row->y_pos) * OSD_LINE_LENGTH + 1); + for (int j = 0; j < menu_pages[current_page].cols_number; j++) { + col = &menu_pages[current_page].cols[j]; + row->print((line + row->y_pos) * OSD_LINE_LENGTH + col->x_pos, j); + } + } + + if (sticks[YAW] > 90) { + if (cursor_row > MAX_MENU_ROWS) { + switch(cursor_col) { + case 0: + in_menu = false; + break; + case 1: + in_menu = false; +#ifdef USE_RTC6705 + if (masterConfig.vtx_channel != current_vtx_channel) { + masterConfig.vtx_channel = current_vtx_channel; + rtc6705_set_channel(vtx_freq[current_vtx_channel]); + } +#endif + writeEEPROM(); + break; + case 2: + if (current_page < (sizeof(menu_pages) / sizeof(page_t) - 1)) + current_page++; + else + current_page = 0; + } + } else { + if (menu_pages[current_page].rows[cursor_row].update) + menu_pages[current_page].rows[cursor_row].update(true, cursor_col); + } + } + + if (sticks[YAW] < 10) { + if (cursor_row > MAX_MENU_ROWS) { + if (cursor_col == 2 && current_page > 0) { + current_page--; + } + } else { + if (menu_pages[current_page].rows[cursor_row].update) + menu_pages[current_page].rows[cursor_row].update(false, cursor_col); + } + } + + if (sticks[PITCH] > 90) { + if (cursor_row > MAX_MENU_ROWS) { + cursor_row = menu_pages[current_page].rows_number - 1; + cursor_col = 0; + } else { + if (cursor_row > 0) + cursor_row--; + } + } + if (sticks[PITCH] < 10) { + if (cursor_row < (menu_pages[current_page].rows_number - 1)) + cursor_row++; + else + cursor_row = 255; + } + if (sticks[ROLL] > 90) { + if (cursor_row > MAX_MENU_ROWS) { + if (cursor_col < 2) + cursor_col++; + } else { + if (cursor_col < (menu_pages[current_page].cols_number - 1)) + cursor_col++; + } + } + if (sticks[ROLL] < 10) { + if (cursor_col > 0) + cursor_col--; + } + + if (cursor_row > MAX_MENU_ROWS) { + cursor_row = 255; + cursor_y = 12; + switch(cursor_col) { + case 0: + cursor_x = 0; + break; + case 1: + cursor_x = 9; + break; + case 2: + cursor_x = 23; + break; + default: + cursor_x = 0; + } + } + max7456_write_string(">", cursor_x + cursor_y * OSD_LINE_LENGTH); +} + +void updateOsd(void) +{ + uint32_t seconds; + char line[30]; + uint32_t now = micros(); + + bool updateNow = (int32_t)(now - next_osd_update_at) >= 0L; + if (!updateNow) { + return; + } + next_osd_update_at = now + OSD_UPDATE_FREQUENCY; + if ((now / 1000000) & 1) { + if (ARMING_FLAG(ARMED)) { + if (!armed) { + armed = true; + armed_at = now; + in_menu = false; + } + } else { + if (armed) { + armed = false; + armed_seconds += (now - armed_at) / 1000000; + } + for (uint8_t channelIndex = 0; channelIndex < 4; channelIndex++) { + sticks[channelIndex] = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); + } + if (!in_menu && sticks[YAW] > 90 && sticks[THROTTLE] > 90 && sticks[ROLL] < 10 && sticks[PITCH] > 90) { + in_menu = true; + cursor_row = 255; + cursor_col = 2; + } + } + if (in_menu) { + show_menu(); + } else { + sprintf(line, "\x97%d.%1d", vbat / 10, vbat % 10); + max7456_write_string(line, 361); + sprintf(line, "\x7e%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + max7456_write_string(line, 381); + seconds = (now - armed_at) / 1000000 + armed_seconds; + sprintf(line, "\x9C %02d:%02d", seconds / 60, seconds % 60); + max7456_write_string(line, 351); + print_average_system_load(26, 0); + } + } else { + max7456_draw_screen_fast(); + } +} + + +void osdInit(void) +{ +#ifdef USE_RTC6705 + rtc6705_init(); + current_vtx_channel = masterConfig.vtx_channel; + rtc6705_set_channel(vtx_freq[current_vtx_channel]); +#endif + max7456_init(); + +} + +#endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h new file mode 100644 index 0000000000..733fdeb580 --- /dev/null +++ b/src/main/io/osd.h @@ -0,0 +1,42 @@ +/* + * 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 . + */ + +#define MAX_MENU_ROWS 8 +#define MAX_MENU_COLS 3 + +typedef struct { + const char* title; + uint8_t x_pos; +} col_t; + +typedef struct { + const char* title; + uint8_t y_pos; + void (*update)(bool increase, uint8_t col); + void (*print)(uint16_t pos, uint8_t col); +} row_t; + +typedef struct { + const char* title; + uint8_t cols_number; + uint8_t rows_number; + col_t cols[MAX_MENU_COLS]; + row_t rows[MAX_MENU_ROWS]; +} page_t; + +void updateOsd(void); +void osdInit(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 6595b1f0fb..8636a8fafd 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -768,6 +768,9 @@ const clivalue_t valueTable[] = { { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } }, { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } }, { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } }, +#ifdef USE_RTC6705 + { "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, +#endif }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3d3b381080..4b84152896 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -44,6 +44,8 @@ #include "drivers/gyro_sync.h" #include "drivers/sdcard.h" #include "drivers/buf_writer.h" +#include "drivers/max7456.h" +#include "drivers/rtc6705.h" #include "rx/rx.h" #include "rx/msp.h" @@ -1279,7 +1281,9 @@ static bool processInCommand(void) uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; #endif - +#ifdef OSD + uint8_t addr, font_data[64]; +#endif switch (currentPort->cmdMSP) { case MSP_SELECT_SETTING: if (!ARMING_FLAG(ARMED)) { @@ -1559,6 +1563,29 @@ static bool processInCommand(void) transponderUpdateData(masterConfig.transponderData); break; #endif +#ifdef OSD + case MSP_SET_OSD_CONFIG: + break; + case MSP_OSD_CHAR_WRITE: + addr = read8(); + for (i = 0; i < 54; i++) { + font_data[i] = read8(); + } + max7456_write_nvm(addr, font_data); + break; +#endif + +#ifdef USE_RTC6705 + case MSP_SET_VTX_CONFIG: + tmp = read16(); + if (tmp < 40) + masterConfig.vtx_channel = tmp; + if (current_vtx_channel != masterConfig.vtx_channel) { + current_vtx_channel = masterConfig.vtx_channel; + rtc6705_set_channel(vtx_freq[current_vtx_channel]); + } + break; +#endif #ifdef USE_FLASHFS case MSP_DATAFLASH_ERASE: diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index d2db79afb5..872160f46a 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -168,6 +168,15 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings #define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings +#define MSP_OSD_CONFIG 84 //in message Get osd settings +#define MSP_SET_OSD_CONFIG 85 //out message Set osd settings + +#define MSP_OSD_CHAR_READ 86 //in message Get osd settings +#define MSP_OSD_CHAR_WRITE 87 //out message Set osd settings + +#define MSP_VTX_CONFIG 88 //in message Get vtx settings +#define MSP_SET_VTX_CONFIG 89 //out message Set vtx settings + // // Baseflight MSP commands (if enabled they exist in Cleanflight) // diff --git a/src/main/main.c b/src/main/main.c index d856a36ea9..395665a9c3 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -131,6 +131,7 @@ void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); void sonarInit(const sonarHardware_t *sonarHardware); void transponderInit(uint8_t* transponderCode); +void osdInit(void); //void usbCableDetectInit(void); #ifdef STM32F303xC @@ -464,6 +465,12 @@ void init(void) } #endif +#ifdef OSD + if (feature(FEATURE_OSD)) { + osdInit(); + } +#endif + if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,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); @@ -724,6 +731,9 @@ int main(void) { #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif +#ifdef OSD + setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); +#endif #ifdef USE_BST setTaskEnabled(TASK_BST_READ_WRITE, true); setTaskEnabled(TASK_BST_MASTER_PROCESS, true); diff --git a/src/main/mw.c b/src/main/mw.c index f9b73c050d..4f7704d5f9 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -65,6 +65,7 @@ #include "io/statusindicator.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" +#include "io/osd.h" #include "rx/rx.h" @@ -966,3 +967,12 @@ void taskTransponder(void) } } #endif + +#ifdef OSD +void taskUpdateOsd(void) +{ + if (feature(FEATURE_OSD)) { + updateOsd(); + } +} +#endif diff --git a/src/main/scheduler.h b/src/main/scheduler.h index f91bca78a3..422a0e914b 100755 --- a/src/main/scheduler.h +++ b/src/main/scheduler.h @@ -79,7 +79,9 @@ typedef enum { #ifdef TRANSPONDER TASK_TRANSPONDER, #endif - +#ifdef OSD + TASK_OSD, +#endif #ifdef USE_BST TASK_BST_READ_WRITE, TASK_BST_MASTER_PROCESS, diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler_tasks.c index a937537c45..b02f597deb 100644 --- a/src/main/scheduler_tasks.c +++ b/src/main/scheduler_tasks.c @@ -40,6 +40,9 @@ void taskTelemetry(void); void taskLedStrip(void); void taskTransponder(void); void taskSystem(void); +#ifdef OSD +void taskUpdateOsd(void); +#endif #ifdef USE_BST void taskBstReadWrite(void); void taskBstMasterProcess(void); @@ -168,7 +171,14 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_LOW, }, #endif - +#ifdef OSD + [TASK_OSD] = { + .taskName = "OSD", + .taskFunc = taskUpdateOsd, + .desiredPeriod = 1000000 / 60, // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif #ifdef TELEMETRY [TASK_TELEMETRY] = { .taskName = "TELEMETRY", diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index cad9b5fe66..6af98e2868 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -130,6 +130,19 @@ const extiConfig_t *selectMPUIntExtiConfig(void) return &spRacingF3MPUIntExtiConfig; #endif +#if defined(SIRINFPV) + static const extiConfig_t fpvRacerMPUIntExtiConfig = { + .gpioAHBPeripherals = RCC_AHBPeriph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = Pin_8, + .exti_port_source = EXTI_PortSourceGPIOA, + .exti_pin_source = EXTI_PinSource8, + .exti_line = EXTI_Line8, + .exti_irqn = EXTI15_10_IRQn + }; + return &fpvRacerMPUIntExtiConfig; +#endif + #if defined(CC3D) static const extiConfig_t cc3dMPUIntExtiConfig = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOA, diff --git a/src/main/target/FPVRACER/system_stm32f30x.c b/src/main/target/FPVRACER/system_stm32f30x.c new file mode 100644 index 0000000000..78274ac322 --- /dev/null +++ b/src/main/target/FPVRACER/system_stm32f30x.c @@ -0,0 +1,371 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.c + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F30x devices, + * and is generated by the clock configuration tool + * stm32f30x_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): 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_stm32f30x.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 (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f30x.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 8MHz, refer to "HSE_VALUE" define + * in "stm32f30x.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 STM32F30x device + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *---------------------------------------------------------------------------- + * PLLMUL | 9 + *----------------------------------------------------------------------------- + * PREDIV | 1 + *----------------------------------------------------------------------------- + * USB Clock | ENABLE + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** @addtogroup STM32F30x_System_Private_Includes + * @{ + */ + +#include "stm32f30x.h" + +uint32_t hse_value = HSE_VALUE; + +/** + * @} + */ + +/* Private typedef -----------------------------------------------------------*/ + +/** @addtogroup STM32F30x_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 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/** + * @} + */ + +/* Private macro -------------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Variables + * @{ + */ + + uint32_t SystemCoreClock = 72000000; + + __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_FunctionPrototypes + * @{ + */ + +void SetSysClock(void); + +/** + * @} + */ + +/** @addtogroup STM32F30x_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(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR &= 0xF87FC00C; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; + + /* Reset USARTSW[1:0], I2CSW and TIMs bits */ + RCC->CFGR3 &= (uint32_t)0xFF00FCCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + //SetSysClock(); // called from main() + +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ +#endif +} + +/** + * @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 stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 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, pllmull = 0, pllsource = 0, prediv1factor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllsource == 0x00) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + } + else + { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @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(void) +{ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + + /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ + /* 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 Prefetch Buffer and set Flash Latency */ + FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; + + /* HCLK = SYSCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 2 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; + + /* PLL configuration */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + /* Enable PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Select PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; + + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) + { + } + } + 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 */ + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/FPVRACER/system_stm32f30x.h b/src/main/target/FPVRACER/system_stm32f30x.h new file mode 100644 index 0000000000..4f999d3058 --- /dev/null +++ b/src/main/target/FPVRACER/system_stm32f30x.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.h + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F30X_H +#define __SYSTEM_STM32F30X_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/** @addtogroup STM32F30x_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F30X_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/FPVRACER/target.h b/src/main/target/FPVRACER/target.h new file mode 100644 index 0000000000..daccbe276f --- /dev/null +++ b/src/main/target/FPVRACER/target.h @@ -0,0 +1,206 @@ +/* + * 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 "FPVR" + +#define BEEP_GPIO GPIOA +#define BEEP_PIN Pin_1 +#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOA + +#define USABLE_TIMER_CHANNEL_COUNT 6 + +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +//#define USE_GYRO_SPI_MPU6500 + +#define ACC +#define USE_ACC_SPI_MPU6000 +//#define USE_ACC_SPI_MPU6500 + +// MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG + +#define MPU6000_CS_GPIO GPIOA +#define MPU6000_CS_PIN GPIO_Pin_4 +#define MPU6000_SPI_INSTANCE SPI1 + +// MPU6500 +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG + +#define MPU6500_CS_GPIO GPIOA +#define MPU6500_CS_PIN GPIO_Pin_4 +#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOA + + +#define BEEPER + +#define USB_IO +#define USB_CABLE_DETECTION + +#define USB_DETECT_PIN GPIO_Pin_2 +#define USB_DETECT_GPIO_PORT GPIOB +#define USB_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOB + +#define USE_VCP +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define SERIAL_PORT_COUNT 4 + +#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 USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +#define USE_SPI_DEVICE_3 + +#define SPI1_GPIO GPIOA +#define SPI1_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOA +#define SPI1_NSS_PIN Pin_4 +#define SPI1_NSS_PIN_SOURCE GPIO_PinSource4 +#define SPI1_SCK_PIN Pin_5 +#define SPI1_SCK_PIN_SOURCE GPIO_PinSource5 +#define SPI1_MISO_PIN Pin_6 +#define SPI1_MISO_PIN_SOURCE GPIO_PinSource6 +#define SPI1_MOSI_PIN Pin_7 +#define SPI1_MOSI_PIN_SOURCE GPIO_PinSource7 + +#define SPI2_GPIO GPIOB +#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB +#define SPI2_NSS_PIN Pin_12 +#define SPI2_NSS_PIN_SOURCE GPIO_PinSource12 +#define SPI2_SCK_PIN Pin_13 +#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13 +#define SPI2_MISO_PIN Pin_14 +#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14 +#define SPI2_MOSI_PIN Pin_15 +#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15 + +#define SPI3_GPIO GPIOB +#define SPI3_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB +#define SPI3_NSS_GPIO GPIOA +#define SPI3_NSS_PERIPHERAL RCC_AHBPeriph_GPIOA +#define SPI3_NSS_PIN GPIO_Pin_15 +#define SPI3_NSS_PIN_SOURCE GPIO_PinSource15 +#define SPI3_SCK_PIN GPIO_Pin_3 +#define SPI3_SCK_PIN_SOURCE GPIO_PinSource3 +#define SPI3_MISO_PIN GPIO_Pin_4 +#define SPI3_MISO_PIN_SOURCE GPIO_PinSource4 +#define SPI3_MOSI_PIN GPIO_Pin_5 +#define SPI3_MOSI_PIN_SOURCE GPIO_PinSource5 + +#define USE_MAX7456 +#define MAX7456_CS_GPIO GPIOA +#define MAX7456_CS_PIN GPIO_Pin_15 +#define MAX7456_SPI_INSTANCE SPI3 + +#define USE_RTC6705 +#define RTC6705_SPIDATA_GPIO GPIOC +#define RTC6705_SPIDATA_PIN Pin_15 +#define RTC6705_SPIDATA_PERIPHERAL RCC_AHBPeriph_GPIOC +#define RTC6705_SPILE_GPIO GPIOC +#define RTC6705_SPILE_PIN Pin_14 +#define RTC6705_SPILE_PERIPHERAL RCC_AHBPeriph_GPIOC +#define RTC6705_SPICLK_GPIO GPIOC +#define RTC6705_SPICLK_PIN Pin_13 +#define RTC6705_SPICLK_PERIPHERAL RCC_AHBPeriph_GPIOC + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_GPIO SPI2_GPIO +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + + +#define ADC_INSTANCE ADC1 +#define ADC_DMA_CHANNEL DMA1_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_CHANNEL ADC_Channel_1 + +#define USE_QUAD_MIXER_ONLY +#define BLACKBOX +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define TELEMETRY +#define SERIAL_RX +#define GTUNE +#define USE_CLI +#define OSD + +//#define SPEKTRUM_BIND +// USART3, +//#define BIND_PORT GPIOB +//#define BIND_PIN Pin_11 + +#define USE_SERIAL_1WIRE + +#define S1W_TX_GPIO UART1_GPIO +#define S1W_TX_PIN UART1_TX_PIN +#define S1W_RX_GPIO UART1_GPIO +#define S1W_RX_PIN UART1_RX_PIN From 8c3554c3037a576ef4a7a264fe1c4a3e60b68357 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Fri, 11 Mar 2016 21:23:36 -0800 Subject: [PATCH 02/38] rename target folder --- src/main/target/{FPVRACER => SIRINFPV}/system_stm32f30x.c | 0 src/main/target/{FPVRACER => SIRINFPV}/system_stm32f30x.h | 0 src/main/target/{FPVRACER => SIRINFPV}/target.h | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename src/main/target/{FPVRACER => SIRINFPV}/system_stm32f30x.c (100%) rename src/main/target/{FPVRACER => SIRINFPV}/system_stm32f30x.h (100%) rename src/main/target/{FPVRACER => SIRINFPV}/target.h (100%) diff --git a/src/main/target/FPVRACER/system_stm32f30x.c b/src/main/target/SIRINFPV/system_stm32f30x.c similarity index 100% rename from src/main/target/FPVRACER/system_stm32f30x.c rename to src/main/target/SIRINFPV/system_stm32f30x.c diff --git a/src/main/target/FPVRACER/system_stm32f30x.h b/src/main/target/SIRINFPV/system_stm32f30x.h similarity index 100% rename from src/main/target/FPVRACER/system_stm32f30x.h rename to src/main/target/SIRINFPV/system_stm32f30x.h diff --git a/src/main/target/FPVRACER/target.h b/src/main/target/SIRINFPV/target.h similarity index 100% rename from src/main/target/FPVRACER/target.h rename to src/main/target/SIRINFPV/target.h From 98bd0f18b3cb91f8f82b5988be2d735a5342f3ea Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Sat, 12 Mar 2016 18:10:50 -0800 Subject: [PATCH 03/38] Fix OSD menu sticks activation, add battery warning and arm/disarm messages --- src/main/io/osd.c | 46 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 34 insertions(+), 12 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9a2e67b7f1..523746c943 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -122,10 +122,12 @@ static bool armed = false; static uint8_t current_page = 0; static uint8_t sticks[] = {0,0,0,0}; -char string_buffer[30]; -uint8_t cursor_row = 255; -uint8_t cursor_col = 0; -bool in_menu = false; +static char string_buffer[30]; +static uint8_t cursor_row = 255; +static uint8_t cursor_col = 0; +static bool in_menu = false; +extern uint16_t rssi; + #ifdef USE_RTC6705 void update_vtx_band(bool increase, uint8_t col) { @@ -519,7 +521,7 @@ void show_menu(void) { } } - if (sticks[YAW] > 90) { + if (sticks[YAW] > 90 && sticks[ROLL] > 10 && sticks[ROLL] < 90 && sticks[PITCH] > 10 && sticks[PITCH] < 90) { if (cursor_row > MAX_MENU_ROWS) { switch(cursor_col) { case 0: @@ -547,7 +549,7 @@ void show_menu(void) { } } - if (sticks[YAW] < 10) { + if (sticks[YAW] < 10 && sticks[ROLL] > 10 && sticks[ROLL] < 90 && sticks[PITCH] > 10 && sticks[PITCH] < 90) { if (cursor_row > MAX_MENU_ROWS) { if (cursor_col == 2 && current_page > 0) { current_page--; @@ -558,7 +560,7 @@ void show_menu(void) { } } - if (sticks[PITCH] > 90) { + if (sticks[PITCH] > 90 && sticks[YAW] > 10 && sticks[YAW] < 90) { if (cursor_row > MAX_MENU_ROWS) { cursor_row = menu_pages[current_page].rows_number - 1; cursor_col = 0; @@ -567,13 +569,13 @@ void show_menu(void) { cursor_row--; } } - if (sticks[PITCH] < 10) { + if (sticks[PITCH] < 10 && sticks[YAW] > 10 && sticks[YAW] < 90) { if (cursor_row < (menu_pages[current_page].rows_number - 1)) cursor_row++; else cursor_row = 255; } - if (sticks[ROLL] > 90) { + if (sticks[ROLL] > 90 && sticks[YAW] > 10 && sticks[YAW] < 90) { if (cursor_row > MAX_MENU_ROWS) { if (cursor_col < 2) cursor_col++; @@ -582,7 +584,7 @@ void show_menu(void) { cursor_col++; } } - if (sticks[ROLL] < 10) { + if (sticks[ROLL] < 10 && sticks[YAW] > 10 && sticks[YAW] < 90) { if (cursor_col > 0) cursor_col--; } @@ -609,6 +611,9 @@ void show_menu(void) { void updateOsd(void) { + static uint8_t skip = 0; + static bool blink = false; + static uint8_t arming = 0; uint32_t seconds; char line[30]; uint32_t now = micros(); @@ -618,12 +623,16 @@ void updateOsd(void) return; } next_osd_update_at = now + OSD_UPDATE_FREQUENCY; - if ((now / 1000000) & 1) { + if ( !(skip % 2)) + blink = !blink; + + if (skip++ & 1) { if (ARMING_FLAG(ARMED)) { if (!armed) { armed = true; armed_at = now; in_menu = false; + arming = 5; } } else { if (armed) { @@ -633,7 +642,7 @@ void updateOsd(void) for (uint8_t channelIndex = 0; channelIndex < 4; channelIndex++) { sticks[channelIndex] = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); } - if (!in_menu && sticks[YAW] > 90 && sticks[THROTTLE] > 90 && sticks[ROLL] < 10 && sticks[PITCH] > 90) { + if (!in_menu && sticks[YAW] < 10 && sticks[THROTTLE] > 10 && sticks[THROTTLE] < 90 && sticks[ROLL] > 10 && sticks[ROLL] < 90 && sticks[PITCH] > 90) { in_menu = true; cursor_row = 255; cursor_col = 2; @@ -642,8 +651,21 @@ void updateOsd(void) if (in_menu) { show_menu(); } else { + if (batteryWarningVoltage > vbat && blink) { + max7456_write_string("LOW VOLTAGE", 310); + } + if (arming && blink) { + max7456_write_string("ARMED", 283); + arming--; + } + if (!armed) { + max7456_write_string("DISARMED", 281); + } + sprintf(line, "\x97%d.%1d", vbat / 10, vbat % 10); max7456_write_string(line, 361); + sprintf(line, "\xba%d", rssi / 10); + max7456_write_string(line, 331); sprintf(line, "\x7e%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); max7456_write_string(line, 381); seconds = (now - armed_at) / 1000000 + armed_seconds; From e2ec4ce2befd36da4be7cae0c7d24e3432f257a6 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Sun, 13 Mar 2016 17:31:33 -0700 Subject: [PATCH 04/38] Make OSD items configurable --- src/main/blackbox/blackbox.c | 1 + src/main/blackbox/blackbox_io.c | 1 + src/main/config/config.c | 14 +++++++++ src/main/config/config_master.h | 1 + src/main/drivers/max7456.c | 21 +++++++++++-- src/main/drivers/max7456.h | 7 +++-- src/main/io/osd.c | 52 ++++++++++++++++++++------------- src/main/io/osd.h | 20 +++++++++++++ src/main/io/serial_cli.c | 28 +++++++++++++++++- src/main/io/serial_msp.c | 4 +++ src/main/main.c | 1 + src/main/telemetry/ltm.c | 1 + src/main/telemetry/smartport.c | 1 + 13 files changed, 124 insertions(+), 28 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 65d5759038..8518f03656 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -60,6 +60,7 @@ #include "io/serial_cli.h" #include "io/serial_msp.h" #include "io/statusindicator.h" +#include "io/osd.h" #include "rx/rx.h" #include "rx/msp.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 916b22ba25..1f3a974964 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -36,6 +36,7 @@ #include "io/escservo.h" #include "rx/rx.h" #include "io/rc_controls.h" +#include "io/osd.h" #include "io/gimbal.h" #include "io/gps.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index f73948de49..d4771e7bbc 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -54,6 +54,7 @@ #include "io/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" +#include "io/osd.h" #include "rx/rx.h" @@ -402,7 +403,20 @@ static void resetConf(void) //masterConfig.batteryConfig.vbatscale = 20; masterConfig.mag_hardware = MAG_NONE; // disabled by default masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS; +#endif + +#ifdef OSD masterConfig.vtx_channel = 0; + masterConfig.osdProfile.system = 0; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; + masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; + masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; + masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; + masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; + masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; + masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; + masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index fde12ac993..e68817937d 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -127,6 +127,7 @@ typedef struct master_t { #ifdef OSD uint8_t vtx_channel; + osd_profile osdProfile; #endif profile_t profile[MAX_PROFILE_COUNT]; diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index c1a7623a59..f4fde56c8b 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -46,7 +46,7 @@ uint8_t max7456_send(uint8_t add, uint8_t data) { // ============================================================ WRITE TO SCREEN -void max7456_init(void) { +void max7456_init(uint8_t system) { uint8_t max7456_reset=0x02; uint8_t max_screen_rows; uint8_t srdata = 0; @@ -68,6 +68,16 @@ void max7456_init(void) { video_signal_type = VIDEO_MODE_NTSC; } + // Override detected type: 0-AUTO, 1-PAL, 2-NTSC + switch(system) { + case 1: + video_signal_type = VIDEO_MODE_PAL; + break; + case 2: + video_signal_type = VIDEO_MODE_NTSC; + break; + } + max7456_reset |= video_signal_type; if (video_signal_type) { //PAL @@ -94,8 +104,13 @@ void max7456_init(void) { } // Copy string from ram into screen buffer -void max7456_write_string(const char *string, int address) { - char *dest = screen + address; +void max7456_write_string(const char *string, int16_t address) { + char *dest; + + if (address >= 0) + dest = screen + address; + else + dest = screen + (max_screen_size + address); while(*string) *dest++ = *string++; diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index c8e34a8a5e..2c5f5a0dbb 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -119,12 +119,13 @@ //uint8_t ENABLE_display; //uint8_t ENABLE_display_vert; //uint8_t DISABLE_display; -uint16_t MAX_screen_size; + +extern uint16_t max_screen_size; char screen[480]; -void max7456_init(void); +void max7456_init(uint8_t system); void max7456_draw_screen(void); void max7456_draw_screen_fast(void); -void max7456_write_string(const char *string, int address); +void max7456_write_string(const char *string, int16_t address); void max7456_write_nvm(uint8_t char_address, uint8_t *font_data); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 523746c943..f03eecc7f3 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -70,6 +70,7 @@ #include "io/display.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" +#include "io/osd.h" #include "sensors/sensors.h" #include "sensors/sonar.h" @@ -104,7 +105,6 @@ #ifdef OSD -#include "io/osd.h" #include "drivers/max7456.h" #include "drivers/rtc6705.h" #include "scheduler.h" @@ -485,10 +485,10 @@ void show_menu(void) { uint16_t pos; col_t *col; row_t *row; - uint16_t cursor_x, cursor_y; + int16_t cursor_x, cursor_y; sprintf(string_buffer, "EXIT SAVE+EXIT PAGE"); - max7456_write_string(string_buffer, 12 * OSD_LINE_LENGTH + 1); + max7456_write_string(string_buffer, -29); pos = (OSD_LINE_LENGTH - strlen(menu_pages[current_page].title)) / 2 + line * OSD_LINE_LENGTH; sprintf(string_buffer, "%s", menu_pages[current_page].title); @@ -591,7 +591,7 @@ void show_menu(void) { if (cursor_row > MAX_MENU_ROWS) { cursor_row = 255; - cursor_y = 12; + cursor_y = -1; switch(cursor_col) { case 0: cursor_x = 0; @@ -651,27 +651,37 @@ void updateOsd(void) if (in_menu) { show_menu(); } else { - if (batteryWarningVoltage > vbat && blink) { - max7456_write_string("LOW VOLTAGE", 310); + if (batteryWarningVoltage > vbat && blink && masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] != -1) { + max7456_write_string("LOW VOLTAGE", masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING]); } - if (arming && blink) { - max7456_write_string("ARMED", 283); + if (arming && blink && masterConfig.osdProfile.item_pos[OSD_ARMED] != -1) { + max7456_write_string("ARMED", masterConfig.osdProfile.item_pos[OSD_ARMED]); arming--; } - if (!armed) { - max7456_write_string("DISARMED", 281); + if (!armed && masterConfig.osdProfile.item_pos[OSD_DISARMED] != -1) { + max7456_write_string("DISARMED", masterConfig.osdProfile.item_pos[OSD_DISARMED]); } - sprintf(line, "\x97%d.%1d", vbat / 10, vbat % 10); - max7456_write_string(line, 361); - sprintf(line, "\xba%d", rssi / 10); - max7456_write_string(line, 331); - sprintf(line, "\x7e%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); - max7456_write_string(line, 381); - seconds = (now - armed_at) / 1000000 + armed_seconds; - sprintf(line, "\x9C %02d:%02d", seconds / 60, seconds % 60); - max7456_write_string(line, 351); - print_average_system_load(26, 0); + if (masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] != -1) { + sprintf(line, "\x97%d.%1d", vbat / 10, vbat % 10); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]); + } + if (masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] != -1) { + sprintf(line, "\xba%d", rssi / 10); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]); + } + if (masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] != -1) { + sprintf(line, "\x7e%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]); + } + if (masterConfig.osdProfile.item_pos[OSD_TIMER] != -1) { + seconds = (now - armed_at) / 1000000 + armed_seconds; + sprintf(line, "\x9C %02d:%02d", seconds / 60, seconds % 60); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_TIMER]); + } + if (masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] != -1) { + print_average_system_load(masterConfig.osdProfile.item_pos[OSD_CPU_LOAD], 0); + } } } else { max7456_draw_screen_fast(); @@ -686,7 +696,7 @@ void osdInit(void) current_vtx_channel = masterConfig.vtx_channel; rtc6705_set_channel(vtx_freq[current_vtx_channel]); #endif - max7456_init(); + max7456_init(masterConfig.osdProfile.system); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 733fdeb580..375e60bbfa 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -38,5 +38,25 @@ typedef struct { row_t rows[MAX_MENU_ROWS]; } page_t; + +typedef enum { + OSD_MAIN_BATT_VOLTAGE, + OSD_RSSI_VALUE, + OSD_TIMER, + OSD_THROTTLE_POS, + OSD_CPU_LOAD, + OSD_VTX_CHANNEL, + OSD_VOLTAGE_WARNING, + OSD_ARMED, + OSD_DISARMED, + OSD_MAX_ITEMS, // MUST BE LAST +} osd_items; + + +typedef struct { + uint8_t system; + int16_t item_pos[OSD_MAX_ITEMS]; +} osd_profile; + void updateOsd(void); void osdInit(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8636a8fafd..62870875de 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -59,6 +59,7 @@ #include "io/flashfs.h" #include "io/beeper.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/osd.h" #include "rx/rx.h" #include "rx/spektrum.h" @@ -437,6 +438,13 @@ static const char * const lookupTableDebug[] = { "MIXER", "AIRMODE" }; +#ifdef OSD +static const char * const lookupTableOsdType[] = { + "AUTO", + "PAL", + "NTSC" +}; +#endif typedef struct lookupTableEntry_s { const char * const *values; @@ -464,6 +472,9 @@ typedef enum { TABLE_MAG_HARDWARE, TABLE_DELTA_METHOD, TABLE_DEBUG, +#ifdef OSD + TABLE_OSD, +#endif } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -486,7 +497,10 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }, - { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) } + { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, +#ifdef OSD + { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, +#endif }; #define VALUE_TYPE_OFFSET 0 @@ -771,6 +785,18 @@ const clivalue_t valueTable[] = { #ifdef USE_RTC6705 { "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, #endif +#ifdef OSD + { "osd_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.system, .config.lookup = { TABLE_OSD } }, + { "osd_main_voltage_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { -480, 480 } }, + { "osd_rssi_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], .config.minmax = { -480, 480 } }, + { "osd_timer_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_TIMER], .config.minmax = { -480, 480 } }, + { "osd_throttle_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], .config.minmax = { -480, 480 } }, + { "osd_cpu_load_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CPU_LOAD], .config.minmax = { -480, 480 } }, + { "osd_vtx_channel_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL], .config.minmax = { -480, 480 } }, + { "osd_voltage_warning_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING], .config.minmax = { -480, 480 } }, + { "osd_armed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ARMED], .config.minmax = { -480, 480 } }, + { "osd_disarmed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_DISARMED], .config.minmax = { -480, 480 } }, +#endif }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b55f74aaca..af529a1704 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -59,6 +59,7 @@ #include "io/flashfs.h" #include "io/transponder_ir.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/osd.h" #include "telemetry/telemetry.h" @@ -1569,6 +1570,9 @@ static bool processInCommand(void) #endif #ifdef OSD case MSP_SET_OSD_CONFIG: + masterConfig.osdProfile.system = read8(); + for (i = 0; i < OSD_MAX_ITEMS; i++) + masterConfig.osdProfile.item_pos[i] = read16(); break; case MSP_OSD_CHAR_WRITE: addr = read8(); diff --git a/src/main/main.c b/src/main/main.c index 3402109942..e862449a09 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -69,6 +69,7 @@ #include "io/display.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" +#include "io/osd.h" #include "sensors/sensors.h" #include "sensors/sonar.h" diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 0f2f1d12f8..d4dd78ae8a 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -62,6 +62,7 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/beeper.h" +#include "io/osd.h" #include "rx/rx.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 281b44dc93..be17061d30 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -37,6 +37,7 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/ledstrip.h" +#include "io/osd.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" From 8face814e85b4cdb4b6215ca1fe5e5cfc05d2b7b Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Fri, 18 Mar 2016 00:00:24 -0700 Subject: [PATCH 05/38] fix osd menu navigation --- src/main/drivers/max7456.c | 19 +++- src/main/io/osd.c | 173 ++++++++++++++++++++++++------------- src/main/io/osd.h | 1 - src/main/io/serial_cli.c | 2 +- 4 files changed, 130 insertions(+), 65 deletions(-) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index f4fde56c8b..07c71a69e3 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -17,9 +17,12 @@ #include #include +#include #include +#include "common/printf.h" #include "platform.h" +#include "version.h" #ifdef USE_MAX7456 @@ -44,13 +47,12 @@ uint8_t max7456_send(uint8_t add, uint8_t data) { } -// ============================================================ WRITE TO SCREEN - void max7456_init(uint8_t system) { uint8_t max7456_reset=0x02; uint8_t max_screen_rows; uint8_t srdata = 0; uint16_t x; + char buf[30]; //Minimum spi clock period for max7456 is 100ns (10Mhz) spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); @@ -98,8 +100,17 @@ void max7456_init(uint8_t system) { DISABLE_MAX7456; delay(100); - for (x=0; x<256; x++) - screen[x] = (char)x; + + x = 160; + for (int i = 1; i < 5; i++) { + for (int j = 3; j < 27; j++) + screen[i * 30 + j] = (char)x++; + } + tfp_sprintf(buf, "BF VERSION: %s", FC_VERSION_STRING); + max7456_write_string(buf, 5*30+5); + max7456_write_string("MENU: THRT MID", 6*30+7); + max7456_write_string("YAW RIGHT", 7*30+13); + max7456_write_string("PITCH UP", 8*30+13); max7456_draw_screen(); } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f03eecc7f3..7cb01c4fdd 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -115,6 +115,10 @@ #define OSD_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5) #define OSD_LINE_LENGTH 30 +#define STICKMIN 10 +#define STICKMAX 90 + + static uint32_t next_osd_update_at = 0; static uint32_t armed_seconds = 0; static uint32_t armed_at = 0; @@ -126,6 +130,7 @@ static char string_buffer[30]; static uint8_t cursor_row = 255; static uint8_t cursor_col = 0; static bool in_menu = false; +static bool activating_menu = false; extern uint16_t rssi; @@ -230,6 +235,20 @@ void print_yaw_rate(uint16_t pos, uint8_t col) { } } +void print_tpa_rate(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->dynThrPID); + max7456_write_string(string_buffer, pos); + } +} + +void print_tpa_brkpt(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->tpa_breakpoint); + max7456_write_string(string_buffer, pos); + } +} + void update_int_pid(bool inc, uint8_t col, int pid_term) { void* ptr; switch(col) { @@ -337,6 +356,28 @@ void update_yaw_rate(bool increase, uint8_t col) { } } +void update_tpa_rate(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (currentControlRateProfile->dynThrPID < CONTROL_RATE_CONFIG_TPA_MAX) + currentControlRateProfile->dynThrPID++; + } else { + if (currentControlRateProfile->dynThrPID > 0) + currentControlRateProfile->dynThrPID--; + } +} + +void update_tpa_brkpt(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (currentControlRateProfile->tpa_breakpoint < PWM_RANGE_MAX) + currentControlRateProfile->tpa_breakpoint++; + } else { + if (currentControlRateProfile->tpa_breakpoint > PWM_RANGE_MIN) + currentControlRateProfile->tpa_breakpoint--; + } +} + void print_average_system_load(uint16_t pos, uint8_t col) { (void)col; sprintf(string_buffer, "%d", averageSystemLoadPercent); @@ -376,13 +417,11 @@ page_t menu_pages[] = { .rows = { { .title = "AVG LOAD", - .y_pos = 0, .update = NULL, .print = print_average_system_load }, { .title = "BATT", - .y_pos = 1, .update = NULL, .print = print_batt_voltage }, @@ -402,19 +441,16 @@ page_t menu_pages[] = { .rows = { { .title = "BAND", - .y_pos = 0, .update = update_vtx_band, .print = print_vtx_band }, { .title = "CHANNEL", - .y_pos = 1, .update = update_vtx_channel, .print = print_vtx_channel }, { .title = "FREQUENCY", - .y_pos = 2, .update = NULL, .print = print_vtx_freq } @@ -442,40 +478,44 @@ page_t menu_pages[] = { .rows = { { .title = "ROLL", - .y_pos = 0, .update = update_roll_pid, .print = print_roll_pid }, { .title = "PITCH", - .y_pos = 1, .update = update_pitch_pid, .print = print_pitch_pid }, { .title = "YAW", - .y_pos = 2, .update = update_yaw_pid, .print = print_yaw_pid }, { - .title = "ROLL_RATE", - .y_pos = 3, + .title = "ROLL RATE", .update = update_roll_rate, .print = print_roll_rate }, { - .title = "PITCH_RATE", - .y_pos = 4, + .title = "PITCH RATE", .update = update_pitch_rate, .print = print_pitch_rate }, { - .title = "YAW_RATE", - .y_pos = 5, + .title = "YAW RATE", .update = update_yaw_rate, .print = print_yaw_rate }, + { + .title = "TPA RATE", + .update = update_tpa_rate, + .print = print_tpa_rate + }, + { + .title = "TPA BRKPT", + .update = update_tpa_brkpt, + .print = print_tpa_brkpt + }, } }, }; @@ -487,41 +527,14 @@ void show_menu(void) { row_t *row; int16_t cursor_x, cursor_y; - sprintf(string_buffer, "EXIT SAVE+EXIT PAGE"); - max7456_write_string(string_buffer, -29); - - pos = (OSD_LINE_LENGTH - strlen(menu_pages[current_page].title)) / 2 + line * OSD_LINE_LENGTH; - sprintf(string_buffer, "%s", menu_pages[current_page].title); - max7456_write_string(string_buffer, pos); - - line += 2; - - for (int i = 0; i < menu_pages[current_page].cols_number; i++){ - col = &menu_pages[current_page].cols[i]; - if (cursor_col == i) - cursor_x = col->x_pos - 1; - - if (col->title) { - sprintf(string_buffer, "%s", col->title); - max7456_write_string(string_buffer, line * OSD_LINE_LENGTH + col->x_pos); - } + if (activating_menu) { + if (sticks[YAW] < 60 && sticks[YAW] > 40 && sticks[PITCH] > 40 && sticks[PITCH] < 60 && sticks[ROLL] > 40 && sticks[ROLL] < 60) + activating_menu = false; + else + return; } - line++; - for (int i = 0; i < menu_pages[current_page].rows_number; i++) { - row = &menu_pages[current_page].rows[i]; - if (cursor_row == i) - cursor_y = line + row->y_pos; - - sprintf(string_buffer, "%s", row->title); - max7456_write_string(string_buffer, (line + row->y_pos) * OSD_LINE_LENGTH + 1); - for (int j = 0; j < menu_pages[current_page].cols_number; j++) { - col = &menu_pages[current_page].cols[j]; - row->print((line + row->y_pos) * OSD_LINE_LENGTH + col->x_pos, j); - } - } - - if (sticks[YAW] > 90 && sticks[ROLL] > 10 && sticks[ROLL] < 90 && sticks[PITCH] > 10 && sticks[PITCH] < 90) { + if (sticks[YAW] > STICKMAX && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMIN && sticks[PITCH] < STICKMAX) { if (cursor_row > MAX_MENU_ROWS) { switch(cursor_col) { case 0: @@ -549,7 +562,7 @@ void show_menu(void) { } } - if (sticks[YAW] < 10 && sticks[ROLL] > 10 && sticks[ROLL] < 90 && sticks[PITCH] > 10 && sticks[PITCH] < 90) { + if (sticks[YAW] < STICKMIN && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMIN && sticks[PITCH] < STICKMAX) { if (cursor_row > MAX_MENU_ROWS) { if (cursor_col == 2 && current_page > 0) { current_page--; @@ -560,7 +573,7 @@ void show_menu(void) { } } - if (sticks[PITCH] > 90 && sticks[YAW] > 10 && sticks[YAW] < 90) { + if (sticks[PITCH] > STICKMAX && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { if (cursor_row > MAX_MENU_ROWS) { cursor_row = menu_pages[current_page].rows_number - 1; cursor_col = 0; @@ -569,13 +582,13 @@ void show_menu(void) { cursor_row--; } } - if (sticks[PITCH] < 10 && sticks[YAW] > 10 && sticks[YAW] < 90) { + if (sticks[PITCH] < STICKMIN && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { if (cursor_row < (menu_pages[current_page].rows_number - 1)) cursor_row++; else cursor_row = 255; } - if (sticks[ROLL] > 90 && sticks[YAW] > 10 && sticks[YAW] < 90) { + if (sticks[ROLL] > STICKMAX && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { if (cursor_row > MAX_MENU_ROWS) { if (cursor_col < 2) cursor_col++; @@ -584,7 +597,7 @@ void show_menu(void) { cursor_col++; } } - if (sticks[ROLL] < 10 && sticks[YAW] > 10 && sticks[YAW] < 90) { + if (sticks[ROLL] < STICKMIN && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { if (cursor_col > 0) cursor_col--; } @@ -606,6 +619,42 @@ void show_menu(void) { cursor_x = 0; } } + + sprintf(string_buffer, "EXIT SAVE+EXIT PAGE"); + max7456_write_string(string_buffer, -29); + + pos = (OSD_LINE_LENGTH - strlen(menu_pages[current_page].title)) / 2 + line * OSD_LINE_LENGTH; + sprintf(string_buffer, "%s", menu_pages[current_page].title); + max7456_write_string(string_buffer, pos); + + line += 2; + + for (int i = 0; i < menu_pages[current_page].cols_number; i++){ + col = &menu_pages[current_page].cols[i]; + if (cursor_col == i && cursor_row < MAX_MENU_ROWS) + cursor_x = col->x_pos - 1; + + if (col->title) { + sprintf(string_buffer, "%s", col->title); + max7456_write_string(string_buffer, line * OSD_LINE_LENGTH + col->x_pos); + } + } + + line++; + for (int i = 0; i < menu_pages[current_page].rows_number; i++) { + row = &menu_pages[current_page].rows[i]; + if (cursor_row == i) + cursor_y = line; + + sprintf(string_buffer, "%s", row->title); + max7456_write_string(string_buffer, line * OSD_LINE_LENGTH + 1); + for (int j = 0; j < menu_pages[current_page].cols_number; j++) { + col = &menu_pages[current_page].cols[j]; + row->print(line * OSD_LINE_LENGTH + col->x_pos, j); + } + line++; + } + max7456_write_string(">", cursor_x + cursor_y * OSD_LINE_LENGTH); } @@ -637,15 +686,16 @@ void updateOsd(void) } else { if (armed) { armed = false; - armed_seconds += (now - armed_at) / 1000000; + armed_seconds += ((now - armed_at) / 1000000); } for (uint8_t channelIndex = 0; channelIndex < 4; channelIndex++) { sticks[channelIndex] = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); } - if (!in_menu && sticks[YAW] < 10 && sticks[THROTTLE] > 10 && sticks[THROTTLE] < 90 && sticks[ROLL] > 10 && sticks[ROLL] < 90 && sticks[PITCH] > 90) { + if (!in_menu && sticks[YAW] > STICKMAX && sticks[THROTTLE] > STICKMIN && sticks[THROTTLE] < STICKMAX && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMAX) { in_menu = true; cursor_row = 255; cursor_col = 2; + activating_menu = true; } } if (in_menu) { @@ -663,20 +713,25 @@ void updateOsd(void) } if (masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] != -1) { - sprintf(line, "\x97%d.%1d", vbat / 10, vbat % 10); + sprintf(line, "\x01%d.%1d", vbat / 10, vbat % 10); max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]); } if (masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] != -1) { - sprintf(line, "\xba%d", rssi / 10); + sprintf(line, "\x02%d", rssi / 10); max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]); } if (masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] != -1) { - sprintf(line, "\x7e%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + sprintf(line, "\x03%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]); } if (masterConfig.osdProfile.item_pos[OSD_TIMER] != -1) { - seconds = (now - armed_at) / 1000000 + armed_seconds; - sprintf(line, "\x9C %02d:%02d", seconds / 60, seconds % 60); + if (armed) { + seconds = armed_seconds + ((now-armed_at) / 1000000); + sprintf(line, "\x04 %02d:%02d", seconds / 60, seconds % 60); + } else { + seconds = now / 1000000; + sprintf(line, "\x05 %02d:%02d", seconds / 60, seconds % 60); + } max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_TIMER]); } if (masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] != -1) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 375e60bbfa..c72a417761 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -25,7 +25,6 @@ typedef struct { typedef struct { const char* title; - uint8_t y_pos; void (*update)(bool increase, uint8_t col); void (*print)(uint16_t pos, uint8_t col); } row_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 62870875de..641e9ec2cd 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -786,7 +786,7 @@ const clivalue_t valueTable[] = { { "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, #endif #ifdef OSD - { "osd_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.system, .config.lookup = { TABLE_OSD } }, + { "osd_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.system, .config.minmax = { 0, 2 } }, { "osd_main_voltage_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { -480, 480 } }, { "osd_rssi_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], .config.minmax = { -480, 480 } }, { "osd_timer_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_TIMER], .config.minmax = { -480, 480 } }, From 46e50fb9c26f5a57f2302b9ec3df707eda6ebf54 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Sun, 20 Mar 2016 20:04:11 -0700 Subject: [PATCH 06/38] Add mpu6500 support for new board revision, cleanup code --- Makefile | 2 ++ src/main/drivers/rtc6705.c | 2 +- src/main/io/osd.c | 4 +--- src/main/target/SIRINFPV/target.h | 33 ++++++++++++++++--------------- 4 files changed, 21 insertions(+), 20 deletions(-) diff --git a/Makefile b/Makefile index a91fdf7f43..8786617fd8 100644 --- a/Makefile +++ b/Makefile @@ -752,7 +752,9 @@ SPRACINGF3MINI_SRC = \ SIRINFPV_SRC = \ $(STM32F30x_COMMON_SRC) \ drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_spi_mpu6500.c \ drivers/serial_usb_vcp.c \ drivers/sdcard.c \ drivers/sdcard_standard.c \ diff --git a/src/main/drivers/rtc6705.c b/src/main/drivers/rtc6705.c index ceb525fae7..319430f526 100644 --- a/src/main/drivers/rtc6705.c +++ b/src/main/drivers/rtc6705.c @@ -142,7 +142,7 @@ void rtc6705_set_channel(uint16_t channel_freq) { uint32_t freq = (uint32_t)channel_freq * 1000; uint32_t N, A; - freq /= 40; // TODO check all channels for the R value + freq /= 40; N = freq / 64; A = freq % 64; rtc6705_write_register(0, 400); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7cb01c4fdd..52d0a8a65a 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -397,8 +397,6 @@ void print_batt_voltage(uint16_t pos, uint8_t col) { { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, - { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, - { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "acro_plus_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } }, { "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } }, */ @@ -460,7 +458,7 @@ page_t menu_pages[] = { { .title = "PID SETTINGS", .cols_number = 3, - .rows_number = 6, + .rows_number = 8, .cols = { { .title = "P", diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index daccbe276f..2f91688911 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -17,7 +17,11 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "FPVR" +#define TARGET_BOARD_IDENTIFIER "SIRF" + +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_2 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB #define BEEP_GPIO GPIOA #define BEEP_PIN Pin_1 @@ -25,18 +29,20 @@ #define USABLE_TIMER_CHANNEL_COUNT 6 -#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO #define USE_GYRO_SPI_MPU6000 -//#define USE_GYRO_SPI_MPU6500 +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 #define ACC #define USE_ACC_SPI_MPU6000 -//#define USE_ACC_SPI_MPU6500 +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 // MPU6000 #define ACC_MPU6000_ALIGN CW180_DEG @@ -47,8 +53,8 @@ #define MPU6000_SPI_INSTANCE SPI1 // MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW90_DEG +#define GYRO_MPU6500_ALIGN CW90_DEG #define MPU6500_CS_GPIO GPIOA #define MPU6500_CS_PIN GPIO_Pin_4 @@ -59,11 +65,11 @@ #define BEEPER #define USB_IO -#define USB_CABLE_DETECTION -#define USB_DETECT_PIN GPIO_Pin_2 -#define USB_DETECT_GPIO_PORT GPIOB -#define USB_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOB +//#define USB_CABLE_DETECTION +//#define USB_DETECT_PIN GPIO_Pin_2 +//#define USB_DETECT_GPIO_PORT GPIOB +//#define USB_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOB #define USE_VCP #define USE_USART1 @@ -175,7 +181,6 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - #define ADC_INSTANCE ADC1 #define ADC_DMA_CHANNEL DMA1_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 @@ -192,11 +197,7 @@ #define GTUNE #define USE_CLI #define OSD - -//#define SPEKTRUM_BIND -// USART3, -//#define BIND_PORT GPIOB -//#define BIND_PIN Pin_11 +#define LED0 #define USE_SERIAL_1WIRE From db5184d6034a84401d49fa8decd6a4c94bd73777 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Fri, 3 Jun 2016 15:18:54 +0100 Subject: [PATCH 07/38] Add rcYawRate into Blackbox header and Calculate Rate Function Add the new Yaw Rate parameter into the log header --- src/main/blackbox/blackbox.c | 77 +++++++++++++++++++----------------- src/main/flight/pid.c | 2 +- 2 files changed, 41 insertions(+), 38 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ebda24dca2..40fa2e2306 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1181,156 +1181,159 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); break; case 14: - blackboxPrintfHeaderLine("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8); + blackboxPrintfHeaderLine("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8); break; case 15: - blackboxPrintfHeaderLine("thrMid:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrMid8); + blackboxPrintfHeaderLine("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8); break; case 16: - blackboxPrintfHeaderLine("thrExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrExpo8); + blackboxPrintfHeaderLine("thrMid:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrMid8); break; case 17: - blackboxPrintfHeaderLine("dynThrPID:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].dynThrPID); + blackboxPrintfHeaderLine("thrExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrExpo8); break; case 18: - blackboxPrintfHeaderLine("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint); + blackboxPrintfHeaderLine("dynThrPID:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].dynThrPID); break; case 19: + blackboxPrintfHeaderLine("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint); + break; + case 20: blackboxPrintfHeaderLine("rates:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL], masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH], masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]); break; - case 20: + case 21: blackboxPrintfHeaderLine("looptime:%d", targetLooptime); break; - case 21: + case 22: blackboxPrintfHeaderLine("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController); break; - case 22: + case 23: blackboxPrintfHeaderLine("rollPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]); break; - case 23: + case 24: blackboxPrintfHeaderLine("pitchPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]); break; - case 24: + case 25: blackboxPrintfHeaderLine("yawPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]); break; - case 25: + case 26: blackboxPrintfHeaderLine("altPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]); break; - case 26: + case 27: blackboxPrintfHeaderLine("posPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]); break; - case 27: + case 28: blackboxPrintfHeaderLine("posrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]); break; - case 28: + case 29: blackboxPrintfHeaderLine("navrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]); break; - case 29: + case 30: blackboxPrintfHeaderLine("levelPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]); break; - case 30: + case 31: blackboxPrintfHeaderLine("magPID:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]); break; - case 31: + case 32: blackboxPrintfHeaderLine("velPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]); break; - case 32: + case 33: blackboxPrintfHeaderLine("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); break; - case 33: + case 34: blackboxPrintfHeaderLine("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); break; - case 34: + case 35: blackboxPrintfHeaderLine("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); break; - case 35: + case 36: blackboxPrintfHeaderLine("dynamic_pid:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid); break; - case 36: + case 37: blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); break; - case 37: + case 38: blackboxPrintfHeaderLine("yawItermResetRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); break; - case 38: + case 39: blackboxPrintfHeaderLine("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); break; - case 39: + case 40: blackboxPrintfHeaderLine("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); break; - case 40: + case 41: blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband); break; - case 41: + case 42: blackboxPrintfHeaderLine("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); break; - case 42: + case 43: blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf); break; - case 43: + case 44: blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); break; - case 44: + case 45: blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); break; - case 45: + case 46: blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware); break; - case 46: + case 47: blackboxPrintfHeaderLine("baro_hardware:%d", masterConfig.baro_hardware); break; - case 47: + case 48: blackboxPrintfHeaderLine("mag_hardware:%d", masterConfig.mag_hardware); break; - case 48: + case 49: blackboxPrintfHeaderLine("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); break; - case 49: + case 50: blackboxPrintfHeaderLine("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); break; - case 50: + case 51: blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing); break; - case 51: + case 52: blackboxPrintfHeaderLine("features:%d", masterConfig.enabledFeatures); break; default: diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f9f2a22317..b9d706dd7d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -79,7 +79,7 @@ float calculateRate(int axis, const controlRateConfig_t *controlRateConfig) { float angleRate; if (isSuperExpoActive()) { - float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / 500.0f) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f))); + float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcYawRate8 / 100.0f))) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f))); rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f)), 0.01f, 1.00f)); angleRate = rcFactor * ((27 * rcCommand[axis]) / 16.0f); From 1e68552a0c7365c80f28402f8e6f7376f7619251 Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Sun, 5 Jun 2016 05:53:27 -0500 Subject: [PATCH 08/38] Initial FURYF3 Target --- Makefile | 37 ++- fake_travis_build.sh | 3 +- src/main/config/config.c | 9 +- src/main/drivers/bus_i2c_stm32f30x.c | 2 + src/main/drivers/pwm_mapping.c | 48 +++ src/main/drivers/system.c | 2 +- src/main/drivers/timer.c | 23 ++ src/main/io/serial_msp.c | 2 +- src/main/main.c | 5 + src/main/sensors/initialisation.c | 13 + src/main/sensors/sonar.c | 2 +- src/main/target/FURYF3/system_stm32f30x.c | 371 ++++++++++++++++++++++ src/main/target/FURYF3/system_stm32f30x.h | 76 +++++ src/main/target/FURYF3/target.h | 221 +++++++++++++ top_makefile | 2 + 15 files changed, 808 insertions(+), 8 deletions(-) create mode 100644 src/main/target/FURYF3/system_stm32f30x.c create mode 100644 src/main/target/FURYF3/system_stm32f30x.h create mode 100644 src/main/target/FURYF3/target.h diff --git a/Makefile b/Makefile index e5a4916594..fe5283d572 100644 --- a/Makefile +++ b/Makefile @@ -46,7 +46,7 @@ FORKNAME = betaflight CC3D_TARGETS = CC3D CC3D_OPBL -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY FURYF3 # Valid targets for OP VCP support VCP_VALID_TARGETS = $(CC3D_TARGETS) @@ -56,9 +56,9 @@ OPBL_VALID_TARGETS = CC3D_OPBL 64K_TARGETS = CJMCU 128K_TARGETS = ALIENFLIGHTF1 $(CC3D_TARGETS) NAZE OLIMEXINO RMDO AFROMINI -256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY +256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY FURYF3 -F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY +F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY FURYF3 # note that there is no hardfault debugging startup file assembly handler for other platforms ifeq ($(DEBUG_HARDFAULTS),F3) @@ -150,6 +150,13 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ VPATH := $(VPATH):$(FATFS_DIR) endif +ifeq ($(TARGET),FURY) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(FATFS_DIR) \ + +VPATH := $(VPATH):$(FATFS_DIR) +endif + LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion @@ -810,6 +817,30 @@ SINGULARITY_SRC = \ $(COMMON_SRC) \ $(VCP_SRC) +FURYF3_SRC = \ + $(STM32F30x_COMMON_SRC) \ + drivers/accgyro_mpu.c \ + drivers/barometer_ms5611.c \ + drivers/barometer_bmp280.c \ + drivers/display_ug2864hsweg01.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_usb_vcp.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + drivers/flash_m25p16.c \ + drivers/sonar_hcsr04.c \ + drivers/serial_softserial.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ + io/flashfs.c \ + $(HIGHEND_SRC) \ + $(COMMON_SRC) \ + $(VCP_SRC) + # Search path and source files for the ST stdperiph library VPATH := $(VPATH):$(STDPERIPH_DIR)/src diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 7749d64354..4babddd212 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -17,7 +17,8 @@ targets=("PUBLISHMETA=True" \ "TARGET=ALIENFLIGHTF1" \ "TARGET=ALIENFLIGHTF3" \ "TARGET=DOGE" \ - "TARGET=SINGULARITY") + "TARGET=SINGULARITY" \ + "TARGET=FURYF3") #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) diff --git a/src/main/config/config.c b/src/main/config/config.c index 46fcd292a0..6f0b952755 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -385,7 +385,7 @@ static void resetConf(void) masterConfig.version = EEPROM_CONF_VERSION; masterConfig.mixerMode = MIXER_QUADX; featureClearAll(); -#if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) || defined(DOGE) || defined(SINGULARITY) +#if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) || defined(DOGE) || defined(SINGULARITY) || defined(FURYF3) featureSet(FEATURE_RX_PPM); #endif @@ -596,6 +596,13 @@ static void resetConf(void) masterConfig.blackbox_rate_denom = 1; #endif +#if defined(FURYF3) + featureSet(FEATURE_BLACKBOX); + masterConfig.blackbox_device = 2; + masterConfig.blackbox_rate_num = 1; + masterConfig.blackbox_rate_denom = 1; +#endif + // alternative defaults settings for COLIBRI RACE targets #if defined(COLIBRI_RACE) masterConfig.escAndServoConfig.minthrottle = 1025; diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 8b33e7e461..b45f6bd6bb 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -30,6 +30,7 @@ #ifndef SOFT_I2C +#if !defined(I2C1_SCL_GPIO) #define I2C1_SCL_GPIO GPIOB #define I2C1_SCL_GPIO_AF GPIO_AF_4 #define I2C1_SCL_PIN GPIO_Pin_6 @@ -40,6 +41,7 @@ #define I2C1_SDA_PIN GPIO_Pin_7 #define I2C1_SDA_PIN_SOURCE GPIO_PinSource7 #define I2C1_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOB +#endif #if !defined(I2C2_SCL_GPIO) #define I2C2_SCL_GPIO GPIOF diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index b786ba0cee..d6b523f8c8 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -748,6 +748,54 @@ static const uint16_t airPWM[] = { }; #endif +#ifdef FURYF3 +static const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static 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_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; +#endif + static const uint16_t * const hardwareMaps[] = { multiPWM, multiPPM, diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 1cc192121f..af25ef0c99 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -83,7 +83,7 @@ void EXTI15_10_IRQHandler(void) extiHandler(EXTI15_10_IRQn); } -#if defined(CC3D) +#if defined(CC3D) || defined(FURYF3) void EXTI3_IRQHandler(void) { extiHandler(EXTI3_IRQn); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index bc52c6e3cc..659236602a 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -416,6 +416,29 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif +#ifdef FURYF3 +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, GPIOB, Pin_3, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_1}, // PPM IN + { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2}, // PWM4 - S1 + { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2}, // PWM5 - S2 + { TIM17, GPIOB, Pin_5, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_10}, // PWM6 - S3 + { TIM16, GPIOB, Pin_4, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_1}, // PWM7 - S4 + + { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // GPIO TIMER - LED_STRIP + +}; + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + +#endif + #define USED_TIMER_COUNT BITCOUNT(USED_TIMERS) #define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a827db5d40..dcae1a0ef1 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -125,7 +125,7 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.mag_hardware = 1; masterConfig.pid_process_denom = 2; } else if (looptime < 375) { -#if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3) || defined(SPRACINGF3EVO) || defined(DOGE) +#if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3) || defined(SPRACINGF3EVO) || defined(DOGE) || defined(FURYF3) masterConfig.acc_hardware = 0; #else masterConfig.acc_hardware = 1; diff --git a/src/main/main.c b/src/main/main.c index d89bb44923..e05eefb0ca 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -426,6 +426,11 @@ void init(void) } #endif +#if defined(FURYF3) && defined(SONAR) && defined(USE_SOFTSERIAL1) + if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { + serialRemovePort(SERIAL_PORT_SOFTSERIAL1); + } +#endif #ifdef USE_I2C #if defined(NAZE) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index ab9b27c898..a27ca0663d 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -223,6 +223,19 @@ const extiConfig_t *selectMPUIntExtiConfig(void) } #endif +#if defined(FURYF3) + static const extiConfig_t FURYF3MPUIntExtiConfig = { + .gpioAHBPeripherals = RCC_AHBPeriph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = Pin_3, + .exti_port_source = EXTI_PortSourceGPIOA, + .exti_pin_source = EXTI_PinSource3, + .exti_line = EXTI_Line3, + .exti_irqn = EXTI3_IRQn + }; + return &FURYF3MPUIntExtiConfig; +#endif + return NULL; } diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 4e0b2bea52..93104d0cd7 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -101,7 +101,7 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon .exti_irqn = EXTI0_IRQn }; return &sonarHardware; -#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI) +#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI) || defined(FURYF3) UNUSED(batteryConfig); static const sonarHardware_t const sonarHardware = { .trigger_pin = Pin_0, // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) diff --git a/src/main/target/FURYF3/system_stm32f30x.c b/src/main/target/FURYF3/system_stm32f30x.c new file mode 100644 index 0000000000..de8a873131 --- /dev/null +++ b/src/main/target/FURYF3/system_stm32f30x.c @@ -0,0 +1,371 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.c + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F30x devices, + * and is generated by the clock configuration tool + * stm32f30x_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): 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_stm32f30x.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 (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f30x.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 8MHz, refer to "HSE_VALUE" define + * in "stm32f30x.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 STM32F30x device + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *---------------------------------------------------------------------------- + * PLLMUL | 9 + *----------------------------------------------------------------------------- + * PREDIV | 1 + *----------------------------------------------------------------------------- + * USB Clock | ENABLE + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** @addtogroup STM32F30x_System_Private_Includes + * @{ + */ + +#include "stm32f30x.h" + +uint32_t hse_value = HSE_VALUE; + +/** + * @} + */ + +/* Private typedef -----------------------------------------------------------*/ + +/** @addtogroup STM32F30x_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 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/** + * @} + */ + +/* Private macro -------------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Variables + * @{ + */ + + uint32_t SystemCoreClock = 72000000; + + __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_FunctionPrototypes + * @{ + */ + +void SetSysClock(void); + +/** + * @} + */ + +/** @addtogroup STM32F30x_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(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR &= 0xF87FC00C; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; + + /* Reset USARTSW[1:0], I2CSW and TIMs bits */ + RCC->CFGR3 &= (uint32_t)0xFF00FCCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + //SetSysClock(); // called from main() + +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ +#endif +} + +/** + * @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 stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 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, pllmull = 0, pllsource = 0, prediv1factor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllsource == 0x00) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + } + else + { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @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(void) +{ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + + /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ + /* 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 Prefetch Buffer and set Flash Latency */ + FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; + + /* HCLK = SYSCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 2 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; + + /* PLL configuration */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + /* Enable PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Select PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; + + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) + { + } + } + 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 */ + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file diff --git a/src/main/target/FURYF3/system_stm32f30x.h b/src/main/target/FURYF3/system_stm32f30x.h new file mode 100644 index 0000000000..57e5e05d74 --- /dev/null +++ b/src/main/target/FURYF3/system_stm32f30x.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.h + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F30X_H +#define __SYSTEM_STM32F30X_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/** @addtogroup STM32F30x_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F30X_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h new file mode 100644 index 0000000000..0fbe17c57b --- /dev/null +++ b/src/main/target/FURYF3/target.h @@ -0,0 +1,221 @@ +/* + * 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 "FURY" + +#define LED0 + +#define LED0_GPIO GPIOC +#define LED0_PIN Pin_14 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOC + +#define BEEPER +#define BEEP_GPIO GPIOC +#define BEEP_PIN Pin_15 +#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC +#define BEEPER_INVERTED + +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define GYRO +#define ACC + +#define MPU6000_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOA +#define MPU6000_CS_GPIO GPIOA +#define MPU6000_CS_PIN GPIO_Pin_4 +#define MPU6000_SPI_INSTANCE SPI1 + +#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOA +#define MPU6500_CS_GPIO GPIOA +#define MPU6500_CS_PIN GPIO_Pin_4 +#define MPU6500_SPI_INSTANCE SPI1 + +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW180_DEG // changedkb 270 +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG // changedkb 270 + +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW90_DEG // changedkb 270 +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW90_DEG // changedkb 270 + +#define BARO +#define USE_BARO_MS5611 +#define USE_BARO_BMP280 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_GPIO GPIOB +#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB +#define SPI2_NSS_PIN Pin_12 +#define SPI2_NSS_PIN_SOURCE GPIO_PinSource12 +#define SPI2_SCK_PIN Pin_13 +#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13 +#define SPI2_MISO_PIN Pin_14 +#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14 +#define SPI2_MOSI_PIN Pin_15 +#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15 + +//#define USE_FLASHFS +//#define USE_FLASH_M25P16 +//#define M25P16_CS_GPIO GPIOB +//#define M25P16_CS_PIN GPIO_Pin_12 +//#define M25P16_SPI_INSTANCE SPI2 + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN GPIO_Pin_2 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 +#define SDCARD_DETECT_GPIO_PORT GPIOB +#define SDCARD_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOB +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB +#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn + +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_GPIO SPI2_GPIO +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + +#define USABLE_TIMER_CHANNEL_COUNT 8 + +#define USB_IO + +#define USE_VCP +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define USE_SOFTSERIAL1 +#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_14 // PA14 +#define UART2_RX_PIN GPIO_Pin_15 // PA15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource14 +#define UART2_RX_PINSOURCE GPIO_PinSource15 + +#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 1 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 2 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) + +#define I2C1_SCL_GPIO GPIOB +#define I2C1_SCL_GPIO_AF GPIO_AF_4 +#define I2C1_SCL_PIN GPIO_Pin_8 +#define I2C1_SCL_PIN_SOURCE GPIO_PinSource8 +#define I2C1_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOB +#define I2C1_SDA_GPIO GPIOB +#define I2C1_SDA_GPIO_AF GPIO_AF_4 +#define I2C1_SDA_PIN GPIO_Pin_9 +#define I2C1_SDA_PIN_SOURCE GPIO_PinSource9 +#define I2C1_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOB + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + +#define ADC_INSTANCE ADC1 +#define ADC_DMA_CHANNEL DMA1_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_CHANNEL ADC_Channel_1 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_2 + +#define CURRENT_METER_ADC_GPIO GPIOA +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_2 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_3 + +#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 BLACKBOX +#define DISPLAY +#define GPS +#define SERIAL_RX +#define TELEMETRY +#define USE_SERVOS +#define USE_CLI +#define SONAR + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PORT GPIOB +#define BIND_PIN Pin_11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE \ No newline at end of file diff --git a/top_makefile b/top_makefile index 4b1bb90e78..bbc1f43f14 100644 --- a/top_makefile +++ b/top_makefile @@ -15,6 +15,7 @@ ALL_TARGETS += ircfusionf3 ALL_TARGETS += afromini ALL_TARGETS += doge ALL_TARGETS += singularity +ALL_TARGETS += furyf3 CLEAN_TARGETS := $(addprefix clean_, $(ALL_TARGETS)) @@ -35,6 +36,7 @@ clean_ircfusionf3 ircfusionf3 : opts := TARGET=IRCFUSIONF3 clean_afromini afromini : opts := TARGET=AFROMINI clean_doge doge : opts := TARGET=DOGE clean_singularity singularity : opts := TARGET=SINGULARITY +clean_furyf3 furyf3 : opts := TARGET=FURYF3 .PHONY: all clean From 047d962e6588e14f71470e0d6596b7c5360b7dc3 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Mon, 6 Jun 2016 18:22:52 +0100 Subject: [PATCH 09/38] Blackbox Coding Simplification Simplified header record writing using macros to outnumber the case statements --- src/main/blackbox/blackbox.c | 288 +++++++++++------------------------ 1 file changed, 91 insertions(+), 197 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 40fa2e2306..80df3ba141 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1118,6 +1118,15 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v return xmitState.headerIndex < headerCount; } +#ifndef BLACKBOX_PRINT_HEADER_LINE +#define BLACKBOX_PRINT_HEADER_LINE(x, ...) case __COUNTER__: \ + blackboxPrintfHeaderLine(x, __VA_ARGS__); \ + break; +#define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \ + {__VA_ARGS__}; \ + break; +#endif + /** * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns * true iff transmission is complete, otherwise call again later to continue transmission. @@ -1130,212 +1139,97 @@ static bool blackboxWriteSysinfo() } switch (xmitState.headerIndex) { - case 0: - blackboxPrintfHeaderLine("Firmware type:Cleanflight"); - break; - case 1: - blackboxPrintfHeaderLine("Firmware revision:Betaflight %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName); - break; - case 2: - blackboxPrintfHeaderLine("Firmware date:%s %s", buildDate, buildTime); - break; - case 3: - blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); - break; - case 4: - blackboxPrintfHeaderLine("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); - break; - case 5: - blackboxPrintfHeaderLine("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); - break; - case 6: - blackboxPrintfHeaderLine("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle); - break; - case 7: - blackboxPrintfHeaderLine("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale)); - break; - case 8: - blackboxPrintfHeaderLine("acc_1G:%u", acc_1G); - break; - case 9: + BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight"); + BLACKBOX_PRINT_HEADER_LINE("Firmware revision:Betaflight %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName); + BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); + BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); + BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); + BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle); + BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale)); + BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc_1G); + + BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { blackboxPrintfHeaderLine("vbatscale:%u", masterConfig.batteryConfig.vbatscale); } else { xmitState.headerIndex += 2; // Skip the next two vbat fields too } - break; - case 10: - blackboxPrintfHeaderLine("vbatcellvoltage:%u,%u,%u", masterConfig.batteryConfig.vbatmincellvoltage, - masterConfig.batteryConfig.vbatwarningcellvoltage, masterConfig.batteryConfig.vbatmaxcellvoltage); - break; - case 11: - blackboxPrintfHeaderLine("vbatref:%u", vbatReference); - break; - case 12: + ); + + BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u", masterConfig.batteryConfig.vbatmincellvoltage, + masterConfig.batteryConfig.vbatwarningcellvoltage, + masterConfig.batteryConfig.vbatmaxcellvoltage); + BLACKBOX_PRINT_HEADER_LINE("vbatref:%u", vbatReference); + + BLACKBOX_PRINT_HEADER_LINE_CUSTOM( //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too: if (feature(FEATURE_CURRENT_METER)) { blackboxPrintfHeaderLine("currentMeter:%d,%d", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); } - break; - case 13: - blackboxPrintfHeaderLine("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); - break; - case 14: - blackboxPrintfHeaderLine("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8); - break; - case 15: - blackboxPrintfHeaderLine("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8); - break; - case 16: - blackboxPrintfHeaderLine("thrMid:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrMid8); - break; - case 17: - blackboxPrintfHeaderLine("thrExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrExpo8); - break; - case 18: - blackboxPrintfHeaderLine("dynThrPID:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].dynThrPID); - break; - case 19: - blackboxPrintfHeaderLine("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint); - break; - case 20: - blackboxPrintfHeaderLine("rates:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL], - masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH], - masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]); - break; - case 21: - blackboxPrintfHeaderLine("looptime:%d", targetLooptime); - break; - case 22: - blackboxPrintfHeaderLine("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController); - break; - case 23: - blackboxPrintfHeaderLine("rollPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]); - break; - case 24: - blackboxPrintfHeaderLine("pitchPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]); - break; - case 25: - blackboxPrintfHeaderLine("yawPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]); - break; - case 26: - blackboxPrintfHeaderLine("altPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]); - break; - case 27: - blackboxPrintfHeaderLine("posPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]); - break; - case 28: - blackboxPrintfHeaderLine("posrPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]); - break; - case 29: - blackboxPrintfHeaderLine("navrPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]); - break; - case 30: - blackboxPrintfHeaderLine("levelPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]); - break; - case 31: - blackboxPrintfHeaderLine("magPID:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]); - break; - case 32: - blackboxPrintfHeaderLine("velPID:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL], - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]); - break; - case 33: - blackboxPrintfHeaderLine("yaw_p_limit:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); - break; - case 34: - blackboxPrintfHeaderLine("yaw_lpf_hz:%d", - (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); - break; - case 35: - blackboxPrintfHeaderLine("dterm_average_count:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); - break; - case 36: - blackboxPrintfHeaderLine("dynamic_pid:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid); - break; - case 37: - blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); - break; - case 38: - blackboxPrintfHeaderLine("yawItermResetRate:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); - break; - case 39: - blackboxPrintfHeaderLine("dterm_lpf_hz:%d", - (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); - break; - case 40: - blackboxPrintfHeaderLine("airmode_activate_throttle:%d", - masterConfig.rxConfig.airModeActivateThreshold); - break; - case 41: - blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband); - break; - case 42: - blackboxPrintfHeaderLine("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); - break; - case 43: - blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf); - break; - case 44: - blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); - break; - case 45: - blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); - break; - case 46: - blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware); - break; - case 47: - blackboxPrintfHeaderLine("baro_hardware:%d", masterConfig.baro_hardware); - break; - case 48: - blackboxPrintfHeaderLine("mag_hardware:%d", masterConfig.mag_hardware); - break; - case 49: - blackboxPrintfHeaderLine("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); - break; - case 50: - blackboxPrintfHeaderLine("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); - break; - case 51: - blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing); - break; - case 52: - blackboxPrintfHeaderLine("features:%d", masterConfig.enabledFeatures); - break; + ); + + BLACKBOX_PRINT_HEADER_LINE("looptime:%d", 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); + BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8); + BLACKBOX_PRINT_HEADER_LINE("thrMid:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrMid8); + BLACKBOX_PRINT_HEADER_LINE("thrExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrExpo8); + BLACKBOX_PRINT_HEADER_LINE("dynThrPID:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].dynThrPID); + BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint); + BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL], + masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH], + masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]); + BLACKBOX_PRINT_HEADER_LINE("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController); + BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]); + BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]); + BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]); + BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]); + BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]); + BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]); + BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]); + BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]); + BLACKBOX_PRINT_HEADER_LINE("magPID:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]); + BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]); + BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); + BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); + BLACKBOX_PRINT_HEADER_LINE("dynamic_pid:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid); + BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); + BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); + BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware); + BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); + BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware); + BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); + BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing); + BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); + BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures); + default: return true; } From 6e228eacf5e53c38e2c69569e9d846a3e2e96a55 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 7 Jun 2016 11:39:37 +1200 Subject: [PATCH 10/38] Fixed rateprofile dump / restore --- src/main/io/serial_cli.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d077b96148..4cad900d68 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2051,24 +2051,27 @@ static void cliDump(char *cmdline) if (dumpMask & DUMP_ALL) { uint8_t activeProfile = masterConfig.current_profile_index; - uint8_t currentRateIndex = currentProfile->activeRateProfile; uint8_t profileCount; - uint8_t rateCount; for (profileCount=0; profileCountactiveRateProfile; + uint8_t rateCount; for (rateCount=0; rateCount Date: Tue, 7 Jun 2016 20:16:06 +0100 Subject: [PATCH 11/38] Added default features and default rx that can be set in the target.h file. --- src/main/config/config.c | 35 +++++++------------------ src/main/target/ALIENFLIGHTF3/target.h | 1 + src/main/target/CC3D/target.h | 1 + src/main/target/CJMCU/target.h | 1 + src/main/target/COLIBRI_RACE/target.h | 1 + src/main/target/DOGE/target.h | 1 + src/main/target/FURYF3/target.h | 2 ++ src/main/target/LUX_RACE/target.h | 1 + src/main/target/MOTOLAB/target.h | 1 + src/main/target/SINGULARITY/target.h | 2 ++ src/main/target/SPARKY/target.h | 1 + src/main/target/SPRACINGF3/target.h | 3 +++ src/main/target/SPRACINGF3EVO/target.h | 5 ++-- src/main/target/SPRACINGF3MINI/target.h | 2 +- 14 files changed, 29 insertions(+), 28 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6f0b952755..46f0f669bb 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -73,6 +73,10 @@ #include "config/config_profile.h" #include "config/config_master.h" +#ifndef DEFAULT_RX_FEATURE +#define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM +#endif + #define BRUSHED_MOTORS_PWM_RATE 16000 #define BRUSHLESS_MOTORS_PWM_RATE 400 @@ -382,25 +386,20 @@ static void resetConf(void) memset(&masterConfig, 0, sizeof(master_t)); setProfile(0); - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.mixerMode = MIXER_QUADX; featureClearAll(); -#if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) || defined(DOGE) || defined(SINGULARITY) || defined(FURYF3) - featureSet(FEATURE_RX_PPM); + featureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES); +#ifdef DEFAULT_FEATURES + featureSet(DEFAULT_FEATURES); #endif -//#if defined(SPRACINGF3MINI) -// featureSet(FEATURE_DISPLAY); -//#endif - #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. featureSet(FEATURE_VBAT); #endif - featureSet(FEATURE_FAILSAFE); - featureSet(FEATURE_SUPEREXPO_RATES); + masterConfig.version = EEPROM_CONF_VERSION; + masterConfig.mixerMode = MIXER_QUADX; // global settings masterConfig.current_profile_index = 0; // default profile @@ -574,7 +573,6 @@ static void resetConf(void) #endif #ifdef SPRACINGF3 - featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = 1; #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware @@ -597,7 +595,6 @@ static void resetConf(void) #endif #if defined(FURYF3) - featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = 2; masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; @@ -609,16 +606,6 @@ static void resetConf(void) masterConfig.escAndServoConfig.maxthrottle = 1980; masterConfig.batteryConfig.vbatmaxcellvoltage = 45; masterConfig.batteryConfig.vbatmincellvoltage = 30; - - featureSet(FEATURE_VBAT); - featureSet(FEATURE_FAILSAFE); -#endif - -#ifdef SPRACINGF3EVO - featureSet(FEATURE_TRANSPONDER); - featureSet(FEATURE_RSSI_ADC); - featureSet(FEATURE_CURRENT_METER); - featureSet(FEATURE_TELEMETRY); #endif // alternative defaults settings for ALIENFLIGHTF1 and ALIENFLIGHTF3 targets @@ -699,14 +686,12 @@ static void resetConf(void) // alternative defaults settings for SINGULARITY target #if defined(SINGULARITY) - featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = 1; masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; masterConfig.batteryConfig.vbatscale = 77; - featureSet(FEATURE_RX_SERIAL); masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; #endif @@ -823,7 +808,7 @@ void activateConfig(void) void validateAndFixConfig(void) { if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) { - featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM + featureSet(FEATURE_RX_PARALLEL_PWM); } if (featureConfigured(FEATURE_RX_PPM)) { diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 79365c62e5..144ba1f7dd 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -157,6 +157,7 @@ //#define DISPLAY #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index ca94199b8f..3f6b753b52 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -128,6 +128,7 @@ #define SERIAL_RX #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SONAR //#define GPS diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index cf32393b42..cd9cb19123 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -60,6 +60,7 @@ #define SERIAL_RX //#define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 92edd4fea2..d95a9cc48a 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -192,5 +192,6 @@ #define SERIAL_RX #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 008557d335..fa18385a83 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -190,6 +190,7 @@ #define SERIAL_RX #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // Use UART3 for speksat diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 0fbe17c57b..99d7340d79 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -212,6 +212,8 @@ #define USE_SERVOS #define USE_CLI #define SONAR +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define SPEKTRUM_BIND // USART3, diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index a3b370df7a..2d04a2fee6 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -154,6 +154,7 @@ #define TELEMETRY #define SERIAL_RX +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 9f168587fd..23e54ab3b8 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -121,6 +121,7 @@ #define TELEMETRY #define BLACKBOX #define SERIAL_RX +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM //#define GPS //#define GTUNE #define DISPLAY diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 54d1824444..fbc58bae34 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -127,6 +127,8 @@ #define GPS #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) #define SPEKTRUM_BIND // USART2, PA15 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 8ff56c86ca..5e69ef1089 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -126,6 +126,7 @@ #define TELEMETRY #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define LED_STRIP #if 1 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index c5a0e5265d..0e0ede72ce 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -158,6 +158,9 @@ #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX + #define SPEKTRUM_BIND // USART3, #define BIND_PORT GPIOB diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 4586dbc2d1..73ea225dca 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -206,8 +206,6 @@ #define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 #define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM - #define GPS #define BLACKBOX #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -217,6 +215,9 @@ #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) + #define SPEKTRUM_BIND // USART3, #define BIND_PORT GPIOB diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 5459d4f9a2..6151d35492 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -212,10 +212,10 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define DISPLAY #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define BUTTONS #define BUTTON_A_PORT GPIOB From a38a2da74f271c649cbe168304f02e9c486b2662 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Wed, 8 Jun 2016 01:18:33 -0700 Subject: [PATCH 12/38] Delay osd initialization, remove floating point pids from osd --- src/main/drivers/max7456.c | 1 + src/main/io/osd.c | 58 ++++--------------------------- src/main/io/serial_cli.c | 2 +- src/main/target/SIRINFPV/target.h | 2 +- 4 files changed, 9 insertions(+), 54 deletions(-) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 07c71a69e3..1238d92cc9 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -57,6 +57,7 @@ void max7456_init(uint8_t system) { //Minimum spi clock period for max7456 is 100ns (10Mhz) spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); + delay(1000); // force soft reset on Max7456 ENABLE_MAX7456; max7456_send(VM0_reg, max7456_reset); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 52d0a8a65a..af63f76380 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -179,22 +179,13 @@ void print_vtx_freq(uint16_t pos, uint8_t col) { void print_pid(uint16_t pos, uint8_t col, int pid_term) { switch(col) { case 0: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) - sprintf(string_buffer, "%d", (int)(currentProfile->pidProfile.P_f[pid_term] * 10.0)); - else - sprintf(string_buffer, "%d", currentProfile->pidProfile.P8[pid_term]); + sprintf(string_buffer, "%d", currentProfile->pidProfile.P8[pid_term]); break; case 1: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) - sprintf(string_buffer, "%d", (int)(currentProfile->pidProfile.I_f[pid_term] * 100.0)); - else - sprintf(string_buffer, "%d", currentProfile->pidProfile.I8[pid_term]); + sprintf(string_buffer, "%d", currentProfile->pidProfile.I8[pid_term]); break; case 2: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) - sprintf(string_buffer, "%d", (int)(currentProfile->pidProfile.D_f[pid_term] * 1000.0)); - else - sprintf(string_buffer, "%d", currentProfile->pidProfile.D8[pid_term]); + sprintf(string_buffer, "%d", currentProfile->pidProfile.D8[pid_term]); break; default: return; @@ -274,53 +265,16 @@ void update_int_pid(bool inc, uint8_t col, int pid_term) { } } -void update_float_pid(bool inc, uint8_t col, int pid_term) { - void* ptr; - float diff; - - switch(col) { - case 0: - ptr = ¤tProfile->pidProfile.P_f[pid_term]; - diff = 0.1; - break; - case 1: - ptr = ¤tProfile->pidProfile.I_f[pid_term]; - diff = 0.01; - break; - case 2: - ptr = ¤tProfile->pidProfile.D_f[pid_term]; - diff = 0.001; - break; - } - - if (inc) { - if (*(float*)ptr < 100.0) - *(float*)ptr += diff; - } else { - if (*(float*)ptr > 0.0) - *(float*)ptr -= diff; - } -} - void update_roll_pid(bool inc, uint8_t col) { - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) - update_float_pid(inc, col, ROLL); - else - update_int_pid(inc, col, ROLL); + update_int_pid(inc, col, ROLL); } void update_pitch_pid(bool inc, uint8_t col) { - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) - update_float_pid(inc, col, PITCH); - else - update_int_pid(inc, col, PITCH); + update_int_pid(inc, col, PITCH); } void update_yaw_pid(bool inc, uint8_t col) { - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) - update_float_pid(inc, col, YAW); - else - update_int_pid(inc, col, YAW); + update_int_pid(inc, col, YAW); } void update_roll_rate(bool inc, uint8_t col) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f75b7d5046..4e6721b679 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -499,7 +499,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, - { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) } + { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, #endif diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 2f91688911..8d2b85977b 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -194,7 +194,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define TELEMETRY #define SERIAL_RX -#define GTUNE +//#define GTUNE #define USE_CLI #define OSD #define LED0 From dd06a5d29ba5c5b4e77378ec8191d1bdc6eb20de Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 13 Jun 2016 09:36:39 +1200 Subject: [PATCH 13/38] Added targets for listing and building all valid target platforms to Makefile. --- Makefile | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/Makefile b/Makefile index fe5283d572..20d6bd130f 100644 --- a/Makefile +++ b/Makefile @@ -968,6 +968,16 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.S ## all : default task; compile C code, build firmware all: binary +## all_targets : build all valid target platforms +all_targets: + for build_target in $(VALID_TARGETS); do \ + echo "Building $$build_target" && \ + make clean && \ + make -j TARGET=$$build_target || \ + break; \ + echo "Building $$build_target succeeded."; \ + done + ## clean : clean up all temporary / machine-generated files clean: rm -f $(CLEAN_ARTIFACTS) @@ -1017,6 +1027,10 @@ help: Makefile @echo "" @sed -n 's/^## //p' $< +## targets : print a list of all valid target platforms (for consumption by scripts) +targets: + @echo $(VALID_TARGETS) + ## test : run the cleanflight test suite test: cd src/test && $(MAKE) test || true From 23240b808184aa77af1f686effa9ff4bd764991a Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Mon, 13 Jun 2016 00:35:12 -0700 Subject: [PATCH 14/38] code cleanup --- fake_travis_build.sh | 3 ++- src/main/config/config.c | 2 +- src/main/drivers/max7456.c | 15 ++++----------- src/main/drivers/max7456.h | 32 +++++++++++++------------------ src/main/drivers/pwm_mapping.c | 14 ++++++++++++++ src/main/target/SIRINFPV/target.h | 11 ++++------- top_makefile | 2 ++ 7 files changed, 40 insertions(+), 39 deletions(-) diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 4babddd212..e081864925 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -18,7 +18,8 @@ targets=("PUBLISHMETA=True" \ "TARGET=ALIENFLIGHTF3" \ "TARGET=DOGE" \ "TARGET=SINGULARITY" \ - "TARGET=FURYF3") + "TARGET=FURYF3" \ + "TARGET=SIRINFPV") #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) diff --git a/src/main/config/config.c b/src/main/config/config.c index 165ee4ee3a..c1fe6f6ecb 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -403,7 +403,7 @@ static void resetConf(void) #endif #ifdef OSD - masterConfig.vtx_channel = 0; + masterConfig.vtx_channel = 19; masterConfig.osdProfile.system = 0; masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 1238d92cc9..9a9605fb2d 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -48,7 +48,6 @@ uint8_t max7456_send(uint8_t add, uint8_t data) { void max7456_init(uint8_t system) { - uint8_t max7456_reset=0x02; uint8_t max_screen_rows; uint8_t srdata = 0; uint16_t x; @@ -60,7 +59,7 @@ void max7456_init(uint8_t system) { delay(1000); // force soft reset on Max7456 ENABLE_MAX7456; - max7456_send(VM0_reg, max7456_reset); + max7456_send(VM0_REG, MAX7456_RESET); delay(100); srdata = max7456_send(0xA0, 0xFF); @@ -81,8 +80,6 @@ void max7456_init(uint8_t system) { break; } - max7456_reset |= video_signal_type; - if (video_signal_type) { //PAL max_screen_size = 480; max_screen_rows = 16; @@ -97,7 +94,7 @@ void max7456_init(uint8_t system) { } // make sure the Max7456 is enabled - max7456_send(VM0_reg, OSD_ENABLE | video_signal_type); + max7456_send(VM0_REG, OSD_ENABLE | video_signal_type); DISABLE_MAX7456; delay(100); @@ -159,10 +156,6 @@ void max7456_draw_screen_fast(void) { } } -#define NVM_RAM_SIZE 54 -#define WRITE_NVR 0xA0 -#define STATUS_REG_NVR_BUSY 0x20 - void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { uint8_t x; @@ -171,7 +164,7 @@ void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { ENABLE_MAX7456; // disable display - max7456_send(VM0_reg, video_signal_type); + max7456_send(VM0_REG, video_signal_type); max7456_send(MAX7456ADD_CMAH, char_address); // set start address high @@ -186,7 +179,7 @@ void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { // wait until bit 5 in the status register returns to 0 (12ms) while ((spiTransferByte(MAX7456_SPI_INSTANCE, MAX7456ADD_STAT) & STATUS_REG_NVR_BUSY) != 0); - max7456_send(VM0_reg, video_signal_type | 0x0C); + max7456_send(VM0_REG, video_signal_type | 0x0C); DISABLE_MAX7456; max7456_lock = 0; } diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 2c5f5a0dbb..86bd4dd1ee 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -17,11 +17,6 @@ #pragma once -#define DATAOUT 11 // MOSI -#define DATAIN 12 // MISO -#define SPICLOCK 13 // sck -#define VSYNC 2 // INT0 - #ifndef WHITEBRIGHTNESS #define WHITEBRIGHTNESS 0x01 #endif @@ -32,16 +27,16 @@ #define BWBRIGHTNESS ((BLACKBRIGHTNESS << 2) | WHITEBRIGHTNESS) //MAX7456 opcodes -#define DMM_reg 0x04 -#define DMAH_reg 0x05 -#define DMAL_reg 0x06 -#define DMDI_reg 0x07 -#define VM0_reg 0x00 -#define VM1_reg 0x01 +#define DMM_REG 0x04 +#define DMAH_REG 0x05 +#define DMAL_REG 0x06 +#define DMDI_REG 0x07 +#define VM0_REG 0x00 +#define VM1_REG 0x01 // video mode register 0 bits #define VIDEO_BUFFER_DISABLE 0x01 -//#define MAX7456_RESET 0x02 +#define MAX7456_RESET 0x02 #define VERTICAL_SYNC_NEXT_VSYNC 0x04 #define OSD_ENABLE 0x08 #define SYNC_MODE_AUTO 0x00 @@ -78,9 +73,9 @@ #define BACKGROUND_MODE_GRAY 0x40 //MAX7456 commands -#define CLEAR_display 0x04 -#define CLEAR_display_vert 0x06 -#define END_string 0xff +#define CLEAR_DISPLAY 0x04 +#define CLEAR_DISPLAY_VERT 0x06 +#define END_STRING 0xff #define MAX7456ADD_VM0 0x00 //0b0011100// 00 // 00 ,0011100 @@ -115,10 +110,9 @@ #define MAX7456ADD_OSDBL 0x6c #define MAX7456ADD_STAT 0xA0 -// Selectable by video mode -//uint8_t ENABLE_display; -//uint8_t ENABLE_display_vert; -//uint8_t DISABLE_display; +#define NVM_RAM_SIZE 54 +#define WRITE_NVR 0xA0 +#define STATUS_REG_NVR_BUSY 0x20 extern uint16_t max_screen_size; char screen[480]; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index e7b32413c3..ff9c05db69 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -750,6 +750,7 @@ static const uint16_t airPWM[] = { #if defined(SIRINFPV) static const uint16_t multiPPM[] = { + // No PPM PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -770,10 +771,23 @@ static const uint16_t multiPWM[] = { }; static const uint16_t airPPM[] = { + // No PPM + PWM1 | (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_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; static const uint16_t airPWM[] = { + PWM1 | (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_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; #endif diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 8d2b85977b..83ef28b928 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -189,19 +189,16 @@ #define VBAT_ADC_GPIO_PIN GPIO_Pin_0 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define USE_QUAD_MIXER_ONLY +//#define USE_QUAD_MIXER_ONLY #define BLACKBOX #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define TELEMETRY #define SERIAL_RX -//#define GTUNE #define USE_CLI #define OSD #define LED0 -#define USE_SERIAL_1WIRE +#define USE_SERVOS +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define S1W_TX_GPIO UART1_GPIO -#define S1W_TX_PIN UART1_TX_PIN -#define S1W_RX_GPIO UART1_GPIO -#define S1W_RX_PIN UART1_RX_PIN +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/top_makefile b/top_makefile index bbc1f43f14..5f57129025 100644 --- a/top_makefile +++ b/top_makefile @@ -16,6 +16,7 @@ ALL_TARGETS += afromini ALL_TARGETS += doge ALL_TARGETS += singularity ALL_TARGETS += furyf3 +ALL_TARGETS += sirinfpv CLEAN_TARGETS := $(addprefix clean_, $(ALL_TARGETS)) @@ -37,6 +38,7 @@ clean_afromini afromini : opts := TARGET=AFROMINI clean_doge doge : opts := TARGET=DOGE clean_singularity singularity : opts := TARGET=SINGULARITY clean_furyf3 furyf3 : opts := TARGET=FURYF3 +clean_sirinfpv sirinfpv: opts := TARGET=SIRINFPV .PHONY: all clean From c146ddc697598079940182df205cf6b2b300c8d0 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Mon, 13 Jun 2016 10:50:33 -0700 Subject: [PATCH 15/38] Fix merging issues --- Makefile | 24 -- fake_travis_build.sh | 1 - src/main/config/config.c | 8 +- src/main/target/FURYF3/system_stm32f30x.c | 371 ---------------------- src/main/target/FURYF3/system_stm32f30x.h | 76 ----- src/main/target/SPRACINGF3/target.h | 3 - top_makefile | 2 - 7 files changed, 1 insertion(+), 484 deletions(-) delete mode 100644 src/main/target/FURYF3/system_stm32f30x.c delete mode 100644 src/main/target/FURYF3/system_stm32f30x.h diff --git a/Makefile b/Makefile index 6f7528a106..96615f8055 100644 --- a/Makefile +++ b/Makefile @@ -982,30 +982,6 @@ FURYF4_SRC = \ $(COMMON_SRC) \ $(VCPF4_SRC) -FURYF3_SRC = \ - $(STM32F30x_COMMON_SRC) \ - drivers/accgyro_mpu.c \ - drivers/barometer_ms5611.c \ - drivers/barometer_bmp280.c \ - drivers/display_ug2864hsweg01.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_usb_vcp.c \ - drivers/sdcard.c \ - drivers/sdcard_standard.c \ - drivers/flash_m25p16.c \ - drivers/sonar_hcsr04.c \ - drivers/serial_softserial.c \ - io/asyncfatfs/asyncfatfs.c \ - io/asyncfatfs/fat_standard.c \ - io/flashfs.c \ - $(HIGHEND_SRC) \ - $(COMMON_SRC) \ - $(VCP_SRC) - # Search path and source files for the ST stdperiph library VPATH := $(VPATH):$(STDPERIPH_DIR)/src diff --git a/fake_travis_build.sh b/fake_travis_build.sh index e081864925..339c4008d0 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -18,7 +18,6 @@ targets=("PUBLISHMETA=True" \ "TARGET=ALIENFLIGHTF3" \ "TARGET=DOGE" \ "TARGET=SINGULARITY" \ - "TARGET=FURYF3" \ "TARGET=SIRINFPV") #fake a travis build environment diff --git a/src/main/config/config.c b/src/main/config/config.c index 9c7f873130..ad8cf1f43b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -652,12 +652,6 @@ static void resetConf(void) masterConfig.blackbox_rate_denom = 1; #endif -#if defined(FURYF3) - masterConfig.blackbox_device = 2; - masterConfig.blackbox_rate_num = 1; - masterConfig.blackbox_rate_denom = 1; -#endif - // alternative defaults settings for COLIBRI RACE targets #if defined(COLIBRI_RACE) masterConfig.escAndServoConfig.minthrottle = 1025; @@ -829,7 +823,7 @@ void activateConfig(void) void validateAndFixConfig(void) { if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) { - featureSet(FEATURE_RX_PARALLEL_PWM); + featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM } if (featureConfigured(FEATURE_RX_PPM)) { diff --git a/src/main/target/FURYF3/system_stm32f30x.c b/src/main/target/FURYF3/system_stm32f30x.c deleted file mode 100644 index de8a873131..0000000000 --- a/src/main/target/FURYF3/system_stm32f30x.c +++ /dev/null @@ -1,371 +0,0 @@ -/** - ****************************************************************************** - * @file system_stm32f30x.c - * @author MCD Application Team - * @version V1.1.1 - * @date 28-March-2014 - * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. - * This file contains the system clock configuration for STM32F30x devices, - * and is generated by the clock configuration tool - * stm32f30x_Clock_Configuration_V1.0.0.xls - * - * 1. This file provides two functions and one global variable to be called from - * user application: - * - SystemInit(): 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_stm32f30x.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 (8 MHz) is used as system clock source. - * Then SystemInit() function is called, in "startup_stm32f30x.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 8MHz, refer to "HSE_VALUE" define - * in "stm32f30x.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 STM32F30x device - *----------------------------------------------------------------------------- - * System Clock source | PLL (HSE) - *----------------------------------------------------------------------------- - * SYSCLK(Hz) | 72000000 - *----------------------------------------------------------------------------- - * HCLK(Hz) | 72000000 - *----------------------------------------------------------------------------- - * AHB Prescaler | 1 - *----------------------------------------------------------------------------- - * APB2 Prescaler | 2 - *----------------------------------------------------------------------------- - * APB1 Prescaler | 2 - *----------------------------------------------------------------------------- - * HSE Frequency(Hz) | 8000000 - *---------------------------------------------------------------------------- - * PLLMUL | 9 - *----------------------------------------------------------------------------- - * PREDIV | 1 - *----------------------------------------------------------------------------- - * USB Clock | ENABLE - *----------------------------------------------------------------------------- - * Flash Latency(WS) | 2 - *----------------------------------------------------------------------------- - * Prefetch Buffer | ON - *----------------------------------------------------------------------------- - *============================================================================= - ****************************************************************************** - * @attention - * - *

© COPYRIGHT 2014 STMicroelectronics

- * - * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); - * You may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.st.com/software_license_agreement_liberty_v2 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ****************************************************************************** - */ -/** @addtogroup CMSIS - * @{ - */ - -/** @addtogroup stm32f30x_system - * @{ - */ - -/** @addtogroup STM32F30x_System_Private_Includes - * @{ - */ - -#include "stm32f30x.h" - -uint32_t hse_value = HSE_VALUE; - -/** - * @} - */ - -/* Private typedef -----------------------------------------------------------*/ - -/** @addtogroup STM32F30x_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 0x0 /*!< Vector Table base offset field. - This value must be a multiple of 0x200. */ -/** - * @} - */ - -/* Private macro -------------------------------------------------------------*/ - -/** @addtogroup STM32F30x_System_Private_Variables - * @{ - */ - - uint32_t SystemCoreClock = 72000000; - - __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; - -/** - * @} - */ - -/** @addtogroup STM32F30x_System_Private_FunctionPrototypes - * @{ - */ - -void SetSysClock(void); - -/** - * @} - */ - -/** @addtogroup STM32F30x_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(void) -{ - /* FPU settings ------------------------------------------------------------*/ - #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) - SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ - #endif - - /* Reset the RCC clock configuration to the default reset state ------------*/ - /* Set HSION bit */ - RCC->CR |= (uint32_t)0x00000001; - - /* Reset CFGR register */ - RCC->CFGR &= 0xF87FC00C; - - /* Reset HSEON, CSSON and PLLON bits */ - RCC->CR &= (uint32_t)0xFEF6FFFF; - - /* Reset HSEBYP bit */ - RCC->CR &= (uint32_t)0xFFFBFFFF; - - /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ - RCC->CFGR &= (uint32_t)0xFF80FFFF; - - /* Reset PREDIV1[3:0] bits */ - RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; - - /* Reset USARTSW[1:0], I2CSW and TIMs bits */ - RCC->CFGR3 &= (uint32_t)0xFF00FCCC; - - /* Disable all interrupts */ - RCC->CIR = 0x00000000; - - /* Configure the System clock source, PLL Multiplier and Divider factors, - AHB/APBx prescalers and Flash settings ----------------------------------*/ - //SetSysClock(); // called from main() - -#ifdef VECT_TAB_SRAM - SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ -#else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ -#endif -} - -/** - * @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 stm32f30x.h file (default value - * 8 MHz) but the real value may vary depending on the variations - * in voltage and temperature. - * - * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value - * 8 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, pllmull = 0, pllsource = 0, prediv1factor = 0; - - /* Get SYSCLK source -------------------------------------------------------*/ - tmp = RCC->CFGR & RCC_CFGR_SWS; - - switch (tmp) - { - case 0x00: /* HSI used as system clock */ - SystemCoreClock = HSI_VALUE; - break; - case 0x04: /* HSE used as system clock */ - SystemCoreClock = HSE_VALUE; - break; - case 0x08: /* PLL used as system clock */ - /* Get PLL clock source and multiplication factor ----------------------*/ - pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; - pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; - pllmull = ( pllmull >> 18) + 2; - - if (pllsource == 0x00) - { - /* HSI oscillator clock divided by 2 selected as PLL clock entry */ - SystemCoreClock = (HSI_VALUE >> 1) * pllmull; - } - else - { - prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; - /* HSE oscillator clock selected as PREDIV1 clock entry */ - SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; - } - break; - default: /* HSI used as system clock */ - SystemCoreClock = HSI_VALUE; - break; - } - /* Compute HCLK clock frequency ----------------*/ - /* Get HCLK prescaler */ - tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; - /* HCLK clock frequency */ - SystemCoreClock >>= tmp; -} - -/** - * @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(void) -{ - __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - -/******************************************************************************/ -/* PLL (clocked by HSE) used as System clock source */ -/******************************************************************************/ - - /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ - /* 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 Prefetch Buffer and set Flash Latency */ - FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; - - /* HCLK = SYSCLK / 1 */ - RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; - - /* PCLK2 = HCLK / 1 */ - RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; - - /* PCLK1 = HCLK / 2 */ - RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; - - /* PLL configuration */ - RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); - RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); - - /* Enable PLL */ - RCC->CR |= RCC_CR_PLLON; - - /* Wait till PLL is ready */ - while((RCC->CR & RCC_CR_PLLRDY) == 0) - { - } - - /* Select PLL as system clock source */ - RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); - RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; - - /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) - { - } - } - 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 */ - } -} - -/** - * @} - */ - -/** - * @} - */ - -/** - * @} - */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file diff --git a/src/main/target/FURYF3/system_stm32f30x.h b/src/main/target/FURYF3/system_stm32f30x.h deleted file mode 100644 index 57e5e05d74..0000000000 --- a/src/main/target/FURYF3/system_stm32f30x.h +++ /dev/null @@ -1,76 +0,0 @@ -/** - ****************************************************************************** - * @file system_stm32f30x.h - * @author MCD Application Team - * @version V1.1.1 - * @date 28-March-2014 - * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. - ****************************************************************************** - * @attention - * - *

© COPYRIGHT 2014 STMicroelectronics

- * - * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); - * You may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.st.com/software_license_agreement_liberty_v2 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ****************************************************************************** - */ - -/** @addtogroup CMSIS - * @{ - */ - -/** @addtogroup stm32f30x_system - * @{ - */ - -/** - * @brief Define to prevent recursive inclusion - */ -#ifndef __SYSTEM_STM32F30X_H -#define __SYSTEM_STM32F30X_H - -#ifdef __cplusplus - extern "C" { -#endif - -/* Exported types ------------------------------------------------------------*/ -extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ -/* Exported constants --------------------------------------------------------*/ -/* Exported macro ------------------------------------------------------------*/ -/* Exported functions ------------------------------------------------------- */ - -/** @addtogroup STM32F30x_System_Exported_Functions - * @{ - */ - -extern void SystemInit(void); -extern void SystemCoreClockUpdate(void); - -/** - * @} - */ - -#ifdef __cplusplus -} -#endif - -#endif /*__SYSTEM_STM32F30X_H */ - -/** - * @} - */ - -/** - * @} - */ -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index d5894eb782..c041b54d92 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -149,9 +149,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES FEATURE_BLACKBOX -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX - #define SPEKTRUM_BIND // USART3, #define BIND_PIN PB11 diff --git a/top_makefile b/top_makefile index 5f57129025..1eeee5cfbb 100644 --- a/top_makefile +++ b/top_makefile @@ -15,7 +15,6 @@ ALL_TARGETS += ircfusionf3 ALL_TARGETS += afromini ALL_TARGETS += doge ALL_TARGETS += singularity -ALL_TARGETS += furyf3 ALL_TARGETS += sirinfpv CLEAN_TARGETS := $(addprefix clean_, $(ALL_TARGETS)) @@ -37,7 +36,6 @@ clean_ircfusionf3 ircfusionf3 : opts := TARGET=IRCFUSIONF3 clean_afromini afromini : opts := TARGET=AFROMINI clean_doge doge : opts := TARGET=DOGE clean_singularity singularity : opts := TARGET=SINGULARITY -clean_furyf3 furyf3 : opts := TARGET=FURYF3 clean_sirinfpv sirinfpv: opts := TARGET=SIRINFPV From 0fd8774d5028cb196a4f8445e0c0d0dcb266756f Mon Sep 17 00:00:00 2001 From: nathan Date: Mon, 13 Jun 2016 22:45:53 -0700 Subject: [PATCH 16/38] takin' a look --- Makefile | 5 + fake_travis_build.sh | 3 +- q | 59 --- src/main/blackbox/blackbox.c | 1 + src/main/blackbox/blackbox_io.c | 1 + src/main/config/config.c | 27 ++ src/main/config/config.h | 1 + src/main/config/config_master.h | 5 + src/main/drivers/max7456.c | 188 ++++++++ src/main/drivers/max7456.h | 125 +++++ src/main/drivers/rtc6705.c | 151 ++++++ src/main/drivers/rtc6705.h | 25 + src/main/flight/mixer.h | 2 + src/main/io/osd.c | 710 +++++++++++++++++++++++++++++ src/main/io/osd.h | 61 +++ src/main/io/serial_cli.c | 29 ++ src/main/io/serial_msp.c | 33 +- src/main/io/serial_msp.h | 9 + src/main/main.c | 11 + src/main/mw.c | 11 + src/main/scheduler.h | 4 +- src/main/scheduler_tasks.c | 12 +- src/main/target/SIRINFPV/target.c | 62 +++ src/main/target/SIRINFPV/target.h | 182 ++++++++ src/main/target/SIRINFPV/target.mk | 16 + src/main/telemetry/ltm.c | 1 + src/main/telemetry/smartport.c | 1 + top_makefile | 2 + 28 files changed, 1674 insertions(+), 63 deletions(-) delete mode 100644 q create mode 100644 src/main/drivers/max7456.c create mode 100644 src/main/drivers/max7456.h create mode 100644 src/main/drivers/rtc6705.c create mode 100644 src/main/drivers/rtc6705.h create mode 100644 src/main/io/osd.c create mode 100644 src/main/io/osd.h create mode 100644 src/main/target/SIRINFPV/target.c create mode 100644 src/main/target/SIRINFPV/target.h create mode 100644 src/main/target/SIRINFPV/target.mk diff --git a/Makefile b/Makefile index 12352a0ba7..40224afded 100644 --- a/Makefile +++ b/Makefile @@ -536,6 +536,11 @@ endif ifneq ($(filter VCP,$(FEATURES)),) TARGET_SRC += $(VCP_SRC) endif + +ifneq ($(filter MAX_OSD, $(FEATURES)),) +TARGET_SRC += $(SRC_DIR)/drivers/max7456.c \ + $(SRC_DIR)/io/osd.c +endif # end target specific make file checks diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 7749d64354..339c4008d0 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -17,7 +17,8 @@ targets=("PUBLISHMETA=True" \ "TARGET=ALIENFLIGHTF1" \ "TARGET=ALIENFLIGHTF3" \ "TARGET=DOGE" \ - "TARGET=SINGULARITY") + "TARGET=SINGULARITY" \ + "TARGET=SIRINFPV") #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) diff --git a/q b/q deleted file mode 100644 index 04586033b6..0000000000 --- a/q +++ /dev/null @@ -1,59 +0,0 @@ -diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c -index fdde2cf..53464ef 100644 ---- a/src/main/io/rc_controls.c -+++ b/src/main/io/rc_controls.c -@@ -67,6 +67,7 @@ static pidProfile_t *pidProfile; - static bool isUsingSticksToArm = true; -  - int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -+int16_t rcCommandSmooth[4]; // Smoothed RcCommand -  - uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e -  -diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h -index eaec277..dd8afaf 100644 ---- a/src/main/io/rc_controls.h -+++ b/src/main/io/rc_controls.h -@@ -147,6 +147,7 @@ typedef struct controlRateConfig_s { - } controlRateConfig_t; -  - extern int16_t rcCommand[4]; -+extern int16_t rcCommandSmooth[4]; // Smoothed RcCommand -  - typedef struct rcControlsConfig_s { - uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. -diff --git a/src/main/mw.c b/src/main/mw.c -index 125674c..5da79cf 100644 ---- a/src/main/mw.c -+++ b/src/main/mw.c -@@ -181,7 +181,7 @@ void filterRc(void) -  - if (isRXDataNew) { - for (int channel=0; channel < 4; channel++) { -- deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); -+ deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcCommand[channel]; - } -  -@@ -194,7 +194,7 @@ void filterRc(void) - // Interpolate steps of rcCommand - if (factor > 0) { - for (int channel=0; channel < 4; channel++) { -- rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; -+ rcCommandSmooth[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; - } - } else { - factor = 0; -@@ -649,8 +649,11 @@ void subTaskMainSubprocesses(void) { -  - const uint32_t startTime = micros(); -  -+ filterRc(); -+ - if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) { -- filterRc(); -+ int axis; -+ for (axis = 0; axis <= 4; axis++) rcCommand[axis] = rcCommandSmooth[axis]; - } -  - // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 80df3ba141..4b3aa51cc8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -60,6 +60,7 @@ #include "io/serial_cli.h" #include "io/serial_msp.h" #include "io/statusindicator.h" +#include "io/osd.h" #include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 0eeead3156..82c105eb4a 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -36,6 +36,7 @@ #include "io/escservo.h" #include "rx/rx.h" #include "io/rc_controls.h" +#include "io/osd.h" #include "io/vtx.h" #include "io/gimbal.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index 7c2fc82ccd..ad8cf1f43b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -55,6 +55,7 @@ #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" @@ -418,6 +419,32 @@ static void resetConf(void) featureSet(DEFAULT_FEATURES); #endif +#ifdef SIRINFPV + featureSet(FEATURE_OSD); + featureSet(FEATURE_RX_SERIAL); + masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; + //masterConfig.batteryConfig.vbatscale = 20; + masterConfig.mag_hardware = MAG_NONE; // disabled by default + masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS; + masterConfig.blackbox_device = 1; + masterConfig.blackbox_rate_num = 1; + masterConfig.blackbox_rate_denom = 1; +#endif + +#ifdef OSD + masterConfig.vtx_channel = 19; + masterConfig.osdProfile.system = 0; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; + masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; + masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; + masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; + masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; + masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; + masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; + masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; +#endif + #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. diff --git a/src/main/config/config.h b/src/main/config/config.h index 7bd7a2e052..219cc31bea 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -46,6 +46,7 @@ typedef enum { FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, + FEATURE_OSD = 1 << 24, } features_e; void handleOneshotFeatureChangeOnRestart(void); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 01bf4e8f3f..6d1c943179 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -125,6 +125,11 @@ typedef struct master_t { uint8_t transponderData[6]; #endif +#ifdef OSD + uint8_t vtx_channel; + osd_profile osdProfile; +#endif + profile_t profile[MAX_PROFILE_COUNT]; uint8_t current_profile_index; diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c new file mode 100644 index 0000000000..9a9605fb2d --- /dev/null +++ b/src/main/drivers/max7456.c @@ -0,0 +1,188 @@ +/* + * 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 + +#include "common/printf.h" +#include "platform.h" +#include "version.h" + +#ifdef USE_MAX7456 + +#include "drivers/bus_spi.h" +#include "drivers/system.h" + +#include "max7456.h" + +#define DISABLE_MAX7456 GPIO_SetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) +#define ENABLE_MAX7456 GPIO_ResetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) + + +uint16_t max_screen_size; +uint8_t video_signal_type = 0; +uint8_t max7456_lock = 0; +char screen[480]; + + +uint8_t max7456_send(uint8_t add, uint8_t data) { + spiTransferByte(MAX7456_SPI_INSTANCE, add); + return spiTransferByte(MAX7456_SPI_INSTANCE, data); +} + + +void max7456_init(uint8_t system) { + uint8_t max_screen_rows; + uint8_t srdata = 0; + uint16_t x; + char buf[30]; + + //Minimum spi clock period for max7456 is 100ns (10Mhz) + spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); + + delay(1000); + // force soft reset on Max7456 + ENABLE_MAX7456; + max7456_send(VM0_REG, MAX7456_RESET); + delay(100); + + srdata = max7456_send(0xA0, 0xFF); + if ((0x01 & srdata) == 0x01){ //PAL + video_signal_type = VIDEO_MODE_PAL; + } + else if((0x02 & srdata) == 0x02){ //NTSC + video_signal_type = VIDEO_MODE_NTSC; + } + + // Override detected type: 0-AUTO, 1-PAL, 2-NTSC + switch(system) { + case 1: + video_signal_type = VIDEO_MODE_PAL; + break; + case 2: + video_signal_type = VIDEO_MODE_NTSC; + break; + } + + if (video_signal_type) { //PAL + max_screen_size = 480; + max_screen_rows = 16; + } else { // NTSC + max_screen_size = 390; + max_screen_rows = 13; + } + + // set all rows to same charactor black/white level + for(x = 0; x < max_screen_rows; x++) { + max7456_send(MAX7456ADD_RB0 + x, BWBRIGHTNESS); + } + + // make sure the Max7456 is enabled + max7456_send(VM0_REG, OSD_ENABLE | video_signal_type); + + DISABLE_MAX7456; + delay(100); + + x = 160; + for (int i = 1; i < 5; i++) { + for (int j = 3; j < 27; j++) + screen[i * 30 + j] = (char)x++; + } + tfp_sprintf(buf, "BF VERSION: %s", FC_VERSION_STRING); + max7456_write_string(buf, 5*30+5); + max7456_write_string("MENU: THRT MID", 6*30+7); + max7456_write_string("YAW RIGHT", 7*30+13); + max7456_write_string("PITCH UP", 8*30+13); + max7456_draw_screen(); +} + +// Copy string from ram into screen buffer +void max7456_write_string(const char *string, int16_t address) { + char *dest; + + if (address >= 0) + dest = screen + address; + else + dest = screen + (max_screen_size + address); + + while(*string) + *dest++ = *string++; +} + +void max7456_draw_screen(void) { + uint16_t xx; + if (!max7456_lock) { + ENABLE_MAX7456; + for (xx = 0; xx < max_screen_size; ++xx) { + max7456_send(MAX7456ADD_DMAH, xx>>8); + max7456_send(MAX7456ADD_DMAL, xx); + max7456_send(MAX7456ADD_DMDI, screen[xx]); + screen[xx] = ' '; + } + DISABLE_MAX7456; + } +} + +void max7456_draw_screen_fast(void) { + uint16_t xx; + if (!max7456_lock) { + ENABLE_MAX7456; + max7456_send(MAX7456ADD_DMAH, 0); + max7456_send(MAX7456ADD_DMAL, 0); + max7456_send(MAX7456ADD_DMM, 1); + for (xx = 0; xx < max_screen_size; ++xx) { + max7456_send(MAX7456ADD_DMDI, screen[xx]); + screen[xx] = ' '; + } + max7456_send(MAX7456ADD_DMDI, 0xFF); + max7456_send(MAX7456ADD_DMM, 0); + DISABLE_MAX7456; + } +} + + +void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { + uint8_t x; + + max7456_lock = 1; + ENABLE_MAX7456; + + // disable display + max7456_send(VM0_REG, video_signal_type); + + max7456_send(MAX7456ADD_CMAH, char_address); // set start address high + + for(x = 0; x < 54; x++) { + max7456_send(MAX7456ADD_CMAL, x); //set start address low + max7456_send(MAX7456ADD_CMDI, font_data[x]); + } + + // transfer 54 bytes from shadow ram to NVM + max7456_send(MAX7456ADD_CMM, WRITE_NVR); + + // wait until bit 5 in the status register returns to 0 (12ms) + while ((spiTransferByte(MAX7456_SPI_INSTANCE, MAX7456ADD_STAT) & STATUS_REG_NVR_BUSY) != 0); + + max7456_send(VM0_REG, video_signal_type | 0x0C); + DISABLE_MAX7456; + max7456_lock = 0; +} + + +#endif diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h new file mode 100644 index 0000000000..86bd4dd1ee --- /dev/null +++ b/src/main/drivers/max7456.h @@ -0,0 +1,125 @@ +/* + * 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 + +#ifndef WHITEBRIGHTNESS + #define WHITEBRIGHTNESS 0x01 +#endif +#ifndef BLACKBRIGHTNESS + #define BLACKBRIGHTNESS 0x00 +#endif + +#define BWBRIGHTNESS ((BLACKBRIGHTNESS << 2) | WHITEBRIGHTNESS) + +//MAX7456 opcodes +#define DMM_REG 0x04 +#define DMAH_REG 0x05 +#define DMAL_REG 0x06 +#define DMDI_REG 0x07 +#define VM0_REG 0x00 +#define VM1_REG 0x01 + +// video mode register 0 bits +#define VIDEO_BUFFER_DISABLE 0x01 +#define MAX7456_RESET 0x02 +#define VERTICAL_SYNC_NEXT_VSYNC 0x04 +#define OSD_ENABLE 0x08 +#define SYNC_MODE_AUTO 0x00 +#define SYNC_MODE_INTERNAL 0x30 +#define SYNC_MODE_EXTERNAL 0x20 +#define VIDEO_MODE_PAL 0x40 +#define VIDEO_MODE_NTSC 0x00 + +// video mode register 1 bits + + +// duty cycle is on_off +#define BLINK_DUTY_CYCLE_50_50 0x00 +#define BLINK_DUTY_CYCLE_33_66 0x01 +#define BLINK_DUTY_CYCLE_25_75 0x02 +#define BLINK_DUTY_CYCLE_75_25 0x03 + +// blinking time +#define BLINK_TIME_0 0x00 +#define BLINK_TIME_1 0x04 +#define BLINK_TIME_2 0x08 +#define BLINK_TIME_3 0x0C + +// background mode brightness (percent) +#define BACKGROUND_BRIGHTNESS_0 0x00 +#define BACKGROUND_BRIGHTNESS_7 0x01 +#define BACKGROUND_BRIGHTNESS_14 0x02 +#define BACKGROUND_BRIGHTNESS_21 0x03 +#define BACKGROUND_BRIGHTNESS_28 0x04 +#define BACKGROUND_BRIGHTNESS_35 0x05 +#define BACKGROUND_BRIGHTNESS_42 0x06 +#define BACKGROUND_BRIGHTNESS_49 0x07 + +#define BACKGROUND_MODE_GRAY 0x40 + +//MAX7456 commands +#define CLEAR_DISPLAY 0x04 +#define CLEAR_DISPLAY_VERT 0x06 +#define END_STRING 0xff + + +#define MAX7456ADD_VM0 0x00 //0b0011100// 00 // 00 ,0011100 +#define MAX7456ADD_VM1 0x01 +#define MAX7456ADD_HOS 0x02 +#define MAX7456ADD_VOS 0x03 +#define MAX7456ADD_DMM 0x04 +#define MAX7456ADD_DMAH 0x05 +#define MAX7456ADD_DMAL 0x06 +#define MAX7456ADD_DMDI 0x07 +#define MAX7456ADD_CMM 0x08 +#define MAX7456ADD_CMAH 0x09 +#define MAX7456ADD_CMAL 0x0a +#define MAX7456ADD_CMDI 0x0b +#define MAX7456ADD_OSDM 0x0c +#define MAX7456ADD_RB0 0x10 +#define MAX7456ADD_RB1 0x11 +#define MAX7456ADD_RB2 0x12 +#define MAX7456ADD_RB3 0x13 +#define MAX7456ADD_RB4 0x14 +#define MAX7456ADD_RB5 0x15 +#define MAX7456ADD_RB6 0x16 +#define MAX7456ADD_RB7 0x17 +#define MAX7456ADD_RB8 0x18 +#define MAX7456ADD_RB9 0x19 +#define MAX7456ADD_RB10 0x1a +#define MAX7456ADD_RB11 0x1b +#define MAX7456ADD_RB12 0x1c +#define MAX7456ADD_RB13 0x1d +#define MAX7456ADD_RB14 0x1e +#define MAX7456ADD_RB15 0x1f +#define MAX7456ADD_OSDBL 0x6c +#define MAX7456ADD_STAT 0xA0 + +#define NVM_RAM_SIZE 54 +#define WRITE_NVR 0xA0 +#define STATUS_REG_NVR_BUSY 0x20 + +extern uint16_t max_screen_size; +char screen[480]; + + +void max7456_init(uint8_t system); +void max7456_draw_screen(void); +void max7456_draw_screen_fast(void); +void max7456_write_string(const char *string, int16_t address); +void max7456_write_nvm(uint8_t char_address, uint8_t *font_data); diff --git a/src/main/drivers/rtc6705.c b/src/main/drivers/rtc6705.c new file mode 100644 index 0000000000..319430f526 --- /dev/null +++ b/src/main/drivers/rtc6705.c @@ -0,0 +1,151 @@ +/* + * 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" + +#ifdef USE_RTC6705 + +#include "drivers/bus_spi.h" +#include "drivers/system.h" +#include "drivers/gpio.h" + +#include "rtc6705.h" + +#define RTC6705_SPICLK_ON GPIO_SetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) +#define RTC6705_SPICLK_OFF GPIO_ResetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) + +#define RTC6705_SPIDATA_ON GPIO_SetBits(RTC6705_SPIDATA_GPIO, RTC6705_SPIDATA_PIN) +#define RTC6705_SPIDATA_OFF GPIO_ResetBits(RTC6705_SPIDATA_GPIO, RTC6705_SPIDATA_PIN) + +#define RTC6705_SPILE_ON GPIO_SetBits(RTC6705_SPILE_GPIO, RTC6705_SPILE_PIN) +#define RTC6705_SPILE_OFF GPIO_ResetBits(RTC6705_SPILE_GPIO, RTC6705_SPILE_PIN) + +char *vtx_bands[] = { + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +uint16_t vtx_freq[] = +{ + 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, + 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, + 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, + 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, + 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, +}; + +uint16_t current_vtx_channel; + +void rtc6705_init(void) { + gpio_config_t gpio; + +#ifdef STM32F303 + #ifdef RTC6705_SPICLK_PERIPHERAL + RCC_AHBPeriphClockCmd(RTC6705_SPICLK_PERIPHERAL, ENABLE); + #endif + #ifdef RTC6705_SPILE_PERIPHERAL + RCC_AHBPeriphClockCmd(RTC6705_SPILE_PERIPHERAL, ENABLE); + #endif + #ifdef RTC6705_SPIDATA_PERIPHERAL + RCC_AHBPeriphClockCmd(RTC6705_SPIDATA_PERIPHERAL, ENABLE); + #endif +#endif +#ifdef STM32F10X + #ifdef RTC6705_SPICLK_PERIPHERAL + RCC_APB2PeriphClockCmd(RTC6705_SPICLK_PERIPHERAL, ENABLE); + #endif + #ifdef RTC6705_SPILE_PERIPHERAL + RCC_APB2PeriphClockCmd(RTC6705_SPILE_PERIPHERAL, ENABLE); + #endif + #ifdef RTC6705_SPIDATA_PERIPHERAL + RCC_APB2PeriphClockCmd(RTC6705_SPIDATA_PERIPHERAL, ENABLE); + #endif +#endif + + gpio.pin = RTC6705_SPICLK_PIN; + gpio.speed = Speed_50MHz; + gpio.mode = Mode_Out_PP; + gpioInit(RTC6705_SPICLK_GPIO, &gpio); + + gpio.pin = RTC6705_SPILE_PIN; + gpio.speed = Speed_50MHz; + gpio.mode = Mode_Out_PP; + gpioInit(RTC6705_SPILE_GPIO, &gpio); + + gpio.pin = RTC6705_SPIDATA_PIN; + gpio.speed = Speed_50MHz; + gpio.mode = Mode_Out_PP; + gpioInit(RTC6705_SPIDATA_GPIO, &gpio); +} + +void rtc6705_write_register(uint8_t addr, uint32_t data) { + uint8_t i; + + RTC6705_SPILE_OFF; + delay(1); + // send address + for (i=0; i<4; i++) { + if ((addr >> i) & 1) + RTC6705_SPIDATA_ON; + else + RTC6705_SPIDATA_OFF; + + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + } + // Write bit + + RTC6705_SPIDATA_ON; + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + for (i=0; i<20; i++) { + if ((data >> i) & 1) + RTC6705_SPIDATA_ON; + else + RTC6705_SPIDATA_OFF; + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + } + RTC6705_SPILE_ON; +} + + +void rtc6705_set_channel(uint16_t channel_freq) { + + uint32_t freq = (uint32_t)channel_freq * 1000; + uint32_t N, A; + + freq /= 40; + N = freq / 64; + A = freq % 64; + rtc6705_write_register(0, 400); + rtc6705_write_register(1, (N << 7) | A); +} +#endif diff --git a/src/main/drivers/rtc6705.h b/src/main/drivers/rtc6705.h new file mode 100644 index 0000000000..511d090878 --- /dev/null +++ b/src/main/drivers/rtc6705.h @@ -0,0 +1,25 @@ +/* + * 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 + +extern char* vtx_bands[]; +extern uint16_t vtx_freq[]; +extern uint16_t current_vtx_channel; + +void rtc6705_init(void); +void rtc6705_set_channel(uint16_t channel_freq); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8828ba08b3..22c1c008d1 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -187,6 +187,8 @@ void writeServos(void); void filterServos(void); #endif +bool motorLimitReached; + extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; bool motorLimitReached; diff --git a/src/main/io/osd.c b/src/main/io/osd.c new file mode 100644 index 0000000000..ebc489dd94 --- /dev/null +++ b/src/main/io/osd.c @@ -0,0 +1,710 @@ +/* + * 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 +#include + +#include "platform.h" +#include "scheduler.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/atomic.h" +#include "common/maths.h" +#include "common/typeconversion.h" + +#include "drivers/nvic.h" + +#include "drivers/sensor.h" +#include "drivers/system.h" +#include "drivers/gpio.h" +#include "drivers/light_led.h" +#include "drivers/sound_beeper.h" +#include "drivers/timer.h" +#include "drivers/serial.h" +#include "drivers/serial_softserial.h" +#include "drivers/serial_uart.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/pwm_mapping.h" +#include "drivers/pwm_rx.h" +#include "drivers/adc.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_bst.h" +#include "drivers/bus_spi.h" +#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" + +#include "rx/rx.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/flashfs.h" +#include "io/gps.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/gimbal.h" +#include "io/ledstrip.h" +#include "io/display.h" +#include "io/asyncfatfs/asyncfatfs.h" +#include "io/transponder_ir.h" +#include "io/osd.h" + +#include "sensors/sensors.h" +#include "sensors/sonar.h" +#include "sensors/barometer.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/initialisation.h" + +#include "telemetry/telemetry.h" +#include "blackbox/blackbox.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/failsafe.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#ifdef USE_HARDWARE_REVISION_DETECTION +#include "hardware_revision.h" +#endif + +#include "build_config.h" +#include "debug.h" + +#ifdef OSD + +#include "drivers/max7456.h" +#include "drivers/rtc6705.h" +#include "scheduler.h" +#include "common/printf.h" + +#define MICROSECONDS_IN_A_SECOND (1000 * 1000) + +#define OSD_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5) +#define OSD_LINE_LENGTH 30 + +#define STICKMIN 10 +#define STICKMAX 90 + + +static uint32_t next_osd_update_at = 0; +static uint32_t armed_seconds = 0; +static uint32_t armed_at = 0; +static bool armed = false; + +static uint8_t current_page = 0; +static uint8_t sticks[] = {0,0,0,0}; +static char string_buffer[30]; +static uint8_t cursor_row = 255; +static uint8_t cursor_col = 0; +static bool in_menu = false; +static bool activating_menu = false; +extern uint16_t rssi; + + +#ifdef USE_RTC6705 +void update_vtx_band(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (current_vtx_channel < 32) + current_vtx_channel += 8; + } else { + if (current_vtx_channel > 7) + current_vtx_channel -= 8; + } +} + +void print_vtx_band(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%s", vtx_bands[current_vtx_channel / 8]); + max7456_write_string(string_buffer, pos); +} + +void update_vtx_channel(bool increase, uint8_t col) { + (void)col; + if (increase) { + if ((current_vtx_channel % 8) < 7) + current_vtx_channel++; + } else { + if ((current_vtx_channel % 8) > 0) + current_vtx_channel--; + } +} + +void print_vtx_channel(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%d", current_vtx_channel % 8 + 1); + max7456_write_string(string_buffer, pos); +} + +void print_vtx_freq(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%d M", vtx_freq[current_vtx_channel]); + max7456_write_string(string_buffer, pos); +} +#endif + +void print_pid(uint16_t pos, uint8_t col, int pid_term) { + switch(col) { + case 0: + sprintf(string_buffer, "%d", currentProfile->pidProfile.P8[pid_term]); + break; + case 1: + sprintf(string_buffer, "%d", currentProfile->pidProfile.I8[pid_term]); + break; + case 2: + sprintf(string_buffer, "%d", currentProfile->pidProfile.D8[pid_term]); + break; + default: + return; + } + max7456_write_string(string_buffer, pos); +} + +void print_roll_pid(uint16_t pos, uint8_t col) { + print_pid(pos, col, ROLL); +} + +void print_pitch_pid(uint16_t pos, uint8_t col) { + print_pid(pos, col, PITCH); +} + +void print_yaw_pid(uint16_t pos, uint8_t col) { + print_pid(pos, col, YAW); +} + +void print_roll_rate(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_ROLL]); + max7456_write_string(string_buffer, pos); + } +} + +void print_pitch_rate(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_PITCH]); + max7456_write_string(string_buffer, pos); + } +} + +void print_yaw_rate(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_YAW]); + max7456_write_string(string_buffer, pos); + } +} + +void print_tpa_rate(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->dynThrPID); + max7456_write_string(string_buffer, pos); + } +} + +void print_tpa_brkpt(uint16_t pos, uint8_t col) { + if (col == 0) { + sprintf(string_buffer, "%d", currentControlRateProfile->tpa_breakpoint); + max7456_write_string(string_buffer, pos); + } +} + +void update_int_pid(bool inc, uint8_t col, int pid_term) { + void* ptr; + switch(col) { + case 0: + ptr = ¤tProfile->pidProfile.P8[pid_term]; + break; + case 1: + ptr = ¤tProfile->pidProfile.I8[pid_term]; + break; + case 2: + ptr = ¤tProfile->pidProfile.D8[pid_term]; + break; + default: + return; + } + + if (inc) { + if (*(uint8_t*)ptr < 200) + *(uint8_t*)ptr += 1; + } else { + if (*(uint8_t*)ptr > 0) + *(uint8_t*)ptr -= 1; + } +} + +void update_roll_pid(bool inc, uint8_t col) { + update_int_pid(inc, col, ROLL); +} + +void update_pitch_pid(bool inc, uint8_t col) { + update_int_pid(inc, col, PITCH); +} + +void update_yaw_pid(bool inc, uint8_t col) { + update_int_pid(inc, col, YAW); +} + +void update_roll_rate(bool inc, uint8_t col) { + (void)col; + if (inc) { + if (currentControlRateProfile->rates[FD_ROLL] < CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX) + currentControlRateProfile->rates[FD_ROLL]++; + } else { + if (currentControlRateProfile->rates[FD_ROLL] > 0) + currentControlRateProfile->rates[FD_ROLL]--; + } +} + +void update_pitch_rate(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (currentControlRateProfile->rates[FD_PITCH] < CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX) + currentControlRateProfile->rates[FD_PITCH]++; + } else { + if (currentControlRateProfile->rates[FD_PITCH] > 0) + currentControlRateProfile->rates[FD_PITCH]--; + } +} + +void update_yaw_rate(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (currentControlRateProfile->rates[FD_YAW] < CONTROL_RATE_CONFIG_YAW_RATE_MAX) + currentControlRateProfile->rates[FD_YAW]++; + } else { + if (currentControlRateProfile->rates[FD_YAW] > 0) + currentControlRateProfile->rates[FD_YAW]--; + } +} + +void update_tpa_rate(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (currentControlRateProfile->dynThrPID < CONTROL_RATE_CONFIG_TPA_MAX) + currentControlRateProfile->dynThrPID++; + } else { + if (currentControlRateProfile->dynThrPID > 0) + currentControlRateProfile->dynThrPID--; + } +} + +void update_tpa_brkpt(bool increase, uint8_t col) { + (void)col; + if (increase) { + if (currentControlRateProfile->tpa_breakpoint < PWM_RANGE_MAX) + currentControlRateProfile->tpa_breakpoint++; + } else { + if (currentControlRateProfile->tpa_breakpoint > PWM_RANGE_MIN) + currentControlRateProfile->tpa_breakpoint--; + } +} + +void print_average_system_load(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%d", averageSystemLoadPercent); + max7456_write_string(string_buffer, pos); +} + +void print_batt_voltage(uint16_t pos, uint8_t col) { + (void)col; + sprintf(string_buffer, "%d.%1d", vbat / 10, vbat % 10); + max7456_write_string(string_buffer, pos); +} + +/* + TODO: add this to menu + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } }, + { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, + { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, + { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, + { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, + { "acro_plus_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } }, + { "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } }, +*/ + +page_t menu_pages[] = { + { + .title = "STATUS", + .cols_number = 1, + .rows_number = 2, + .cols = { + { + .title = NULL, + .x_pos = 15 + } + }, + .rows = { + { + .title = "AVG LOAD", + .update = NULL, + .print = print_average_system_load + }, + { + .title = "BATT", + .update = NULL, + .print = print_batt_voltage + }, + } + }, +#ifdef USE_RTC6705 + { + .title = "VTX SETTINGS", + .cols_number = 1, + .rows_number = 3, + .cols = { + { + .title = NULL, + .x_pos = 15 + } + }, + .rows = { + { + .title = "BAND", + .update = update_vtx_band, + .print = print_vtx_band + }, + { + .title = "CHANNEL", + .update = update_vtx_channel, + .print = print_vtx_channel + }, + { + .title = "FREQUENCY", + .update = NULL, + .print = print_vtx_freq + } + } + }, +#endif + { + .title = "PID SETTINGS", + .cols_number = 3, + .rows_number = 8, + .cols = { + { + .title = "P", + .x_pos = 13 + }, + { + .title = "I", + .x_pos = 19 + }, + { + .title = "D", + .x_pos = 25 + } + }, + .rows = { + { + .title = "ROLL", + .update = update_roll_pid, + .print = print_roll_pid + }, + { + .title = "PITCH", + .update = update_pitch_pid, + .print = print_pitch_pid + }, + { + .title = "YAW", + .update = update_yaw_pid, + .print = print_yaw_pid + }, + { + .title = "ROLL RATE", + .update = update_roll_rate, + .print = print_roll_rate + }, + { + .title = "PITCH RATE", + .update = update_pitch_rate, + .print = print_pitch_rate + }, + { + .title = "YAW RATE", + .update = update_yaw_rate, + .print = print_yaw_rate + }, + { + .title = "TPA RATE", + .update = update_tpa_rate, + .print = print_tpa_rate + }, + { + .title = "TPA BRKPT", + .update = update_tpa_brkpt, + .print = print_tpa_brkpt + }, + } + }, +}; + +void show_menu(void) { + uint8_t line = 1; + uint16_t pos; + col_t *col; + row_t *row; + int16_t cursor_x, cursor_y; + + if (activating_menu) { + if (sticks[YAW] < 60 && sticks[YAW] > 40 && sticks[PITCH] > 40 && sticks[PITCH] < 60 && sticks[ROLL] > 40 && sticks[ROLL] < 60) + activating_menu = false; + else + return; + } + + if (sticks[YAW] > STICKMAX && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMIN && sticks[PITCH] < STICKMAX) { + if (cursor_row > MAX_MENU_ROWS) { + switch(cursor_col) { + case 0: + in_menu = false; + break; + case 1: + in_menu = false; +#ifdef USE_RTC6705 + if (masterConfig.vtx_channel != current_vtx_channel) { + masterConfig.vtx_channel = current_vtx_channel; + rtc6705_set_channel(vtx_freq[current_vtx_channel]); + } +#endif + writeEEPROM(); + break; + case 2: + if (current_page < (sizeof(menu_pages) / sizeof(page_t) - 1)) + current_page++; + else + current_page = 0; + } + } else { + if (menu_pages[current_page].rows[cursor_row].update) + menu_pages[current_page].rows[cursor_row].update(true, cursor_col); + } + } + + if (sticks[YAW] < STICKMIN && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMIN && sticks[PITCH] < STICKMAX) { + if (cursor_row > MAX_MENU_ROWS) { + if (cursor_col == 2 && current_page > 0) { + current_page--; + } + } else { + if (menu_pages[current_page].rows[cursor_row].update) + menu_pages[current_page].rows[cursor_row].update(false, cursor_col); + } + } + + if (sticks[PITCH] > STICKMAX && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { + if (cursor_row > MAX_MENU_ROWS) { + cursor_row = menu_pages[current_page].rows_number - 1; + cursor_col = 0; + } else { + if (cursor_row > 0) + cursor_row--; + } + } + if (sticks[PITCH] < STICKMIN && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { + if (cursor_row < (menu_pages[current_page].rows_number - 1)) + cursor_row++; + else + cursor_row = 255; + } + if (sticks[ROLL] > STICKMAX && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { + if (cursor_row > MAX_MENU_ROWS) { + if (cursor_col < 2) + cursor_col++; + } else { + if (cursor_col < (menu_pages[current_page].cols_number - 1)) + cursor_col++; + } + } + if (sticks[ROLL] < STICKMIN && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { + if (cursor_col > 0) + cursor_col--; + } + + if (cursor_row > MAX_MENU_ROWS) { + cursor_row = 255; + cursor_y = -1; + switch(cursor_col) { + case 0: + cursor_x = 0; + break; + case 1: + cursor_x = 9; + break; + case 2: + cursor_x = 23; + break; + default: + cursor_x = 0; + } + } + + sprintf(string_buffer, "EXIT SAVE+EXIT PAGE"); + max7456_write_string(string_buffer, -29); + + pos = (OSD_LINE_LENGTH - strlen(menu_pages[current_page].title)) / 2 + line * OSD_LINE_LENGTH; + sprintf(string_buffer, "%s", menu_pages[current_page].title); + max7456_write_string(string_buffer, pos); + + line += 2; + + for (int i = 0; i < menu_pages[current_page].cols_number; i++){ + col = &menu_pages[current_page].cols[i]; + if (cursor_col == i && cursor_row < MAX_MENU_ROWS) + cursor_x = col->x_pos - 1; + + if (col->title) { + sprintf(string_buffer, "%s", col->title); + max7456_write_string(string_buffer, line * OSD_LINE_LENGTH + col->x_pos); + } + } + + line++; + for (int i = 0; i < menu_pages[current_page].rows_number; i++) { + row = &menu_pages[current_page].rows[i]; + if (cursor_row == i) + cursor_y = line; + + sprintf(string_buffer, "%s", row->title); + max7456_write_string(string_buffer, line * OSD_LINE_LENGTH + 1); + for (int j = 0; j < menu_pages[current_page].cols_number; j++) { + col = &menu_pages[current_page].cols[j]; + row->print(line * OSD_LINE_LENGTH + col->x_pos, j); + } + line++; + } + + max7456_write_string(">", cursor_x + cursor_y * OSD_LINE_LENGTH); +} + +void updateOsd(void) +{ + static uint8_t skip = 0; + static bool blink = false; + static uint8_t arming = 0; + uint32_t seconds; + char line[30]; + uint32_t now = micros(); + + bool updateNow = (int32_t)(now - next_osd_update_at) >= 0L; + if (!updateNow) { + return; + } + next_osd_update_at = now + OSD_UPDATE_FREQUENCY; + if ( !(skip % 2)) + blink = !blink; + + if (skip++ & 1) { + if (ARMING_FLAG(ARMED)) { + if (!armed) { + armed = true; + armed_at = now; + in_menu = false; + arming = 5; + } + } else { + if (armed) { + armed = false; + armed_seconds += ((now - armed_at) / 1000000); + } + for (uint8_t channelIndex = 0; channelIndex < 4; channelIndex++) { + sticks[channelIndex] = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); + } + if (!in_menu && sticks[YAW] > STICKMAX && sticks[THROTTLE] > STICKMIN && sticks[THROTTLE] < STICKMAX && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMAX) { + in_menu = true; + cursor_row = 255; + cursor_col = 2; + activating_menu = true; + } + } + if (in_menu) { + show_menu(); + } else { + if (batteryWarningVoltage > vbat && blink && masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] != -1) { + max7456_write_string("LOW VOLTAGE", masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING]); + } + if (arming && blink && masterConfig.osdProfile.item_pos[OSD_ARMED] != -1) { + max7456_write_string("ARMED", masterConfig.osdProfile.item_pos[OSD_ARMED]); + arming--; + } + if (!armed && masterConfig.osdProfile.item_pos[OSD_DISARMED] != -1) { + max7456_write_string("DISARMED", masterConfig.osdProfile.item_pos[OSD_DISARMED]); + } + + if (masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] != -1) { + sprintf(line, "\x01%d.%1d", vbat / 10, vbat % 10); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]); + } + if (masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] != -1) { + sprintf(line, "\x02%d", rssi / 10); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]); + } + if (masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] != -1) { + sprintf(line, "\x03%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]); + } + if (masterConfig.osdProfile.item_pos[OSD_TIMER] != -1) { + if (armed) { + seconds = armed_seconds + ((now-armed_at) / 1000000); + sprintf(line, "\x04 %02d:%02d", seconds / 60, seconds % 60); + } else { + seconds = now / 1000000; + sprintf(line, "\x05 %02d:%02d", seconds / 60, seconds % 60); + } + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_TIMER]); + } + if (masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] != -1) { + print_average_system_load(masterConfig.osdProfile.item_pos[OSD_CPU_LOAD], 0); + } + } + } else { + max7456_draw_screen_fast(); + } +} + + +void osdInit(void) +{ +#ifdef USE_RTC6705 + rtc6705_init(); + current_vtx_channel = masterConfig.vtx_channel; + rtc6705_set_channel(vtx_freq[current_vtx_channel]); +#endif + max7456_init(masterConfig.osdProfile.system); + +} + +#endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h new file mode 100644 index 0000000000..c72a417761 --- /dev/null +++ b/src/main/io/osd.h @@ -0,0 +1,61 @@ +/* + * 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 . + */ + +#define MAX_MENU_ROWS 8 +#define MAX_MENU_COLS 3 + +typedef struct { + const char* title; + uint8_t x_pos; +} col_t; + +typedef struct { + const char* title; + void (*update)(bool increase, uint8_t col); + void (*print)(uint16_t pos, uint8_t col); +} row_t; + +typedef struct { + const char* title; + uint8_t cols_number; + uint8_t rows_number; + col_t cols[MAX_MENU_COLS]; + row_t rows[MAX_MENU_ROWS]; +} page_t; + + +typedef enum { + OSD_MAIN_BATT_VOLTAGE, + OSD_RSSI_VALUE, + OSD_TIMER, + OSD_THROTTLE_POS, + OSD_CPU_LOAD, + OSD_VTX_CHANNEL, + OSD_VOLTAGE_WARNING, + OSD_ARMED, + OSD_DISARMED, + OSD_MAX_ITEMS, // MUST BE LAST +} osd_items; + + +typedef struct { + uint8_t system; + int16_t item_pos[OSD_MAX_ITEMS]; +} osd_profile; + +void updateOsd(void); +void osdInit(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2ea0b022fc..bba0361042 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -60,6 +60,7 @@ #include "io/flashfs.h" #include "io/beeper.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/osd.h" #include "io/vtx.h" #include "rx/rx.h" @@ -446,6 +447,13 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "AIRMODE", "PIDLOOP", }; +#ifdef OSD +static const char * const lookupTableOsdType[] = { + "AUTO", + "PAL", + "NTSC" +}; +#endif static const char * const lookupTableSuperExpoYaw[] = { "OFF", "ON", "ALWAYS" @@ -482,6 +490,9 @@ typedef enum { TABLE_DEBUG, TABLE_SUPEREXPO_YAW, TABLE_MOTOR_PWM_PROTOCOL, +#ifdef OSD + TABLE_OSD, +#endif } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -506,6 +517,9 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, { lookupTableFastPwm, sizeof(lookupTableFastPwm) / sizeof(char *) }, +#ifdef OSD + { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, +#endif }; #define VALUE_TYPE_OFFSET 0 @@ -786,6 +800,21 @@ const clivalue_t valueTable[] = { #ifdef LED_STRIP { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, #endif +#ifdef USE_RTC6705 + { "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, +#endif +#ifdef OSD + { "osd_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.system, .config.minmax = { 0, 2 } }, + { "osd_main_voltage_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { -480, 480 } }, + { "osd_rssi_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], .config.minmax = { -480, 480 } }, + { "osd_timer_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_TIMER], .config.minmax = { -480, 480 } }, + { "osd_throttle_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], .config.minmax = { -480, 480 } }, + { "osd_cpu_load_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CPU_LOAD], .config.minmax = { -480, 480 } }, + { "osd_vtx_channel_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL], .config.minmax = { -480, 480 } }, + { "osd_voltage_warning_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING], .config.minmax = { -480, 480 } }, + { "osd_armed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ARMED], .config.minmax = { -480, 480 } }, + { "osd_disarmed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_DISARMED], .config.minmax = { -480, 480 } }, +#endif }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 31756bbe08..1eb219ab1f 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -44,6 +44,8 @@ #include "drivers/gyro_sync.h" #include "drivers/sdcard.h" #include "drivers/buf_writer.h" +#include "drivers/max7456.h" +#include "drivers/rtc6705.h" #include "rx/rx.h" #include "rx/msp.h" @@ -57,6 +59,7 @@ #include "io/flashfs.h" #include "io/transponder_ir.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/osd.h" #include "io/vtx.h" #include "telemetry/telemetry.h" @@ -1255,7 +1258,9 @@ static bool processInCommand(void) uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; #endif - +#ifdef OSD + uint8_t addr, font_data[64]; +#endif switch (currentPort->cmdMSP) { case MSP_SELECT_SETTING: if (!ARMING_FLAG(ARMED)) { @@ -1516,6 +1521,32 @@ static bool processInCommand(void) transponderUpdateData(masterConfig.transponderData); break; #endif +#ifdef OSD + case MSP_SET_OSD_CONFIG: + masterConfig.osdProfile.system = read8(); + for (i = 0; i < OSD_MAX_ITEMS; i++) + masterConfig.osdProfile.item_pos[i] = read16(); + break; + case MSP_OSD_CHAR_WRITE: + addr = read8(); + for (i = 0; i < 54; i++) { + font_data[i] = read8(); + } + max7456_write_nvm(addr, font_data); + break; +#endif + +#ifdef USE_RTC6705 + case MSP_SET_VTX_CONFIG: + tmp = read16(); + if (tmp < 40) + masterConfig.vtx_channel = tmp; + if (current_vtx_channel != masterConfig.vtx_channel) { + current_vtx_channel = masterConfig.vtx_channel; + rtc6705_set_channel(vtx_freq[current_vtx_channel]); + } + break; +#endif #ifdef USE_FLASHFS case MSP_DATAFLASH_ERASE: diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index c1041cf618..7db0e06942 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -168,6 +168,15 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings #define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings +#define MSP_OSD_CONFIG 84 //in message Get osd settings +#define MSP_SET_OSD_CONFIG 85 //out message Set osd settings + +#define MSP_OSD_CHAR_READ 86 //in message Get osd settings +#define MSP_OSD_CHAR_WRITE 87 //out message Set osd settings + +#define MSP_VTX_CONFIG 88 //in message Get vtx settings +#define MSP_SET_VTX_CONFIG 89 //out message Set vtx settings + // // Baseflight MSP commands (if enabled they exist in Cleanflight) // diff --git a/src/main/main.c b/src/main/main.c index 2a08223606..d175872d6a 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -72,6 +72,7 @@ #include "io/display.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" +#include "io/osd.h" #include "io/vtx.h" #include "sensors/sensors.h" @@ -134,6 +135,7 @@ void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); void sonarInit(const sonarHardware_t *sonarHardware); +void osdInit(void); typedef enum { SYSTEM_STATE_INITIALISING = 0, @@ -467,6 +469,12 @@ void init(void) } #endif +#ifdef OSD + if (feature(FEATURE_OSD)) { + osdInit(); + } +#endif + if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, @@ -737,6 +745,9 @@ void main_init(void) #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif +#ifdef OSD + setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); +#endif #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 717dc10e8b..a52258129e 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -65,6 +65,8 @@ #include "io/statusindicator.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" +#include "io/osd.h" + #include "io/vtx.h" #include "rx/rx.h" @@ -963,3 +965,12 @@ void taskTransponder(void) } } #endif + +#ifdef OSD +void taskUpdateOsd(void) +{ + if (feature(FEATURE_OSD)) { + updateOsd(); + } +} +#endif diff --git a/src/main/scheduler.h b/src/main/scheduler.h index 237776c259..c55bbaeb05 100755 --- a/src/main/scheduler.h +++ b/src/main/scheduler.h @@ -79,7 +79,9 @@ typedef enum { #ifdef TRANSPONDER TASK_TRANSPONDER, #endif - +#ifdef OSD + TASK_OSD, +#endif #ifdef USE_BST TASK_BST_MASTER_PROCESS, #endif diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler_tasks.c index c2b949259a..393f6d2c0e 100644 --- a/src/main/scheduler_tasks.c +++ b/src/main/scheduler_tasks.c @@ -40,6 +40,9 @@ void taskUpdateDisplay(void); void taskTelemetry(void); void taskLedStrip(void); void taskTransponder(void); +#ifdef OSD +void taskUpdateOsd(void); +#endif #ifdef USE_BST void taskBstReadWrite(void); void taskBstMasterProcess(void); @@ -168,7 +171,14 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_LOW, }, #endif - +#ifdef OSD + [TASK_OSD] = { + .taskName = "OSD", + .taskFunc = taskUpdateOsd, + .desiredPeriod = 1000000 / 60, // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif #ifdef TELEMETRY [TASK_TELEMETRY] = { .taskName = "TELEMETRY", diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c new file mode 100644 index 0000000000..4ccf123ea2 --- /dev/null +++ b/src/main/target/SIRINFPV/target.c @@ -0,0 +1,62 @@ + +#include +#include + +#include +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + // No PPM + PWM1 | (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_MOTOR_OUTPUT << 8), + PWM6 | (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), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + // No PPM + PWM1 | (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_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (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_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + + { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM1 - PB6 + { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2, 0}, // PWM2 - PB6 + { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2, 0}, // PWM3 - PB8 + { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2, 0}, // PWM4 - PB9 + + { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // PWM5 - PB0 - *TIM3_CH3 + { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM6 - PB1 - *TIM3_CH4 + +}; + + diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h new file mode 100644 index 0000000000..0fb5ac2fdb --- /dev/null +++ b/src/main/target/SIRINFPV/target.h @@ -0,0 +1,182 @@ +/* + * 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 "SIRF" + +#define LED0 PB2 +#define BEEPER PA1 + +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define MPU_INT_EXTI PA8 +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 + +// MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 + +// MPU6500 +#define ACC_MPU6500_ALIGN CW90_DEG +#define GYRO_MPU6500_ALIGN CW90_DEG + +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 + +#define USB_IO + +#define USE_VCP +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define SERIAL_PORT_COUNT 4 + +#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 + +#undef USE_I2C + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +#define USE_SPI_DEVICE_3 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_MAX7456 +#define MAX7456_CS_GPIO GPIOA +#define MAX7456_CS_PIN GPIO_Pin_15 +#define MAX7456_SPI_INSTANCE SPI3 + +#define USE_RTC6705 +#define RTC6705_SPIDATA_GPIO GPIOC +#define RTC6705_SPIDATA_PIN Pin_15 +#define RTC6705_SPIDATA_PERIPHERAL RCC_AHBPeriph_GPIOC +#define RTC6705_SPILE_GPIO GPIOC +#define RTC6705_SPILE_PIN Pin_14 +#define RTC6705_SPILE_PERIPHERAL RCC_AHBPeriph_GPIOC +#define RTC6705_SPICLK_GPIO GPIOC +#define RTC6705_SPICLK_PIN Pin_13 +#define RTC6705_SPICLK_PERIPHERAL RCC_AHBPeriph_GPIOC + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_GPIO SPI2_GPIO +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + +#define ADC_INSTANCE ADC1 +#define ADC_DMA_CHANNEL DMA1_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_CHANNEL ADC_Channel_1 + +//#define USE_QUAD_MIXER_ONLY +#define BLACKBOX +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define TELEMETRY +#define SERIAL_RX +#define USE_CLI +#define OSD + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT + +#define USE_SERVOS +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM + +#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 USABLE_TIMER_CHANNEL_COUNT 6 +#define USED_TIMERS (TIM_N(3) | TIM_N(4)) +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOB) + diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk new file mode 100644 index 0000000000..a4543b227c --- /dev/null +++ b/src/main/target/SIRINFPV/target.mk @@ -0,0 +1,16 @@ +FEATURES = VCP SDCARD MAX_OSD +F3_TARGETS += $(TARGET) + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/rtc6705.c + diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index ad184b5aa6..fd775c8773 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -62,6 +62,7 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/beeper.h" +#include "io/osd.h" #include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 47685ab0a8..36b7c95569 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -37,6 +37,7 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/ledstrip.h" +#include "io/osd.h" #include "io/vtx.h" #include "sensors/boardalignment.h" diff --git a/top_makefile b/top_makefile index 4b1bb90e78..1eeee5cfbb 100644 --- a/top_makefile +++ b/top_makefile @@ -15,6 +15,7 @@ ALL_TARGETS += ircfusionf3 ALL_TARGETS += afromini ALL_TARGETS += doge ALL_TARGETS += singularity +ALL_TARGETS += sirinfpv CLEAN_TARGETS := $(addprefix clean_, $(ALL_TARGETS)) @@ -35,6 +36,7 @@ clean_ircfusionf3 ircfusionf3 : opts := TARGET=IRCFUSIONF3 clean_afromini afromini : opts := TARGET=AFROMINI clean_doge doge : opts := TARGET=DOGE clean_singularity singularity : opts := TARGET=SINGULARITY +clean_sirinfpv sirinfpv: opts := TARGET=SIRINFPV .PHONY: all clean From d1841df0d5afecb067b6ade8175e30c9c820e4c8 Mon Sep 17 00:00:00 2001 From: nathan Date: Mon, 13 Jun 2016 22:53:46 -0700 Subject: [PATCH 17/38] init cursor_x --- src/main/io/osd.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ebc489dd94..1cae90f982 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -477,7 +477,8 @@ void show_menu(void) { uint16_t pos; col_t *col; row_t *row; - int16_t cursor_x, cursor_y; + int16_t cursor_x = 0; + int16_t cursor_y = 0; if (activating_menu) { if (sticks[YAW] < 60 && sticks[YAW] > 40 && sticks[PITCH] > 40 && sticks[PITCH] < 60 && sticks[ROLL] > 40 && sticks[ROLL] < 60) From 067c02bbd6a48c48aeb67de7289312523614f763 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Tue, 14 Jun 2016 01:05:51 -0700 Subject: [PATCH 18/38] remove sirinfpv related stuff from config.c, rename rtc6705 to soft_spi_rtc6705, fix some minor bugs --- src/main/config/config.c | 18 +++++------------ src/main/drivers/max7456.c | 12 +++++++---- .../{rtc6705.c => vtx_soft_spi_rtc6705.c} | 8 ++++---- .../{rtc6705.h => vtx_soft_spi_rtc6705.h} | 4 ++-- src/main/flight/mixer.h | 2 -- src/main/io/osd.c | 20 +++++++++++-------- src/main/io/osd.h | 12 +++++------ src/main/io/serial_msp.c | 4 ++-- src/main/target/SIRINFPV/target.mk | 4 ++-- 9 files changed, 41 insertions(+), 43 deletions(-) rename src/main/drivers/{rtc6705.c => vtx_soft_spi_rtc6705.c} (95%) rename src/main/drivers/{rtc6705.h => vtx_soft_spi_rtc6705.h} (89%) diff --git a/src/main/config/config.c b/src/main/config/config.c index ad8cf1f43b..ff7a2ac1b6 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -419,20 +419,8 @@ static void resetConf(void) featureSet(DEFAULT_FEATURES); #endif -#ifdef SIRINFPV - featureSet(FEATURE_OSD); - featureSet(FEATURE_RX_SERIAL); - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; - //masterConfig.batteryConfig.vbatscale = 20; - masterConfig.mag_hardware = MAG_NONE; // disabled by default - masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS; - masterConfig.blackbox_device = 1; - masterConfig.blackbox_rate_num = 1; - masterConfig.blackbox_rate_denom = 1; -#endif - #ifdef OSD - masterConfig.vtx_channel = 19; + featureSet(FEATURE_OSD); masterConfig.osdProfile.system = 0; masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; @@ -445,6 +433,10 @@ static void resetConf(void) masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; #endif +#ifdef USE_RTC6705 + masterConfig.vtx_channel = 19; // default to Boscam E channel 4 +#endif + #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 9a9605fb2d..57874fbf31 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -34,11 +34,15 @@ #define DISABLE_MAX7456 GPIO_SetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) #define ENABLE_MAX7456 GPIO_ResetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) +/** PAL or NTSC, value is number of chars total */ +#define VIDEO_MODE_PIXELS_NTSC 390 +#define VIDEO_MODE_PIXELS_PAL 480 + uint16_t max_screen_size; uint8_t video_signal_type = 0; uint8_t max7456_lock = 0; -char screen[480]; +char screen[VIDEO_MODE_PIXELS_PAL]; uint8_t max7456_send(uint8_t add, uint8_t data) { @@ -81,10 +85,10 @@ void max7456_init(uint8_t system) { } if (video_signal_type) { //PAL - max_screen_size = 480; + max_screen_size = VIDEO_MODE_PIXELS_PAL; max_screen_rows = 16; } else { // NTSC - max_screen_size = 390; + max_screen_size = VIDEO_MODE_PIXELS_NTSC; max_screen_rows = 13; } @@ -121,7 +125,7 @@ void max7456_write_string(const char *string, int16_t address) { else dest = screen + (max_screen_size + address); - while(*string) + while(*string && dest < (screen + max_screen_size)) *dest++ = *string++; } diff --git a/src/main/drivers/rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c similarity index 95% rename from src/main/drivers/rtc6705.c rename to src/main/drivers/vtx_soft_spi_rtc6705.c index 319430f526..e66e5de603 100644 --- a/src/main/drivers/rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -27,7 +27,7 @@ #include "drivers/system.h" #include "drivers/gpio.h" -#include "rtc6705.h" +#include "vtx_soft_spi_rtc6705.h" #define RTC6705_SPICLK_ON GPIO_SetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) #define RTC6705_SPICLK_OFF GPIO_ResetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) @@ -57,7 +57,7 @@ uint16_t vtx_freq[] = uint16_t current_vtx_channel; -void rtc6705_init(void) { +void rtc6705_soft_spi_init(void) { gpio_config_t gpio; #ifdef STM32F303 @@ -99,7 +99,7 @@ void rtc6705_init(void) { gpioInit(RTC6705_SPIDATA_GPIO, &gpio); } -void rtc6705_write_register(uint8_t addr, uint32_t data) { +static void rtc6705_write_register(uint8_t addr, uint32_t data) { uint8_t i; RTC6705_SPILE_OFF; @@ -137,7 +137,7 @@ void rtc6705_write_register(uint8_t addr, uint32_t data) { } -void rtc6705_set_channel(uint16_t channel_freq) { +void rtc6705_soft_spi_set_channel(uint16_t channel_freq) { uint32_t freq = (uint32_t)channel_freq * 1000; uint32_t N, A; diff --git a/src/main/drivers/rtc6705.h b/src/main/drivers/vtx_soft_spi_rtc6705.h similarity index 89% rename from src/main/drivers/rtc6705.h rename to src/main/drivers/vtx_soft_spi_rtc6705.h index 511d090878..6d761418bf 100644 --- a/src/main/drivers/rtc6705.h +++ b/src/main/drivers/vtx_soft_spi_rtc6705.h @@ -21,5 +21,5 @@ extern char* vtx_bands[]; extern uint16_t vtx_freq[]; extern uint16_t current_vtx_channel; -void rtc6705_init(void); -void rtc6705_set_channel(uint16_t channel_freq); +void rtc6705_soft_spi_init(void); +void rtc6705_soft_spi_set_channel(uint16_t channel_freq); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 22c1c008d1..8828ba08b3 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -187,8 +187,6 @@ void writeServos(void); void filterServos(void); #endif -bool motorLimitReached; - extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; bool motorLimitReached; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1cae90f982..99c38e8bf6 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -106,7 +106,11 @@ #ifdef OSD #include "drivers/max7456.h" -#include "drivers/rtc6705.h" + +#ifdef USE_RTC6705 +#include "drivers/vtx_soft_spi_rtc6705.h" +#endif + #include "scheduler.h" #include "common/printf.h" @@ -355,7 +359,7 @@ void print_batt_voltage(uint16_t pos, uint8_t col) { { "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } }, */ -page_t menu_pages[] = { +osd_page_t menu_pages[] = { { .title = "STATUS", .cols_number = 1, @@ -475,8 +479,8 @@ page_t menu_pages[] = { void show_menu(void) { uint8_t line = 1; uint16_t pos; - col_t *col; - row_t *row; + osd_col_t *col; + osd_row_t *row; int16_t cursor_x = 0; int16_t cursor_y = 0; @@ -498,13 +502,13 @@ void show_menu(void) { #ifdef USE_RTC6705 if (masterConfig.vtx_channel != current_vtx_channel) { masterConfig.vtx_channel = current_vtx_channel; - rtc6705_set_channel(vtx_freq[current_vtx_channel]); + rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); } #endif writeEEPROM(); break; case 2: - if (current_page < (sizeof(menu_pages) / sizeof(page_t) - 1)) + if (current_page < (sizeof(menu_pages) / sizeof(osd_page_t) - 1)) current_page++; else current_page = 0; @@ -700,9 +704,9 @@ void updateOsd(void) void osdInit(void) { #ifdef USE_RTC6705 - rtc6705_init(); + rtc6705_soft_spi_init(); current_vtx_channel = masterConfig.vtx_channel; - rtc6705_set_channel(vtx_freq[current_vtx_channel]); + rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); #endif max7456_init(masterConfig.osdProfile.system); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index c72a417761..688a0872f8 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -21,21 +21,21 @@ typedef struct { const char* title; uint8_t x_pos; -} col_t; +} osd_col_t; typedef struct { const char* title; void (*update)(bool increase, uint8_t col); void (*print)(uint16_t pos, uint8_t col); -} row_t; +} osd_row_t; typedef struct { const char* title; uint8_t cols_number; uint8_t rows_number; - col_t cols[MAX_MENU_COLS]; - row_t rows[MAX_MENU_ROWS]; -} page_t; + osd_col_t cols[MAX_MENU_COLS]; + osd_row_t rows[MAX_MENU_ROWS]; +} osd_page_t; typedef enum { @@ -49,7 +49,7 @@ typedef enum { OSD_ARMED, OSD_DISARMED, OSD_MAX_ITEMS, // MUST BE LAST -} osd_items; +} osd_items_t; typedef struct { diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 1eb219ab1f..0592833529 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -45,7 +45,7 @@ #include "drivers/sdcard.h" #include "drivers/buf_writer.h" #include "drivers/max7456.h" -#include "drivers/rtc6705.h" +#include "drivers/vtx_soft_spi_rtc6705.h" #include "rx/rx.h" #include "rx/msp.h" @@ -1543,7 +1543,7 @@ static bool processInCommand(void) masterConfig.vtx_channel = tmp; if (current_vtx_channel != masterConfig.vtx_channel) { current_vtx_channel = masterConfig.vtx_channel; - rtc6705_set_channel(vtx_freq[current_vtx_channel]); + rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); } break; #endif diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index a4543b227c..6efc5c17e4 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -1,5 +1,5 @@ -FEATURES = VCP SDCARD MAX_OSD F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD MAX_OSD TARGET_SRC = \ drivers/accgyro_mpu.c \ @@ -12,5 +12,5 @@ TARGET_SRC = \ drivers/flash_m25p16.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ - drivers/rtc6705.c + drivers/vtx_soft_spi_rtc6705.c From e07808e2a87c4c0d439e1420d4a8828ebfef0d48 Mon Sep 17 00:00:00 2001 From: nathan Date: Wed, 15 Jun 2016 19:59:04 -0700 Subject: [PATCH 19/38] it builds --- fake_travis_build.sh | 2 + src/main/drivers/adc_stm32f30x.c | 3 +- src/main/drivers/barometer_bmp085.c | 1 - src/main/drivers/io.c | 22 +- src/main/drivers/io.h | 55 ++--- src/main/drivers/io_impl.h | 21 +- src/main/drivers/max7456_symbols.h | 233 ++++++++++++++++++ src/main/drivers/pwm_mapping.c | 30 ++- src/main/drivers/pwm_output.c | 14 +- src/main/drivers/pwm_rx.c | 14 +- src/main/drivers/resource.h | 7 +- src/main/drivers/serial_softserial.c | 39 ++- src/main/drivers/serial_usb_vcp.c | 9 +- src/main/drivers/sound_beeper.c | 4 +- src/main/drivers/timer.c | 24 +- src/main/drivers/timer.h | 10 +- src/main/io/ledstrip.c | 1 + src/main/io/osd.c | 1 - src/main/io/serial_4way.c | 6 +- src/main/io/serial_cli.c | 49 ++++ src/main/io/serial_msp.h | 2 +- src/main/main.c | 11 +- src/main/sensors/initialisation.c | 42 +--- src/main/sensors/sonar.c | 2 +- .../target/ALIENFLIGHTF3/hardware_revision.c | 21 +- .../target/ALIENFLIGHTF3/hardware_revision.h | 5 + src/main/target/ALIENFLIGHTF3/target.c | 29 +-- src/main/target/ALIENFLIGHTF3/target.mk | 1 - src/main/target/ALIENFLIGHTF4/target.c | 27 +- src/main/target/BLUEJAYF4/target.c | 15 +- src/main/target/BLUEJAYF4/target.h | 2 +- src/main/target/CC3D/target.c | 26 +- src/main/target/CHEBUZZF3/target.c | 39 ++- src/main/target/CHEBUZZF3/target.h | 4 +- src/main/target/CJMCU/hardware_revision.c | 6 + src/main/target/CJMCU/hardware_revision.h | 5 + src/main/target/CJMCU/target.c | 29 +-- src/main/target/CJMCU/target.mk | 1 - .../COLIBRI_RACE}/bus_bst.h | 0 .../COLIBRI_RACE}/bus_bst_stm32f30x.c | 2 +- .../{io => target/COLIBRI_RACE}/i2c_bst.c | 1 + .../{io => target/COLIBRI_RACE}/i2c_bst.h | 2 - src/main/target/COLIBRI_RACE/target.c | 26 +- src/main/target/COLIBRI_RACE/target.mk | 4 +- src/main/target/DOGE/target.c | 18 +- src/main/target/EUSTM32F103RC/target.c | 28 +-- src/main/target/FURYF3/target.c | 16 +- src/main/target/FURYF4/target.c | 10 +- src/main/target/IRCFUSIONF3/target.c | 34 +-- src/main/target/KISSFC/target.c | 24 +- src/main/target/LUX_RACE/target.c | 25 +- src/main/target/MOTOLAB/target.c | 19 +- src/main/target/NAZE/hardware_revision.c | 24 ++ src/main/target/NAZE/hardware_revision.h | 5 + src/main/target/NAZE/target.c | 28 +-- src/main/target/NAZE/target.mk | 3 +- src/main/target/NAZE32PRO/target.c | 29 ++- src/main/target/OLIMEXINO/target.c | 29 +-- src/main/target/OMNIBUS/target.c | 89 +++++++ src/main/target/OMNIBUS/target.h | 231 +++++++++++++++++ src/main/target/OMNIBUS/target.mk | 16 ++ src/main/target/PIKOBLX/target.c | 55 +++++ src/main/target/PIKOBLX/target.h | 173 +++++++++++++ src/main/target/PIKOBLX/target.mk | 13 + src/main/target/PORT103R/target.c | 28 +-- src/main/target/REVO/target.c | 26 +- src/main/target/RMDO/target.c | 34 +-- src/main/target/SINGULARITY/target.c | 23 +- src/main/target/SIRINFPV/target.mk | 4 +- src/main/target/SPARKY/target.c | 44 +--- src/main/target/SPRACINGF3/target.c | 35 +-- src/main/target/SPRACINGF3EVO/target.c | 30 +-- src/main/target/SPRACINGF3MINI/target.c | 27 +- src/main/target/STM32F3DISCOVERY/target.c | 29 +-- src/main/target/STM32F3DISCOVERY/target.h | 8 + .../target/STM32F3DISCOVERYMAX7456/target.c | 90 +++++++ .../target/STM32F3DISCOVERYMAX7456/target.h | 196 +++++++++++++++ .../target/STM32F3DISCOVERYMAX7456/target.mk | 18 ++ src/test/unit/scheduler_unittest.cc | 2 + support/openocd/stm32f3.cfg | 10 + 80 files changed, 1746 insertions(+), 574 deletions(-) create mode 100644 src/main/drivers/max7456_symbols.h rename src/main/{drivers => target/COLIBRI_RACE}/bus_bst.h (100%) rename src/main/{drivers => target/COLIBRI_RACE}/bus_bst_stm32f30x.c (99%) rename src/main/{io => target/COLIBRI_RACE}/i2c_bst.c (99%) rename src/main/{io => target/COLIBRI_RACE}/i2c_bst.h (96%) create mode 100644 src/main/target/OMNIBUS/target.c create mode 100644 src/main/target/OMNIBUS/target.h create mode 100644 src/main/target/OMNIBUS/target.mk create mode 100644 src/main/target/PIKOBLX/target.c create mode 100644 src/main/target/PIKOBLX/target.h create mode 100644 src/main/target/PIKOBLX/target.mk create mode 100644 src/main/target/STM32F3DISCOVERYMAX7456/target.c create mode 100644 src/main/target/STM32F3DISCOVERYMAX7456/target.h create mode 100644 src/main/target/STM32F3DISCOVERYMAX7456/target.mk create mode 100644 support/openocd/stm32f3.cfg diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 339c4008d0..5bae319ada 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -8,11 +8,13 @@ targets=("PUBLISHMETA=True" \ "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" \ diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 68f1177b5c..97f6ee525b 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -21,7 +21,7 @@ #include "platform.h" #include "system.h" - +#include "common/utils.h" #include "gpio.h" #include "sensor.h" @@ -38,6 +38,7 @@ void adcInit(drv_adc_config_t *init) { + UNUSED(init); ADC_InitTypeDef ADC_InitStructure; DMA_InitTypeDef DMA_InitStructure; GPIO_InitTypeDef GPIO_InitStructure; diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index 951bc62ab9..a746500b56 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -52,7 +52,6 @@ void bmp085_extiHandler(extiCallbackRec_t* cb) bool bmp085TestEOCConnected(const bmp085Config_t *config); # endif - typedef struct { int16_t ac1; int16_t ac2; diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 7d1d206b09..57bbecdc44 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -11,7 +11,7 @@ struct ioPortDef_s { rccPeriphTag_t rcc; }; -#if defined(STM32F10X) +#if defined(STM32F1) const struct ioPortDef_s ioPortDefs[] = { { RCC_APB2(IOPA) }, { RCC_APB2(IOPB) }, @@ -33,7 +33,7 @@ const struct ioPortDef_s ioPortDefs[] = { #endif }, }; -#elif defined(STM32F303xC) +#elif defined(STM32F3) const struct ioPortDef_s ioPortDefs[] = { { RCC_AHB(GPIOA) }, { RCC_AHB(GPIOB) }, @@ -111,11 +111,11 @@ uint32_t IO_EXTI_Line(IO_t io) { if (!io) return 0; -#if defined(STM32F10X) +#if defined(STM32F1) return 1 << IO_GPIOPinIdx(io); -#elif defined(STM32F303xC) +#elif defined(STM32F3) return IO_GPIOPinIdx(io); -#elif defined(STM32F40_41xxx) || defined(STM32F411xE) +#elif defined(STM32F4) return 1 << IO_GPIOPinIdx(io); #else # error "Unknown target type" @@ -133,7 +133,7 @@ void IOWrite(IO_t io, bool hi) { if (!io) return; -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#ifdef STM32F4 if (hi) { IO_GPIO(io)->BSRRL = IO_Pin(io); } @@ -149,7 +149,7 @@ void IOHi(IO_t io) { if (!io) return; -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#ifdef STM32F4 IO_GPIO(io)->BSRRL = IO_Pin(io); #else IO_GPIO(io)->BSRR = IO_Pin(io); @@ -160,7 +160,7 @@ void IOLo(IO_t io) { if (!io) return; -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#ifdef STM32F4 IO_GPIO(io)->BSRRH = IO_Pin(io); #else IO_GPIO(io)->BRR = IO_Pin(io); @@ -175,7 +175,7 @@ void IOToggle(IO_t io) // Read pin state from ODR but write to BSRR because it only changes the pins // high in the mask value rather than all pins. XORing ODR directly risks // setting other pins incorrectly because it change all pins' state. -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#ifdef STM32F4 if (IO_GPIO(io)->ODR & mask) { IO_GPIO(io)->BSRRH = mask; } else { @@ -216,7 +216,7 @@ resourceType_t IOGetResources(IO_t io) return ioRec->resourcesUsed; } -#if defined(STM32F10X) +#if defined(STM32F1) void IOConfigGPIO(IO_t io, ioConfig_t cfg) { @@ -233,7 +233,7 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg) GPIO_Init(IO_GPIO(io), &init); } -#elif defined(STM32F303xC) || defined(STM32F40_41xxx) || defined(STM32F411xE) +#elif defined(STM32F3) || defined(STM32F4) void IOConfigGPIO(IO_t io, ioConfig_t cfg) { diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index c8e0561aa6..debc78d950 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -3,11 +3,12 @@ #include #include +#include #include "resource.h" // IO pin identification // make sure that ioTag_t can't be assigned into IO_t without warning -typedef uint8_t ioTag_t; // packet tag to specify IO pin +typedef uint8_t ioTag_t; // packet tag to specify IO pin typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change // NONE initializer for IO_t variable @@ -33,42 +34,32 @@ typedef void* IO_t; // type specifying IO pin. Currently ioRec_t poin // helps masking CPU differences typedef uint8_t ioConfig_t; // packed IO configuration -#if defined(STM32F10X) +#if defined(STM32F1) // mode is using only bits 6-2 -# define IO_CONFIG(mode, speed) ((mode) | (speed)) +#define IO_CONFIG(mode, speed) ((mode) | (speed)) -# define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_2MHz) -# 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_OUT_PP IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_2MHz) +#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) -#elif defined(STM32F303xC) +#elif defined(STM32F3) || defined(STM32F4) -# define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5)) +#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_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_OD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) -# 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) - -#elif defined(STM32F40_41xxx) || defined(STM32F411xE) - -# 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_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_OD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) -# 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_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO +#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) +#define IOCFG_AF_PP_UP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_UP) +#define IOCFG_AF_OD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) +#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) #elif defined(UNIT_TEST) @@ -100,7 +91,7 @@ resourceType_t IOGetResources(IO_t io); IO_t IOGetByTag(ioTag_t tag); void IOConfigGPIO(IO_t io, ioConfig_t cfg); -#if defined(STM32F303xC) || defined(STM32F40_41xxx) || defined(STM32F411xE) +#if defined(STM32F3) || defined(STM32F4) void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af); #endif diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h index 1b5cb15e09..b7bbcec7f3 100644 --- a/src/main/drivers/io_impl.h +++ b/src/main/drivers/io_impl.h @@ -19,19 +19,20 @@ extern ioRec_t ioRecs[DEFIO_IO_USED_COUNT]; int IO_GPIOPortIdx(IO_t io); int IO_GPIOPinIdx(IO_t io); -#if defined(STM32F1) -int IO_GPIO_PinSource(IO_t io); -int IO_GPIO_PortSource(IO_t io); -#elif defined(STM32F3) -int IO_GPIO_PinSource(IO_t io); -int IO_GPIO_PortSource(IO_t io); -int IO_EXTI_PortSourceGPIO(IO_t io); -int IO_EXTI_PinSource(IO_t io); -#elif defined(STM32F4) + int IO_GPIO_PinSource(IO_t io); int IO_GPIO_PortSource(IO_t io); + +#if defined(STM32F3) || defined(STM32F4) int IO_EXTI_PortSourceGPIO(IO_t io); int IO_EXTI_PinSource(IO_t io); -# endif +#endif + +GPIO_TypeDef* IO_GPIO(IO_t io); +uint16_t IO_Pin(IO_t io); + +#define IO_GPIOBYTAG(tag) IO_GPIO(IOGetByTag(tag)) +#define IO_PINBYTAG(tag) IO_Pin(IOGetByTag(tag)) + uint32_t IO_EXTI_Line(IO_t io); ioRec_t *IO_Rec(IO_t io); diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h new file mode 100644 index 0000000000..2d7862ebd9 --- /dev/null +++ b/src/main/drivers/max7456_symbols.h @@ -0,0 +1,233 @@ +/* @file max7456_symbols.h + * @brief max7456 preprocessor symbols + * + * @author Nathan Tsoi nathan@vertile.com + * + * Copyright (C) 2016 Nathan Tsoi + * + * This program 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. + * + * This program 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 this program. If not, see + */ + +#pragma once + +#ifdef MAX_OSD + +// Character Symbols +#define SYM_BLANK 0X20 + +// Satellite Graphics +#define SYM_SAT_L 0X1E +#define SYM_SAT_R 0X1F +//#define SYM_SAT 0X0F // Not used + +// Degrees Icon for HEADING/DIRECTION HOME +#define SYM_DEGREES 0XBD + +// Direction arrows +#define SYM_ARROW_SOUTH 0X60 +#define SYM_ARROW_2 0X61 +#define SYM_ARROW_3 0X62 +#define SYM_ARROW_4 0X63 +#define SYM_ARROW_EAST 0X64 +#define SYM_ARROW_6 0X65 +#define SYM_ARROW_7 0X66 +#define SYM_ARROW_8 0X67 +#define SYM_ARROW_NORTH 0X68 +#define SYM_ARROW_10 0X69 +#define SYM_ARROW_11 0X6A +#define SYM_ARROW_12 0X6B +#define SYM_ARROW_WEST 0X6C +#define SYM_ARROW_14 0X6D +#define SYM_ARROW_15 0X6E +#define SYM_ARROW_16 0X6F + +// Heading Graphics +#define SYM_HEADING_N 0X18 +#define SYM_HEADING_S 0X19 +#define SYM_HEADING_E 0X1A +#define SYM_HEADING_W 0X1B +#define SYM_HEADING_DIVIDED_LINE 0X1C +#define SYM_HEADING_LINE 0X1D + +// FRSKY HUB +#define SYM_CELL0 0xF0 +#define SYM_CELL1 0xF1 +#define SYM_CELL2 0xF2 +#define SYM_CELL3 0xF3 +#define SYM_CELL4 0xF4 +#define SYM_CELL5 0xF5 +#define SYM_CELL6 0xF6 +#define SYM_CELL7 0xF7 +#define SYM_CELL8 0xF8 +#define SYM_CELL9 0xF9 +#define SYM_CELLA 0xFA +#define SYM_CELLB 0xFB +#define SYM_CELLC 0xFC +#define SYM_CELLD 0xFD +#define SYM_CELLE 0xFE +#define SYM_CELLF 0xC3 + +// Map mode +#define SYM_HOME 0x04 +#define SYM_AIRCRAFT 0X05 +#define SYM_RANGE_100 0x21 +#define SYM_RANGE_500 0x22 +#define SYM_RANGE_2500 0x23 +#define SYM_RANGE_MAX 0x24 +#define SYM_DIRECTION 0x72 + +// GPS Coordinates and Altitude +#define SYM_LAT 0xCA +#define SYM_LON 0XCB +#define SYM_ALT 0XCC + +// GPS Mode and Autopilot +#define SYM_3DFIX 0XDF +#define SYM_HOLD 0XEF +#define SYM_G_HOME 0XFF +#define SYM_GHOME 0X9D +#define SYM_GHOME1 0X9E +#define SYM_GHOLD 0XCD +#define SYM_GHOLD1 0XCE +#define SYM_GMISSION 0XB5 +#define SYM_GMISSION1 0XB6 +#define SYM_GLAND 0XB7 +#define SYM_GLAND1 0XB8 + +// Gimbal active Mode +#define SYM_GIMBAL 0X16 +#define SYM_GIMBAL1 0X17 + + +// Sensor´s Presence +#define SYM_ACC 0XA0 +#define SYM_MAG 0XA1 +#define SYM_BAR 0XA2 +#define SYM_GPS 0XA3 +#define SYM_MAN 0XC0 +#define SYM_MAN1 0XC1 +#define SYM_MAN2 0XC2 +#define SYM_CHECK 0XBE +#define SYM_BARO10 0XB7 +#define SYM_BARO11 0XB8 +#define SYM_MAG10 0XB5 +#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_RIGHT 0X02 +#define SYM_AH_LEFT 0X03 +#define SYM_AH_DECORATION_UP 0XC9 +#define SYM_AH_DECORATION_DOWN 0XCF + + +// AH Bars +#define SYM_AH_BAR9_0 0x80 + + +// Temperature +#define SYM_TEMP_F 0X0D +#define SYM_TEMP_C 0X0E + +// Batt evolution +#define SYM_BATT_FULL 0X90 +#define SYM_BATT_5 0X91 +#define SYM_BATT_4 0X92 +#define SYM_BATT_3 0X93 +#define SYM_BATT_2 0X94 +#define SYM_BATT_1 0X95 +#define SYM_BATT_EMPTY 0X96 + +// Vario +#define SYM_VARIO 0x7F + +// Glidescope +#define SYM_GLIDESCOPE 0xE0 + +// Batt Icon´s +#define SYM_MAIN_BATT 0X97 +#define SYM_VID_BAT 0XBF + +// Unit Icon´s (Metric) +#define SYM_MS 0X9F +#define SYM_KMH 0XA5 +#define SYM_ALTM 0XA7 +#define SYM_DISTHOME_M 0XBB +#define SYM_M 0X0C + +// Unit Icon´s (Imperial) +#define SYM_FTS 0X99 +#define SYM_MPH 0XA6 +#define SYM_ALTFT 0XA8 +#define SYM_DISTHOME_FT 0XB9 +#define SYM_FT 0X0F + +// Voltage and amperage +#define SYM_VOLT 0XA9 +#define SYM_AMP 0X9A +#define SYM_MAH 0XA4 +#define SYM_WATT 0X57 + +// Flying Mode +#define SYM_ACRO 0XAE +#define SYM_ACROGY 0X98 +#define SYM_ACRO1 0XAF +#define SYM_STABLE 0XAC +#define SYM_STABLE1 0XAD +#define SYM_HORIZON 0XC4 +#define SYM_HORIZON1 0XC5 +#define SYM_PASS 0XAA +#define SYM_PASS1 0XAB +#define SYM_AIR 0XEA +#define SYM_AIR1 0XEB +#define SYM_PLUS 0X89 + +// Note, these change with scrolling enabled (scrolling is TODO) +//#define SYM_AH_DECORATION_LEFT 0x13 +//#define SYM_AH_DECORATION_RIGHT 0x13 +#define SYM_AH_DECORATION 0x13 + +// Time +#define SYM_ON_M 0X9B +#define SYM_FLY_M 0X9C +#define SYM_ON_H 0X70 +#define SYM_FLY_H 0X71 + +// Throttle Position (%) +#define SYM_THR 0XC8 +#define SYM_THR1 0XC9 + +// RSSI +#define SYM_RSSI 0XBA + +// Menu cursor +#define SYM_CURSOR SYM_AH_LEFT + +//Misc +#define SYM_COLON 0X2D + +//sport +#define SYM_MIN 0xB3 +#define SYM_AVG 0xB4 + +#endif //MAX_OSD diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 99650a91b9..6e9ceb3a58 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -23,6 +23,8 @@ #include "platform.h" #include "gpio.h" +#include "io.h" +#include "io_impl.h" #include "timer.h" #include "pwm_output.h" @@ -91,6 +93,16 @@ pwmOutputConfiguration_t *pwmGetOutputConfiguration(void) return &pwmOutputConfiguration; } +bool CheckGPIOPin(ioTag_t tag, GPIO_TypeDef *gpio, uint16_t pin) +{ + return IO_GPIOBYTAG(tag) == gpio && IO_PINBYTAG(tag) == pin; +} + +bool CheckGPIOPinSource(ioTag_t tag, GPIO_TypeDef *gpio, uint16_t pin) +{ + return IO_GPIOBYTAG(tag) == gpio && IO_GPIO_PinSource(IOGetByTag(tag)) == pin; +} + pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) { int i = 0; @@ -132,7 +144,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #if defined(STM32F303xC) && defined(USE_USART3) // skip UART3 ports (PB10/PB11) - if (init->useUART3 && timerHardwarePtr->gpio == UART3_GPIO && (timerHardwarePtr->pin == UART3_TX_PIN || timerHardwarePtr->pin == UART3_RX_PIN)) + if (init->useUART3 && (CheckGPIOPin(timerHardwarePtr->pin, UART3_GPIO, UART3_TX_PIN) || CheckGPIOPin(timerHardwarePtr->pin, UART3_GPIO, UART3_RX_PIN))) continue; #endif @@ -151,7 +163,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (timerHardwarePtr->tim == LED_STRIP_TIMER) continue; #if defined(STM32F303xC) && defined(WS2811_GPIO) && defined(WS2811_PIN_SOURCE) - if (timerHardwarePtr->gpio == WS2811_GPIO && timerHardwarePtr->gpioPinSource == WS2811_PIN_SOURCE) + if (CheckGPIOPinSource(timerHardwarePtr->pin, WS2811_GPIO, WS2811_PIN_SOURCE)) continue; #endif } @@ -159,28 +171,28 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #endif #ifdef VBAT_ADC_GPIO - if (init->useVbat && timerHardwarePtr->gpio == VBAT_ADC_GPIO && timerHardwarePtr->pin == VBAT_ADC_GPIO_PIN) { + if (init->useVbat && CheckGPIOPin(timerHardwarePtr->pin, VBAT_ADC_GPIO, VBAT_ADC_GPIO_PIN)) { continue; } #endif #ifdef RSSI_ADC_GPIO - if (init->useRSSIADC && timerHardwarePtr->gpio == RSSI_ADC_GPIO && timerHardwarePtr->pin == RSSI_ADC_GPIO_PIN) { + if (init->useRSSIADC && CheckGPIOPin(timerHardwarePtr->pin, RSSI_ADC_GPIO, RSSI_ADC_GPIO_PIN)) { continue; } #endif #ifdef CURRENT_METER_ADC_GPIO - if (init->useCurrentMeterADC && timerHardwarePtr->gpio == CURRENT_METER_ADC_GPIO && timerHardwarePtr->pin == CURRENT_METER_ADC_GPIO_PIN) { + if (init->useCurrentMeterADC && CheckGPIOPin(timerHardwarePtr->pin, CURRENT_METER_ADC_GPIO, CURRENT_METER_ADC_GPIO_PIN)) { continue; } #endif #ifdef SONAR - if (init->sonarGPIOConfig && timerHardwarePtr->gpio == init->sonarGPIOConfig->gpio && + if (init->sonarGPIOConfig && ( - timerHardwarePtr->pin == init->sonarGPIOConfig->triggerPin || - timerHardwarePtr->pin == init->sonarGPIOConfig->echoPin + CheckGPIOPin(timerHardwarePtr->pin, init->sonarGPIOConfig->gpio, init->sonarGPIOConfig->triggerPin) || + CheckGPIOPin(timerHardwarePtr->pin, init->sonarGPIOConfig->gpio, init->sonarGPIOConfig->echoPin) ) ) { continue; @@ -226,7 +238,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) type = MAP_TO_SERVO_OUTPUT; #endif -#if defined(SPRACINGF3MINI) +#if defined(SPRACINGF3MINI) || defined(OMNIBUS) // remap PWM6+7 as servos if ((timerIndex == PWM6 || timerIndex == PWM7) && timerHardwarePtr->tim == TIM15) type = MAP_TO_SERVO_OUTPUT; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 5b047875e7..cd703c557a 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -22,7 +22,7 @@ #include "platform.h" -#include "gpio.h" +#include "io.h" #include "timer.h" #include "flight/failsafe.h" // FIXME dependency into the main code from a driver @@ -83,14 +83,10 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 } } -static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) +static void pwmGPIOConfig(ioTag_t pin, ioConfig_t mode) { - gpio_config_t cfg; - - cfg.pin = pin; - cfg.mode = mode; - cfg.speed = Speed_2MHz; - gpioInit(gpio, &cfg); + IOInit(IOGetByTag(pin), OWNER_PWMOUTPUT_MOTOR, RESOURCE_OUTPUT); + IOConfigGPIO(IOGetByTag(pin), mode); } static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) @@ -98,7 +94,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; configTimeBase(timerHardware->tim, period, mhz); - pwmGPIOConfig(timerHardware->gpio, timerHardware->pin, Mode_AF_PP); + pwmGPIOConfig(timerHardware->pin, IOCFG_AF_PP); pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->outputInverted); diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index df91273f2e..50a24e28e7 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -337,14 +337,10 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture } } -static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) +static void pwmGPIOConfig(ioTag_t pin, ioConfig_t mode) { - gpio_config_t cfg; - - cfg.pin = pin; - cfg.mode = mode; - cfg.speed = Speed_2MHz; - gpioInit(gpio, &cfg); + IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT); + IOConfigGPIO(IOGetByTag(pin), mode); } void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) @@ -376,7 +372,7 @@ void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel) self->mode = INPUT_MODE_PWM; self->timerHardware = timerHardwarePtr; - pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode); + pwmGPIOConfig(timerHardwarePtr->pin, timerHardwarePtr->ioMode); pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); timerConfigure(timerHardwarePtr, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ); @@ -405,7 +401,7 @@ void ppmInConfig(const timerHardware_t *timerHardwarePtr) self->mode = INPUT_MODE_PPM; self->timerHardware = timerHardwarePtr; - pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode); + pwmGPIOConfig(timerHardwarePtr->pin, timerHardwarePtr->ioMode); pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ); diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 7c10ba7ecb..dc1d71f3d5 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -6,8 +6,6 @@ typedef enum { OWNER_PWMINPUT, OWNER_PPMINPUT, OWNER_PWMOUTPUT_MOTOR, - OWNER_PWMOUTPUT_FAST, - OWNER_PWMOUTPUT_ONESHOT, OWNER_PWMOUTPUT_SERVO, OWNER_SOFTSERIAL_RX, OWNER_SOFTSERIAL_TX, @@ -23,7 +21,9 @@ typedef enum { OWNER_SYSTEM, OWNER_SDCARD, OWNER_FLASH, - OWNER_USB + OWNER_USB, + OWNER_BEEPER, + OWNER_OSD } resourceOwner_t; // Currently TIMER should be shared resource (softserial dualtimer and timerqueue needs to allocate timer channel, but pin can be used for other function) @@ -41,3 +41,4 @@ typedef enum { RESOURCE_I2C = 1 << 7, RESOURCE_SPI = 1 << 8, } resourceType_t; + diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 446302b551..5c930955b3 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -30,7 +30,7 @@ #include "nvic.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "timer.h" #include "serial.h" @@ -48,6 +48,8 @@ typedef struct softSerial_s { serialPort_t port; + IO_t rxIO; + IO_t txIO; const timerHardware_t *rxTimerHardware; volatile uint8_t rxBuffer[SOFTSERIAL_BUFFER_SIZE]; @@ -92,30 +94,24 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state) } if (state) { - digitalHi(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin); + IOHi(softSerial->txIO); } else { - digitalLo(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin); + IOLo(softSerial->txIO); } } -static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint16_t pin, GPIO_Mode mode) +static void softSerialGPIOConfig(ioTag_t pin, ioConfig_t mode) { - gpio_config_t cfg; - - cfg.pin = pin; - cfg.mode = mode; - cfg.speed = Speed_2MHz; - gpioInit(gpio, &cfg); + IOInit(IOGetByTag(pin), OWNER_SOFTSERIAL_RXTX, RESOURCE_USART); + IOConfigGPIO(IOGetByTag(pin), mode); } -void serialInputPortConfig(const timerHardware_t *timerHardwarePtr) +void serialInputPortConfig(ioTag_t pin) { -#ifdef STM32F10X - softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU); +#ifdef STM32F1 + softSerialGPIOConfig(pin, IOCFG_IPU); #else -#ifdef STM32F303xC - softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_AF_PP_PU); -#endif + softSerialGPIOConfig(pin, IOCFG_AF_PP_UP); #endif } @@ -168,9 +164,9 @@ static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t timerChConfigCallbacks(timerHardwarePtr, &softSerialPorts[reference].edgeCb, NULL); } -static void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr) +static void serialOutputPortConfig(ioTag_t pin) { - softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP); + softSerialGPIOConfig(pin, IOCFG_OUT_PP); } static void resetBuffers(softSerial_t *softSerial) @@ -222,8 +218,11 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb softSerial->softSerialPortIndex = portIndex; - serialOutputPortConfig(softSerial->txTimerHardware); - serialInputPortConfig(softSerial->rxTimerHardware); + softSerial->txIO = IOGetByTag(softSerial->txTimerHardware->pin); + serialOutputPortConfig(softSerial->txTimerHardware->pin); + + softSerial->rxIO = IOGetByTag(softSerial->rxTimerHardware->pin); + serialInputPortConfig(softSerial->rxTimerHardware->pin); setTxSignal(softSerial, ENABLE); delay(50); diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 0954027753..45a3702562 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -24,6 +24,7 @@ #include "build_config.h" #include "common/utils.h" +#include "drivers/io.h" #include "usb_core.h" #ifdef STM32F4 @@ -178,11 +179,9 @@ serialPort_t *usbVcpOpen(void) vcpPort_t *s; #ifdef STM32F4 - USBD_Init(&USB_OTG_dev, - USB_OTG_FS_CORE_ID, - &USR_desc, - &USBD_CDC_cb, - &USR_cb); + 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); #else Set_System(); Set_USBClock(); diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 09294c2609..1e71845d20 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -24,7 +24,7 @@ #include "common/utils.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "sound_beeper.h" @@ -61,7 +61,7 @@ void beeperInit(const beeperConfig_t *config) beeperInverted = config->isInverted; if (beeperIO) { - IOInit(beeperIO, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT); IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); } systemBeep(false); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index ed8b0d587e..8e78241681 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -353,14 +353,10 @@ void timerChClearCCFlag(const timerHardware_t *timHw) } // configure timer channel GPIO mode -void timerChConfigGPIO(const timerHardware_t *timHw, GPIO_Mode mode) +void timerChConfigGPIO(const timerHardware_t* timHw, ioConfig_t mode) { - gpio_config_t cfg; - - cfg.pin = timHw->pin; - cfg.mode = mode; - cfg.speed = Speed_2MHz; - gpioInit(timHw->gpio, &cfg); + IOInit(IOGetByTag(timHw->pin), OWNER_TIMER, RESOURCE_TIMER); + IOConfigGPIO(IOGetByTag(timHw->pin), mode); } // calculate input filter constant @@ -656,20 +652,14 @@ void timerInit(void) RCC_AHBPeriphClockCmd(TIMER_AHB_PERIPHERALS, ENABLE); #endif -#ifdef STM32F303xC +#if defined(STM32F3) || defined(STM32F4) for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - GPIO_PinAFConfig(timerHardwarePtr->gpio, (uint16_t)timerHardwarePtr->gpioPinSource, timerHardwarePtr->alternateFunction); + IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->pin), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); } #endif - -#if defined(STM32F40_41xxx) || defined (STM32F411xE) - for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { - const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - GPIO_PinAFConfig(timerHardwarePtr->gpio, (uint16_t)timerHardwarePtr->gpioPinSource, timerHardwarePtr->alternateFunction); - } -#endif -// initialize timer channel structures + + // initialize timer channel structures for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { timerChannelInfo[i].type = TYPE_FREE; } diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index d8f0b2051c..9ca4c7d957 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -17,6 +17,8 @@ #pragma once +#include "io.h" + #if !defined(USABLE_TIMER_CHANNEL_COUNT) #define USABLE_TIMER_CHANNEL_COUNT 14 #endif @@ -64,14 +66,12 @@ typedef struct timerOvrHandlerRec_s { typedef struct { TIM_TypeDef *tim; - GPIO_TypeDef *gpio; - uint16_t pin; + ioTag_t pin; uint8_t channel; uint8_t irq; uint8_t outputEnable; - GPIO_Mode gpioInputMode; + ioConfig_t ioMode; #if defined(STM32F3) || defined(STM32F4) - uint8_t gpioPinSource; // TODO - this can be removed and pinSource calculated from pin uint8_t alternateFunction; #endif uint8_t outputInverted; @@ -106,7 +106,7 @@ volatile timCCR_t* timerChCCR(const timerHardware_t* timHw); volatile timCCR_t* timerChCCRLo(const timerHardware_t* timHw); volatile timCCR_t* timerChCCRHi(const timerHardware_t* timHw); void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh); -void timerChConfigGPIO(const timerHardware_t* timHw, GPIO_Mode mode); +void timerChConfigGPIO(const timerHardware_t* timHw, ioConfig_t mode); void timerChCCHandlerInit(timerCCHandlerRec_t *self, timerCCHandlerCallback *fn); void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *fn); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index afb78f3529..061f0d798a 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -58,6 +58,7 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/gps.h" +#include "io/osd.h" #include "io/vtx.h" #include "flight/failsafe.h" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 99c38e8bf6..88377fa0d7 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -47,7 +47,6 @@ #include "drivers/pwm_rx.h" #include "drivers/adc.h" #include "drivers/bus_i2c.h" -#include "drivers/bus_bst.h" #include "drivers/bus_spi.h" #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index d7710cee42..91ed53db4e 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -26,6 +26,8 @@ #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/serial.h" #include "drivers/gpio.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" @@ -149,8 +151,8 @@ int esc4wayInit(void) for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[escIdx].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; - escHardware[escIdx].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; + escHardware[escIdx].gpio = IO_GPIO(IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->pin)); + escHardware[escIdx].pin = IO_Pin(IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->pin)); escHardware[escIdx].pinpos = getPinPos(escHardware[escIdx].pin); escHardware[escIdx].gpio_config_INPUT.pin = escHardware[escIdx].pin; escHardware[escIdx].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig() diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index bba0361042..8bd73c94b0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -44,6 +44,8 @@ #include "drivers/serial.h" #include "drivers/bus_i2c.h" #include "drivers/gpio.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/sdcard.h" @@ -140,6 +142,7 @@ static void cliTasks(char *cmdline); #endif static void cliVersion(char *cmdline); static void cliRxRange(char *cmdline); +static void cliResource(char *cmdline); #ifdef GPS static void cliGpsPassthrough(char *cmdline); @@ -1889,6 +1892,10 @@ static void dumpValues(uint16_t valueSection) cliPrintf("set %s = ", valueTable[i].name); cliPrintVar(value, 0); cliPrint("\r\n"); + +#ifdef STM32F4 + delayMicroseconds(1000); +#endif } } @@ -3022,6 +3029,48 @@ void cliProcess(void) } } +const char * const ownerNames[] = { + "FREE", + "PWM IN", + "PPM IN", + "MOTOR", + "SERVO", + "SOFTSERIAL RX", + "SOFTSERIAL TX", + "SOFTSERIAL RXTX", // bidirectional pin for softserial + "SOFTSERIAL AUXTIMER", // timer channel is used for softserial. No IO function on pin + "ADC", + "SERIAL RX", + "SERIAL TX", + "SERIAL RXTX", + "PINDEBUG", + "TIMER", + "SONAR", + "SYSTEM", + "SDCARD", + "FLASH", + "USB", + "BEEPER", +}; + +static void cliResource(char *cmdline) +{ + UNUSED(cmdline); + cliPrintf("IO:\r\n"); + for (unsigned i = 0; i < DEFIO_IO_USED_COUNT; i++) { + const char* owner; + char buff[15]; + if (ioRecs[i].owner < ARRAYLEN(ownerNames)) { + owner = ownerNames[ioRecs[i].owner]; + } + else { + sprintf(buff, "O=%d", ioRecs[i].owner); + owner = buff; + } + cliPrintf("%c%02d: %19s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner); + } +} + void cliInit(serialConfig_t *serialConfig) { UNUSED(serialConfig); diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 7db0e06942..b7a56e2610 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 16 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 17 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 diff --git a/src/main/main.c b/src/main/main.c index d175872d6a..83690f2a26 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -47,7 +47,6 @@ #include "drivers/pwm_output.h" #include "drivers/adc.h" #include "drivers/bus_i2c.h" -#include "drivers/bus_bst.h" #include "drivers/bus_spi.h" #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" @@ -59,6 +58,10 @@ #include "drivers/io.h" #include "drivers/exti.h" +#ifdef USE_BST +#include "bus_bst.h" +#endif + #include "rx/rx.h" #include "io/beeper.h" @@ -194,7 +197,7 @@ void init(void) EXTIInit(); #endif -#ifdef SPRACINGF3MINI +#if defined(SPRACINGF3MINI) || defined(OMNIBUS) gpio_config_t buttonAGpioConfig = { BUTTON_A_PIN, Mode_IPU, @@ -418,11 +421,13 @@ void init(void) } #endif -#if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1) +#if defined(SPRACINGF3MINI) || defined(OMNIBUS) +#if defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif +#endif #ifdef USE_I2C #if defined(NAZE) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 637ded62b4..e856112948 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -88,47 +88,11 @@ const extiConfig_t *selectMPUIntExtiConfig(void) #if defined(MPU_INT_EXTI) static const extiConfig_t mpuIntExtiConfig = { .io = IO_TAG(MPU_INT_EXTI) }; return &mpuIntExtiConfig; -#endif - -#ifdef NAZE - // MPU_INT output on rev4 PB13 - static const extiConfig_t nazeRev4MPUIntExtiConfig = { - .io = IO_TAG(PB13) - }; - // MPU_INT output on rev5 hardware PC13 - static const extiConfig_t nazeRev5MPUIntExtiConfig = { - .io = IO_TAG(PC13) - }; - -#ifdef AFROMINI - return &nazeRev5MPUIntExtiConfig; +#elif defined(USE_HARDWARE_REVISION_DETECTION) + return selectMPUIntExtiConfigByHardwareRevision(); #else - if (hardwareRevision < NAZE32_REV5) { - return &nazeRev4MPUIntExtiConfig; - } - else { - return &nazeRev5MPUIntExtiConfig; - } -#endif -#endif - -#ifdef ALIENFLIGHTF3 - // MPU_INT output on V1 PA15 - static const extiConfig_t alienFlightF3V1MPUIntExtiConfig = { - .io = IO_TAG(PA15) - }; - // MPU_INT output on V2 PB13 - static const extiConfig_t alienFlightF3V2MPUIntExtiConfig = { - .io = IO_TAG(PB13) - }; - if (hardwareRevision == AFF3_REV_1) { - return &alienFlightF3V1MPUIntExtiConfig; - } - else { - return &alienFlightF3V2MPUIntExtiConfig; - } -#endif return NULL; +#endif } #ifdef USE_FAKE_GYRO diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 6a55389b8f..23bef4dc8c 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -99,7 +99,7 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon return &sonarHardware; // TODO - move sonar pin selection to CLI -#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI) || defined(FURYF3) || defined(RMDO) +#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI) || defined(FURYF3) || defined(RMDO) || defined(OMNIBUS) UNUSED(batteryConfig); static const sonarHardware_t const sonarHardware = { .trigger_pin = Pin_0, // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.c b/src/main/target/ALIENFLIGHTF3/hardware_revision.c index 16a9de5b28..0905b449c5 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.c +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.c @@ -24,7 +24,7 @@ #include "drivers/system.h" #include "drivers/io.h" - +#include "drivers/exti.h" #include "hardware_revision.h" static const char * const hardwareRevisionNames[] = { @@ -56,3 +56,22 @@ void detectHardwareRevision(void) void updateHardwareRevision(void) { } + +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) +{ + // MPU_INT output on V1 PA15 + static const extiConfig_t alienFlightF3V1MPUIntExtiConfig = { + .io = IO_TAG(PA15) + }; + // MPU_INT output on V2 PB13 + static const extiConfig_t alienFlightF3V2MPUIntExtiConfig = { + .io = IO_TAG(PB13) + }; + + if (hardwareRevision == AFF3_REV_1) { + return &alienFlightF3V1MPUIntExtiConfig; + } + else { + return &alienFlightF3V2MPUIntExtiConfig; + } +} \ No newline at end of file diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.h b/src/main/target/ALIENFLIGHTF3/hardware_revision.h index f4c0e90952..7e60ddc55f 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.h +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.h @@ -14,6 +14,9 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ +#pragma once + +#include "drivers/exti.h" typedef enum awf3HardwareRevision_t { UNKNOWN = 0, @@ -25,3 +28,5 @@ extern uint8_t hardwareRevision; void updateHardwareRevision(void); void detectHardwareRevision(void); + +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void); \ No newline at end of file diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index 5af01ce9d8..7b7e6c163d 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -51,33 +52,33 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // 6 x 3 pin headers // - { TIM15, GPIOB, Pin_15, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_1, 0}, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, GPIOB, Pin_14, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource14, GPIO_AF_1, 0}, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6, 0}, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource2, 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, 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 // // 6 pin header // // PWM7-10 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1, 0}, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, GPIOA, Pin_4, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2, 0}, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, 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, 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 // // PPM PORT - Also USART2 RX (AF5) // - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource3, GPIO_AF_1, 0} // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 - //{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, Mode_AF_PP_PD, 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, 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 // USART3 RX/TX // RX conflicts with PPM port - //{ TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM3_IRQn, 0, Mode_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, Mode_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, 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 }; diff --git a/src/main/target/ALIENFLIGHTF3/target.mk b/src/main/target/ALIENFLIGHTF3/target.mk index fc9d28c289..04ba9c48f6 100644 --- a/src/main/target/ALIENFLIGHTF3/target.mk +++ b/src/main/target/ALIENFLIGHTF3/target.mk @@ -8,6 +8,5 @@ TARGET_SRC = \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \ drivers/compass_ak8963.c \ - hardware_revision.c \ drivers/sonar_hcsr04.c diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index 2b90d71750..02efa0b5e6 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -1,6 +1,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -72,19 +73,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource8, GPIO_AF_TIM1, 0}, // PWM1 - PA8 RC1 - { TIM1, GPIOB, Pin_0, TIM_Channel_2, TIM1_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM1, 0}, // PWM2 - PB0 RC2 - { TIM1, GPIOB, Pin_1, TIM_Channel_3, TIM1_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource1, GPIO_AF_TIM1, 0}, // PWM3 - PB1 RC3 - { TIM8, GPIOB, Pin_14,TIM_Channel_2, TIM8_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource14, GPIO_AF_TIM8, 0}, // PWM4 - PA14 RC4 - { TIM8, GPIOB, Pin_15,TIM_Channel_3, TIM8_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource15, GPIO_AF_TIM8, 0}, // PWM5 - PA15 RC5 + { 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 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource8, GPIO_AF_TIM4, 0}, // PWM6 - PB8 OUT1 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource9, GPIO_AF_TIM4, 0}, // PWM7 - PB9 OUT2 - { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5, 0}, // PWM8 - PA0 OUT3 - { TIM5, GPIOA, Pin_1, TIM_Channel_2, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource1, GPIO_AF_TIM5, 0}, // PWM9 - PA1 OUT4 - { TIM3, GPIOC, Pin_6, TIM_Channel_1, TIM3_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource6, GPIO_AF_TIM3, 0}, // PWM10 - PC6 OUT5 - { TIM3, GPIOC, Pin_7, TIM_Channel_2, TIM3_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource7, GPIO_AF_TIM3, 0}, // PWM11 - PC7 OUT6 - { TIM3, GPIOC, Pin_8, TIM_Channel_3, TIM3_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource8, GPIO_AF_TIM3, 0}, // PWM13 - PC8 OUT7 - { TIM3, GPIOC, Pin_9, TIM_Channel_4, TIM3_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource9, GPIO_AF_TIM3, 0}, // PWM13 - PC9 OUT8 + { 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 }; diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index d7f1f4ef10..e7e0aaf82d 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -50,12 +51,12 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, GPIOC, Pin_7, TIM_Channel_2, TIM8_CC_IRQn, 0, Mode_IPD, GPIO_PinSource7, GPIO_AF_TIM8, 0 }, // PPM IN - { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_TIM5, 0 }, // S1_OUT - { TIM5, GPIOA, Pin_1, TIM_Channel_2, TIM5_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_TIM5, 0 }, // S2_OUT - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_TIM2, 0 }, // S3_OUT - { TIM9, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_TIM9, 0 }, // S4_OUT - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_TIM3, 0 }, // S5_OUT - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_TIM3, 0 }, // S6_OUT + { 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 }; diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 5cb81ea900..da0b3921c4 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -146,7 +146,7 @@ #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(5) | TIM_N(8) | TIM_N(9)) #define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM5 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC) diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index db26673d11..b146e6a72b 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -127,18 +128,17 @@ const uint16_t airPWM_BP6[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, Mode_IPD, 0}, // S1_IN - { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_IPD, 0}, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_IPD, 0}, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_IPD, 0}, // S4_IN - Current - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_IPD, 0}, // S5_IN - Vbattery - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_IPD, 0}, // S6_IN - PPM IN - - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, GPIO_Mode_AF_PP, 0}, // S1_OUT - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, GPIO_Mode_AF_PP, 0}, // S2_OUT - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 1, GPIO_Mode_AF_PP, 0}, // S3_OUT - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, GPIO_Mode_AF_PP, 0}, // S4_OUT - { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 1, GPIO_Mode_AF_PP, 0}, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 1, GPIO_Mode_AF_PP, 0} // S6_OUT + { 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 }; diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 9bfabe7fb3..3099e24e73 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -73,25 +74,23 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // INPUTS CH1-8 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_6, 0}, // PWM1 - PA8 - { TIM16, GPIOB, Pin_8, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_1, 0}, // PWM2 - PB8 - { TIM17, GPIOB, Pin_9, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource9, GPIO_AF_1, 0}, // PWM3 - PB9 - { TIM8, GPIOC, Pin_6, TIM_Channel_1, TIM8_CC_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource6, GPIO_AF_4, 0}, // PWM4 - PC6 - { TIM8, GPIOC, Pin_7, TIM_Channel_2, TIM8_CC_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource7, GPIO_AF_4, 0}, // PWM5 - PC7 - { TIM8, GPIOC, Pin_8, TIM_Channel_3, TIM8_CC_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_4, 0}, // PWM6 - PC8 - { TIM15, GPIOF, Pin_9, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource9, GPIO_AF_3, 0}, // PWM7 - PF9 - { TIM15, GPIOF, Pin_10, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource10, GPIO_AF_3, 0}, // PWM8 - PF10 - - // OUTPUTS CH1-10 - { TIM4, GPIOD, Pin_12, TIM_Channel_1, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource12, GPIO_AF_2, 0}, // PWM9 - PD12 - { TIM4, GPIOD, Pin_13, TIM_Channel_2, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource13, GPIO_AF_2, 0}, // PWM10 - PD13 - { TIM4, GPIOD, Pin_14, TIM_Channel_3, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource14, GPIO_AF_2, 0}, // PWM11 - PD14 - { TIM4, GPIOD, Pin_15, TIM_Channel_4, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_2, 0}, // PWM12 - PD15 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // PWM13 - PA1 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_1, 0}, // PWM14 - PA2 - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_1, 0}, // PWM15 - PA3 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // PWM16 - PB0 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM17 - PB1 - { TIM3, GPIOA, Pin_4, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2, 0} // PWM18 - PA4 + { 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 }; diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 29d60c5491..b15505cab4 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -126,9 +126,9 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTD (BIT(2)|BIT(10)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTE 0xffff -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)|BIT(9)|BIT(10)) #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/CJMCU/hardware_revision.c b/src/main/target/CJMCU/hardware_revision.c index f424ee2aff..29071314b8 100755 --- a/src/main/target/CJMCU/hardware_revision.c +++ b/src/main/target/CJMCU/hardware_revision.c @@ -28,6 +28,7 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/accgyro_spi_mpu6500.h" +#include "drivers/exti.h" #include "hardware_revision.h" @@ -51,3 +52,8 @@ void detectHardwareRevision(void) void updateHardwareRevision(void) { } + +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) +{ + return NULL; +} \ No newline at end of file diff --git a/src/main/target/CJMCU/hardware_revision.h b/src/main/target/CJMCU/hardware_revision.h index 4eee9078fa..4b3c13d674 100755 --- a/src/main/target/CJMCU/hardware_revision.h +++ b/src/main/target/CJMCU/hardware_revision.h @@ -14,7 +14,10 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ +#pragma once + #include "drivers/exti.h" + typedef enum cjmcuHardwareRevision_t { UNKNOWN = 0, REV_1, // Blue LED3 @@ -27,3 +30,5 @@ void updateHardwareRevision(void); void detectHardwareRevision(void); void spiBusInit(void); + +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void); \ No newline at end of file diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index c71aad1e7f..b84a104543 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -37,19 +38,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM1 - RC1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM2 - RC2 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM3 - RC3 - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM4 - RC4 - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM5 - RC5 - { TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM6 - RC6 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM7 - RC7 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM8 - RC8 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_IPD, 0}, // PWM9 - OUT1 - { TIM1, GPIOA, Pin_11,TIM_Channel_4, TIM1_CC_IRQn, 1, Mode_IPD, 0}, // PWM10 - OUT2 - { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM11 - OUT3 - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM12 - OUT4 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM13 - OUT5 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, Mode_IPD, 0} // PWM14 - OUT6 + { 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 }; diff --git a/src/main/target/CJMCU/target.mk b/src/main/target/CJMCU/target.mk index 5aa48e4f21..69a1ecc9f4 100644 --- a/src/main/target/CJMCU/target.mk +++ b/src/main/target/CJMCU/target.mk @@ -5,7 +5,6 @@ TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ drivers/compass_hmc5883l.c \ - hardware_revision.c \ flight/gtune.c \ blackbox/blackbox.c \ blackbox/blackbox_io.c diff --git a/src/main/drivers/bus_bst.h b/src/main/target/COLIBRI_RACE/bus_bst.h similarity index 100% rename from src/main/drivers/bus_bst.h rename to src/main/target/COLIBRI_RACE/bus_bst.h diff --git a/src/main/drivers/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c similarity index 99% rename from src/main/drivers/bus_bst_stm32f30x.c rename to src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c index 201985e9e0..8ab8f6cf51 100644 --- a/src/main/drivers/bus_bst_stm32f30x.c +++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c @@ -12,7 +12,7 @@ #include -#include "nvic.h" +#include "drivers/nvic.h" #include "bus_bst.h" diff --git a/src/main/io/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c similarity index 99% rename from src/main/io/i2c_bst.c rename to src/main/target/COLIBRI_RACE/i2c_bst.c index f0891e8e02..25638fb546 100644 --- a/src/main/io/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -69,6 +69,7 @@ #include "hardware_revision.h" #endif +#include "bus_bst.h" #include "i2c_bst.h" void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); diff --git a/src/main/io/i2c_bst.h b/src/main/target/COLIBRI_RACE/i2c_bst.h similarity index 96% rename from src/main/io/i2c_bst.h rename to src/main/target/COLIBRI_RACE/i2c_bst.h index d2678764ec..cf9ee46118 100644 --- a/src/main/io/i2c_bst.h +++ b/src/main/target/COLIBRI_RACE/i2c_bst.h @@ -17,8 +17,6 @@ #pragma once -#include "drivers/bus_bst.h" - void bstProcessInCommand(void); void bstSlaveProcessInCommand(void); void taskBstMasterProcess(void); diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index b6d3dfc23e..d26176e38e 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -66,19 +67,16 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_6, 0}, // PWM1 - PA8 - - { TIM3, GPIOC, Pin_6, TIM_Channel_1, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM2 - PC6 - { TIM3, GPIOC, Pin_7, TIM_Channel_2, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2, 0}, // PWM3 - PC7 - { TIM3, GPIOC, Pin_8, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2, 0}, // PMW4 - PC8 - { TIM3, GPIOC, Pin_9, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2, 0}, // PWM5 - PC9 - - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1, 0}, // PWM6 - PA0 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // PWM7 - PA1 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_1, 0}, // PWM8 - PA2 - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_1, 0}, // PWM9 - PA3 - - { TIM15, GPIOB, Pin_14, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource14, GPIO_AF_1, 0}, // PWM10 - PB14 - { TIM15, GPIOB, Pin_15, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource15, GPIO_AF_1, 0}, // PWM11 - PB15 + { 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 }; diff --git a/src/main/target/COLIBRI_RACE/target.mk b/src/main/target/COLIBRI_RACE/target.mk index 3158000738..b59d32d0e1 100644 --- a/src/main/target/COLIBRI_RACE/target.mk +++ b/src/main/target/COLIBRI_RACE/target.mk @@ -2,8 +2,8 @@ F3_TARGETS += $(TARGET) FEATURES = VCP TARGET_SRC = \ - io/i2c_bst.c \ - drivers/bus_bst_stm32f30x.c \ + i2c_bst.c \ + bus_bst_stm32f30x.c \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6000.c \ diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 4743d6e432..aa34221f91 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, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_6, 0}, // PWM1 - PA8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PWM1 - PA8 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2, 0}, // PWM2 - PB8 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2, 0}, // PWM3 - PB9 + { 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 - { TIM2, GPIOA, Pin_10, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_10, 0}, // PMW4 - PA10 - { TIM2, GPIOA, Pin_9, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_10, 0}, // PWM5 - PA9 - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1, 0}, // PWM6 - PA0 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // PWM7 - PA1 + { 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 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource0, GPIO_AF_2, 0}, // PWM8 - PB1 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM9 - PB0 + { 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 }; diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index 99b0b1003b..b917526d7e 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, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_IPD, 0 }, // PWM1 - RC1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_IPD, 0 }, // PWM2 - RC2 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_IPD, 0 }, // PWM3 - RC3 - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, Mode_IPD, 0 }, // PWM4 - RC4 - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, Mode_IPD, 0 }, // PWM5 - RC5 - { TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, Mode_IPD, 0 }, // PWM6 - RC6 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_IPD, 0 }, // PWM7 - RC7 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_IPD, 0 }, // PWM8 - RC8 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_IPD, 0 }, // PWM9 - OUT1 - { TIM1, GPIOA, Pin_11, TIM_Channel_4, TIM1_CC_IRQn, 1, Mode_IPD, 0 }, // PWM10 - OUT2 - { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, Mode_IPD, 0 }, // PWM11 - OUT3 - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, Mode_IPD, 0 }, // PWM12 - OUT4 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, Mode_IPD, 0 }, // PWM13 - OUT5 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, Mode_IPD, 0 } // PWM14 - OUT6 + { 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 }; diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 1a21ff33f8..5a011433c3 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, GPIOB, Pin_3, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_1, 0}, // PPM IN - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, 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, 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 - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2, 0}, // PWM4 - S1 - { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM5 - S2 - { TIM17, GPIOB, Pin_5, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_10,0}, // PWM6 - S3 - { TIM16, GPIOB, Pin_4, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_1, 0}, // PWM7 - S4 + { 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 + { 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 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, 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/FURYF4/target.c b/src/main/target/FURYF4/target.c index b1a0e82218..b6dbf5fe7f 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -42,12 +42,12 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, GPIOC, Pin_9, TIM_Channel_4, TIM8_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource9, GPIO_AF_TIM8, 0}, // PPM_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PPM_IN - { TIM9, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource3, GPIO_AF_TIM9, 0}, // S1_OUT - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM3, 0}, // S2_OUT - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource1, GPIO_AF_TIM3, 0}, // S3_OUT - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource2, GPIO_AF_TIM2, 0}, // S4_OUT + { 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 // { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5, 0}, // LED Strip }; diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index 828387c1c1..9eaf0c4d9d 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, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, GPIOA, Pin_11, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_10, 0}, // PWM3 - PA11 - { TIM4, GPIOA, Pin_12, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource12, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { 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 }; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 2cac728fcf..30d03b9f69 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, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6, PWM_INVERTED}, - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, PWM_INVERTED}, - { TIM15, GPIOB, Pin_14, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource14, GPIO_AF_1, PWM_INVERTED}, - { TIM15, GPIOB, Pin_15, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_1, PWM_INVERTED}, - { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1, PWM_INVERTED}, - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1, PWM_INVERTED}, + { 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}, - { TIM2, GPIOB, Pin_3, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_1, 0}, - { TIM2, GPIOA, Pin_15, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_1, 0}, - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_1, 0}, - { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1, 0}, - { TIM4, GPIOA, Pin_13, TIM_Channel_2, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource13, GPIO_AF_10,0}, - { TIM8, GPIOA, Pin_14, TIM_Channel_3, TIM8_CC_IRQn, 0, Mode_AF_PP, GPIO_PinSource14, GPIO_AF_5, 0}, + { 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}, }; diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index b6d3dfc23e..7f80590d9c 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -66,19 +66,16 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_6, 0}, // PWM1 - PA8 - - { TIM3, GPIOC, Pin_6, TIM_Channel_1, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM2 - PC6 - { TIM3, GPIOC, Pin_7, TIM_Channel_2, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2, 0}, // PWM3 - PC7 - { TIM3, GPIOC, Pin_8, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2, 0}, // PMW4 - PC8 - { TIM3, GPIOC, Pin_9, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2, 0}, // PWM5 - PC9 - - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1, 0}, // PWM6 - PA0 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // PWM7 - PA1 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_1, 0}, // PWM8 - PA2 - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_1, 0}, // PWM9 - PA3 - - { TIM15, GPIOB, Pin_14, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource14, GPIO_AF_1, 0}, // PWM10 - PB14 - { TIM15, GPIOB, Pin_15, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource15, GPIO_AF_1, 0}, // PWM11 - PB15 + { 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 }; diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index dab944eaca..514839eada 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -42,15 +42,14 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, GPIOA, Pin_4, TIM_Channel_2, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2}, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1}, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_1}, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource7, GPIO_AF_1}, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { 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 }; diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c index dae3698442..d9f540eb9f 100755 --- a/src/main/target/NAZE/hardware_revision.c +++ b/src/main/target/NAZE/hardware_revision.c @@ -26,6 +26,7 @@ #include "drivers/system.h" #include "drivers/bus_spi.h" #include "drivers/sensor.h" +#include "drivers/io.h" #include "drivers/exti.h" #include "drivers/accgyro.h" #include "drivers/accgyro_mpu.h" @@ -109,3 +110,26 @@ void updateHardwareRevision(void) hardwareRevision = NAZE32_SP; #endif } + +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) +{ + // MPU_INT output on rev4 PB13 + static const extiConfig_t nazeRev4MPUIntExtiConfig = { + .io = IO_TAG(PB13) + }; + // MPU_INT output on rev5 hardware PC13 + static const extiConfig_t nazeRev5MPUIntExtiConfig = { + .io = IO_TAG(PC13) + }; + +#ifdef AFROMINI + return &nazeRev5MPUIntExtiConfig; +#else + if (hardwareRevision < NAZE32_REV5) { + return &nazeRev4MPUIntExtiConfig; + } + else { + return &nazeRev5MPUIntExtiConfig; + } +#endif +} diff --git a/src/main/target/NAZE/hardware_revision.h b/src/main/target/NAZE/hardware_revision.h index 9f663bb6c2..08895620f0 100755 --- a/src/main/target/NAZE/hardware_revision.h +++ b/src/main/target/NAZE/hardware_revision.h @@ -14,6 +14,9 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ +#pragma once + +#include "drivers/exti.h" typedef enum nazeHardwareRevision_t { UNKNOWN = 0, @@ -28,3 +31,5 @@ void updateHardwareRevision(void); void detectHardwareRevision(void); void spiBusInit(void); + +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void); diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index be1c6ff53d..83a8862aa8 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, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM1 - RC1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM2 - RC2 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM3 - RC3 - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM4 - RC4 - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM5 - RC5 - { TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM6 - RC6 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM7 - RC7 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM8 - RC8 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_IPD, 0}, // PWM9 - OUT1 - { TIM1, GPIOA, Pin_11,TIM_Channel_4, TIM1_CC_IRQn, 1, Mode_IPD, 0}, // PWM10 - OUT2 - { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM11 - OUT3 - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM12 - OUT4 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM13 - OUT5 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, Mode_IPD, 0} // PWM14 - OUT6 + { 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 }; diff --git a/src/main/target/NAZE/target.mk b/src/main/target/NAZE/target.mk index 45d87e4b78..07d81cf447 100644 --- a/src/main/target/NAZE/target.mk +++ b/src/main/target/NAZE/target.mk @@ -17,5 +17,4 @@ TARGET_SRC = \ drivers/compass_hmc5883l.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f10x.c \ - drivers/sonar_hcsr04.c \ - hardware_revision.c + drivers/sonar_hcsr04.c diff --git a/src/main/target/NAZE32PRO/target.c b/src/main/target/NAZE32PRO/target.c index 425ca38c41..5c8baef256 100644 --- a/src/main/target/NAZE32PRO/target.c +++ b/src/main/target/NAZE32PRO/target.c @@ -72,20 +72,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_6, 0}, // PA8 - AF6 - { TIM1, GPIOA, Pin_9, TIM_Channel_2, TIM1_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource9, GPIO_AF_6, 0}, // PA9 - AF6 - { TIM1, GPIOA, Pin_10, TIM_Channel_3, TIM1_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource10, GPIO_AF_6, 0}, // PA10 - AF6 - { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource4, GPIO_AF_2, 0}, // PB4 - AF2 - { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource6, GPIO_AF_2, 0}, // PB6 - AF2 - not working yet - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource7, GPIO_AF_2, 0}, // PB7 - AF2 - not working yet - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_2, 0}, // PB8 - AF2 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource9, GPIO_AF_2, 0}, // PB9 - AF2 - - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // PA0 - untested - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // PA1 - untested - { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9, 0}, // PA2 - untested - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9, 0}, // PA3 - untested - { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1, 0}, // PA6 - untested - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1, 0} // PA7 - untested + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PA8 - AF6 + { TIM1, IO_TAG(PA9), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PA9 - AF6 + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6, 0}, // PA10 - AF6 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PB4 - AF2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PB6 - AF2 - not working yet + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PB7 - AF2 - not working yet + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PB8 - AF2 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, 0}, // PB9 - AF2 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PA0 - untested + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PA1 - untested + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PA2 - untested + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PA3 - untested + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PA6 - untested + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0} // PA7 - untested }; diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c index defeac77ef..72b22c8db1 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -70,20 +70,21 @@ const uint16_t airPWM[] = { PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 0xFFFF }; + const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM1 - RC1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM2 - RC2 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM3 - RC3 - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM4 - RC4 - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM5 - RC5 - { TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM6 - RC6 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM7 - RC7 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM8 - RC8 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_IPD, 0}, // PWM9 - OUT1 - { TIM1, GPIOA, Pin_11,TIM_Channel_4, TIM1_CC_IRQn, 1, Mode_IPD, 0}, // PWM10 - OUT2 - { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM11 - OUT3 - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM12 - OUT4 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM13 - OUT5 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, Mode_IPD, 0} // PWM14 - OUT6 + { 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 }; diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c new file mode 100644 index 0000000000..1784dcf51b --- /dev/null +++ b/src/main/target/OMNIBUS/target.c @@ -0,0 +1,89 @@ + +#include +#include + +#include +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (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), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (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), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + 0xFFFF +}; + +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 + // 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 + + { 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 + + // 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) + + // 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 +}; diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h new file mode 100644 index 0000000000..756f7fbb82 --- /dev/null +++ b/src/main/target/OMNIBUS/target.h @@ -0,0 +1,231 @@ +/* + * 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 "OMNI" // https://en.wikipedia.org/wiki/Omnibus + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +#define LED0 PB3 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 10 // 6 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. + +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#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_FAKE_GYRO +#define USE_GYRO_MPU6500 + +#define ACC +//#define USE_FAKE_ACC +#define USE_ACC_MPU6500 + +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG + +#define BARO +#define USE_BARO_BMP280 + +#define MAG +#define USE_MPU9250_MAG // Enables bypass configuration +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 // External + +#define MAG_AK8975_ALIGN CW90_DEG_FLIP + +#define SONAR + +#define USB_IO +#define USB_CABLE_DETECTION + +#define USB_DETECT_PIN PB5 + +#define USE_VCP +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define USE_SOFTSERIAL1 +#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_14 // PA14 / SWCLK +#define UART2_RX_PIN GPIO_Pin_15 // PA15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource14 +#define UART2_RX_PINSOURCE GPIO_PinSource15 + +#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 TIM2 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 9 // PA0 / PAD3 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 10 // PA1 / PAD4 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + + +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MAX_OSD +#define MAX_OSD_SPI_INSTANCE SPI1 +#define MAX_OSD_SPI_CS_PIN SPI1_NSS_PIN + + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + + +#define ADC_INSTANCE ADC2 +#define ADC_DMA_CHANNEL DMA2_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_1 + +#define CURRENT_METER_ADC_GPIO GPIOA +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 + +#define RSSI_ADC_GPIO GPIOB +#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_CHANNEL ADC_Channel_12 + +#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_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 TRANSPONDER +//#define TRANSPONDER_GPIO GPIOA +//#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +//#define TRANSPONDER_GPIO_AF GPIO_AF_6 +//#define TRANSPONDER_PIN GPIO_Pin_8 +//#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 +//#define TRANSPONDER_TIMER TIM1 +//#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +//#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 +//#define TRANSPONDER_IRQ DMA1_Channel2_IRQn +//#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 +//#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +//#define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM + +#define BUTTONS +#define BUTTON_A_PORT GPIOB +#define BUTTON_A_PIN Pin_1 +#define BUTTON_B_PORT GPIOB +#define BUTTON_B_PIN Pin_0 + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define HARDWARE_BIND_PLUG +#define BINDPLUG_PIN PB0 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#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(4)) + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk new file mode 100644 index 0000000000..435a35a196 --- /dev/null +++ b/src/main/target/OMNIBUS/target.mk @@ -0,0 +1,16 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD OSD + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/serial_usb_vcp.c \ + drivers/sonar_hcsr04.c + diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c new file mode 100644 index 0000000000..514839eada --- /dev/null +++ b/src/main/target/PIKOBLX/target.c @@ -0,0 +1,55 @@ + +#include +#include + +#include +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM1 | (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_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (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), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + // TODO + 0xFFFF +}; + +const uint16_t airPWM[] = { + // TODO + 0xFFFF +}; + +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 +}; + diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h new file mode 100644 index 0000000000..b15f0202ec --- /dev/null +++ b/src/main/target/PIKOBLX/target.h @@ -0,0 +1,173 @@ +/* + * 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 "PIKO" // Furious FPV Piko BLX +#define USE_CLI + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT + +#define LED0 PB9 +#define LED1 PB5 + +#define BEEPER PA0 +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 9 + +// MPU6000 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PA15 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready +#define USE_MPU_DATA_READY_SIGNAL + +#define GYRO +#define ACC + +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW180_DEG +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + +#define MPU6000_CS_GPIO GPIOB +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 + +#define USE_VCP +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN GPIO_Pin_6 // PB6 +#define UART1_RX_PIN GPIO_Pin_7 // PB7 +#define UART1_GPIO GPIOB +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource6 +#define UART1_RX_PINSOURCE GPIO_PinSource7 + +#define UART2_TX_PIN GPIO_Pin_3 // PB3 +#define UART2_RX_PIN GPIO_Pin_4 // PB4 +#define UART2_GPIO GPIOB +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource3 +#define UART2_RX_PINSOURCE GPIO_PinSource4 + +#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 + +#define USE_SPI +#define USE_SPI_DEVICE_2 + +#define SENSORS_SET (SENSOR_ACC) + +#define TELEMETRY +#define BLACKBOX +#define SERIAL_RX +#define USE_SERVOS + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + +#define ADC_INSTANCE ADC2 +#define ADC_DMA_CHANNEL DMA2_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 + +#define CURRENT_METER_ADC_GPIO GPIOA +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_2 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_3 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_5 +#define VBAT_ADC_CHANNEL ADC_Channel_2 + +#define RSSI_ADC_GPIO GPIOB +#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_CHANNEL ADC_Channel_12 + +#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_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 + +#if 0 +// Alternate LED strip pin +// FIXME DMA IRQ Transfer Complete is never called because the TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1 +#define LED_STRIP_TIMER TIM17 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL7 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_7 // TIM17_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource7 +#define WS2811_TIMER TIM17 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 +#define WS2811_DMA_CHANNEL DMA1_Channel7 +#define WS2811_IRQ DMA1_Channel7_IRQn +#endif + +#define TRANSPONDER +#define TRANSPONDER_GPIO GPIOA +#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define TRANSPONDER_GPIO_AF GPIO_AF_6 +#define TRANSPONDER_PIN GPIO_Pin_8 +#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 +#define TRANSPONDER_TIMER TIM1 +#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 +#define TRANSPONDER_IRQ DMA1_Channel2_IRQn +#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 +#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define SPEKTRUM_BIND +// USART3, PB11 +#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)) +// !!TODO - check the following line is correct +#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(15) | TIM_N(17)) + +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM17) +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/PIKOBLX/target.mk b/src/main/target/PIKOBLX/target.mk new file mode 100644 index 0000000000..3662ea997a --- /dev/null +++ b/src/main/target/PIKOBLX/target.mk @@ -0,0 +1,13 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/transponder_ir.c \ + drivers/transponder_ir_stm32f30x.c \ + io/transponder_ir.c + diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 36728e8c59..e014e430d1 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, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM1 - RC1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM2 - RC2 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM3 - RC3 - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, Mode_IPD, 0}, // PWM4 - RC4 - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM5 - RC5 - { TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM6 - RC6 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM7 - RC7 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_IPD, 0}, // PWM8 - RC8 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_IPD, 0}, // PWM9 - OUT1 - { TIM1, GPIOA, Pin_11,TIM_Channel_4, TIM1_CC_IRQn, 1, Mode_IPD, 0}, // PWM10 - OUT2 - { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM11 - OUT3 - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM12 - OUT4 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, Mode_IPD, 0}, // PWM13 - OUT5 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, Mode_IPD, 0} // PWM14 - OUT6 + { 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 }; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 7e93d4d16f..af6b8eb1e8 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, GPIOB, Pin_14, TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource14, GPIO_AF_TIM12, 0}, // PPM (5th pin on FlexiIO port) - { TIM12, GPIOB, Pin_15, TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource15, GPIO_AF_TIM12, 0}, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, GPIOC, Pin_6, TIM_Channel_1, TIM8_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource6, GPIO_AF_TIM8, 0}, // S3_IN - { TIM8, GPIOC, Pin_7, TIM_Channel_2, TIM8_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource7, GPIO_AF_TIM8, 0}, // S4_IN - { TIM8, GPIOC, Pin_8, TIM_Channel_3, TIM8_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource8, GPIO_AF_TIM8, 0}, // S5_IN - { TIM8, GPIOC, Pin_9, TIM_Channel_4, TIM8_CC_IRQn, 0, GPIO_Mode_AF, GPIO_PinSource9, GPIO_AF_TIM8, 0}, // S6_IN - - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM3, 0}, // S1_OUT - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource1, GPIO_AF_TIM3, 0}, // S2_OUT - { TIM9, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource3, GPIO_AF_TIM9, 0}, // S3_OUT - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource2, GPIO_AF_TIM2, 0}, // S4_OUT - { TIM5, GPIOA, Pin_1, TIM_Channel_2, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource1, GPIO_AF_TIM5, 0}, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5, 0}, // S6_OUT + { 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 + + { 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 }; diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index fea9723b25..9eaf0c4d9d 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, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1 ,0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1 ,0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1 ,0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1 ,0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2 ,0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2 ,0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2 ,0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2 ,0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1 ,0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1 ,0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, GPIOA, Pin_11, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_10 ,0}, // PWM3 - PA11 - { TIM4, GPIOA, Pin_12, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource12, GPIO_AF_10 ,0}, // PWM4 - PA12 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2 ,0}, // PWM5 - PB8 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2 ,0}, // PWM6 - PB9 - { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9 ,0}, // PWM7 - PA2 - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9 ,0}, // PWM8 - PA3 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6 ,0}, // GPIO_TIMER / LED_STRIP + { 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 }; diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 137e6bdeb6..fe6106ec9e 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -60,18 +60,15 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, GPIOA, Pin_15, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_1, 0}, // PPM/SERIAL RX - - { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2, 0}, // PWM1 - { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2, 0}, // PWM2 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // PWM3 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM4 - { TIM16, GPIOB, Pin_8, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_1, 0}, // PWM5 - { TIM17, GPIOB, Pin_9, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_1, 0}, // PWM6 - - { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9, 0}, // SOFTSERIAL1 RX (NC) - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9, 0}, // SOFTSERIAL1 TX - - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6, 0}, // LED_STRIP + { 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 }; diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 6efc5c17e4..151c7f138a 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -1,5 +1,5 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD MAX_OSD +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD OSD TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 76d27d777a..2b27f994f6 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -47,38 +47,16 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - // - // 6 x 3 pin headers - // - - { TIM15, GPIOB, Pin_15, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_1, 0}, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, GPIOB, Pin_14, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource14, GPIO_AF_1, 0}, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6, 0}, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_1, 0}, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - - // - // 6 pin header - // - - // PWM7-10 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1, 0}, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, GPIOA, Pin_4, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2, 0}, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N - - // - // PPM PORT - Also USART2 RX (AF5) - // - - { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource3, GPIO_AF_1, 0} // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 - //{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource3, GPIO_AF_9, 0} // 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, Mode_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, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1, 0} // TX - PB10 - *TIM2_CH3, USART3_TX (AF7) - PWM12 - + { 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 + { 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 + { 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 }; diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index 5c68f9a98b..829d7ef003 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -81,25 +82,25 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, 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, 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, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, 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, 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, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, GPIOA, Pin_11, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_10, 0}, // PWM3 - PA11 - { TIM4, GPIOA, Pin_12, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource12, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9, 0}, // PWM8 - PA3 + { 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, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, 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/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 81cc1ed06a..1b601e8818 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -66,22 +67,17 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, GPIOA, Pin_15, TIM_Channel_1, TIM8_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource15, GPIO_AF_2, 0}, // PPM - - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1, 0}, // PWM1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // PWM2 - { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9, 0}, // PWM3 - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9, 0}, // PWM4 - { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM5 - { TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2, 0}, // PWM6 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // PWM7 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM8 - - // UART3 RX/TX - { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - - // IR / LED Strip Pad - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, 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, 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 }; diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index 5ef0c6abb5..daa8bbe8c8 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -67,27 +68,27 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2, 0}, // PPM - PB5 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2, 0}, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1, 0}, // PWM1 - PA6 - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1, 0}, // PWM2 - PA7 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2, 0}, // PWM3 - PB8 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2, 0}, // PWM4 - PB9 - { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9, 0}, // PWM5 - PA2 - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9, 0}, // PWM6 - PA3 - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1, 0}, // PWM7 - PA0 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // PWM8 - PA1 + { 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 // UART3 RX/TX - { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1, 0}, // PWM9 - PB10 - TIM2_CH3 / USART3_TX (AF7) - { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, 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, 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) // LED Strip Pad - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, 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/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 8d7f6173b0..1d2e615ead 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -3,6 +3,7 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -72,19 +73,19 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_6, 0}, // PWM1 - PA8 - { TIM16, GPIOB, Pin_8, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_1, 0}, // PWM2 - PB8 - { TIM17, GPIOB, Pin_9, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource9, GPIO_AF_1, 0}, // PWM3 - PB9 - { TIM8, GPIOC, Pin_6, TIM_Channel_1, TIM8_CC_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource6, GPIO_AF_4, 0}, // PWM4 - PC6 - { TIM8, GPIOC, Pin_7, TIM_Channel_2, TIM8_CC_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource7, GPIO_AF_4, 0}, // PWM5 - PC7 - { TIM8, GPIOC, Pin_8, TIM_Channel_3, TIM8_CC_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_4, 0}, // PWM6 - PC8 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM7 - PB1 - { TIM3, GPIOA, Pin_4, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource4, GPIO_AF_2, 0}, // PWM8 - PA2 - { TIM4, GPIOD, Pin_12, TIM_Channel_1, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource12, GPIO_AF_2, 0}, // PWM9 - PD12 - { TIM4, GPIOD, Pin_13, TIM_Channel_2, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource13, GPIO_AF_2, 0}, // PWM10 - PD13 - { TIM4, GPIOD, Pin_14, TIM_Channel_3, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource14, GPIO_AF_2, 0}, // PWM11 - PD14 - { TIM4, GPIOD, Pin_15, TIM_Channel_4, TIM4_IRQn, 0, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_2, 0}, // PWM12 - PD15 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1, 0}, // PWM13 - PA1 - { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_1, 0} // PWM14 - PA2 + { 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 }; diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 1295b8edf4..e54b919189 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -98,6 +98,14 @@ #define USE_USART2 #define SERIAL_PORT_COUNT 3 +// uart2 gpio for shared serial rx/ppm +//#define UART2_TX_PIN GPIO_Pin_5 // PD5 +//#define UART2_RX_PIN GPIO_Pin_6 // PD6 +//#define UART2_GPIO GPIOD +//#define UART2_GPIO_AF GPIO_AF_7 +//#define UART2_TX_PINSOURCE GPIO_PinSource5 +//#define UART2_RX_PINSOURCE GPIO_PinSource6 + #define USE_I2C #define I2C_DEVICE (I2CDEV_1) diff --git a/src/main/target/STM32F3DISCOVERYMAX7456/target.c b/src/main/target/STM32F3DISCOVERYMAX7456/target.c new file mode 100644 index 0000000000..ab538410ff --- /dev/null +++ b/src/main/target/STM32F3DISCOVERYMAX7456/target.c @@ -0,0 +1,90 @@ + +#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), // 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 + 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), // 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 + 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), // 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 + 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), // servo #4 + 0xFFFF +}; + +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 +}; + diff --git a/src/main/target/STM32F3DISCOVERYMAX7456/target.h b/src/main/target/STM32F3DISCOVERYMAX7456/target.h new file mode 100644 index 0000000000..4a67680a6c --- /dev/null +++ b/src/main/target/STM32F3DISCOVERYMAX7456/target.h @@ -0,0 +1,196 @@ +/* + * Supports the GY-91 MPU9250 and BMP280 development board via SPI1 + * + * Put the MAX7456 on SPI2 instead of an SDCARD + * MAX7456 CS -> PB12 (default) + * Uses the default pins for SPI2: + * #define SPI2_NSS_PIN PB12 + * #define SPI2_SCK_PIN PB13 + * #define SPI2_MISO_PIN PB14 + * #define SPI2_MOSI_PIN PB15 + * + * @author Nathan Tsoi + * + * This software 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. + * + * This software 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 this software. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "DF3M" // stm32f3 DiscoveryF3 Max7456 + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +#define LED0 PE8 // Blue LEDs - PE8/PE12 +#define LED0_INVERTED +#define LED1 PE10 // Orange LEDs - PE10/PE14 +#define LED1_INVERTED + +#define BEEPER PE9 // Red LEDs - PE9/PE13 +#define BEEPER_INVERTED + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +//#define USE_SD_CARD +// +//#define SD_DETECT_PIN PC14 +//#define SD_CS_PIN PB12 +//#define SD_SPI_INSTANCE SPI2 + +//#define USE_FLASHFS +//#define USE_FLASH_M25P16 + +//#define M25P16_CS_GPIO GPIOB +//#define M25P16_CS_PIN GPIO_Pin_12 +//#define M25P16_SPI_INSTANCE SPI2 +// SPI1 +// PB5 SPI1_MOSI +// PB4 SPI1_MISO +// PB3 SPI1_SCK +// PA15 SPI1_NSS + +// SPI2 +// PB15 SPI2_MOSI +// PB14 SPI2_MISO +// PB13 SPI2_SCK +// PB12 SPI2_NSS + +#define GYRO + +#define USE_GYRO_L3GD20 +#define L3GD20_SPI SPI1 +#define L3GD20_CS_PIN PE3 +#define GYRO_L3GD20_ALIGN CW270_DEG + +// Support the GY-91 MPU9250 dev board +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 +#define MPU6500_CS_PIN PC14 +#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP + +#define ACC +#define USE_ACC_LSM303DLHC +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG_FLIP + +//#define BARO +//#define BMP280_CS_PIN PB12 +//#define BMP280_SPI_INSTANCE SPI2 +//#define USE_BARO_BMP280 +//#define USE_BARO_SPI_BMP280 + +#define MAX_OSD +#define MAX_OSD_SPI_INSTANCE SPI2 +#define MAX_OSD_SPI_CS_PIN SPI2_NSS_PIN + +//#define USE_SDCARD +//#define USE_SDCARD_SPI2 +// +//#define SDCARD_SPI_INSTANCE SPI2 +//#define SDCARD_SPI_CS_PIN PB12 +//// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +//#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +//// Divide to under 25MHz for normal operation: +//#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 +// +//// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +//#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +//#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + +#define MAG +#define USE_MAG_HMC5883 + +#define USE_VCP +#define USE_USART1 +#define USE_USART2 +#define SERIAL_PORT_COUNT 3 + +// uart2 gpio for shared serial rx/ppm +//#define UART2_TX_PIN GPIO_Pin_5 // PD5 +//#define UART2_RX_PIN GPIO_Pin_6 // PD6 +//#define UART2_GPIO GPIOD +//#define UART2_GPIO_AF GPIO_AF_7 +//#define UART2_TX_PINSOURCE GPIO_PinSource5 +//#define UART2_RX_PINSOURCE GPIO_PinSource6 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) + +#define USE_ADC + +#define ADC_INSTANCE ADC1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 +#define ADC_DMA_CHANNEL DMA1_Channel1 + +#define VBAT_ADC_GPIO GPIOC +#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_CHANNEL ADC_Channel_6 + +#define CURRENT_METER_ADC_GPIO GPIOC +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 + +#define RSSI_ADC_GPIO GPIOC +#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_CHANNEL ADC_Channel_8 + +#define EXTERNAL1_ADC_GPIO GPIOC +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 + +#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_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 + +#define LED_STRIP +#define LED_STRIP_TIMER TIM16 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - 303 in 100pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0x00ff + + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB | RCC_AHBPeriph_GPIOC | RCC_AHBPeriph_GPIOD) + diff --git a/src/main/target/STM32F3DISCOVERYMAX7456/target.mk b/src/main/target/STM32F3DISCOVERYMAX7456/target.mk new file mode 100644 index 0000000000..c8ddcc7ddd --- /dev/null +++ b/src/main/target/STM32F3DISCOVERYMAX7456/target.mk @@ -0,0 +1,18 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP MAX_OSD + +TARGET_SRC = \ + drivers/light_ws2811strip.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_l3gd20.c \ + drivers/accgyro_lsm303dlhc.c \ + drivers/compass_hmc5883l.c \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/accgyro_l3g4200d.c \ + drivers/barometer_ms5611.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8975.c diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index 95c21755c8..7944809cfa 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/src/test/unit/scheduler_unittest.cc @@ -39,6 +39,7 @@ enum { updateSonarTime = 10, calculateAltitudeTime = 154, updateDisplayTime = 10, + updateMaxOSDTime = 10, telemetryTime = 10, ledStripTime = 10, transponderTime = 10 @@ -68,6 +69,7 @@ extern "C" { void taskUpdateSonar(void) {simulatedTime+=updateSonarTime;} void taskCalculateAltitude(void) {simulatedTime+=calculateAltitudeTime;} void taskUpdateDisplay(void) {simulatedTime+=updateDisplayTime;} + void taskUpdateMaxOSD(void) {simulatedTime+=updateMaxOSDTime;} void taskTelemetry(void) {simulatedTime+=telemetryTime;} void taskLedStrip(void) {simulatedTime+=ledStripTime;} void taskTransponder(void) {simulatedTime+=transponderTime;} diff --git a/support/openocd/stm32f3.cfg b/support/openocd/stm32f3.cfg new file mode 100644 index 0000000000..d04cad7034 --- /dev/null +++ b/support/openocd/stm32f3.cfg @@ -0,0 +1,10 @@ +# This is an STM32F3 discovery board with a single STM32F303VCT6 chip. +# http://www.st.com/internet/evalboard/product/254044.jsp + +source [find interface/stlink-v2.cfg] + +transport select hla_swd + +source [find target/stm32f3x.cfg] + +reset_config none separate From cd5335ddec421b4aa921c26fc02fb400dc08867f Mon Sep 17 00:00:00 2001 From: nathan Date: Wed, 15 Jun 2016 20:56:19 -0700 Subject: [PATCH 20/38] max7456 driver io tags, moved some sirin #defines out (they're in common now) --- src/main/drivers/max7456.c | 12 ++++++++++-- src/main/target/SIRINFPV/target.h | 9 ++------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 57874fbf31..06af3ccb23 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -31,8 +31,10 @@ #include "max7456.h" -#define DISABLE_MAX7456 GPIO_SetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) -#define ENABLE_MAX7456 GPIO_ResetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) +#define DISABLE_MAX7456 IOHi(max7456CsPin) +#define ENABLE_MAX7456 IOLo(max7456CsPin) + +static IO_t max7456CsPin = IO_NONE; /** PAL or NTSC, value is number of chars total */ #define VIDEO_MODE_PIXELS_NTSC 390 @@ -57,6 +59,12 @@ void max7456_init(uint8_t system) { uint16_t x; char buf[30]; +#ifdef MAX7456_SPI_CS_PIN + max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN)); +#endif + IOInit(max7456CsPin, OWNER_SYSTEM, RESOURCE_SPI); + IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); + //Minimum spi clock period for max7456 is 100ns (10Mhz) spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 0fb5ac2fdb..7b98c90619 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -109,9 +109,8 @@ #define SPI3_MOSI_PIN PB5 #define USE_MAX7456 -#define MAX7456_CS_GPIO GPIOA -#define MAX7456_CS_PIN GPIO_Pin_15 -#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 #define USE_RTC6705 #define RTC6705_SPIDATA_GPIO GPIOC @@ -155,11 +154,7 @@ #define VBAT_ADC_CHANNEL ADC_Channel_1 //#define USE_QUAD_MIXER_ONLY -#define BLACKBOX #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define TELEMETRY -#define SERIAL_RX -#define USE_CLI #define OSD #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT From f6ca9de8e1c88923424719cb3ec55de25d76a562 Mon Sep 17 00:00:00 2001 From: nathan Date: Wed, 15 Jun 2016 21:01:29 -0700 Subject: [PATCH 21/38] clean up #defines and comment --- src/main/drivers/max7456_symbols.h | 4 ++-- src/main/target/OMNIBUS/target.h | 11 ++++++++--- src/main/target/OMNIBUS/target.mk | 2 +- src/main/target/SIRINFPV/target.mk | 2 +- src/main/target/STM32F3DISCOVERYMAX7456/target.h | 7 ++++--- 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index 2d7862ebd9..e2f633b8cf 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -21,7 +21,7 @@ #pragma once -#ifdef MAX_OSD +#ifdef USE_MAX7456 // Character Symbols #define SYM_BLANK 0X20 @@ -230,4 +230,4 @@ #define SYM_MIN 0xB3 #define SYM_AVG 0xB4 -#endif //MAX_OSD +#endif diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 756f7fbb82..228d843fa3 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -115,9 +115,14 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define MAX_OSD -#define MAX_OSD_SPI_INSTANCE SPI1 -#define MAX_OSD_SPI_CS_PIN SPI1_NSS_PIN +// OSD define info: +// feature name (includes source) -> MAX_OSD, used in target.mk +// include the osd code +#define OSD +// include the max7456 driver +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI1 +#define MAX7456_SPI_CS_PIN SPI1_NSS_PIN #define USE_SPI diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 435a35a196..8259bfa925 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD OSD +FEATURES = VCP SDCARD MAX_OSD TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 151c7f138a..1dfe1ea325 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD OSD +FEATURES = VCP SDCARD MAX_OSD TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/main/target/STM32F3DISCOVERYMAX7456/target.h b/src/main/target/STM32F3DISCOVERYMAX7456/target.h index 4a67680a6c..8fdef97b44 100644 --- a/src/main/target/STM32F3DISCOVERYMAX7456/target.h +++ b/src/main/target/STM32F3DISCOVERYMAX7456/target.h @@ -98,9 +98,10 @@ //#define USE_BARO_BMP280 //#define USE_BARO_SPI_BMP280 -#define MAX_OSD -#define MAX_OSD_SPI_INSTANCE SPI2 -#define MAX_OSD_SPI_CS_PIN SPI2_NSS_PIN +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN //#define USE_SDCARD //#define USE_SDCARD_SPI2 From 437ca5025b95dc8a95e6d616ba2c61c2a657d835 Mon Sep 17 00:00:00 2001 From: nathan Date: Wed, 15 Jun 2016 23:38:33 -0700 Subject: [PATCH 22/38] use the mw/scarab-osd font symbols --- src/main/drivers/max7456_symbols.h | 224 ++++++++++++++--------------- src/main/io/osd.c | 17 ++- 2 files changed, 124 insertions(+), 117 deletions(-) diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index e2f633b8cf..68de428937 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -1,5 +1,5 @@ /* @file max7456_symbols.h - * @brief max7456 preprocessor symbols + * @brief max7456 symbols for the mwosd font set * * @author Nathan Tsoi nathan@vertile.com * @@ -24,41 +24,41 @@ #ifdef USE_MAX7456 // Character Symbols -#define SYM_BLANK 0X20 +#define SYM_BLANK 0x20 // Satellite Graphics -#define SYM_SAT_L 0X1E -#define SYM_SAT_R 0X1F -//#define SYM_SAT 0X0F // Not used +#define SYM_SAT_L 0x1E +#define SYM_SAT_R 0x1F +//#define SYM_SAT 0x0F // Not used // Degrees Icon for HEADING/DIRECTION HOME -#define SYM_DEGREES 0XBD +#define SYM_DEGREES 0xBD // Direction arrows -#define SYM_ARROW_SOUTH 0X60 -#define SYM_ARROW_2 0X61 -#define SYM_ARROW_3 0X62 -#define SYM_ARROW_4 0X63 -#define SYM_ARROW_EAST 0X64 -#define SYM_ARROW_6 0X65 -#define SYM_ARROW_7 0X66 -#define SYM_ARROW_8 0X67 -#define SYM_ARROW_NORTH 0X68 -#define SYM_ARROW_10 0X69 -#define SYM_ARROW_11 0X6A -#define SYM_ARROW_12 0X6B -#define SYM_ARROW_WEST 0X6C -#define SYM_ARROW_14 0X6D -#define SYM_ARROW_15 0X6E -#define SYM_ARROW_16 0X6F +#define SYM_ARROW_SOUTH 0x60 +#define SYM_ARROW_2 0x61 +#define SYM_ARROW_3 0x62 +#define SYM_ARROW_4 0x63 +#define SYM_ARROW_EAST 0x64 +#define SYM_ARROW_6 0x65 +#define SYM_ARROW_7 0x66 +#define SYM_ARROW_8 0x67 +#define SYM_ARROW_NORTH 0x68 +#define SYM_ARROW_10 0x69 +#define SYM_ARROW_11 0x6A +#define SYM_ARROW_12 0x6B +#define SYM_ARROW_WEST 0x6C +#define SYM_ARROW_14 0x6D +#define SYM_ARROW_15 0x6E +#define SYM_ARROW_16 0x6F // Heading Graphics -#define SYM_HEADING_N 0X18 -#define SYM_HEADING_S 0X19 -#define SYM_HEADING_E 0X1A -#define SYM_HEADING_W 0X1B -#define SYM_HEADING_DIVIDED_LINE 0X1C -#define SYM_HEADING_LINE 0X1D +#define SYM_HEADING_N 0x18 +#define SYM_HEADING_S 0x19 +#define SYM_HEADING_E 0x1A +#define SYM_HEADING_W 0x1B +#define SYM_HEADING_DIVIDED_LINE 0x1C +#define SYM_HEADING_LINE 0x1D // FRSKY HUB #define SYM_CELL0 0xF0 @@ -80,7 +80,7 @@ // Map mode #define SYM_HOME 0x04 -#define SYM_AIRCRAFT 0X05 +#define SYM_AIRCRAFT 0x05 #define SYM_RANGE_100 0x21 #define SYM_RANGE_500 0x22 #define SYM_RANGE_2500 0x23 @@ -89,56 +89,56 @@ // GPS Coordinates and Altitude #define SYM_LAT 0xCA -#define SYM_LON 0XCB -#define SYM_ALT 0XCC +#define SYM_LON 0xCB +#define SYM_ALT 0xCC // GPS Mode and Autopilot -#define SYM_3DFIX 0XDF -#define SYM_HOLD 0XEF -#define SYM_G_HOME 0XFF -#define SYM_GHOME 0X9D -#define SYM_GHOME1 0X9E -#define SYM_GHOLD 0XCD -#define SYM_GHOLD1 0XCE -#define SYM_GMISSION 0XB5 -#define SYM_GMISSION1 0XB6 -#define SYM_GLAND 0XB7 -#define SYM_GLAND1 0XB8 +#define SYM_3DFIX 0xDF +#define SYM_HOLD 0xEF +#define SYM_G_HOME 0xFF +#define SYM_GHOME 0x9D +#define SYM_GHOME1 0x9E +#define SYM_GHOLD 0xCD +#define SYM_GHOLD1 0xCE +#define SYM_GMISSION 0xB5 +#define SYM_GMISSION1 0xB6 +#define SYM_GLAND 0xB7 +#define SYM_GLAND1 0xB8 // Gimbal active Mode -#define SYM_GIMBAL 0X16 -#define SYM_GIMBAL1 0X17 +#define SYM_GIMBAL 0x16 +#define SYM_GIMBAL1 0x17 // Sensor´s Presence -#define SYM_ACC 0XA0 -#define SYM_MAG 0XA1 -#define SYM_BAR 0XA2 -#define SYM_GPS 0XA3 -#define SYM_MAN 0XC0 -#define SYM_MAN1 0XC1 -#define SYM_MAN2 0XC2 -#define SYM_CHECK 0XBE -#define SYM_BARO10 0XB7 -#define SYM_BARO11 0XB8 -#define SYM_MAG10 0XB5 -#define SYM_MAG11 0XB6 +#define SYM_ACC 0xA0 +#define SYM_MAG 0xA1 +#define SYM_BAR 0xA2 +#define SYM_GPS 0xA3 +#define SYM_MAN 0xC0 +#define SYM_MAN1 0xC1 +#define SYM_MAN2 0xC2 +#define SYM_CHECK 0xBE +#define SYM_BARO10 0xB7 +#define SYM_BARO11 0xB8 +#define SYM_MAG10 0xB5 +#define SYM_MAG11 0xB6 // AH Center screen Graphics -//#define SYM_AH_CENTER 0X01 +//#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 + #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 + #define SYM_AH_CENTER_LINE 0x26 + #define SYM_AH_CENTER 0x7E + #define SYM_AH_CENTER_LINE_RIGHT 0xBC #endif -#define SYM_AH_RIGHT 0X02 -#define SYM_AH_LEFT 0X03 -#define SYM_AH_DECORATION_UP 0XC9 -#define SYM_AH_DECORATION_DOWN 0XCF +#define SYM_AH_RIGHT 0x02 +#define SYM_AH_LEFT 0x03 +#define SYM_AH_DECORATION_UP 0xC9 +#define SYM_AH_DECORATION_DOWN 0xCF // AH Bars @@ -146,17 +146,17 @@ // Temperature -#define SYM_TEMP_F 0X0D -#define SYM_TEMP_C 0X0E +#define SYM_TEMP_F 0x0D +#define SYM_TEMP_C 0x0E // Batt evolution -#define SYM_BATT_FULL 0X90 -#define SYM_BATT_5 0X91 -#define SYM_BATT_4 0X92 -#define SYM_BATT_3 0X93 -#define SYM_BATT_2 0X94 -#define SYM_BATT_1 0X95 -#define SYM_BATT_EMPTY 0X96 +#define SYM_BATT_FULL 0x90 +#define SYM_BATT_5 0x91 +#define SYM_BATT_4 0x92 +#define SYM_BATT_3 0x93 +#define SYM_BATT_2 0x94 +#define SYM_BATT_1 0x95 +#define SYM_BATT_EMPTY 0x96 // Vario #define SYM_VARIO 0x7F @@ -165,42 +165,42 @@ #define SYM_GLIDESCOPE 0xE0 // Batt Icon´s -#define SYM_MAIN_BATT 0X97 -#define SYM_VID_BAT 0XBF +#define SYM_MAIN_BATT 0x97 +#define SYM_VID_BAT 0xBF // Unit Icon´s (Metric) -#define SYM_MS 0X9F -#define SYM_KMH 0XA5 -#define SYM_ALTM 0XA7 -#define SYM_DISTHOME_M 0XBB -#define SYM_M 0X0C +#define SYM_MS 0x9F +#define SYM_KMH 0xA5 +#define SYM_ALTM 0xA7 +#define SYM_DISTHOME_M 0xBB +#define SYM_M 0x0C // Unit Icon´s (Imperial) -#define SYM_FTS 0X99 -#define SYM_MPH 0XA6 -#define SYM_ALTFT 0XA8 -#define SYM_DISTHOME_FT 0XB9 -#define SYM_FT 0X0F +#define SYM_FTS 0x99 +#define SYM_MPH 0xA6 +#define SYM_ALTFT 0xA8 +#define SYM_DISTHOME_FT 0xB9 +#define SYM_FT 0x0F // Voltage and amperage -#define SYM_VOLT 0XA9 -#define SYM_AMP 0X9A -#define SYM_MAH 0XA4 -#define SYM_WATT 0X57 +#define SYM_VOLT 0xA9 +#define SYM_AMP 0x9A +#define SYM_MAH 0xA4 +#define SYM_WATT 0x57 // Flying Mode -#define SYM_ACRO 0XAE -#define SYM_ACROGY 0X98 -#define SYM_ACRO1 0XAF -#define SYM_STABLE 0XAC -#define SYM_STABLE1 0XAD -#define SYM_HORIZON 0XC4 -#define SYM_HORIZON1 0XC5 -#define SYM_PASS 0XAA -#define SYM_PASS1 0XAB -#define SYM_AIR 0XEA -#define SYM_AIR1 0XEB -#define SYM_PLUS 0X89 +#define SYM_ACRO 0xAE +#define SYM_ACROGY 0x98 +#define SYM_ACRO1 0xAF +#define SYM_STABLE 0xAC +#define SYM_STABLE1 0xAD +#define SYM_HORIZON 0xC4 +#define SYM_HORIZON1 0xC5 +#define SYM_PASS 0xAA +#define SYM_PASS1 0xAB +#define SYM_AIR 0xEA +#define SYM_AIR1 0xEB +#define SYM_PLUS 0x89 // Note, these change with scrolling enabled (scrolling is TODO) //#define SYM_AH_DECORATION_LEFT 0x13 @@ -208,23 +208,23 @@ #define SYM_AH_DECORATION 0x13 // Time -#define SYM_ON_M 0X9B -#define SYM_FLY_M 0X9C -#define SYM_ON_H 0X70 -#define SYM_FLY_H 0X71 +#define SYM_ON_M 0x9B +#define SYM_FLY_M 0x9C +#define SYM_ON_H 0x70 +#define SYM_FLY_H 0x71 // Throttle Position (%) -#define SYM_THR 0XC8 -#define SYM_THR1 0XC9 +#define SYM_THR 0xC8 +#define SYM_THR1 0xC9 // RSSI -#define SYM_RSSI 0XBA +#define SYM_RSSI 0xBA // Menu cursor #define SYM_CURSOR SYM_AH_LEFT //Misc -#define SYM_COLON 0X2D +#define SYM_COLON 0x2D //sport #define SYM_MIN 0xB3 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 88377fa0d7..40fbcd3600 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -105,6 +105,7 @@ #ifdef OSD #include "drivers/max7456.h" +#include "drivers/max7456_symbols.h" #ifdef USE_RTC6705 #include "drivers/vtx_soft_spi_rtc6705.h" @@ -669,24 +670,30 @@ void updateOsd(void) } if (masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] != -1) { - sprintf(line, "\x01%d.%1d", vbat / 10, vbat % 10); + line[0] = SYM_VOLT; + sprintf(line+1, "%d.%1d", vbat / 10, vbat % 10); max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]); } if (masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] != -1) { - sprintf(line, "\x02%d", rssi / 10); + line[0] = SYM_RSSI; + sprintf(line+1, "%d", rssi / 10); max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]); } if (masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] != -1) { - sprintf(line, "\x03%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + line[0] = SYM_THR; + line[1] = SYM_THR1; + sprintf(line+2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]); } if (masterConfig.osdProfile.item_pos[OSD_TIMER] != -1) { if (armed) { seconds = armed_seconds + ((now-armed_at) / 1000000); - sprintf(line, "\x04 %02d:%02d", seconds / 60, seconds % 60); + line[0] = SYM_FLY_M; + sprintf(line+1, " %02d:%02d", seconds / 60, seconds % 60); } else { + line[0] = SYM_ON_M; seconds = now / 1000000; - sprintf(line, "\x05 %02d:%02d", seconds / 60, seconds % 60); + sprintf(line+1, " %02d:%02d", seconds / 60, seconds % 60); } max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_TIMER]); } From 74ec4a033717ec54faf72030c07d7e01731a5a01 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Wed, 15 Jun 2016 23:54:31 -0700 Subject: [PATCH 23/38] Fix constatnt names, move vtx_channel config variable out of OSD ifdef --- src/main/config/config_master.h | 5 ++++- src/main/drivers/max7456.c | 10 +++++----- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 6d1c943179..6adb9d48d7 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -125,8 +125,11 @@ typedef struct master_t { uint8_t transponderData[6]; #endif -#ifdef OSD +#ifdef USE_RTC6705 uint8_t vtx_channel; +#endif + +#ifdef OSD osd_profile osdProfile; #endif diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 57874fbf31..ec161138d8 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -35,14 +35,14 @@ #define ENABLE_MAX7456 GPIO_ResetBits(MAX7456_CS_GPIO, MAX7456_CS_PIN) /** PAL or NTSC, value is number of chars total */ -#define VIDEO_MODE_PIXELS_NTSC 390 -#define VIDEO_MODE_PIXELS_PAL 480 +#define VIDEO_BUFFER_CHARS_NTSC 390 +#define VIDEO_BUFFER_CHARS_PAL 480 uint16_t max_screen_size; uint8_t video_signal_type = 0; uint8_t max7456_lock = 0; -char screen[VIDEO_MODE_PIXELS_PAL]; +char screen[VIDEO_BUFFER_CHARS_PAL]; uint8_t max7456_send(uint8_t add, uint8_t data) { @@ -85,10 +85,10 @@ void max7456_init(uint8_t system) { } if (video_signal_type) { //PAL - max_screen_size = VIDEO_MODE_PIXELS_PAL; + max_screen_size = VIDEO_BUFFER_CHARS_PAL; max_screen_rows = 16; } else { // NTSC - max_screen_size = VIDEO_MODE_PIXELS_NTSC; + max_screen_size = VIDEO_BUFFER_CHARS_NTSC; max_screen_rows = 13; } From d73f28fe94b024736619e385ba78e13a4a38af53 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Thu, 16 Jun 2016 01:34:01 -0700 Subject: [PATCH 24/38] Add support for VTX output power attenuation, Fix timerHardware definition --- src/main/config/config.c | 1 + src/main/config/config_master.h | 1 + src/main/drivers/vtx_soft_spi_rtc6705.c | 5 +++++ src/main/drivers/vtx_soft_spi_rtc6705.h | 12 ++++++++++++ src/main/io/osd.c | 5 ++--- src/main/io/serial_cli.c | 1 + src/main/target/SIRINFPV/target.c | 12 ++++++------ 7 files changed, 28 insertions(+), 9 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ff7a2ac1b6..3d39672e59 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -435,6 +435,7 @@ static void resetConf(void) #ifdef USE_RTC6705 masterConfig.vtx_channel = 19; // default to Boscam E channel 4 + masterConfig.vtx_power = 1; #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 6adb9d48d7..eb336ed999 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -127,6 +127,7 @@ typedef struct master_t { #ifdef USE_RTC6705 uint8_t vtx_channel; + uint8_t vtx_power; #endif #ifdef OSD diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index e66e5de603..b4b8ffce13 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -148,4 +148,9 @@ void rtc6705_soft_spi_set_channel(uint16_t channel_freq) { rtc6705_write_register(0, 400); rtc6705_write_register(1, (N << 7) | A); } + +void rtc6705_soft_spi_set_rf_power(uint8_t power) { + rtc6705_write_register(7, (power ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)))); +} + #endif diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.h b/src/main/drivers/vtx_soft_spi_rtc6705.h index 6d761418bf..5f4314de1d 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.h +++ b/src/main/drivers/vtx_soft_spi_rtc6705.h @@ -17,9 +17,21 @@ #pragma once +#define DP_5G_MASK 0x7000 +#define PA5G_BS_MASK 0x0E00 +#define PA5G_PW_MASK 0x0180 +#define PD_Q5G_MASK 0x0040 +#define QI_5G_MASK 0x0038 +#define PA_BS_MASK 0x0007 + +#define PA_CONTROL_DEFAULT 0x4FBD + + extern char* vtx_bands[]; extern uint16_t vtx_freq[]; extern uint16_t current_vtx_channel; void rtc6705_soft_spi_init(void); void rtc6705_soft_spi_set_channel(uint16_t channel_freq); +void rtc6705_soft_spi_set_rf_power(uint8_t power); + diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 99c38e8bf6..d8fb10c69c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -22,7 +22,7 @@ #include #include "platform.h" -#include "scheduler.h" +#include "scheduler/scheduler.h" #include "common/axis.h" #include "common/color.h" @@ -47,7 +47,6 @@ #include "drivers/pwm_rx.h" #include "drivers/adc.h" #include "drivers/bus_i2c.h" -#include "drivers/bus_bst.h" #include "drivers/bus_spi.h" #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" @@ -111,7 +110,6 @@ #include "drivers/vtx_soft_spi_rtc6705.h" #endif -#include "scheduler.h" #include "common/printf.h" #define MICROSECONDS_IN_A_SECOND (1000 * 1000) @@ -707,6 +705,7 @@ void osdInit(void) rtc6705_soft_spi_init(); current_vtx_channel = masterConfig.vtx_channel; rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); + rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); #endif max7456_init(masterConfig.osdProfile.system); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7e13230922..e8d5ae9372 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -806,6 +806,7 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_RTC6705 { "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, + { "vtx_power", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_power, .config.minmax = { 0, 1 } }, #endif #ifdef OSD { "osd_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.system, .config.minmax = { 0, 2 } }, diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index 4ccf123ea2..4ac409d394 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, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM1 - PB6 - { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2, 0}, // PWM2 - PB6 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2, 0}, // PWM3 - PB8 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2, 0}, // PWM4 - PB9 + { 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 - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2, 0}, // PWM5 - PB0 - *TIM3_CH3 - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2, 0}, // PWM6 - PB1 - *TIM3_CH4 + { 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 }; From a08e70f48c67f2d7fe92cd06309938c09663f14e Mon Sep 17 00:00:00 2001 From: nathan Date: Thu, 16 Jun 2016 02:21:12 -0700 Subject: [PATCH 25/38] flash the led on font upload --- src/main/drivers/max7456.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 06af3ccb23..9b03896914 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -27,6 +27,7 @@ #ifdef USE_MAX7456 #include "drivers/bus_spi.h" +#include "drivers/light_led.h" #include "drivers/system.h" #include "max7456.h" @@ -40,7 +41,6 @@ static IO_t max7456CsPin = IO_NONE; #define VIDEO_MODE_PIXELS_NTSC 390 #define VIDEO_MODE_PIXELS_PAL 480 - uint16_t max_screen_size; uint8_t video_signal_type = 0; uint8_t max7456_lock = 0; @@ -183,6 +183,11 @@ void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { for(x = 0; x < 54; x++) { max7456_send(MAX7456ADD_CMAL, x); //set start address low max7456_send(MAX7456ADD_CMDI, font_data[x]); +#ifdef LED0_TOGGLE + LED0_TOGGLE; +#else + LED1_TOGGLE; +#endif } // transfer 54 bytes from shadow ram to NVM From aef9b02d49471b34817ada1a2415cb9432850272 Mon Sep 17 00:00:00 2001 From: nathan Date: Thu, 16 Jun 2016 02:34:34 -0700 Subject: [PATCH 26/38] define sonar pins for omnibus target --- src/main/target/OMNIBUS/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 228d843fa3..9eb39d6dbf 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -61,6 +61,8 @@ #define MAG_AK8975_ALIGN CW90_DEG_FLIP #define SONAR +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define USB_IO #define USB_CABLE_DETECTION From 7662f944df5502f9e1ababc38fdbee8df28cc9de Mon Sep 17 00:00:00 2001 From: nathan Date: Thu, 16 Jun 2016 02:39:00 -0700 Subject: [PATCH 27/38] extra files --- src/test/unit/scheduler_unittest.cc | 409 ---------------------------- support/openocd/stm32f3.cfg | 10 - 2 files changed, 419 deletions(-) delete mode 100644 src/test/unit/scheduler_unittest.cc delete mode 100644 support/openocd/stm32f3.cfg diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc deleted file mode 100644 index 10814d1790..0000000000 --- a/src/test/unit/scheduler_unittest.cc +++ /dev/null @@ -1,409 +0,0 @@ -/* - * 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 - -extern "C" { - #include "platform.h" - #include "scheduler/scheduler.h" -} - -#include "unittest_macros.h" -#include "gtest/gtest.h" -enum { - systemTime = 10, - pidLoopCheckerTime = 650, - updateAccelerometerTime = 192, - handleSerialTime = 30, - updateBeeperTime = 1, - updateBatteryTime = 1, - updateRxCheckTime = 34, - updateRxMainTime = 10, - processGPSTime = 10, - updateCompassTime = 195, - updateBaroTime = 201, - updateSonarTime = 10, - calculateAltitudeTime = 154, - updateDisplayTime = 10, - updateMaxOSDTime = 10, - telemetryTime = 10, - ledStripTime = 10, - transponderTime = 10 -}; - -extern "C" { - cfTask_t * unittest_scheduler_selectedTask; - uint8_t unittest_scheduler_selectedTaskDynPrio; - uint16_t unittest_scheduler_waitingTasks; - uint32_t unittest_scheduler_timeToNextRealtimeTask; - bool unittest_outsideRealtimeGuardInterval; - -// set up micros() to simulate time - uint32_t simulatedTime = 0; - uint32_t micros(void) {return simulatedTime;} -// set up tasks to take a simulated representative time to execute - void taskMainPidLoopChecker(void) {simulatedTime+=pidLoopCheckerTime;} - void taskUpdateAccelerometer(void) {simulatedTime+=updateAccelerometerTime;} - void taskHandleSerial(void) {simulatedTime+=handleSerialTime;} - void taskUpdateBeeper(void) {simulatedTime+=updateBeeperTime;} - void taskUpdateBattery(void) {simulatedTime+=updateBatteryTime;} - bool taskUpdateRxCheck(uint32_t currentDeltaTime) {UNUSED(currentDeltaTime);simulatedTime+=updateRxCheckTime;return false;} - void taskUpdateRxMain(void) {simulatedTime+=updateRxMainTime;} - void taskProcessGPS(void) {simulatedTime+=processGPSTime;} - void taskUpdateCompass(void) {simulatedTime+=updateCompassTime;} - void taskUpdateBaro(void) {simulatedTime+=updateBaroTime;} - void taskUpdateSonar(void) {simulatedTime+=updateSonarTime;} - void taskCalculateAltitude(void) {simulatedTime+=calculateAltitudeTime;} - void taskUpdateDisplay(void) {simulatedTime+=updateDisplayTime;} - void taskUpdateMaxOSD(void) {simulatedTime+=updateMaxOSDTime;} - void taskTelemetry(void) {simulatedTime+=telemetryTime;} - void taskLedStrip(void) {simulatedTime+=ledStripTime;} - void taskTransponder(void) {simulatedTime+=transponderTime;} - - extern cfTask_t* taskQueueArray[]; - - extern void queueClear(void); - extern int queueSize(); - extern bool queueContains(cfTask_t *task); - extern bool queueAdd(cfTask_t *task); - extern bool queueRemove(cfTask_t *task); - extern cfTask_t *queueFirst(void); - extern cfTask_t *queueNext(void); -} - -TEST(SchedulerUnittest, TestPriorites) -{ - EXPECT_EQ(14, TASK_COUNT); - // if any of these fail then task priorities have changed and ordering in TestQueue needs to be re-checked - EXPECT_EQ(TASK_PRIORITY_HIGH, cfTasks[TASK_SYSTEM].staticPriority); - EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority); - EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority); - EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority); - EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY].staticPriority); -} - -TEST(SchedulerUnittest, TestQueueInit) -{ - queueClear(); - EXPECT_EQ(0, queueSize()); - EXPECT_EQ(0, queueFirst()); - EXPECT_EQ(0, queueNext()); - for (int ii = 0; ii <= TASK_COUNT; ++ii) { - EXPECT_EQ(0, taskQueueArray[ii]); - } -} - -cfTask_t *deadBeefPtr = reinterpret_cast(0xDEADBEEF); - -TEST(SchedulerUnittest, TestQueue) -{ - queueClear(); - taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; - - queueAdd(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH - EXPECT_EQ(1, queueSize()); - EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME - EXPECT_EQ(2, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); - EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); - EXPECT_EQ(NULL, queueNext()); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW - EXPECT_EQ(3, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); - EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); - EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); - EXPECT_EQ(NULL, queueNext()); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - queueAdd(&cfTasks[TASK_BATTERY]); // TASK_PRIORITY_MEDIUM - EXPECT_EQ(4, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); - EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); - EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext()); - EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); - EXPECT_EQ(NULL, queueNext()); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH - EXPECT_EQ(5, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); - EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); - EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); - EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext()); - EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); - EXPECT_EQ(NULL, queueNext()); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH - EXPECT_EQ(4, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); - EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); - EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext()); - EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); - EXPECT_EQ(NULL, queueNext()); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); -} - -TEST(SchedulerUnittest, TestQueueAddAndRemove) -{ - queueClear(); - taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; - - // fill up the queue - for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { - const bool added = queueAdd(&cfTasks[taskId]); - EXPECT_EQ(true, added); - EXPECT_EQ(taskId + 1, queueSize()); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - } - // double check end of queue - EXPECT_EQ(TASK_COUNT, queueSize()); - EXPECT_NE(static_cast(0), taskQueueArray[TASK_COUNT - 1]); // last item was indeed added to queue - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // null pointer at end of queue is preserved - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // there hasn't been an out by one error - - // and empty it again - for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { - const bool removed = queueRemove(&cfTasks[taskId]); - EXPECT_EQ(true, removed); - EXPECT_EQ(TASK_COUNT - taskId - 1, queueSize()); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - } - // double check size and end of queue - EXPECT_EQ(0, queueSize()); // queue is indeed empty - EXPECT_EQ(NULL, taskQueueArray[0]); // there is a null pointer at the end of the queueu - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // no accidental overwrites past end of queue -} - -TEST(SchedulerUnittest, TestQueueArray) -{ - // test there are no "out by one" errors or buffer overruns when items are added and removed - queueClear(); - taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; // note, must set deadBeefPtr after queueClear - - for (int taskId = 0; taskId < TASK_COUNT - 1; ++taskId) { - setTaskEnabled(static_cast(taskId), true); - EXPECT_EQ(taskId + 1, queueSize()); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - } - EXPECT_EQ(TASK_COUNT - 1, queueSize()); - EXPECT_NE(static_cast(0), taskQueueArray[TASK_COUNT - 2]); - const cfTask_t *lastTaskPrev = taskQueueArray[TASK_COUNT - 2]; - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - setTaskEnabled(TASK_SYSTEM, false); - EXPECT_EQ(TASK_COUNT - 2, queueSize()); - EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); // NULL at end of queue - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - taskQueueArray[TASK_COUNT - 2] = 0; - setTaskEnabled(TASK_SYSTEM, true); - EXPECT_EQ(TASK_COUNT - 1, queueSize()); - EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 2]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - cfTaskInfo_t taskInfo; - getTaskInfo(static_cast(TASK_COUNT - 1), &taskInfo); - EXPECT_EQ(false, taskInfo.isEnabled); - setTaskEnabled(static_cast(TASK_COUNT - 1), true); - EXPECT_EQ(TASK_COUNT, queueSize()); - EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // check no buffer overrun - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - setTaskEnabled(TASK_SYSTEM, false); - EXPECT_EQ(TASK_COUNT - 1, queueSize()); - //EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - setTaskEnabled(TASK_ACCEL, false); - EXPECT_EQ(TASK_COUNT - 2, queueSize()); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - - setTaskEnabled(TASK_BATTERY, false); - EXPECT_EQ(TASK_COUNT - 3, queueSize()); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 3]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); -} - -TEST(SchedulerUnittest, TestSchedulerInit) -{ - schedulerInit(); - EXPECT_EQ(1, queueSize()); - EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); -} - -TEST(SchedulerUnittest, TestScheduleEmptyQueue) -{ - queueClear(); - simulatedTime = 4000; - // run the with an empty queue - scheduler(); - EXPECT_EQ(NULL, unittest_scheduler_selectedTask); -} - -TEST(SchedulerUnittest, TestSingleTask) -{ - schedulerInit(); - // disable all tasks except TASK_GYROPID - for (int taskId=0; taskId < TASK_COUNT; ++taskId) { - setTaskEnabled(static_cast(taskId), false); - } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 1000; - simulatedTime = 4000; - // run the scheduler and check the task has executed - scheduler(); - EXPECT_NE(static_cast(0), unittest_scheduler_selectedTask); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); - EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime); - EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt); - EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_GYROPID].totalExecutionTime); - // task has run, so its dynamic priority should have been set to zero - EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority); -} - -TEST(SchedulerUnittest, TestTwoTasks) -{ - // disable all tasks except TASK_GYROPID and TASK_ACCEL - for (int taskId=0; taskId < TASK_COUNT; ++taskId) { - setTaskEnabled(static_cast(taskId), false); - } - setTaskEnabled(TASK_ACCEL, true); - setTaskEnabled(TASK_GYROPID, true); - - // set it up so that TASK_ACCEL ran just before TASK_GYROPID - static const uint32_t startTime = 4000; - simulatedTime = startTime; - cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime; - cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - updateAccelerometerTime; - EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles); - // run the scheduler - scheduler(); - // no tasks should have run, since neither task's desired time has elapsed - EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); - - // NOTE: - // TASK_GYROPID desiredPeriod is 1000 microseconds - // TASK_ACCEL desiredPeriod is 10000 microseconds - // 500 microseconds later - simulatedTime += 500; - // no tasks should run, since neither task's desired time has elapsed - scheduler(); - EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); - EXPECT_EQ(0, unittest_scheduler_waitingTasks); - - // 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed - simulatedTime += 500; - // TASK_GYROPID should now run - scheduler(); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); - EXPECT_EQ(1, unittest_scheduler_waitingTasks); - EXPECT_EQ(5000 + pidLoopCheckerTime, simulatedTime); - - simulatedTime += 1000 - pidLoopCheckerTime; - scheduler(); - // TASK_GYROPID should run again - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); - - scheduler(); - EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); - EXPECT_EQ(0, unittest_scheduler_waitingTasks); - - simulatedTime = startTime + 10500; // TASK_GYROPID and TASK_ACCEL desiredPeriods have elapsed - // of the two TASK_GYROPID should run first - scheduler(); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); - // and finally TASK_ACCEL should now run - scheduler(); - EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); -} - -TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun) -{ - // disable all tasks except TASK_GYROPID and TASK_SYSTEM - for (int taskId=0; taskId < TASK_COUNT; ++taskId) { - setTaskEnabled(static_cast(taskId), false); - } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 200000; - simulatedTime = 200700; - - setTaskEnabled(TASK_SYSTEM, true); - cfTasks[TASK_SYSTEM].lastExecutedAt = 100000; - - scheduler(); - - EXPECT_EQ(false, unittest_outsideRealtimeGuardInterval); - EXPECT_EQ(300, unittest_scheduler_timeToNextRealtimeTask); - - // Nothing should be scheduled in guard period - EXPECT_EQ(NULL, unittest_scheduler_selectedTask); - EXPECT_EQ(100000, cfTasks[TASK_SYSTEM].lastExecutedAt); - - EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); -} - -TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun) -{ - // disable all tasks except TASK_GYROPID and TASK_SYSTEM - for (int taskId=0; taskId < TASK_COUNT; ++taskId) { - setTaskEnabled(static_cast(taskId), false); - } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 200000; - simulatedTime = 200699; - - setTaskEnabled(TASK_SYSTEM, true); - cfTasks[TASK_SYSTEM].lastExecutedAt = 100000; - - scheduler(); - - EXPECT_EQ(true, unittest_outsideRealtimeGuardInterval); - EXPECT_EQ(301, unittest_scheduler_timeToNextRealtimeTask); - - // System should be scheduled as not in guard period - EXPECT_EQ(&cfTasks[TASK_SYSTEM], unittest_scheduler_selectedTask); - EXPECT_EQ(200699, cfTasks[TASK_SYSTEM].lastExecutedAt); - - EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); -} - -// STUBS -extern "C" { -} diff --git a/support/openocd/stm32f3.cfg b/support/openocd/stm32f3.cfg deleted file mode 100644 index d04cad7034..0000000000 --- a/support/openocd/stm32f3.cfg +++ /dev/null @@ -1,10 +0,0 @@ -# This is an STM32F3 discovery board with a single STM32F303VCT6 chip. -# http://www.st.com/internet/evalboard/product/254044.jsp - -source [find interface/stlink-v2.cfg] - -transport select hla_swd - -source [find target/stm32f3x.cfg] - -reset_config none separate From a960bfd1cddcd124f16bc2ec195de7c1632b7da1 Mon Sep 17 00:00:00 2001 From: nathan Date: Thu, 16 Jun 2016 19:12:04 -0700 Subject: [PATCH 28/38] merge max code into stm32f3dicovery target, comment out the SDCARD, switch the spi between the SDCARD and MAX7456 for testing --- src/main/target/STM32F3DISCOVERY/target.h | 83 +++++--- .../target/STM32F3DISCOVERYMAX7456/target.c | 90 -------- .../target/STM32F3DISCOVERYMAX7456/target.h | 197 ------------------ .../target/STM32F3DISCOVERYMAX7456/target.mk | 18 -- 4 files changed, 56 insertions(+), 332 deletions(-) delete mode 100644 src/main/target/STM32F3DISCOVERYMAX7456/target.c delete mode 100644 src/main/target/STM32F3DISCOVERYMAX7456/target.h delete mode 100644 src/main/target/STM32F3DISCOVERYMAX7456/target.mk diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index e54b919189..2d78bf929d 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -1,18 +1,28 @@ /* - * This file is part of Cleanflight. + * Supports the GY-91 MPU9250 and BMP280 development board via SPI1 * - * Cleanflight is free software: you can redistribute it and/or modify + * Put the MAX7456 on SPI2 instead of an SDCARD + * MAX7456 CS -> PB12 (default) + * Uses the default pins for SPI2: + * #define SPI2_NSS_PIN PB12 + * #define SPI2_SCK_PIN PB13 + * #define SPI2_MISO_PIN PB14 + * #define SPI2_MOSI_PIN PB15 + * + * @author Nathan Tsoi + * + * This software 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, + * This software 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 . + * along with this software. If not, see . */ #pragma once @@ -38,11 +48,11 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_SD_CARD - -#define SD_DETECT_PIN PC14 -#define SD_CS_PIN PB12 -#define SD_SPI_INSTANCE SPI2 +//#define USE_SD_CARD +// +//#define SD_DETECT_PIN PC14 +//#define SD_CS_PIN PB12 +//#define SD_SPI_INSTANCE SPI2 //#define USE_FLASHFS //#define USE_FLASH_M25P16 @@ -64,31 +74,50 @@ #define GYRO #define USE_GYRO_L3GD20 - #define L3GD20_SPI SPI1 #define L3GD20_CS_PIN PE3 - #define GYRO_L3GD20_ALIGN CW270_DEG -#define USE_SDCARD -#define USE_SDCARD_SPI2 - -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN PB12 -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - -// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. -#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 - -// Performance logging for SD card operations: -// #define AFATFS_USE_INTROSPECTIVE_LOGGING +// Support the GY-91 MPU9250 dev board +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 +#define MPU6500_CS_PIN PC14 +#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP #define ACC #define USE_ACC_LSM303DLHC +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG_FLIP + +//#define BARO +//#define BMP280_CS_PIN PB12 +//#define BMP280_SPI_INSTANCE SPI2 +//#define USE_BARO_BMP280 +//#define USE_BARO_SPI_BMP280 + +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN + +//#define USE_SDCARD +//#define USE_SDCARD_SPI2 +// +//#define SDCARD_SPI_INSTANCE SPI2 +//#define SDCARD_SPI_CS_PIN PB12 +//// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +//#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +//// Divide to under 25MHz for normal operation: +//#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 +// +//// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +//#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +//#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING #define MAG #define USE_MAG_HMC5883 diff --git a/src/main/target/STM32F3DISCOVERYMAX7456/target.c b/src/main/target/STM32F3DISCOVERYMAX7456/target.c deleted file mode 100644 index ab538410ff..0000000000 --- a/src/main/target/STM32F3DISCOVERYMAX7456/target.c +++ /dev/null @@ -1,90 +0,0 @@ - -#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), // 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 - 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), // 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 - 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), // 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 - 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), // servo #4 - 0xFFFF -}; - -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 -}; - diff --git a/src/main/target/STM32F3DISCOVERYMAX7456/target.h b/src/main/target/STM32F3DISCOVERYMAX7456/target.h deleted file mode 100644 index 8fdef97b44..0000000000 --- a/src/main/target/STM32F3DISCOVERYMAX7456/target.h +++ /dev/null @@ -1,197 +0,0 @@ -/* - * Supports the GY-91 MPU9250 and BMP280 development board via SPI1 - * - * Put the MAX7456 on SPI2 instead of an SDCARD - * MAX7456 CS -> PB12 (default) - * Uses the default pins for SPI2: - * #define SPI2_NSS_PIN PB12 - * #define SPI2_SCK_PIN PB13 - * #define SPI2_MISO_PIN PB14 - * #define SPI2_MOSI_PIN PB15 - * - * @author Nathan Tsoi - * - * This software 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. - * - * This software 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 this software. If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "DF3M" // stm32f3 DiscoveryF3 Max7456 - -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE - -#define LED0 PE8 // Blue LEDs - PE8/PE12 -#define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 -#define LED1_INVERTED - -#define BEEPER PE9 // Red LEDs - PE9/PE13 -#define BEEPER_INVERTED - -#define USE_SPI -#define USE_SPI_DEVICE_1 -#define USE_SPI_DEVICE_2 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -//#define USE_SD_CARD -// -//#define SD_DETECT_PIN PC14 -//#define SD_CS_PIN PB12 -//#define SD_SPI_INSTANCE SPI2 - -//#define USE_FLASHFS -//#define USE_FLASH_M25P16 - -//#define M25P16_CS_GPIO GPIOB -//#define M25P16_CS_PIN GPIO_Pin_12 -//#define M25P16_SPI_INSTANCE SPI2 -// SPI1 -// PB5 SPI1_MOSI -// PB4 SPI1_MISO -// PB3 SPI1_SCK -// PA15 SPI1_NSS - -// SPI2 -// PB15 SPI2_MOSI -// PB14 SPI2_MISO -// PB13 SPI2_SCK -// PB12 SPI2_NSS - -#define GYRO - -#define USE_GYRO_L3GD20 -#define L3GD20_SPI SPI1 -#define L3GD20_CS_PIN PE3 -#define GYRO_L3GD20_ALIGN CW270_DEG - -// Support the GY-91 MPU9250 dev board -#define USE_GYRO_MPU6500 -#define USE_GYRO_SPI_MPU6500 -#define MPU6500_CS_PIN PC14 -#define MPU6500_SPI_INSTANCE SPI2 -#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP - -#define ACC -#define USE_ACC_LSM303DLHC -#define USE_ACC_MPU6500 -#define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG_FLIP - -//#define BARO -//#define BMP280_CS_PIN PB12 -//#define BMP280_SPI_INSTANCE SPI2 -//#define USE_BARO_BMP280 -//#define USE_BARO_SPI_BMP280 - -#define OSD -#define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI2 -#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN - -//#define USE_SDCARD -//#define USE_SDCARD_SPI2 -// -//#define SDCARD_SPI_INSTANCE SPI2 -//#define SDCARD_SPI_CS_PIN PB12 -//// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -//#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -//// Divide to under 25MHz for normal operation: -//#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 -// -//// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. -//#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -//#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 - -// Performance logging for SD card operations: -// #define AFATFS_USE_INTROSPECTIVE_LOGGING - -#define MAG -#define USE_MAG_HMC5883 - -#define USE_VCP -#define USE_USART1 -#define USE_USART2 -#define SERIAL_PORT_COUNT 3 - -// uart2 gpio for shared serial rx/ppm -//#define UART2_TX_PIN GPIO_Pin_5 // PD5 -//#define UART2_RX_PIN GPIO_Pin_6 // PD6 -//#define UART2_GPIO GPIOD -//#define UART2_GPIO_AF GPIO_AF_7 -//#define UART2_TX_PINSOURCE GPIO_PinSource5 -//#define UART2_RX_PINSOURCE GPIO_PinSource6 - -#define USE_I2C -#define I2C_DEVICE (I2CDEV_1) - -#define USE_ADC - -#define ADC_INSTANCE ADC1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define ADC_DMA_CHANNEL DMA1_Channel1 - -#define VBAT_ADC_GPIO GPIOC -#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 -#define VBAT_ADC_CHANNEL ADC_Channel_6 - -#define CURRENT_METER_ADC_GPIO GPIOC -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 - -#define RSSI_ADC_GPIO GPIOC -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 -#define RSSI_ADC_CHANNEL ADC_Channel_8 - -#define EXTERNAL1_ADC_GPIO GPIOC -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 - -#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_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 - -#define LED_STRIP -#define LED_STRIP_TIMER TIM16 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -// IO - 303 in 100pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff -#define TARGET_IO_PORTF 0x00ff - - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB | RCC_AHBPeriph_GPIOC | RCC_AHBPeriph_GPIOD) - diff --git a/src/main/target/STM32F3DISCOVERYMAX7456/target.mk b/src/main/target/STM32F3DISCOVERYMAX7456/target.mk deleted file mode 100644 index c8ddcc7ddd..0000000000 --- a/src/main/target/STM32F3DISCOVERYMAX7456/target.mk +++ /dev/null @@ -1,18 +0,0 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP MAX_OSD - -TARGET_SRC = \ - drivers/light_ws2811strip.c \ - drivers/accgyro_mpu.c \ - drivers/accgyro_l3gd20.c \ - drivers/accgyro_lsm303dlhc.c \ - drivers/compass_hmc5883l.c \ - drivers/accgyro_adxl345.c \ - drivers/accgyro_bma280.c \ - drivers/accgyro_mma845x.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_l3g4200d.c \ - drivers/barometer_ms5611.c \ - drivers/barometer_bmp280.c \ - drivers/compass_ak8975.c From e9868c4e378cddf9e9239b954c4182b7b6ffe4c3 Mon Sep 17 00:00:00 2001 From: nathan Date: Thu, 16 Jun 2016 19:16:31 -0700 Subject: [PATCH 29/38] namespace a few vars add some #defines for clarity --- src/main/config/config.c | 3 ++- src/main/drivers/max7456.c | 38 ++++++++++++++++++-------------------- src/main/drivers/max7456.h | 30 +++++++++++++++++++++++++++++- src/main/io/osd.c | 2 +- src/main/io/osd.h | 3 ++- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 2 +- 7 files changed, 54 insertions(+), 26 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 3d39672e59..137cdf3b07 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -38,6 +38,7 @@ #include "drivers/serial.h" #include "drivers/gyro_sync.h" #include "drivers/pwm_output.h" +#include "drivers/max7456.h" #include "sensors/sensors.h" #include "sensors/gyro.h" @@ -421,7 +422,7 @@ static void resetConf(void) #ifdef OSD featureSet(FEATURE_OSD); - masterConfig.osdProfile.system = 0; + masterConfig.osdProfile.video_system = AUTO; masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 2911b68949..497fd62895 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -31,20 +31,18 @@ #include "drivers/system.h" #include "max7456.h" +#include "max7456_symbols.h" #define DISABLE_MAX7456 IOHi(max7456CsPin) #define ENABLE_MAX7456 IOLo(max7456CsPin) static IO_t max7456CsPin = IO_NONE; -/** PAL or NTSC, value is number of chars total */ -#define VIDEO_BUFFER_CHARS_NTSC 390 -#define VIDEO_BUFFER_CHARS_PAL 480 uint16_t max_screen_size; uint8_t video_signal_type = 0; uint8_t max7456_lock = 0; -char screen[VIDEO_BUFFER_CHARS_PAL]; +char max7456_screen[VIDEO_BUFFER_CHARS_PAL]; uint8_t max7456_send(uint8_t add, uint8_t data) { @@ -53,11 +51,11 @@ uint8_t max7456_send(uint8_t add, uint8_t data) { } -void max7456_init(uint8_t system) { +void max7456_init(uint8_t video_system) { uint8_t max_screen_rows; uint8_t srdata = 0; uint16_t x; - char buf[30]; + char buf[LINE]; #ifdef MAX7456_SPI_CS_PIN max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN)); @@ -83,21 +81,21 @@ void max7456_init(uint8_t system) { } // Override detected type: 0-AUTO, 1-PAL, 2-NTSC - switch(system) { - case 1: + switch(video_system) { + case PAL: video_signal_type = VIDEO_MODE_PAL; break; - case 2: + case NTSC: video_signal_type = VIDEO_MODE_NTSC; break; } if (video_signal_type) { //PAL max_screen_size = VIDEO_BUFFER_CHARS_PAL; - max_screen_rows = 16; + max_screen_rows = VIDEO_LINES_PAL; } else { // NTSC max_screen_size = VIDEO_BUFFER_CHARS_NTSC; - max_screen_rows = 13; + max_screen_rows = VIDEO_LINES_NTSC; } // set all rows to same charactor black/white level @@ -114,13 +112,13 @@ void max7456_init(uint8_t system) { x = 160; for (int i = 1; i < 5; i++) { for (int j = 3; j < 27; j++) - screen[i * 30 + j] = (char)x++; + max7456_screen[i * LINE + j] = (char)x++; } tfp_sprintf(buf, "BF VERSION: %s", FC_VERSION_STRING); - max7456_write_string(buf, 5*30+5); - max7456_write_string("MENU: THRT MID", 6*30+7); - max7456_write_string("YAW RIGHT", 7*30+13); - max7456_write_string("PITCH UP", 8*30+13); + max7456_write_string(buf, LINE06+5); + max7456_write_string("MENU: THRT MID", LINE07+7); + max7456_write_string("YAW RIGHT", LINE08+13); + max7456_write_string("PITCH UP", LINE09+13); max7456_draw_screen(); } @@ -144,8 +142,8 @@ void max7456_draw_screen(void) { for (xx = 0; xx < max_screen_size; ++xx) { max7456_send(MAX7456ADD_DMAH, xx>>8); max7456_send(MAX7456ADD_DMAL, xx); - max7456_send(MAX7456ADD_DMDI, screen[xx]); - screen[xx] = ' '; + max7456_send(MAX7456ADD_DMDI, max7456_screen[xx]); + max7456_screen[xx] = ' '; } DISABLE_MAX7456; } @@ -159,8 +157,8 @@ void max7456_draw_screen_fast(void) { max7456_send(MAX7456ADD_DMAL, 0); max7456_send(MAX7456ADD_DMM, 1); for (xx = 0; xx < max_screen_size; ++xx) { - max7456_send(MAX7456ADD_DMDI, screen[xx]); - screen[xx] = ' '; + max7456_send(MAX7456ADD_DMDI, max7456_screen[xx]); + max7456_screen[xx] = ' '; } max7456_send(MAX7456ADD_DMDI, 0xFF); max7456_send(MAX7456ADD_DMM, 0); diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 86bd4dd1ee..d14d0c0dd4 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -114,8 +114,36 @@ #define WRITE_NVR 0xA0 #define STATUS_REG_NVR_BUSY 0x20 +/** Line multiples, for convenience & one less op at runtime **/ +#define LINE 30 +#define LINE01 0 +#define LINE02 30 +#define LINE03 60 +#define LINE04 90 +#define LINE05 120 +#define LINE06 150 +#define LINE07 180 +#define LINE08 210 +#define LINE09 240 +#define LINE10 270 +#define LINE11 300 +#define LINE12 330 +#define LINE13 360 +#define LINE14 390 +#define LINE15 420 +#define LINE16 450 + + +/** PAL or NTSC, value is number of chars total */ +#define VIDEO_BUFFER_CHARS_NTSC 390 +#define VIDEO_BUFFER_CHARS_PAL 480 +#define VIDEO_LINES_NTSC 13 +#define VIDEO_LINES_PAL 16 + +enum VIDEO_TYPES { AUTO = 0, PAL, NTSC }; + extern uint16_t max_screen_size; -char screen[480]; +char max7456_screen[VIDEO_BUFFER_CHARS_PAL]; void max7456_init(uint8_t system); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 16a3ec96c1..2236fabe6f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -714,7 +714,7 @@ void osdInit(void) rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); #endif - max7456_init(masterConfig.osdProfile.system); + max7456_init(masterConfig.osdProfile.video_system); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 688a0872f8..a1f71e5866 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -53,7 +53,8 @@ typedef enum { typedef struct { - uint8_t system; + // AUTO / PAL / NTSC in VIDEO_TYPES enum + uint8_t video_system; int16_t item_pos[OSD_MAX_ITEMS]; } osd_profile; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e8d5ae9372..e08e932287 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -809,7 +809,7 @@ const clivalue_t valueTable[] = { { "vtx_power", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_power, .config.minmax = { 0, 1 } }, #endif #ifdef OSD - { "osd_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.system, .config.minmax = { 0, 2 } }, + { "osd_video_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.video_system, .config.minmax = { 0, 2 } }, { "osd_main_voltage_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { -480, 480 } }, { "osd_rssi_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], .config.minmax = { -480, 480 } }, { "osd_timer_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_TIMER], .config.minmax = { -480, 480 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index fe3d2cf8e9..f1a5e8d318 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1524,7 +1524,7 @@ static bool processInCommand(void) #endif #ifdef OSD case MSP_SET_OSD_CONFIG: - masterConfig.osdProfile.system = read8(); + masterConfig.osdProfile.video_system = read8(); for (i = 0; i < OSD_MAX_ITEMS; i++) masterConfig.osdProfile.item_pos[i] = read16(); break; From 1a7ed10ef1c8dc8a1ab548362692e6dfc2dda722 Mon Sep 17 00:00:00 2001 From: nathan Date: Thu, 16 Jun 2016 19:17:21 -0700 Subject: [PATCH 30/38] artificial horizon --- src/main/config/config.c | 20 +++++++++-------- src/main/drivers/max7456.c | 46 +++++++++++++++++++++++++++++++++++--- src/main/drivers/max7456.h | 1 + src/main/io/osd.c | 3 +++ src/main/io/osd.h | 2 ++ 5 files changed, 60 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 137cdf3b07..6e803a920b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -423,15 +423,17 @@ static void resetConf(void) #ifdef OSD featureSet(FEATURE_OSD); masterConfig.osdProfile.video_system = AUTO; - masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; - masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; - masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; - masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; - masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; - masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; - masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; - masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; - masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; + masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; + masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; + masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; + masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; + masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; + masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; + masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; + masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] = 1; + masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; #endif #ifdef USE_RTC6705 diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 497fd62895..b03c1d616f 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -38,6 +38,11 @@ static IO_t max7456CsPin = IO_NONE; +/** Artificial Horizon limits **/ +#define AHIPITCHMAX 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees +#define AHIROLLMAX 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees +#define AHISIDEBARWIDTHPOSITION 7 +#define AHISIDEBARHEIGHTPOSITION 3 uint16_t max_screen_size; uint8_t video_signal_type = 0; @@ -127,14 +132,49 @@ void max7456_write_string(const char *string, int16_t address) { char *dest; if (address >= 0) - dest = screen + address; + dest = max7456_screen + address; else - dest = screen + (max_screen_size + address); + dest = max7456_screen + (max_screen_size + address); - while(*string && dest < (screen + max_screen_size)) + while(*string && dest < (max7456_screen + max_screen_size)) *dest++ = *string++; } + +// Write the artifical horizon to the screen buffer +void max7456_artificial_horizon(int rollAngle, int pitchAngle, uint8_t show_sidebars) { + uint16_t position = 194; + + if(pitchAngle>AHIPITCHMAX) pitchAngle=AHIPITCHMAX; + if(pitchAngle<-AHIPITCHMAX) pitchAngle=-AHIPITCHMAX; + if(rollAngle>AHIROLLMAX) rollAngle=AHIROLLMAX; + if(rollAngle<-AHIROLLMAX) rollAngle=-AHIROLLMAX; + + for(uint8_t X=0; X<=8; X++) { + if (X==4) X=5; + int Y = (rollAngle * (4-X)) / 64; + Y -= pitchAngle / 8; + Y += 41; + if(Y >= 0 && Y <= 81) { + uint16_t pos = position -7 + LINE*(Y/9) + 3 - 4*LINE + X; + max7456_screen[pos] = SYM_AH_BAR9_0+(Y%9); + } + } + + if (show_sidebars) { + // Draw AH sides + int8_t hudwidth = AHISIDEBARWIDTHPOSITION; + int8_t hudheight = AHISIDEBARHEIGHTPOSITION; + for(int8_t X=-hudheight; X<=hudheight; X++) { + max7456_screen[position-hudwidth+(X*LINE)] = SYM_AH_DECORATION; + max7456_screen[position+hudwidth+(X*LINE)] = SYM_AH_DECORATION; + } + // AH level indicators + max7456_screen[position-hudwidth+1] = SYM_AH_LEFT; + max7456_screen[position+hudwidth-1] = SYM_AH_RIGHT; + } +} + void max7456_draw_screen(void) { uint16_t xx; if (!max7456_lock) { diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index d14d0c0dd4..cd33eae5e9 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -149,5 +149,6 @@ char max7456_screen[VIDEO_BUFFER_CHARS_PAL]; void max7456_init(uint8_t system); void max7456_draw_screen(void); void max7456_draw_screen_fast(void); +void max7456_artificial_horizon(int rollAngle, int pitchAngle, uint8_t show_sidebars); void max7456_write_string(const char *string, int16_t address); void max7456_write_nvm(uint8_t char_address, uint8_t *font_data); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2236fabe6f..8963aa4ba6 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -699,6 +699,9 @@ void updateOsd(void) if (masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] != -1) { print_average_system_load(masterConfig.osdProfile.item_pos[OSD_CPU_LOAD], 0); } + if (masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] != -1) { + max7456_artificial_horizon(attitude.values.roll, attitude.values.pitch, masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] != -1); + } } } else { max7456_draw_screen_fast(); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a1f71e5866..486ff655a9 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -48,6 +48,8 @@ typedef enum { OSD_VOLTAGE_WARNING, OSD_ARMED, OSD_DISARMED, + OSD_ARTIFICIAL_HORIZON, + OSD_HORIZON_SIDEBARS, OSD_MAX_ITEMS, // MUST BE LAST } osd_items_t; From b01103e655410b63c66841c3037760fe0603cee9 Mon Sep 17 00:00:00 2001 From: nathan Date: Thu, 16 Jun 2016 19:40:58 -0700 Subject: [PATCH 31/38] add the center icon, this could be optional? --- src/main/drivers/max7456.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index b03c1d616f..a8065b0758 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -160,6 +160,9 @@ void max7456_artificial_horizon(int rollAngle, int pitchAngle, uint8_t show_side max7456_screen[pos] = SYM_AH_BAR9_0+(Y%9); } } + max7456_screen[position-1] = SYM_AH_CENTER_LINE; + max7456_screen[position+1] = SYM_AH_CENTER_LINE_RIGHT; + max7456_screen[position] = SYM_AH_CENTER; if (show_sidebars) { // Draw AH sides From 8b581d945b8ad5fb6ad770ceb9e2e1d29958bc30 Mon Sep 17 00:00:00 2001 From: nathan Date: Thu, 16 Jun 2016 23:49:15 -0700 Subject: [PATCH 32/38] msp for display and layout --- src/main/config/config.c | 14 +------------- src/main/io/osd.c | 17 +++++++++++++++++ src/main/io/osd.h | 1 + src/main/io/serial_cli.c | 4 +++- src/main/io/serial_msp.c | 27 ++++++++++++++++++++++++--- 5 files changed, 46 insertions(+), 17 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6e803a920b..c0dce56c91 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -421,19 +421,7 @@ static void resetConf(void) #endif #ifdef OSD - featureSet(FEATURE_OSD); - masterConfig.osdProfile.video_system = AUTO; - masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; - masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; - masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; - masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; - masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; - masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; - masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; - masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; - masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; - masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] = 1; - masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; + resetOsdConfig(); #endif #ifdef USE_RTC6705 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8963aa4ba6..e7651c88fc 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -721,4 +721,21 @@ void osdInit(void) } +void resetOsdConfig(void) +{ + featureSet(FEATURE_OSD); + masterConfig.osdProfile.video_system = AUTO; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; + masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; + masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; + masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; + masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; + masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; + masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; + masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; + masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] = -1; + masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; +} + #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 486ff655a9..82932b14c7 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -62,3 +62,4 @@ typedef struct { void updateOsd(void); void osdInit(void); +void resetOsdConfig(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e08e932287..e87ab7ae6c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -204,7 +204,7 @@ static const char * const featureNames[] = { "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", - NULL + "OSD", NULL }; // sync this with rxFailsafeChannelMode_e @@ -819,6 +819,8 @@ const clivalue_t valueTable[] = { { "osd_voltage_warning_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING], .config.minmax = { -480, 480 } }, { "osd_armed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ARMED], .config.minmax = { -480, 480 } }, { "osd_disarmed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_DISARMED], .config.minmax = { -480, 480 } }, + { "osd_artificial_horizon", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { -1, 0 } }, + { "osd_horizon_sidebars", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], .config.minmax = { -1, 0 } }, #endif }; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f1a5e8d318..983cf5884d 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1214,6 +1214,21 @@ static bool processOutCommand(uint8_t cmdMSP) #endif break; + case MSP_OSD_CONFIG: +#ifdef OSD + headSerialReply(2 + (OSD_MAX_ITEMS * 2)); + serialize8(1); // OSD supported + // send video system (AUTO/PAL/NTSC) + serialize8(masterConfig.osdProfile.video_system); + for (i = 0; i < OSD_MAX_ITEMS; i++) { + serialize16(masterConfig.osdProfile.item_pos[i]); + } +#else + headSerialReply(1); + serialize8(0); // OSD not supported +#endif + break; + case MSP_BF_BUILD_INFO: headSerialReply(11 + 4 + 4); for (i = 0; i < 11; i++) @@ -1524,9 +1539,15 @@ static bool processInCommand(void) #endif #ifdef OSD case MSP_SET_OSD_CONFIG: - masterConfig.osdProfile.video_system = read8(); - for (i = 0; i < OSD_MAX_ITEMS; i++) - masterConfig.osdProfile.item_pos[i] = read16(); + addr = read8(); + // set all the other settings + if ((int8_t)addr == -1) { + masterConfig.osdProfile.video_system = read8(); + } + // set a position setting + else { + masterConfig.osdProfile.item_pos[addr] = read16(); + } break; case MSP_OSD_CHAR_WRITE: addr = read8(); From ddf0fb5fdae1084b1ba3283444ccc2c2a9c13558 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Fri, 17 Jun 2016 02:37:41 -0700 Subject: [PATCH 33/38] Use io tags in vtx, move vtx initialization out of OSD init --- Makefile | 4 + src/main/config/config.c | 1 + src/main/config/config.h | 1 + src/main/drivers/max7456.c | 1 + src/main/drivers/vtx_soft_spi_rtc6705.c | 62 ++-- src/main/io/osd.c | 18 -- src/main/main.c | 10 + src/main/target/SIRINFPV/target.h | 12 +- src/main/target/SIRINFPV/target.mk | 2 +- src/test/unit/scheduler_unittest.cc | 407 ++++++++++++++++++++++++ 10 files changed, 448 insertions(+), 70 deletions(-) create mode 100644 src/test/unit/scheduler_unittest.cc diff --git a/Makefile b/Makefile index 0fd2760637..8ef29e0914 100644 --- a/Makefile +++ b/Makefile @@ -537,6 +537,10 @@ ifneq ($(filter VCP,$(FEATURES)),) TARGET_SRC += $(VCP_SRC) endif +ifneq ($(filter VTX_SOFT, $(FEATURES)),) +TARGET_SRC += $(SRC_DIR)/drivers/rtc6705_soft_spi.c +endif + ifneq ($(filter MAX_OSD, $(FEATURES)),) TARGET_SRC += $(SRC_DIR)/drivers/max7456.c \ $(SRC_DIR)/io/osd.c diff --git a/src/main/config/config.c b/src/main/config/config.c index c0dce56c91..8f94f7de61 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -425,6 +425,7 @@ static void resetConf(void) #endif #ifdef USE_RTC6705 + featureSet(FEATURE_VTX); masterConfig.vtx_channel = 19; // default to Boscam E channel 4 masterConfig.vtx_power = 1; #endif diff --git a/src/main/config/config.h b/src/main/config/config.h index 219cc31bea..9150540cfd 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -47,6 +47,7 @@ typedef enum { FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_OSD = 1 << 24, + FEATURE_VTX = 1 << 25, } features_e; void handleOneshotFeatureChangeOnRestart(void); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index a8065b0758..f82473cba6 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -114,6 +114,7 @@ void max7456_init(uint8_t video_system) { DISABLE_MAX7456; delay(100); + // display logo x = 160; for (int i = 1; i < 5; i++) { for (int j = 3; j < 27; j++) diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index b4b8ffce13..9dbd17d856 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -25,18 +25,18 @@ #include "drivers/bus_spi.h" #include "drivers/system.h" -#include "drivers/gpio.h" +#include "drivers/light_led.h" #include "vtx_soft_spi_rtc6705.h" -#define RTC6705_SPICLK_ON GPIO_SetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) -#define RTC6705_SPICLK_OFF GPIO_ResetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) +#define RTC6705_SPICLK_ON IOHi(rtc6705ClkPin) +#define RTC6705_SPICLK_OFF IOLo(rtc6705ClkPin) -#define RTC6705_SPIDATA_ON GPIO_SetBits(RTC6705_SPIDATA_GPIO, RTC6705_SPIDATA_PIN) -#define RTC6705_SPIDATA_OFF GPIO_ResetBits(RTC6705_SPIDATA_GPIO, RTC6705_SPIDATA_PIN) +#define RTC6705_SPIDATA_ON IOHi(rtc6705DataPin) +#define RTC6705_SPIDATA_OFF IOLo(rtc6705DataPin) -#define RTC6705_SPILE_ON GPIO_SetBits(RTC6705_SPILE_GPIO, RTC6705_SPILE_PIN) -#define RTC6705_SPILE_OFF GPIO_ResetBits(RTC6705_SPILE_GPIO, RTC6705_SPILE_PIN) +#define RTC6705_SPILE_ON IOHi(rtc6705LePin) +#define RTC6705_SPILE_OFF IOLo(rtc6705LePin) char *vtx_bands[] = { "BOSCAM A", @@ -57,46 +57,24 @@ uint16_t vtx_freq[] = uint16_t current_vtx_channel; +static IO_t rtc6705DataPin = IO_NONE; +static IO_t rtc6705LePin = IO_NONE; +static IO_t rtc6705ClkPin = IO_NONE; + void rtc6705_soft_spi_init(void) { - gpio_config_t gpio; -#ifdef STM32F303 - #ifdef RTC6705_SPICLK_PERIPHERAL - RCC_AHBPeriphClockCmd(RTC6705_SPICLK_PERIPHERAL, ENABLE); - #endif - #ifdef RTC6705_SPILE_PERIPHERAL - RCC_AHBPeriphClockCmd(RTC6705_SPILE_PERIPHERAL, ENABLE); - #endif - #ifdef RTC6705_SPIDATA_PERIPHERAL - RCC_AHBPeriphClockCmd(RTC6705_SPIDATA_PERIPHERAL, ENABLE); - #endif -#endif -#ifdef STM32F10X - #ifdef RTC6705_SPICLK_PERIPHERAL - RCC_APB2PeriphClockCmd(RTC6705_SPICLK_PERIPHERAL, ENABLE); - #endif - #ifdef RTC6705_SPILE_PERIPHERAL - RCC_APB2PeriphClockCmd(RTC6705_SPILE_PERIPHERAL, ENABLE); - #endif - #ifdef RTC6705_SPIDATA_PERIPHERAL - RCC_APB2PeriphClockCmd(RTC6705_SPIDATA_PERIPHERAL, ENABLE); - #endif -#endif + rtc6705DataPin = IOGetByTag(IO_TAG(RTC6705_SPIDATA_PIN)); + rtc6705LePin = IOGetByTag(IO_TAG(RTC6705_SPILE_PIN)); + rtc6705ClkPin = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN)); - gpio.pin = RTC6705_SPICLK_PIN; - gpio.speed = Speed_50MHz; - gpio.mode = Mode_Out_PP; - gpioInit(RTC6705_SPICLK_GPIO, &gpio); + IOInit(rtc6705DataPin, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP); - gpio.pin = RTC6705_SPILE_PIN; - gpio.speed = Speed_50MHz; - gpio.mode = Mode_Out_PP; - gpioInit(RTC6705_SPILE_GPIO, &gpio); + IOInit(rtc6705LePin, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(rtc6705LePin, IOCFG_OUT_PP); - gpio.pin = RTC6705_SPIDATA_PIN; - gpio.speed = Speed_50MHz; - gpio.mode = Mode_Out_PP; - gpioInit(RTC6705_SPIDATA_GPIO, &gpio); + IOInit(rtc6705ClkPin, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP); } static void rtc6705_write_register(uint8_t addr, uint32_t data) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e7651c88fc..f23134d89f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -347,17 +347,6 @@ void print_batt_voltage(uint16_t pos, uint8_t col) { max7456_write_string(string_buffer, pos); } -/* - TODO: add this to menu - { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } }, - { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, - { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, - { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, - { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, - { "acro_plus_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } }, - { "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } }, -*/ - osd_page_t menu_pages[] = { { .title = "STATUS", @@ -711,14 +700,7 @@ void updateOsd(void) void osdInit(void) { -#ifdef USE_RTC6705 - rtc6705_soft_spi_init(); - current_vtx_channel = masterConfig.vtx_channel; - rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); - rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); -#endif max7456_init(masterConfig.osdProfile.video_system); - } void resetOsdConfig(void) diff --git a/src/main/main.c b/src/main/main.c index e221da6dcb..6ab9e369f7 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -55,6 +55,7 @@ #include "drivers/transponder_ir.h" #include "drivers/io.h" #include "drivers/exti.h" +#include "drivers/vtx_soft_spi_rtc6705.h" #ifdef USE_BST #include "bus_bst.h" @@ -469,6 +470,15 @@ void init(void) } #endif +#ifdef USE_RTC6705 + if (feature(feature(FEATURE_VTX))) { + rtc6705_soft_spi_init(); + current_vtx_channel = masterConfig.vtx_channel; + rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); + rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); + } +#endif + #ifdef OSD if (feature(FEATURE_OSD)) { osdInit(); diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 7b98c90619..667f6d173b 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -113,15 +113,9 @@ #define MAX7456_SPI_CS_PIN PA15 #define USE_RTC6705 -#define RTC6705_SPIDATA_GPIO GPIOC -#define RTC6705_SPIDATA_PIN Pin_15 -#define RTC6705_SPIDATA_PERIPHERAL RCC_AHBPeriph_GPIOC -#define RTC6705_SPILE_GPIO GPIOC -#define RTC6705_SPILE_PIN Pin_14 -#define RTC6705_SPILE_PERIPHERAL RCC_AHBPeriph_GPIOC -#define RTC6705_SPICLK_GPIO GPIOC -#define RTC6705_SPICLK_PIN Pin_13 -#define RTC6705_SPICLK_PERIPHERAL RCC_AHBPeriph_GPIOC +#define RTC6705_SPIDATA_PIN PC15 +#define RTC6705_SPILE_PIN PC14 +#define RTC6705_SPICLK_PIN PC13 #define USE_SDCARD #define USE_SDCARD_SPI2 diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 1dfe1ea325..39004bec7e 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD MAX_OSD +FEATURES = VCP SDCARD MAX_OSD SOFT_VTX TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc new file mode 100644 index 0000000000..d998f2f4ec --- /dev/null +++ b/src/test/unit/scheduler_unittest.cc @@ -0,0 +1,407 @@ +/* + * 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 + +extern "C" { + #include "platform.h" + #include "scheduler/scheduler.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" +enum { + systemTime = 10, + pidLoopCheckerTime = 650, + updateAccelerometerTime = 192, + handleSerialTime = 30, + updateBeeperTime = 1, + updateBatteryTime = 1, + updateRxCheckTime = 34, + updateRxMainTime = 10, + processGPSTime = 10, + updateCompassTime = 195, + updateBaroTime = 201, + updateSonarTime = 10, + calculateAltitudeTime = 154, + updateDisplayTime = 10, + telemetryTime = 10, + ledStripTime = 10, + transponderTime = 10 +}; + +extern "C" { + cfTask_t * unittest_scheduler_selectedTask; + uint8_t unittest_scheduler_selectedTaskDynPrio; + uint16_t unittest_scheduler_waitingTasks; + uint32_t unittest_scheduler_timeToNextRealtimeTask; + bool unittest_outsideRealtimeGuardInterval; + +// set up micros() to simulate time + uint32_t simulatedTime = 0; + uint32_t micros(void) {return simulatedTime;} +// set up tasks to take a simulated representative time to execute + void taskMainPidLoopChecker(void) {simulatedTime+=pidLoopCheckerTime;} + void taskUpdateAccelerometer(void) {simulatedTime+=updateAccelerometerTime;} + void taskHandleSerial(void) {simulatedTime+=handleSerialTime;} + void taskUpdateBeeper(void) {simulatedTime+=updateBeeperTime;} + void taskUpdateBattery(void) {simulatedTime+=updateBatteryTime;} + bool taskUpdateRxCheck(uint32_t currentDeltaTime) {UNUSED(currentDeltaTime);simulatedTime+=updateRxCheckTime;return false;} + void taskUpdateRxMain(void) {simulatedTime+=updateRxMainTime;} + void taskProcessGPS(void) {simulatedTime+=processGPSTime;} + void taskUpdateCompass(void) {simulatedTime+=updateCompassTime;} + void taskUpdateBaro(void) {simulatedTime+=updateBaroTime;} + void taskUpdateSonar(void) {simulatedTime+=updateSonarTime;} + void taskCalculateAltitude(void) {simulatedTime+=calculateAltitudeTime;} + void taskUpdateDisplay(void) {simulatedTime+=updateDisplayTime;} + void taskTelemetry(void) {simulatedTime+=telemetryTime;} + void taskLedStrip(void) {simulatedTime+=ledStripTime;} + void taskTransponder(void) {simulatedTime+=transponderTime;} + + extern cfTask_t* taskQueueArray[]; + + extern void queueClear(void); + extern int queueSize(); + extern bool queueContains(cfTask_t *task); + extern bool queueAdd(cfTask_t *task); + extern bool queueRemove(cfTask_t *task); + extern cfTask_t *queueFirst(void); + extern cfTask_t *queueNext(void); +} + +TEST(SchedulerUnittest, TestPriorites) +{ + EXPECT_EQ(14, TASK_COUNT); + // if any of these fail then task priorities have changed and ordering in TestQueue needs to be re-checked + EXPECT_EQ(TASK_PRIORITY_HIGH, cfTasks[TASK_SYSTEM].staticPriority); + EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority); + EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority); + EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority); + EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY].staticPriority); +} + +TEST(SchedulerUnittest, TestQueueInit) +{ + queueClear(); + EXPECT_EQ(0, queueSize()); + EXPECT_EQ(0, queueFirst()); + EXPECT_EQ(0, queueNext()); + for (int ii = 0; ii <= TASK_COUNT; ++ii) { + EXPECT_EQ(0, taskQueueArray[ii]); + } +} + +cfTask_t *deadBeefPtr = reinterpret_cast(0xDEADBEEF); + +TEST(SchedulerUnittest, TestQueue) +{ + queueClear(); + taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; + + queueAdd(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH + EXPECT_EQ(1, queueSize()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME + EXPECT_EQ(2, queueSize()); + EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); + EXPECT_EQ(NULL, queueNext()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW + EXPECT_EQ(3, queueSize()); + EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); + EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); + EXPECT_EQ(NULL, queueNext()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + queueAdd(&cfTasks[TASK_BATTERY]); // TASK_PRIORITY_MEDIUM + EXPECT_EQ(4, queueSize()); + EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); + EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext()); + EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); + EXPECT_EQ(NULL, queueNext()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH + EXPECT_EQ(5, queueSize()); + EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); + EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); + EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext()); + EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); + EXPECT_EQ(NULL, queueNext()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH + EXPECT_EQ(4, queueSize()); + EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); + EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext()); + EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); + EXPECT_EQ(NULL, queueNext()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); +} + +TEST(SchedulerUnittest, TestQueueAddAndRemove) +{ + queueClear(); + taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; + + // fill up the queue + for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { + const bool added = queueAdd(&cfTasks[taskId]); + EXPECT_EQ(true, added); + EXPECT_EQ(taskId + 1, queueSize()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + } + // double check end of queue + EXPECT_EQ(TASK_COUNT, queueSize()); + EXPECT_NE(static_cast(0), taskQueueArray[TASK_COUNT - 1]); // last item was indeed added to queue + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // null pointer at end of queue is preserved + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // there hasn't been an out by one error + + // and empty it again + for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { + const bool removed = queueRemove(&cfTasks[taskId]); + EXPECT_EQ(true, removed); + EXPECT_EQ(TASK_COUNT - taskId - 1, queueSize()); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + } + // double check size and end of queue + EXPECT_EQ(0, queueSize()); // queue is indeed empty + EXPECT_EQ(NULL, taskQueueArray[0]); // there is a null pointer at the end of the queueu + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // no accidental overwrites past end of queue +} + +TEST(SchedulerUnittest, TestQueueArray) +{ + // test there are no "out by one" errors or buffer overruns when items are added and removed + queueClear(); + taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; // note, must set deadBeefPtr after queueClear + + for (int taskId = 0; taskId < TASK_COUNT - 1; ++taskId) { + setTaskEnabled(static_cast(taskId), true); + EXPECT_EQ(taskId + 1, queueSize()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + } + EXPECT_EQ(TASK_COUNT - 1, queueSize()); + EXPECT_NE(static_cast(0), taskQueueArray[TASK_COUNT - 2]); + const cfTask_t *lastTaskPrev = taskQueueArray[TASK_COUNT - 2]; + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + setTaskEnabled(TASK_SYSTEM, false); + EXPECT_EQ(TASK_COUNT - 2, queueSize()); + EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); // NULL at end of queue + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + taskQueueArray[TASK_COUNT - 2] = 0; + setTaskEnabled(TASK_SYSTEM, true); + EXPECT_EQ(TASK_COUNT - 1, queueSize()); + EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 2]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + cfTaskInfo_t taskInfo; + getTaskInfo(static_cast(TASK_COUNT - 1), &taskInfo); + EXPECT_EQ(false, taskInfo.isEnabled); + setTaskEnabled(static_cast(TASK_COUNT - 1), true); + EXPECT_EQ(TASK_COUNT, queueSize()); + EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // check no buffer overrun + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + setTaskEnabled(TASK_SYSTEM, false); + EXPECT_EQ(TASK_COUNT - 1, queueSize()); + //EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + setTaskEnabled(TASK_ACCEL, false); + EXPECT_EQ(TASK_COUNT - 2, queueSize()); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + setTaskEnabled(TASK_BATTERY, false); + EXPECT_EQ(TASK_COUNT - 3, queueSize()); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 3]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); +} + +TEST(SchedulerUnittest, TestSchedulerInit) +{ + schedulerInit(); + EXPECT_EQ(1, queueSize()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); +} + +TEST(SchedulerUnittest, TestScheduleEmptyQueue) +{ + queueClear(); + simulatedTime = 4000; + // run the with an empty queue + scheduler(); + EXPECT_EQ(NULL, unittest_scheduler_selectedTask); +} + +TEST(SchedulerUnittest, TestSingleTask) +{ + schedulerInit(); + // disable all tasks except TASK_GYROPID + for (int taskId=0; taskId < TASK_COUNT; ++taskId) { + setTaskEnabled(static_cast(taskId), false); + } + setTaskEnabled(TASK_GYROPID, true); + cfTasks[TASK_GYROPID].lastExecutedAt = 1000; + simulatedTime = 4000; + // run the scheduler and check the task has executed + scheduler(); + EXPECT_NE(static_cast(0), unittest_scheduler_selectedTask); + EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime); + EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt); + EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_GYROPID].totalExecutionTime); + // task has run, so its dynamic priority should have been set to zero + EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority); +} + +TEST(SchedulerUnittest, TestTwoTasks) +{ + // disable all tasks except TASK_GYROPID and TASK_ACCEL + for (int taskId=0; taskId < TASK_COUNT; ++taskId) { + setTaskEnabled(static_cast(taskId), false); + } + setTaskEnabled(TASK_ACCEL, true); + setTaskEnabled(TASK_GYROPID, true); + + // set it up so that TASK_ACCEL ran just before TASK_GYROPID + static const uint32_t startTime = 4000; + simulatedTime = startTime; + cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime; + cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - updateAccelerometerTime; + EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles); + // run the scheduler + scheduler(); + // no tasks should have run, since neither task's desired time has elapsed + EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); + + // NOTE: + // TASK_GYROPID desiredPeriod is 1000 microseconds + // TASK_ACCEL desiredPeriod is 10000 microseconds + // 500 microseconds later + simulatedTime += 500; + // no tasks should run, since neither task's desired time has elapsed + scheduler(); + EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); + EXPECT_EQ(0, unittest_scheduler_waitingTasks); + + // 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed + simulatedTime += 500; + // TASK_GYROPID should now run + scheduler(); + EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + EXPECT_EQ(1, unittest_scheduler_waitingTasks); + EXPECT_EQ(5000 + pidLoopCheckerTime, simulatedTime); + + simulatedTime += 1000 - pidLoopCheckerTime; + scheduler(); + // TASK_GYROPID should run again + EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + + scheduler(); + EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); + EXPECT_EQ(0, unittest_scheduler_waitingTasks); + + simulatedTime = startTime + 10500; // TASK_GYROPID and TASK_ACCEL desiredPeriods have elapsed + // of the two TASK_GYROPID should run first + scheduler(); + EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + // and finally TASK_ACCEL should now run + scheduler(); + EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); +} + +TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun) +{ + // disable all tasks except TASK_GYROPID and TASK_SYSTEM + for (int taskId=0; taskId < TASK_COUNT; ++taskId) { + setTaskEnabled(static_cast(taskId), false); + } + setTaskEnabled(TASK_GYROPID, true); + cfTasks[TASK_GYROPID].lastExecutedAt = 200000; + simulatedTime = 200700; + + setTaskEnabled(TASK_SYSTEM, true); + cfTasks[TASK_SYSTEM].lastExecutedAt = 100000; + + scheduler(); + + EXPECT_EQ(false, unittest_outsideRealtimeGuardInterval); + EXPECT_EQ(300, unittest_scheduler_timeToNextRealtimeTask); + + // Nothing should be scheduled in guard period + EXPECT_EQ(NULL, unittest_scheduler_selectedTask); + EXPECT_EQ(100000, cfTasks[TASK_SYSTEM].lastExecutedAt); + + EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); +} + +TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun) +{ + // disable all tasks except TASK_GYROPID and TASK_SYSTEM + for (int taskId=0; taskId < TASK_COUNT; ++taskId) { + setTaskEnabled(static_cast(taskId), false); + } + setTaskEnabled(TASK_GYROPID, true); + cfTasks[TASK_GYROPID].lastExecutedAt = 200000; + simulatedTime = 200699; + + setTaskEnabled(TASK_SYSTEM, true); + cfTasks[TASK_SYSTEM].lastExecutedAt = 100000; + + scheduler(); + + EXPECT_EQ(true, unittest_outsideRealtimeGuardInterval); + EXPECT_EQ(301, unittest_scheduler_timeToNextRealtimeTask); + + // System should be scheduled as not in guard period + EXPECT_EQ(&cfTasks[TASK_SYSTEM], unittest_scheduler_selectedTask); + EXPECT_EQ(200699, cfTasks[TASK_SYSTEM].lastExecutedAt); + + EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); +} + +// STUBS +extern "C" { +} From 135ffc1c92dd3b62bab0605d65a480c95d1ae100 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Sat, 18 Jun 2016 00:33:10 -0700 Subject: [PATCH 34/38] cleanup config.c, Makefile, OSD, and VTX --- Makefile | 9 -- src/main/config/config.c | 6 - src/main/drivers/max7456.c | 20 +--- src/main/drivers/max7456.h | 2 +- src/main/drivers/vtx_soft_spi_rtc6705.c | 24 ++-- src/main/drivers/vtx_soft_spi_rtc6705.h | 21 ++-- src/main/io/osd.c | 147 +++++++++++++++--------- src/main/io/osd.h | 2 +- src/main/main.c | 2 +- src/main/target/SIRINFPV/target.h | 2 +- src/main/target/SIRINFPV/target.mk | 6 +- 11 files changed, 123 insertions(+), 118 deletions(-) diff --git a/Makefile b/Makefile index 8ef29e0914..c083a61906 100644 --- a/Makefile +++ b/Makefile @@ -536,15 +536,6 @@ endif ifneq ($(filter VCP,$(FEATURES)),) TARGET_SRC += $(VCP_SRC) endif - -ifneq ($(filter VTX_SOFT, $(FEATURES)),) -TARGET_SRC += $(SRC_DIR)/drivers/rtc6705_soft_spi.c -endif - -ifneq ($(filter MAX_OSD, $(FEATURES)),) -TARGET_SRC += $(SRC_DIR)/drivers/max7456.c \ - $(SRC_DIR)/io/osd.c -endif # end target specific make file checks diff --git a/src/main/config/config.c b/src/main/config/config.c index 8f94f7de61..6cc4d50188 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -424,12 +424,6 @@ static void resetConf(void) resetOsdConfig(); #endif -#ifdef USE_RTC6705 - featureSet(FEATURE_VTX); - masterConfig.vtx_channel = 19; // default to Boscam E channel 4 - masterConfig.vtx_power = 1; -#endif - #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index f82473cba6..b8a59bcfc9 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -36,8 +36,6 @@ #define DISABLE_MAX7456 IOHi(max7456CsPin) #define ENABLE_MAX7456 IOLo(max7456CsPin) -static IO_t max7456CsPin = IO_NONE; - /** Artificial Horizon limits **/ #define AHIPITCHMAX 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees #define AHIROLLMAX 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees @@ -45,10 +43,11 @@ static IO_t max7456CsPin = IO_NONE; #define AHISIDEBARHEIGHTPOSITION 3 uint16_t max_screen_size; -uint8_t video_signal_type = 0; -uint8_t max7456_lock = 0; char max7456_screen[VIDEO_BUFFER_CHARS_PAL]; +static uint8_t video_signal_type = 0; +static uint8_t max7456_lock = 0; +static IO_t max7456CsPin = IO_NONE; uint8_t max7456_send(uint8_t add, uint8_t data) { spiTransferByte(MAX7456_SPI_INSTANCE, add); @@ -113,19 +112,6 @@ void max7456_init(uint8_t video_system) { DISABLE_MAX7456; delay(100); - - // display logo - x = 160; - for (int i = 1; i < 5; i++) { - for (int j = 3; j < 27; j++) - max7456_screen[i * LINE + j] = (char)x++; - } - tfp_sprintf(buf, "BF VERSION: %s", FC_VERSION_STRING); - max7456_write_string(buf, LINE06+5); - max7456_write_string("MENU: THRT MID", LINE07+7); - max7456_write_string("YAW RIGHT", LINE08+13); - max7456_write_string("PITCH UP", LINE09+13); - max7456_draw_screen(); } // Copy string from ram into screen buffer diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index cd33eae5e9..1e04320a88 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -143,7 +143,7 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC }; extern uint16_t max_screen_size; -char max7456_screen[VIDEO_BUFFER_CHARS_PAL]; +extern char max7456_screen[VIDEO_BUFFER_CHARS_PAL]; void max7456_init(uint8_t system); diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index 9dbd17d856..cdeb23d5b4 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -38,21 +38,13 @@ #define RTC6705_SPILE_ON IOHi(rtc6705LePin) #define RTC6705_SPILE_OFF IOLo(rtc6705LePin) -char *vtx_bands[] = { - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; - -uint16_t vtx_freq[] = +const uint16_t vtx_freq[] = { - 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, - 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, - 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, - 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, - 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, + 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, // Boacam A + 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, // Boscam B + 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, // Boscam E + 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, // FatShark + 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, // RaceBand }; uint16_t current_vtx_channel; @@ -127,8 +119,8 @@ void rtc6705_soft_spi_set_channel(uint16_t channel_freq) { rtc6705_write_register(1, (N << 7) | A); } -void rtc6705_soft_spi_set_rf_power(uint8_t power) { - rtc6705_write_register(7, (power ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)))); +void rtc6705_soft_spi_set_rf_power(uint8_t reduce_power) { + rtc6705_write_register(7, (reduce_power ? (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)) : PA_CONTROL_DEFAULT)); } #endif diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.h b/src/main/drivers/vtx_soft_spi_rtc6705.h index 5f4314de1d..9d67786c3a 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.h +++ b/src/main/drivers/vtx_soft_spi_rtc6705.h @@ -17,21 +17,22 @@ #pragma once -#define DP_5G_MASK 0x7000 -#define PA5G_BS_MASK 0x0E00 -#define PA5G_PW_MASK 0x0180 -#define PD_Q5G_MASK 0x0040 -#define QI_5G_MASK 0x0038 -#define PA_BS_MASK 0x0007 +#define DP_5G_MASK 0x7000 +#define PA5G_BS_MASK 0x0E00 +#define PA5G_PW_MASK 0x0180 +#define PD_Q5G_MASK 0x0040 +#define QI_5G_MASK 0x0038 +#define PA_BS_MASK 0x0007 -#define PA_CONTROL_DEFAULT 0x4FBD +#define PA_CONTROL_DEFAULT 0x4FBD +#define CHANNELS_PER_BAND 8 +#define BANDS_NUMBER 5 -extern char* vtx_bands[]; -extern uint16_t vtx_freq[]; +extern uint16_t const vtx_freq[]; extern uint16_t current_vtx_channel; void rtc6705_soft_spi_init(void); void rtc6705_soft_spi_set_channel(uint16_t channel_freq); -void rtc6705_soft_spi_set_rf_power(uint8_t power); +void rtc6705_soft_spi_set_rf_power(uint8_t reduce_power); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f23134d89f..db4c8ad4e0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -22,6 +22,7 @@ #include #include "platform.h" +#include "version.h" #include "scheduler/scheduler.h" #include "common/axis.h" @@ -123,57 +124,73 @@ static uint32_t next_osd_update_at = 0; -static uint32_t armed_seconds = 0; -static uint32_t armed_at = 0; -static bool armed = false; +static uint32_t armed_seconds = 0; +static uint32_t armed_at = 0; +static uint8_t armed = 0; + +static uint8_t current_page = 0; +static uint8_t sticks[] = {0,0,0,0}; + +static uint8_t cursor_row = 255; +static uint8_t cursor_col = 0; +static uint8_t in_menu = 0; +static uint8_t activating_menu = 0; -static uint8_t current_page = 0; -static uint8_t sticks[] = {0,0,0,0}; static char string_buffer[30]; -static uint8_t cursor_row = 255; -static uint8_t cursor_col = 0; -static bool in_menu = false; -static bool activating_menu = false; + extern uint16_t rssi; +enum { + MENU_VALUE_DECREASE = 0, + MENU_VALUE_INCREASE, +}; #ifdef USE_RTC6705 -void update_vtx_band(bool increase, uint8_t col) { - (void)col; - if (increase) { - if (current_vtx_channel < 32) - current_vtx_channel += 8; + +static const char *vtx_bands[] = { + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +void update_vtx_band(int value_change_direction, uint8_t col) { + UNUSED(col); + if (value_change_direction) { + if (current_vtx_channel < (BANDS_NUMBER * CHANNELS_PER_BAND - CHANNELS_PER_BAND)) + current_vtx_channel += CHANNELS_PER_BAND; } else { - if (current_vtx_channel > 7) - current_vtx_channel -= 8; + if (current_vtx_channel >= CHANNELS_PER_BAND) + current_vtx_channel -= CHANNELS_PER_BAND; } } void print_vtx_band(uint16_t pos, uint8_t col) { - (void)col; - sprintf(string_buffer, "%s", vtx_bands[current_vtx_channel / 8]); + UNUSED(col); + sprintf(string_buffer, "%s", vtx_bands[current_vtx_channel / CHANNELS_PER_BAND]); max7456_write_string(string_buffer, pos); } -void update_vtx_channel(bool increase, uint8_t col) { - (void)col; - if (increase) { - if ((current_vtx_channel % 8) < 7) +void update_vtx_channel(int value_change_direction, uint8_t col) { + UNUSED(col); + if (value_change_direction) { + if ((current_vtx_channel % CHANNELS_PER_BAND) < (CHANNELS_PER_BAND - 1)) current_vtx_channel++; } else { - if ((current_vtx_channel % 8) > 0) + if ((current_vtx_channel % CHANNELS_PER_BAND) > 0) current_vtx_channel--; } } void print_vtx_channel(uint16_t pos, uint8_t col) { - (void)col; - sprintf(string_buffer, "%d", current_vtx_channel % 8 + 1); + UNUSED(col); + sprintf(string_buffer, "%d", current_vtx_channel % CHANNELS_PER_BAND + 1); max7456_write_string(string_buffer, pos); } void print_vtx_freq(uint16_t pos, uint8_t col) { - (void)col; + UNUSED(col); sprintf(string_buffer, "%d M", vtx_freq[current_vtx_channel]); max7456_write_string(string_buffer, pos); } @@ -243,8 +260,9 @@ void print_tpa_brkpt(uint16_t pos, uint8_t col) { } } -void update_int_pid(bool inc, uint8_t col, int pid_term) { +void update_int_pid(int value_change_direction, uint8_t col, int pid_term) { void* ptr; + switch(col) { case 0: ptr = ¤tProfile->pidProfile.P8[pid_term]; @@ -259,7 +277,7 @@ void update_int_pid(bool inc, uint8_t col, int pid_term) { return; } - if (inc) { + if (value_change_direction) { if (*(uint8_t*)ptr < 200) *(uint8_t*)ptr += 1; } else { @@ -268,21 +286,22 @@ void update_int_pid(bool inc, uint8_t col, int pid_term) { } } -void update_roll_pid(bool inc, uint8_t col) { - update_int_pid(inc, col, ROLL); +void update_roll_pid(int value_change_direction, uint8_t col) { + update_int_pid(value_change_direction, col, ROLL); } -void update_pitch_pid(bool inc, uint8_t col) { - update_int_pid(inc, col, PITCH); +void update_pitch_pid(int value_change_direction, uint8_t col) { + update_int_pid(value_change_direction, col, PITCH); } -void update_yaw_pid(bool inc, uint8_t col) { - update_int_pid(inc, col, YAW); +void update_yaw_pid(int value_change_direction, uint8_t col) { + update_int_pid(value_change_direction, col, YAW); } -void update_roll_rate(bool inc, uint8_t col) { - (void)col; - if (inc) { +void update_roll_rate(int value_change_direction, uint8_t col) { + UNUSED(col); + + if (value_change_direction) { if (currentControlRateProfile->rates[FD_ROLL] < CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX) currentControlRateProfile->rates[FD_ROLL]++; } else { @@ -291,9 +310,10 @@ void update_roll_rate(bool inc, uint8_t col) { } } -void update_pitch_rate(bool increase, uint8_t col) { - (void)col; - if (increase) { +void update_pitch_rate(int value_change_direction, uint8_t col) { + UNUSED(col); + + if (value_change_direction) { if (currentControlRateProfile->rates[FD_PITCH] < CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX) currentControlRateProfile->rates[FD_PITCH]++; } else { @@ -302,9 +322,10 @@ void update_pitch_rate(bool increase, uint8_t col) { } } -void update_yaw_rate(bool increase, uint8_t col) { - (void)col; - if (increase) { +void update_yaw_rate(int value_change_direction, uint8_t col) { + UNUSED(col); + + if (value_change_direction) { if (currentControlRateProfile->rates[FD_YAW] < CONTROL_RATE_CONFIG_YAW_RATE_MAX) currentControlRateProfile->rates[FD_YAW]++; } else { @@ -313,9 +334,10 @@ void update_yaw_rate(bool increase, uint8_t col) { } } -void update_tpa_rate(bool increase, uint8_t col) { - (void)col; - if (increase) { +void update_tpa_rate(int value_change_direction, uint8_t col) { + UNUSED(col); + + if (value_change_direction) { if (currentControlRateProfile->dynThrPID < CONTROL_RATE_CONFIG_TPA_MAX) currentControlRateProfile->dynThrPID++; } else { @@ -324,9 +346,10 @@ void update_tpa_rate(bool increase, uint8_t col) { } } -void update_tpa_brkpt(bool increase, uint8_t col) { - (void)col; - if (increase) { +void update_tpa_brkpt(int value_change_direction, uint8_t col) { + UNUSED(col); + + if (value_change_direction) { if (currentControlRateProfile->tpa_breakpoint < PWM_RANGE_MAX) currentControlRateProfile->tpa_breakpoint++; } else { @@ -336,13 +359,15 @@ void update_tpa_brkpt(bool increase, uint8_t col) { } void print_average_system_load(uint16_t pos, uint8_t col) { - (void)col; + UNUSED(col); + sprintf(string_buffer, "%d", averageSystemLoadPercent); max7456_write_string(string_buffer, pos); } void print_batt_voltage(uint16_t pos, uint8_t col) { - (void)col; + UNUSED(col); + sprintf(string_buffer, "%d.%1d", vbat / 10, vbat % 10); max7456_write_string(string_buffer, pos); } @@ -503,7 +528,7 @@ void show_menu(void) { } } else { if (menu_pages[current_page].rows[cursor_row].update) - menu_pages[current_page].rows[cursor_row].update(true, cursor_col); + menu_pages[current_page].rows[cursor_row].update(MENU_VALUE_INCREASE, cursor_col); } } @@ -514,7 +539,7 @@ void show_menu(void) { } } else { if (menu_pages[current_page].rows[cursor_row].update) - menu_pages[current_page].rows[cursor_row].update(false, cursor_col); + menu_pages[current_page].rows[cursor_row].update(MENU_VALUE_DECREASE, cursor_col); } } @@ -697,10 +722,24 @@ void updateOsd(void) } } - void osdInit(void) { + uint16_t x; + max7456_init(masterConfig.osdProfile.video_system); + + // display logo and help + x = 160; + for (int i = 1; i < 5; i++) { + for (int j = 3; j < 27; j++) + max7456_screen[i * LINE + j] = (char)x++; + } + sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); + max7456_write_string(string_buffer, LINE06 + 5); + max7456_write_string("MENU: THRT MID", LINE07 + 7); + max7456_write_string("YAW RIGHT", LINE08 + 13); + max7456_write_string("PITCH UP", LINE09 + 13); + max7456_draw_screen(); } void resetOsdConfig(void) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 82932b14c7..e348e4573c 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -25,7 +25,7 @@ typedef struct { typedef struct { const char* title; - void (*update)(bool increase, uint8_t col); + void (*update)(int value_change_direction, uint8_t col); void (*print)(uint16_t pos, uint8_t col); } osd_row_t; diff --git a/src/main/main.c b/src/main/main.c index 6ab9e369f7..5aa3151e39 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -471,7 +471,7 @@ void init(void) #endif #ifdef USE_RTC6705 - if (feature(feature(FEATURE_VTX))) { + if (feature(FEATURE_VTX)) { rtc6705_soft_spi_init(); current_vtx_channel = masterConfig.vtx_channel; rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 667f6d173b..c27b1ec689 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -135,7 +135,7 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING - +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 39004bec7e..945fb1fe9d 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD MAX_OSD SOFT_VTX +FEATURES = VCP SDCARD TARGET_SRC = \ drivers/accgyro_mpu.c \ @@ -12,5 +12,7 @@ TARGET_SRC = \ drivers/flash_m25p16.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ - drivers/vtx_soft_spi_rtc6705.c + drivers/vtx_soft_spi_rtc6705.c \ + drivers/max7456.c \ + io/osd.c From dbb5386f1b27c73a0bf1c7f74a0aa22d3a30b448 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 18 Jun 2016 21:40:32 +1000 Subject: [PATCH 35/38] Removed need to specify timer peripherals in the target.h files. Now present in the MCU timer specific files. --- src/main/drivers/timer.c | 64 ++++++++++++----------- src/main/drivers/timer.h | 21 ++++++++ src/main/drivers/timer_stm32f10x.c | 23 ++++++++ src/main/drivers/timer_stm32f30x.c | 16 ++++++ src/main/drivers/timer_stm32f4xx.c | 20 +++++++ src/main/target/ALIENFLIGHTF3/target.h | 6 +-- src/main/target/ALIENFLIGHTF4/target.h | 5 +- src/main/target/BLUEJAYF4/target.c | 12 ++--- src/main/target/BLUEJAYF4/target.h | 2 - src/main/target/CC3D/target.h | 7 +-- src/main/target/CHEBUZZF3/target.h | 4 -- src/main/target/CJMCU/target.h | 6 +-- src/main/target/COLIBRI_RACE/target.h | 4 -- src/main/target/DOGE/target.h | 6 +-- src/main/target/EUSTM32F103RC/target.h | 5 +- src/main/target/FURYF3/target.h | 6 +-- src/main/target/FURYF4/target.h | 3 -- src/main/target/IRCFUSIONF3/target.h | 4 -- src/main/target/KISSFC/target.h | 7 +-- src/main/target/LUX_RACE/target.h | 4 -- src/main/target/MOTOLAB/target.h | 5 +- src/main/target/NAZE/target.h | 8 +-- src/main/target/OLIMEXINO/target.h | 6 +-- src/main/target/PIKOBLX/target.h | 4 -- src/main/target/PORT103R/target.h | 4 +- src/main/target/REVO/target.h | 4 -- src/main/target/REVONANO/target.h | 2 - src/main/target/RMDO/target.h | 7 +-- src/main/target/SINGULARITY/target.h | 4 -- src/main/target/SPARKY/target.h | 5 +- src/main/target/SPRACINGF3/target.h | 5 +- src/main/target/SPRACINGF3EVO/target.h | 6 +-- src/main/target/SPRACINGF3MINI/target.h | 4 -- src/main/target/STM32F3DISCOVERY/target.h | 4 -- 34 files changed, 138 insertions(+), 155 deletions(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 8e78241681..ba732d42f9 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -27,6 +27,8 @@ #include "nvic.h" #include "gpio.h" +#include "gpio.h" +#include "rcc.h" #include "system.h" #include "timer.h" @@ -51,7 +53,7 @@ typedef struct timerConfig_s { timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER]; timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER]; timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks - uint32_t forcedOverflowTimerValue; + uint32_t forcedOverflowTimerValue; } timerConfig_t; timerConfig_t timerConfig[USED_TIMER_COUNT]; @@ -141,6 +143,16 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) return channel >> 2; } +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; +} + void timerNVICConfigure(uint8_t irq) { NVIC_InitTypeDef NVIC_InitStructure; @@ -162,19 +174,19 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) // "The counter clock frequency (CK_CNT) is equal to f CK_PSC / (PSC[15:0] + 1)." - STM32F10x Reference Manual 14.4.11 // Thus for 1Mhz: 72000000 / 1000000 = 72, 72 - 1 = 71 = TIM_Prescaler #if defined (STM32F40_41xxx) - if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) { - TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; - } - else { - TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 2 / ((uint32_t)mhz * 1000000)) - 1; - } + if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) { + TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; + } + else { + TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 2 / ((uint32_t)mhz * 1000000)) - 1; + } #elif defined (STM32F411xE) - if (tim == TIM1 || tim == TIM9 || tim == TIM10 || tim == TIM11) { - TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; - } - else { - TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 2 / ((uint32_t)mhz * 1000000)) - 1; - } + if (tim == TIM1 || tim == TIM9 || tim == TIM10 || tim == TIM11) { + TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; + } + else { + TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 2 / ((uint32_t)mhz * 1000000)) - 1; + } #else TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; #endif @@ -197,17 +209,14 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui timerNVICConfigure(TIM1_UP_IRQn); break; #endif -#if defined (STM32F40_41xxx) +#if defined (STM32F40_41xxx) || defined(STM32F411xE) case TIM1_CC_IRQn: timerNVICConfigure(TIM1_UP_TIM10_IRQn); break; - case TIM8_CC_IRQn: - timerNVICConfigure(TIM8_UP_TIM13_IRQn); - break; #endif -#if defined (STM32F411xE) - case TIM1_CC_IRQn: - timerNVICConfigure(TIM1_UP_TIM10_IRQn); +#if defined (STM32F40_41xxx) + case TIM8_CC_IRQn: + timerNVICConfigure(TIM8_UP_TIM13_IRQn); break; #endif #ifdef STM32F303xC @@ -640,17 +649,10 @@ void timerInit(void) GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE); #endif -#ifdef TIMER_APB1_PERIPHERALS - RCC_APB1PeriphClockCmd(TIMER_APB1_PERIPHERALS, ENABLE); -#endif - -#ifdef TIMER_APB2_PERIPHERALS - RCC_APB2PeriphClockCmd(TIMER_APB2_PERIPHERALS, ENABLE); -#endif - -#ifdef TIMER_AHB_PERIPHERALS - RCC_AHBPeriphClockCmd(TIMER_AHB_PERIPHERALS, ENABLE); -#endif + /* enable the timer peripherals */ + for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + RCC_ClockCmd(timerRCC(timerHardware[i].tim), ENABLE); + } #if defined(STM32F3) || defined(STM32F4) for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 9ca4c7d957..642e184b59 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -18,6 +18,7 @@ #pragma once #include "io.h" +#include "rcc.h" #if !defined(USABLE_TIMER_CHANNEL_COUNT) #define USABLE_TIMER_CHANNEL_COUNT 14 @@ -64,6 +65,11 @@ typedef struct timerOvrHandlerRec_s { struct timerOvrHandlerRec_s* next; } timerOvrHandlerRec_t; +typedef struct timerDef_s { + TIM_TypeDef *TIMx; + rccPeriphTag_t rcc; +} timerDef_t; + typedef struct { TIM_TypeDef *tim; ioTag_t pin; @@ -77,7 +83,21 @@ typedef struct { uint8_t outputInverted; } timerHardware_t; +#ifdef STM32F1 +#if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) +#define HARDWARE_TIMER_DEFINITION_COUNT 14 +#elif defined(STM32F10X_HD) || defined(STM32F10X_CL) +#define HARDWARE_TIMER_DEFINITION_COUNT 7 +#else +#define HARDWARE_TIMER_DEFINITION_COUNT 4 +#endif +#elif defined(STM32F3) +#define HARDWARE_TIMER_DEFINITION_COUNT 10 +#elif defined(STM32F4) +#define HARDWARE_TIMER_DEFINITION_COUNT 14 +#endif extern const timerHardware_t timerHardware[]; +extern const timerDef_t timerDefinitions[]; typedef enum { TYPE_FREE, @@ -124,3 +144,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 diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index f8016950f4..27e9df381f 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -6,6 +6,29 @@ */ #include "stm32f10x.h" +#include "rcc.h" +#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) }, +#if defined(STM32F10X_HD) || defined(STM32F10X_CL) || defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5) }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, +#endif +#if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) + { .TIMx = TIM8, .rcc = RCC_APB1(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) }, +#endif +}; /** * @brief Selects the TIM Output Compare Mode. diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 458ef824f6..ca80287522 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -6,6 +6,22 @@ */ #include "stm32f30x.h" +#include "rcc.h" +#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) }, +}; + /** * @brief Selects the TIM Output Compare Mode. diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index beb5701760..8916a526c0 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -6,6 +6,8 @@ */ #include "stm32f4xx.h" +#include "timer.h" +#include "rcc.h" /** * @brief Selects the TIM Output Compare Mode. @@ -32,6 +34,23 @@ #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) }, +}; + void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode) { uint32_t tmp = 0; @@ -65,3 +84,4 @@ void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); } } + diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index a336abe638..e52c98d128 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -152,9 +152,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 746642c454..88ce6a5791 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -202,6 +202,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_TIM5 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8) +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) ) + diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index fbcd700808..ff191e8225 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -52,11 +52,11 @@ 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 + { 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 }; diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index da0b3921c4..e22152a86d 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -149,5 +149,3 @@ #define TARGET_IO_PORTD (BIT(2)) #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM5 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM9) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index dc39a80c52..965942e6e6 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -135,9 +135,6 @@ // IO - from schematics #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(14)) +#define TARGET_IO_PORTC ( BIT(14) ) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index b15505cab4..d9ee885574 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -133,7 +133,3 @@ #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)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB | RCC_AHBPeriph_GPIOC | RCC_AHBPeriph_GPIOD | RCC_AHBPeriph_GPIOF) - diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 3b79c9754f..d92a591ab8 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -68,8 +68,4 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB) - +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 26af72c17d..527ffbbfbf 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -180,7 +180,3 @@ #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOC) - diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 0fca6bf1e6..ecae485536 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -188,9 +188,5 @@ #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)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 83fe60e92d..2545ff49f6 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -125,8 +125,5 @@ #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index c68ce2c085..afba1e344e 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -202,9 +202,5 @@ #define TARGET_IO_PORTD 0xffff #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)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) +#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/FURYF4/target.h b/src/main/target/FURYF4/target.h index c68e9cd6ea..0c524c3e6e 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -163,6 +163,3 @@ #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM9) - diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 3a37a19077..963fc0e6bf 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -138,7 +138,3 @@ #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) - diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index c837d0b43c..211b0ea211 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -100,9 +100,4 @@ #define TARGET_IO_PORTD 0xffff #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)) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM17 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM8) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) - +#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.h b/src/main/target/LUX_RACE/target.h index 125a081f95..44ef0818c2 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -150,7 +150,3 @@ #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOC) - diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 09a94f4897..d4f03bfec8 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -186,8 +186,5 @@ // !!TODO - check the following line is correct #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(15) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM17) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index a20ecaedf2..e1d72f823a 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -194,11 +194,7 @@ // IO - assuming all IOs on 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_PORTC ( BIT(13) | BIT(14) | BIT(15) ) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index c50c1d51fc..8a0b35cf2c 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -110,8 +110,4 @@ #define TARGET_IO_PORTD (BIT(2)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB) - +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index b15f0202ec..8a25943114 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -167,7 +167,3 @@ #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(15) | TIM_N(17)) - -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM17) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 1cf06398cb..2e0292bc51 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -141,8 +141,6 @@ #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 3e53243b52..4c3972a69a 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -134,7 +134,3 @@ #define TARGET_IO_PORTD 0xffff #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM5 | RCC_APB1Periph_TIM12 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM9) - diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 8f27a42e26..3769f0f701 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -120,5 +120,3 @@ #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) ) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_TIM5 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1) \ No newline at end of file diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 24234ea27b..3bf23f6916 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -155,9 +155,4 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|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)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) - +#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/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 77320ee905..298d183b42 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -136,7 +136,3 @@ #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) - diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 32b783d64e..29b764d9f8 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -169,9 +169,6 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 52263deb8b..929a5f3fd3 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -163,9 +163,6 @@ #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)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 36852eb174..d71254270c 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -200,9 +200,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM15) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 6063abd97d..7c18fb2356 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -215,7 +215,3 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|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)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 1295b8edf4..570b319774 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -153,7 +153,3 @@ #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB | RCC_AHBPeriph_GPIOC | RCC_AHBPeriph_GPIOD) - From 6a64caa2c7ec451f5f0985373c9582caf4594a7e Mon Sep 17 00:00:00 2001 From: DTF UHF Date: Sat, 18 Jun 2016 12:05:14 -0400 Subject: [PATCH 36/38] Add servo_tilt and flash chip to doge target (dev branch commit) --- src/main/drivers/flash_m25p16.c | 6 ++++++ src/main/drivers/pwm_mapping.c | 6 ++++++ src/main/target/DOGE/target.h | 6 ++++++ src/main/target/DOGE/target.mk | 2 +- 4 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index f26734433b..7640ab768f 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -44,6 +44,7 @@ #define JEDEC_ID_MICRON_M25P16 0x202015 #define JEDEC_ID_MICRON_N25Q064 0x20BA17 #define JEDEC_ID_WINBOND_W25Q64 0xEF4017 +#define JEDEC_ID_MACRONIX_MX25L6406E 0xC22017 #define JEDEC_ID_MICRON_N25Q128 0x20ba18 #define JEDEC_ID_WINBOND_W25Q128 0xEF4018 @@ -163,6 +164,7 @@ static bool m25p16_readIdentification() break; case JEDEC_ID_MICRON_N25Q064: case JEDEC_ID_WINBOND_W25Q64: + case JEDEC_ID_MACRONIX_MX25L6406E: geometry.sectors = 128; geometry.pagesPerSector = 256; break; @@ -204,8 +206,12 @@ bool m25p16_init() IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI); IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG); + DISABLE_M25P16; + +#ifndef M25P16_SPI_SHARED //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz spiSetDivisor(M25P16_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); +#endif return m25p16_readIdentification(); } diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index df6bd2019c..7109011170 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -213,6 +213,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) type = MAP_TO_SERVO_OUTPUT; #endif +#if defined(DOGE) + // remap outputs 1+2 (PWM2+3) as servos + if ((timerIndex == PWM2 || timerIndex == PWM3) && timerHardwarePtr->tim == TIM4) + type = MAP_TO_SERVO_OUTPUT; +#endif + #if defined(COLIBRI_RACE) || defined(LUX_RACE) // remap PWM1+2 as servos if ((timerIndex == PWM6 || timerIndex == PWM7 || timerIndex == PWM8 || timerIndex == PWM9) && timerHardwarePtr->tim == TIM2) diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 0fca6bf1e6..45e397eba1 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -73,6 +73,12 @@ #define SPI2_MOSI_PIN PB15 #define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_SHARED +#define M25P16_CS_PIN PC15 +#define M25P16_SPI_INSTANCE SPI2 + // timer definitions in drivers/timer.c // channel mapping in drivers/pwm_mapping.c // only 6 outputs available on hardware diff --git a/src/main/target/DOGE/target.mk b/src/main/target/DOGE/target.mk index 529ed9f58b..f44e26c5bf 100644 --- a/src/main/target/DOGE/target.mk +++ b/src/main/target/DOGE/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP +FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_mpu.c \ From 34c0b309450320497a502fd00a664160b1ddd42a Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 18 Jun 2016 09:12:29 -0700 Subject: [PATCH 37/38] max_osd is not a build feature anymore, just include the source --- src/main/target/OMNIBUS/target.mk | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 8259bfa925..1cb198e263 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD MAX_OSD +FEATURES = VCP SDCARD TARGET_SRC = \ drivers/accgyro_mpu.c \ @@ -12,5 +12,6 @@ TARGET_SRC = \ drivers/light_ws2811strip_stm32f30x.c \ drivers/serial_softserial.c \ drivers/serial_usb_vcp.c \ - drivers/sonar_hcsr04.c - + drivers/sonar_hcsr04.c \ + drivers/max7456.c \ + io/osd.c From 692713926110085f6cf6a93a58ec524e64127e3a Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sat, 18 Jun 2016 21:22:07 +0200 Subject: [PATCH 38/38] STM32F3DISCOVERY bulld failed. Added a few missing modules in TARGET_SRC. --- src/main/target/STM32F3DISCOVERY/target.mk | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index e90db6d09f..9f3645c97c 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -12,8 +12,11 @@ TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_l3g4200d.c \ drivers/barometer_ms5611.c \ drivers/barometer_bmp280.c \ - drivers/compass_ak8975.c - + drivers/compass_ak8975.c \ + drivers/max7456.c \ + io/osd.c