diff --git a/.gitignore b/.gitignore index d5ba321dd..63a87cbe4 100644 --- a/.gitignore +++ b/.gitignore @@ -20,9 +20,9 @@ docs/Manual.pdf README.pdf # artefacts of top-level Makefile -/downloads -/tools -/build +/downloads/ +/tools/ +/build/ # local changes only make/local.mk @@ -32,8 +32,8 @@ mcu.mak.old stm32.mak # artefacts for Visual Studio Code -/.vscode +/.vscode/ # artefacts for CLion -/cmake-build-debug +/cmake-build-debug/ /CMakeLists.txt diff --git a/Makefile b/Makefile index dc544a85a..f415d80fe 100644 --- a/Makefile +++ b/Makefile @@ -842,23 +842,14 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ common/typeconversion.c \ drivers/adc.c \ drivers/buf_writer.c \ - drivers/bus_i2c_soft.c \ drivers/bus_spi.c \ - drivers/bus_spi_soft.c \ drivers/exti.c \ drivers/gyro_sync.c \ drivers/io.c \ - drivers/light_led.c \ - drivers/resource.c \ - drivers/rx_nrf24l01.c \ - drivers/rx_spi.c \ - drivers/rx_xn297.c \ drivers/pwm_output.c \ drivers/rcc.c \ - drivers/rx_pwm.c \ drivers/serial.c \ drivers/serial_uart.c \ - drivers/sound_beeper.c \ drivers/system.c \ drivers/timer.c \ fc/fc_core.c \ @@ -869,16 +860,9 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ - flight/servos.c \ io/serial.c \ rx/ibus.c \ rx/jetiexbus.c \ - rx/nrf24_cx10.c \ - rx/nrf24_inav.c \ - rx/nrf24_h8_3d.c \ - rx/nrf24_syma.c \ - rx/nrf24_v202.c \ - rx/pwm.c \ rx/rx.c \ rx/rx_spi.c \ rx/crsf.c \ @@ -894,9 +878,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ sensors/gyroanalyse.c \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) \ - drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ - drivers/serial_softserial.c \ io/displayport_max7456.c \ io/osd.c \ io/osd_slave.c @@ -1145,6 +1127,7 @@ endif CROSS_CC := $(CCACHE) $(ARM_SDK_PREFIX)gcc CROSS_CXX := $(CCACHE) $(ARM_SDK_PREFIX)g++ OBJCOPY := $(ARM_SDK_PREFIX)objcopy +OBJDUMP := $(ARM_SDK_PREFIX)objdump SIZE := $(ARM_SDK_PREFIX)size # @@ -1269,6 +1252,7 @@ CPPCHECK = cppcheck $(CSOURCES) --enable=all --platform=unix64 \ TARGET_BIN = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET).bin TARGET_HEX = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET).hex TARGET_ELF = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf +TARGET_LST = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).lst TARGET_OBJS = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $(SRC)))) TARGET_DEPS = $(addsuffix .d,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $(SRC)))) TARGET_MAP = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map @@ -1277,6 +1261,7 @@ TARGET_MAP = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map CLEAN_ARTIFACTS := $(TARGET_BIN) CLEAN_ARTIFACTS += $(TARGET_HEX) CLEAN_ARTIFACTS += $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) +CLEAN_ARTIFACTS += $(TARGET_LST) # Make sure build date and revision is updated on every incremental build $(OBJECT_DIR)/$(TARGET)/build/version.o : $(SRC) @@ -1284,6 +1269,9 @@ $(OBJECT_DIR)/$(TARGET)/build/version.o : $(SRC) # List of buildable ELF files and their object dependencies. # It would be nice to compute these lists, but that seems to be just beyond make. +$(TARGET_LST): $(TARGET_ELF) + $(V0) $(OBJDUMP) -S --disassemble $< > $@ + $(TARGET_HEX): $(TARGET_ELF) $(V0) $(OBJCOPY) -O ihex --set-start 0x8000000 $< $@ diff --git a/Vagrantfile b/Vagrantfile index 95106df24..ad8698227 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -11,8 +11,12 @@ Vagrant.configure(2) do |config| # https://docs.vagrantup.com. # Every Vagrant development environment requires a box. You can search for - # boxes at https://atlas.hashicorp.com/search. - config.vm.box = "ubuntu/trusty64" + # boxes at https://app.vagrantup.com/boxes/search + config.vm.box = "ubuntu/xenial64" + + config.vm.provider "virtualbox" do |v| + v.memory = 4096 + end # Enable provisioning with a shell script. Additional provisioners such as # Puppet, Chef, Ansible, Salt, and Docker are also available. Please see the @@ -21,7 +25,8 @@ Vagrant.configure(2) do |config| apt-get remove -y binutils-arm-none-eabi gcc-arm-none-eabi add-apt-repository ppa:team-gcc-arm-embedded/ppa apt-get update - apt-get install -y git ccache gcc-arm-embedded=6-2017q1-1~trusty3 + apt-get install -y git gcc-arm-embedded=6-2017q2-1~xenial1 + apt-get install -y make python gcc clang SHELL end diff --git a/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_spi.h b/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_spi.h index efe31c678..4cbc10ec7 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_spi.h +++ b/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_spi.h @@ -622,9 +622,9 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef *hspi); * @{ */ /* I/O operation functions ***************************************************/ -HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, const uint8_t *pData, uint16_t Size, uint32_t Timeout); HAL_StatusTypeDef HAL_SPI_Receive(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout); -HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, +HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, const uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, uint32_t Timeout); HAL_StatusTypeDef HAL_SPI_Transmit_IT(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size); HAL_StatusTypeDef HAL_SPI_Receive_IT(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size); diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_spi.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_spi.c index 18a6ce9d7..e86849003 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_spi.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_spi.c @@ -493,7 +493,7 @@ __weak void HAL_SPI_MspDeInit(SPI_HandleTypeDef *hspi) * @param Timeout: Timeout duration * @retval HAL status */ -HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout) +HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, const uint8_t *pData, uint16_t Size, uint32_t Timeout) { uint32_t tickstart = 0U; HAL_StatusTypeDef errorcode = HAL_OK; @@ -882,7 +882,7 @@ error : * @param Timeout: Timeout duration * @retval HAL status */ -HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, +HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, const uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, uint32_t Timeout) { uint32_t tmp = 0U, tmp1 = 0U; diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c42eca5a8..05a8e18f2 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -906,7 +906,7 @@ bool startedLoggingInTestMode = false; void startInTestMode(void) { - if(!startedLoggingInTestMode) { + if (!startedLoggingInTestMode) { if (blackboxConfig()->device == BLACKBOX_DEVICE_SERIAL) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); if (sharedBlackboxAndMspPort) { @@ -919,7 +919,7 @@ void startInTestMode(void) } void stopInTestMode(void) { - if(startedLoggingInTestMode) { + if (startedLoggingInTestMode) { blackboxFinish(); startedLoggingInTestMode = false; } @@ -954,7 +954,7 @@ bool inMotorTestMode(void) { for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) atLeastOneMotorActivated |= (motor_disarmed[i] != inactiveMotorCommand); - if(atLeastOneMotorActivated) { + if (atLeastOneMotorActivated) { resetTime = millis() + 5000; // add 5 seconds return true; } else { @@ -1638,7 +1638,7 @@ void blackboxUpdate(timeUs_t currentTimeUs) blackboxSetState(BLACKBOX_STATE_ERASED); beeper(BEEPER_BLACKBOX_ERASE); } - break; + break; case BLACKBOX_STATE_ERASED: if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) { blackboxSetState(BLACKBOX_STATE_STOPPED); @@ -1663,13 +1663,13 @@ void blackboxUpdate(timeUs_t currentTimeUs) } #endif } else { // Only log in test mode if there is room! - if(blackboxConfig()->on_motor_test) { + if (blackboxConfig()->on_motor_test) { // Handle Motor Test Mode - if(inMotorTestMode()) { - if(blackboxState==BLACKBOX_STATE_STOPPED) + if (inMotorTestMode()) { + if (blackboxState==BLACKBOX_STATE_STOPPED) startInTestMode(); } else { - if(blackboxState!=BLACKBOX_STATE_STOPPED) + if (blackboxState!=BLACKBOX_STATE_STOPPED) stopInTestMode(); } } diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 8e86e962e..d74f243a2 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -221,7 +221,7 @@ bool blackboxDeviceOpen(void) */ - switch(baudRateIndex) { + switch (baudRateIndex) { case BAUD_1000000: case BAUD_1500000: case BAUD_2000000: diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 6df56ffef..bd6039eb3 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -606,7 +606,7 @@ static void cmsTraverseGlobalExit(const CMS_Menu *pMenu) } long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) -{ +{ int exitType = (int)ptr; switch (exitType) { case CMS_EXIT_SAVE: diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 43e8e5f6f..2020db105 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -134,7 +134,6 @@ static OSD_Entry menuMainEntries[] = {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, #ifdef OSD {"OSD", OME_Submenu, cmsMenuChange, &cmsx_menuOsd, 0}, - {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, #endif {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, {"MISC", OME_Submenu, cmsMenuChange, &cmsx_menuMisc, 0}, diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 2a811bc7b..4219e6367 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -39,51 +39,6 @@ #include "io/displayport_max7456.h" #include "io/osd.h" -static uint8_t osdConfig_rssi_alarm; -static uint16_t osdConfig_cap_alarm; -static uint16_t osdConfig_time_alarm; -static uint16_t osdConfig_alt_alarm; - -static long cmsx_menuAlarmsOnEnter(void) -{ - osdConfig_rssi_alarm = osdConfig()->rssi_alarm; - osdConfig_cap_alarm = osdConfig()->cap_alarm; - osdConfig_time_alarm = osdConfig()->time_alarm; - osdConfig_alt_alarm = osdConfig()->alt_alarm; - return 0; -} - -static long cmsx_menuAlarmsOnExit(const OSD_Entry *self) -{ - UNUSED(self); - - osdConfigMutable()->rssi_alarm = osdConfig_rssi_alarm; - osdConfigMutable()->cap_alarm = osdConfig_cap_alarm; - osdConfigMutable()->time_alarm = osdConfig_time_alarm; - osdConfigMutable()->alt_alarm = osdConfig_alt_alarm; - return 0; -} - -OSD_Entry cmsx_menuAlarmsEntries[] = -{ - {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, - {"RSSI", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_alarm, 5, 90, 5}, 0}, - {"MAIN BAT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_cap_alarm, 50, 30000, 50}, 0}, - {"FLY TIME", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_time_alarm, 1, 200, 1}, 0}, - {"MAX ALT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_alt_alarm, 1, 200, 1}, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -CMS_Menu cmsx_menuAlarms = { - .GUARD_text = "MENUALARMS", - .GUARD_type = OME_MENU, - .onEnter = cmsx_menuAlarmsOnEnter, - .onExit = cmsx_menuAlarmsOnExit, - .onGlobalExit = NULL, - .entries = cmsx_menuAlarmsEntries, -}; - #ifndef DISABLE_EXTENDED_CMS_OSD_MENU static uint16_t osdConfig_item_pos[OSD_ITEM_COUNT]; @@ -111,8 +66,8 @@ OSD_Entry menuOsdActiveElemsEntries[] = {"CROSSHAIRS", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_CROSSHAIRS], 0}, {"HORIZON", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ARTIFICIAL_HORIZON], 0}, {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_HORIZON_SIDEBARS], 0}, - {"UPTIME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ONTIME], 0}, - {"FLY TIME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_FLYTIME], 0}, + {"TIMER 1", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_TIMER_1], 0}, + {"TIMER 2", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_TIMER_2], 0}, {"FLY MODE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_FLYMODE], 0}, {"NAME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_CRAFT_NAME], 0}, {"THROTTLE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_THROTTLE_POS], 0}, @@ -141,7 +96,6 @@ OSD_Entry menuOsdActiveElemsEntries[] = {"DISARMED", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_DISARMED], 0}, {"PIT ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PITCH_ANGLE], 0}, {"ROL ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_ANGLE], 0}, - {"ARMED TIME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ARMED_TIME], 0}, {"HEADING", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_NUMERICAL_HEADING], 0}, {"VARIO", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_NUMERICAL_VARIO], 0}, {"BACK", OME_Back, NULL, NULL, 0}, @@ -156,6 +110,100 @@ CMS_Menu menuOsdActiveElems = { .onGlobalExit = NULL, .entries = menuOsdActiveElemsEntries }; + +static uint8_t osdConfig_rssi_alarm; +static uint16_t osdConfig_cap_alarm; +static uint16_t osdConfig_alt_alarm; + +static long menuAlarmsOnEnter(void) +{ + osdConfig_rssi_alarm = osdConfig()->rssi_alarm; + osdConfig_cap_alarm = osdConfig()->cap_alarm; + osdConfig_alt_alarm = osdConfig()->alt_alarm; + + return 0; +} + +static long menuAlarmsOnExit(const OSD_Entry *self) +{ + UNUSED(self); + + osdConfigMutable()->rssi_alarm = osdConfig_rssi_alarm; + osdConfigMutable()->cap_alarm = osdConfig_cap_alarm; + osdConfigMutable()->alt_alarm = osdConfig_alt_alarm; + + return 0; +} + +OSD_Entry menuAlarmsEntries[] = +{ + {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_alarm, 5, 90, 5}, 0}, + {"MAIN BAT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_cap_alarm, 50, 30000, 50}, 0}, + {"MAX ALT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_alt_alarm, 1, 200, 1}, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu menuAlarms = { + .GUARD_text = "MENUALARMS", + .GUARD_type = OME_MENU, + .onEnter = menuAlarmsOnEnter, + .onExit = menuAlarmsOnExit, + .onGlobalExit = NULL, + .entries = menuAlarmsEntries, +}; + +osd_timer_source_e timerSource[OSD_TIMER_COUNT]; +osd_timer_precision_e timerPrecision[OSD_TIMER_COUNT]; +uint8_t timerAlarm[OSD_TIMER_COUNT]; + +static long menuTimersOnEnter(void) +{ + for (int i = 0; i < OSD_TIMER_COUNT; i++) { + const uint16_t timer = osdConfig()->timers[i]; + timerSource[i] = OSD_TIMER_SRC(timer); + timerPrecision[i] = OSD_TIMER_PRECISION(timer); + timerAlarm[i] = OSD_TIMER_ALARM(timer); + } + + return 0; +} + +static long menuTimersOnExit(const OSD_Entry *self) +{ + UNUSED(self); + + for (int i = 0; i < OSD_TIMER_COUNT; i++) { + osdConfigMutable()->timers[i] = OSD_TIMER(timerSource[i], timerPrecision[i], timerAlarm[i]); + } + + return 0; +} + +static const char * osdTimerPrecisionNames[] = {"SCND", "HDTH"}; + +OSD_Entry menuTimersEntries[] = +{ + {"--- TIMERS ---", OME_Label, NULL, NULL, 0}, + {"1 SRC", OME_TAB, NULL, &(OSD_TAB_t){&timerSource[OSD_TIMER_1], OSD_TIMER_SRC_COUNT - 1, osdTimerSourceNames}, 0 }, + {"1 PREC", OME_TAB, NULL, &(OSD_TAB_t){&timerPrecision[OSD_TIMER_1], OSD_TIMER_PREC_COUNT - 1, osdTimerPrecisionNames}, 0}, + {"1 ALARM", OME_UINT8, NULL, &(OSD_UINT8_t){&timerAlarm[OSD_TIMER_1], 0, 0xFF, 1}, 0}, + {"2 SRC", OME_TAB, NULL, &(OSD_TAB_t){&timerSource[OSD_TIMER_2], OSD_TIMER_SRC_COUNT - 1, osdTimerSourceNames}, 0 }, + {"2 PREC", OME_TAB, NULL, &(OSD_TAB_t){&timerPrecision[OSD_TIMER_2], OSD_TIMER_PREC_COUNT - 1, osdTimerPrecisionNames}, 0}, + {"2 ALARM", OME_UINT8, NULL, &(OSD_UINT8_t){&timerAlarm[OSD_TIMER_2], 0, 0xFF, 1}, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu menuTimers = { + .GUARD_text = "MENUTIMERS", + .GUARD_type = OME_MENU, + .onEnter = menuTimersOnEnter, + .onExit = menuTimersOnExit, + .onGlobalExit = NULL, + .entries = menuTimersEntries, +}; #endif /* DISABLE_EXTENDED_CMS_OSD_MENU */ #ifdef USE_MAX7456 @@ -193,6 +241,8 @@ OSD_Entry cmsx_menuOsdEntries[] = {"---OSD---", OME_Label, NULL, NULL, 0}, #ifndef DISABLE_EXTENDED_CMS_OSD_MENU {"ACTIVE ELEM", OME_Submenu, cmsMenuChange, &menuOsdActiveElems, 0}, + {"TIMERS", OME_Submenu, cmsMenuChange, &menuTimers, 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, &menuAlarms, 0}, #endif #ifdef USE_MAX7456 {"INVERT", OME_Bool, NULL, &displayPortProfileMax7456_invert, 0}, diff --git a/src/main/cms/cms_menu_osd.h b/src/main/cms/cms_menu_osd.h index c4e306a9e..033d28c37 100644 --- a/src/main/cms/cms_menu_osd.h +++ b/src/main/cms/cms_menu_osd.h @@ -17,5 +17,4 @@ #pragma once -extern CMS_Menu cmsx_menuAlarms; extern CMS_Menu cmsx_menuOsd; diff --git a/src/main/common/filter.c b/src/main/common/filter.c index b070586fe..8732c7ebc 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -154,11 +154,11 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input) { /* compute result */ const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2; - + /* shift x1 to x2, input to x1 */ filter->x2 = filter->x1; filter->x1 = input; - + /* shift y1 to y2, result to y1 */ filter->y2 = filter->y1; filter->y1 = result; diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index b9d34aa94..14c800d90 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -235,7 +235,7 @@ static int write_word(config_streamer_t *c, uint32_t value) EraseInitStruct.Sector = getFLASHSectorForEEPROM(); uint32_t SECTORError; const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); - if (status != HAL_OK){ + if (status != HAL_OK) { return -1; } } diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 4890a95f1..16ab6c52f 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -87,7 +87,7 @@ extern const uint8_t __pg_resetdata_end[]; do { \ extern const pgRegistry_t _name ##_Registry; \ pgReset(&_name ## _Registry); \ - } while(0) \ + } while (0) \ /**/ // Declare system config diff --git a/src/main/drivers/accgyro/accgyro_mma845x.c b/src/main/drivers/accgyro/accgyro_mma845x.c index 9cf7ec6ec..fd6e9decc 100644 --- a/src/main/drivers/accgyro/accgyro_mma845x.c +++ b/src/main/drivers/accgyro/accgyro_mma845x.c @@ -85,7 +85,7 @@ static inline void mma8451ConfigureInterrupt(void) #ifdef MMA8451_INT_PIN IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0); // TODO - maybe pullup / pulldown ? - IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING); + IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING); #endif i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 971deffc0..1ca97c798 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -29,6 +29,7 @@ #include "common/maths.h" #include "common/utils.h" +#include "drivers/bus_spi.h" #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "drivers/exti.h" @@ -217,6 +218,23 @@ bool mpuGyroRead(gyroDev_t *gyro) return true; } +bool mpuGyroReadSPI(gyroDev_t *gyro) +{ + static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + uint8_t data[7]; + + const bool ack = spiBusTransfer(&gyro->bus, data, dataToSend, 7); + if (!ack) { + return false; + } + + gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]); + gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]); + gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]); + + return true; +} + bool mpuCheckDataReady(gyroDev_t* gyro) { bool ret; diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 2da4ce109..445af4dfe 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -199,6 +199,7 @@ void mpuGyroInit(struct gyroDev_s *gyro); struct accDev_s; bool mpuAccRead(struct accDev_s *acc); bool mpuGyroRead(struct gyroDev_s *gyro); +bool mpuGyroReadSPI(struct gyroDev_s *gyro); void mpuDetect(struct gyroDev_s *gyro); bool mpuCheckDataReady(struct gyroDev_s *gyro); void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn); diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index ee3077421..f9ad9aa3c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -110,7 +110,7 @@ bool bmi160Detect(const busDevice_t *bus) delay(10); // Give SPI some time to start up /* Check the chip ID */ - if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1){ + if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) { return false; } @@ -130,7 +130,7 @@ static void BMI160_Init(const busDevice_t *bus) } /* Configure the BMI160 Sensor */ - if (BMI160_Config(bus) != 0){ + if (BMI160_Config(bus) != 0) { return; } @@ -152,12 +152,12 @@ static int32_t BMI160_Config(const busDevice_t *bus) { // Set normal power mode for gyro and accelerometer - if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0) { return -1; } delay(100); // can take up to 80ms - if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0) { return -2; } delay(5); // can take up to 3.8ms @@ -170,47 +170,47 @@ static int32_t BMI160_Config(const busDevice_t *bus) // Set odr and ranges // Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used - if (BMI160_WriteReg(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0) { return -3; } delay(1); // Set gyr_bwp = 0b010 so only the first filter stage is used - if (BMI160_WriteReg(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0) { return -4; } delay(1); - if (BMI160_WriteReg(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0) { return -5; } delay(1); - if (BMI160_WriteReg(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0) { return -6; } delay(1); // Enable offset compensation uint8_t val = spiReadRegister(bus, BMI160_REG_OFFSET_0); - if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0) { return -7; } // Enable data ready interrupt - if (BMI160_WriteReg(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0) { return -8; } delay(1); // Enable INT1 pin - if (BMI160_WriteReg(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0) { return -9; } delay(1); // Map data ready interrupt to INT1 pin - if (BMI160_WriteReg(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0) { return -10; } delay(1); @@ -351,7 +351,7 @@ bool bmi160GyroRead(gyroDev_t *gyro) }; uint8_t bmi160_rec_buf[BUFFER_SIZE]; - uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; + static const uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; IOLo(gyro->bus.spi.csnPin); spiTransfer(gyro->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 762ac1a61..438b67b05 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -140,7 +140,7 @@ bool icm20689SpiGyroDetect(gyroDev_t *gyro) } gyro->initFn = icm20689GyroInit; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpuGyroReadSPI; gyro->intStatusFn = mpuCheckDataReady; // 16.4 dps/lsb scalefactor diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 94a9e2e56..2413174b7 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -244,7 +244,7 @@ bool mpu6000SpiGyroDetect(gyroDev_t *gyro) } gyro->initFn = mpu6000SpiGyroInit; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpuGyroReadSPI; gyro->intStatusFn = mpuCheckDataReady; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 8f2f4db21..eae203e65 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -136,7 +136,7 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro) } gyro->initFn = mpu6500SpiGyroInit; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpuGyroReadSPI; gyro->intStatusFn = mpuCheckDataReady; // 16.4 dps/lsb scalefactor diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 1df220046..000d18a6f 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -210,7 +210,7 @@ bool mpu9250SpiGyroDetect(gyroDev_t *gyro) } gyro->initFn = mpu9250SpiGyroInit; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpuGyroReadSPI; gyro->intStatusFn = mpuCheckDataReady; // 16.4 dps/lsb scalefactor diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index 316ea4ab9..6c2d339a4 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -200,7 +200,7 @@ void adcInit(const adcConfig_t *config) //HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues, configuredAdcChannels); /*##-4- Start the conversion process #######################################*/ - if(HAL_ADC_Start_DMA(&adc.ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK) + if (HAL_ADC_Start_DMA(&adc.ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK) { /* Start Conversation Error */ } diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index ac8508447..ffa0dfec4 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -65,7 +65,7 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = { }, #endif #ifdef USE_I2C_DEVICE_3 - { + { .device = I2CDEV_3, .reg = I2C3, .sclPins = { DEFIO_TAG_E(PA8) }, @@ -163,12 +163,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, HAL_StatusTypeDef status; - if(reg_ == 0xFF) + if (reg_ == 0xFF) status = HAL_I2C_Master_Transmit(pHandle ,addr_ << 1, data, len_, I2C_DEFAULT_TIMEOUT); else status = HAL_I2C_Mem_Write(pHandle ,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT); - if(status != HAL_OK) + if (status != HAL_OK) return i2cHandleHardwareFailure(device); return true; @@ -193,12 +193,12 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t HAL_StatusTypeDef status; - if(reg_ == 0xFF) + if (reg_ == 0xFF) status = HAL_I2C_Master_Receive(pHandle ,addr_ << 1, buf, len, I2C_DEFAULT_TIMEOUT); else status = HAL_I2C_Mem_Read(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT); - if(status != HAL_OK) + if (status != HAL_OK) return i2cHandleHardwareFailure(device); return true; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 009a424bc..41ad4a4fb 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -51,7 +51,7 @@ SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) #ifdef USE_SPI_DEVICE_4 if (instance == SPI4) - return SPIDEV_3; + return SPIDEV_4; #endif return SPIINVALID; @@ -240,6 +240,14 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len return true; } +bool spiBusTransfer(const busDevice_t *bus, uint8_t *rxData, const uint8_t *txData, int length) +{ + IOLo(bus->spi.csnPin); + spiTransfer(bus->spi.instance, rxData, txData, length); + IOHi(bus->spi.csnPin); + return true; +} + void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) { #define BR_CLEAR_MASK 0xFFC7 diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 3f3a50704..799ffe745 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,6 +17,7 @@ #pragma once +#include "drivers/bus.h" #include "drivers/io_types.h" #include "drivers/bus.h" #include "drivers/rcc_types.h" @@ -93,6 +94,8 @@ uint16_t spiGetErrorCounter(SPI_TypeDef *instance); void spiResetErrorCounter(SPI_TypeDef *instance); SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); +bool spiBusTransfer(const busDevice_t *bus, uint8_t *rxData, const uint8_t *txData, int length); + #if defined(USE_HAL_DRIVER) SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance); DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 356fc5709..e970f3cb4 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -23,6 +23,7 @@ #ifdef USE_SPI +#include "drivers/bus.h" #include "drivers/bus_spi.h" #include "drivers/bus_spi_impl.h" #include "drivers/dma.h" @@ -143,7 +144,7 @@ void spiInitDevice(SPIDevice device) spi->leadingEdge = true; } #endif - + // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); RCC_ResetCmd(spi->rcc, ENABLE); @@ -153,7 +154,7 @@ void spiInitDevice(SPIDevice device) IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device)); #if defined(STM32F7) - if(spi->leadingEdge == true) + if (spi->leadingEdge == true) IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF); else IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); @@ -161,7 +162,7 @@ void spiInitDevice(SPIDevice device) IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF); #endif #if defined(STM32F3) || defined(STM32F4) - if(spi->leadingEdge == true) + if (spi->leadingEdge == true) IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->af); else IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af); @@ -254,7 +255,7 @@ uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance) bool spiIsBusBusy(SPI_TypeDef *instance) { SPIDevice device = spiDeviceByInstance(instance); - if(spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY) + if (spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY) return true; else return false; @@ -265,20 +266,20 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len SPIDevice device = spiDeviceByInstance(instance); HAL_StatusTypeDef status; - if(!out) // Tx only + if (!out) // Tx only { status = HAL_SPI_Transmit(&spiDevice[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT); } - else if(!in) // Rx only + else if (!in) // Rx only { status = HAL_SPI_Receive(&spiDevice[device].hspi, out, len, SPI_DEFAULT_TIMEOUT); } else // Tx and Rx { - status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, (uint8_t *)in, out, len, SPI_DEFAULT_TIMEOUT); + status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, in, out, len, SPI_DEFAULT_TIMEOUT); } - if( status != HAL_OK) + if ( status != HAL_OK) spiTimeoutUserCallback(instance); return true; @@ -310,6 +311,17 @@ static uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t in) return in; } +bool spiBusTransfer(const busDevice_t *bus, uint8_t *rxData, const uint8_t *txData, int len) +{ + IOLo(bus->spi.csnPin); + const HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(bus->spi.handle, txData, rxData, len, SPI_DEFAULT_TIMEOUT); + IOHi(bus->spi.csnPin); + if (status != HAL_OK) { + spiTimeoutUserCallback(bus->spi.instance); + } + return true; +} + void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) { SPIDevice device = spiDeviceByInstance(instance); diff --git a/src/main/drivers/bus_spi_soft.c b/src/main/drivers/bus_spi_soft.c index 2400124dc..9dfdfe160 100644 --- a/src/main/drivers/bus_spi_soft.c +++ b/src/main/drivers/bus_spi_soft.c @@ -70,7 +70,7 @@ void softSpiInit(const softSPIDevice_t *dev) uint8_t softSpiTransferByte(const softSPIDevice_t *dev, uint8_t byte) { - for(int ii = 0; ii < 8; ++ii) { + for (int ii = 0; ii < 8; ++ii) { if (byte & 0x80) { IOHi(IOGetByTag(dev->mosiTag)); } else { diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 981d8e392..924c3f582 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -61,7 +61,8 @@ #define AK8963_MAG_REG_HZL 0x07 #define AK8963_MAG_REG_HZH 0x08 #define AK8963_MAG_REG_STATUS2 0x09 -#define AK8963_MAG_REG_CNTL 0x0a +#define AK8963_MAG_REG_CNTL1 0x0a +#define AK8963_MAG_REG_CNTL2 0x0b #define AK8963_MAG_REG_ASCT 0x0c // self test #define AK8963_MAG_REG_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value #define AK8963_MAG_REG_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value @@ -75,37 +76,26 @@ #define STATUS2_DATA_ERROR 0x02 #define STATUS2_MAG_SENSOR_OVERFLOW 0x03 -#define CNTL_MODE_POWER_DOWN 0x00 -#define CNTL_MODE_ONCE 0x01 -#define CNTL_MODE_CONT1 0x02 -#define CNTL_MODE_CONT2 0x06 -#define CNTL_MODE_SELF_TEST 0x08 -#define CNTL_MODE_FUSE_ROM 0x0F +#define CNTL1_MODE_POWER_DOWN 0x00 +#define CNTL1_MODE_ONCE 0x01 +#define CNTL1_MODE_CONT1 0x02 +#define CNTL1_MODE_CONT2 0x06 +#define CNTL1_MODE_SELF_TEST 0x08 +#define CNTL1_MODE_FUSE_ROM 0x0F + +#define CNTL2_SOFT_RESET 0x01 static float magGain[3] = { 1.0f, 1.0f, 1.0f }; -#if defined(USE_SPI) -static busDevice_t *bus = NULL; // HACK -#endif +#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) +static busDevice_t *bus = NULL; -// FIXME pretend we have real MPU9250 support -// Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same. -#if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE) -#define MPU9250_SPI_INSTANCE -#define mpu9250SpiWriteRegisterVerify mpu6500SpiWriteRegDelayed -static bool mpu6500SpiWriteRegDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data) +static bool spiWriteRegisterDelay(const busDevice_t *bus, uint8_t reg, uint8_t data) { - IOLo(bus->spi.csnPin); - spiTransferByte(bus->spi.instance, reg); - spiTransferByte(bus->spi.instance, data); - IOHi(bus->spi.csnPin); + spiWriteRegister(bus, reg, data); delayMicroseconds(10); - return true; } -#endif - -#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) typedef struct queuedReadState_s { bool waiting; @@ -123,22 +113,22 @@ static queuedReadState_t queuedRead = { false, 0, 0}; static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes - delay(10); + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + delay(4); __disable_irq(); - bool ack = spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C + bool ack = spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C __enable_irq(); return ack; } static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) { - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte return true; } @@ -150,9 +140,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) queuedRead.len = len_; - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes queuedRead.readStartedAt = micros(); queuedRead.waiting = true; @@ -187,7 +177,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf) queuedRead.waiting = false; - spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer + spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer return true; } #else @@ -207,32 +197,22 @@ static bool ak8963Init() uint8_t calibration[3]; uint8_t status; - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode - delay(20); - - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode - delay(10); - + ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode + ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, sizeof(calibration), calibration); // Read the x-, y-, and z-axis calibration values - delay(10); magGain[X] = (((((float)(int8_t)calibration[X] - 128) / 256) + 1) * 30); magGain[Y] = (((((float)(int8_t)calibration[Y] - 128) / 256) + 1) * 30); magGain[Z] = (((((float)(int8_t)calibration[Z] - 128) / 256) + 1) * 30); - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading. - delay(10); + ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading. // Clear status registers ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &status); ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status); // Trigger first measurement -#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1); -#else - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); -#endif + ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_ONCE); return true; } @@ -241,7 +221,7 @@ static bool ak8963Read(int16_t *magData) bool ack = false; uint8_t buf[7]; -#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) +#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) // we currently need a different approach for the MPU9250 connected via SPI. // we cannot use the ak8963SensorRead() method for SPI, it is to slow and blocks for far too long. @@ -267,7 +247,7 @@ restart: uint8_t status = buf[0]; - if (!ack || ((status & STATUS1_DATA_READY) == 0 && (status & STATUS1_DATA_OVERRUN) == 0)) { + if (!ack || (status & STATUS1_DATA_READY) == 0) { // too early. queue the status read again state = CHECK_STATUS; if (retry) { @@ -315,37 +295,34 @@ restart: magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y]; magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z]; -#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) +#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) state = CHECK_STATUS; - return true; -#else - return ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again #endif + return ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_ONCE); // start reading again } bool ak8963Detect(magDev_t *mag) { uint8_t sig = 0; -#if defined(USE_SPI) +#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) bus = &mag->bus; #if defined(MPU6500_SPI_INSTANCE) spiBusSetInstance(&mag->bus, MPU6500_SPI_INSTANCE); #elif defined(MPU9250_SPI_INSTANCE) spiBusSetInstance(&mag->bus, MPU9250_SPI_INSTANCE); +#endif // initialze I2C master via SPI bus (MPU9250) - mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); - delay(10); - mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz - delay(10); - mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only - delay(10); -#endif + spiWriteRegisterDelay(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); + spiWriteRegisterDelay(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + spiWriteRegisterDelay(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only #endif - // check for AK8963 - bool ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); + ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG + delay(4); + + bool ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); // check for AK8963 if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H' { mag->init = ak8963Init; diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index a0366ee19..4c6dc3ad2 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -169,7 +169,7 @@ static bool hmc5883lRead(int16_t *magData) { uint8_t buf[6]; #ifdef USE_MAG_SPI_HMC5883 - bool ack = hmc5883SpiReadCommand(MAG_DATA_REGISTER, 6, buf); + bool ack = hmc5883SpiReadCommand(MAG_DATA_REGISTER, 6, buf); #else bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); #endif @@ -202,7 +202,7 @@ static bool hmc5883lInit(void) // Note that the very first measurement after a gain change maintains the same gain as the previous setting. // The new gain setting is effective from the second measurement and on. #ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) + hmc5883SpiWriteCommand(HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) #else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) #endif @@ -211,7 +211,7 @@ static bool hmc5883lInit(void) for (i = 0; i < 10; i++) { // Collect 10 samples #ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_MODE, 1); + hmc5883SpiWriteCommand(HMC58X3_R_MODE, 1); #else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); #endif @@ -233,7 +233,7 @@ static bool hmc5883lInit(void) // Apply the negative bias. (Same gain) #ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. + hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. #else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. #endif diff --git a/src/main/drivers/display_ug2864hsweg01.c b/src/main/drivers/display_ug2864hsweg01.c index abcc402cf..527a81aa5 100644 --- a/src/main/drivers/display_ug2864hsweg01.c +++ b/src/main/drivers/display_ug2864hsweg01.c @@ -196,7 +196,7 @@ void i2c_OLED_clear_display(busDevice_t *bus) i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0 i2c_OLED_send_cmd(bus, 0); // Set low col address to 0 i2c_OLED_send_cmd(bus, 0x10); // Set high col address to 0 - for(uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture + for (uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture i2c_OLED_send_byte(bus, 0x00); // clear } i2c_OLED_send_cmd(bus, 0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction @@ -210,7 +210,7 @@ void i2c_OLED_clear_display_quick(busDevice_t *bus) i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0 i2c_OLED_send_cmd(bus, 0); // Set low col address to 0 i2c_OLED_send_cmd(bus, 0x10); // Set high col address to 0 - for(uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture + for (uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture i2c_OLED_send_byte(bus, 0x00); // clear } } diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index c5831c95e..beb8bc2f5 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -77,7 +77,7 @@ typedef enum { dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ } -#define DMA_CLEAR_FLAG(d, flag) if(d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift) +#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift) #define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) diff --git a/src/main/drivers/dma_stm32f7xx.c b/src/main/drivers/dma_stm32f7xx.c index 0cc4b8b41..d1da2519e 100644 --- a/src/main/drivers/dma_stm32f7xx.c +++ b/src/main/drivers/dma_stm32f7xx.c @@ -77,7 +77,7 @@ static void enableDmaClock(uint32_t rcc) /* Delay after an RCC peripheral clock enabling */ tmpreg = READ_BIT(RCC->AHB1ENR, rcc); UNUSED(tmpreg); - } while(0); + } while (0); } void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index d01db12c3..f76f25d99 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -78,7 +78,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t conf (void)config; int chIdx; chIdx = IO_GPIOPinIdx(io); - if(chIdx < 0) + if (chIdx < 0) return; extiChannelRec_t *rec = &extiChannelRecs[chIdx]; int group = extiGroups[chIdx]; @@ -96,7 +96,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t conf //EXTI_ClearITPendingBit(extiLine); - if(extiGroupPriority[group] > irqPriority) { + if (extiGroupPriority[group] > irqPriority) { extiGroupPriority[group] = irqPriority; HAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); HAL_NVIC_EnableIRQ(extiGroupIRQn[group]); @@ -108,7 +108,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ { int chIdx; chIdx = IO_GPIOPinIdx(io); - if(chIdx < 0) + if (chIdx < 0) return; extiChannelRec_t *rec = &extiChannelRecs[chIdx]; int group = extiGroups[chIdx]; @@ -134,7 +134,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ EXTIInit.EXTI_LineCmd = ENABLE; EXTI_Init(&EXTIInit); - if(extiGroupPriority[group] > irqPriority) { + if (extiGroupPriority[group] > irqPriority) { extiGroupPriority[group] = irqPriority; NVIC_InitTypeDef NVIC_InitStructure; @@ -154,7 +154,7 @@ void EXTIRelease(IO_t io) int chIdx; chIdx = IO_GPIOPinIdx(io); - if(chIdx < 0) + if (chIdx < 0) return; extiChannelRec_t *rec = &extiChannelRecs[chIdx]; rec->handler = NULL; @@ -164,18 +164,18 @@ void EXTIEnable(IO_t io, bool enable) { #if defined(STM32F1) || defined(STM32F4) || defined(STM32F7) uint32_t extiLine = IO_EXTI_Line(io); - if(!extiLine) + if (!extiLine) return; - if(enable) + if (enable) EXTI->IMR |= extiLine; else EXTI->IMR &= ~extiLine; #elif defined(STM32F303xC) int extiLine = IO_EXTI_Line(io); - if(extiLine < 0) + if (extiLine < 0) return; // assume extiLine < 32 (valid for all EXTI pins) - if(enable) + if (enable) EXTI->IMR |= 1 << extiLine; else EXTI->IMR &= ~(1 << extiLine); @@ -188,7 +188,7 @@ void EXTI_IRQHandler(void) { uint32_t exti_active = EXTI->IMR & EXTI->PR; - while(exti_active) { + while (exti_active) { unsigned idx = 31 - __builtin_clz(exti_active); uint32_t mask = 1 << idx; extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler); diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index a21672640..ea9d01a0d 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -29,17 +29,23 @@ PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONF static IO_t leds[STATUS_LED_NUMBER]; static uint8_t ledInversion = 0; +#ifndef LED0_PIN +#define LED0_PIN NONE +#endif + +#ifndef LED1_PIN +#define LED1_PIN NONE +#endif + +#ifndef LED2_PIN +#define LED2_PIN NONE +#endif + void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) { -#ifdef LED0_PIN statusLedConfig->ioTags[0] = IO_TAG(LED0_PIN); -#endif -#ifdef LED1_PIN statusLedConfig->ioTags[1] = IO_TAG(LED1_PIN); -#endif -#ifdef LED2_PIN statusLedConfig->ioTags[2] = IO_TAG(LED2_PIN); -#endif statusLedConfig->inversion = 0 #ifdef LED0_INVERTED diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index 2fb155fbd..4ae9ac2b9 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -40,7 +40,7 @@ static bool timerNChannel = false; void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) { - if(htim->Instance == TimHandle.Instance) { + if (htim->Instance == TimHandle.Instance) { //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL); ws2811LedDataTransferInProgress = 0; } @@ -77,7 +77,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) TimHandle.Init.Period = period; // 800kHz TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; - if(HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) { + if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) { /* Initialization Error */ return; } @@ -116,7 +116,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaSource); /* Initialize TIMx DMA handle */ - if(HAL_DMA_Init(TimHandle.hdma[dmaSource]) != HAL_OK) { + if (HAL_DMA_Init(TimHandle.hdma[dmaSource]) != HAL_OK) { /* Initialization Error */ return; } @@ -139,7 +139,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) } TIM_OCInitStructure.Pulse = 0; TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; - if(HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, timerChannel) != HAL_OK) { + if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, timerChannel) != HAL_OK) { /* Configuration Error */ return; } @@ -154,8 +154,8 @@ void ws2811LedStripDMAEnable(void) return; } - if(timerNChannel) { - if(HAL_TIMEx_PWMN_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { + if (timerNChannel) { + if (HAL_TIMEx_PWMN_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { /* Starting PWM generation Error */ ws2811LedDataTransferInProgress = 0; return; diff --git a/src/main/drivers/light_ws2811strip_stdperiph.c b/src/main/drivers/light_ws2811strip_stdperiph.c index 50f3b019e..342b3a3f4 100644 --- a/src/main/drivers/light_ws2811strip_stdperiph.c +++ b/src/main/drivers/light_ws2811strip_stdperiph.c @@ -120,7 +120,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - + dmaRef = timerHardware->dmaRef; DMA_DeInit(dmaRef); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 3ba0c6098..f2abc383f 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -330,7 +330,7 @@ void max7456ReInit(void) ENABLE_MAX7456; - switch(videoSignalCfg) { + switch (videoSignalCfg) { case VIDEO_SYSTEM_PAL: videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; break; @@ -363,7 +363,7 @@ void max7456ReInit(void) // Set all rows to same charactor black/white level. - for(x = 0; x < maxScreenRows; x++) { + for (x = 0; x < maxScreenRows; x++) { max7456Send(MAX7456ADD_RB0 + x, BWBRIGHTNESS); } @@ -627,7 +627,7 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) max7456Send(MAX7456ADD_CMAH, char_address); // set start address high - for(x = 0; x < 54; x++) { + for (x = 0; x < 54; x++) { max7456Send(MAX7456ADD_CMAL, x); //set start address low max7456Send(MAX7456ADD_CMDI, font_data[x]); #ifdef LED0_TOGGLE diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index d065eefce..9c05608fb 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -45,14 +45,14 @@ static pwmOutputPort_t beeperPwm; static uint16_t freqBeep = 0; #endif -bool pwmMotorsEnabled = false; -bool isDshot = false; +static bool pwmMotorsEnabled = false; +static bool isDshot = false; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim); - if(Handle == NULL) return; + if (Handle == NULL) return; TIM_OC_InitTypeDef TIM_OCInitStructure; @@ -100,7 +100,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard { #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim); - if(Handle == NULL) return; + if (Handle == NULL) return; #endif configTimeBase(timerHardware->tim, period, hz); @@ -111,7 +111,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard ); #if defined(USE_HAL_DRIVER) - if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL) + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) HAL_TIMEx_PWMN_Start(Handle, timerHardware->channel); else HAL_TIM_PWM_Start(Handle, timerHardware->channel); @@ -151,7 +151,7 @@ static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet for (int i = 0; i < 16; i++) { motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first packet <<= 1; - } + } return DSHOT_DMA_BUFFER_SIZE; } @@ -169,9 +169,7 @@ static uint8_t loadDmaBufferProshot(motorDmaOutput_t *const motor, uint16_t pack void pwmWriteMotor(uint8_t index, float value) { - if (pwmMotorsEnabled) { - pwmWrite(index, value); - } + pwmWrite(index, value); } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) @@ -203,7 +201,7 @@ bool pwmAreMotorsEnabled(void) static void pwmCompleteWriteUnused(uint8_t motorCount) { - UNUSED(motorCount); + UNUSED(motorCount); } static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) @@ -226,7 +224,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) { memset(motors, 0, sizeof(motors)); - + bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; float sMin = 0; @@ -328,7 +326,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 */ motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f; motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); - + pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion); bool timerAlreadyUsed = false; @@ -397,8 +395,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) for (; repeats; repeats--) { motor->requestTelemetry = true; pwmWriteDshotInt(index, command); - pwmCompleteMotorUpdate(0); - + pwmCompleteDshotMotorUpdate(0); delay(1); } } @@ -466,9 +463,9 @@ void servoDevInit(const servoDevConfig_t *servoConfig) #ifdef BEEPER void pwmWriteBeeper(bool onoffBeep) { - if(!beeperPwm.io) + if (!beeperPwm.io) return; - if(onoffBeep == true) { + if (onoffBeep == true) { *beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2; beeperPwm.enabled = true; } else { diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index f0ef307c2..5c2b3cf7c 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -125,8 +125,6 @@ typedef struct { motorDmaOutput_t *getMotorDmaOutput(uint8_t index); -extern bool pwmMotorsEnabled; - struct timerHardware_s; typedef void pwmWriteFunc(uint8_t index, float value); // function pointer used to write motors typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer used after motors are written @@ -134,11 +132,11 @@ typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer use typedef struct { volatile timCCR_t *ccr; TIM_TypeDef *tim; + float pulseScale; + float pulseOffset; bool forceOverflow; bool enabled; IO_t io; - float pulseScale; - float pulseOffset; } pwmOutputPort_t; typedef struct motorDevConfig_s { diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 305876102..ff3843093 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -74,10 +74,6 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); - if (!pwmMotorsEnabled) { - return; - } - for (int i = 0; i < dmaMotorTimerCount; i++) { TIM_SetCounter(dmaMotorTimers[i].timer, 0); TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE); @@ -169,7 +165,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m DMA_Cmd(dmaRef, DISABLE); DMA_DeInit(dmaRef); - + DMA_StructInit(&DMA_InitStructure); #if defined(STM32F3) DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer; diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index f922c8350..bdd17a094 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -322,7 +322,7 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim); - if(Handle == NULL) return; + if (Handle == NULL) return; TIM_IC_InitTypeDef TIM_ICInitStructure; diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 4181fed7e..714359de5 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -553,7 +553,7 @@ void sdcard_init(bool useDMA) #ifdef SDCARD_DMA_CHANNEL_TX useDMAForTx = useDMA; if (useDMAForTx) { - dmaInit(dmaGetIdentifier(SDCARD_DMA_CHANNEL_TX), OWNER_SDCARD, 0); + dmaInit(dmaGetIdentifier(SDCARD_DMA_CHANNEL_TX), OWNER_SDCARD, 0); } #else // DMA is not available diff --git a/src/main/drivers/sdcard_standard.c b/src/main/drivers/sdcard_standard.c index 00fb770ff..0bcf8ccf9 100644 --- a/src/main/drivers/sdcard_standard.c +++ b/src/main/drivers/sdcard_standard.c @@ -1,8 +1,7 @@ #include #include "sdcard_standard.h" - -#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#include "common/maths.h" /** * Read a bitfield from an array of bits (the bit at index 0 being the most-significant bit of the first byte in diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 30aad79a2..38d129e69 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -56,6 +56,9 @@ typedef enum { static serialPort_t *escPort = NULL; static serialPort_t *passPort = NULL; +#define ICPOLARITY_RISING true +#define ICPOLARITY_FALLING false + typedef struct escSerial_s { serialPort_t port; @@ -67,6 +70,11 @@ typedef struct escSerial_s { const timerHardware_t *txTimerHardware; volatile uint8_t txBuffer[ESCSERIAL_BUFFER_SIZE]; +#ifdef USE_HAL_DRIVER + const TIM_HandleTypeDef *txTimerHandle; + const TIM_HandleTypeDef *rxTimerHandle; +#endif + uint8_t isSearchingForStartBit; uint8_t rxBitIndex; uint8_t rxLastLeadingEdgeAtBitIndex; @@ -119,15 +127,22 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); -static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); + +// XXX No TIM_DeInit equivalent in HAL driver??? +#ifdef USE_HAL_DRIVER +static void TIM_DeInit(TIM_TypeDef *tim) +{ + UNUSED(tim); +} +#endif void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) { - if(escSerial->mode == PROTOCOL_KISSALL) + if (escSerial->mode == PROTOCOL_KISSALL) { for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) { uint8_t state_temp = state; - if(escOutputs[i].inverted) { + if (escOutputs[i].inverted) { state_temp ^= ENABLE; } @@ -140,7 +155,7 @@ void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) } else { - if(escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) { + if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) { state ^= ENABLE; } @@ -152,22 +167,28 @@ void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) } } -static void escSerialGPIOConfig(ioTag_t tag, ioConfig_t cfg) +static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg) { + ioTag_t tag = timhw->tag; + if (!tag) { return; } IOInit(IOGetByTag(tag), OWNER_MOTOR, 0); +#ifdef STM32F7 + IOConfigGPIOAF(IOGetByTag(tag), cfg, timhw->alternateFunction); +#else IOConfigGPIO(IOGetByTag(tag), cfg); +#endif } void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr) { #ifdef STM32F10X - escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU); + escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU); #else - escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_AF_PP_UP); + escSerialGPIOConfig(timerHardwarePtr, IOCFG_AF_PP_UP); #endif timerChClearCCFlag(timerHardwarePtr); timerChITConfig(timerHardwarePtr,ENABLE); @@ -206,7 +227,7 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 // start bit is usually a FALLING signal TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, 0xFFFF, SystemCoreClock / 2); - escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + timerChConfigIC(timerHardwarePtr, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); } @@ -220,33 +241,19 @@ static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); } -static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) -{ - TIM_ICInitTypeDef TIM_ICInitStructure; - - TIM_ICStructInit(&TIM_ICInitStructure); - TIM_ICInitStructure.TIM_Channel = channel; - TIM_ICInitStructure.TIM_ICPolarity = polarity; - TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; - TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0x0; - - TIM_ICInit(tim, &TIM_ICInitStructure); -} - static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { // start bit is usually a FALLING signal TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1)); - escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); + timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); } static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr) { - escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_OUT_PP); + escSerialGPIOConfig(timerHardwarePtr, IOCFG_OUT_PP); timerChITConfig(timerHardwarePtr,DISABLE); } @@ -267,8 +274,11 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac { escSerial_t *escSerial = &(escSerialPorts[portIndex]); - if(mode != PROTOCOL_KISSALL){ + if (mode != PROTOCOL_KISSALL) { escSerial->rxTimerHardware = &(timerHardware[output]); +#ifdef USE_HAL_DRIVER + escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim); +#endif } escSerial->mode = mode; @@ -278,6 +288,10 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac return NULL; } +#ifdef USE_HAL_DRIVER + escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim); +#endif + escSerial->port.vTable = escSerialVTable; escSerial->port.baudRate = baud; escSerial->port.mode = MODE_RXTX; @@ -297,7 +311,7 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac escSerial->escSerialPortIndex = portIndex; - if(mode != PROTOCOL_KISSALL) + if (mode != PROTOCOL_KISSALL) { escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag); escSerialInputPortConfig(escSerial->rxTimerHardware); @@ -305,19 +319,19 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac } delay(50); - if(mode==PROTOCOL_SIMONK){ + if (mode==PROTOCOL_SIMONK) { escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex); escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex); } - else if(mode==PROTOCOL_BLHELI){ + else if (mode==PROTOCOL_BLHELI) { serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options); } - else if(mode==PROTOCOL_KISS) { + else if (mode==PROTOCOL_KISS) { escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); } - else if(mode==PROTOCOL_KISSALL) { + else if (mode==PROTOCOL_KISSALL) { escSerial->outputCount = 0; memset(&escOutputs, 0, sizeof(escOutputs)); pwmOutputPort_t *pwmMotors = pwmGetMotors(); @@ -325,10 +339,10 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac if (pwmMotors[i].enabled) { if (pwmMotors[i].io != IO_NONE) { for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) { - if(pwmMotors[i].io == IOGetByTag(timerHardware[j].tag)) + if (pwmMotors[i].io == IOGetByTag(timerHardware[j].tag)) { escSerialOutputPortConfig(&timerHardware[j]); - if(timerHardware[j].output & TIMER_OUTPUT_INVERTED) { + if (timerHardware[j].output & TIMER_OUTPUT_INVERTED) { escOutputs[escSerial->outputCount].inverted = 1; } break; @@ -342,7 +356,7 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac setTxSignalEsc(escSerial, ENABLE); serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); } - else if(mode == PROTOCOL_CASTLE){ + else if (mode == PROTOCOL_CASTLE){ escSerialOutputPortConfig(escSerial->rxTimerHardware); serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options); @@ -355,7 +369,7 @@ void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr) { timerChClearCCFlag(timerHardwarePtr); timerChITConfig(timerHardwarePtr,DISABLE); - escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU); + escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU); } @@ -363,7 +377,7 @@ void closeEscSerial(escSerialPortIndex_e portIndex, uint8_t mode) { escSerial_t *escSerial = &(escSerialPorts[portIndex]); - if(mode != PROTOCOL_KISSALL){ + if (mode != PROTOCOL_KISSALL) { escSerialInputPortDeConfig(escSerial->rxTimerHardware); timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL); TIM_DeInit(escSerial->rxTimerHardware->tim); @@ -383,7 +397,7 @@ void processTxStateEsc(escSerial_t *escSerial) return; } - if(transmitStart==0) + if (transmitStart==0) { setTxSignalEsc(escSerial, 1); } @@ -396,13 +410,13 @@ reload: return; } - if(transmitStart<3) + if (transmitStart<3) { - if(transmitStart==0) + if (transmitStart==0) byteToSend = 0xff; - if(transmitStart==1) + if (transmitStart==1) byteToSend = 0xff; - if(transmitStart==2) + if (transmitStart==2) byteToSend = 0x7f; transmitStart++; } @@ -427,35 +441,35 @@ reload: if (escSerial->bitsLeftToTransmit) { mask = escSerial->internalTxBuffer & 1; - if(mask) + if (mask) { - if(bitq==0 || bitq==1) + if (bitq==0 || bitq==1) { setTxSignalEsc(escSerial, 1); } - if(bitq==2 || bitq==3) + if (bitq==2 || bitq==3) { setTxSignalEsc(escSerial, 0); } } else { - if(bitq==0 || bitq==2) + if (bitq==0 || bitq==2) { setTxSignalEsc(escSerial, 1); } - if(bitq==1 ||bitq==3) + if (bitq==1 ||bitq==3) { setTxSignalEsc(escSerial, 0); } } bitq++; - if(bitq>3) + if (bitq>3) { escSerial->internalTxBuffer >>= 1; escSerial->bitsLeftToTransmit--; bitq=0; - if(escSerial->bitsLeftToTransmit==0) + if (escSerial->bitsLeftToTransmit==0) { goto reload; } @@ -499,7 +513,7 @@ void processTxStateBL(escSerial_t *escSerial) //set output - if(escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) { + if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) { escSerialOutputPortConfig(escSerial->rxTimerHardware); } return; @@ -516,7 +530,7 @@ void processTxStateBL(escSerial_t *escSerial) escSerial->isTransmittingData = false; if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { - if(escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) + if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) { escSerialInputPortConfig(escSerial->rxTimerHardware); } @@ -547,10 +561,9 @@ void prepareForNextRxByteBL(escSerial_t *escSerial) escSerial->isSearchingForStartBit = true; if (escSerial->rxEdge == LEADING) { escSerial->rxEdge = TRAILING; - escSerialICConfig( - escSerial->rxTimerHardware->tim, - escSerial->rxTimerHardware->channel, - (escSerial->port.options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling + timerChConfigIC( + escSerial->rxTimerHardware, + (escSerial->port.options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0 ); } } @@ -627,15 +640,19 @@ void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) } if (escSerial->isSearchingForStartBit) { - // synchronise bit counter - // FIXME this reduces functionality somewhat as receiving breaks concurrent transmission on all ports because - // the next callback to the onSerialTimer will happen too early causing transmission errors. + // Adjust the timing so it will interrupt on the middle. + // This is clobbers transmission, but it is okay because we are + // always half-duplex. +#ifdef USE_HAL_DRIVER + __HAL_TIM_SetCounter(escSerial->txTimerHandle, __HAL_TIM_GetAutoreload(escSerial->txTimerHandle) / 2); +#else TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2); +#endif if (escSerial->isTransmittingData) { escSerial->transmissionErrors++; } - escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0); escSerial->rxEdge = LEADING; escSerial->rxBitIndex = 0; @@ -653,10 +670,10 @@ void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) if (escSerial->rxEdge == TRAILING) { escSerial->rxEdge = LEADING; - escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0); } else { escSerial->rxEdge = TRAILING; - escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); } } /*-------------------------BL*/ @@ -682,14 +699,14 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture) UNUSED(capture); escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb); - if(escSerial->isReceivingData) + if (escSerial->isReceivingData) { escSerial->receiveTimeout++; - if(escSerial->receiveTimeout>8) + if (escSerial->receiveTimeout>8) { escSerial->isReceivingData=0; escSerial->receiveTimeout=0; - escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling); + timerChConfigIC(escSerial->rxTimerHardware, ICPOLARITY_FALLING, 0); } } @@ -707,19 +724,23 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb); //clear timer +#ifdef USE_HAL_DRIVER + __HAL_TIM_SetCounter(escSerial->rxTimerHandle, 0); +#else TIM_SetCounter(escSerial->rxTimerHardware->tim,0); +#endif - if(capture > 40 && capture < 90) + if (capture > 40 && capture < 90) { zerofirst++; - if(zerofirst>1) + if (zerofirst>1) { zerofirst=0; escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; bits++; } } - else if(capture>90 && capture < 200) + else if (capture>90 && capture < 200) { zerofirst=0; escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; @@ -728,7 +749,7 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture } else { - if(!escSerial->isReceivingData) + if (!escSerial->isReceivingData) { //start //lets reset @@ -739,16 +760,16 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture bits=1; escSerial->internalRxBuffer = 0x80; - escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising); + timerChConfigIC(escSerial->rxTimerHardware, ICPOLARITY_RISING, 0); } } escSerial->receiveTimeout = 0; - if(bits==8) + if (bits==8) { bits=0; bytes++; - if(bytes>3) + if (bytes>3) { extractAndStoreRxByteEsc(escSerial); } @@ -847,9 +868,9 @@ void escSerialInitialize() for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { // set outputs to pullup - if(timerHardware[i].output & TIMER_OUTPUT_ENABLED) + if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) { - escSerialGPIOConfig(timerHardware[i].tag, IOCFG_IPU); //GPIO_Mode_IPU + escSerialGPIOConfig(&timerHardware[i], IOCFG_IPU); //GPIO_Mode_IPU } } } @@ -911,7 +932,7 @@ static bool ProcessExitCommand(uint8_t c) if (currentPort.checksum == c) { currentPort.c_state = COMMAND_RECEIVED; - if((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0)) + if ((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0)) { currentPort.c_state = IDLE; return true; @@ -948,14 +969,14 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin break; } - if((mode == PROTOCOL_KISS) && (output == 255)){ + if ((mode == PROTOCOL_KISS) && (output == 255)) { motor_output = 255; mode = PROTOCOL_KISSALL; } else { uint8_t first_output = 0; for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if(timerHardware[i].output & TIMER_OUTPUT_ENABLED) + if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) { first_output=i; break; @@ -964,7 +985,7 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin //doesn't work with messy timertable motor_output=first_output+output-1; - if(motor_output >=USABLE_TIMER_CHANNEL_COUNT) + if (motor_output >=USABLE_TIMER_CHANNEL_COUNT) return; } @@ -975,12 +996,12 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin } uint8_t ch; - while(1) { - if(mode!=2) + while (1) { + if (mode!=2) { if (serialRxBytesWaiting(escPort)) { LED0_ON; - while(serialRxBytesWaiting(escPort)) + while (serialRxBytesWaiting(escPort)) { ch = serialRead(escPort); serialWrite(escPassthroughPort, ch); @@ -990,11 +1011,11 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin } if (serialRxBytesWaiting(escPassthroughPort)) { LED1_ON; - while(serialRxBytesWaiting(escPassthroughPort)) + while (serialRxBytesWaiting(escPassthroughPort)) { ch = serialRead(escPassthroughPort); exitEsc = ProcessExitCommand(ch); - if(exitEsc) + if (exitEsc) { serialWrite(escPassthroughPort, 0x24); serialWrite(escPassthroughPort, 0x4D); @@ -1005,14 +1026,14 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin closeEscSerial(ESCSERIAL1, mode); return; } - if(mode==PROTOCOL_BLHELI){ + if (mode==PROTOCOL_BLHELI) { serialWrite(escPassthroughPort, ch); // blheli loopback } serialWrite(escPort, ch); } LED1_OFF; } - if(mode != PROTOCOL_CASTLE){ + if (mode != PROTOCOL_CASTLE) { delay(5); } } diff --git a/src/main/drivers/serial_pinconfig.c b/src/main/drivers/serial_pinconfig.c index 3bf873a48..e1c4e2bb7 100644 --- a/src/main/drivers/serial_pinconfig.c +++ b/src/main/drivers/serial_pinconfig.c @@ -262,8 +262,6 @@ PG_REGISTER_WITH_RESET_FN(serialPinConfig_t, serialPinConfig, PG_SERIAL_PIN_CONF void pgResetFn_serialPinConfig(serialPinConfig_t *serialPinConfig) { - memset(serialPinConfig, 0, sizeof(*serialPinConfig)); - for (size_t index = 0 ; index < ARRAYLEN(serialDefaultPin) ; index++) { const serialDefaultPin_t *defpin = &serialDefaultPin[index]; serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_INDEX(defpin->ident)] = defpin->rxIO; diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index a132549b3..ba31b7cdc 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -53,7 +53,7 @@ static void onClose(dyad_Event *e) { s->clientCount--; s->conn = NULL; fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount); - if(s->clientCount == 0) { + if (s->clientCount == 0) { s->connected = false; } } @@ -62,7 +62,7 @@ static void onAccept(dyad_Event *e) { fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount); s->connected = true; - if(s->clientCount > 0) { + if (s->clientCount > 0) { dyad_close(e->remote); return; } @@ -76,7 +76,7 @@ static void onAccept(dyad_Event *e) { } static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id) { - if(tcpPortInitialized[id]) { + if (tcpPortInitialized[id]) { fprintf(stderr, "port is already initialized!\n"); return s; } @@ -103,7 +103,7 @@ static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id) dyad_setNoDelay(s->serv, 1); dyad_addListener(s->serv, DYAD_EVENT_ACCEPT, onAccept, s); - if(dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) { + if (dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) { fprintf(stderr, "bind port %u for UART%u\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1); } else { fprintf(stderr, "bind port %u for UART%u failed!!\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1); @@ -116,11 +116,11 @@ serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t b tcpPort_t *s = NULL; #if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8) - if(id >= 0 && id < SERIAL_PORT_COUNT) { + if (id >= 0 && id < SERIAL_PORT_COUNT) { s = tcpReconfigure(&tcpSerialPorts[id], id); } #endif - if(!s) + if (!s) return NULL; s->port.vTable = &tcpVTable; @@ -219,7 +219,7 @@ void tcpWrite(serialPort_t *instance, uint8_t ch) void tcpDataOut(tcpPort_t *instance) { tcpPort_t *s = (tcpPort_t *)instance; - if(s->conn == NULL) return; + if (s->conn == NULL) return; pthread_mutex_lock(&s->txLock); if (s->port.txBufferHead < s->port.txBufferTail) { @@ -229,7 +229,7 @@ void tcpDataOut(tcpPort_t *instance) s->port.txBufferTail = 0; } int chunk = s->port.txBufferHead - s->port.txBufferTail; - if(chunk) + if (chunk) dyad_write(s->conn, (const void*)&s->port.txBuffer[s->port.txBufferTail], chunk); s->port.txBufferTail = s->port.txBufferHead; @@ -241,7 +241,7 @@ void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size) tcpPort_t *s = (tcpPort_t *)instance; pthread_mutex_lock(&s->rxLock); - while(size--) { + while (size--) { // printf("%c", *ch); s->port.rxBuffer[s->port.rxBufferHead] = *(ch++); if (s->port.rxBufferHead + 1 >= s->port.rxBufferSize) { diff --git a/src/main/drivers/serial_tcp.h b/src/main/drivers/serial_tcp.h index 2c74488ee..5efe413f8 100644 --- a/src/main/drivers/serial_tcp.h +++ b/src/main/drivers/serial_tcp.h @@ -29,13 +29,13 @@ typedef struct { uint8_t rxBuffer[RX_BUFFER_SIZE]; uint8_t txBuffer[TX_BUFFER_SIZE]; - dyad_Stream *serv; - dyad_Stream *conn; - pthread_mutex_t txLock; - pthread_mutex_t rxLock; - bool connected; - uint16_t clientCount; - uint8_t id; + dyad_Stream *serv; + dyad_Stream *conn; + pthread_mutex_t txLock; + pthread_mutex_t rxLock; + bool connected; + uint16_t clientCount; + uint8_t id; } tcpPort_t; serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options); diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index fa213eeab..a4a7056c5 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -43,7 +43,7 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { bool inverted = uartPort->port.options & SERIAL_INVERTED; - if(inverted) + if (inverted) { if (uartPort->port.mode & MODE_RX) { @@ -94,7 +94,7 @@ void uartReconfigure(uartPort_t *uartPort) usartConfigurePinInversion(uartPort); - if(uartPort->port.options & SERIAL_BIDIR) + if (uartPort->port.options & SERIAL_BIDIR) { HAL_HalfDuplex_Init(&uartPort->Handle); } @@ -167,9 +167,9 @@ void uartReconfigure(uartPort_t *uartPort) HAL_DMA_DeInit(&uartPort->txDMAHandle); HAL_StatusTypeDef status = HAL_DMA_Init(&uartPort->txDMAHandle); - if(status != HAL_OK) + if (status != HAL_OK) { - while(1); + while (1); } /* Associate the initialized DMA handle to the UART handle */ __HAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle); @@ -225,7 +225,7 @@ void uartStartTxDMA(uartPort_t *s) uint16_t size = 0; uint32_t fromwhere=0; HAL_UART_StateTypeDef state = HAL_UART_GetState(&s->Handle); - if((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX) + if ((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX) return; if (s->port.txBufferHead > s->port.txBufferTail) { diff --git a/src/main/drivers/serial_uart_init.c b/src/main/drivers/serial_uart_init.c index 8852e2472..780c822a6 100644 --- a/src/main/drivers/serial_uart_init.c +++ b/src/main/drivers/serial_uart_init.c @@ -104,7 +104,7 @@ void uartReconfigure(uartPort_t *uartPort) usartConfigurePinInversion(uartPort); - if(uartPort->port.options & SERIAL_BIDIR) + if (uartPort->port.options & SERIAL_BIDIR) USART_HalfDuplexCmd(uartPort->USARTx, ENABLE); else USART_HalfDuplexCmd(uartPort->USARTx, DISABLE); diff --git a/src/main/drivers/serial_uart_pinconfig.c b/src/main/drivers/serial_uart_pinconfig.c index b16f5fb87..0cf2ec679 100644 --- a/src/main/drivers/serial_uart_pinconfig.c +++ b/src/main/drivers/serial_uart_pinconfig.c @@ -43,7 +43,7 @@ void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig) { uartDevice_t *uartdev = uartDevice; - for (size_t hindex = 0; hindex < UARTDEV_COUNT_MAX; hindex++) { + for (size_t hindex = 0; hindex < UARTDEV_COUNT; hindex++) { const uartHardware_t *hardware = &uartHardware[hindex]; UARTDevice device = hardware->device; diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index f6ecd7e56..feb2ab2c0 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -74,7 +74,7 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { }, #endif #ifdef USE_UART2 - { + { .device = UARTDEV_2, .reg = USART2, .rxDMAChannel = UART2_RX_DMA_CHANNEL, @@ -101,8 +101,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .irqn = USART3_IRQn, .txPriority = NVIC_PRIO_SERIALUART3, .rxPriority = NVIC_PRIO_SERIALUART3 - }, -#endif + }, +#endif }; void uart_tx_dma_IRQHandler(dmaChannelDescriptor_t* descriptor) diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c index aeb6906a1..25252d133 100644 --- a/src/main/drivers/serial_uart_stm32f7xx.c +++ b/src/main/drivers/serial_uart_stm32f7xx.c @@ -228,7 +228,7 @@ void uartIrqHandler(uartPort_t *s) { UART_HandleTypeDef *huart = &s->Handle; /* UART in mode Receiver ---------------------------------------------------*/ - if((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) + if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) { uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff); @@ -247,37 +247,37 @@ void uartIrqHandler(uartPort_t *s) } /* UART parity error interrupt occurred -------------------------------------*/ - if((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) + if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) { __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF); } /* UART frame error interrupt occurred --------------------------------------*/ - if((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) + if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) { __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF); } /* UART noise error interrupt occurred --------------------------------------*/ - if((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) + if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) { __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF); } /* UART Over-Run interrupt occurred -----------------------------------------*/ - if((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) + if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) { __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF); } /* UART in mode Transmitter ------------------------------------------------*/ - if((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) + if ((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) { HAL_UART_IRQHandler(huart); } /* UART in mode Transmitter (transmission end) -----------------------------*/ - if((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) + if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) { HAL_UART_IRQHandler(huart); handleUsartTxDma(s); @@ -308,7 +308,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po uartDevice_t *uartdev = uartDevmap[device]; if (!uartdev) { return NULL; - } + } uartPort_t *s = &(uartdev->port); @@ -370,7 +370,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po //HAL_NVIC_SetPriority(hardware->txIrq, NVIC_PRIORITY_BASE(hardware->txPriority), NVIC_PRIORITY_SUB(hardware->txPriority)); //HAL_NVIC_EnableIRQ(hardware->txIrq); - if(!s->rxDMAChannel) + if (!s->rxDMAChannel) { HAL_NVIC_SetPriority(hardware->rxIrq, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority)); HAL_NVIC_EnableIRQ(hardware->rxIrq); diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 04a67f41d..ebef30d1d 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -35,7 +35,7 @@ static uint16_t beeperFrequency = 0; void systemBeep(bool onoff) { #ifdef BEEPER - if(beeperFrequency == 0) { + if (beeperFrequency == 0) { IOWrite(beeperIO, beeperInverted ? onoff : !onoff); } else { pwmWriteBeeper(onoff); @@ -48,7 +48,7 @@ void systemBeep(bool onoff) void systemBeepToggle(void) { #ifdef BEEPER - if(beeperFrequency == 0) { + if (beeperFrequency == 0) { IOToggle(beeperIO); } else { pwmToggleBeeper(); @@ -60,7 +60,7 @@ void beeperInit(const beeperDevConfig_t *config) { #ifdef BEEPER beeperFrequency = config->frequency; - if(beeperFrequency == 0) { + if (beeperFrequency == 0) { beeperIO = IOGetByTag(config->ioTag); beeperInverted = config->isInverted; if (beeperIO) { diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index ef85ec173..e2b2a2478 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -24,9 +24,9 @@ #define BEEP_OFF systemBeep(false) #define BEEP_ON systemBeep(true) #else -#define BEEP_TOGGLE do {} while(0) -#define BEEP_OFF do {} while(0) -#define BEEP_ON do {} while(0) +#define BEEP_TOGGLE do {} while (0) +#define BEEP_OFF do {} while (0) +#define BEEP_ON do {} while (0) #endif typedef struct beeperDevConfig_s { diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index b27a917d9..a3eee7939 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -162,49 +162,45 @@ void delay(uint32_t ms) } #define SHORT_FLASH_DURATION 50 +#define SHORT_FLASH_COUNT 5 #define CODE_FLASH_DURATION 250 -void failureLedCode(failureMode_e mode, int codeRepeatsRemaining) +static void indicate(uint8_t count, uint16_t duration) { - int codeFlashesRemaining; - int shortFlashesRemaining; - - while (codeRepeatsRemaining--) { + if (count) { LED1_ON; LED0_OFF; - shortFlashesRemaining = 5; - codeFlashesRemaining = mode + 1; - uint8_t flashDuration = SHORT_FLASH_DURATION; - while (shortFlashesRemaining || codeFlashesRemaining) { + while (count--) { LED1_TOGGLE; LED0_TOGGLE; BEEP_ON; - delay(flashDuration); + delay(duration); LED1_TOGGLE; LED0_TOGGLE; BEEP_OFF; - delay(flashDuration); - - if (shortFlashesRemaining) { - shortFlashesRemaining--; - if (shortFlashesRemaining == 0) { - delay(500); - flashDuration = CODE_FLASH_DURATION; - } - } else { - codeFlashesRemaining--; - } + delay(duration); } + } +} + +void indicateFailure(failureMode_e mode, int codeRepeatsRemaining) +{ + while (codeRepeatsRemaining--) { + indicate(SHORT_FLASH_COUNT, SHORT_FLASH_DURATION); + + delay(500); + + indicate(mode + 1, CODE_FLASH_DURATION); + delay(1000); } - } void failureMode(failureMode_e mode) { - failureLedCode(mode, 10); + indicateFailure(mode, 10); #ifdef DEBUG systemReset(); diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 4a5b83535..6e52268d6 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -33,7 +33,7 @@ typedef enum { } failureMode_e; // failure -void failureLedCode(failureMode_e mode, int repeatCount); +void indicateFailure(failureMode_e mode, int repeatCount); void failureMode(failureMode_e mode); // bootloader/IAP diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 57a2b69f4..a53128bbd 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -78,7 +78,7 @@ static uint8_t lookupTimerIndex(const TIM_TypeDef *tim) #define _CASE(i) _CASE_(TIM##i##_BASE, TIMER_INDEX(i)) // let gcc do the work, switch should be quite optimized - switch((unsigned)tim >> _CASE_SHF) { + switch ((unsigned)tim >> _CASE_SHF) { #if USED_TIMERS & TIM_N(1) _CASE(1); #endif @@ -254,7 +254,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui uint8_t irq = timerInputIrq(timerHardwarePtr->tim); timerNVICConfigure(irq); // HACK - enable second IRQ on timers that need it - switch(irq) { + switch (irq) { #if defined(STM32F10X) case TIM1_CC_IRQn: timerNVICConfigure(TIM1_UP_IRQn); @@ -287,14 +287,14 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq) { unsigned channel = timHw - timerHardware; - if(channel >= USABLE_TIMER_CHANNEL_COUNT) + if (channel >= USABLE_TIMER_CHANNEL_COUNT) return; timerChannelInfo[channel].type = type; unsigned timer = lookupTimerIndex(timHw->tim); - if(timer >= USED_TIMER_COUNT) + if (timer >= USED_TIMER_COUNT) return; - if(irqPriority < timerInfo[timer].priority) { + if (irqPriority < timerInfo[timer].priority) { // it would be better to set priority in the end, but current startup sequence is not ready configTimeBase(usedTimers[timer], 0, 1); TIM_Cmd(usedTimers[timer], ENABLE); @@ -327,8 +327,8 @@ void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback * static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) { timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive; ATOMIC_BLOCK(NVIC_PRIO_TIMER) { - for(int i = 0; i < CC_CHANNELS_PER_TIMER; i++) - if(cfg->overflowCallback[i]) { + for (int i = 0; i < CC_CHANNELS_PER_TIMER; i++) + if (cfg->overflowCallback[i]) { *chain = cfg->overflowCallback[i]; chain = &cfg->overflowCallback[i]->next; } @@ -346,13 +346,13 @@ void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *e return; } uint8_t channelIndex = lookupChannelIndex(timHw->channel); - if(edgeCallback == NULL) // disable irq before changing callback to NULL + if (edgeCallback == NULL) // disable irq before changing callback to NULL TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), DISABLE); // setup callback info timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback; timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback; // enable channel IRQ - if(edgeCallback) + if (edgeCallback) TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), ENABLE); timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim); @@ -371,9 +371,9 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_ uint16_t chHi = timHw->channel | TIM_Channel_2; // upper channel uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel - if(edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL + if (edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL TIM_ITConfig(timHw->tim, TIM_IT_CCx(chLo), DISABLE); - if(edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL + if (edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL TIM_ITConfig(timHw->tim, TIM_IT_CCx(chHi), DISABLE); // setup callback info @@ -383,11 +383,11 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_ timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL; // enable channel IRQs - if(edgeCallbackLo) { + if (edgeCallbackLo) { TIM_ClearFlag(timHw->tim, TIM_IT_CCx(chLo)); TIM_ITConfig(timHw->tim, TIM_IT_CCx(chLo), ENABLE); } - if(edgeCallbackHi) { + if (edgeCallbackHi) { TIM_ClearFlag(timHw->tim, TIM_IT_CCx(chHi)); TIM_ITConfig(timHw->tim, TIM_IT_CCx(chHi), ENABLE); } @@ -433,8 +433,8 @@ static unsigned getFilter(unsigned ticks) 16*5, 16*6, 16*8, 32*5, 32*6, 32*8 }; - for(unsigned i = 1; i < ARRAYLEN(ftab); i++) - if(ftab[i] > ticks) + for (unsigned i = 1; i < ARRAYLEN(ftab); i++) + if (ftab[i] > ticks) return i - 1; return 0x0f; } @@ -504,7 +504,7 @@ void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHig TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCStructInit(&TIM_OCInitStructure); - if(outEnable) { + if (outEnable) { TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; if (timHw->output & TIMER_OUTPUT_INVERTED) { @@ -541,17 +541,17 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) unsigned tim_status; tim_status = tim->SR & tim->DIER; #if 1 - while(tim_status) { + while (tim_status) { // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway) unsigned bit = __builtin_clz(tim_status); unsigned mask = ~(0x80000000 >> bit); tim->SR = mask; tim_status &= mask; - switch(bit) { + switch (bit) { case __builtin_clz(TIM_IT_Update): { - if(timerConfig->forcedOverflowTimerValue != 0){ + if (timerConfig->forcedOverflowTimerValue != 0) { capture = timerConfig->forcedOverflowTimerValue - 1; timerConfig->forcedOverflowTimerValue = 0; } else { @@ -559,7 +559,7 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) } timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; - while(cb) { + while (cb) { cb->fn(cb, capture); cb = cb->next; } @@ -584,7 +584,7 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) tim->SR = ~TIM_IT_Update; capture = tim->ARR; timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; - while(cb) { + while (cb) { cb->fn(cb, capture); cb = cb->next; } @@ -714,10 +714,10 @@ void timerInit(void) #endif // initialize timer channel structures - for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { timerChannelInfo[i].type = TYPE_FREE; } - for(int i = 0; i < USED_TIMER_COUNT; i++) { + for (int i = 0; i < USED_TIMER_COUNT; i++) { timerInfo[i].priority = ~0; } } @@ -728,18 +728,18 @@ void timerInit(void) void timerStart(void) { #if 0 - for(unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) { + for (unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) { int priority = -1; int irq = -1; - for(unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++) - if((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) { + for (unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++) + if ((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) { // TODO - move IRQ to timer info irq = timerHardware[hwc].irq; } // TODO - aggregate required timer paramaters configTimeBase(usedTimers[timer], 0, 1); TIM_Cmd(usedTimers[timer], ENABLE); - if(priority >= 0) { // maybe none of the channels was configured + if (priority >= 0) { // maybe none of the channels was configured NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = irq; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 1127a1c45..67375f3c0 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -182,7 +182,6 @@ void timerInit(void); void timerStart(void); void timerForceOverflow(TIM_TypeDef *tim); -uint8_t timerClockDivisor(TIM_TypeDef *tim); uint32_t timerClock(TIM_TypeDef *tim); void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz); // TODO - just for migration diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 30eb04e83..95653b451 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -86,7 +86,7 @@ static uint8_t lookupTimerIndex(const TIM_TypeDef *tim) #define _CASE(i) _CASE_(TIM##i##_BASE, TIMER_INDEX(i)) // let gcc do the work, switch should be quite optimized - switch((unsigned)tim >> _CASE_SHF) { + switch ((unsigned)tim >> _CASE_SHF) { #if USED_TIMERS & TIM_N(1) _CASE(1); #endif @@ -247,7 +247,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) if (timerIndex >= USED_TIMER_COUNT) { return; } - if(timerHandle[timerIndex].Handle.Instance == tim) + if (timerHandle[timerIndex].Handle.Instance == tim) { // already configured return; @@ -263,7 +263,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) timerHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000; HAL_TIM_Base_Init(&timerHandle[timerIndex].Handle); - if(tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 || tim == TIM9) + if (tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 || tim == TIM9) { TIM_ClockConfigTypeDef sClockSourceConfig; memset(&sClockSourceConfig, 0, sizeof(sClockSourceConfig)); @@ -273,7 +273,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) return; } } - if(tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 ) + if (tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 ) { TIM_MasterConfigTypeDef sMasterConfig; memset(&sMasterConfig, 0, sizeof(sMasterConfig)); @@ -300,7 +300,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui uint8_t irq = timerInputIrq(timerHardwarePtr->tim); timerNVICConfigure(irq); // HACK - enable second IRQ on timers that need it - switch(irq) { + switch (irq) { case TIM1_CC_IRQn: timerNVICConfigure(TIM1_UP_TIM10_IRQn); @@ -320,14 +320,14 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori return; } unsigned channel = timHw - timerHardware; - if(channel >= USABLE_TIMER_CHANNEL_COUNT) + if (channel >= USABLE_TIMER_CHANNEL_COUNT) return; timerChannelInfo[channel].type = type; unsigned timer = lookupTimerIndex(timHw->tim); - if(timer >= USED_TIMER_COUNT) + if (timer >= USED_TIMER_COUNT) return; - if(irqPriority < timerInfo[timer].priority) { + if (irqPriority < timerInfo[timer].priority) { // it would be better to set priority in the end, but current startup sequence is not ready configTimeBase(usedTimers[timer], 0, 1); HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle); @@ -361,15 +361,15 @@ static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) { timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive; ATOMIC_BLOCK(NVIC_PRIO_TIMER) { - for(int i = 0; i < CC_CHANNELS_PER_TIMER; i++) - if(cfg->overflowCallback[i]) { + for (int i = 0; i < CC_CHANNELS_PER_TIMER; i++) + if (cfg->overflowCallback[i]) { *chain = cfg->overflowCallback[i]; chain = &cfg->overflowCallback[i]->next; } *chain = NULL; } // enable or disable IRQ - if(cfg->overflowCallbackActive) + if (cfg->overflowCallbackActive) __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_UPDATE); else __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_UPDATE); @@ -383,13 +383,13 @@ void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *e return; } uint8_t channelIndex = lookupChannelIndex(timHw->channel); - if(edgeCallback == NULL) // disable irq before changing callback to NULL + if (edgeCallback == NULL) // disable irq before changing callback to NULL __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); // setup callback info timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback; timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback; // enable channel IRQ - if(edgeCallback) + if (edgeCallback) __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim); @@ -408,9 +408,9 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_ uint16_t chHi = timHw->channel | TIM_CHANNEL_2; // upper channel uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel - if(edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL + if (edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); - if(edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL + if (edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); // setup callback info @@ -420,11 +420,11 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_ timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL; // enable channel IRQs - if(edgeCallbackLo) { + if (edgeCallbackLo) { __HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); } - if(edgeCallbackHi) { + if (edgeCallbackHi) { __HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); } @@ -443,7 +443,7 @@ void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newStat return; } - if(newState) + if (newState) __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2)); else __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2)); @@ -462,7 +462,7 @@ void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState) return; } - if(newState) + if (newState) __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); else __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); @@ -505,8 +505,8 @@ static unsigned getFilter(unsigned ticks) 16*5, 16*6, 16*8, 32*5, 32*6, 32*8 }; - for(unsigned i = 1; i < ARRAYLEN(ftab); i++) - if(ftab[i] > ticks) + for (unsigned i = 1; i < ARRAYLEN(ftab); i++) + if (ftab[i] > ticks) return i - 1; return 0x0f; } @@ -515,7 +515,7 @@ static unsigned getFilter(unsigned ticks) void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) { unsigned timer = lookupTimerIndex(timHw->tim); - if(timer >= USED_TIMER_COUNT) + if (timer >= USED_TIMER_COUNT) return; TIM_IC_InitTypeDef TIM_ICInitStructure; @@ -532,7 +532,7 @@ void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned void timerChConfigICDual(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) { unsigned timer = lookupTimerIndex(timHw->tim); - if(timer >= USED_TIMER_COUNT) + if (timer >= USED_TIMER_COUNT) return; TIM_IC_InitTypeDef TIM_ICInitStructure; @@ -579,7 +579,7 @@ volatile timCCR_t* timerChCCR(const timerHardware_t *timHw) void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh) { unsigned timer = lookupTimerIndex(timHw->tim); - if(timer >= USED_TIMER_COUNT) + if (timer >= USED_TIMER_COUNT) return; TIM_OC_InitTypeDef TIM_OCInitStructure; @@ -593,7 +593,7 @@ void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHig HAL_TIM_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel); - if(outEnable) { + if (outEnable) { TIM_OCInitStructure.OCMode = TIM_OCMODE_INACTIVE; HAL_TIM_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel); HAL_TIM_OC_Start(&timerHandle[timer].Handle, timHw->channel); @@ -610,17 +610,17 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) unsigned tim_status; tim_status = tim->SR & tim->DIER; #if 1 - while(tim_status) { + while (tim_status) { // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway) unsigned bit = __builtin_clz(tim_status); unsigned mask = ~(0x80000000 >> bit); tim->SR = mask; tim_status &= mask; - switch(bit) { + switch (bit) { case __builtin_clz(TIM_IT_UPDATE): { - if(timerConfig->forcedOverflowTimerValue != 0){ + if (timerConfig->forcedOverflowTimerValue != 0) { capture = timerConfig->forcedOverflowTimerValue - 1; timerConfig->forcedOverflowTimerValue = 0; } else { @@ -628,7 +628,7 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) } timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; - while(cb) { + while (cb) { cb->fn(cb, capture); cb = cb->next; } @@ -653,7 +653,7 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) tim->SR = ~TIM_IT_Update; capture = tim->ARR; timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; - while(cb) { + while (cb) { cb->fn(cb, capture); cb = cb->next; } @@ -812,10 +812,10 @@ void timerInit(void) #endif // initialize timer channel structures - for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { timerChannelInfo[i].type = TYPE_FREE; } - for(int i = 0; i < USED_TIMER_COUNT; i++) { + for (int i = 0; i < USED_TIMER_COUNT; i++) { timerInfo[i].priority = ~0; } } @@ -826,18 +826,18 @@ void timerInit(void) void timerStart(void) { #if 0 - for(unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) { + for (unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) { int priority = -1; int irq = -1; - for(unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++) - if((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) { + for (unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++) + if ((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) { // TODO - move IRQ to timer info irq = timerHardware[hwc].irq; } // TODO - aggregate required timer paramaters configTimeBase(usedTimers[timer], 0, 1); TIM_Cmd(usedTimers[timer], ENABLE); - if(priority >= 0) { // maybe none of the channels was configured + if (priority >= 0) { // maybe none of the channels was configured NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = irq; @@ -913,4 +913,4 @@ uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz) return 0; } return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1; -} \ No newline at end of file +} diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 5f8fd63c3..7fa5c315c 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -44,12 +44,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { #endif }; -uint8_t timerClockDivisor(TIM_TypeDef *tim) -{ - UNUSED(tim); - return 1; -} - uint32_t timerClock(TIM_TypeDef *tim) { UNUSED(tim); diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index ddc140285..6b44e7487 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -36,12 +36,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { { .TIMx = TIM17, .rcc = RCC_APB2(TIM17), .inputIrq = TIM1_TRG_COM_TIM17_IRQn }, }; -uint8_t timerClockDivisor(TIM_TypeDef *tim) -{ - UNUSED(tim); - return 1; -} - uint32_t timerClock(TIM_TypeDef *tim) { UNUSED(tim); diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index 93786cd39..7c17447c6 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -80,26 +80,18 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { 7 TIM8_CH1 TIM8_CH2 TIM8_CH3 TIM8_CH4 */ -uint8_t timerClockDivisor(TIM_TypeDef *tim) -{ -#if defined (STM32F40_41xxx) - if (tim == TIM8) return 1; -#endif - if (tim == TIM1 || tim == TIM9 || tim == TIM10 || tim == TIM11) { - return 1; - } else { - return 2; - } -} - uint32_t timerClock(TIM_TypeDef *tim) { -#if defined (STM32F40_41xxx) - if (tim == TIM8) return SystemCoreClock; -#endif - if (tim == TIM1 || tim == TIM9 || tim == TIM10 || tim == TIM11) { +#if defined (STM32F411xE) + UNUSED(tim); + return SystemCoreClock; +#elif defined (STM32F40_41xxx) + if (tim == TIM8 || tim == TIM1 || tim == TIM9 || tim == TIM10 || tim == TIM11) { return SystemCoreClock; } else { return SystemCoreClock / 2; } +#else + #error "No timer clock defined correctly for MCU" +#endif } diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c index 5059d1bbc..1decd01c3 100644 --- a/src/main/drivers/timer_stm32f7xx.c +++ b/src/main/drivers/timer_stm32f7xx.c @@ -75,11 +75,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { 6 TIM1_CH1 TIM1_CH2 TIM1_CH1 TIM1_CH4 TIM1_CH3 7 TIM8_CH1 TIM8_CH2 TIM8_CH3 TIM8_CH4 */ -uint8_t timerClockDivisor(TIM_TypeDef *tim) -{ - UNUSED(tim); - return 1; -} uint32_t timerClock(TIM_TypeDef *tim) { diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 1b26e691a..d0d81e075 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -188,7 +188,7 @@ void transponderIrWaitForTransmitComplete(void) { static uint32_t waitCounter = 0; - while(transponderIrDataTransferInProgress) { + while (transponderIrDataTransferInProgress) { waitCounter++; } } diff --git a/src/main/drivers/transponder_ir_arcitimer.c b/src/main/drivers/transponder_ir_arcitimer.c index 2ec8214df..e8b9a7e22 100644 --- a/src/main/drivers/transponder_ir_arcitimer.c +++ b/src/main/drivers/transponder_ir_arcitimer.c @@ -60,7 +60,7 @@ void updateTransponderDMABufferArcitimer(transponder_t *transponder, const uint8 const struct transponderVTable arcitimerTansponderVTable = { - updateTransponderDMABufferArcitimer, + updateTransponderDMABufferArcitimer, }; #endif diff --git a/src/main/drivers/transponder_ir_erlt.c b/src/main/drivers/transponder_ir_erlt.c index 6b948fb94..86e85e117 100644 --- a/src/main/drivers/transponder_ir_erlt.c +++ b/src/main/drivers/transponder_ir_erlt.c @@ -55,7 +55,7 @@ void updateTransponderDMABufferERLT(transponder_t *transponder, const uint8_t* t addBitToBuffer(transponder, ERLTCyclesForZeroBit, ERLTBitQuiet); //add data bits, only the 6 LSB - for(int i = 5; i >= 0; i--) + for (int i = 5; i >= 0; i--) { uint8_t bv = (byteToSend >> i) & 0x01; paritysum += bv; diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 2136549f5..fff7fcab6 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -67,7 +67,7 @@ void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel) if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) return; - + if (vtxDevice->vTable->setBandAndChannel) vtxDevice->vTable->setBandAndChannel(band, channel); } @@ -80,7 +80,7 @@ void vtxCommonSetPowerByIndex(uint8_t index) if (index > vtxDevice->capability.powerCount) return; - + if (vtxDevice->vTable->setPowerByIndex) vtxDevice->vTable->setPowerByIndex(index); } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 29358ccb8..ad97c4ba2 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -190,13 +190,6 @@ static const char * const *sensorHardwareNames[] = { }; #endif // USE_SENSOR_NAMES -#ifndef MINIMAL_CLI -static const char *armingDisableFlagNames[] = { - "NOGYRO", "FAILSAFE", "BOXFAILSAFE", "THROTTLE", - "ANGLE", "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST" -}; -#endif - static void cliPrint(const char *str) { while (*str) { @@ -333,7 +326,7 @@ static void printValuePointer(const clivalue_t *var, const void *valuePointer, b break; } - switch(var->type & VALUE_MODE_MASK) { + switch (var->type & VALUE_MODE_MASK) { case MODE_DIRECT: cliPrintf("%d", value); if (full) { @@ -812,7 +805,7 @@ static void cliSerial(char *cmdline) break; } - switch(i) { + switch (i) { case 0: if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_1000000) { continue; @@ -866,7 +859,7 @@ static void cliSerialPassthrough(char *cmdline) int index = 0; while (tok != NULL) { - switch(index) { + switch (index) { case 0: id = atoi(tok); break; @@ -1332,7 +1325,7 @@ static void cliModeColor(char *cmdline) int modeIdx = args[MODE]; int funIdx = args[FUNCTION]; int color = args[COLOR]; - if(!setModeColor(modeIdx, funIdx, color)) { + if (!setModeColor(modeIdx, funIdx, color)) { cliShowParseError(); return; } @@ -2023,7 +2016,7 @@ static void cliBeeper(char *cmdline) if (len == 0) { cliPrintf("Disabled:"); for (int32_t i = 0; ; i++) { - if (i == beeperCount - 2){ + if (i == beeperCount - 2) { if (mask == 0) cliPrint(" none"); break; @@ -2090,7 +2083,7 @@ static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfi char buf[16]; char bufDefault[16]; uint32_t i; - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { + for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) { buf[rxConfig->rcmap[i]] = rcChannelLetters[i]; if (defaultRxConfig) { bufDefault[defaultRxConfig->rcmap[i]] = rcChannelLetters[i]; @@ -2133,7 +2126,7 @@ static void cliMap(char *cmdline) static char *checkCommand(char *cmdLine, const char *command) { - if(!strncasecmp(cmdLine, command, strlen(command)) // command names match + if (!strncasecmp(cmdLine, command, strlen(command)) // command names match && (isspace((unsigned)cmdLine[strlen(command)]) || cmdLine[strlen(command)] == 0)) { return cmdLine + strlen(command) + 1; } else { @@ -2238,7 +2231,7 @@ static void cliDshotProg(char *cmdline) break; default: - motorControlEnable = false; + pwmDisableMotors(); int command = atoi(pch); if (command >= 0 && command < DSHOT_MIN_THROTTLE) { @@ -2266,7 +2259,7 @@ static void cliDshotProg(char *cmdline) pch = strtok_r(NULL, " ", &saveptr); } - motorControlEnable = true; + pwmEnableMotors(); } #endif @@ -2287,13 +2280,13 @@ static void cliEscPassthrough(char *cmdline) while (pch != NULL) { switch (pos) { case 0: - if(strncasecmp(pch, "sk", strlen(pch)) == 0) { + if (strncasecmp(pch, "sk", strlen(pch)) == 0) { mode = PROTOCOL_SIMONK; - } else if(strncasecmp(pch, "bl", strlen(pch)) == 0) { + } else if (strncasecmp(pch, "bl", strlen(pch)) == 0) { mode = PROTOCOL_BLHELI; - } else if(strncasecmp(pch, "ki", strlen(pch)) == 0) { + } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) { mode = PROTOCOL_KISS; - } else if(strncasecmp(pch, "cc", strlen(pch)) == 0) { + } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) { mode = PROTOCOL_KISSALL; } else { cliShowParseError(); @@ -3474,7 +3467,7 @@ void cliProcess(void) break; } } - if(cmd < cmdTable + ARRAYLEN(cmdTable)) + if (cmd < cmdTable + ARRAYLEN(cmdTable)) cmd->func(options); else cliPrint("Unknown command, try 'help'"); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index b4d3d6822..8fc0ce935 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -120,6 +120,16 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); #ifndef USE_OSD_SLAVE +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) +PG_RESET_TEMPLATE(systemConfig_t, systemConfig, + .pidProfileIndex = 0, + .activeRateProfile = 0, + .debug_mode = DEBUG_MODE, + .task_statistics = true, + .cpu_overclock = false, + .name = { 0 } // FIXME misplaced, see PG_PILOT_CONFIG in CF v1.x +); +#else PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .pidProfileIndex = 0, .activeRateProfile = 0, @@ -128,6 +138,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = { 0 } // FIXME misplaced, see PG_PILOT_CONFIG in CF v1.x ); #endif +#endif #ifdef USE_OSD_SLAVE PG_RESET_TEMPLATE(systemConfig_t, systemConfig, @@ -340,7 +351,7 @@ void validateAndFixConfig(void) #endif #ifndef USE_OSD_SLAVE - if((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){ + if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { motorConfigMutable()->mincommand = 1000; } @@ -456,7 +467,7 @@ void validateAndFixGyroConfig(void) // check for looptime restrictions based on motor protocol. Motor times have safety margin const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom; float motorUpdateRestriction; - switch(motorConfig()->dev.motorPwmProtocol) { + switch (motorConfig()->dev.motorPwmProtocol) { case (PWM_TYPE_STANDARD): motorUpdateRestriction = 1.0f/BRUSHLESS_MOTORS_PWM_RATE; break; @@ -487,7 +498,7 @@ void validateAndFixGyroConfig(void) if (motorConfig()->dev.useUnsyncedPwm && (motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD) { uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); - if(motorConfig()->dev.motorPwmRate > maxEscRate) + if (motorConfig()->dev.motorPwmRate > maxEscRate) motorConfigMutable()->dev.motorPwmRate = maxEscRate; } } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index a1b96a08a..8a27a4e25 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -72,6 +72,9 @@ typedef struct systemConfig_s { uint8_t activeRateProfile; uint8_t debug_mode; uint8_t task_statistics; +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + uint8_t cpu_overclock; +#endif char name[MAX_NAME_LENGTH + 1]; // FIXME misplaced, see PG_PILOT_CONFIG in CF v1.x } systemConfig_t; #endif diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3aa78b71d..754abfca1 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -17,6 +17,7 @@ #include #include +#include #include "platform.h" @@ -35,6 +36,7 @@ #include "drivers/gyro_sync.h" #include "drivers/light_led.h" +#include "drivers/system.h" #include "drivers/time.h" #include "drivers/transponder_ir.h" @@ -106,12 +108,11 @@ int16_t magHold; int16_t headFreeModeHold; -uint8_t motorControlEnable = false; static bool reverseMotors = false; static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; -static bool armingCalibrationWasInitialised; +static int lastArmingDisabledReason = 0; PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0); @@ -141,6 +142,11 @@ static bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } +void resetArmingDisabled(void) +{ + lastArmingDisabledReason = 0; +} + void updateArmingStatus(void) { if (ARMING_FLAG(ARMED)) { @@ -188,8 +194,6 @@ void updateArmingStatus(void) void disarm(void) { - armingCalibrationWasInitialised = false; - if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); @@ -205,12 +209,8 @@ void disarm(void) void tryArm(void) { - static bool firstArmingCalibrationWasCompleted; - - if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { - gyroStartCalibration(); - armingCalibrationWasInitialised = true; - firstArmingCalibrationWasCompleted = true; + if (armingConfig()->gyro_cal_on_first_arm) { + gyroStartCalibration(true); } updateArmingStatus(); @@ -233,7 +233,7 @@ void tryArm(void) pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); } } - } + } #endif ENABLE_ARMING_FLAG(ARMED); @@ -242,6 +242,8 @@ void tryArm(void) disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero + lastArmingDisabledReason = 0; + //beep to indicate arming #ifdef GPS if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { @@ -252,12 +254,15 @@ void tryArm(void) #else beeper(BEEPER_ARMING); #endif + } else { + if (!isFirstArmingGyroCalibrationRunning()) { + int armingDisabledReason = ffs(getArmingDisableFlags()); + if (lastArmingDisabledReason != armingDisabledReason) { + lastArmingDisabledReason = armingDisabledReason; - return; - } - - if (!ARMING_FLAG(ARMED)) { - beeperConfirmationBeeps(1); + beeperWarningBeeps(armingDisabledReason); + } + } } } @@ -612,7 +617,7 @@ static void subTaskMotorUpdate(void) startTime = micros(); } - mixTable(currentPidProfile); + mixTable(currentPidProfile->vbatPidCompensation); #ifdef USE_SERVOS // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos. @@ -621,9 +626,8 @@ static void subTaskMotorUpdate(void) } #endif - if (motorControlEnable) { - writeMotors(); - } + writeMotors(); + DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime); } @@ -643,7 +647,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) static uint8_t pidUpdateCountdown; #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC) - if(lockMainPID() != 0) return; + if (lockMainPID() != 0) return; #endif if (debugMode == DEBUG_CYCLETIME) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 8bc8b4253..085ce3d75 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -27,8 +27,6 @@ extern int16_t magHold; extern bool isRXDataNew; extern int16_t headFreeModeHold; -extern uint8_t motorControlEnable; - typedef struct throttleCorrectionConfig_s { uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. @@ -40,6 +38,8 @@ union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); void handleInflightCalibrationStickPosition(); +void resetArmingDisabled(void); + void disarm(void); void tryArm(void); diff --git a/src/main/fc/fc_dispatch.c b/src/main/fc/fc_dispatch.c index 1e50135dd..61d19d2ad 100644 --- a/src/main/fc/fc_dispatch.c +++ b/src/main/fc/fc_dispatch.c @@ -42,8 +42,8 @@ void dispatchEnable(void) void dispatchProcess(uint32_t currentTime) { - for(dispatchEntry_t **p = &head; *p; ) { - if(cmp32(currentTime, (*p)->delayedUntil) < 0) + for (dispatchEntry_t **p = &head; *p; ) { + if (cmp32(currentTime, (*p)->delayedUntil) < 0) break; // unlink entry first, so handler can replan self dispatchEntry_t *current = *p; @@ -56,7 +56,7 @@ void dispatchAdd(dispatchEntry_t *entry, int delayUs) { uint32_t delayedUntil = micros() + delayUs; dispatchEntry_t **p = &head; - while(*p && cmp32((*p)->delayedUntil, delayedUntil) < 0) + while (*p && cmp32((*p)->delayedUntil, delayedUntil) < 0) p = &(*p)->next; entry->next = *p; *p = entry; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index c76f936b2..0ea310861 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -137,8 +137,6 @@ void targetPreInit(void); #endif -extern uint8_t motorControlEnable; - #ifdef SOFTSERIAL_LOOPBACK serialPort_t *loopbackPort; #endif @@ -266,6 +264,20 @@ void init(void) ensureEEPROMContainsValidData(); readEEPROM(); +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + // If F4 Overclocking is set and System core clock is not correct a reset is forced + if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } + +#endif + systemState |= SYSTEM_STATE_CONFIG_LOADED; //i2cSetOverclock(masterConfig.i2c_overclock); @@ -441,10 +453,10 @@ void init(void) #endif #ifdef USE_I2C_DEVICE_3 i2cInit(I2CDEV_3); -#endif +#endif #ifdef USE_I2C_DEVICE_4 i2cInit(I2CDEV_4); -#endif +#endif #endif /* USE_I2C */ #endif /* TARGET_BUS_INIT */ @@ -481,7 +493,7 @@ void init(void) if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, notify and don't arm. - failureLedCode(FAILURE_MISSING_ACC, 2); + indicateFailure(FAILURE_MISSING_ACC, 2); setArmingDisabled(ARMING_DISABLED_NO_GYRO); } @@ -632,7 +644,7 @@ void init(void) if (mixerConfig()->mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroStartCalibration(); + gyroStartCalibration(false); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif @@ -695,7 +707,7 @@ void init(void) // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); - motorControlEnable = true; + pwmEnableMotors(); #ifdef USE_OSD_SLAVE osdSlaveTasksInit(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 97a562c85..6e1af8e5d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -214,7 +214,7 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr escPortIndex = sbufReadU8(src); } - switch(escMode) { + switch (escMode) { case ESC_4WAY: // get channel number // switch all motor lines HI @@ -284,7 +284,7 @@ const box_t *findBoxByPermanentId(uint8_t permanentId) static bool activeBoxIdGet(boxId_e boxId) { - if(boxId > sizeof(activeBoxIds) * 8) + if (boxId > sizeof(activeBoxIds) * 8) return false; return bitArrayGet(&activeBoxIds, boxId); } @@ -328,7 +328,7 @@ void initActiveBoxIds(void) memset(&ena, 0, sizeof(ena)); // macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only -#define BME(boxId) do { bitArraySet(&ena, boxId); } while(0) +#define BME(boxId) do { bitArraySet(&ena, boxId); } while (0) BME(BOXARM); if (!feature(FEATURE_AIRMODE)) { @@ -840,17 +840,32 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce #ifdef OSD // OSD specific, not applicable to OSD slaves. + + // Configuration sbufWriteU8(dst, osdConfig()->units); + + // Alarms sbufWriteU8(dst, osdConfig()->rssi_alarm); sbufWriteU16(dst, osdConfig()->cap_alarm); - sbufWriteU16(dst, osdConfig()->time_alarm); + sbufWriteU16(dst, 0); sbufWriteU16(dst, osdConfig()->alt_alarm); + + // Element position and visibility for (int i = 0; i < OSD_ITEM_COUNT; i++) { sbufWriteU16(dst, osdConfig()->item_pos[i]); } + + // Post flight statistics + sbufWriteU8(dst, OSD_STAT_COUNT); for (int i = 0; i < OSD_STAT_COUNT; i++ ) { sbufWriteU8(dst, osdConfig()->enabled_stats[i]); } + + // Timers + sbufWriteU8(dst, OSD_TIMER_COUNT); + for (int i = 0; i < OSD_TIMER_COUNT; i++) { + sbufWriteU16(dst, osdConfig()->timers[i]); + } #endif break; } @@ -1185,7 +1200,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_RX_MAP: - sbufWriteData(dst, rxConfig()->rcmap, MAX_MAPPABLE_RX_INPUTS); + sbufWriteData(dst, rxConfig()->rcmap, RX_MAPPABLE_CHANNEL_COUNT); break; case MSP_BF_CONFIG: @@ -1940,7 +1955,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_RX_MAP: - for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { + for (int i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) { rxConfigMutable()->rcmap[i] = sbufReadU8(src); } break; @@ -2154,11 +2169,23 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif #if defined(OSD) osdConfigMutable()->units = sbufReadU8(src); + + // Alarms osdConfigMutable()->rssi_alarm = sbufReadU8(src); osdConfigMutable()->cap_alarm = sbufReadU16(src); - osdConfigMutable()->time_alarm = sbufReadU16(src); + sbufReadU16(src); // Skip unused (previously fly timer) osdConfigMutable()->alt_alarm = sbufReadU16(src); #endif + } else if ((int8_t)addr == -2) { +#if defined(OSD) + // Timers + uint8_t index = sbufReadU8(src); + if (index > OSD_TIMER_COUNT) { + return MSP_RESULT_ERROR; + } + osdConfigMutable()->timers[index] = sbufReadU16(src); +#endif + return MSP_RESULT_ERROR; } else { #if defined(OSD) const uint16_t value = sbufReadU16(src); @@ -2172,6 +2199,8 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } else if (addr < OSD_ITEM_COUNT) { /* Set element positions */ osdConfigMutable()->item_pos[addr] = value; + } else { + return MSP_RESULT_ERROR; } #else return MSP_RESULT_ERROR; diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index afc1f45b7..ee4da972d 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -139,7 +139,7 @@ static void scaleRcCommandToFpvCamAngle(void) { static float cosFactor = 1.0; static float sinFactor = 0.0; - if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){ + if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees) { lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees; cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD); sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD); @@ -167,7 +167,7 @@ static void scaleRcCommandToFpvCamAngle(void) { const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; - if(ABS(rcCommandSpeed) > throttleVelocityThreshold) + if (ABS(rcCommandSpeed) > throttleVelocityThreshold) pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); else pidSetItermAccelerator(1.0f); @@ -193,7 +193,7 @@ void processRcCommand(void) if (rxConfig()->rcInterpolation) { // Set RC refresh rate for sampling and channels to filter - switch(rxConfig()->rcInterpolation) { + switch (rxConfig()->rcInterpolation) { case(RC_SMOOTHING_AUTO): rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps break; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 3ad6b0486..beeeabe14 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -221,7 +221,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t beeperConfirmationBeeps(delta > 0 ? 2 : 1); int newValue; - switch(adjustmentFunction) { + switch (adjustmentFunction) { case ADJUSTMENT_RC_RATE: newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c controlRateConfig->rcRate8 = newValue; @@ -339,7 +339,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) { uint8_t beeps = 0; - switch(adjustmentFunction) { + switch (adjustmentFunction) { case ADJUSTMENT_RATE_PROFILE: { if (getCurrentControlRateProfileIndex() != position) { @@ -352,7 +352,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) case ADJUSTMENT_HORIZON_STRENGTH: { uint8_t newValue = constrain(position, 0, 200); // FIXME magic numbers repeated in serial_cli.c - if(pidProfile->pid[PID_LEVEL].D != newValue) { + if (pidProfile->pid[PID_LEVEL].D != newValue) { beeps = ((newValue - pidProfile->pid[PID_LEVEL].D) / 8) + 1; pidProfile->pid[PID_LEVEL].D = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HORIZON_STRENGTH, position); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index f90445703..4cc665034 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -147,6 +147,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) tryArm(); } else { // Disarming via ARM BOX + resetArmingDisabled(); if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { rcDisarmTicks++; @@ -187,7 +188,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - gyroStartCalibration(); + gyroStartCalibration(false); #ifdef GPS if (feature(FEATURE_GPS)) { @@ -232,6 +233,8 @@ void processRcStickPositions(throttleStatus_e throttleStatus) tryArm(); return; + } else { + resetArmingDisabled(); } } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 17bd29105..14b8325d4 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -29,6 +29,13 @@ uint16_t flightModeFlags = 0; static uint32_t enabledSensors = 0; +#if defined(OSD) || !defined(MINIMAL_CLI) +const char *armingDisableFlagNames[]= { + "NOGYRO", "FAILSAFE", "BOXFAILSAFE", "THROTTLE", + "ANGLE", "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST" +}; +#endif + static armingDisableFlags_e armingDisableFlags = 0; void setArmingDisabled(armingDisableFlags_e flag) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index a54f287bd..6fdf3f211 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -47,6 +47,11 @@ typedef enum { ARMING_DISABLED_BST = (1 << 10), } armingDisableFlags_e; +#define NUM_ARMING_DISABLE_FLAGS 11 +#if defined(OSD) || !defined(MINIMAL_CLI) +extern const char *armingDisableFlagNames[NUM_ARMING_DISABLE_FLAGS]; +#endif + void setArmingDisabled(armingDisableFlags_e flag); void unsetArmingDisabled(armingDisableFlags_e flag); bool isArmingDisabled(void); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8f3a028ec..b2742f3e8 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -634,13 +634,15 @@ const clivalue_t valueTable[] = { { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_alarm) }, { "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 20000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, cap_alarm) }, - { "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 60 }, PG_OSD_CONFIG, offsetof(osdConfig_t, time_alarm) }, { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 10000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, alt_alarm) }, + { "osd_tim1", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, (uint16_t)0xFFFF }, PG_OSD_CONFIG, offsetof(osdConfig_t, timers[OSD_TIMER_1]) }, + { "osd_tim2", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, (uint16_t)0xFFFF }, PG_OSD_CONFIG, offsetof(osdConfig_t, timers[OSD_TIMER_2]) }, + { "osd_vbat_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAIN_BATT_VOLTAGE]) }, { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_RSSI_VALUE]) }, - { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_FLYTIME]) }, - { "osd_ontimer_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ONTIME]) }, + { "osd_tim_1_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ITEM_TIMER_1]) }, + { "osd_tim_2_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ITEM_TIMER_2]) }, { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_FLYMODE]) }, { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_THROTTLE_POS]) }, { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_VTX_CHANNEL]) }, @@ -668,7 +670,6 @@ const clivalue_t valueTable[] = { { "osd_pit_ang_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PITCH_ANGLE]) }, { "osd_rol_ang_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_ANGLE]) }, { "osd_battery_usage_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAIN_BATT_USAGE]) }, - { "osd_arm_time_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ARMED_TIME]) }, { "osd_disarmed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DISARMED]) }, { "osd_nheading_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_HEADING]) }, { "osd_nvario_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_VARIO]) }, @@ -684,9 +685,9 @@ const clivalue_t valueTable[] = { { "osd_stat_max_alt", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_MAX_ALTITUDE])}, { "osd_stat_bbox", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_BLACKBOX])}, { "osd_stat_endbatt", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_END_BATTERY])}, - { "osd_stat_flytime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_FLYTIME])}, - { "osd_stat_armtime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_ARMEDTIME])}, { "osd_stat_bb_no", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_BLACKBOX_NUMBER])}, + { "osd_stat_tim_1", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_TIMER_1])}, + { "osd_stat_tim_2", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_TIMER_2])}, #endif // PG_SYSTEM_CONFIG @@ -694,6 +695,9 @@ const clivalue_t valueTable[] = { { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) }, #endif { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) }, +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + { "cpu_overclock", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, cpu_overclock) }, +#endif // PG_VTX_CONFIG #ifdef VTX_RTC6705 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index efa4b853c..d00e0c078 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -317,7 +317,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, } // Compute and apply integral feedback if enabled - if(imuRuntimeConfig.dcm_ki > 0.0f) { + if (imuRuntimeConfig.dcm_ki > 0.0f) { // Stop integrating if spinning beyond the certain limit if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) { const float dcmKiGain = imuRuntimeConfig.dcm_ki; @@ -427,16 +427,16 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) #endif #if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) - UNUSED(imuMahonyAHRSupdate); - UNUSED(useAcc); - UNUSED(useMag); - UNUSED(useYaw); - UNUSED(rawYawError); + UNUSED(imuMahonyAHRSupdate); + UNUSED(useAcc); + UNUSED(useMag); + UNUSED(useYaw); + UNUSED(rawYawError); #else #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC) -// printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real()); - deltaT = imuDeltaT; +// printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real()); + deltaT = imuDeltaT; #endif imuMahonyAHRSupdate(deltaT * 1e-6f, @@ -455,7 +455,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) { IMU_LOCK; #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC) - if(imuUpdated == false){ + if (imuUpdated == false) { IMU_UNLOCK; return; } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 18f4689fc..fc5f5531f 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -339,7 +339,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate) // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // DSHOT scaling is done to the actual dshot range void initEscEndpoints(void) { - switch(motorConfig()->dev.motorPwmProtocol) { + switch (motorConfig()->dev.motorPwmProtocol) { #ifdef USE_DSHOT case PWM_TYPE_PROSHOT1000: case PWM_TYPE_DSHOT1200: @@ -352,7 +352,7 @@ void initEscEndpoints(void) { else motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); motorOutputHigh = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; break; @@ -469,9 +469,8 @@ void writeMotors(void) for (int i = 0; i < motorCount; i++) { pwmWriteMotor(i, motor[i]); } + pwmCompleteMotorUpdate(motorCount); } - - pwmCompleteMotorUpdate(motorCount); } static void writeAllMotors(int16_t mc) @@ -497,39 +496,37 @@ void stopPwmAllMotors(void) delayMicroseconds(1500); } -void mixTable(pidProfile_t *pidProfile) -{ - // Scale roll/pitch/yaw uniformly to fit within throttle range - // Initial mixer concept by bdoiron74 reused and optimized for Air Mode - float throttle = 0, currentThrottleInputRange = 0; - float motorOutputMin, motorOutputMax; - static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions - bool mixerInversion = false; +float throttle = 0; +float motorOutputMin, motorOutputMax; +bool mixerInversion = false; +float motorOutputRange; - // Find min and max throttle based on condition. - if (feature(FEATURE_3D)) { +void calculateThrottleAndCurrentMotorEndpoints(void) +{ + static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions + float currentThrottleInputRange = 0; + + if(feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. - if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling + if((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { motorOutputMax = deadbandMotor3dLow; motorOutputMin = motorOutputLow; - throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange3dLow; if(isMotorProtocolDshot()) mixerInversion = true; - } else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling + } else if(rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { motorOutputMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; throttlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive + } else if((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { motorOutputMax = deadbandMotor3dLow; motorOutputMin = motorOutputLow; throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; currentThrottleInputRange = rcCommandThrottleRange3dLow; if(isMotorProtocolDshot()) mixerInversion = true; - } else { // Deadband handling from positive to negative + } else { motorOutputMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; throttle = 0; @@ -543,61 +540,10 @@ void mixTable(pidProfile_t *pidProfile) } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); - const float motorOutputRange = motorOutputMax - motorOutputMin; - - // Calculate and Limit the PIDsum - const float scaledAxisPidRoll = - constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit); - const float scaledAxisPidPitch = - constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit); - const float scaledAxisPidYaw = - constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, -pidSumLimitYaw, pidSumLimitYaw); - - // Calculate voltage compensation - const float vbatCompensationFactor = (pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; - - // Find roll/pitch/yaw desired output - float motorMix[MAX_SUPPORTED_MOTORS]; - float motorMixMax = 0, motorMixMin = 0; - const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed); - int motorDirection = GET_DIRECTION(isMotorsReversed()); - - - for (int i = 0; i < motorCount; i++) { - float mix = - scaledAxisPidRoll * currentMixer[i].roll * (motorDirection) + - scaledAxisPidPitch * currentMixer[i].pitch * (motorDirection) + - scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection) * (motorDirection); - - if (vbatCompensationFactor > 1.0f) { - mix *= vbatCompensationFactor; // Add voltage compensation - } - - if (mix > motorMixMax) { - motorMixMax = mix; - } else if (mix < motorMixMin) { - motorMixMin = mix; - } - motorMix[i] = mix; - } - - motorMixRange = motorMixMax - motorMixMin; - - if (motorMixRange > 1.0f) { - for (int i = 0; i < motorCount; i++) { - motorMix[i] /= motorMixRange; - } - // Get the maximum correction by setting offset to center when airmode enabled - if (isAirmodeActive()) { - throttle = 0.5f; - } - } else { - if (isAirmodeActive() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle - const float throttleLimitOffset = motorMixRange / 2.0f; - throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); - } - } + motorOutputRange = motorOutputMax - motorOutputMin; +} +void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (uint32_t i = 0; i < motorCount; i++) { @@ -635,6 +581,68 @@ void mixTable(pidProfile_t *pidProfile) } } +void mixTable(uint8_t vbatPidCompensation) +{ + // Find min and max throttle based on conditions. Throttle has to be known before mixing + calculateThrottleAndCurrentMotorEndpoints(); + + float motorMix[MAX_SUPPORTED_MOTORS]; + + // Calculate and Limit the PIDsum + const float scaledAxisPidRoll = + constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit); + const float scaledAxisPidPitch = + constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit); + const float scaledAxisPidYaw = + constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, -pidSumLimitYaw, pidSumLimitYaw); + + // Calculate voltage compensation + const float vbatCompensationFactor = (vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; + + // Find roll/pitch/yaw desired output + float motorMixMax = 0, motorMixMin = 0; + const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed); + int motorDirection = GET_DIRECTION(isMotorsReversed()); + + for (int i = 0; i < motorCount; i++) { + float mix = + scaledAxisPidRoll * currentMixer[i].roll * (motorDirection) + + scaledAxisPidPitch * currentMixer[i].pitch * (motorDirection) + + scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection) * (motorDirection); + + if (vbatCompensationFactor > 1.0f) { + mix *= vbatCompensationFactor; // Add voltage compensation + } + + if (mix > motorMixMax) { + motorMixMax = mix; + } else if (mix < motorMixMin) { + motorMixMin = mix; + } + motorMix[i] = mix; + } + + motorMixRange = motorMixMax - motorMixMin; + + if (motorMixRange > 1.0f) { + for (int i = 0; i < motorCount; i++) { + motorMix[i] /= motorMixRange; + } + // Get the maximum correction by setting offset to center when airmode enabled + if (isAirmodeActive()) { + throttle = 0.5f; + } + } else { + if (isAirmodeActive() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle + const float throttleLimitOffset = motorMixRange / 2.0f; + throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); + } + } + + // Apply the mix to motor endpoints + applyMixToMotors(motorMix); +} + float convertExternalToMotor(uint16_t externalValue) { uint16_t motorValue = externalValue; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 64535fd7c..2480da3c6 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -116,7 +116,7 @@ void pidInitMixer(const struct pidProfile_s *pidProfile); void mixerConfigureOutput(void); void mixerResetDisarmedMotors(void); -void mixTable(struct pidProfile_s *pidProfile); +void mixTable(uint8_t vbatPidCompensation); void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 46411982e..b1929cc5e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -243,7 +243,7 @@ static float crashDtermThreshold; static float crashGyroThreshold; void pidInitConfig(const pidProfile_t *pidProfile) { - for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { Kp[axis] = PTERM_SCALE * pidProfile->pid[axis].P; Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I; Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D; @@ -359,7 +359,7 @@ static float accelerationLimit(int axis, float currentPidSetpoint) { static float previousSetpoint[3]; const float currentVelocity = currentPidSetpoint- previousSetpoint[axis]; - if(ABS(currentVelocity) > maxVelocity[axis]) + if (ABS(currentVelocity) > maxVelocity[axis]) currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis]; previousSetpoint[axis] = currentPidSetpoint; @@ -383,7 +383,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { float currentPidSetpoint = getSetpointRate(axis); - if(maxVelocity[axis]) + if (maxVelocity[axis]) currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); // Yaw control is GYRO based, direct sticks control is applied to rate PID diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index d3c926929..8d6617002 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -28,6 +28,7 @@ #include "fat_standard.h" #include "drivers/sdcard.h" +#include "common/maths.h" #ifdef AFATFS_DEBUG #define ONLY_EXPOSE_FOR_TESTING @@ -92,9 +93,6 @@ #define AFATFS_INTROSPEC_LOG_FILENAME "ASYNCFAT.LOG" -#define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define MAX(a, b) ((a) > (b) ? (a) : (b)) - typedef enum { AFATFS_SAVE_DIRECTORY_NORMAL, AFATFS_SAVE_DIRECTORY_FOR_CLOSE, diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 7b3a9d6e7..5d7afdca9 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -78,7 +78,7 @@ PG_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig, #define BEEPER_NAMES #endif -#define MAX_MULTI_BEEPS 20 //size limit for 'beep_multiBeeps[]' +#define MAX_MULTI_BEEPS 64 //size limit for 'beep_multiBeeps[]' #define BEEPER_COMMAND_REPEAT 0xFE #define BEEPER_COMMAND_STOP 0xFF @@ -151,11 +151,15 @@ static const uint8_t beep_gyroCalibrated[] = { }; // array used for variable # of beeps (reporting GPS sat count, etc) -static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2]; +static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 1]; #define BEEPER_CONFIRMATION_BEEP_DURATION 2 #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20 +#define BEEPER_WARNING_BEEP_1_DURATION 20 +#define BEEPER_WARNING_BEEP_2_DURATION 5 +#define BEEPER_WARNING_BEEP_GAP_DURATION 10 + // Beeper off = 0 Beeper on = 1 static uint8_t beeperIsOn = 0; @@ -270,25 +274,43 @@ void beeperSilence(void) currentBeeperEntry = NULL; } + /* * Emits the given number of 20ms beeps (with 200ms spacing). * This function returns immediately (does not block). */ void beeperConfirmationBeeps(uint8_t beepCount) { - int i; - int cLimit; - - i = 0; - cLimit = beepCount * 2; - if(cLimit > MAX_MULTI_BEEPS) - cLimit = MAX_MULTI_BEEPS; //stay within array size + uint32_t i = 0; + uint32_t cLimit = beepCount * 2; + if (cLimit > MAX_MULTI_BEEPS) { + cLimit = MAX_MULTI_BEEPS; + } do { - beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_DURATION; // 20ms beep - beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_GAP_DURATION; // 200ms pause + beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_DURATION; + beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_GAP_DURATION; } while (i < cLimit); - beep_multiBeeps[i] = BEEPER_COMMAND_STOP; //sequence end - beeper(BEEPER_MULTI_BEEPS); //initiate sequence + beep_multiBeeps[i] = BEEPER_COMMAND_STOP; + + beeper(BEEPER_MULTI_BEEPS); +} + +void beeperWarningBeeps(uint8_t beepCount) +{ + uint32_t i = 0; + uint32_t cLimit = beepCount * 4; + if (cLimit >= MAX_MULTI_BEEPS) { + cLimit = MAX_MULTI_BEEPS; + } + do { + beep_multiBeeps[i++] = BEEPER_WARNING_BEEP_1_DURATION; + beep_multiBeeps[i++] = BEEPER_WARNING_BEEP_GAP_DURATION; + beep_multiBeeps[i++] = BEEPER_WARNING_BEEP_2_DURATION; + beep_multiBeeps[i++] = BEEPER_WARNING_BEEP_GAP_DURATION; + } while (i < cLimit); + beep_multiBeeps[i] = BEEPER_COMMAND_STOP; + + beeper(BEEPER_MULTI_BEEPS); } #ifdef GPS @@ -302,7 +324,7 @@ void beeperGpsStatus(void) beep_multiBeeps[i++] = 10; } while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2); - beep_multiBeeps[i-1] = 50; // extend last pause + beep_multiBeeps[i - 1] = 50; // extend last pause beep_multiBeeps[i] = BEEPER_COMMAND_STOP; beeper(BEEPER_MULTI_BEEPS); //initiate sequence @@ -341,7 +363,7 @@ void beeperUpdate(timeUs_t currentTimeUs) } #ifdef USE_DSHOT - if (!ARMING_FLAG(ARMED) && currentBeeperEntry->mode == BEEPER_RX_SET) { + if (!ARMING_FLAG(ARMED) && currentBeeperEntry->mode == BEEPER_RX_SET) { for (unsigned index = 0; index < getMotorCount(); index++) { pwmWriteDshotCommand(index, DSHOT_CMD_BEEP3); } @@ -444,6 +466,7 @@ bool isBeeperOn(void) void beeper(beeperMode_e mode) {UNUSED(mode);} void beeperSilence(void) {} void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} +void beeperWarningBeeps(uint8_t beepCount) {UNUSED(beepCount);} void beeperUpdate(timeUs_t currentTimeUs) {UNUSED(currentTimeUs);} uint32_t getArmingBeepTimeMicros(void) {return 0;} beeperMode_e beeperModeForTableIndex(int idx) {UNUSED(idx); return BEEPER_SILENCE;} diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 8a9c0430e..c48c8b4ea 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -61,6 +61,7 @@ void beeper(beeperMode_e mode); void beeperSilence(void); void beeperUpdate(timeUs_t currentTimeUs); void beeperConfirmationBeeps(uint8_t beepCount); +void beeperWarningBeeps(uint8_t beepCount); uint32_t getArmingBeepTimeMicros(void); beeperMode_e beeperModeForTableIndex(int idx); const char *beeperNameForTableIndex(int idx); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 46b086be6..102b21fbe 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -118,14 +118,14 @@ static const uint8_t ubloxInit[] = { 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // capturing the data from the U-Center binary console. 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xC2, - + //Preprocessor Airborne_1g Dynamic Platform Model Option #elif defined(GPS_UBLOX_MODE_AIRBORNE_1G) 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, // CFG-NAV5 - Set engine settings 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Airborne with 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // <1g acceleration and capturing the data from the U-Center binary console. 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1A, 0x28, - + //Default Airborne_4g Dynamic Platform Model #else 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, // CFG-NAV5 - Set engine settings @@ -133,7 +133,7 @@ static const uint8_t ubloxInit[] = { 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // <4g acceleration and capturing the data from the U-Center binary console. 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1C, 0x6C, #endif - + // DISABLE NMEA messages 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // VGS: Course over ground and Ground speed 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, // GSV: GNSS Satellites in View @@ -273,7 +273,7 @@ void gpsInitNmea(void) #if defined(COLIBRI_RACE) || defined(LUX_RACE) uint32_t now; #endif - switch(gpsData.state) { + switch (gpsData.state) { case GPS_INITIALIZING: #if defined(COLIBRI_RACE) || defined(LUX_RACE) now = millis(); @@ -361,7 +361,7 @@ void gpsInitUblox(void) case GPS_CONFIGURE: // Either use specific config file for GPS or let dynamically upload config - if( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) { + if ( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) { gpsSetState(GPS_RECEIVING_DATA); break; } @@ -628,7 +628,7 @@ static bool gpsNewFrameNMEA(char c) switch (gps_frame) { case FRAME_GGA: //************* GPGGA FRAME parsing - switch(param) { + switch (param) { // case 1: // Time information // break; case 2: @@ -661,7 +661,7 @@ static bool gpsNewFrameNMEA(char c) } break; case FRAME_RMC: //************* GPRMC FRAME parsing - switch(param) { + switch (param) { case 7: gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis break; @@ -671,7 +671,7 @@ static bool gpsNewFrameNMEA(char c) } break; case FRAME_GSV: - switch(param) { + switch (param) { /*case 1: // Total number of messages of this type in this cycle break; */ @@ -684,17 +684,17 @@ static bool gpsNewFrameNMEA(char c) GPS_numCh = grab_fields(string, 0); break; } - if(param < 4) + if (param < 4) break; svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4 svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite - if(svSatNum > GPS_SV_MAXSATS) + if (svSatNum > GPS_SV_MAXSATS) break; - switch(svSatParam) { + switch (svSatParam) { case 1: // SV PRN number GPS_svinfo_chn[svSatNum - 1] = svSatNum; @@ -980,7 +980,7 @@ static bool UBLOX_parse_gps(void) GPS_numCh = _buffer.svinfo.numCh; if (GPS_numCh > 16) GPS_numCh = 16; - for (i = 0; i < GPS_numCh; i++){ + for (i = 0; i < GPS_numCh; i++) { GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn; GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid; GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality; @@ -1103,7 +1103,7 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) waitForSerialPortToFinishTransmitting(gpsPort); waitForSerialPortToFinishTransmitting(gpsPassthroughPort); - if(!(gpsPort->mode & MODE_TX)) + if (!(gpsPort->mode & MODE_TX)) serialSetMode(gpsPort, gpsPort->mode | MODE_TX); #ifdef USE_DASHBOARD diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 4bd3b4eb6..69e4332f6 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -446,52 +446,52 @@ static void applyLedFixedLayers() for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex]; hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); - hsvColor_t nextColor = *getSC(LED_SCOLOR_BACKGROUND); //next color above the one selected, or color 0 if your are at the maximum - hsvColor_t previousColor = *getSC(LED_SCOLOR_BACKGROUND); //Previous color to the one selected, modulo color count + hsvColor_t nextColor = *getSC(LED_SCOLOR_BACKGROUND); //next color above the one selected, or color 0 if your are at the maximum + hsvColor_t previousColor = *getSC(LED_SCOLOR_BACKGROUND); //Previous color to the one selected, modulo color count int fn = ledGetFunction(ledConfig); int hOffset = HSV_HUE_MAX; switch (fn) { - case LED_FUNCTION_COLOR: - color = ledStripConfig()->colors[ledGetColor(ledConfig)]; - nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; - previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; - break; + case LED_FUNCTION_COLOR: + color = ledStripConfig()->colors[ledGetColor(ledConfig)]; + nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; + previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; + break; - case LED_FUNCTION_FLIGHT_MODE: - for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) - if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { - const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]); - if (directionalColor) { - color = *directionalColor; - } - - break; // stop on first match + case LED_FUNCTION_FLIGHT_MODE: + for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) + if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { + const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]); + if (directionalColor) { + color = *directionalColor; } - break; - case LED_FUNCTION_ARM_STATE: - color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED); - break; + break; // stop on first match + } + break; - case LED_FUNCTION_BATTERY: - color = HSV(RED); - hOffset += scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120); - break; + case LED_FUNCTION_ARM_STATE: + color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED); + break; - case LED_FUNCTION_RSSI: - color = HSV(RED); - hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120); - break; + case LED_FUNCTION_BATTERY: + color = HSV(RED); + hOffset += scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120); + break; - default: - break; + case LED_FUNCTION_RSSI: + color = HSV(RED); + hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120); + break; + + default: + break; } if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor - { - int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2; + { + int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2; if (auxInput < centerPWM) { color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h); @@ -600,35 +600,35 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer) // check if last vtx values have changed. check = pit + (power << 1) + (band << 4) + (channel << 8); - if(!showSettings && check != lastCheck) { + if (!showSettings && check != lastCheck) { // display settings for 3 seconds. showSettings = 15; } lastCheck = check; // quick way to check if any settings changed. - if(showSettings) { + if (showSettings) { showSettings--; } blink = !blink; *timer += HZ_TO_US(5); // check 5 times a second } - if(!active) { // no vtx device detected + if (!active) { // no vtx device detected return; } hsvColor_t color = {0, 0, 0}; - if(showSettings) { // show settings + if (showSettings) { // show settings uint8_t vtxLedCount = 0; for (int i = 0; i < ledCounts.count && vtxLedCount < 6; ++i) { const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; if (ledGetOverlayBit(ledConfig, LED_OVERLAY_VTX)) { - if(vtxLedCount == 0) { + if (vtxLedCount == 0) { color.h = HSV(GREEN).h; color.s = HSV(GREEN).s; color.v = blink ? 15 : 0; // blink received settings } - else if(vtxLedCount > 0 && power >= vtxLedCount && !pit) { // show power + else if (vtxLedCount > 0 && power >= vtxLedCount && !pit) { // show power color.h = HSV(ORANGE).h; color.s = HSV(ORANGE).s; color.v = blink ? 15 : 0; // blink received settings @@ -949,7 +949,7 @@ static void applyLedAnimationLayer(bool updateNow, timeUs_t *timer) { static uint8_t frameCounter = 0; const int animationFrames = ledGridRows; - if(updateNow) { + if (updateNow) { frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; *timer += HZ_TO_US(20); } @@ -1087,7 +1087,7 @@ bool parseColor(int index, const char *colorConfig) }; for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { int val = atoi(remainingCharacters); - if(val > hsv_limit[componentIndex]) { + if (val > hsv_limit[componentIndex]) { result = false; break; } @@ -1128,7 +1128,7 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT) return false; if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough - if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) + if (modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) return false; ledStripConfigMutable()->modeColors[modeIndex].color[modeColorIndex] = colorIndex; } else if (modeIndex == LED_SPECIAL) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index afb3ea9d7..f5bc83c01 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -89,6 +89,12 @@ #define VIDEO_BUFFER_CHARS_PAL 480 +const char * const osdTimerSourceNames[] = { + "ON TIME ", + "TOTAL ARM", + "LAST ARM " +}; + // Blink control static bool blinkState = true; @@ -106,7 +112,7 @@ static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32]; #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) -static uint16_t flyTime = 0; +static timeUs_t flyTime = 0; static uint8_t statRssi; typedef struct statistic_s { @@ -116,7 +122,7 @@ typedef struct statistic_s { int16_t min_rssi; int16_t max_altitude; int16_t max_distance; - uint16_t armed_time; + timeUs_t armed_time; } statistic_t; static statistic_t stats; @@ -128,7 +134,6 @@ static uint8_t armState; static displayPort_t *osdDisplayPort; - #define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees #define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees #define AH_SIDEBAR_WIDTH_POS 7 @@ -178,7 +183,7 @@ static int osdGetBatteryAverageCellVoltage(void) static char osdGetBatterySymbol(int cellVoltage) { - if(getBatteryState() == BATTERY_CRITICAL) { + if (getBatteryState() == BATTERY_CRITICAL) { return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark } else { /* Calculate a symbol offset using cell voltage over full cell voltage range */ @@ -227,6 +232,65 @@ static uint8_t osdGetDirectionSymbolFromHeading(int heading) return SYM_ARROW_SOUTH + heading; } +static char osdGetTimerSymbol(osd_timer_source_e src) +{ + switch (src) { + case OSD_TIMER_SRC_ON: + return SYM_ON_M; + case OSD_TIMER_SRC_TOTAL_ARMED: + case OSD_TIMER_SRC_LAST_ARMED: + return SYM_FLY_M; + default: + return ' '; + } +} + +static timeUs_t osdGetTimerValue(osd_timer_source_e src) +{ + switch (src) { + case OSD_TIMER_SRC_ON: + return micros(); + case OSD_TIMER_SRC_TOTAL_ARMED: + return flyTime; + case OSD_TIMER_SRC_LAST_ARMED: + return stats.armed_time; + default: + return 0; + } +} + +STATIC_UNIT_TESTED void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time) +{ + int seconds = time / 1000000; + const int minutes = seconds / 60; + seconds = seconds % 60; + + switch (precision) { + case OSD_TIMER_PREC_SECOND: + default: + tfp_sprintf(buff, "%02d:%02d", minutes, seconds); + break; + case OSD_TIMER_PREC_HUNDREDTHS: + { + const int hundredths = (time / 10000) % 100; + tfp_sprintf(buff, "%02d:%02d.%02d", minutes, seconds, hundredths); + break; + } + } +} + +STATIC_UNIT_TESTED void osdFormatTimer(char *buff, bool showSymbol, int timerIndex) +{ + const uint16_t timer = osdConfig()->timers[timerIndex]; + const uint8_t src = OSD_TIMER_SRC(timer); + + if (showSymbol) { + *(buff++) = osdGetTimerSymbol(src); + } + + osdFormatTime(buff, OSD_TIMER_PRECISION(timer), osdGetTimerValue(src)); +} + static void osdDrawSingleElement(uint8_t item) { if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) { @@ -236,7 +300,7 @@ static void osdDrawSingleElement(uint8_t item) uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]); uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]); uint8_t elemOffsetX = 0; - char buff[32]; + char buff[OSD_ELEMENT_BUFFER_LENGTH]; switch (item) { case OSD_RSSI_VALUE: @@ -351,24 +415,14 @@ static void osdDrawSingleElement(uint8_t item) break; } - case OSD_ONTIME: + case OSD_ITEM_TIMER_1: + case OSD_ITEM_TIMER_2: { - const uint32_t seconds = micros() / 1000000; - buff[0] = SYM_ON_M; - tfp_sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60); + const int timer = item - OSD_ITEM_TIMER_1; + osdFormatTimer(buff, true, timer); break; } - case OSD_FLYTIME: - buff[0] = SYM_FLY_M; - tfp_sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60); - break; - - case OSD_ARMED_TIME: - buff[0] = SYM_FLY_M; - tfp_sprintf(buff + 1, "%02d:%02d", stats.armed_time / 60, stats.armed_time % 60); - break; - case OSD_FLYMODE: { char *p = "ACRO"; @@ -520,7 +574,20 @@ static void osdDrawSingleElement(uint8_t item) } case OSD_WARNINGS: - switch(getBatteryState()) { + /* Show common reason for arming being disabled */ + if (IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) { + const armingDisableFlags_e flags = getArmingDisableFlags(); + for (int i = 0; i < NUM_ARMING_DISABLE_FLAGS; i++) { + if (flags & (1 << i)) { + tfp_sprintf(buff, "%s", armingDisableFlagNames[i]); + break; + } + } + break; + } + + /* Show battery state warning */ + switch (getBatteryState()) { case BATTERY_WARNING: tfp_sprintf(buff, "LOW BATTERY"); break; @@ -530,6 +597,7 @@ static void osdDrawSingleElement(uint8_t item) break; default: + /* Show visual beeper if battery is OK */ if (showVisualBeeper) { tfp_sprintf(buff, " * * * *"); } else { @@ -573,7 +641,7 @@ static void osdDrawSingleElement(uint8_t item) //Create empty battery indicator bar buff[0] = SYM_PB_START; - for(uint8_t i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) { + for (uint8_t i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) { if (i <= mAhUsedProgress) buff[i] = SYM_PB_FULL; else @@ -645,8 +713,8 @@ static void osdDrawElements(void) osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); osdDrawSingleElement(OSD_RSSI_VALUE); osdDrawSingleElement(OSD_CROSSHAIRS); - osdDrawSingleElement(OSD_FLYTIME); - osdDrawSingleElement(OSD_ONTIME); + osdDrawSingleElement(OSD_ITEM_TIMER_1); + osdDrawSingleElement(OSD_ITEM_TIMER_2); osdDrawSingleElement(OSD_FLYMODE); osdDrawSingleElement(OSD_THROTTLE_POS); osdDrawSingleElement(OSD_VTX_CHANNEL); @@ -665,7 +733,6 @@ static void osdDrawElements(void) osdDrawSingleElement(OSD_PITCH_ANGLE); osdDrawSingleElement(OSD_ROLL_ANGLE); osdDrawSingleElement(OSD_MAIN_BATT_USAGE); - osdDrawSingleElement(OSD_ARMED_TIME); osdDrawSingleElement(OSD_DISARMED); osdDrawSingleElement(OSD_NUMERICAL_HEADING); osdDrawSingleElement(OSD_NUMERICAL_VARIO); @@ -697,8 +764,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[OSD_CROSSHAIRS] = OSD_POS(8, 6) | VISIBLE_FLAG; osdConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; osdConfig->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; - osdConfig->item_pos[OSD_ONTIME] = OSD_POS(22, 1) | VISIBLE_FLAG; - osdConfig->item_pos[OSD_FLYTIME] = OSD_POS(1, 1) | VISIBLE_FLAG; + osdConfig->item_pos[OSD_ITEM_TIMER_1] = OSD_POS(22, 1) | VISIBLE_FLAG; + osdConfig->item_pos[OSD_ITEM_TIMER_2] = OSD_POS(1, 1) | VISIBLE_FLAG; osdConfig->item_pos[OSD_FLYMODE] = OSD_POS(13, 10) | VISIBLE_FLAG; osdConfig->item_pos[OSD_CRAFT_NAME] = OSD_POS(10, 11) | VISIBLE_FLAG; osdConfig->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 7) | VISIBLE_FLAG; @@ -724,7 +791,6 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[OSD_HOME_DIR] = OSD_POS(14, 9) | VISIBLE_FLAG; osdConfig->item_pos[OSD_COMPASS_BAR] = OSD_POS(10, 8) | VISIBLE_FLAG; osdConfig->item_pos[OSD_MAIN_BATT_USAGE] = OSD_POS(8, 12) | VISIBLE_FLAG; - osdConfig->item_pos[OSD_ARMED_TIME] = OSD_POS(1, 2) | VISIBLE_FLAG; osdConfig->item_pos[OSD_DISARMED] = OSD_POS(10, 4) | VISIBLE_FLAG; osdConfig->item_pos[OSD_NUMERICAL_HEADING] = OSD_POS(23, 9) | VISIBLE_FLAG; osdConfig->item_pos[OSD_NUMERICAL_VARIO] = OSD_POS(23, 8) | VISIBLE_FLAG; @@ -739,16 +805,18 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false; osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true; osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false; - osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false; - osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true; osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false; osdConfig->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = true; + osdConfig->enabled_stats[OSD_STAT_TIMER_1] = false; + osdConfig->enabled_stats[OSD_STAT_TIMER_2] = true; osdConfig->units = OSD_UNIT_METRIC; + osdConfig->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10); + osdConfig->timers[OSD_TIMER_2] = OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10); + osdConfig->rssi_alarm = 20; osdConfig->cap_alarm = 2200; - osdConfig->time_alarm = 10; // in minutes osdConfig->alt_alarm = 100; // meters or feet depend on configuration } @@ -824,10 +892,15 @@ void osdUpdateAlarms(void) else CLR_BLINK(OSD_GPS_SATS); - if (flyTime / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) - SET_BLINK(OSD_FLYTIME); - else - CLR_BLINK(OSD_FLYTIME); + for (int i = 0; i < OSD_TIMER_COUNT; i++) { + const uint16_t timer = osdConfig()->timers[i]; + const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer)); + const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us + if (alarmTime != 0 && time >= alarmTime) + SET_BLINK(OSD_ITEM_TIMER_1 + i); + else + CLR_BLINK(OSD_ITEM_TIMER_1 + i); + } if (getMAhDrawn() >= osdConfig()->cap_alarm) { SET_BLINK(OSD_MAH_DRAWN); @@ -849,11 +922,12 @@ void osdResetAlarms(void) CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); CLR_BLINK(OSD_WARNINGS); CLR_BLINK(OSD_GPS_SATS); - CLR_BLINK(OSD_FLYTIME); CLR_BLINK(OSD_MAH_DRAWN); CLR_BLINK(OSD_ALTITUDE); CLR_BLINK(OSD_AVG_CELL_VOLTAGE); CLR_BLINK(OSD_MAIN_BATT_USAGE); + CLR_BLINK(OSD_ITEM_TIMER_1); + CLR_BLINK(OSD_ITEM_TIMER_2); } static void osdResetStats(void) @@ -954,14 +1028,14 @@ static void osdShowStats(void) displayClearScreen(osdDisplayPort); displayWrite(osdDisplayPort, 2, top++, " --- STATS ---"); - if (osdConfig()->enabled_stats[OSD_STAT_ARMEDTIME]) { - tfp_sprintf(buff, "%02d:%02d", stats.armed_time / 60, stats.armed_time % 60); - osdDisplayStatisticLabel(top++, "ARMED TIME", buff); + if (osdConfig()->enabled_stats[OSD_STAT_TIMER_1]) { + osdFormatTimer(buff, false, OSD_TIMER_1); + osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff); } - if (osdConfig()->enabled_stats[OSD_STAT_FLYTIME]) { - tfp_sprintf(buff, "%02d:%02d", flyTime / 60, flyTime % 60); - osdDisplayStatisticLabel(top++, "FLY TIME", buff); + if (osdConfig()->enabled_stats[OSD_STAT_TIMER_2]) { + osdFormatTimer(buff, false, OSD_TIMER_2); + osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff); } if (osdConfig()->enabled_stats[OSD_STAT_MAX_SPEED] && STATE(GPS_FIX)) { @@ -1033,8 +1107,7 @@ static void osdShowArmed(void) STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs) { - static uint8_t lastSec = 0; - uint8_t sec; + static timeUs_t lastTimeUs = 0; // detect arm/disarm if (armState != ARMING_FLAG(ARMED)) { @@ -1054,13 +1127,12 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs) osdUpdateStats(); - sec = currentTimeUs / 1000000; - - if (ARMING_FLAG(ARMED) && sec != lastSec) { - flyTime++; - stats.armed_time++; - lastSec = sec; + if (ARMING_FLAG(ARMED)) { + timeUs_t deltaT = currentTimeUs - lastTimeUs; + flyTime += deltaT; + stats.armed_time += deltaT; } + lastTimeUs = currentTimeUs; if (resumeRefreshAt) { if (cmp32(currentTimeUs, resumeRefreshAt) < 0) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b6cb82dc8..7f9ae5434 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -21,27 +21,38 @@ #include "common/time.h" #include "config/parameter_group.h" +#define OSD_NUM_TIMER_TYPES 3 +extern const char * const osdTimerSourceNames[OSD_NUM_TIMER_TYPES]; + +#define OSD_ELEMENT_BUFFER_LENGTH 32 + #define VISIBLE_FLAG 0x0800 #define VISIBLE(x) (x & VISIBLE_FLAG) #define OSD_POS_MAX 0x3FF #define OSD_POSCFG_MAX (VISIBLE_FLAG|0x3FF) // For CLI values // Character coordinate - #define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31 #define OSD_POSITION_XY_MASK ((1 << OSD_POSITION_BITS) - 1) #define OSD_POS(x,y) ((x & OSD_POSITION_XY_MASK) | ((y & OSD_POSITION_XY_MASK) << OSD_POSITION_BITS)) #define OSD_X(x) (x & OSD_POSITION_XY_MASK) #define OSD_Y(x) ((x >> OSD_POSITION_BITS) & OSD_POSITION_XY_MASK) +// Timer configuration +// Stored as 15[alarm:8][precision:4][source:4]0 +#define OSD_TIMER(src, prec, alarm) ((src & 0x0F) | ((prec & 0x0F) << 4) | ((alarm & 0xFF ) << 8)) +#define OSD_TIMER_SRC(timer) (timer & 0x0F) +#define OSD_TIMER_PRECISION(timer) ((timer >> 4) & 0x0F) +#define OSD_TIMER_ALARM(timer) ((timer >> 8) & 0xFF) + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, OSD_CROSSHAIRS, OSD_ARTIFICIAL_HORIZON, OSD_HORIZON_SIDEBARS, - OSD_ONTIME, - OSD_FLYTIME, + OSD_ITEM_TIMER_1, + OSD_ITEM_TIMER_2, OSD_FLYMODE, OSD_CRAFT_NAME, OSD_THROTTLE_POS, @@ -64,7 +75,6 @@ typedef enum { OSD_PITCH_ANGLE, OSD_ROLL_ANGLE, OSD_MAIN_BATT_USAGE, - OSD_ARMED_TIME, OSD_DISARMED, OSD_HOME_DIR, OSD_HOME_DIST, @@ -85,8 +95,8 @@ typedef enum { OSD_STAT_MAX_ALTITUDE, OSD_STAT_BLACKBOX, OSD_STAT_END_BATTERY, - OSD_STAT_FLYTIME, - OSD_STAT_ARMEDTIME, + OSD_STAT_TIMER_1, + OSD_STAT_TIMER_2, OSD_STAT_MAX_DISTANCE, OSD_STAT_BLACKBOX_NUMBER, OSD_STAT_COUNT // MUST BE LAST @@ -97,6 +107,25 @@ typedef enum { OSD_UNIT_METRIC } osd_unit_e; +typedef enum { + OSD_TIMER_1, + OSD_TIMER_2, + OSD_TIMER_COUNT +} osd_timer_e; + +typedef enum { + OSD_TIMER_SRC_ON, + OSD_TIMER_SRC_TOTAL_ARMED, + OSD_TIMER_SRC_LAST_ARMED, + OSD_TIMER_SRC_COUNT +} osd_timer_source_e; + +typedef enum { + OSD_TIMER_PREC_SECOND, + OSD_TIMER_PREC_HUNDREDTHS, + OSD_TIMER_PREC_COUNT +} osd_timer_precision_e; + typedef struct osdConfig_s { uint16_t item_pos[OSD_ITEM_COUNT]; bool enabled_stats[OSD_STAT_COUNT]; @@ -104,10 +133,11 @@ typedef struct osdConfig_s { // Alarms uint8_t rssi_alarm; uint16_t cap_alarm; - uint16_t time_alarm; uint16_t alt_alarm; osd_unit_e units; + + uint16_t timers[OSD_TIMER_COUNT]; } osdConfig_t; extern uint32_t resumeRefreshAt; diff --git a/src/main/io/rcsplit.c b/src/main/io/rcsplit.c index 774d40aa6..bf6b8ac64 100644 --- a/src/main/io/rcsplit.c +++ b/src/main/io/rcsplit.c @@ -48,7 +48,7 @@ static uint8_t crc_high_first(uint8_t *ptr, uint8_t len) uint8_t i; uint8_t crc=0x00; while (len--) { - crc ^= *ptr++; + crc ^= *ptr++; for (i=8; i>0; --i) { if (crc & 0x80) crc = (crc << 1) ^ 0x31; @@ -111,7 +111,7 @@ static void rcSplitProcessMode() argument = RCSPLIT_CTRL_ARGU_INVALID; break; } - + if (argument != RCSPLIT_CTRL_ARGU_INVALID) { sendCtrlCommand(argument); switchStates[switchIndex].isActivated = true; @@ -140,7 +140,7 @@ bool rcSplitInit(void) uint8_t switchIndex = i - BOXCAMERA1; switchStates[switchIndex].isActivated = true; } - + cameraState = RCSPLIT_STATE_IS_READY; #ifdef USE_RCSPLIT diff --git a/src/main/io/serial.c b/src/main/io/serial.c index ec303ca91..65c59c022 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -345,7 +345,7 @@ serialPort_t *openSerialPort( serialPort_t *serialPort = NULL; - switch(identifier) { + switch (identifier) { #ifdef USE_VCP case SERIAL_PORT_USB_VCP: serialPort = usbVcpOpen(); @@ -536,7 +536,7 @@ void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer * // Either port might be open in a mode other than MODE_RXTX. We rely on // serialRxBytesWaiting() to do the right thing for a TX only port. No // special handling is necessary OR performed. - while(1) { + while (1) { // TODO: maintain a timestamp of last data received. Use this to // implement a guard interval and check for `+++` as an escape sequence // to return to CLI command mode. diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index fdb028629..ebdb0d21d 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -310,7 +310,7 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { int i; crc = crc ^ ((uint16_t)data << 8); - for (i=0; i < 8; i++){ + for (i=0; i < 8; i++) { if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else @@ -432,7 +432,7 @@ void esc4wayProcess(serialPort_t *mspPort) #endif bool isExitScheduled = false; - while(1) { + while (1) { // restart looking for new sequence from host do { CRC_in.word = 0; @@ -460,7 +460,7 @@ void esc4wayProcess(serialPort_t *mspPort) CRC_check.bytes[1] = ReadByte(); CRC_check.bytes[0] = ReadByte(); - if(CRC_check.word == CRC_in.word) { + if (CRC_check.word == CRC_in.word) { ACK_OUT = ACK_OK; } else { ACK_OUT = ACK_I_INVALID_CRC; @@ -475,12 +475,12 @@ void esc4wayProcess(serialPort_t *mspPort) ioMem.D_PTR_I = ParamBuf; - switch(CMD) { + switch (CMD) { // ******* Interface related stuff ******* case cmd_InterfaceTestAlive: { - if (isMcuConnected()){ - switch(CurrentInterfaceMode) + if (isMcuConnected()) { + switch (CurrentInterfaceMode) { #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER case imATM_BLB: @@ -599,7 +599,7 @@ void esc4wayProcess(serialPort_t *mspPort) } O_PARAM_LEN = DeviceInfoSize; //4 O_PARAM = (uint8_t *)&DeviceInfo; - if(Connect(&DeviceInfo)) { + if (Connect(&DeviceInfo)) { DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; } else { SET_DISCONNECTED; @@ -611,7 +611,7 @@ void esc4wayProcess(serialPort_t *mspPort) #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER case cmd_DeviceEraseAll: { - switch(CurrentInterfaceMode) + switch (CurrentInterfaceMode) { case imSK: { @@ -661,14 +661,14 @@ void esc4wayProcess(serialPort_t *mspPort) wtf.D_FLASH_ADDR_L=Adress_L; wtf.D_PTR_I = BUF_I; */ - switch(CurrentInterfaceMode) + switch (CurrentInterfaceMode) { #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER case imSIL_BLB: case imATM_BLB: case imARM_BLB: { - if(!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) + if (!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) { ACK_OUT = ACK_D_GENERAL_ERROR; } @@ -678,7 +678,7 @@ void esc4wayProcess(serialPort_t *mspPort) #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER case imSK: { - if(!Stk_ReadFlash(&ioMem)) + if (!Stk_ReadFlash(&ioMem)) { ACK_OUT = ACK_D_GENERAL_ERROR; } @@ -729,7 +729,7 @@ void esc4wayProcess(serialPort_t *mspPort) default: ACK_OUT = ACK_I_INVALID_CMD; } - if(ACK_OUT == ACK_OK) + if (ACK_OUT == ACK_OK) { O_PARAM_LEN = ioMem.D_NUM_BYTES; O_PARAM = (uint8_t *)&ParamBuf; diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 47142dc6e..3b711a1c5 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -86,7 +86,7 @@ static uint8_t suart_getc_(uint8_t *bt) uint16_t bitmask = 0; uint8_t bit = 0; while (micros() < btime); - while(1) { + while (1) { if (ESC_IS_HI) { bitmask |= (1 << bit); @@ -109,8 +109,8 @@ static void suart_putc_(uint8_t *tx_b) // shift out stopbit first uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); uint32_t btime = micros(); - while(1) { - if(bitmask & 1) { + while (1) { + if (bitmask & 1) { ESC_SET_HI; // 1 } else { @@ -148,22 +148,22 @@ static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) LastCRC_16.word = 0; uint8_t LastACK = brNONE; do { - if(!suart_getc_(pstring)) goto timeout; + if (!suart_getc_(pstring)) goto timeout; ByteCrc(pstring); pstring++; len--; - } while(len > 0); + } while (len > 0); - if(isMcuConnected()) { + if (isMcuConnected()) { //With CRC read 3 more - if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; - if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; - if(!suart_getc_(&LastACK)) goto timeout; + if (!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; + if (!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; + if (!suart_getc_(&LastACK)) goto timeout; if (CRC_16.word != LastCRC_16.word) { LastACK = brERRORCRC; } } else { - if(!suart_getc_(&LastACK)) goto timeout; + if (!suart_getc_(&LastACK)) goto timeout; } timeout: return (LastACK == brSUCCESS); @@ -252,7 +252,7 @@ void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr { // skip if adr == 0xFFFF - if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; + if ((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L }; BL_SendBuf(sCMD, 4); return (BL_GetACK(2) == brSUCCESS); @@ -294,7 +294,7 @@ static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) { - if(interface_mode == imATM_BLB) { + if (interface_mode == imATM_BLB) { return BL_ReadA(CMD_READ_FLASH_ATM, pMem); } else { return BL_ReadA(CMD_READ_FLASH_SIL, pMem); @@ -311,7 +311,7 @@ uint8_t BL_PageErase(ioMem_t *pMem) if (BL_SendCMDSetAddress(pMem)) { uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; BL_SendBuf(sCMD, 2); - return (BL_GetACK((1000 / START_BIT_TIMEOUT_MS)) == brSUCCESS); + return (BL_GetACK((1400 / START_BIT_TIMEOUT_MS)) == brSUCCESS); } return 0; } @@ -323,7 +323,7 @@ uint8_t BL_WriteEEprom(ioMem_t *pMem) uint8_t BL_WriteFlash(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); + return BL_WriteA(CMD_PROG_FLASH, pMem, (500 / START_BIT_TIMEOUT_MS)); } uint8_t BL_VerifyFlash(ioMem_t *pMem) diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 8df63d3e6..8bd873960 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -176,9 +176,9 @@ static uint8_t StkReadLeader(void) // Wait for the first bit uint32_t waitcycl; //250uS each - if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { + if ((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { waitcycl = STK_WAITCYLCES_EXT; - } else if(StkCmd == CMD_SIGN_ON) { + } else if (StkCmd == CMD_SIGN_ON) { waitcycl = STK_WAITCYLCES_START; } else { waitcycl= STK_WAITCYLCES; @@ -189,7 +189,7 @@ static uint8_t StkReadLeader(void) } //Skip the first bits - if (waitcycl == 0){ + if (waitcycl == 0) { goto timeout; } @@ -271,7 +271,7 @@ static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem) { // ignore 0xFFFF // assume address is set before and we read or write the immediately following package - if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; + if ((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; StkCmd = CMD_LOAD_ADDRESS; StkSendPacketHeader(); StkSendByte(0); // hi byte Msg len diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index e952bd95f..622f656e7 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -226,7 +226,7 @@ void handleVTXControlButton(void) LED1_OFF; - switch(actionCounter) { + switch (actionCounter) { case 4: vtxCycleBandOrChannel(0, +1); break; diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 5b3743ce9..8298b8eb8 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -332,7 +332,7 @@ static void saProcessResponse(uint8_t *buf, int len) dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp)); } - switch(resp) { + switch (resp) { case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings if (len < 7) @@ -420,7 +420,7 @@ static void saReceiveFramer(uint8_t c) static int len; static int dlen; - switch(state) { + switch (state) { case S_WAITPRE1: if (c == 0xAA) { state = S_WAITPRE2; diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index c62d625c8..c00375cf8 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -134,7 +134,7 @@ void trampCmdU16(uint8_t cmd, uint16_t param) void trampSetFreq(uint16_t freq) { trampConfFreq = freq; - if(trampConfFreq != trampCurFreq) + if (trampConfFreq != trampCurFreq) trampFreqRetries = TRAMP_MAX_RETRIES; } @@ -151,7 +151,7 @@ void trampSetBandAndChannel(uint8_t band, uint8_t channel) void trampSetRFPower(uint16_t level) { trampConfPower = level; - if(trampConfPower != trampPower) + if (trampConfPower != trampPower) trampPowerRetries = TRAMP_MAX_RETRIES; } @@ -163,7 +163,7 @@ void trampSendRFPower(uint16_t level) // return false if error bool trampCommitChanges() { - if(trampStatus != TRAMP_STATUS_ONLINE) + if (trampStatus != TRAMP_STATUS_ONLINE) return false; trampStatus = TRAMP_STATUS_SET_FREQ_PW; @@ -184,7 +184,7 @@ char trampHandleResponse(void) case 'r': { uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - if(min_freq != 0) { + if (min_freq != 0) { trampRFFreqMin = min_freq; trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8); @@ -198,15 +198,15 @@ char trampHandleResponse(void) case 'v': { uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - if(freq != 0) { + if (freq != 0) { trampCurFreq = freq; trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampPitMode = trampRespBuffer[7]; trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); vtx58_Freq2Bandchan(trampCurFreq, &trampBand, &trampChannel); - if(trampConfFreq == 0) trampConfFreq = trampCurFreq; - if(trampConfPower == 0) trampConfPower = trampPower; + if (trampConfFreq == 0) trampConfFreq = trampCurFreq; + if (trampConfPower == 0) trampConfPower = trampPower; return 'v'; } @@ -217,7 +217,7 @@ char trampHandleResponse(void) case 's': { uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); - if(temp != 0) { + if (temp != 0) { trampTemperature = temp; return 's'; } @@ -263,7 +263,7 @@ static char trampReceive(uint32_t currentTimeUs) uint8_t c = serialRead(trampSerialPort); trampRespBuffer[trampReceivePos++] = c; - switch(trampReceiveState) { + switch (trampReceiveState) { case S_WAIT_LEN: if (c == 0x0F) { trampReceiveState = S_WAIT_CODE; @@ -339,7 +339,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) debug[0] = trampStatus; #endif - switch(replyCode) { + switch (replyCode) { case 'r': if (trampStatus <= TRAMP_STATUS_OFFLINE) trampStatus = TRAMP_STATUS_ONLINE; @@ -351,7 +351,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) break; } - switch(trampStatus) { + switch (trampStatus) { case TRAMP_STATUS_OFFLINE: case TRAMP_STATUS_ONLINE: @@ -361,7 +361,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) trampQueryR(); else { static unsigned int cnt = 0; - if(((cnt++) & 1) == 0) + if (((cnt++) & 1) == 0) trampQueryV(); else trampQueryS(); @@ -391,7 +391,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) done = false; } - if(!done) { + if (!done) { trampStatus = TRAMP_STATUS_CHECK_FREQ_PW; // delay next status query by 300ms @@ -560,8 +560,8 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) static void trampCmsInitSettings() { - if(trampBand > 0) trampCmsBand = trampBand; - if(trampChannel > 0) trampCmsChan = trampChannel; + if (trampBand > 0) trampCmsBand = trampBand; + if (trampChannel > 0) trampCmsChan = trampChannel; trampCmsUpdateFreqRef(); trampCmsPitMode = trampPitMode + 1; @@ -725,7 +725,7 @@ bool vtxTrampInit(void) #if defined(VTX_COMMON) vtxCommonRegisterDevice(&vtxTramp); #endif - + return true; } diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index fd7b9bfa4..7e05410cf 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -83,7 +83,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE; } else if (mspPort->c_state == MSP_HEADER_M) { mspPort->c_state = MSP_IDLE; - switch(c) { + switch (c) { case '<': // COMMAND mspPort->packetType = MSP_PACKET_COMMAND; mspPort->c_state = MSP_HEADER_ARROW; diff --git a/src/main/osd_slave/osd_slave_init.c b/src/main/osd_slave/osd_slave_init.c index b63566538..176408c6c 100644 --- a/src/main/osd_slave/osd_slave_init.c +++ b/src/main/osd_slave/osd_slave_init.c @@ -209,10 +209,10 @@ void init(void) #endif #ifdef USE_I2C_DEVICE_3 i2cInit(I2CDEV_3); -#endif +#endif #ifdef USE_I2C_DEVICE_4 i2cInit(I2CDEV_4); -#endif +#endif #endif /* USE_I2C */ #endif /* TARGET_BUS_INIT */ diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 1a00d6a8e..70b8a3331 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -231,7 +231,7 @@ uint16_t calcCRC16(uint8_t *pt, uint8_t msgLen) uint16_t crc16_data = 0; uint8_t data=0; - for (uint8_t mlen = 0; mlen < msgLen; mlen++){ + for (uint8_t mlen = 0; mlen < msgLen; mlen++) { data = pt[mlen] ^ ((uint8_t)(crc16_data) & (uint8_t)(0xFF)); data ^= data << 4; crc16_data = ((((uint16_t)data << 8) | ((crc16_data & 0xFF00) >> 8)) @@ -265,7 +265,7 @@ void jetiExBusDecodeChannelFrame(uint8_t *exBusFrame) uint8_t frameAddr; // Decode header - switch (((uint16_t)exBusFrame[EXBUS_HEADER_SYNC] << 8) | ((uint16_t)exBusFrame[EXBUS_HEADER_REQ])){ + switch (((uint16_t)exBusFrame[EXBUS_HEADER_SYNC] << 8) | ((uint16_t)exBusFrame[EXBUS_HEADER_REQ])) { case EXBUS_CHANNELDATA_DATA_REQUEST: // not yet specified case EXBUS_CHANNELDATA: @@ -322,7 +322,7 @@ static void jetiExBusDataReceive(uint16_t c) // Check if we shall start a frame? if (jetiExBusFramePosition == 0) { - switch(c){ + switch (c) { case EXBUS_START_CHANNEL_FRAME: jetiExBusFrameState = EXBUS_STATE_IN_PROGRESS; jetiExBusFrame = jetiExBusChannelFrame; @@ -345,12 +345,12 @@ static void jetiExBusDataReceive(uint16_t c) // Check the header for the message length if (jetiExBusFramePosition == EXBUS_HEADER_LEN) { - if((jetiExBusFrameState == EXBUS_STATE_IN_PROGRESS) && (jetiExBusFrame[EXBUS_HEADER_MSG_LEN] <= EXBUS_MAX_CHANNEL_FRAME_SIZE)) { + if ((jetiExBusFrameState == EXBUS_STATE_IN_PROGRESS) && (jetiExBusFrame[EXBUS_HEADER_MSG_LEN] <= EXBUS_MAX_CHANNEL_FRAME_SIZE)) { jetiExBusFrameLength = jetiExBusFrame[EXBUS_HEADER_MSG_LEN]; return; } - if((jetiExBusRequestState == EXBUS_STATE_IN_PROGRESS) && (jetiExBusFrame[EXBUS_HEADER_MSG_LEN] <= EXBUS_MAX_REQUEST_FRAME_SIZE)) { + if ((jetiExBusRequestState == EXBUS_STATE_IN_PROGRESS) && (jetiExBusFrame[EXBUS_HEADER_MSG_LEN] <= EXBUS_MAX_REQUEST_FRAME_SIZE)) { jetiExBusFrameLength = jetiExBusFrame[EXBUS_HEADER_MSG_LEN]; return; } @@ -381,7 +381,7 @@ static uint8_t jetiExBusFrameStatus() if (jetiExBusFrameState != EXBUS_STATE_RECEIVED) return RX_FRAME_PENDING; - if(calcCRC16(jetiExBusChannelFrame, jetiExBusChannelFrame[EXBUS_HEADER_MSG_LEN]) == 0) { + if (calcCRC16(jetiExBusChannelFrame, jetiExBusChannelFrame[EXBUS_HEADER_MSG_LEN]) == 0) { jetiExBusDecodeChannelFrame(jetiExBusChannelFrame); jetiExBusFrameState = EXBUS_STATE_ZERO; return RX_FRAME_COMPLETE; @@ -453,18 +453,18 @@ uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t itemStart) if ((item & 0x0F) == 0) item++; - if(item >= JETI_EX_SENSOR_COUNT) + if (item >= JETI_EX_SENSOR_COUNT) item = 1; exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID uint8_t *p = &exMessage[EXTEL_HEADER_ID]; - while(item <= (itemStart | 0x0F)) { + while (item <= (itemStart | 0x0F)) { *p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType; // Sensor ID (%16) | EX Data Type sensorValue = jetiExSensors[item].value; iCount = exDataTypeLen[jetiExSensors[item].exDataType]; - while(iCount > 1) { + while (iCount > 1) { *p++ = sensorValue; sensorValue = sensorValue >> 8; iCount--; @@ -472,9 +472,9 @@ uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t itemStart) *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; item++; - if(item > JETI_EX_SENSOR_COUNT) + if (item > JETI_EX_SENSOR_COUNT) break; - if(EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType]) + 1) + if (EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType]) + 1) break; } @@ -517,13 +517,13 @@ void handleJetiExBusTelemetry(void) // to prevent timing issues from request to answer - max. 4ms timeDiff = micros() - jetiTimeStampRequest; - if(timeDiff > 3000) { // include reserved time + if (timeDiff > 3000) { // include reserved time jetiExBusRequestState = EXBUS_STATE_ZERO; framesLost++; return; } - if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { + if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage(); jetiExSensors[EX_CURRENT].value = getAmperage(); jetiExSensors[EX_ALTITUDE].value = getEstimatedAltitude(); @@ -562,7 +562,7 @@ void sendJetiExBusTelemetry(uint8_t packetID) static uint8_t requestLoop = 0; uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; - if (requestLoop == 100){ //every nth request send the name of a value + if (requestLoop == 100) { //every nth request send the name of a value if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT ) sensorDescriptionCounter = 0; diff --git a/src/main/rx/nrf24_v202.c b/src/main/rx/nrf24_v202.c index 9de7865e8..f542b7dbe 100644 --- a/src/main/rx/nrf24_v202.c +++ b/src/main/rx/nrf24_v202.c @@ -153,7 +153,7 @@ static void decode_bind_packet(uint8_t *packet) // Returns whether the data was successfully decoded static rx_spi_received_e decode_packet(uint8_t *packet) { - if(bind_phase != PHASE_BOUND) { + if (bind_phase != PHASE_BOUND) { decode_bind_packet(packet); return RX_SPI_RECEIVED_BIND; } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 5ae38829e..aee1074a7 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -81,7 +81,7 @@ static uint32_t needRxSignalMaxDelayUs; static uint32_t suspendRxSignalUntil = 0; static uint8_t skipRxSamples = 0; -int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] +static int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; @@ -457,7 +457,7 @@ static uint16_t getRxfailValue(uint8_t channel) { const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel); - switch(channelFailsafeConfig->mode) { + switch (channelFailsafeConfig->mode) { case RX_FAILSAFE_MODE_AUTO: switch (channel) { case ROLL: @@ -516,7 +516,7 @@ static void readRxChannelsApplyRanges(void) const int channelCount = getRxChannelCount(); for (int channel = 0; channel < channelCount; channel++) { - const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); + const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, RX_MAPPABLE_CHANNEL_COUNT, channel); // sample the channel uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel); @@ -617,7 +617,7 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig) { for (const char *c = input; *c; c++) { const char *s = strchr(rcChannelLetters, *c); - if (s && (s < rcChannelLetters + MAX_MAPPABLE_RX_INPUTS)) { + if (s && (s < rcChannelLetters + RX_MAPPABLE_CHANNEL_COUNT)) { rxConfig->rcmap[s - rcChannelLetters] = c - input; } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 48772e0d1..2c8e21248 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -78,7 +78,7 @@ extern const char rcChannelLetters[]; extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -#define MAX_MAPPABLE_RX_INPUTS 8 +#define RX_MAPPABLE_CHANNEL_COUNT 8 #define RSSI_SCALE_MIN 1 #define RSSI_SCALE_MAX 255 @@ -115,7 +115,7 @@ typedef struct rxChannelRangeConfig_s { PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs); typedef struct rxConfig_s { - uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order + uint8_t rcmap[RX_MAPPABLE_CHANNEL_COUNT]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. uint8_t sbus_inversion; // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers. uint8_t halfDuplex; // allow rx to operate in half duplex mode on F4, ignored for F1 and F3. @@ -145,8 +145,6 @@ typedef struct rxConfig_s { PG_DECLARE(rxConfig_t, rxConfig); -#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) - struct rxRuntimeConfig_s; typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data typedef uint8_t (*rcFrameStatusFnPtr)(void); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index a3a176373..0119fca5b 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -126,11 +126,11 @@ static uint8_t spektrumFrameStatus(void) // This is the first frame status received. spek_fade_last_sec_count = fade; spek_fade_last_sec = current_secs; - } else if(spek_fade_last_sec != current_secs) { + } else if (spek_fade_last_sec != current_secs) { // If the difference is > 1, then we missed several seconds worth of frames and // should just throw out the fade calc (as it's likely a full signal loss). - if((current_secs - spek_fade_last_sec) == 1) { - if(rssi_channel != 0) { + if ((current_secs - spek_fade_last_sec) == 1) { + if (rssi_channel != 0) { if (spekHiRes) spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC)); else @@ -144,7 +144,7 @@ static uint8_t spektrumFrameStatus(void) for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) { const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) { - if(rssi_channel == 0 || spekChannel != rssi_channel) { + if (rssi_channel == 0 || spekChannel != rssi_channel) { spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; } } diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 10c164d15..d82d8d55e 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -225,7 +225,7 @@ retry: #ifdef ACC_MPU6500_ALIGN dev->accAlign = ACC_MPU6500_ALIGN; #endif - switch(dev->mpuDetectionResult.sensor) { + switch (dev->mpuDetectionResult.sensor) { case MPU_9250_SPI: accHardware = ACC_MPU9250; break; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 9b02c2ff4..e7d774ade 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -110,7 +110,7 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - switch(batteryConfig()->voltageMeterSource) { + switch (batteryConfig()->voltageMeterSource) { #ifdef USE_ESC_SENSOR case VOLTAGE_METER_ESC: if (feature(FEATURE_ESC_SENSOR)) { @@ -138,7 +138,7 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs) static void updateBatteryBeeperAlert(void) { - switch(getBatteryState()) { + switch (getBatteryState()) { case BATTERY_WARNING: beeper(BEEPER_BAT_LOW); @@ -158,12 +158,12 @@ void batteryUpdatePresence(void) static uint16_t previousVoltage = 0; bool isVoltageStable = ABS(voltageMeter.filtered - previousVoltage) <= VBAT_STABLE_MAX_DELTA; - + bool isVoltageFromBat = (voltageMeter.filtered >= batteryConfig()->vbatnotpresentcellvoltage //above ~0V && voltageMeter.filtered <= batteryConfig()->vbatmaxcellvoltage) //1s max cell voltage check || voltageMeter.filtered > batteryConfig()->vbatnotpresentcellvoltage*2; //USB voltage - 2s or more check - + if ( voltageState == BATTERY_NOT_PRESENT && isVoltageFromBat @@ -208,7 +208,7 @@ void batteryUpdatePresence(void) static void batteryUpdateVoltageState(void) { // alerts are currently used by beeper, osd and other subsystems - switch(voltageState) { + switch (voltageState) { case BATTERY_OK: if (voltageMeter.filtered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) { voltageState = BATTERY_WARNING; @@ -270,7 +270,7 @@ void batteryInit(void) batteryCriticalVoltage = 0; voltageMeterReset(&voltageMeter); - switch(batteryConfig()->voltageMeterSource) { + switch (batteryConfig()->voltageMeterSource) { case VOLTAGE_METER_ESC: #ifdef USE_ESC_SENSOR voltageMeterESCInit(); @@ -290,7 +290,7 @@ void batteryInit(void) // consumptionState = BATTERY_OK; currentMeterReset(¤tMeter); - switch(batteryConfig()->currentMeterSource) { + switch (batteryConfig()->currentMeterSource) { case CURRENT_METER_ADC: currentMeterADCInit(); break; @@ -345,7 +345,7 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced); ibatLastServiced = currentTimeUs; - switch(batteryConfig()->currentMeterSource) { + switch (batteryConfig()->currentMeterSource) { case CURRENT_METER_ADC: currentMeterADCRefresh(lastUpdateAt); currentMeterADCRead(¤tMeter); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 04f0fa89b..d710437f0 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -77,7 +77,7 @@ retry: dev->magAlign = ALIGN_DEFAULT; - switch(magHardwareToUse) { + switch (magHardwareToUse) { case MAG_DEFAULT: ; // fallthrough diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 7bf485559..75120fe4e 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -215,7 +215,7 @@ static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen) { uint8_t crc = 0; - for(int i=0; igyroAlign = ALIGN_DEFAULT; - switch(gyroHardware) { + switch (gyroHardware) { case GYRO_DEFAULT: #ifdef USE_GYRO_MPU6050 case GYRO_MPU6050: @@ -220,7 +222,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) #else if (mpu6500GyroDetect(dev)) { #endif - switch(dev->mpuDetectionResult.sensor) { + switch (dev->mpuDetectionResult.sensor) { case MPU_9250_SPI: gyroHardware = GYRO_MPU9250; break; @@ -489,9 +491,20 @@ static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor) gyroSensor->calibration.calibratingG = gyroCalculateCalibratingCycles(); } -void gyroStartCalibration(void) +void gyroStartCalibration(bool isFirstArmingCalibration) { - gyroSetCalibrationCycles(&gyroSensor0); + if (!(isFirstArmingCalibration && firstArmingCalibrationWasStarted)) { + gyroSetCalibrationCycles(&gyroSensor0); + + if (isFirstArmingCalibration) { + firstArmingCalibrationWasStarted = true; + } + } +} + +bool isFirstArmingGyroCalibrationRunning(void) +{ + return firstArmingCalibrationWasStarted && !isGyroCalibrationComplete(); } STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold) @@ -525,7 +538,9 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) { schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics - beeper(BEEPER_GYRO_CALIBRATED); + if (!firstArmingCalibrationWasStarted || !isArmingDisabled()) { + beeper(BEEPER_GYRO_CALIBRATED); + } } --gyroSensor->calibration.calibratingG; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index ba7693e58..ce29ea471 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -74,7 +74,8 @@ struct mpuConfiguration_s; const struct mpuConfiguration_s *gyroMpuConfiguration(void); struct mpuDetectionResult_s; const struct mpuDetectionResult_s *gyroMpuDetectionResult(void); -void gyroStartCalibration(void); +void gyroStartCalibration(bool isFirstArmingCalibration); +bool isFirstArmingGyroCalibrationRunning(void); bool isGyroCalibrationComplete(void); void gyroReadTemperature(void); int16_t gyroGetTemperature(void); diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 8f29c37bf..b680a131b 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -8,11 +8,11 @@ * * 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 + * 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 Cleanflight. If not, see . */ #include diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s index db7c6635f..801251b17 100644 --- a/src/main/startup/startup_stm32f40xx.s +++ b/src/main/startup/startup_stm32f40xx.s @@ -87,6 +87,14 @@ Reset_Handler: cmp r2, r1 // mj666 beq Reboot_Loader // mj666 + // Check for overclocking request + ldr r0, =0x2001FFF8 // Faduf + ldr r1, =0xBABEFACE // Faduf + ldr r2, [r0, #0] // Faduf + str r0, [r0, #0] // Faduf + cmp r2, r1 // Faduf + beq Boot_OC // Faduf + /* Copy the data segment initializers from flash to SRAM */ movs r1, #0 b LoopCopyDataInit @@ -135,7 +143,8 @@ LoopMarkHeapStack: str r1,[r0] /* Call the clock system intitialization function.*/ - bl SystemInit +/* Done in system_stm32f4xx.c */ + bl SystemInit /* Call the application's entry point.*/ bl main @@ -144,6 +153,63 @@ LoopMarkHeapStack: LoopForever: b LoopForever +Boot_OC: +/* Copy the data segment initializers from flash to SRAM */ + movs r1, #0 + b LoopCopyDataInitOC + +CopyDataInitOC: + ldr r3, =_sidata + ldr r3, [r3, r1] + str r3, [r0, r1] + adds r1, r1, #4 + +LoopCopyDataInitOC: + ldr r0, =_sdata + ldr r3, =_edata + adds r2, r0, r1 + cmp r2, r3 + bcc CopyDataInitOC + ldr r2, =_sbss + b LoopFillZerobssOC +/* Zero fill the bss segment. */ +FillZerobssOC: + movs r3, #0 + str r3, [r2], #4 + +LoopFillZerobssOC: + ldr r3, = _ebss + cmp r2, r3 + bcc FillZerobssOC + +/* Mark the heap and stack */ + ldr r2, =_heap_stack_begin + b LoopMarkHeapStackOC + +MarkHeapStackOC: + movs r3, 0xa5a5a5a5 + str r3, [r2], #4 + +LoopMarkHeapStackOC: + ldr r3, = _heap_stack_end + cmp r2, r3 + bcc MarkHeapStackOC + +/*FPU settings*/ + ldr r0, =0xE000ED88 /* Enable CP10,CP11 */ + ldr r1,[r0] + orr r1,r1,#(0xF << 20) + str r1,[r0] + +/* Call the clock system intitialization function.*/ +/* Done in system_stm32f4xx.c */ + bl SystemInitOC + +/* Call the application's entry point.*/ + bl main + bx lr + + Reboot_Loader: // mj666 // Reboot to ROM // mj666 diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 200915262..5ae5a67af 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -95,7 +95,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->sbus_inversion = 0; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; - telemetryConfigMutable()->telemetry_inverted = true; + telemetryConfigMutable()->telemetry_inverted = false; featureSet(FEATURE_TELEMETRY); } diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 560e8bdc4..cabae587c 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -65,7 +65,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->sbus_inversion = 0; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; - telemetryConfigMutable()->telemetry_inverted = true; + telemetryConfigMutable()->telemetry_inverted = false; batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; featureSet(FEATURE_TELEMETRY); diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index b069a88c3..0d3a7d23c 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -64,7 +64,7 @@ #define USE_SDCARD -//#define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PB11 #define SDCARD_DETECT_EXTI_LINE EXTI_Line10 @@ -88,11 +88,11 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -//#define M25P16_CS_PIN SPI2_NSS_PIN -//#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_PIN SPI2_NSS_PIN +#define M25P16_SPI_INSTANCE SPI2 -//#define USE_FLASHFS -//#define USE_FLASH_M25P16 +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define USE_VCP diff --git a/src/main/target/ALIENFLIGHTF4/target.mk b/src/main/target/ALIENFLIGHTF4/target.mk index cdd87e9dc..18a5e7586 100644 --- a/src/main/target/ALIENFLIGHTF4/target.mk +++ b/src/main/target/ALIENFLIGHTF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index c1cdbaad2..4fa4e376a 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -64,7 +64,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->sbus_inversion = 0; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; - telemetryConfigMutable()->telemetry_inverted = true; + telemetryConfigMutable()->telemetry_inverted = false; batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; featureSet(FEATURE_TELEMETRY); diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 815471d6a..701a365ea 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -74,7 +74,7 @@ #define USE_SDCARD -//#define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PB11 #define SDCARD_DETECT_EXTI_LINE EXTI_Line10 @@ -86,9 +86,9 @@ #define SDCARD_SPI_CS_PIN PB10 // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz // Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 @@ -98,11 +98,11 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -//#define M25P16_CS_PIN SPI2_NSS_PIN -//#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_PIN SPI2_NSS_PIN +#define M25P16_SPI_INSTANCE SPI2 -//#define USE_FLASHFS -//#define USE_FLASH_M25P16 +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define USE_VCP @@ -127,8 +127,8 @@ #define SERIAL_PORT_COUNT 6 -//#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_PIN PA8 // (HARDARE=0,PPM) +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_PIN PA8 // (Hardware=0, PPM/LED_STRIP) XXX Crash if using an LED strip. #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/ALIENFLIGHTNGF7/target.mk b/src/main/target/ALIENFLIGHTNGF7/target.mk index 393e77ca0..9b34460ca 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.mk +++ b/src/main/target/ALIENFLIGHTNGF7/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index 45016aa6b..d54f7569c 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -21,8 +21,8 @@ #define USBD_PRODUCT_STRING "AnyFCF7" -#define LED0 PB7 -#define LED1 PB6 +#define LED0_PIN PB7 +#define LED1_PIN PB6 #define BEEPER PB2 // Unused pin, can be mapped to elsewhere #define BEEPER_INVERTED @@ -94,6 +94,10 @@ #define SERIAL_PORT_COUNT 11 //VCP, USART1, USART2, USART3, UART4, UART5, USART6, USART7, USART8, SOFTSERIAL x 2 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define ESCSERIAL_TIMER_TX_PIN PB14 // XXX Provisional (Hardware=0, PPM) + #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_4 diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index 1dfd62e31..fa18b8179 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -23,8 +23,8 @@ #define USE_ESC_SENSOR -#define LED0 PB6 //red -#define LED1 PB9 //blue +#define LED0_PIN PB6 //red +#define LED1_PIN PB9 //blue #define BEEPER PB2 // Unused pin, can be mapped to elsewhere #define BEEPER_INVERTED @@ -88,6 +88,10 @@ #define SERIAL_PORT_COUNT 7 //VCP, USART1, UART4, UART5, USART6, SOFTSERIAL x 2 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define ESCSERIAL_TIMER_TX_PIN PB14 // XXX Provisional (Hardware=0, PPM) + #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 diff --git a/src/main/target/BETAFLIGHTF4/target.c b/src/main/target/BETAFLIGHTF4/target.c new file mode 100755 index 000000000..39a6a8c90 --- /dev/null +++ b/src/main/target/BETAFLIGHTF4/target.c @@ -0,0 +1,37 @@ +/* + * 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 "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + + DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S3_OUT D1_ST6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT + DEF_TIM(TIM3, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT D1_ST2 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED +}; diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h new file mode 100755 index 000000000..53a127519 --- /dev/null +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -0,0 +1,144 @@ +/* + * 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 "BFF4" +#define USBD_PRODUCT_STRING "BetaFlightF4" + +#define USE_ESC_SENSOR + +#define LED0_PIN PB5 + +// Leave beeper here but with none as io - so disabled unless mapped. +#define BEEPER PB4 + +// PC0 used as inverter select GPIO +#define INVERTER_PIN_UART6 PC13 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW180_DEG + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + + + +// MPU6000 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define BARO +#define USE_BARO_SPI_BMP280 + +#define BMP280_SPI_INSTANCE SPI2 +#define BMP280_CS_PIN PB3 + +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN PB12 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + +#define M25P16_CS_PIN PA15 +#define M25P16_SPI_INSTANCE SPI3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_VCP +#define VBUS_SENSING_PIN PC5 + + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART2, USART3, USART6 + +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + + +#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_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + + +#define USE_ADC +#define CURRENT_METER_ADC_PIN PC1 +#define VBAT_ADC_PIN PC2 + +#define LED_STRIP + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL + +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD) + + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + + +#define USABLE_TIMER_CHANNEL_COUNT 8 + +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/BETAFLIGHTF4/target.mk b/src/main/target/BETAFLIGHTF4/target.mk new file mode 100755 index 000000000..cd4076ba1 --- /dev/null +++ b/src/main/target/BETAFLIGHTF4/target.mk @@ -0,0 +1,10 @@ + +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_spi_bmp280.c \ + drivers/max7456.c \ No newline at end of file diff --git a/src/main/target/CLRACINGF7/target.h b/src/main/target/CLRACINGF7/target.h index 73ec82022..81afd0d04 100644 --- a/src/main/target/CLRACINGF7/target.h +++ b/src/main/target/CLRACINGF7/target.h @@ -104,6 +104,10 @@ #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define ESCSERIAL_TIMER_TX_PIN PB8 // XXX Provisional (Hardware=0, PPM) + // XXX To target maintainer: Bus device to configure must be specified. //#define USE_I2C diff --git a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c index 338186507..813fa8b80 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c +++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c @@ -80,9 +80,9 @@ uint8_t interruptCounter = 0; void bstProcessInCommand(void); void I2C_EV_IRQHandler() { - if(I2C_GetITStatus(BSTx, I2C_IT_ADDR)) { + if (I2C_GetITStatus(BSTx, I2C_IT_ADDR)) { CRC8 = 0; - if(I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) { + if (I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) { currentWriteBufferPointer = 0; receiverAddress = true; I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]); @@ -92,11 +92,11 @@ void I2C_EV_IRQHandler() bufferPointer = 1; } I2C_ClearITPendingBit(BSTx, I2C_IT_ADDR); - } else if(I2C_GetITStatus(BSTx, I2C_IT_RXNE)) { + } else if (I2C_GetITStatus(BSTx, I2C_IT_RXNE)) { uint8_t data = I2C_ReceiveData(BSTx); readData[bufferPointer] = data; - if(bufferPointer > 1) { - if(readData[1]+1 == bufferPointer) { + if (bufferPointer > 1) { + if (readData[1]+1 == bufferPointer) { crc8Cal(0); bstProcessInCommand(); } else { @@ -105,21 +105,21 @@ void I2C_EV_IRQHandler() } bufferPointer++; I2C_ClearITPendingBit(BSTx, I2C_IT_RXNE); - } else if(I2C_GetITStatus(BSTx, I2C_IT_TXIS)) { - if(receiverAddress) { - if(currentWriteBufferPointer > 0) { - if(!cleanflight_data_ready) { + } else if (I2C_GetITStatus(BSTx, I2C_IT_TXIS)) { + if (receiverAddress) { + if (currentWriteBufferPointer > 0) { + if (!cleanflight_data_ready) { I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); return; } - if(interruptCounter < DELAY_SENDING_BYTE) { + if (interruptCounter < DELAY_SENDING_BYTE) { interruptCounter++; I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); return; } else { interruptCounter = 0; } - if(writeData[0] == currentWriteBufferPointer) { + if (writeData[0] == currentWriteBufferPointer) { receiverAddress = false; crc8Cal(0); I2C_SendData(BSTx, (uint8_t) CRC8); @@ -129,11 +129,11 @@ void I2C_EV_IRQHandler() I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]); } } - } else if(bstWriteDataLen) { + } else if (bstWriteDataLen) { I2C_SendData(BSTx, (uint8_t) dataBuffer[dataBufferPointer]); - if(bstWriteDataLen > 1) + if (bstWriteDataLen > 1) dataBufferPointer++; - if(dataBufferPointer == bstWriteDataLen) { + if (dataBufferPointer == bstWriteDataLen) { I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE); dataBufferPointer = 0; bstWriteDataLen = 0; @@ -141,19 +141,19 @@ void I2C_EV_IRQHandler() } else { } I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); - } else if(I2C_GetITStatus(BSTx, I2C_IT_NACKF)) { - if(receiverAddress) { + } else if (I2C_GetITStatus(BSTx, I2C_IT_NACKF)) { + if (receiverAddress) { receiverAddress = false; I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE); } I2C_ClearITPendingBit(BSTx, I2C_IT_NACKF); - } else if(I2C_GetITStatus(BSTx, I2C_IT_STOPF)) { - if(bstWriteDataLen && dataBufferPointer == bstWriteDataLen) { + } else if (I2C_GetITStatus(BSTx, I2C_IT_STOPF)) { + if (bstWriteDataLen && dataBufferPointer == bstWriteDataLen) { dataBufferPointer = 0; bstWriteDataLen = 0; } I2C_ClearITPendingBit(BSTx, I2C_IT_STOPF); - } else if(I2C_GetITStatus(BSTx, I2C_IT_BERR) + } else if (I2C_GetITStatus(BSTx, I2C_IT_BERR) || I2C_GetITStatus(BSTx, I2C_IT_ARLO) || I2C_GetITStatus(BSTx, I2C_IT_OVR)) { bstTimeoutUserCallback(); @@ -193,7 +193,7 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/) GPIO_InitTypeDef GPIO_InitStructure; I2C_InitTypeDef BST_InitStructure; - if(BSTx == I2C1) { + if (BSTx == I2C1) { RCC_AHBPeriphClockCmd(BST1_SCL_CLK_SOURCE | BST1_SDA_CLK_SOURCE, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK); @@ -241,7 +241,7 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/) I2C_Cmd(I2C1, ENABLE); } - if(BSTx == I2C2) { + if (BSTx == I2C2) { RCC_AHBPeriphClockCmd(BST2_SCL_CLK_SOURCE | BST2_SDA_CLK_SOURCE, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK); @@ -314,7 +314,7 @@ uint16_t bstGetErrorCounter(void) bool bstWriteBusy(void) { - if(bstWriteDataLen) + if (bstWriteDataLen) return true; else return false; @@ -322,14 +322,14 @@ bool bstWriteBusy(void) bool bstMasterWrite(uint8_t* data) { - if(bstWriteDataLen==0) { + if (bstWriteDataLen==0) { CRC8 = 0; dataBufferPointer = 0; dataBuffer[0] = *data; dataBuffer[1] = *(data+1); bstWriteDataLen = dataBuffer[1] + 2; - for(uint8_t i=2; iIDR&BST1_SCL_PIN; else scl_set = BST2_SCL_GPIO->IDR&BST2_SCL_PIN; - if(I2C_GetFlagStatus(BSTx, I2C_FLAG_BUSY)==RESET && scl_set) { + if (I2C_GetFlagStatus(BSTx, I2C_FLAG_BUSY)==RESET && scl_set) { I2C_TransferHandling(BSTx, dataBuffer[dataBufferPointer], dataBuffer[dataBufferPointer+1]+1, I2C_AutoEnd_Mode, I2C_Generate_Start_Write); I2C_ITConfig(BSTx, I2C_IT_TXI, ENABLE); dataBufferPointer = 1; bstMasterWriteTimeout = micros(); } - } else if(currentTime>bstMasterWriteTimeout+BST_SHORT_TIMEOUT) { + } else if (currentTime>bstMasterWriteTimeout+BST_SHORT_TIMEOUT) { bstTimeoutUserCallback(); } } diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 9c93858bd..845785a9f 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -269,7 +269,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) { uint32_t i, tmp, junk; - switch(bstRequest) { + switch (bstRequest) { case BST_API_VERSION: bstWrite8(BST_PROTOCOL_VERSION); @@ -411,7 +411,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_RX_MAP: - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) + for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) bstWrite8(rxConfig()->rcmap[i]); break; @@ -455,7 +455,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) uint16_t tmp; bool ret = BST_PASSED; - switch(bstWriteCommand) { + switch (bstWriteCommand) { case BST_SELECT_SETTING: if (!ARMING_FLAG(ARMED)) { changePidProfile(bstRead8()); @@ -563,7 +563,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) } break; case BST_SET_RX_MAP: - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { + for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) { rxConfigMutable()->rcmap[i] = bstRead8(); } break; @@ -620,7 +620,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) } bstWrite8(ret); - if(ret == BST_FAILED) + if (ret == BST_FAILED) return false; return true; @@ -651,18 +651,18 @@ extern bool cleanflight_data_ready; void bstProcessInCommand(void) { readBufferPointer = 2; - if(bstCurrentAddress() == I2C_ADDR_CLEANFLIGHT_FC) { - if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) { + if (bstCurrentAddress() == I2C_ADDR_CLEANFLIGHT_FC) { + if (bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) { uint8_t i; writeBufferPointer = 1; cleanflight_data_ready = false; - for(i = 0; i < BST_BUFFER_SIZE; i++) { + for (i = 0; i < BST_BUFFER_SIZE; i++) { writeData[i] = 0; } switch (bstRead8()) { case BST_USB_DEVICE_INFO_REQUEST: bstRead8(); - if(bstSlaveUSBCommandFeedback(/*bstRead8()*/)) + if (bstSlaveUSBCommandFeedback(/*bstRead8()*/)) coreProReady = true; break; case BST_READ_COMMANDS: @@ -679,8 +679,8 @@ void bstProcessInCommand(void) } cleanflight_data_ready = true; } - } else if(bstCurrentAddress() == 0x00) { - if(bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) { + } else if (bstCurrentAddress() == 0x00) { + if (bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) { resetBstTimer = micros(); needResetCheck = true; } @@ -689,8 +689,8 @@ void bstProcessInCommand(void) static void resetBstChecker(timeUs_t currentTimeUs) { - if(needResetCheck) { - if(currentTimeUs >= (resetBstTimer + BST_RESET_TIME)) + if (needResetCheck) { + if (currentTimeUs >= (resetBstTimer + BST_RESET_TIME)) { bstTimeoutUserCallback(); needResetCheck = false; @@ -709,23 +709,23 @@ static uint8_t sendCounter = 0; void taskBstMasterProcess(timeUs_t currentTimeUs) { - if(coreProReady) { - if(currentTimeUs >= next02hzUpdateAt_1 && !bstWriteBusy()) { + if (coreProReady) { + if (currentTimeUs >= next02hzUpdateAt_1 && !bstWriteBusy()) { writeFCModeToBST(); next02hzUpdateAt_1 = currentTimeUs + UPDATE_AT_02HZ; } - if(currentTimeUs >= next20hzUpdateAt_1 && !bstWriteBusy()) { - if(sendCounter == 0) + if (currentTimeUs >= next20hzUpdateAt_1 && !bstWriteBusy()) { + if (sendCounter == 0) writeRCChannelToBST(); - else if(sendCounter == 1) + else if (sendCounter == 1) writeRollPitchYawToBST(); sendCounter++; - if(sendCounter > 1) + if (sendCounter > 1) sendCounter = 0; next20hzUpdateAt_1 = currentTimeUs + UPDATE_AT_20HZ; } #ifdef GPS - if(sensors(SENSOR_GPS) && !bstWriteBusy()) + if (sensors(SENSOR_GPS) && !bstWriteBusy()) writeGpsPositionPrameToBST(); #endif @@ -779,7 +779,7 @@ static uint8_t numOfSat = 0; #ifdef GPS bool writeGpsPositionPrameToBST(void) { - if((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) { + if ((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) { lat = gpsSol.llh.lat; lon = gpsSol.llh.lon; alt = gpsSol.llh.alt; @@ -826,7 +826,7 @@ bool writeRCChannelToBST(void) uint8_t i = 0; bstMasterStartBuffer(PUBLIC_ADDRESS); bstMasterWrite8(RC_CHANNEL_FRAME_ID); - for(i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) { + for (i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) { bstMasterWrite16(rcData[i]); } diff --git a/src/main/target/CRAZYFLIE2/serialrx.c b/src/main/target/CRAZYFLIE2/serialrx.c index e698eaf27..3d9b6b074 100644 --- a/src/main/target/CRAZYFLIE2/serialrx.c +++ b/src/main/target/CRAZYFLIE2/serialrx.c @@ -68,10 +68,10 @@ static bool rcFrameComplete = false; static void routeIncommingPacket(syslinkPacket_t* slp) { // Only support packets of type SYSLINK_RADIO_RAW - if(slp->type == SYSLINK_RADIO_RAW) { + if (slp->type == SYSLINK_RADIO_RAW) { crtpPacket_t *crtpPacket = (crtpPacket_t*)(slp->data); - switch(crtpPacket->header.port) { + switch (crtpPacket->header.port) { case CRTP_PORT_SETPOINT: { crtpCommanderRPYT_t *crtpRYPTPacket = @@ -95,7 +95,7 @@ static void routeIncommingPacket(syslinkPacket_t* slp) case CRTP_PORT_SETPOINT_GENERIC: // First byte of the packet is the type // Only support the CPPM Emulation type - if(crtpPacket->data[0] == cppmEmuType) { + if (crtpPacket->data[0] == cppmEmuType) { crtpCommanderCPPMEmuPacket_t *crtpCppmPacket = (crtpCommanderCPPMEmuPacket_t*)&crtpPacket->data[1]; @@ -124,7 +124,7 @@ static void routeIncommingPacket(syslinkPacket_t* slp) static void dataReceive(uint16_t c) { counter++; - switch(rxState) { + switch (rxState) { case waitForFirstStart: rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart; break; @@ -205,7 +205,7 @@ bool targetCustomSerialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxR { rxRuntimeConfigPtr = rxRuntimeConfig; - if(rxConfig->serialrx_provider != SERIALRX_TARGET_CUSTOM) + if (rxConfig->serialrx_provider != SERIALRX_TARGET_CUSTOM) { return false; } diff --git a/src/main/target/FF_PIKOBLX/config.c b/src/main/target/FF_PIKOBLX/config.c index 4d595e3a8..a7a94f5e0 100644 --- a/src/main/target/FF_PIKOBLX/config.c +++ b/src/main/target/FF_PIKOBLX/config.c @@ -63,7 +63,7 @@ void targetConfiguration(void) featureSet(FEATURE_TELEMETRY); #endif parseRcChannels("TAER1234", rxConfigMutable()); - + pidProfilesMutable(0)->pid[PID_ROLL].P = 80; pidProfilesMutable(0)->pid[PID_ROLL].I = 37; pidProfilesMutable(0)->pid[PID_ROLL].D = 35; @@ -72,7 +72,7 @@ void targetConfiguration(void) pidProfilesMutable(0)->pid[PID_PITCH].D = 35; pidProfilesMutable(0)->pid[PID_YAW].P = 180; pidProfilesMutable(0)->pid[PID_YAW].D = 45; - + controlRateProfilesMutable(0)->rcRate8 = 100; controlRateProfilesMutable(0)->rcYawRate8 = 100; controlRateProfilesMutable(0)->rcExpo8 = 15; diff --git a/src/main/target/FRSKYF3/target.c b/src/main/target/FRSKYF3/target.c index 0e9830300..4b6f68df3 100644 --- a/src/main/target/FRSKYF3/target.c +++ b/src/main/target/FRSKYF3/target.c @@ -24,14 +24,14 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM1 - DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM2 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM3 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM4 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM5 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM6 - DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM8 + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM1 + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM3 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM4 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM5 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM6 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD), // PWM8 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED|TIM_USE_TRANSPONDER, TIMER_OUTPUT_ENABLED), // LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED|TIM_USE_TRANSPONDER, TIMER_OUTPUT_ENABLED), // LED }; diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h index 9c7aafeca..f636da9af 100644 --- a/src/main/target/FRSKYF3/target.h +++ b/src/main/target/FRSKYF3/target.h @@ -17,7 +17,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "FRF3" +#define TARGET_BOARD_IDENTIFIER "FRF3" #define TARGET_CONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE @@ -111,7 +111,7 @@ #define SDCARD_SPI_CS_PIN SPI1_NSS_PIN #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 #define USE_ESC_SENSOR diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index f2bf48187..c208c286c 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -91,9 +91,9 @@ #define USE_FLASH_M25P16 #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 - + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - + #define DEFAULT_FEATURES (FEATURE_OSD) #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #else @@ -102,11 +102,11 @@ #define USE_SDCARD_SPI2 #define SDCARD_DETECT_INVERTED - + #define SDCARD_DETECT_PIN PB2 #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: @@ -118,9 +118,9 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING - + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - + #define BARO #define USE_BARO_MS5611 diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 0e27b07b1..fe0c20d30 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -78,11 +78,11 @@ #define MAX7456_SPI_CS_PIN PB12 #define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) #define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) - + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - - #define DEFAULT_FEATURES FEATURE_OSD - + + #define DEFAULT_FEATURES FEATURE_OSD + #else #define BARO @@ -111,7 +111,7 @@ #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CHANNEL DMA_Channel_0 - + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #endif diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 96fa4bb4a..e165741b7 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -118,6 +118,10 @@ #define SERIAL_PORT_COUNT 6 //VCP, USART1, USART3, USART6, SOFTSERIAL x 2 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define ESCSERIAL_TIMER_TX_PIN PC9 // XXX Provisional (Hardware=0, PPM) + #define USE_I2C #define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index ab70a3486..8ce1e39b7 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -21,7 +21,7 @@ #define TARGET_BOARD_IDENTIFIER "PLUM" #define USBD_PRODUCT_STRING "PLUMF4" -#elif defined(KIWIF4V2) +#elif defined(KIWIF4V2) #define TARGET_BOARD_IDENTIFIER "KIW2" #define USBD_PRODUCT_STRING "KIWIF4V2" @@ -36,7 +36,7 @@ #else #define LED0_PIN PB5 -#define LED1_PIN PB4 +#define LED1_PIN PB4 #endif #define BEEPER PA8 diff --git a/src/main/target/KROOZX/config.c b/src/main/target/KROOZX/config.c index e18f3672e..a08100eb1 100755 --- a/src/main/target/KROOZX/config.c +++ b/src/main/target/KROOZX/config.c @@ -33,7 +33,7 @@ #ifdef TARGET_CONFIG void targetConfiguration(void) { - voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = VBAT_SCALE; + voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = VBAT_SCALE; barometerConfigMutable()->baro_hardware = 0; compassConfigMutable()->mag_hardware = 0; osdConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1) | VISIBLE_FLAG; diff --git a/src/main/target/KROOZX/initialisation.c b/src/main/target/KROOZX/initialisation.c index d6b8c009c..fa8310910 100755 --- a/src/main/target/KROOZX/initialisation.c +++ b/src/main/target/KROOZX/initialisation.c @@ -23,9 +23,9 @@ void targetPreInit(void) { - IO_t osdChSwitch = IOGetByTag(IO_TAG(OSD_CH_SWITCH)); + IO_t osdChSwitch = IOGetByTag(IO_TAG(OSD_CH_SWITCH)); IOInit(osdChSwitch, OWNER_SYSTEM, 0); IOConfigGPIO(osdChSwitch, IOCFG_OUT_PP); - IOLo(osdChSwitch); + IOLo(osdChSwitch); } diff --git a/src/main/target/KROOZX/target.c b/src/main/target/KROOZX/target.c index 2151586bf..c98d7c945 100755 --- a/src/main/target/KROOZX/target.c +++ b/src/main/target/KROOZX/target.c @@ -34,5 +34,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM6 DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM7 DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM8 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0), // LED_STRIP + DEF_TIM(TIM8, CH1, PC6, TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0), // LED_STRIP }; diff --git a/src/main/target/LUMBAF3/target.c b/src/main/target/LUMBAF3/target.c index ebb643ed9..2044d0054 100644 --- a/src/main/target/LUMBAF3/target.c +++ b/src/main/target/LUMBAF3/target.c @@ -26,14 +26,14 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM | TIM_USE_TRANSPONDER, TIMER_INPUT_ENABLED), // PPM IN - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Rx - DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Tx - - DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S1 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S2 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S3 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S4 - - DEF_TIM(TIM15, CH1, PA2, TIM_USE_LED, 1), // GPIO TIMER - LED_STRIP + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM | TIM_USE_TRANSPONDER, TIMER_INPUT_ENABLED), // PPM IN + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Rx + DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Tx + + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S1 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S3 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S4 + + DEF_TIM(TIM15, CH1, PA2, TIM_USE_LED, 1), // GPIO TIMER - LED_STRIP }; diff --git a/src/main/target/LUMBAF3/target.h b/src/main/target/LUMBAF3/target.h index 04c665077..9f1323d1e 100644 --- a/src/main/target/LUMBAF3/target.h +++ b/src/main/target/LUMBAF3/target.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -#define TARGET_BOARD_IDENTIFIER "MCF3" // LumbaF3 Flight Controller by mC +#define TARGET_BOARD_IDENTIFIER "MCF3" // LumbaF3 Flight Controller by mC #define LED0_PIN PB3 #define BEEPER PC15 @@ -42,11 +42,11 @@ #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_MPU6000_ALIGN CW90_DEG #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_MPU6000_ALIGN CW90_DEG #define USE_VCP #define USE_UART1 @@ -70,17 +70,17 @@ #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define USE_ADC #define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA0 +#define VBAT_ADC_PIN PA0 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define LED_STRIP +#define LED_STRIP #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART1 -#define DEFAULT_FEATURES FEATURE_TELEMETRY - +#define DEFAULT_FEATURES FEATURE_TELEMETRY + // IO - from schematics #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index d29cf723a..9e6249a0b 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -25,16 +25,16 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0), // PPM - - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S1 DMA1_ST4 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // S2 DMA2_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // S3 DMA2_ST4 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S4 DMA2_ST7 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0), // PPM - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S5 DMA1_ST5 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S6 DMA2_ST6 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S1 DMA1_ST4 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // S2 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // S3 DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S4 DMA2_ST7 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0) // LED DMA1_ST0 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S5 DMA1_ST5 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S6 DMA2_ST6 + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0) // LED DMA1_ST0 }; diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index bff65e156..916ed6aa3 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -33,8 +33,8 @@ #define USE_SPI_DEVICE_1 #define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 #define MPU6500_CS_PIN PC2 #define MPU6500_SPI_INSTANCE SPI1 @@ -57,8 +57,8 @@ #define USE_SPI_DEVICE_3 #define SPI3_SCK_PIN PB3 -#define SPI3_MISO_PIN PB4 -#define SPI3_MOSI_PIN PB5 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PC1 @@ -68,16 +68,16 @@ // Divide to under 25MHz for normal operation: #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream7 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF7 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream7 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF7 +#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +#define SDCARD_DMA_CHANNEL DMA_Channel_0 // *************** OSD ***************************** #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 #define OSD #define USE_MAX7456 diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 29b7048fa..dc198f4c6 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -110,6 +110,6 @@ void targetValidateConfiguration(void) { if (hardwareRevision < NAZE32_REV5 && accelerometerConfig()->acc_hardware == ACC_ADXL345) { accelerometerConfigMutable()->acc_hardware = ACC_NONE; - } + } } #endif diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index cc59f45bc..caab21c38 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -94,8 +94,8 @@ #define SERIAL_PORT_COUNT 6 -//#define USE_ESCSERIAL //TODO: make ESC serial F7 compatible -//#define ESCSERIAL_TIMER_TX_PIN PC7 // (HARDARE=0,PPM) +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_PIN PC7 // (Hardware=0, PPM) #define USE_SPI diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h index e683c82c3..61b6d46cd 100644 --- a/src/main/target/NUCLEOF7/target.h +++ b/src/main/target/NUCLEOF7/target.h @@ -21,8 +21,8 @@ #define USBD_PRODUCT_STRING "NucleoF7" -#define LED0 PB7 -#define LED1 PB14 +#define LED0_PIN PB7 +#define LED1_PIN PB14 #define BEEPER PA0 #define BEEPER_INVERTED @@ -93,6 +93,10 @@ #define SERIAL_PORT_COUNT 10 //VCP, USART2, USART3, UART4, UART5, USART6, USART7, USART8, SOFTSERIAL x 2 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PPM +#define ESCSERIAL_TIMER_TX_PIN PB15 // XXX Provisional (Hardware=0, PPM) + #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_4 diff --git a/src/main/target/NUCLEOF722/target.h b/src/main/target/NUCLEOF722/target.h index 82e09fe95..c54dc9cfa 100644 --- a/src/main/target/NUCLEOF722/target.h +++ b/src/main/target/NUCLEOF722/target.h @@ -23,8 +23,8 @@ //#define USE_ESC_TELEMETRY -#define LED0 PB7 // blue -#define LED1 PB14 // red +#define LED0_PIN PB7 // blue +#define LED1_PIN PB14 // red #define BEEPER PA0 #define BEEPER_INVERTED @@ -95,6 +95,10 @@ #define SERIAL_PORT_COUNT 6 //VCP, USART2, USART3, UART4,SOFTSERIAL x 2 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PPM +#define ESCSERIAL_TIMER_TX_PIN PB15 // XXX Provisional (Hardware=0, PPM) + #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_4 diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 2461db0b3..6c1a04410 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -78,6 +78,10 @@ #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PPM +#define ESCSERIAL_TIMER_TX_PIN PE13 // XXX Provisional (Hardware=0, PPM) + #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 1e573f77f..2189ddf3e 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -79,7 +79,7 @@ void updateState(const fdm_packet* pkt) { clock_gettime(CLOCK_MONOTONIC, &now_ts); const uint64_t realtime_now = micros64_real(); - if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout + if (realtime_now > last_realtime + 500*1e3) { // 500ms timeout last_timestamp = pkt->timestamp; last_realtime = realtime_now; sendMotorUpdate(); @@ -87,7 +87,7 @@ void updateState(const fdm_packet* pkt) { } const double deltaSim = pkt->timestamp - last_timestamp; // in seconds - if(deltaSim < 0) { // don't use old packet + if (deltaSim < 0) { // don't use old packet return; } @@ -141,7 +141,7 @@ void updateState(const fdm_packet* pkt) { #endif - if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz + if (deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz // simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5; struct timespec out_ts; timeval_sub(&out_ts, &now_ts, &last_ts); @@ -168,7 +168,7 @@ static void* udpThread(void* data) { while (workerRunning) { n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100); - if(n == sizeof(fdm_packet)) { + if (n == sizeof(fdm_packet)) { // printf("[data]new fdm %d\n", n); updateState(&fdmPkt); } @@ -215,7 +215,7 @@ void systemInit(void) { } ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL); - if(ret != 0) { + if (ret != 0) { printf("Create tcpWorker error!\n"); exit(1); } @@ -227,7 +227,7 @@ void systemInit(void) { printf("start UDP server...%d\n", ret); ret = pthread_create(&udpWorker, NULL, udpThread, NULL); - if(ret != 0) { + if (ret != 0) { printf("Create udpWorker error!\n"); exit(1); } @@ -266,13 +266,13 @@ void timerStart(void) { void failureMode(failureMode_e mode) { printf("[failureMode]!!! %d\n", mode); - while(1); + while (1); } -void failureLedCode(failureMode_e mode, int repeatCount) +void indicateFailure(failureMode_e mode, int repeatCount) { UNUSED(repeatCount); - printf("Failure LED flash for: [failureMode]!!! %d\n", mode); + printf("Failure LED flash for: [failureMode]!!! %d\n", mode); } // Time part @@ -374,7 +374,7 @@ int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y) // PWM part -bool pwmMotorsEnabled = false; +static bool pwmMotorsEnabled = false; static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; @@ -406,6 +406,10 @@ pwmOutputPort_t *pwmGetMotors(void) { return motors; } +void pwmEnableMotors(void) { + pwmMotorsEnabled = true; +} + bool pwmAreMotorsEnabled(void) { return pwmMotorsEnabled; } @@ -440,7 +444,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) { pwmPkt.motor_speed[2] = motorsPwm[3] / outScale; // get one "fdm_packet" can only send one "servo_packet"!! - if(pthread_mutex_trylock(&updateLock) != 0) return; + if (pthread_mutex_trylock(&updateLock) != 0) return; udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); // printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]); } @@ -483,7 +487,7 @@ void FLASH_Unlock(void) { printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (long)(&__config_end - &__config_start)); for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) { int c = fgetc(eepromFd); - if(c == EOF) break; + if (c == EOF) break; eeprom[i] = (uint8_t)c; } } else { diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index d7d39d97b..68a5212b3 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -222,12 +222,12 @@ typedef enum } FLASH_Status; typedef struct { - double timestamp; // in seconds - double imu_angular_velocity_rpy[3]; // rad/s -> range: +/- 8192; +/- 2000 deg/se - double imu_linear_acceleration_xyz[3]; // m/s/s NED, body frame -> sim 1G = 9.80665, FC 1G = 256 - double imu_orientation_quat[4]; //w, x, y, z - double velocity_xyz[3]; // m/s, earth frame - double position_xyz[3]; // meters, NED from origin + double timestamp; // in seconds + double imu_angular_velocity_rpy[3]; // rad/s -> range: +/- 8192; +/- 2000 deg/se + double imu_linear_acceleration_xyz[3]; // m/s/s NED, body frame -> sim 1G = 9.80665, FC 1G = 256 + double imu_orientation_quat[4]; //w, x, y, z + double velocity_xyz[3]; // m/s, earth frame + double position_xyz[3]; // meters, NED from origin } fdm_packet; typedef struct { float motor_speed[4]; // normal: [0.0, 1.0], 3D: [-1.0, 1.0] diff --git a/src/main/target/SITL/udplink.c b/src/main/target/SITL/udplink.c index 8080b7686..2a4a4deaf 100644 --- a/src/main/target/SITL/udplink.c +++ b/src/main/target/SITL/udplink.c @@ -13,56 +13,56 @@ #include "udplink.h" int udpInit(udpLink_t* link, const char* addr, int port, bool isServer) { - int one = 1; + int one = 1; - if((link->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { - return -2; - } + if ((link->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { + return -2; + } - setsockopt(link->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); // can multi-bind - fcntl(link->fd, F_SETFL, fcntl(link->fd, F_GETFL, 0) | O_NONBLOCK); // nonblock + setsockopt(link->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); // can multi-bind + fcntl(link->fd, F_SETFL, fcntl(link->fd, F_GETFL, 0) | O_NONBLOCK); // nonblock - link->isServer = isServer; - memset(&link->si, 0, sizeof(link->si)); - link->si.sin_family = AF_INET; - link->si.sin_port = htons(port); - link->port = port; + link->isServer = isServer; + memset(&link->si, 0, sizeof(link->si)); + link->si.sin_family = AF_INET; + link->si.sin_port = htons(port); + link->port = port; - if(addr == NULL) { - link->si.sin_addr.s_addr = htonl(INADDR_ANY); - }else{ - link->si.sin_addr.s_addr = inet_addr(addr); - } + if (addr == NULL) { + link->si.sin_addr.s_addr = htonl(INADDR_ANY); + }else{ + link->si.sin_addr.s_addr = inet_addr(addr); + } - if(isServer) { - if (bind(link->fd, (const struct sockaddr *)&link->si, sizeof(link->si)) == -1) { - return -1; - } - } - return 0; + if (isServer) { + if (bind(link->fd, (const struct sockaddr *)&link->si, sizeof(link->si)) == -1) { + return -1; + } + } + return 0; } int udpSend(udpLink_t* link, const void* data, size_t size) { - return sendto(link->fd, data, size, 0, (struct sockaddr *)&link->si, sizeof(link->si)); + return sendto(link->fd, data, size, 0, (struct sockaddr *)&link->si, sizeof(link->si)); } int udpRecv(udpLink_t* link, void* data, size_t size, uint32_t timeout_ms) { - fd_set fds; - struct timeval tv; + fd_set fds; + struct timeval tv; - FD_ZERO(&fds); - FD_SET(link->fd, &fds); + FD_ZERO(&fds); + FD_SET(link->fd, &fds); - tv.tv_sec = timeout_ms / 1000; - tv.tv_usec = (timeout_ms % 1000) * 1000UL; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000UL; - if (select(link->fd+1, &fds, NULL, NULL, &tv) != 1) { - return -1; - } + if (select(link->fd+1, &fds, NULL, NULL, &tv) != 1) { + return -1; + } - socklen_t len; - int ret; - ret = recvfrom(link->fd, data, size, 0, (struct sockaddr *)&link->recv, &len); - return ret; + socklen_t len; + int ret; + ret = recvfrom(link->fd, data, size, 0, (struct sockaddr *)&link->recv, &len); + return ret; } diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index dc8ca8bd2..5c11f6406 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -162,6 +162,7 @@ #endif #define OSD +#define DISABLE_EXTENDED_CMS_OSD_MENU #define USE_OSD_OVER_MSP_DISPLAYPORT #define USE_MSP_CURRENT_METER diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index 2657b98f1..0d39a94de 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -21,8 +21,8 @@ #define USBD_PRODUCT_STRING "VRRACE" -#define LED0 PD14 -#define LED1 PD15 +#define LED0_PIN PD14 +#define LED1_PIN PD15 #define BEEPER PA0 #define BEEPER_INVERTED diff --git a/src/main/target/stm32f1xx_hal_conf.h b/src/main/target/stm32f1xx_hal_conf.h index deb836888..2f5a9c23f 100644 --- a/src/main/target/stm32f1xx_hal_conf.h +++ b/src/main/target/stm32f1xx_hal_conf.h @@ -83,7 +83,7 @@ /** * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. * This value is used by the RCC HAL module to compute the system frequency - * (when HSE is used as system clock source, directly or through the PLL). + * (when HSE is used as system clock source, directly or through the PLL). */ #if !defined (HSE_VALUE) #if defined(USE_STM3210C_EVAL) @@ -114,22 +114,22 @@ #define LSE_VALUE ((uint32_t)32768) /*!< Value of the External oscillator in Hz*/ #endif /* LSE_VALUE */ - + #if !defined (LSE_STARTUP_TIMEOUT) #define LSE_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for LSE start up, in ms */ #endif /* HSE_STARTUP_TIMEOUT */ - + /* Tip: To avoid modifying this file each time you need to use different HSE, === you can define the HSE value in your toolchain compiler preprocessor. */ /* ########################### System Configuration ######################### */ /** * @brief This is the HAL system configuration section - */ -#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */ -#define TICK_INT_PRIORITY ((uint32_t)0x000F) /*!< tick interrupt priority */ -#define USE_RTOS 0 + */ +#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */ +#define TICK_INT_PRIORITY ((uint32_t)0x000F) /*!< tick interrupt priority */ +#define USE_RTOS 0 #define PREFETCH_ENABLE 1 /* ########################## Assert Selection ############################## */ @@ -152,7 +152,7 @@ #define MAC_ADDR4 0 #define MAC_ADDR5 0 -/* Definition of the Ethernet driver buffers size and count */ +/* Definition of the Ethernet driver buffers size and count */ #define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ #define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ #define ETH_RXBUFNB ((uint32_t)8) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ @@ -189,7 +189,7 @@ #define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020) /*!< Auto-Negotiation process completed */ #define PHY_LINKED_STATUS ((uint16_t)0x0004) /*!< Valid link established */ #define PHY_JABBER_DETECTION ((uint16_t)0x0002) /*!< Jabber condition detected */ - + /* Section 4: Extended PHY Registers */ #define PHY_SR ((uint16_t)0x10) /*!< PHY status register Offset */ @@ -220,15 +220,15 @@ #ifdef HAL_GPIO_MODULE_ENABLED #include "stm32f1xx_hal_gpio.h" #endif /* HAL_GPIO_MODULE_ENABLED */ - + #ifdef HAL_DMA_MODULE_ENABLED #include "stm32f1xx_hal_dma.h" #endif /* HAL_DMA_MODULE_ENABLED */ - + #ifdef HAL_ETH_MODULE_ENABLED #include "stm32f1xx_hal_eth.h" -#endif /* HAL_ETH_MODULE_ENABLED */ - +#endif /* HAL_ETH_MODULE_ENABLED */ + #ifdef HAL_CAN_MODULE_ENABLED #include "stm32f1xx_hal_can.h" #endif /* HAL_CAN_MODULE_ENABLED */ @@ -291,11 +291,11 @@ #ifdef HAL_SD_MODULE_ENABLED #include "stm32f1xx_hal_sd.h" -#endif /* HAL_SD_MODULE_ENABLED */ +#endif /* HAL_SD_MODULE_ENABLED */ #ifdef HAL_NAND_MODULE_ENABLED #include "stm32f1xx_hal_nand.h" -#endif /* HAL_NAND_MODULE_ENABLED */ +#endif /* HAL_NAND_MODULE_ENABLED */ #ifdef HAL_SPI_MODULE_ENABLED #include "stm32f1xx_hal_spi.h" @@ -332,8 +332,8 @@ #ifdef HAL_HCD_MODULE_ENABLED #include "stm32f1xx_hal_hcd.h" -#endif /* HAL_HCD_MODULE_ENABLED */ - +#endif /* HAL_HCD_MODULE_ENABLED */ + /* Exported macro ------------------------------------------------------------*/ #ifdef USE_FULL_ASSERT diff --git a/src/main/target/stm32f3xx_hal_conf.h b/src/main/target/stm32f3xx_hal_conf.h index be3c623d6..a7172724d 100644 --- a/src/main/target/stm32f3xx_hal_conf.h +++ b/src/main/target/stm32f3xx_hal_conf.h @@ -86,7 +86,7 @@ /** * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. * This value is used by the RCC HAL module to compute the system frequency - * (when HSE is used as system clock source, directly or through the PLL). + * (when HSE is used as system clock source, directly or through the PLL). */ #if !defined (HSE_VALUE) #define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */ @@ -115,13 +115,13 @@ */ #if !defined (HSI_STARTUP_TIMEOUT) #define HSI_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for HSI start up */ -#endif /* HSI_STARTUP_TIMEOUT */ +#endif /* HSI_STARTUP_TIMEOUT */ /** * @brief Internal Low Speed oscillator (LSI) value. */ #if !defined (LSI_VALUE) - #define LSI_VALUE ((uint32_t)40000) + #define LSI_VALUE ((uint32_t)40000) #endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz The real value may vary depending on the variations in voltage and temperature. */ @@ -130,7 +130,7 @@ */ #if !defined (LSE_VALUE) #define LSE_VALUE ((uint32_t)32768) /*!< Value of the External Low Speed oscillator in Hz */ -#endif /* LSE_VALUE */ +#endif /* LSE_VALUE */ /** * @brief Time out for LSE start up value in ms. @@ -156,7 +156,7 @@ /* ########################### System Configuration ######################### */ /** * @brief This is the HAL system configuration section - */ + */ #define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */ #define TICK_INT_PRIORITY ((uint32_t)(1<<__NVIC_PRIO_BITS) - 1) /*!< tick interrupt priority (lowest by default) */ #define USE_RTOS 0 @@ -187,7 +187,7 @@ #ifdef HAL_DMA_MODULE_ENABLED #include "stm32f3xx_hal_dma.h" #endif /* HAL_DMA_MODULE_ENABLED */ - + #ifdef HAL_CORTEX_MODULE_ENABLED #include "stm32f3xx_hal_cortex.h" #endif /* HAL_CORTEX_MODULE_ENABLED */ @@ -235,7 +235,7 @@ #ifdef HAL_PCCARD_MODULE_ENABLED #include "stm32f3xx_hal_pccard.h" #endif /* HAL_PCCARD_MODULE_ENABLED */ - + #ifdef HAL_HRTIM_MODULE_ENABLED #include "stm32f3xx_hal_hrtim.h" #endif /* HAL_HRTIM_MODULE_ENABLED */ @@ -323,8 +323,8 @@ void assert_failed(uint8_t* file, uint32_t line); #else #define assert_param(expr) ((void)0U) -#endif /* USE_FULL_ASSERT */ - +#endif /* USE_FULL_ASSERT */ + #ifdef __cplusplus } #endif diff --git a/src/main/target/stm32f4xx_hal_conf.h b/src/main/target/stm32f4xx_hal_conf.h index acabfce95..17bb3238a 100644 --- a/src/main/target/stm32f4xx_hal_conf.h +++ b/src/main/target/stm32f4xx_hal_conf.h @@ -47,7 +47,7 @@ /** * @brief This is the list of modules to be used in the HAL driver */ -#define HAL_MODULE_ENABLED +#define HAL_MODULE_ENABLED #define HAL_ADC_MODULE_ENABLED //#define HAL_CAN_MODULE_ENABLED //#define HAL_CRC_MODULE_ENABLED @@ -97,7 +97,7 @@ /** * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. * This value is used by the RCC HAL module to compute the system frequency - * (when HSE is used as system clock source, directly or through the PLL). + * (when HSE is used as system clock source, directly or through the PLL). */ #if !defined (HSE_VALUE) #define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */ @@ -150,7 +150,7 @@ /* ########################### System Configuration ######################### */ /** * @brief This is the HAL system configuration section - */ + */ #define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */ #define TICK_INT_PRIORITY ((uint32_t)0x0FU) /*!< tick interrupt priority */ #define USE_RTOS 0U @@ -177,7 +177,7 @@ #define MAC_ADDR4 0U #define MAC_ADDR5 0U -/* Definition of the Ethernet driver buffers size and count */ +/* Definition of the Ethernet driver buffers size and count */ #define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ #define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ #define ETH_RXBUFNB ((uint32_t)4U) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ @@ -214,7 +214,7 @@ #define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020U) /*!< Auto-Negotiation process completed */ #define PHY_LINKED_STATUS ((uint16_t)0x0004U) /*!< Valid link established */ #define PHY_JABBER_DETECTION ((uint16_t)0x0002U) /*!< Jabber condition detected */ - + /* Section 4: Extended PHY Registers */ #define PHY_SR ((uint16_t)0x0010U) /*!< PHY status register Offset */ @@ -256,7 +256,7 @@ #ifdef HAL_DMA_MODULE_ENABLED #include "stm32f4xx_hal_dma.h" #endif /* HAL_DMA_MODULE_ENABLED */ - + #ifdef HAL_CORTEX_MODULE_ENABLED #include "stm32f4xx_hal_cortex.h" #endif /* HAL_CORTEX_MODULE_ENABLED */ @@ -312,7 +312,7 @@ #ifdef HAL_PCCARD_MODULE_ENABLED #include "stm32f4xx_hal_pccard.h" #endif /* HAL_PCCARD_MODULE_ENABLED */ - + #ifdef HAL_SDRAM_MODULE_ENABLED #include "stm32f4xx_hal_sdram.h" #endif /* HAL_SDRAM_MODULE_ENABLED */ @@ -392,7 +392,7 @@ #ifdef HAL_HCD_MODULE_ENABLED #include "stm32f4xx_hal_hcd.h" #endif /* HAL_HCD_MODULE_ENABLED */ - + #ifdef HAL_DSI_MODULE_ENABLED #include "stm32f4xx_hal_dsi.h" #endif /* HAL_DSI_MODULE_ENABLED */ diff --git a/src/main/target/stm32f7xx_hal_conf.h b/src/main/target/stm32f7xx_hal_conf.h index 26afa82fa..e8114a5a0 100644 --- a/src/main/target/stm32f7xx_hal_conf.h +++ b/src/main/target/stm32f7xx_hal_conf.h @@ -156,10 +156,12 @@ * @brief This is the HAL system configuration section */ #define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */ -#define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */ +#define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */ #define USE_RTOS 0U #define PREFETCH_ENABLE 0U #define ART_ACCLERATOR_ENABLE 0U /* To enable instruction cache and prefetch */ +#define INSTRUCTION_CACHE_ENABLE 1U +#define DATA_CACHE_ENABLE 0U /* ########################## Assert Selection ############################## */ /** diff --git a/src/main/target/system_stm32f30x.c b/src/main/target/system_stm32f30x.c index e7f691817..e948db3ca 100644 --- a/src/main/target/system_stm32f30x.c +++ b/src/main/target/system_stm32f30x.c @@ -304,7 +304,7 @@ void SetSysClock(void) { HSEStatus = RCC->CR & RCC_CR_HSERDY; StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); if ((RCC->CR & RCC_CR_HSERDY) != RESET) { @@ -343,7 +343,7 @@ void SetSysClock(void) RCC->CR |= RCC_CR_PLLON; /* Wait till PLL is ready */ - while((RCC->CR & RCC_CR_PLLRDY) == 0) + while ((RCC->CR & RCC_CR_PLLRDY) == 0) { } diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index e7e6d0406..20b31ebd8 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -422,8 +422,6 @@ uint32_t hse_value = HSE_VALUE; /** @addtogroup STM32F4xx_System_Private_Variables * @{ */ -/* core clock is simply a mhz of PLL_N / PLL_P */ -uint32_t SystemCoreClock = (PLL_N / PLL_P) * 1000000; __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; @@ -456,8 +454,15 @@ static void SystemInit_ExtMemCtl(void); * @param None * @retval None */ + +uint32_t SystemCoreClock; +uint32_t pll_p = PLL_P, pll_n = PLL_N, pll_q = PLL_Q; + void SystemInit(void) { + /* core clock is simply a mhz of PLL_N / PLL_P */ + SystemCoreClock = (pll_n / pll_p) * 1000000; + /* FPU settings ------------------------------------------------------------*/ #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ @@ -487,7 +492,7 @@ void SystemInit(void) /* Configure the System clock source, PLL Multiplier and Divider factors, AHB/APBx prescalers and Flash settings ----------------------------------*/ - SetSysClock(); + //SetSysClock(); /* Configure the Vector Table location add offset address ------------------*/ #ifdef VECT_TAB_SRAM @@ -497,6 +502,16 @@ void SystemInit(void) #endif } +void SystemInitOC(void) +{ + /* PLL setting for overclocking */ + pll_n = PLL_N_OC; + pll_p = PLL_P_OC; + pll_q = PLL_Q_OC; + + SystemInit(); +} + /** * @brief Update SystemCoreClock variable according to Clock Register Values. * The SystemCoreClock variable contains the core clock (HCLK), it can @@ -627,7 +642,7 @@ void SetSysClock(void) { HSEStatus = RCC->CR & RCC_CR_HSERDY; StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); if ((RCC->CR & RCC_CR_HSERDY) != RESET) { @@ -673,30 +688,30 @@ void SetSysClock(void) #if defined(STM32F446xx) /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24) | (PLL_R << 28); + RCC->PLLCFGR = PLL_M | (pll_n << 6) | (((pll_p >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (pll_q << 24) | (PLL_R << 28); #else /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); + RCC->PLLCFGR = PLL_M | (pll_n << 6) | (((pll_p >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (pll_q << 24); #endif /* STM32F446xx */ /* Enable the main PLL */ RCC->CR |= RCC_CR_PLLON; /* Wait till the main PLL is ready */ - while((RCC->CR & RCC_CR_PLLRDY) == 0) + while ((RCC->CR & RCC_CR_PLLRDY) == 0) { } #if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx) /* Enable the Over-drive to extend the clock frequency to 180 Mhz */ PWR->CR |= PWR_CR_ODEN; - while((PWR->CSR & PWR_CSR_ODRDY) == 0) + while ((PWR->CSR & PWR_CSR_ODRDY) == 0) { } PWR->CR |= PWR_CR_ODSWEN; - while((PWR->CSR & PWR_CSR_ODSWRDY) == 0) + while ((PWR->CSR & PWR_CSR_ODSWRDY) == 0) { } #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ @@ -1003,7 +1018,7 @@ void SystemInit_ExtMemCtl(void) /* Clock enable command */ FMC_Bank5_6->SDCMR = 0x00000011; tmpreg = FMC_Bank5_6->SDSR & 0x00000020; - while((tmpreg != 0) & (timeout-- > 0)) + while ((tmpreg != 0) & (timeout-- > 0)) { tmpreg = FMC_Bank5_6->SDSR & 0x00000020; } @@ -1014,7 +1029,7 @@ void SystemInit_ExtMemCtl(void) /* PALL command */ FMC_Bank5_6->SDCMR = 0x00000012; timeout = 0xFFFF; - while((tmpreg != 0) & (timeout-- > 0)) + while ((tmpreg != 0) & (timeout-- > 0)) { tmpreg = FMC_Bank5_6->SDSR & 0x00000020; } @@ -1022,7 +1037,7 @@ void SystemInit_ExtMemCtl(void) /* Auto refresh command */ FMC_Bank5_6->SDCMR = 0x00000073; timeout = 0xFFFF; - while((tmpreg != 0) & (timeout-- > 0)) + while ((tmpreg != 0) & (timeout-- > 0)) { tmpreg = FMC_Bank5_6->SDSR & 0x00000020; } @@ -1030,7 +1045,7 @@ void SystemInit_ExtMemCtl(void) /* MRD register program */ FMC_Bank5_6->SDCMR = 0x00046014; timeout = 0xFFFF; - while((tmpreg != 0) & (timeout-- > 0)) + while ((tmpreg != 0) & (timeout-- > 0)) { tmpreg = FMC_Bank5_6->SDSR & 0x00000020; } diff --git a/src/main/target/system_stm32f4xx.h b/src/main/target/system_stm32f4xx.h index e8f457048..159b095c5 100644 --- a/src/main/target/system_stm32f4xx.h +++ b/src/main/target/system_stm32f4xx.h @@ -32,8 +32,14 @@ extern "C" { #endif +#define PLL_N_OC 480 +#define PLL_P_OC 2 +#define PLL_Q_OC 10 +#define OC_FREQUENCY_HZ (1000000*PLL_N_OC/PLL_P_OC) + extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ extern void SystemInit(void); +extern void SystemInitOC(void); extern void SystemCoreClockUpdate(void); #ifdef __cplusplus diff --git a/src/main/target/system_stm32f7xx.c b/src/main/target/system_stm32f7xx.c index 74991e2d1..b928ecf60 100644 --- a/src/main/target/system_stm32f7xx.c +++ b/src/main/target/system_stm32f7xx.c @@ -167,16 +167,14 @@ RCC_OscInitStruct.PLL.PLLQ = PLL_Q; ret = HAL_RCC_OscConfig(&RCC_OscInitStruct); - if(ret != HAL_OK) - { - while(1) { ; } + if (ret != HAL_OK) { + while (1); } /* Activate the OverDrive to reach the 216 MHz Frequency */ ret = HAL_PWREx_EnableOverDrive(); - if(ret != HAL_OK) - { - while(1) { ; } + if (ret != HAL_OK) { + while (1); } /* Select PLLSAI output as USB clock source */ PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_CLK48; @@ -184,9 +182,8 @@ PeriphClkInitStruct.PLLSAI.PLLSAIN = PLL_SAIN; PeriphClkInitStruct.PLLSAI.PLLSAIQ = PLL_SAIQ; PeriphClkInitStruct.PLLSAI.PLLSAIP = PLL_SAIP; - if(HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) - { - while(1) {}; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) { + while (1); } /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers */ @@ -197,9 +194,8 @@ RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; ret = HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7); - if(ret != HAL_OK) - { - while(1) { ; } + if (ret != HAL_OK) { + while (1); } PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART1|RCC_PERIPHCLK_USART2 @@ -221,9 +217,8 @@ PeriphClkInitStruct.I2c3ClockSelection = RCC_I2C3CLKSOURCE_PCLK1; PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_PCLK1; ret = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct); - if (ret != HAL_OK) - { - while(1) { ; } + if (ret != HAL_OK) { + while (1); } // Activating the timerprescalers while the APBx prescalers are 1/2/4 will connect the TIMxCLK to HCLK which has been configured to 216MHz @@ -249,54 +244,53 @@ */ void SystemInit(void) { - /* FPU settings ------------------------------------------------------------*/ - #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + /* 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; +#endif + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; - /* Reset CFGR register */ - RCC->CFGR = 0x00000000; + /* Reset CFGR register */ + RCC->CFGR = 0x00000000; - /* Reset HSEON, CSSON and PLLON bits */ - RCC->CR &= (uint32_t)0xFEF6FFFF; + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; - /* Reset PLLCFGR register */ - RCC->PLLCFGR = 0x24003010; + /* Reset PLLCFGR register */ + RCC->PLLCFGR = 0x24003010; - /* Reset HSEBYP bit */ - RCC->CR &= (uint32_t)0xFFFBFFFF; + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; - /* Disable all interrupts */ - RCC->CIR = 0x00000000; + /* Disable all interrupts */ + RCC->CIR = 0x00000000; /* Configure the Vector Table location add offset address ------------------*/ #ifdef VECT_TAB_SRAM - SCB->VTOR = RAMDTCM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ + SCB->VTOR = RAMDTCM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ #else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ #endif - /* Enable I-Cache */ - SCB_EnableICache(); - - /* Enable D-Cache */ - //SCB_EnableDCache(); - - /* Configure the system clock to 216 MHz */ - SystemClock_Config(); - - if(SystemCoreClock != 216000000) - { - while(1) - { - // There is a mismatch between the configured clock and the expected clock in portable.h - } - } + /* Enable I-Cache */ + if (INSTRUCTION_CACHE_ENABLE) { + SCB_EnableICache(); + } + /* Enable D-Cache */ + if (DATA_CACHE_ENABLE) { + SCB_EnableDCache(); + } + + /* Configure the system clock to 216 MHz */ + SystemClock_Config(); + if (SystemCoreClock != 216000000) { + // There is a mismatch between the configured clock and the expected clock in portable.h + while (1); + } } /** @@ -337,61 +331,46 @@ void SystemInit(void) */ void SystemCoreClockUpdate(void) { - uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2; + uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2; - /* Get SYSCLK source -------------------------------------------------------*/ - tmp = RCC->CFGR & RCC_CFGR_SWS; + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; - switch (tmp) - { + switch (tmp) { case 0x00: /* HSI used as system clock source */ - SystemCoreClock = HSI_VALUE; - break; + SystemCoreClock = HSI_VALUE; + break; case 0x04: /* HSE used as system clock source */ - SystemCoreClock = HSE_VALUE; - break; + SystemCoreClock = HSE_VALUE; + break; case 0x08: /* PLL used as system clock source */ - /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N SYSCLK = PLL_VCO / PLL_P */ - pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; - pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; + pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; - if (pllsource != 0) - { - /* HSE used as PLL clock source */ - pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); - } - else - { - /* HSI used as PLL clock source */ - pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); - } + if (pllsource != 0) { + /* HSE used as PLL clock source */ + pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } else { + /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } - pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; - SystemCoreClock = pllvco/pllp; - break; + pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >> 16) + 1) * 2; + SystemCoreClock = pllvco/pllp; + break; default: - SystemCoreClock = HSI_VALUE; - break; - } - /* Compute HCLK frequency --------------------------------------------------*/ - /* Get HCLK prescaler */ - tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; - /* HCLK frequency */ - SystemCoreClock >>= tmp; + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK frequency --------------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK frequency */ + SystemCoreClock >>= tmp; } -/** - * @} - */ - -/** - * @} - */ - -/** - * @} - */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 4f5d893bf..1aa65b646 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -231,10 +231,10 @@ static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) { batteryState_e batteryState; - if (shouldTriggerBatteryAlarmNow()){ + if (shouldTriggerBatteryAlarmNow()) { lastHottAlarmSoundTime = millis(); batteryState = getBatteryState(); - if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL){ + if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { hottEAMMessage->warning_beeps = 0x10; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1; } @@ -282,7 +282,7 @@ static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage) int32_t vario = getEstimatedVario(); hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF; hottEAMMessage->climbrate_H = (30000 + vario) >> 8; - hottEAMMessage->climbrate3s = 120 + (vario / 100); + hottEAMMessage->climbrate3s = 120 + (vario / 100); } void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage) @@ -401,7 +401,7 @@ void configureHoTTTelemetryPort(void) static void hottSendResponse(uint8_t *buffer, int length) { - if(hottIsSending) { + if (hottIsSending) { return; } @@ -514,7 +514,7 @@ static void hottSendTelemetryData(void) { hottReconfigurePort(); --hottMsgRemainingBytesToSendCount; - if(hottMsgRemainingBytesToSendCount == 0) { + if (hottMsgRemainingBytesToSendCount == 0) { hottSerialWrite(hottMsgCrc++); return; } @@ -572,7 +572,7 @@ void handleHoTTTelemetry(timeUs_t currentTimeUs) return; if (hottIsSending) { - if(currentTimeUs - serialTimer < HOTT_TX_DELAY_US) { + if (currentTimeUs - serialTimer < HOTT_TX_DELAY_US) { return; } } diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index a60d00f00..ce6df3973 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -122,7 +122,7 @@ static ibusAddress_t getAddress(const uint8_t *ibusPacket) static uint8_t dispatchMeasurementReply(ibusAddress_t address) { int value; - + switch (sensorAddressTypeLookup[address - ibusBaseAddress]) { case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE: value = getBatteryVoltage() * 10; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 6b0ae90e5..b84085477 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -436,7 +436,7 @@ void mavlinkSendHUDAndHeartbeat(void) mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; uint8_t mavSystemType; - switch(mixerConfig()->mixerMode) + switch (mixerConfig()->mixerMode) { case MIXER_TRI: mavSystemType = MAV_TYPE_TRICOPTER; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 3f06e0039..53a959011 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -242,7 +242,7 @@ static void smartPortDataReceive(uint16_t c) rxBuffer[smartPortRxBytes++] = c; - if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) { + if (smartPortRxBytes == SMARTPORT_FRAME_SIZE) { if (c == (0xFF - checksum)) { smartPortFrameReceived = true; } @@ -280,7 +280,7 @@ static void smartPortSendPackageEx(uint8_t frameId, uint8_t* data) { uint16_t crc = 0; smartPortSendByte(frameId, &crc); - for(unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++) { + for (unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++) { smartPortSendByte(*data++, &crc); } smartPortSendByte(0xFF - (uint8_t)crc, NULL); @@ -559,11 +559,11 @@ void handleSmartPortTelemetry(void) smartPortDataReceive(c); } - if(smartPortFrameReceived) { + if (smartPortFrameReceived) { smartPortFrameReceived = false; // do not check the physical ID here again // unless we start receiving other sensors' packets - if(smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { + if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { // Pass only the payload: skip sensorId & frameId handleSmartPortMspFrame(&smartPortRxBuffer); @@ -577,7 +577,7 @@ void handleSmartPortTelemetry(void) return; } - if(smartPortMspReplyPending) { + if (smartPortMspReplyPending) { smartPortMspReplyPending = smartPortSendMspReply(); smartPortHasRequest = 0; return; @@ -596,7 +596,7 @@ void handleSmartPortTelemetry(void) static uint8_t t1Cnt = 0; static uint8_t t2Cnt = 0; - switch(id) { + switch (id) { #ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { @@ -742,7 +742,7 @@ void handleSmartPortTelemetry(void) } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; - } else if (telemetryConfig()->pidValuesAsTelemetry){ + } else if (telemetryConfig()->pidValuesAsTelemetry) { switch (t2Cnt) { case 0: tmp2 = currentPidProfile->pid[PID_ROLL].P; diff --git a/src/main/vcp/usb_istr.c b/src/main/vcp/usb_istr.c index beb3e2beb..74a1a56c5 100644 --- a/src/main/vcp/usb_istr.c +++ b/src/main/vcp/usb_istr.c @@ -193,7 +193,7 @@ void USB_Istr(void) _SetCNTR(wCNTR); /*poll for RESET flag in ISTR*/ - while((_GetISTR()&ISTR_RESET) == 0); + while ((_GetISTR()&ISTR_RESET) == 0); /* clear RESET flag in ISTR */ _SetISTR((uint16_t)CLR_RESET); diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c index 4a4cf497d..c91e9da83 100644 --- a/src/main/vcp_hal/usbd_cdc_interface.c +++ b/src/main/vcp_hal/usbd_cdc_interface.c @@ -123,7 +123,7 @@ static int8_t CDC_Itf_Init(void) /*##-4- Start the TIM Base generation in interrupt mode ####################*/ /* Start Channel1 */ - if(HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK) + if (HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK) { /* Starting Error */ Error_Handler(); @@ -222,14 +222,14 @@ static int8_t CDC_Itf_Control (uint8_t cmd, uint8_t* pbuf, uint16_t length) */ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { - if(htim->Instance != TIMusb) return; + if (htim->Instance != TIMusb) return; uint32_t buffptr; uint32_t buffsize; - if(UserTxBufPtrOut != UserTxBufPtrIn) + if (UserTxBufPtrOut != UserTxBufPtrIn) { - if(UserTxBufPtrOut > UserTxBufPtrIn) /* Roll-back */ + if (UserTxBufPtrOut > UserTxBufPtrIn) /* Roll-back */ { buffsize = APP_RX_DATA_SIZE - UserTxBufPtrOut; } @@ -242,7 +242,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) USBD_CDC_SetTxBuffer(&USBD_Device, (uint8_t*)&UserTxBuffer[buffptr], buffsize); - if(USBD_CDC_TransmitPacket(&USBD_Device) == USBD_OK) + if (USBD_CDC_TransmitPacket(&USBD_Device) == USBD_OK) { UserTxBufPtrOut += buffsize; if (UserTxBufPtrOut == APP_TX_DATA_SIZE) @@ -288,7 +288,7 @@ static void TIM_Config(void) TimHandle.Init.Prescaler = (SystemCoreClock / 2 / (1000000)) - 1; TimHandle.Init.ClockDivision = 0; TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; - if(HAL_TIM_Base_Init(&TimHandle) != HAL_OK) + if (HAL_TIM_Base_Init(&TimHandle) != HAL_OK) { /* Initialization Error */ Error_Handler(); @@ -318,7 +318,7 @@ static void Error_Handler(void) uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) { uint32_t count = 0; - if( (rxBuffPtr != NULL)) + if ( (rxBuffPtr != NULL)) { while ((rxAvailable > 0) && count < len) { @@ -326,7 +326,7 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) rxBuffPtr++; rxAvailable--; count++; - if(rxAvailable < 1) + if (rxAvailable < 1) USBD_CDC_ReceivePacket(&USBD_Device); } } @@ -361,7 +361,7 @@ uint32_t CDC_Send_FreeBytes(void) uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) { USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*)USBD_Device.pClassData; - while(hcdc->TxState != 0); + while (hcdc->TxState != 0); for (uint32_t i = 0; i < sendLength; i++) { diff --git a/src/main/vcp_hal/usbd_conf.c b/src/main/vcp_hal/usbd_conf.c index 4d12dd3ae..2cbd0452f 100644 --- a/src/main/vcp_hal/usbd_conf.c +++ b/src/main/vcp_hal/usbd_conf.c @@ -84,7 +84,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd) { GPIO_InitTypeDef GPIO_InitStruct; - if(hpcd->Instance == USB_OTG_FS) + if (hpcd->Instance == USB_OTG_FS) { /* Configure USB FS GPIOs */ __HAL_RCC_GPIOA_CLK_ENABLE(); @@ -97,7 +97,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd) GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - if(hpcd->Init.vbus_sensing_enable == 1) + if (hpcd->Init.vbus_sensing_enable == 1) { /* Configure VBUS Pin */ GPIO_InitStruct.Pin = GPIO_PIN_9; @@ -122,7 +122,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd) /* Enable USBFS Interrupt */ HAL_NVIC_EnableIRQ(OTG_FS_IRQn); } - else if(hpcd->Instance == USB_OTG_HS) + else if (hpcd->Instance == USB_OTG_HS) { #ifdef USE_USB_HS_IN_FS @@ -136,7 +136,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd) GPIO_InitStruct.Alternate = GPIO_AF12_OTG_HS_FS; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - if(hpcd->Init.vbus_sensing_enable == 1) + if (hpcd->Init.vbus_sensing_enable == 1) { /* Configure VBUS Pin */ GPIO_InitStruct.Pin = GPIO_PIN_13 ; @@ -216,13 +216,13 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd) */ void HAL_PCD_MspDeInit(PCD_HandleTypeDef *hpcd) { - if(hpcd->Instance == USB_OTG_FS) + if (hpcd->Instance == USB_OTG_FS) { /* Disable USB FS Clock */ __HAL_RCC_USB_OTG_FS_CLK_DISABLE(); __HAL_RCC_SYSCFG_CLK_DISABLE(); } - else if(hpcd->Instance == USB_OTG_HS) + else if (hpcd->Instance == USB_OTG_HS) { /* Disable USB HS Clocks */ __HAL_RCC_USB_OTG_HS_CLK_DISABLE(); @@ -286,7 +286,7 @@ void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd) USBD_SpeedTypeDef speed = USBD_SPEED_FULL; /* Set USB Current Speed */ - switch(hpcd->Init.speed) + switch (hpcd->Init.speed) { case PCD_SPEED_HIGH: speed = USBD_SPEED_HIGH; @@ -559,7 +559,7 @@ uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { PCD_HandleTypeDef *hpcd = pdev->pData; - if((ep_addr & 0x80) == 0x80) + if ((ep_addr & 0x80) == 0x80) { return hpcd->IN_ep[ep_addr & 0x7F].is_stall; } diff --git a/src/main/vcp_hal/usbd_desc.c b/src/main/vcp_hal/usbd_desc.c index 7da957d5e..fa0324783 100644 --- a/src/main/vcp_hal/usbd_desc.c +++ b/src/main/vcp_hal/usbd_desc.c @@ -174,7 +174,7 @@ uint8_t *USBD_VCP_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) */ uint8_t *USBD_VCP_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - if(speed == USBD_SPEED_HIGH) + if (speed == USBD_SPEED_HIGH) { USBD_GetString((uint8_t *)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length); } @@ -223,7 +223,7 @@ uint8_t *USBD_VCP_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) */ uint8_t *USBD_VCP_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - if(speed == USBD_SPEED_HIGH) + if (speed == USBD_SPEED_HIGH) { USBD_GetString((uint8_t *)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length); } @@ -242,7 +242,7 @@ uint8_t *USBD_VCP_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) */ uint8_t *USBD_VCP_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - if(speed == USBD_SPEED_HIGH) + if (speed == USBD_SPEED_HIGH) { USBD_GetString((uint8_t *)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length); } @@ -286,9 +286,9 @@ static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len) { uint8_t idx = 0; - for( idx = 0; idx < len; idx ++) + for ( idx = 0; idx < len; idx ++) { - if( ((value >> 28)) < 0xA ) + if ( ((value >> 28)) < 0xA ) { pbuf[ 2* idx] = (value >> 28) + '0'; } diff --git a/src/main/vcpf4/stm32f4xx_it.c b/src/main/vcpf4/stm32f4xx_it.c index d2392e642..b7f01a04e 100644 --- a/src/main/vcpf4/stm32f4xx_it.c +++ b/src/main/vcpf4/stm32f4xx_it.c @@ -59,7 +59,7 @@ void PendSV_Handler(void) #ifdef USE_USB_OTG_FS void OTG_FS_WKUP_IRQHandler(void) { - if(USB_OTG_dev.cfg.low_power) + if (USB_OTG_dev.cfg.low_power) { *(uint32_t *)(0xE000ED10) &= 0xFFFFFFF9 ; SystemInit(); @@ -77,7 +77,7 @@ void OTG_FS_WKUP_IRQHandler(void) #ifdef USE_USB_OTG_HS void OTG_HS_WKUP_IRQHandler(void) { - if(USB_OTG_dev.cfg.low_power) + if (USB_OTG_dev.cfg.low_power) { *(uint32_t *)(0xE000ED10) &= 0xFFFFFFF9 ; SystemInit(); diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index 73ea774b1..cdba125ba 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -225,7 +225,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) { - if(speed == 0) + if (speed == 0) USBD_GetString ((uint8_t*)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length); else USBD_GetString ((uint8_t*)USBD_PRODUCT_FS_STRING, USBD_StrDesc, length); @@ -256,7 +256,7 @@ uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length) { - if(speed == USB_OTG_SPEED_HIGH) + if (speed == USB_OTG_SPEED_HIGH) USBD_GetString ((uint8_t*)USBD_SERIALNUMBER_HS_STRING, USBD_StrDesc, length); else USBD_GetString ((uint8_t*)USBD_SERIALNUMBER_FS_STRING, USBD_StrDesc, length); @@ -273,7 +273,7 @@ uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length) { - if(speed == USB_OTG_SPEED_HIGH) + if (speed == USB_OTG_SPEED_HIGH) USBD_GetString ((uint8_t*)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length); else USBD_GetString ((uint8_t*)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc, length); @@ -291,7 +291,7 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) { - if(speed == 0) + if (speed == 0) USBD_GetString ((uint8_t*)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length); else USBD_GetString ((uint8_t*)USBD_INTERFACE_FS_STRING, USBD_StrDesc, length); diff --git a/src/test/Makefile b/src/test/Makefile index a30ab5cfd..1575e7236 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -113,7 +113,8 @@ osd_unittest_SRC := \ $(USER_DIR)/common/typeconversion.c \ $(USER_DIR)/drivers/display.c \ $(USER_DIR)/common/maths.c \ - $(USER_DIR)/common/printf.c + $(USER_DIR)/common/printf.c \ + $(USER_DIR)/fc/runtime_config.c osd_unittest_DEFINES := \ OSD diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index de1524b11..c0f324f12 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -47,10 +47,9 @@ extern "C" { #include "rx/rx.h" void osdRefresh(timeUs_t currentTimeUs); + void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time); + void osdFormatTimer(char *buff, bool showSymbol, int timerIndex); - uint8_t stateFlags; - uint8_t armingFlags; - uint16_t flightModeFlags; uint16_t rssi; attitudeEulerAngles_t attitude; pidProfile_t *currentPidProfile; @@ -272,8 +271,8 @@ TEST(OsdTest, TestStatsImperial) osdConfigMutable()->enabled_stats[OSD_STAT_MAX_ALTITUDE] = true; osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX] = false; osdConfigMutable()->enabled_stats[OSD_STAT_END_BATTERY] = true; - osdConfigMutable()->enabled_stats[OSD_STAT_FLYTIME] = true; - osdConfigMutable()->enabled_stats[OSD_STAT_ARMEDTIME] = true; + osdConfigMutable()->enabled_stats[OSD_STAT_TIMER_1] = true; + osdConfigMutable()->enabled_stats[OSD_STAT_TIMER_2] = true; osdConfigMutable()->enabled_stats[OSD_STAT_MAX_DISTANCE] = true; osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = false; @@ -281,6 +280,14 @@ TEST(OsdTest, TestStatsImperial) // using imperial unit system osdConfigMutable()->units = OSD_UNIT_IMPERIAL; + // and + // this timer 1 configuration + osdConfigMutable()->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_HUNDREDTHS, 0); + + // and + // this timer 2 configuration + osdConfigMutable()->timers[OSD_TIMER_2] = OSD_TIMER(OSD_TIMER_SRC_LAST_ARMED, OSD_TIMER_PREC_SECOND, 0); + // and // a GPS fix is present stateFlags |= GPS_FIX | GPS_FIX_HOME; @@ -321,14 +328,15 @@ TEST(OsdTest, TestStatsImperial) // then // statistics screen should display the following - displayPortTestBufferSubstring(2, 3, "ARMED TIME : 00:04"); - displayPortTestBufferSubstring(2, 4, "FLY TIME : 00:07"); - displayPortTestBufferSubstring(2, 5, "MAX SPEED : 28"); - displayPortTestBufferSubstring(2, 6, "MAX DISTANCE : 328%c", SYM_FT); - displayPortTestBufferSubstring(2, 7, "MIN BATTERY : 14.7%c", SYM_VOLT); - displayPortTestBufferSubstring(2, 8, "END BATTERY : 15.2%c", SYM_VOLT); - displayPortTestBufferSubstring(2, 9, "MIN RSSI : 25%%"); - displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE : 6.5%c", SYM_FT); + int row = 3; + displayPortTestBufferSubstring(2, row++, "TOTAL ARM : 00:05.00"); + displayPortTestBufferSubstring(2, row++, "LAST ARM : 00:03"); + displayPortTestBufferSubstring(2, row++, "MAX SPEED : 28"); + displayPortTestBufferSubstring(2, row++, "MAX DISTANCE : 328%c", SYM_FT); + displayPortTestBufferSubstring(2, row++, "MIN BATTERY : 14.7%c", SYM_VOLT); + displayPortTestBufferSubstring(2, row++, "END BATTERY : 15.2%c", SYM_VOLT); + displayPortTestBufferSubstring(2, row++, "MIN RSSI : 25%%"); + displayPortTestBufferSubstring(2, row++, "MAX ALTITUDE : 6.5%c", SYM_FT); } /* @@ -370,14 +378,15 @@ TEST(OsdTest, TestStatsMetric) // then // statistics screen should display the following - displayPortTestBufferSubstring(2, 3, "ARMED TIME : 00:02"); - displayPortTestBufferSubstring(2, 4, "FLY TIME : 00:09"); - displayPortTestBufferSubstring(2, 5, "MAX SPEED : 28"); - displayPortTestBufferSubstring(2, 6, "MAX DISTANCE : 100%c", SYM_M); - displayPortTestBufferSubstring(2, 7, "MIN BATTERY : 14.7%c", SYM_VOLT); - displayPortTestBufferSubstring(2, 8, "END BATTERY : 15.2%c", SYM_VOLT); - displayPortTestBufferSubstring(2, 9, "MIN RSSI : 25%%"); - displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE : 2.0%c", SYM_M); + int row = 3; + displayPortTestBufferSubstring(2, row++, "TOTAL ARM : 00:07.50"); + displayPortTestBufferSubstring(2, row++, "LAST ARM : 00:02"); + displayPortTestBufferSubstring(2, row++, "MAX SPEED : 28"); + displayPortTestBufferSubstring(2, row++, "MAX DISTANCE : 100%c", SYM_M); + displayPortTestBufferSubstring(2, row++, "MIN BATTERY : 14.7%c", SYM_VOLT); + displayPortTestBufferSubstring(2, row++, "END BATTERY : 15.2%c", SYM_VOLT); + displayPortTestBufferSubstring(2, row++, "MIN RSSI : 25%%"); + displayPortTestBufferSubstring(2, row++, "MAX ALTITUDE : 2.0%c", SYM_M); } /* @@ -393,16 +402,30 @@ TEST(OsdTest, TestAlarms) // the following OSD elements are visible osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(8, 1) | VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1) | VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_FLYTIME] = OSD_POS(1, 1) | VISIBLE_FLAG; + osdConfigMutable()->item_pos[OSD_ITEM_TIMER_1] = OSD_POS(20, 1) | VISIBLE_FLAG; + osdConfigMutable()->item_pos[OSD_ITEM_TIMER_2] = OSD_POS(1, 1) | VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_ALTITUDE] = OSD_POS(23, 7) | VISIBLE_FLAG; // and // this set of alarm values osdConfigMutable()->rssi_alarm = 20; osdConfigMutable()->cap_alarm = 2200; - osdConfigMutable()->time_alarm = 1; // in minutes osdConfigMutable()->alt_alarm = 100; // meters + // and + // this timer 1 configuration + osdConfigMutable()->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_HUNDREDTHS, 2); + EXPECT_EQ(OSD_TIMER_SRC_ON, OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])); + EXPECT_EQ(OSD_TIMER_PREC_HUNDREDTHS, OSD_TIMER_PRECISION(osdConfig()->timers[OSD_TIMER_1])); + EXPECT_EQ(2, OSD_TIMER_ALARM(osdConfig()->timers[OSD_TIMER_1])); + + // and + // this timer 2 configuration + osdConfigMutable()->timers[OSD_TIMER_2] = OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 1); + EXPECT_EQ(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])); + EXPECT_EQ(OSD_TIMER_PREC_SECOND, OSD_TIMER_PRECISION(osdConfig()->timers[OSD_TIMER_2])); + EXPECT_EQ(1, OSD_TIMER_ALARM(osdConfig()->timers[OSD_TIMER_2])); + // and // using the metric unit system osdConfigMutable()->units = OSD_UNIT_METRIC; @@ -424,6 +447,7 @@ TEST(OsdTest, TestAlarms) displayPortTestBufferSubstring(8, 1, "%c99", SYM_RSSI); displayPortTestBufferSubstring(12, 1, "%c16.8%c", SYM_BATT_FULL, SYM_VOLT); displayPortTestBufferSubstring(1, 1, "%c00:", SYM_FLY_M); // only test the minute part of the timer + displayPortTestBufferSubstring(20, 1, "%c01:", SYM_ON_M); // only test the minute part of the timer displayPortTestBufferSubstring(23, 7, " 0.0%c", SYM_M); } @@ -433,11 +457,8 @@ TEST(OsdTest, TestAlarms) simulationBatteryState = BATTERY_CRITICAL; simulationBatteryVoltage = 135; simulationAltitude = 12000; - // Fly timer is incremented on periodic calls to osdRefresh, can't simply just increment the simulated system clock - for (int i = 0; i < 60; i++) { - simulationTime += 1e6; - osdRefresh(simulationTime); - } + simulationTime += 60e6; + osdRefresh(simulationTime); // then // elements showing values in alarm range should flash @@ -454,6 +475,7 @@ TEST(OsdTest, TestAlarms) displayPortTestBufferSubstring(8, 1, "%c12", SYM_RSSI); displayPortTestBufferSubstring(12, 1, "%c13.5%c", SYM_MAIN_BATT, SYM_VOLT); displayPortTestBufferSubstring(1, 1, "%c01:", SYM_FLY_M); // only test the minute part of the timer + displayPortTestBufferSubstring(20, 1, "%c02:", SYM_ON_M); // only test the minute part of the timer displayPortTestBufferSubstring(23, 7, " 120.0%c", SYM_M); } else { displayPortTestBufferIsEmpty(); @@ -495,12 +517,63 @@ TEST(OsdTest, TestElementRssi) displayPortTestBufferSubstring(8, 1, "%c50", SYM_RSSI); } +/* + * Tests the time string formatting function with a series of precision settings and time values. + */ +TEST(OsdTest, TestFormatTimeString) +{ + char buff[OSD_ELEMENT_BUFFER_LENGTH]; + + /* Seconds precision, 0 us */ + osdFormatTime(buff, OSD_TIMER_PREC_SECOND, 0); + EXPECT_EQ(0, strcmp("00:00", buff)); + + /* Seconds precision, 0.9 seconds */ + osdFormatTime(buff, OSD_TIMER_PREC_SECOND, 0.9e6); + EXPECT_EQ(0, strcmp("00:00", buff)); + + /* Seconds precision, 10 seconds */ + osdFormatTime(buff, OSD_TIMER_PREC_SECOND, 10e6); + EXPECT_EQ(0, strcmp("00:10", buff)); + + /* Seconds precision, 1 minute */ + osdFormatTime(buff, OSD_TIMER_PREC_SECOND, 60e6); + EXPECT_EQ(0, strcmp("01:00", buff)); + + /* Seconds precision, 1 minute 59 seconds */ + osdFormatTime(buff, OSD_TIMER_PREC_SECOND, 119e6); + EXPECT_EQ(0, strcmp("01:59", buff)); + + /* Hundredths precision, 0 us */ + osdFormatTime(buff, OSD_TIMER_PREC_HUNDREDTHS, 0); + EXPECT_EQ(0, strcmp("00:00.00", buff)); + + /* Hundredths precision, 10 milliseconds (one 100th of a second) */ + osdFormatTime(buff, OSD_TIMER_PREC_HUNDREDTHS, 10e3); + EXPECT_EQ(0, strcmp("00:00.01", buff)); + + /* Hundredths precision, 0.9 seconds */ + osdFormatTime(buff, OSD_TIMER_PREC_HUNDREDTHS, 0.9e6); + EXPECT_EQ(0, strcmp("00:00.90", buff)); + + /* Hundredths precision, 10 seconds */ + osdFormatTime(buff, OSD_TIMER_PREC_HUNDREDTHS, 10e6); + EXPECT_EQ(0, strcmp("00:10.00", buff)); + + /* Hundredths precision, 1 minute */ + osdFormatTime(buff, OSD_TIMER_PREC_HUNDREDTHS, 60e6); + EXPECT_EQ(0, strcmp("01:00.00", buff)); + + /* Hundredths precision, 1 minute 59 seconds */ + osdFormatTime(buff, OSD_TIMER_PREC_HUNDREDTHS, 119e6); + EXPECT_EQ(0, strcmp("01:59.00", buff)); +} + // STUBS extern "C" { - bool sensors(uint32_t mask) { - UNUSED(mask); - return true; + void beeperConfirmationBeeps(uint8_t beepCount) { + UNUSED(beepCount); } bool IS_RC_MODE_ACTIVE(boxId_e boxId) { @@ -574,7 +647,4 @@ extern "C" { UNUSED(pDisplay); return false; } - - void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } - void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 75f8e8358..edd846db6 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -701,4 +701,5 @@ uint8_t stateFlags = 0; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); +void resetArmingDisabled(void) {} }