From f0539b2b515e8d340d407a56073fddd8811189b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 30 Jun 2018 22:23:59 +0100 Subject: [PATCH 01/57] Refactor the inverter driver Split INVERTER_PIN_UART into INVERTER_PIN_UART_RX and INVERTER_PIN_UART_TX. This allows us to invert just one line of the port in F4 based controllers, like we can do on F3. Fixes #3466 --- make/mcu/STM32F4.mk | 2 +- make/mcu/STM32F7.mk | 2 +- src/main/drivers/inverter.c | 117 ----------- src/main/drivers/serial_uart.c | 17 +- src/main/drivers/uart_inverter.c | 188 ++++++++++++++++++ .../drivers/{inverter.h => uart_inverter.h} | 12 +- src/main/fc/fc_init.c | 6 +- src/main/target/OMNIBUSF4/target.h | 6 +- 8 files changed, 215 insertions(+), 135 deletions(-) delete mode 100644 src/main/drivers/inverter.c create mode 100644 src/main/drivers/uart_inverter.c rename src/main/drivers/{inverter.h => uart_inverter.h} (70%) diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk index 09fe494405..4f68ada8a3 100644 --- a/make/mcu/STM32F4.mk +++ b/make/mcu/STM32F4.mk @@ -156,13 +156,13 @@ MCU_COMMON_SRC = \ drivers/adc_stm32f4xx.c \ drivers/adc_stm32f4xx.c \ drivers/bus_i2c_stm32f40x.c \ - drivers/inverter.c \ drivers/serial_softserial.c \ drivers/serial_uart_stm32f4xx.c \ drivers/system_stm32f4xx.c \ drivers/timer.c \ drivers/timer_impl_stdperiph.c \ drivers/timer_stm32f4xx.c \ + drivers/uart_inverter.c \ drivers/dma_stm32f4xx.c ifeq ($(PERIPH_DRIVER), HAL) diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index e2139a7c81..43c8455c77 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -146,11 +146,11 @@ MCU_COMMON_SRC = \ drivers/adc_stm32f7xx.c \ drivers/bus_i2c_hal.c \ drivers/dma_stm32f7xx.c \ - drivers/inverter.c \ drivers/bus_spi_hal.c \ drivers/timer.c \ drivers/timer_impl_hal.c \ drivers/timer_stm32f7xx.c \ + drivers/uart_inverter.c \ drivers/system_stm32f7xx.c \ drivers/serial_uart_stm32f7xx.c \ drivers/serial_softserial.c \ diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c deleted file mode 100644 index cc8d90cec0..0000000000 --- a/src/main/drivers/inverter.c +++ /dev/null @@ -1,117 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include "platform.h" - -#include "drivers/io.h" -#include "io_impl.h" - -#include "inverter.h" - -#ifdef USE_INVERTER -static void inverterSet(IO_t pin, bool on) -{ - IOWrite(pin, on); -} - -static void initInverter(ioTag_t ioTag) -{ - IO_t pin = IOGetByTag(ioTag); - IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 0); - IOConfigGPIO(pin, IOCFG_OUT_PP); - - inverterSet(pin, false); -} -#endif - -void initInverters(void) -{ -#ifdef INVERTER_PIN_UART1 - initInverter(IO_TAG(INVERTER_PIN_UART1)); -#endif - -#ifdef INVERTER_PIN_UART2 - initInverter(IO_TAG(INVERTER_PIN_UART2)); -#endif - -#ifdef INVERTER_PIN_UART3 - initInverter(IO_TAG(INVERTER_PIN_UART3)); -#endif - -#ifdef INVERTER_PIN_USART4 - initInverter(IO_TAG(INVERTER_PIN_USART4)); -#endif - -#ifdef INVERTER_PIN_USART5 - initInverter(IO_TAG(INVERTER_PIN_USART5)); -#endif - -#ifdef INVERTER_PIN_UART6 - initInverter(IO_TAG(INVERTER_PIN_UART6)); -#endif -} - -void enableInverter(USART_TypeDef *USARTx, bool on) -{ -#ifdef USE_INVERTER - IO_t pin = IO_NONE; - -#ifdef INVERTER_PIN_UART1 - if (USARTx == USART1) { - pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART1)); - } -#endif - -#ifdef INVERTER_PIN_UART2 - if (USARTx == USART2) { - pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART2)); - } -#endif - -#ifdef INVERTER_PIN_UART3 - if (USARTx == USART3) { - pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART3)); - } -#endif - -#ifdef INVERTER_PIN_USART4 - if (USARTx == USART4) { - pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART4)); - } -#endif - -#ifdef INVERTER_PIN_USART5 - if (USARTx == USART5) { - pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART5)); - } -#endif - -#ifdef INVERTER_PIN_UART6 - if (USARTx == USART6) { - pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART6)); - } -#endif - - inverterSet(pin, on); -#else - UNUSED(USARTx); - UNUSED(on); -#endif -} diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 85e98102bc..f04676bff2 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -28,23 +28,28 @@ #include "build/build_config.h" #include "common/utils.h" -#include "inverter.h" + +#include "drivers/uart_inverter.h" #include "serial.h" #include "serial_uart.h" #include "serial_uart_impl.h" static void usartConfigurePinInversion(uartPort_t *uartPort) { -#if !defined(USE_INVERTER) && !defined(STM32F303xC) +#if !defined(USE_UART_INVERTER) && !defined(STM32F303xC) UNUSED(uartPort); #else bool inverted = uartPort->port.options & SERIAL_INVERTED; -#ifdef USE_INVERTER - if (inverted) { - // Enable hardware inverter if available. - enableInverter(uartPort->USARTx, true); +#ifdef USE_UART_INVERTER + uartInverterLine_e invertedLines = UART_INVERTER_LINE_NONE; + if (uartPort->port.mode & MODE_RX) { + invertedLines |= UART_INVERTER_LINE_RX; } + if (uartPort->port.mode & MODE_TX) { + invertedLines |= UART_INVERTER_LINE_TX; + } + uartInverterSet(uartPort->USARTx, invertedLines, inverted); #endif #ifdef STM32F303xC diff --git a/src/main/drivers/uart_inverter.c b/src/main/drivers/uart_inverter.c new file mode 100644 index 0000000000..0cb015671b --- /dev/null +++ b/src/main/drivers/uart_inverter.c @@ -0,0 +1,188 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/uart_inverter.h" + +#if defined(USE_UART_INVERTER) + +static void inverterPinSet(IO_t pin, bool on) +{ + IOWrite(pin, on); +} + +static void initInverter(ioTag_t ioTag) +{ + IO_t pin = IOGetByTag(ioTag); + IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 0); + IOConfigGPIO(pin, IOCFG_OUT_PP); + + inverterPinSet(pin, false); +} + +void uartInverterInit(void) +{ + +// UART1 +#ifdef INVERTER_PIN_UART1_RX + initInverter(IO_TAG(INVERTER_PIN_UART1_RX)); +#endif + +#ifdef INVERTER_PIN_UART1_TX + initInverter(IO_TAG(INVERTER_PIN_UART1_TX)); +#endif + +// UART2 +#ifdef INVERTER_PIN_UART2_RX + initInverter(IO_TAG(INVERTER_PIN_UART2_RX)); +#endif + +#ifdef INVERTER_PIN_UART2_TX + initInverter(IO_TAG(INVERTER_PIN_UART2_TX)); +#endif + +// UART3 +#ifdef INVERTER_PIN_UART3_RX + initInverter(IO_TAG(INVERTER_PIN_UART3_RX)); +#endif + +#ifdef INVERTER_PIN_UART3_TX + initInverter(IO_TAG(INVERTER_PIN_UART3_TX)); +#endif + +// UART4 +#ifdef INVERTER_PIN_UART4_RX + initInverter(IO_TAG(INVERTER_PIN_UART4_RX)); +#endif + +#ifdef INVERTER_PIN_UART4_TX + initInverter(IO_TAG(INVERTER_PIN_UART4_TX)); +#endif + +// UART5 +#ifdef INVERTER_PIN_UART5_RX + initInverter(IO_TAG(INVERTER_PIN_UART5_RX)); +#endif + +#ifdef INVERTER_PIN_UART5_TX + initInverter(IO_TAG(INVERTER_PIN_UART5_TX)); +#endif + +// UART6 +#ifdef INVERTER_PIN_UART6_RX + initInverter(IO_TAG(INVERTER_PIN_UART6_RX)); +#endif + +#ifdef INVERTER_PIN_UART6_TX + initInverter(IO_TAG(INVERTER_PIN_UART6_TX)); +#endif + +} + +void uartInverterSet(USART_TypeDef *USARTx, uartInverterLine_e line, bool enable) +{ + IO_t rx_pin = IO_NONE; + IO_t tx_pin = IO_NONE; + +// UART1 +#if defined(INVERTER_PIN_UART1_RX) || defined(INVERTER_PIN_UART1_TX) + if (USARTx == USART1) { +#if defined(INVERTER_PIN_UART1_RX) + rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART1_RX)); +#endif +#if defined(INVERTER_PIN_UART1_TX) + tx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART1_TX)); +#endif + } +#endif + +// UART2 +#if defined(INVERTER_PIN_UART2_RX) || defined(INVERTER_PIN_UART2_TX) + if (USARTx == USART2) { +#if defined(INVERTER_PIN_UART2_RX) + rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART2_RX)); +#endif +#if defined(INVERTER_PIN_UART2_TX) + tx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART2_TX)); +#endif + } +#endif + +// UART3 +#if defined(INVERTER_PIN_UART3_RX) || defined(INVERTER_PIN_UART3_TX) + if (USARTx == USART3) { +#if defined(INVERTER_PIN_UART3_RX) + rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART3_RX)); +#endif +#if defined(INVERTER_PIN_UART3_TX) + tx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART3_TX)); +#endif + } +#endif + +// UART4 +#if defined(INVERTER_PIN_UART4_RX) || defined(INVERTER_PIN_UART4_TX) + if (USARTx == USART4) { +#if defined(INVERTER_PIN_UART4_RX) + rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART4_RX)); +#endif +#if defined(INVERTER_PIN_UART4_TX) + tx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART4_TX)); +#endif + } +#endif + +// UART5 +#if defined(INVERTER_PIN_UART5_RX) || defined(INVERTER_PIN_UART5_TX) + if (USARTx == USART5) { +#if defined(INVERTER_PIN_UART5_RX) + rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART5_RX)); +#endif +#if defined(INVERTER_PIN_UART5_TX) + tx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART5_TX)); +#endif + } +#endif + +// UART6 +#if defined(INVERTER_PIN_UART6_RX) || defined(INVERTER_PIN_UART6_TX) + if (USARTx == USART6) { +#if defined(INVERTER_PIN_UART6_RX) + rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART6_RX)); +#endif +#if defined(INVERTER_PIN_UART6_TX) + tx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART6_TX)); +#endif + } +#endif + + // Now do the actual work + if (rx_pin != IO_NONE && (line & UART_INVERTER_LINE_RX)) { + inverterPinSet(rx_pin, enable); + } + if (tx_pin != IO_NONE && (line & UART_INVERTER_LINE_TX)) { + inverterPinSet(tx_pin, enable); + } +} + +#endif diff --git a/src/main/drivers/inverter.h b/src/main/drivers/uart_inverter.h similarity index 70% rename from src/main/drivers/inverter.h rename to src/main/drivers/uart_inverter.h index b03ce094c3..0acf1dddf5 100644 --- a/src/main/drivers/inverter.h +++ b/src/main/drivers/uart_inverter.h @@ -17,10 +17,12 @@ #pragma once -#if defined(INVERTER_PIN_UART1) || defined(INVERTER_PIN_UART2) || defined(INVERTER_PIN_UART3) || defined(INVERTER_PIN_UART4) || defined(INVERTER_PIN_UART5) || defined(INVERTER_PIN_UART6) -#define USE_INVERTER -#endif +typedef enum { + UART_INVERTER_LINE_NONE = 0, + UART_INVERTER_LINE_RX = 1 << 0, + UART_INVERTER_LINE_TX = 1 << 1, +} uartInverterLine_e; -void initInverters(void); +void uartInverterInit(void); -void enableInverter(USART_TypeDef *USARTx, bool on); +void uartInverterSet(USART_TypeDef *USARTx, uartInverterLine_e line, bool enable); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index dabbc43d1e..9d89d7ad98 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -48,7 +48,6 @@ #include "drivers/dma.h" #include "drivers/exti.h" #include "drivers/flash_m25p16.h" -#include "drivers/inverter.h" #include "drivers/io.h" #include "drivers/io_pca9685.h" #include "drivers/light_led.h" @@ -69,6 +68,7 @@ #include "drivers/system.h" #include "drivers/time.h" #include "drivers/timer.h" +#include "drivers/uart_inverter.h" #include "drivers/vcd.h" #include "drivers/io.h" #include "drivers/exti.h" @@ -375,8 +375,8 @@ void init(void) lightsInit(); #endif -#ifdef USE_INVERTER - initInverters(); +#ifdef USE_UART_INVERTER + uartInverterInit(); #endif // Initialize buses diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index b9d750d978..f697824321 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -41,10 +41,12 @@ #define BEEPER PB4 #define BEEPER_INVERTED +#define USE_UART_INVERTER #if defined(OMNIBUSF4V3) - #define INVERTER_PIN_UART6 PC8 + #define INVERTER_PIN_UART6_RX PC8 + #define INVERTER_PIN_UART6_TX PC9 #else - #define INVERTER_PIN_UART1 PC0 // PC0 has never been used as inverter control on genuine OMNIBUS F4 variants, but leave it as is since some clones actually implement it. + #define INVERTER_PIN_UART1_RX PC0 // PC0 has never been used as inverter control on genuine OMNIBUS F4 variants, but leave it as is since some clones actually implement it. #endif #define USE_I2C From 2b3e1df045fb85328d4afafaba90d31981752dcb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 1 Jul 2018 16:33:12 +0100 Subject: [PATCH 02/57] Change all targets to separate RX and TX inverters For now, assume all targets that define a single pin inverter intend to define an inverter affecting only the RX line. --- src/main/target/AIRBOTF4/target.h | 5 +++-- src/main/target/ANYFC/target.h | 5 +++-- src/main/target/BEEROTORF4/target.h | 8 ++++---- src/main/target/BETAFLIGHTF4/target.h | 7 +++---- src/main/target/BLUEJAYF4/target.h | 7 ++++--- src/main/target/CLRACINGF4AIR/target.h | 5 ++++- src/main/target/COLIBRI/target.h | 5 +++-- src/main/target/F4BY/target.h | 6 +++--- src/main/target/FF_F35_LIGHTNING/target.h | 5 +++-- src/main/target/FF_FORTINIF4/target.h | 4 +++- src/main/target/FF_PIKOF4/target.h | 6 ++++-- src/main/target/FISHDRONEF4/target.h | 6 +++--- src/main/target/FRSKYF4/target.h | 5 +++-- src/main/target/KAKUTEF4/target.h | 5 +++-- src/main/target/KROOZX/target.h | 7 ++++--- src/main/target/OMNIBUSF4/target.h | 17 +++++++++-------- src/main/target/REVO/target.h | 7 ++++--- src/main/target/SPARKY2/target.h | 5 +++-- src/main/target/SPRACINGF4EVO/target.h | 6 ++++-- src/main/target/YUPIF4/target.h | 5 +++-- 20 files changed, 73 insertions(+), 53 deletions(-) diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index dd97eac146..d0f0b7de12 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -25,8 +25,6 @@ #define BEEPER PB4 #define BEEPER_INVERTED -#define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO - #define USE_I2C #define USE_I2C_DEVICE_2 #define I2C_DEVICE_2_SHARES_UART3 @@ -85,10 +83,13 @@ #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define INVERTER_PIN_UART1_RX PC0 // PC0 used as inverter select GPIO #define USE_UART3 #define UART3_RX_PIN PB11 diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index 10396ee972..1fd4b700ec 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -27,8 +27,6 @@ #define BEEPER PB2 #define BEEPER_INVERTED -#define INVERTER_PIN_UART1 PC3 - #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 @@ -69,9 +67,12 @@ #define USE_VCP #define VBUS_SENSING_PIN PA8 +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 +#define INVERTER_PIN_UART1_RX PC3 #define USE_UART2 #define UART2_RX_PIN PA3 diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 1d5d7bddc6..1bf8dc2fa2 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -81,6 +81,8 @@ #define VBUS_SENSING_ENABLED #define VBUS_SENSING_PIN PC5 +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -90,15 +92,13 @@ #define USE_UART2 #define UART2_RX_PIN PA3 //Shared with PPM #define UART2_TX_PIN PA2 - -#define INVERTER_PIN_UART2 PC15 +#define INVERTER_PIN_UART2_RX PC15 //Telemetry #define USE_UART3 #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 - -#define INVERTER_PIN_UART3 PC14 +#define INVERTER_PIN_UART3_RX PC14 #define SERIAL_PORT_COUNT 4 diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 4687719145..c792cc7851 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -27,9 +27,6 @@ #define BEEPER PB4 #define BEEPER_INVERTED -// PC13 used as inverter select GPIO for UART2 -#define INVERTER_PIN_UART2 PC13 - #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 @@ -71,6 +68,7 @@ #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED +#define USE_UART_INVERTER #define USE_UART1 #define UART1_RX_PIN PA10 @@ -80,7 +78,8 @@ #define USE_UART2 #define UART2_RX_PIN PA3 #define UART2_TX_PIN PA2 - +// PC13 used as inverter select GPIO for UART2 +#define INVERTER_PIN_UART2_RX PC13 #define USE_UART3 #define UART3_RX_PIN PB11 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 6059d67eee..2b909cc83c 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -35,9 +35,6 @@ #define BEEPER_OPT PB7 #define BEEPER_INVERTED -#define INVERTER_PIN_UART6 PB15 -//#define INVERTER_PIN_UART1 PC9 - // MPU6500 interrupt #define USE_MPU_DATA_READY_SIGNAL #define GYRO_INT_EXTI PC5 @@ -92,9 +89,12 @@ //#define VBUS_SENSING_PIN PA8 //#define VBUS_SENSING_ENABLED +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 +//#define INVERTER_PIN_UART1_TX PC9 #define USE_UART3 #define UART3_RX_PIN PB11 @@ -103,6 +103,7 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PB15 #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 5 diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index 343a9e46ab..f0fae5da88 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -32,7 +32,6 @@ #define BEEPER PB4 #define BEEPER_INVERTED -#define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_3 @@ -78,10 +77,14 @@ #define WS2811_DMA_CHANNEL DMA_Channel_2 #define USE_VCP + +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define INVERTER_PIN_UART1_RX PC0 // PC0 used as inverter select GPIO #if defined( CLRACINGF4AIRV2) || defined(CLRACINGF4AIRV3) #define USE_UART2 diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index e288b37d62..1effbc56d7 100755 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -36,8 +36,6 @@ #define BEEPER PC5 -#define INVERTER_PIN_UART2 PB2 // PB2 used as inverter select GPIO - #define MPU6000_CS_PIN PC4 #define MPU6000_SPI_BUS BUS_SPI1 @@ -83,6 +81,8 @@ #define USE_VCP #define VBUS_SENSING_PIN PA9 +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PB7 #define UART1_TX_PIN PB6 @@ -91,6 +91,7 @@ #define USE_UART2 #define UART2_RX_PIN PA3 #define UART2_TX_PIN PA2 +#define INVERTER_PIN_UART2_RX PB2 // PB2 used as inverter select GPIO #define USE_UART3 #define UART3_RX_PIN PB11 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 4613437bab..da20cd9c33 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -27,9 +27,6 @@ #define BEEPER PE5 #define BEEPER_INVERTED -#define INVERTER_PIN_UART6 PD3 - - // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL @@ -74,6 +71,8 @@ #define USE_VCP #define VBUS_SENSING_PIN PA9 +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PB7 #define UART1_TX_PIN PB6 @@ -95,6 +94,7 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PD3 #define SERIAL_PORT_COUNT 6 //VCP, UART1, UART2, UART3, UART4, UART6 diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index 0215393c7a..7a82ab8244 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -61,6 +61,8 @@ #define USE_VCP // #define VBUS_SENSING_PIN PA9 +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -69,11 +71,10 @@ #define UART2_RX_PIN PA3 #define UART2_TX_PIN PA2 -#define INVERTER_PIN_UART3 PA8 - #define USE_UART3 #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 +#define INVERTER_PIN_UART3_RX PA8 #define USE_UART4 #define UART4_RX_PIN PC11 diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 084662d030..79e6de2ba5 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -78,6 +78,8 @@ #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -85,7 +87,7 @@ #define USE_UART3 #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 -#define INVERTER_PIN_UART3 PC15 +#define INVERTER_PIN_UART3_RX PC15 #define USE_UART4 #define UART4_TX_PIN PA0 diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index 89061bbfda..0d4a48d7f7 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -105,6 +105,8 @@ //#define VBUS_SENSING_PIN PA8 //#define VBUS_SENSING_ENABLED +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -113,9 +115,9 @@ #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 #if defined(FF_PIKOF4OSD) -#define INVERTER_PIN_UART3 PC3 +#define INVERTER_PIN_UART3_RX PC3 #else -#define INVERTER_PIN_UART3 PC8 +#define INVERTER_PIN_UART3_RX PC8 #endif #define USE_UART4 diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 666fc342ed..abe0b2bec1 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -28,9 +28,6 @@ // *************** BEEPER ***************************** #define BEEPER PC15 -// *************** INVERTER ***************************** -#define INVERTER_PIN_UART2 PB2 - // *************** SPI ***************************** #define USE_SPI @@ -114,6 +111,8 @@ // *************** UART ***************************** #define USE_VCP +#define USE_UART_INVERTER + // provide for Telemetry module #define USE_UART1 #define UART1_RX_PIN PA10 @@ -123,6 +122,7 @@ #define USE_UART2 #define UART2_RX_PIN PA3 #define UART2_TX_PIN PA2 +#define INVERTER_PIN_UART2_RX PB2 // provide for GPS module #define USE_UART5 diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index d91cf6fa52..d5862d9b69 100755 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -23,8 +23,6 @@ #define BEEPER PB4 #define BEEPER_INVERTED -#define INVERTER_PIN_UART6 PC8 - #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 @@ -61,6 +59,8 @@ #define USE_VCP #define VBUS_SENSING_PIN PC5 +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -72,6 +72,7 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PC8 #define SERIAL_PORT_COUNT 4 // VCP, UART1, UART3, UART6 diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 58162e0b2c..52ed20bf3d 100755 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -40,8 +40,6 @@ #define BEEPER PC9 #define BEEPER_INVERTED -#define INVERTER_PIN_UART3 PB15 - #define USE_EXTI #define GYRO_INT_EXTI PC5 #define USE_MPU_DATA_READY_SIGNAL @@ -94,6 +92,8 @@ #define VBUS_SENSING_PIN PA8 #define VBUS_SENSING_ENABLED +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -101,6 +101,7 @@ #define USE_UART3 #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 +#define INVERTER_PIN_UART3_RX PB15 #define USE_UART6 #define UART6_RX_PIN PC7 diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index ac006bcfc9..bd84473dd6 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -28,9 +28,6 @@ #define BEEPER PC1 #define BEEPER_INVERTED -#define INVERTER_PIN_UART1 PB13 -#define INVERTER_PIN_UART6 PB12 - #define MPU6000_CS_PIN PB2 #define MPU6000_SPI_BUS BUS_SPI1 @@ -93,9 +90,12 @@ #define USE_VCP +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 +#define INVERTER_PIN_UART1_RX PB13 #define USE_UART3 #define UART3_RX_PIN PB11 @@ -112,6 +112,7 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PB12 #define SERIAL_PORT_COUNT 6 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index f697824321..a99054c2ec 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -41,14 +41,6 @@ #define BEEPER PB4 #define BEEPER_INVERTED -#define USE_UART_INVERTER -#if defined(OMNIBUSF4V3) - #define INVERTER_PIN_UART6_RX PC8 - #define INVERTER_PIN_UART6_TX PC9 -#else - #define INVERTER_PIN_UART1_RX PC0 // PC0 has never been used as inverter control on genuine OMNIBUS F4 variants, but leave it as is since some clones actually implement it. -#endif - #define USE_I2C #define USE_I2C_DEVICE_2 #define I2C_DEVICE_2_SHARES_UART3 @@ -134,10 +126,15 @@ #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#if !defined(OMNIBUSF4V3) +#define INVERTER_PIN_UART1_RX PC0 // PC0 has never been used as inverter control on genuine OMNIBUS F4 variants, but leave it as is since some clones actually implement it. +#endif #define USE_UART3 #define UART3_RX_PIN PB11 @@ -146,6 +143,10 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 +#if defined(OMNIBUSF4V3) + #define INVERTER_PIN_UART6_RX PC8 + #define INVERTER_PIN_UART6_TX PC9 +#endif #if defined(OMNIBUSF4V3) #define USE_SOFTSERIAL1 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index d1c013267b..2ad89c28ee 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -32,9 +32,6 @@ #define BEEPER PB4 -// PC0 used as inverter select GPIO -#define INVERTER_PIN_UART1 PC0 - // MPU6000 interrupts #define USE_EXTI #define MPU6000_EXTI_PIN PC4 @@ -89,10 +86,14 @@ #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +// PC0 used as inverter select GPIO +#define INVERTER_PIN_UART1_RX PC0 #define USE_UART3 #define I2C_DEVICE_2_SHARES_UART3 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index a2cc4cbafe..a160af46b0 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -29,8 +29,6 @@ #define BEEPER PC9 -#define INVERTER_PIN_UART6 PC6 - #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -77,6 +75,8 @@ #define USE_VCP #define VBUS_SENSING_PIN PA8 +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -88,6 +88,7 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 //inverter +#define INVERTER_PIN_UART6_RX PC6 #define SERIAL_PORT_COUNT 4 diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index b33946b76e..f9f4638d50 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -31,8 +31,6 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define INVERTER_PIN_UART2 PB2 - #define USE_EXTI #define GYRO_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL @@ -69,6 +67,9 @@ #define USE_MAG_MAG3110 #define USE_VCP + +#define USE_UART_INVERTER + #define USE_UART1 #define USE_UART2 #define USE_UART3 @@ -81,6 +82,7 @@ #define UART2_TX_PIN PA2 #define UART2_RX_PIN PA3 +#define INVERTER_PIN_UART2_RX PB2 #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index b77a25577c..65c2bc43ad 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -47,8 +47,6 @@ #define BEEPER_PWM_FREQUENCY 3150 #endif -#define INVERTER_PIN_UART6 PB15 - #define USE_EXTI #define GYRO_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL @@ -109,6 +107,8 @@ #define USB_IO #define USE_VCP +#define USE_UART_INVERTER + #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -124,6 +124,7 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PB15 #define SERIAL_PORT_COUNT 5 From 851afb3ca2436c0c0f6904116c89ebd32154a1eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 1 Jul 2018 19:17:29 +0100 Subject: [PATCH 03/57] Remove unneeded and outdated include from serial_uart_hal driver Driver was including inverter.h without using it, but since that file no longer exists it wouldn't compile. --- src/main/drivers/serial_uart_hal.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index b93face72b..12540cc81f 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -31,7 +31,6 @@ #include "common/utils.h" #include "drivers/io.h" #include "drivers/nvic.h" -#include "inverter.h" #include "dma.h" #include "serial.h" From b846044322d80ded767353db31304749063f939d Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 3 Jul 2018 15:39:02 +0200 Subject: [PATCH 04/57] Fix flight timer for fixed wing when not using launch mode --- src/main/fc/fc_core.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0e1759ec41..1121978b1f 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -706,7 +706,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; - if (ARMING_FLAG(ARMED) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected()))) { + if (ARMING_FLAG(ARMED) && ((!STATE(FIXED_WING)) || (!isNavLaunchEnabled()) || (isNavLaunchEnabled() && isFixedWingLaunchDetected()))) { flightTime += cycleTime; } From 2b4c6acbb25c73b844e2bacbadee3336d67b244f Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 3 Jul 2018 15:58:36 +0200 Subject: [PATCH 05/57] Replace feature(FEATURE_CURRENT_METER) by isAmperageConfigured() where it makes sense The `CURRENT_METER` feature can be enableb but the current meter type could have been set to none --- src/main/fc/fc_tasks.c | 6 +++--- src/main/fc/stats.c | 2 +- src/main/sensors/battery.c | 4 ++-- src/main/telemetry/ibus_shared.c | 4 ++-- src/main/telemetry/mavlink.c | 2 +- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 2bebfb1a05..653bd6f468 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -101,12 +101,12 @@ void taskUpdateBattery(timeUs_t currentTimeUs) static timeUs_t batMonitoringLastServiced = 0; timeUs_t BatMonitoringTimeSinceLastServiced = cmpTimeUs(currentTimeUs, batMonitoringLastServiced); - if (feature(FEATURE_CURRENT_METER)) + if (isAmperageConfigured()) currentMeterUpdate(BatMonitoringTimeSinceLastServiced); #ifdef USE_ADC if (feature(FEATURE_VBAT)) batteryUpdate(BatMonitoringTimeSinceLastServiced); - if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { + if (feature(FEATURE_VBAT) && isAmperageConfigured()) { powerMeterUpdate(BatMonitoringTimeSinceLastServiced); sagCompensatedVBatUpdate(currentTimeUs, BatMonitoringTimeSinceLastServiced); } @@ -306,7 +306,7 @@ void fcTasksInit(void) #ifdef USE_LIGHTS setTaskEnabled(TASK_LIGHTS, true); #endif - setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); + setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || isAmperageConfigured()); setTaskEnabled(TASK_TEMPERATURE, true); setTaskEnabled(TASK_RX, true); #ifdef USE_GPS diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c index 6ce2c0cb8e..43534a62a0 100644 --- a/src/main/fc/stats.c +++ b/src/main/fc/stats.c @@ -57,7 +57,7 @@ void statsOnDisarm(void) statsConfigMutable()->stats_total_time += dt; //[s] statsConfigMutable()->stats_total_dist += (getTotalTravelDistance() - arm_distance_cm) / 100; //[m] #ifdef USE_ADC - if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { + if (feature(FEATURE_VBAT) && isAmperageConfigured()) { const uint32_t energy = getMWhDrawn() - arm_mWhDrawn; statsConfigMutable()->stats_total_energy += energy; flyingEnergy += energy; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index b7183b97c6..60b2da8475 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -253,7 +253,7 @@ void batteryUpdate(timeUs_t timeDelta) batteryCriticalVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMin; batteryFullWhenPluggedIn = batteryAdcToVoltage(vbatLatestADC) >= (batteryFullVoltage - batteryCellCount * VBATT_CELL_FULL_MAX_DIFF); - batteryUseCapacityThresholds = feature(FEATURE_CURRENT_METER) && batteryFullWhenPluggedIn && (currentBatteryProfile->capacity.value > 0) && + batteryUseCapacityThresholds = isAmperageConfigured() && batteryFullWhenPluggedIn && (currentBatteryProfile->capacity.value > 0) && (currentBatteryProfile->capacity.warning > 0) && (currentBatteryProfile->capacity.critical > 0); } @@ -559,7 +559,7 @@ uint8_t calculateBatteryPercentage(void) if (batteryState == BATTERY_NOT_PRESENT) return 0; - if (batteryFullWhenPluggedIn && feature(FEATURE_CURRENT_METER) && (currentBatteryProfile->capacity.value > 0) && (currentBatteryProfile->capacity.critical > 0)) { + if (batteryFullWhenPluggedIn && isAmperageConfigured() && (currentBatteryProfile->capacity.value > 0) && (currentBatteryProfile->capacity.critical > 0)) { uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical; return constrain(batteryRemainingCapacity * 100 / capacityDiffBetweenFullAndEmpty, 0, 100); } else diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 1492a6977e..c2525fd0fa 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -158,10 +158,10 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { return sendIbusMeasurement2(address, getBatteryVoltage()); } } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_CURRENT) { //CURR in 10*mA, 1 = 10 mA - if (feature(FEATURE_CURRENT_METER)) return sendIbusMeasurement2(address, (uint16_t) getAmperage()); //int32_t + if (isAmperageConfigured()) return sendIbusMeasurement2(address, (uint16_t) getAmperage()); //int32_t else return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_FUEL) { //capacity in mAh - if (feature(FEATURE_CURRENT_METER)) return sendIbusMeasurement2(address, (uint16_t) getMAhDrawn()); //int32_t + if (isAmperageConfigured()) return sendIbusMeasurement2(address, (uint16_t) getMAhDrawn()); //int32_t else return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_CLIMB) { return sendIbusMeasurement2(address, (int16_t) (getEstimatedActualVelocity(Z))); // diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 73b39c3bf3..17e23c8e0a 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -226,7 +226,7 @@ void mavlinkSendSystemStatus(void) // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0, // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - feature(FEATURE_CURRENT_METER) ? getAmperage() : -1, + isAmperageConfigured() ? getAmperage() : -1, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) From 884acf7cafd566521056dbfa5be8aab37ddce1ef Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 3 Jul 2018 23:53:40 +0200 Subject: [PATCH 06/57] Make failsafe_throttle_low_delay default 0 --- src/main/flight/failsafe.c | 2 +- src/main/target/FALCORE/config.c | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 46fc04fdd2..e9f4b50661 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -71,7 +71,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_recovery_delay = 5, // 0.5 seconds (plus 200ms explicit delay) .failsafe_off_delay = 200, // 20sec .failsafe_throttle = 1000, // default throttle off. - .failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition + .failsafe_throttle_low_delay = 0, // default throttle low delay for "just disarm" on failsafe condition .failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING, // default full failsafe procedure .failsafe_fw_roll_angle = -200, // 20 deg left .failsafe_fw_pitch_angle = 100, // 10 deg dive (yes, positive means dive) diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index 322f3ba5d7..4d9f1c1363 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -93,7 +93,6 @@ void targetConfiguration(void) failsafeConfigMutable()->failsafe_recovery_delay = 5; failsafeConfigMutable()->failsafe_off_delay = 200; failsafeConfigMutable()->failsafe_throttle = 1200; - failsafeConfigMutable()->failsafe_throttle_low_delay = 100; failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_RTH; boardAlignmentMutable()->rollDeciDegrees = 0; From c1fc5480d0b506036f1be98670e27d891636ecdf Mon Sep 17 00:00:00 2001 From: giacomo892 Date: Wed, 4 Jul 2018 15:33:18 +0200 Subject: [PATCH 07/57] Remove posZ constrain --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4d3999ea43..fe1b401f15 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1816,7 +1816,7 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo } #if defined(NAV_BLACKBOX) - navLatestActualPosition[Z] = constrain(navGetCurrentActualPositionAndVelocity()->pos.z, -32678, 32767); + navLatestActualPosition[Z] = navGetCurrentActualPositionAndVelocity()->pos.z; navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767); #endif } From 4920fb3c7603dbb9421cff89b3be90765990c19b Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 4 Jul 2018 18:48:29 +0200 Subject: [PATCH 08/57] Remove trailing space in CLI servo output lines --- src/main/fc/cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 91aaf2e38b..908dbcfb0c 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1263,7 +1263,7 @@ static void cliModeColor(char *cmdline) static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const servoParam_t *defaultServoParam) { // print out servo settings - const char *format = "servo %u %d %d %d %d "; + const char *format = "servo %u %d %d %d %d"; for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { const servoParam_t *servoConf = &servoParam[i]; bool equalsDefault = false; From afc56027589d0dfa852d7e20f6aada0324390132 Mon Sep 17 00:00:00 2001 From: Tim Eckel Date: Wed, 4 Jul 2018 15:18:19 -0400 Subject: [PATCH 09/57] frsky telemetry hdop tweak --- src/main/telemetry/frsky.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 070fa3a702..cd8d5e78b8 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -70,8 +70,8 @@ uint16_t frskyGetGPSState(void) // ones and tens columns (# of satellites 0 - 99) tmpi += constrain(gpsSol.numSat, 0, 99); - // hundreds column (satellite accuracy HDOP: 0 = worst [HDOP 6.5], 9 = best [HDOP 2.0]) - tmpi += (9 - constrain((gpsSol.hdop - 151) / 50, 0, 9)) * 100; + // hundreds column (satellite accuracy HDOP: 0 = worst [HDOP > 5.5], 9 = best [HDOP <= 1.0]) + tmpi += (9 - constrain((gpsSol.hdop - 51) / 50, 0, 9)) * 100; // thousands column (GPS fix status) if (STATE(GPS_FIX)) From 365d5c8f03ee7807b2c324dce7c21a3d0766cd0f Mon Sep 17 00:00:00 2001 From: giacomo892 Date: Thu, 5 Jul 2018 14:27:04 +0200 Subject: [PATCH 10/57] Log estimated wind speed to BB slow frames --- src/main/blackbox/blackbox.c | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 69c9510254..154542f860 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -75,6 +75,7 @@ #include "sensors/pitotmeter.h" #include "sensors/rangefinder.h" #include "sensors/sensors.h" +#include "flight/wind_estimator.h" #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) @@ -339,6 +340,9 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"horizontalWindSpeed", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"verticalWindSpeed", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"windDirection", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, }; typedef enum BlackboxState { @@ -436,6 +440,9 @@ typedef struct blackboxSlowState_s { int32_t hwHealthStatus; uint16_t powerSupplyImpedance; uint16_t sagCompensatedVBat; + int16_t horizontalWindSpeed; + int16_t verticalWindSpeed; + uint16_t windDirection; } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From rc_controls.c @@ -1021,6 +1028,10 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance); blackboxWriteUnsignedVB(slowHistory.sagCompensatedVBat); + blackboxWriteUnsignedVB(slowHistory.horizontalWindSpeed); + blackboxWriteUnsignedVB(slowHistory.verticalWindSpeed); + blackboxWriteUnsignedVB(slowHistory.windDirection); + blackboxSlowFrameIterationTimer = 0; } @@ -1043,6 +1054,8 @@ static void loadSlowState(blackboxSlowState_t *slow) (getHwPitotmeterStatus() << 2 * 6); slow->powerSupplyImpedance = getPowerSupplyImpedance(); slow->sagCompensatedVBat = getBatterySagCompensatedVoltage(); + slow->horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&slow->windDirection); + slow->verticalWindSpeed = getEstimatedWindSpeed(Z); } /** From a046a780ce87eeb967e6e988e8c92a57e9c8a1dc Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sun, 1 Jul 2018 02:47:59 +0200 Subject: [PATCH 11/57] Fix battery voltage throttle compensation --- docs/Battery.md | 12 +++++++++++- docs/Cli.md | 1 + src/main/fc/settings.yaml | 4 ++++ src/main/flight/mixer.c | 2 +- src/main/sensors/battery.c | 6 ++++-- src/main/sensors/battery.h | 2 ++ 6 files changed, 23 insertions(+), 4 deletions(-) diff --git a/docs/Battery.md b/docs/Battery.md index e4c53ceafa..6889175d39 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -311,4 +311,14 @@ If `---` is displayed during flight instead of the remaining flight time/distanc ## Automatic throttle compensation based on battery voltage -Automatic throttle compensation based on battery voltage can be used by enabling the `THR_VBAT_COMP` feature. It is working like this: used_throttle = requested_throttle * battery_max_voltage / sag_compensated_voltage. +This features aims to compensate the throttle to get constant thrust with the same throttle request despite the battery voltage going down during flight. It can be used by enabling the `THR_VBAT_COMP` feature. This feature needs the sag compensated voltage which needs a current sensor (real or virtual) to be calculated. + +It is working like this: `used_throttle = requested_throttle * (1 + (battery_full_voltage / sag_compensated_voltage - 1) * thr_comp_weight)`. + +The default `thr_comp_weight` of 1 should be close to idal but if you want to tune this feature you need to find the difference in throttle value to achieve the same thrust (same power) when your battery is full and when your battery is almost empty then set `thr_comp_weight` to `(empty_battery_throttle / full_battery_throttle - 1) / (battery_full_voltage / battery_empty_sag_compensated_voltage - 1)` + +Example: + If the drawn power is 100W when the battery is full (12.6V) with 53% throttle and the drawn power is 100W with 58% throttle when the battery is almost empty with the sag compensated voltage being 11.0V `thr_comp_weight` needs to be set to this value to compensate the throttle automatically: + `(58 / 53 - 1) / (12.6 / 11.0 - 1) = 0.649` + +Known limitation: it doesn't work in 3D mode (3D feature) diff --git a/docs/Cli.md b/docs/Cli.md index 0ffd3425ec..fb0576308d 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -420,6 +420,7 @@ Re-apply any new defaults as desired. | vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | | motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | | motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | +| thr_comp_weight | 0.692 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | This Markdown table is made by MarkdwonTableMaker addon for google spreadsheet. Original Spreadsheet used to make this table can be found here https://docs.google.com/spreadsheets/d/1ubjYdMGmZ2aAMUNYkdfe3hhIF7wRfIjcuPOi_ysmp00/edit?usp=sharing diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5f31e01918..1f50a04854 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -545,6 +545,10 @@ groups: - name: rth_energy_margin min: 0 max: 100 + - name: thr_comp_weight + field: throttle_compensation_weight + min: 0 + max: 2 - name: PG_BATTERY_PROFILES type: batteryProfile_t diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a5a85ffc13..a216fa1b14 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -312,7 +312,7 @@ void mixTable(const float dT) // Throttle compensation based on battery voltage if (feature(FEATURE_THR_VBAT_COMP) && feature(FEATURE_VBAT) && isAmperageConfigured()) - throttleCommand = MIN(throttleCommand * calculateThrottleCompensationFactor(), throttleMax); + throttleCommand = MIN(throttleMin + (throttleCommand - throttleMin) * calculateThrottleCompensationFactor(), throttleMax); } throttleRange = throttleMax - throttleMin; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 19b2bf60ea..a329ad21e1 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -133,7 +133,9 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, .cruise_power = 0, .idle_power = 0, - .rth_energy_margin = 5 + .rth_energy_margin = 5, + + .throttle_compensation_weight = 1.0f ); @@ -358,7 +360,7 @@ uint16_t getBatterySagCompensatedVoltage(void) float calculateThrottleCompensationFactor(void) { - return batteryFullVoltage / sagCompensatedVBat; + return 1.0f + ((float)batteryFullVoltage / sagCompensatedVBat - 1.0f) * batteryMetersConfig()->throttle_compensation_weight; } uint16_t getBatteryVoltageLatestADC(void) diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 3fb063264f..c796915a84 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -76,6 +76,8 @@ typedef struct batteryMetersConfig_s { uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW) uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH + float throttle_compensation_weight; + } batteryMetersConfig_t; typedef struct batteryProfile_s { From 4baf5fdadf836c179631f03720eea4334e049f78 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sun, 1 Jul 2018 18:59:36 +0200 Subject: [PATCH 12/57] Fix PID controllers output display and logging scaling --- src/main/blackbox/blackbox.c | 27 +++++++++++++++++++-------- src/main/io/osd.c | 33 +++++++++++++++++---------------- 2 files changed, 36 insertions(+), 24 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 69c9510254..a83bd603f9 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -193,6 +193,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)}, {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)}, {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)}, + {"fwAltP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, {"fwAltI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, {"fwAltD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, @@ -201,6 +202,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"fwPosI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, {"fwPosD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, {"fwPosOut", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"mcPosAxisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, {"mcPosAxisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, {"mcPosAxisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, @@ -1256,25 +1258,34 @@ static void loadMainState(timeUs_t currentTimeUs) #endif #ifdef USE_NAV if (!STATE(FIXED_WING)) { - blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained / 10); - blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional / 10); - blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral / 10); - blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative / 10); + + // log requested velocity in cm/s + blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained); + + // log requested acceleration in cm/s^2 and throttle adjustment in µs + blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional); + blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral); + blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative); + } #endif } #ifdef USE_NAV if (STATE(FIXED_WING)) { - blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional / 10); - blackboxCurrent->fwAltPID[1] = lrintf(nav_pids->fw_alt.integral / 10); - blackboxCurrent->fwAltPID[2] = lrintf(nav_pids->fw_alt.derivative / 10); - blackboxCurrent->fwAltPIDOutput = lrintf(nav_pids->fw_alt.output_constrained / 10); + // log requested pitch in decidegrees + blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional); + blackboxCurrent->fwAltPID[1] = lrintf(nav_pids->fw_alt.integral); + blackboxCurrent->fwAltPID[2] = lrintf(nav_pids->fw_alt.derivative); + blackboxCurrent->fwAltPIDOutput = lrintf(nav_pids->fw_alt.output_constrained); + + // log requested roll in decidegrees blackboxCurrent->fwPosPID[0] = lrintf(nav_pids->fw_nav.proportional / 10); blackboxCurrent->fwPosPID[1] = lrintf(nav_pids->fw_nav.integral / 10); blackboxCurrent->fwPosPID[2] = lrintf(nav_pids->fw_nav.derivative / 10); blackboxCurrent->fwPosPIDOutput = lrintf(nav_pids->fw_nav.output_constrained / 10); + } else { blackboxCurrent->mcSurfacePID[0] = lrintf(nav_pids->surface.proportional / 10); blackboxCurrent->mcSurfacePID[1] = lrintf(nav_pids->surface.integral / 10); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8f4e383247..0a2b5942f8 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1036,17 +1036,17 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) #endif -void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController) { +static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { strcpy(buff, label); - uint8_t label_len = strlen(label); - for (uint8_t i = label_len; i < 5; ++i) buff[i] = ' '; - osdFormatCentiNumber(buff + 5, pidController->proportional, 0, 1, 0, 4); + for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' '; + uint8_t decimals = showDecimal ? 1 : 0; + osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4); buff[9] = ' '; - osdFormatCentiNumber(buff + 10, pidController->integrator, 0, 1, 0, 4); + osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4); buff[14] = ' '; - osdFormatCentiNumber(buff + 15, pidController->derivative, 0, 1, 0, 4); + osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4); buff[19] = ' '; - osdFormatCentiNumber(buff + 20, pidController->output_constrained, 0, 1, 0, 4); + osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4); buff[24] = '\0'; } @@ -1888,35 +1888,35 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_FW_ALT_PID_OUTPUTS: { const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt); + osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt, 10, true); // display requested pitch degrees break; } case OSD_FW_POS_PID_OUTPUTS: { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav); + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); // display requested roll degrees + osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav, 1, true); break; } case OSD_MC_VEL_Z_PID_OUTPUTS: { const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z]); + osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z], 100, false); // display throttle adjustment µs break; } case OSD_MC_VEL_X_PID_OUTPUTS: { const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X]); + osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X], 100, false); // display requested acceleration cm/s^2 break; } case OSD_MC_VEL_Y_PID_OUTPUTS: { const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y]); + osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y], 100, false); // display requested acceleration cm/s^2 break; } @@ -1924,11 +1924,12 @@ static bool osdDrawSingleElement(uint8_t item) { const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); strcpy(buff, "POSO "); - osdFormatCentiNumber(buff + 5, nav_pids->pos[X].output_constrained, 0, 1, 0, 4); + // display requested velocity cm/s + tfp_sprintf(buff + 5, "%4d", lrintf(nav_pids->pos[X].output_constrained * 100)); buff[9] = ' '; - osdFormatCentiNumber(buff + 10, nav_pids->pos[Y].output_constrained, 0, 1, 0, 4); + tfp_sprintf(buff + 10, "%4d", lrintf(nav_pids->pos[Y].output_constrained * 100)); buff[14] = ' '; - osdFormatCentiNumber(buff + 15, nav_pids->pos[Z].output_constrained, 0, 1, 0, 4); + tfp_sprintf(buff + 15, "%4d", lrintf(nav_pids->pos[Z].output_constrained * 100)); buff[19] = '\0'; break; } From e9ac8b91f96dac19753fa6c23ad66dd7e5af3fca Mon Sep 17 00:00:00 2001 From: giacomo892 Date: Fri, 6 Jul 2018 09:40:19 +0200 Subject: [PATCH 13/57] decrease CPU load on wind logging --- src/main/blackbox/blackbox.c | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 154542f860..090c4dc517 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -340,9 +340,9 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"horizontalWindSpeed", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"verticalWindSpeed", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"windDirection", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"windX", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"windY", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"windZ", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, }; typedef enum BlackboxState { @@ -440,9 +440,9 @@ typedef struct blackboxSlowState_s { int32_t hwHealthStatus; uint16_t powerSupplyImpedance; uint16_t sagCompensatedVBat; - int16_t horizontalWindSpeed; - int16_t verticalWindSpeed; - uint16_t windDirection; + int16_t windX; + int16_t windY; + int16_t windZ; } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From rc_controls.c @@ -1028,9 +1028,9 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance); blackboxWriteUnsignedVB(slowHistory.sagCompensatedVBat); - blackboxWriteUnsignedVB(slowHistory.horizontalWindSpeed); - blackboxWriteUnsignedVB(slowHistory.verticalWindSpeed); - blackboxWriteUnsignedVB(slowHistory.windDirection); + blackboxWriteUnsignedVB(slowHistory.windX); + blackboxWriteUnsignedVB(slowHistory.windY); + blackboxWriteUnsignedVB(slowHistory.windZ); blackboxSlowFrameIterationTimer = 0; } @@ -1054,8 +1054,9 @@ static void loadSlowState(blackboxSlowState_t *slow) (getHwPitotmeterStatus() << 2 * 6); slow->powerSupplyImpedance = getPowerSupplyImpedance(); slow->sagCompensatedVBat = getBatterySagCompensatedVoltage(); - slow->horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&slow->windDirection); - slow->verticalWindSpeed = getEstimatedWindSpeed(Z); + slow->windX = getEstimatedWindSpeed(X); + slow->windY = getEstimatedWindSpeed(Y); + slow->windZ = getEstimatedWindSpeed(Z); } /** From 950f96f8cc22280f502ed55ce097687e5b85a5a5 Mon Sep 17 00:00:00 2001 From: giacomo892 Date: Fri, 6 Jul 2018 17:16:26 +0200 Subject: [PATCH 14/57] more optimization --- src/main/blackbox/blackbox.c | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 090c4dc517..7e9edabf9e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -340,9 +340,9 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"windX", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"windY", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"windZ", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"wind", 0, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"wind", 1, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"wind", 2, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, }; typedef enum BlackboxState { @@ -440,9 +440,7 @@ typedef struct blackboxSlowState_s { int32_t hwHealthStatus; uint16_t powerSupplyImpedance; uint16_t sagCompensatedVBat; - int16_t windX; - int16_t windY; - int16_t windZ; + int16_t wind[XYZ_AXIS_COUNT]; } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From rc_controls.c @@ -1027,10 +1025,8 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance); blackboxWriteUnsignedVB(slowHistory.sagCompensatedVBat); - - blackboxWriteUnsignedVB(slowHistory.windX); - blackboxWriteUnsignedVB(slowHistory.windY); - blackboxWriteUnsignedVB(slowHistory.windZ); + + blackboxWriteSigned16VBArray(slowHistory.wind, XYZ_AXIS_COUNT); blackboxSlowFrameIterationTimer = 0; } @@ -1054,9 +1050,12 @@ static void loadSlowState(blackboxSlowState_t *slow) (getHwPitotmeterStatus() << 2 * 6); slow->powerSupplyImpedance = getPowerSupplyImpedance(); slow->sagCompensatedVBat = getBatterySagCompensatedVoltage(); - slow->windX = getEstimatedWindSpeed(X); - slow->windY = getEstimatedWindSpeed(Y); - slow->windZ = getEstimatedWindSpeed(Z); + + for (int i = 0; i < XYZ_AXIS_COUNT; i++) + { + slow->wind[i] = getEstimatedWindSpeed(i); + } + } /** From cd96a6f88cf38a4076d43485d0a8f96da2ad7fff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Pomezn=C3=BD?= Date: Sat, 7 Jul 2018 02:51:39 +0200 Subject: [PATCH 15/57] fixed broken PWM_SERVO_DRIVER --- src/main/drivers/io_pca9685.c | 1 - src/main/target/common_hardware.c | 9 +++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/io_pca9685.c b/src/main/drivers/io_pca9685.c index 86ebfac123..12803c0176 100644 --- a/src/main/drivers/io_pca9685.c +++ b/src/main/drivers/io_pca9685.c @@ -13,7 +13,6 @@ #include "drivers/bus.h" #include "drivers/bus_i2c.h" -#define PCA9685_ADDR 0x40 #define PCA9685_MODE1 0x00 #define PCA9685_PRESCALE 0xFE diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 5bbcd4fe52..0d5a6b791b 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -228,4 +228,13 @@ BUSDEV_REGISTER_I2C(busdev_ug2864, DEVHW_UG2864, UG2864_I2C_BUS, 0x3C, NONE, DEVFLAGS_NONE); #endif +#if defined(USE_PMW_SERVO_DRIVER) + #if defined(USE_PWM_DRIVER_PCA9685) + #if !defined(PCA9685_I2C_BUS) + #define PCA9685_I2C_BUS BUS_I2C1 + #endif + BUSDEV_REGISTER_I2C(busdev_pca9685, DEVHW_PCA9685, PCA9685_I2C_BUS, 0x40, NONE, DEVFLAGS_NONE); + #endif +#endif + #endif // USE_TARGET_HARDWARE_DESCRIPTORS From a056c2dd17b56c58010789ee7ac4b2391f2c368f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Pomezn=C3=BD?= Date: Sat, 7 Jul 2018 08:42:22 +0200 Subject: [PATCH 16/57] aditional I2C define check --- src/main/target/common_hardware.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 0d5a6b791b..63c6c15c61 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -229,7 +229,7 @@ #endif #if defined(USE_PMW_SERVO_DRIVER) - #if defined(USE_PWM_DRIVER_PCA9685) + #if defined(USE_PWM_DRIVER_PCA9685) && defined(USE_I2C) #if !defined(PCA9685_I2C_BUS) #define PCA9685_I2C_BUS BUS_I2C1 #endif From c146a3f833d05547236de66f63fef14b1c749466 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Sat, 7 Jul 2018 15:08:20 +0200 Subject: [PATCH 17/57] Fixed PWM9 and PWM10 for KFC32F3_INAV target --- src/main/target/KFC32F3_INAV/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/KFC32F3_INAV/target.c b/src/main/target/KFC32F3_INAV/target.c index a1b560b374..7f1edfe84b 100755 --- a/src/main/target/KFC32F3_INAV/target.c +++ b/src/main/target/KFC32F3_INAV/target.c @@ -33,8 +33,8 @@ const timerHardware_t timerHardware[] = { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_IPD, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 { TIM2, IO_TAG(PB10), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM7 { TIM2, IO_TAG(PB11), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_SERVO | TIM_USE_ANY | TIM_USE_LED }, // S1_out - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_IPD, GPIO_AF_1, TIM_USE_MC_SERVO | TIM_USE_ANY }, // S2_out + { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_ANY | TIM_USE_LED }, // S1_out + { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_IPD, GPIO_AF_1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO |TIM_USE_ANY }, // S2_out }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); From a30fde56e31194a2db0abc97db94158022c9bd23 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Sat, 7 Jul 2018 15:36:10 +0200 Subject: [PATCH 18/57] Fixed ground course fro eleres telemetry --- src/main/rx/eleres.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/rx/eleres.c b/src/main/rx/eleres.c index 3c9d8120cd..bf17950f36 100755 --- a/src/main/rx/eleres.c +++ b/src/main/rx/eleres.c @@ -325,7 +325,7 @@ static void telemetryRX(void) case 2: if (sensors(SENSOR_GPS)) { uint16_t gpsspeed = (gpsSol.groundSpeed*9L)/250L; - int16_t course = (gpsSol.groundCourse+360)%360; + int16_t course = (gpsSol.groundCourse/10 + 360)%360; #ifdef USE_NAV int32_t alt = getEstimatedActualPosition(Z); #else From ea685fe2f59cf48724d5beaafea97e71e6b45d82 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Sat, 7 Jul 2018 16:15:26 +0200 Subject: [PATCH 19/57] Add definition for MAX_SPI_SPEED to limit spi seed if defined for target --- src/main/drivers/bus_busdev_spi.c | 6 ++++++ src/main/target/KFC32F3_INAV/target.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/src/main/drivers/bus_busdev_spi.c b/src/main/drivers/bus_busdev_spi.c index 74ff0db899..cbed43a4bf 100755 --- a/src/main/drivers/bus_busdev_spi.c +++ b/src/main/drivers/bus_busdev_spi.c @@ -33,6 +33,12 @@ void spiBusSetSpeed(const busDevice_t * dev, busSpeed_e speed) { const SPIClockSpeed_e spiClock[] = { SPI_CLOCK_INITIALIZATON, SPI_CLOCK_SLOW, SPI_CLOCK_STANDARD, SPI_CLOCK_FAST, SPI_CLOCK_ULTRAFAST }; SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); + +#ifdef MAX_SPI_SPEED + if (speed > MAX_SPI_SPEED) + speed = MAX_SPI_SPEED; +#endif + spiSetSpeed(instance, spiClock[speed]); } diff --git a/src/main/target/KFC32F3_INAV/target.h b/src/main/target/KFC32F3_INAV/target.h index 2fc9cf9f7b..cb2930e67b 100755 --- a/src/main/target/KFC32F3_INAV/target.h +++ b/src/main/target/KFC32F3_INAV/target.h @@ -34,6 +34,8 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW +#define MAX_SPI_SPEED BUS_SPEED_SLOW + #define MPU6000_CS_PIN PB5 #define MPU6000_SPI_BUS BUS_SPI2 From 6d3a338f931a4825e4e0bd5bb543943bc9c2e013 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Sat, 7 Jul 2018 23:00:19 +0200 Subject: [PATCH 20/57] MAX_SPI_SPEED changed to BUS_SPI_SPEED_MAX --- src/main/drivers/bus_busdev_spi.c | 6 +++--- src/main/target/KFC32F3_INAV/target.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/bus_busdev_spi.c b/src/main/drivers/bus_busdev_spi.c index cbed43a4bf..6e9670e8a5 100755 --- a/src/main/drivers/bus_busdev_spi.c +++ b/src/main/drivers/bus_busdev_spi.c @@ -34,9 +34,9 @@ void spiBusSetSpeed(const busDevice_t * dev, busSpeed_e speed) const SPIClockSpeed_e spiClock[] = { SPI_CLOCK_INITIALIZATON, SPI_CLOCK_SLOW, SPI_CLOCK_STANDARD, SPI_CLOCK_FAST, SPI_CLOCK_ULTRAFAST }; SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); -#ifdef MAX_SPI_SPEED - if (speed > MAX_SPI_SPEED) - speed = MAX_SPI_SPEED; +#ifdef BUS_SPI_SPEED_MAX + if (speed > BUS_SPI_SPEED_MAX) + speed = BUS_SPI_SPEED_MAX; #endif spiSetSpeed(instance, spiClock[speed]); diff --git a/src/main/target/KFC32F3_INAV/target.h b/src/main/target/KFC32F3_INAV/target.h index cb2930e67b..2df606f1b0 100755 --- a/src/main/target/KFC32F3_INAV/target.h +++ b/src/main/target/KFC32F3_INAV/target.h @@ -34,7 +34,7 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define MAX_SPI_SPEED BUS_SPEED_SLOW +#define BUS_SPI_SPEED_MAX BUS_SPEED_SLOW #define MPU6000_CS_PIN PB5 #define MPU6000_SPI_BUS BUS_SPI2 From e311f4ac1e7d4072d7acadd1b05de879adb47cb6 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sun, 1 Jul 2018 17:39:18 +0200 Subject: [PATCH 21/57] Fix current BB logging and improve battery code --- src/main/blackbox/blackbox.c | 6 +-- src/main/sensors/battery.c | 83 ++++++++++++++---------------------- src/main/sensors/battery.h | 2 - 3 files changed, 35 insertions(+), 56 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5bb6d18fac..ceb978a046 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1139,7 +1139,7 @@ void blackboxStart(void) blackboxHistory[1] = &blackboxHistoryRing[1]; blackboxHistory[2] = &blackboxHistoryRing[2]; - vbatReference = getBatteryVoltageLatestADC(); + vbatReference = getBatteryRawVoltage(); //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it @@ -1301,8 +1301,8 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->motor[i] = motor[i]; } - blackboxCurrent->vbatLatest = getBatteryVoltageLatestADC(); - blackboxCurrent->amperageLatest = getAmperageLatestADC(); + blackboxCurrent->vbatLatest = getBatteryRawVoltage(); + blackboxCurrent->amperageLatest = getAmperage(); #ifdef USE_BARO blackboxCurrent->BaroAlt = baro.BaroAlt; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 19b2bf60ea..c15d6c0e6b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -78,8 +78,6 @@ static bool batteryFullWhenPluggedIn = false; static bool profileAutoswitchDisable = false; static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) -static uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC -static uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm static uint16_t sagCompensatedVBat = 0; // calculated no load vbat static bool powerSupplyImpedanceIsValid = false; @@ -137,19 +135,6 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, ); -uint16_t batteryAdcToVoltage(uint16_t src) -{ - // calculate battery voltage based on ADC reading - // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k) - return((uint64_t)src * batteryMetersConfig()->voltage_scale * ADCVREF / (0xFFF * 1000)); -} - -int16_t currentSensorToCentiamps(uint16_t src) -{ - int32_t microvolts = ((uint32_t)src * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100; - return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps -} - void batteryInit(void) { batteryState = BATTERY_NOT_PRESENT; @@ -184,9 +169,8 @@ static int8_t profileDetect() { qsort(profile_comp_array, MAX_BATTERY_PROFILE_COUNT, sizeof(*profile_comp_array), (int (*)(const void *, const void *))profile_compare); // Return index of the first profile where vbat <= profile_max_voltage - uint16_t vbatLatest = batteryAdcToVoltage(vbatLatestADC); for (uint8_t i = 0; i < MAX_BATTERY_PROFILE_COUNT; ++i) - if ((profile_comp_array[i].max_voltage > 0) && (vbatLatest <= profile_comp_array[i].max_voltage)) + if ((profile_comp_array[i].max_voltage > 0) && (vbat <= profile_comp_array[i].max_voltage)) return profile_comp_array[i].profile_index; // No matching profile found @@ -206,24 +190,26 @@ void activateBatteryProfile(void) batteryInit(); } -static void updateBatteryVoltage(timeUs_t timeDelta) +static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) { - uint16_t vbatSample; static pt1Filter_t vbatFilterState; - // store the battery voltage with some other recent battery voltage readings - vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); - vbatSample = pt1FilterApply4(&vbatFilterState, vbatSample, VBATT_LPF_FREQ, timeDelta * 1e-6f); - vbat = batteryAdcToVoltage(vbatSample); + // calculate battery voltage based on ADC reading + // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k) + vbat = (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage_scale * ADCVREF / (0xFFF * 1000); + + if (justConnected) { + pt1FilterReset(&vbatFilterState, vbat); + } else { + vbat = pt1FilterApply4(&vbatFilterState, vbat, VBATT_LPF_FREQ, timeDelta * 1e-6f); + } } void batteryUpdate(timeUs_t timeDelta) { - updateBatteryVoltage(timeDelta); - /* battery has just been connected*/ - if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) - { + if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) { + /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ batteryState = BATTERY_OK; /* wait for VBatt to stabilise then we can calc number of cells @@ -231,7 +217,7 @@ void batteryUpdate(timeUs_t timeDelta) We only do this on the ground so don't care if we do block, not worse than original code anyway*/ delay(VBATT_STABLE_DELAY); - updateBatteryVoltage(timeDelta); + updateBatteryVoltage(timeDelta, true); int8_t detectedProfileIndex = -1; if (feature(FEATURE_BAT_PROFILE_AUTOSWITCH) && (!profileAutoswitchDisable)) @@ -244,7 +230,7 @@ void batteryUpdate(timeUs_t timeDelta) } else if (currentBatteryProfile->cells > 0) batteryCellCount = currentBatteryProfile->cells; else { - batteryCellCount = (batteryAdcToVoltage(vbatLatestADC) / currentBatteryProfile->voltage.cellDetect) + 1; + batteryCellCount = (vbat / currentBatteryProfile->voltage.cellDetect) + 1; if (batteryCellCount > 8) batteryCellCount = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) } @@ -252,17 +238,20 @@ void batteryUpdate(timeUs_t timeDelta) batteryWarningVoltage = batteryCellCount * currentBatteryProfile->voltage.cellWarning; batteryCriticalVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMin; - batteryFullWhenPluggedIn = batteryAdcToVoltage(vbatLatestADC) >= (batteryFullVoltage - batteryCellCount * VBATT_CELL_FULL_MAX_DIFF); + batteryFullWhenPluggedIn = vbat >= (batteryFullVoltage - batteryCellCount * VBATT_CELL_FULL_MAX_DIFF); batteryUseCapacityThresholds = isAmperageConfigured() && batteryFullWhenPluggedIn && (currentBatteryProfile->capacity.value > 0) && (currentBatteryProfile->capacity.warning > 0) && (currentBatteryProfile->capacity.critical > 0); - } - /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ - else if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD) { - batteryState = BATTERY_NOT_PRESENT; - batteryCellCount = 0; - batteryWarningVoltage = 0; - batteryCriticalVoltage = 0; + } else { + updateBatteryVoltage(timeDelta, false); + + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ + if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD) { + batteryState = BATTERY_NOT_PRESENT; + batteryCellCount = 0; + batteryWarningVoltage = 0; + batteryCriticalVoltage = 0; + } } if (batteryState != BATTERY_NOT_PRESENT) { @@ -361,11 +350,6 @@ float calculateThrottleCompensationFactor(void) return batteryFullVoltage / sagCompensatedVBat; } -uint16_t getBatteryVoltageLatestADC(void) -{ - return vbatLatestADC; -} - uint16_t getBatteryWarningVoltage(void) { return batteryWarningVoltage; @@ -415,11 +399,6 @@ int16_t getAmperage(void) return amperage; } -int16_t getAmperageLatestADC(void) -{ - return amperageLatestADC; -} - int32_t getPower(void) { return power; @@ -443,10 +422,12 @@ void currentMeterUpdate(timeUs_t timeDelta) switch (batteryMetersConfig()->current.type) { case CURRENT_SENSOR_ADC: - amperageLatestADC = adcGetChannel(ADC_CURRENT); - amperageLatestADC = pt1FilterApply4(&erageFilterState, amperageLatestADC, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f); - amperage = currentSensorToCentiamps(amperageLatestADC); - break; + { + int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100; + amperage = microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps + amperage = pt1FilterApply4(&erageFilterState, amperage, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f); + break; + } case CURRENT_SENSOR_VIRTUAL: amperage = batteryMetersConfig()->current.offset; if (ARMING_FLAG(ARMED)) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 3fb063264f..cd4ae70d55 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -125,7 +125,6 @@ bool isPowerSupplyImpedanceValid(void); uint16_t getBatteryVoltage(void); uint16_t getBatteryRawVoltage(void); uint16_t getBatterySagCompensatedVoltage(void); -uint16_t getBatteryVoltageLatestADC(void); uint16_t getBatteryWarningVoltage(void); uint8_t getBatteryCellCount(void); uint16_t getBatteryRawAverageCellVoltage(void); @@ -136,7 +135,6 @@ uint16_t getPowerSupplyImpedance(void); bool isAmperageConfigured(void); int16_t getAmperage(void); -int16_t getAmperageLatestADC(void); int32_t getPower(void); int32_t getMAhDrawn(void); int32_t getMWhDrawn(void); From 8fd2af31deb8ceda6efef159998fd04e0fcd8bd2 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Mon, 2 Jul 2018 16:34:12 +0200 Subject: [PATCH 22/57] Fix division by 0 in generateThrottleCurve() When `thr_mid` is set to 100 a division by 0 is done in `generateThrottleCurve()` --- src/main/fc/rc_curves.c | 14 ++++++++++---- src/main/fc/rc_curves.h | 2 +- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index f7516fe852..b9ff0b8fb5 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -17,9 +17,12 @@ #include #include +#include #include "platform.h" +#include "common/maths.h" + #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" @@ -31,7 +34,7 @@ #define PITCH_LOOKUP_LENGTH 7 #define YAW_LOOKUP_LENGTH 7 -#define THROTTLE_LOOKUP_LENGTH 12 +#define THROTTLE_LOOKUP_LENGTH 11 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE int16_t lookupThrottleRCMid; // THROTTLE curve mid point @@ -55,12 +58,15 @@ void generateThrottleCurve(const controlRateConfig_t *controlRateConfig) int16_t rcLookup(int32_t stickDeflection, uint8_t expo) { float tmpf = stickDeflection / 100.0f; - return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf / 25.0f); + return lrintf((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf / 25.0f); } -int16_t rcLookupThrottle(int32_t absoluteDeflection) +uint16_t rcLookupThrottle(uint16_t absoluteDeflection) { - const int32_t lookupStep = absoluteDeflection / 100; + if (absoluteDeflection > 999) + return motorConfig()->maxthrottle; + + const uint8_t lookupStep = absoluteDeflection / 100; return lookupThrottleRC[lookupStep] + (absoluteDeflection - lookupStep * 100) * (lookupThrottleRC[lookupStep + 1] - lookupThrottleRC[lookupStep]) / 100; } diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h index 547b5bbe66..af7cd618e7 100644 --- a/src/main/fc/rc_curves.h +++ b/src/main/fc/rc_curves.h @@ -21,5 +21,5 @@ struct controlRateConfig_s; void generateThrottleCurve(const struct controlRateConfig_s *controlRateConfig); int16_t rcLookup(int32_t stickDeflection, uint8_t expo); -int16_t rcLookupThrottle(int32_t tmp); +uint16_t rcLookupThrottle(uint16_t tmp); int16_t rcLookupThrottleMid(void); From 4514aad8242bc0d23742d4faa99b6eac6bff3c43 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Sun, 8 Jul 2018 10:28:32 +0200 Subject: [PATCH 23/57] PWM driver - startup value fix --- src/main/drivers/pwm_output.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 96e9fe85cf..e09f98bfe9 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -76,8 +76,6 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, const timerHardware_t *timerH p->ccr = timerCCR(timerHardware->tim, timerHardware->channel); p->period = period; p->tim = timerHardware->tim; - - *p->ccr = 0; } static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, bool enableOutput) From 74e5ae86677f622836ea8b903d26f0ca5b2da7ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 7 Jul 2018 20:40:53 +0100 Subject: [PATCH 24/57] Prevent time comparison overflow in S.Port driver Would stop S.Port telemetry after ~36 minutes. After discussing this with @mikeller, we've decided to enforce the 500us response delay for S.Port, since it might be needed for some receivers. Fixes #3282 --- src/main/telemetry/smartport.c | 14 +++++++------- src/main/telemetry/smartport.h | 4 +++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 2069110374..25c704d980 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -153,7 +153,7 @@ static smartPortWriteFrameFn *smartPortWriteFrame; static bool smartPortMspReplyPending = false; #endif -smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum) +smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, timeUs_t *clearToSendAt, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum) { static uint8_t rxBuffer[sizeof(smartPortPayload_t)]; static uint8_t smartPortRxBytes = 0; @@ -178,6 +178,7 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) { // our slot is starting, no need to decode more *clearToSend = true; + *clearToSendAt = micros() + SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US; skipUntilStart = true; } else if (c == FSSP_SENSOR_ID2) { checksum = 0; @@ -199,6 +200,7 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor checksum += c; if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) { + *clearToSendAt = micros() + SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US; skipUntilStart = true; return (smartPortPayload_t *)&rxBuffer; @@ -209,6 +211,7 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor checksum += c; checksum = (checksum & 0xFF) + (checksum >> 8); if (checksum == 0xFF) { + *clearToSendAt = micros() + SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US; return (smartPortPayload_t *)&rxBuffer; } } @@ -524,7 +527,7 @@ static bool serialCheckQueueEmpty(void) void handleSmartPortTelemetry(void) { static bool clearToSend = false; - static volatile timeUs_t lastTelemetryFrameReceivedUs; + static timeUs_t clearToSendAt; static smartPortPayload_t *payload = NULL; const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS; @@ -532,13 +535,10 @@ void handleSmartPortTelemetry(void) if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) { while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) { uint8_t c = serialRead(smartPortSerialPort); - payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true); - if (payload) { - lastTelemetryFrameReceivedUs = micros(); - } + payload = smartPortDataReceive(c, &clearToSend, &clearToSendAt, serialCheckQueueEmpty, true); } - if (cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) { + if (clearToSendAt > 0 && micros() >= clearToSendAt) { processSmartPortTelemetry(payload, &clearToSend, &requestTimeout); payload = NULL; } diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 72eafc44a3..f56b0a3820 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -10,6 +10,8 @@ #include #include +#include "common/time.h" + #define SMARTPORT_MSP_TX_BUF_SIZE 256 #define SMARTPORT_MSP_RX_BUF_SIZE 64 @@ -50,7 +52,7 @@ bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameEx void handleSmartPortTelemetry(void); void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout); -smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool withChecksum); +smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, timeUs_t *clearToSendAt, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool withChecksum); struct serialPort_s; void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum); From fa4c1431c9a416bd988d83e2f7ad7d386a665202 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 8 Jul 2018 17:27:36 +0100 Subject: [PATCH 25/57] Fix automatic DST support - Apply an offset of 3600s, not 3600ms. - Update the applied offset to the datetime, so callers can format the UTC offset properly. - Apply automatic DST even when non-DST tz offset is zero. --- src/main/common/time.c | 60 +++++++++++++++++++++++------------------- 1 file changed, 33 insertions(+), 27 deletions(-) diff --git a/src/main/common/time.c b/src/main/common/time.c index 64ca72feb1..0de4d2b3b6 100644 --- a/src/main/common/time.c +++ b/src/main/common/time.c @@ -157,6 +157,26 @@ static bool isDST(rtcTime_t t) switch ((tz_automatic_dst_e) timeConfig()->tz_automatic_dst) { case TZ_AUTO_DST_OFF: break; + case TZ_AUTO_DST_EU: // begins at 1:00 a.m. on the last Sunday of March and ends at 1:00 a.m. on the last Sunday of October + if (dateTime.month < 3 || dateTime.month > 10) { + return false; + } + if (dateTime.month > 3 && dateTime.month < 10) { + return true; + } + lastSunday = lastSundayOfMonth(dateTime.year, dateTime.month); + if ((dateTime.day < lastSunday) || (dateTime.day > lastSunday)) { + return !(dateTime.month == 3); + } + if (dateTime.day == lastSunday) { + if (dateTime.month == 3) { + return dateTime.hours >= 1; + } + if (dateTime.month == 10) { + return dateTime.hours < 1; + } + } + break; case TZ_AUTO_DST_USA: // begins at 2:00 a.m. on the second Sunday of March and ends at 2:00 a.m. on the first Sunday of November if (dateTime.month < 3 || dateTime.month > 11) { return false; @@ -180,39 +200,24 @@ static bool isDST(rtcTime_t t) return dateTime.day < firstSunday; } break; - case TZ_AUTO_DST_EU: // begins at 1:00 a.m. on the last Sunday of March and ends at 1:00 a.m. on the last Sunday of October - if (dateTime.month < 3 || dateTime.month > 10) { - return false; - } - if (dateTime.month > 3 && dateTime.month < 10) { - return true; - } - lastSunday = lastSundayOfMonth(dateTime.year, dateTime.month); - if ((dateTime.day < lastSunday) || (dateTime.day > lastSunday)) { - return !(dateTime.month == 3); - } - if (dateTime.day == lastSunday) { - if (dateTime.month == 3) { - return dateTime.hours >= 1; - } - if (dateTime.month == 10) { - return dateTime.hours < 1; - } - } - break; } return false; } #endif -static void dateTimeWithOffset(dateTime_t *dateTimeOffset, dateTime_t *dateTimeInitial, int16_t minutes, bool automatic_dst) +static void dateTimeWithOffset(dateTime_t *dateTimeOffset, dateTime_t *dateTimeInitial, int16_t *minutes, bool automatic_dst) { rtcTime_t initialTime = dateTimeToRtcTime(dateTimeInitial); - rtcTime_t offsetTime = rtcTimeMake(rtcTimeGetSeconds(&initialTime) + minutes * 60, rtcTimeGetMillis(&initialTime)); + rtcTime_t offsetTime = rtcTimeMake(rtcTimeGetSeconds(&initialTime) + *minutes * 60, rtcTimeGetMillis(&initialTime)); #if defined(RTC_AUTOMATIC_DST) if (automatic_dst && isDST(offsetTime)) { - offsetTime += 3600; + // Add one hour. Tell the caller that the + // offset has changed. + *minutes += 60; + offsetTime += 60 * 60 * MILLIS_PER_SECOND; } +#else + UNUSED(automatic_dst); #endif rtcTimeToDateTime(dateTimeOffset, offsetTime); } @@ -226,10 +231,10 @@ static bool dateTimeFormat(char *buf, dateTime_t *dateTime, int16_t offset, bool bool retVal = true; // Apply offset if necessary - if (offset != 0) { + if (offset != 0 || automatic_dst) { + dateTimeWithOffset(&local, dateTime, &offset, automatic_dst); tz_hours = offset / 60; tz_minutes = ABS(offset % 60); - dateTimeWithOffset(&local, dateTime, offset, automatic_dst); dateTime = &local; } @@ -271,12 +276,13 @@ bool dateTimeFormatUTC(char *buf, dateTime_t *dt) bool dateTimeFormatLocal(char *buf, dateTime_t *dt) { - return dateTimeFormat(buf, dt, timeConfig()->tz_offset, timeConfig()->tz_automatic_dst); + return dateTimeFormat(buf, dt, timeConfig()->tz_offset, true); } void dateTimeUTCToLocal(dateTime_t *utcDateTime, dateTime_t *localDateTime) { - dateTimeWithOffset(localDateTime, utcDateTime, timeConfig()->tz_offset, timeConfig()->tz_automatic_dst); + int16_t offset = timeConfig()->tz_offset; + dateTimeWithOffset(localDateTime, utcDateTime, &offset, true); } bool dateTimeSplitFormatted(char *formatted, char **date, char **time) From 6f79b8948da163cb9b43b843b2348608717007f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 7 Jul 2018 22:35:15 +0100 Subject: [PATCH 26/57] Use local times (+TZ when possible) for external interfaces - BB logs format the date in the local TZ, including the UTC offset in the header line. - Files created in the SD card use the local time for timestamps, since FAT doesn't have any notion of timezones. - Time sync with RC Split uses the local time, since it doesn't support timezones. - Added new rtcGetDateTimeLocal() function, which allows removing some branching in osd.c Saves 40 bytes of flash on F3 Fixes #3531 --- src/main/blackbox/blackbox.c | 2 +- src/main/common/time.c | 15 ++++++++++++--- src/main/common/time.h | 3 ++- src/main/io/asyncfatfs/asyncfatfs.c | 2 +- src/main/io/osd.c | 4 +--- src/main/io/rcdevice_cam.c | 2 +- 6 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 32890e9c6b..e6bca4d7e7 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1485,7 +1485,7 @@ static char *blackboxGetStartDateTime(char *buf) // rtcGetDateTime will fill dt with 0000-01-01T00:00:00 // when time is not known. rtcGetDateTime(&dt); - dateTimeFormatUTC(buf, &dt); + dateTimeFormatLocal(buf, &dt); return buf; } diff --git a/src/main/common/time.c b/src/main/common/time.c index 0de4d2b3b6..a497889ddc 100644 --- a/src/main/common/time.c +++ b/src/main/common/time.c @@ -58,7 +58,7 @@ PG_RESET_TEMPLATE(timeConfig_t, timeConfig, .tz_automatic_dst = TZ_AUTO_DST_OFF, ); -static rtcTime_t dateTimeToRtcTime(dateTime_t *dt) +static rtcTime_t dateTimeToRtcTime(const dateTime_t *dt) { unsigned int second = dt->seconds; // 0-59 unsigned int minute = dt->minutes; // 0-59 @@ -205,7 +205,7 @@ static bool isDST(rtcTime_t t) } #endif -static void dateTimeWithOffset(dateTime_t *dateTimeOffset, dateTime_t *dateTimeInitial, int16_t *minutes, bool automatic_dst) +static void dateTimeWithOffset(dateTime_t *dateTimeOffset, const dateTime_t *dateTimeInitial, int16_t *minutes, bool automatic_dst) { rtcTime_t initialTime = dateTimeToRtcTime(dateTimeInitial); rtcTime_t offsetTime = rtcTimeMake(rtcTimeGetSeconds(&initialTime) + *minutes * 60, rtcTimeGetMillis(&initialTime)); @@ -279,7 +279,7 @@ bool dateTimeFormatLocal(char *buf, dateTime_t *dt) return dateTimeFormat(buf, dt, timeConfig()->tz_offset, true); } -void dateTimeUTCToLocal(dateTime_t *utcDateTime, dateTime_t *localDateTime) +void dateTimeUTCToLocal(dateTime_t *localDateTime, const dateTime_t *utcDateTime) { int16_t offset = timeConfig()->tz_offset; dateTimeWithOffset(localDateTime, utcDateTime, &offset, true); @@ -332,6 +332,15 @@ bool rtcGetDateTime(dateTime_t *dt) return false; } +bool rtcGetDateTimeLocal(dateTime_t *dt) +{ + if (rtcGetDateTime(dt)) { + dateTimeUTCToLocal(dt, dt); + return true; + } + return false; +} + bool rtcSetDateTime(dateTime_t *dt) { rtcTime_t t = dateTimeToRtcTime(dt); diff --git a/src/main/common/time.h b/src/main/common/time.h index 87365bec90..bef2f6b6a2 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -93,7 +93,7 @@ typedef struct _dateTime_s { bool dateTimeFormatUTC(char *buf, dateTime_t *dt); bool dateTimeFormatLocal(char *buf, dateTime_t *dt); -void dateTimeUTCToLocal(dateTime_t *utcDateTime, dateTime_t *localDateTime); +void dateTimeUTCToLocal(dateTime_t *localDateTime, const dateTime_t *utcDateTime); // dateTimeSplitFormatted splits a formatted date into its date // and time parts. Note that the string pointed by formatted will // be modifed and will become invalid after calling this function. @@ -105,4 +105,5 @@ bool rtcGet(rtcTime_t *t); bool rtcSet(rtcTime_t *t); bool rtcGetDateTime(dateTime_t *dt); +bool rtcGetDateTimeLocal(dateTime_t *dt); bool rtcSetDateTime(dateTime_t *dt); diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 2ebe00c3a9..53a7fb5981 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -2605,7 +2605,7 @@ static void afatfs_createFileContinue(afatfsFile_t *file) memcpy(entry->filename, opState->filename, FAT_FILENAME_LENGTH); entry->attrib = file->attrib; - if (rtcGetDateTime(&now)) { + if (rtcGetDateTimeLocal(&now)) { entry->creationDate = FAT_MAKE_DATE(now.year, now.month, now.day); entry->creationTime = FAT_MAKE_TIME(now.hours, now.minutes, now.seconds); } else { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0a2b5942f8..bb6778d608 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1956,9 +1956,7 @@ static bool osdDrawSingleElement(uint8_t item) { // RTC not configured will show 00:00 dateTime_t dateTime; - if (rtcGetDateTime(&dateTime)) { - dateTimeUTCToLocal(&dateTime, &dateTime); - } + rtcGetDateTimeLocal(&dateTime); buff[0] = SYM_CLOCK; tfp_sprintf(buff + 1, "%02u:%02u", dateTime.hours, dateTime.minutes); break; diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index 68d848f409..aa890e2317 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -100,7 +100,7 @@ static void rcdeviceCameraUpdateTime(void) if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_DEVICE_SETTINGS_ACCESS) && !hasSynchronizedTime && retries < 3) { - if (rtcGetDateTime(&dt)) { + if (rtcGetDateTimeLocal(&dt)) { retries++; tfp_sprintf(buf, "%04d%02d%02dT%02d%02d%02d.0", dt.year, dt.month, dt.day, From c4401dbc1dcc2772cfe6d15b1dc89ea5085702ac Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Sun, 8 Jul 2018 21:23:46 +0200 Subject: [PATCH 27/57] Fix datasheet incompliance for IST8310 compass chip --- src/main/drivers/compass/compass_ist8310.c | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c index 67def51300..6deb284251 100644 --- a/src/main/drivers/compass/compass_ist8310.c +++ b/src/main/drivers/compass/compass_ist8310.c @@ -92,6 +92,7 @@ #define IST8310_REG_CNTRL1 0x0A #define IST8310_REG_CNTRL2 0x0B #define IST8310_REG_AVERAGE 0x41 +#define IST8310_REG_PDCNTL 0x42 // Parameter // ODR = Output Data Rate, we use single measure mode for getting more data. @@ -103,7 +104,8 @@ // Device ID (ist8310 -> 0x10) #define IST8310_CHIP_ID 0x10 -#define IST8310_AVG_16 0x24 +#define IST8310_AVG_16 0x24 +#define IST8310_PULSE_DURATION_NORMAL 0xC0 #define IST8310_CNTRL2_RESET 0x01 #define IST8310_CNTRL2_DRPOL 0x04 @@ -117,6 +119,9 @@ static bool ist8310Init(magDev_t * mag) busWrite(mag->busDev, IST8310_REG_AVERAGE, IST8310_AVG_16); delay(5); + busWrite(mag->busDev, IST8310_REG_PDCNTL, IST8310_PULSE_DURATION_NORMAL); + delay(5); + return true; } @@ -135,10 +140,10 @@ static bool ist8310Read(magDev_t * mag) return false; } - // need to modify when confirming the pcb direction - mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV; - mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV; - mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV; + // Looks like datasheet is incorrect and we need to invert Y axis to conform to right hand rule + mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV; + mag->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV; + mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV; return true; } From db9d29f89617f2c4db5c76c811cbefa23caf4bdc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 8 Jul 2018 20:33:00 +0100 Subject: [PATCH 28/57] Update Slack auto-invite link Replace signup.team, which has been down for some time, with publicslack.com --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 225cd9bf18..3056e1c4f7 100644 --- a/README.md +++ b/README.md @@ -54,7 +54,7 @@ See: https://github.com/iNavFlight/inav/blob/master/docs/Installation.md * [Official documentation](https://github.com/iNavFlight/inav/tree/master/docs) * [Official Wiki](https://github.com/iNavFlight/inav/wiki) -* [Slack channel](https://inavflight.signup.team/) +* [Slack channel](https://publicslack.com/slacks/inavflight/invites/new) * [RC Groups Support](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-(navigation-rewrite)-project) * [Video series by Painless360](https://www.youtube.com/playlist?list=PLYsWjANuAm4qdXEGFSeUhOZ10-H8YTSnH) * [Video series by Paweł Spychalski](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLloACrA6f1_daCjhqY2x0fB) @@ -83,4 +83,4 @@ Please refer to the development section in the [docs/development](https://github ## INAV Releases -https://github.com/iNavFlight/inav/releases \ No newline at end of file +https://github.com/iNavFlight/inav/releases From a4a09f0f5913e991647595ad37b313e44199cd09 Mon Sep 17 00:00:00 2001 From: Krzysztof Matula Date: Mon, 9 Jul 2018 10:55:28 +0200 Subject: [PATCH 29/57] doc update: document 'diff' command --- docs/Cli.md | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index fb0576308d..67c513fde1 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -16,7 +16,7 @@ To exit the CLI without saving power off the flight controller or type in 'exit' To see a list of other commands type in 'help' and press return. -To dump your configuration (including the current profile), use the 'dump' command. +To dump your configuration (including the current profile), use the 'dump' or 'diff' command. See the other documentation sections for details of the cli commands and settings that are available. @@ -42,23 +42,21 @@ dump profile copy screen output to a file and save it. +Alternatively, use the `diff` command to dump only those settings that differ from their default values (those that have been changed). + + ## Restore via CLI. Use the cli `defaults` command first. -When restoring from a backup it is a good idea to do a dump of the latest defaults so you know what has changed - if you do this each time a firmware release is created you will be able to see the cli changes between firmware versions. For instance, in December 2014 the default GPS navigation PIDs changed. If you blindly restore your backup you would not benefit from these new defaults. +When restoring from backup it's a good idea to do a dump of the latest defaults so you know what has changed - if you do this each time a firmware release is created you will be able to see the cli changes between firmware versions. If you blindly restore your backup you would not benefit from these new defaults or may even end up with completely wrong settings in case some parameters changed semantics and/or value ranges. -Use the CLI and send all the output from the saved backup commands. +It may be good idea to restore settings using the `diff` output rather than complete `dump`. This way you can have more control on what is restored and the risk of mistakenly restoring bad values if the semantics changes is minimised. -Do not send the file too fast, if you do the FC might not be able to keep up when using USART adapters (including built in ones) since there is no hardware serial flow control. +To perform the restore simply paste the saved commands in the Configurator CLI tab and then type `save`. -You may find you have to copy/paste a few lines at a time. +After restoring it's always a good idea to `dump` or `diff` the settings once again and compare the output with previous one to verify if everything is set as it should be. -Repeat the backup process again! - -Compare the two backups to make sure you are happy with your restored settings. - -Re-apply any new defaults as desired. ## CLI Command Reference @@ -72,6 +70,7 @@ Re-apply any new defaults as desired. | `color` | configure colors | | `defaults` | reset to defaults and reboot | | `dump` | print configurable settings in a pastable form | +| `diff` | print only settings that have been modified | | `exit` | | | `feature` | list or -val or val | | `get` | get variable value | From 257e32622112fa756745c72a920cae02f3498b7b Mon Sep 17 00:00:00 2001 From: Krzysztof Matula Date: Mon, 9 Jul 2018 10:56:22 +0200 Subject: [PATCH 30/57] doc update: OMNIBUSF4V5 UART6TX inverter --- docs/Board - Omnibus F4.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/Board - Omnibus F4.md b/docs/Board - Omnibus F4.md index 6871efadab..3087b33012 100644 --- a/docs/Board - Omnibus F4.md +++ b/docs/Board - Omnibus F4.md @@ -132,6 +132,8 @@ For Omnibus F4 Pro clones (Banggood, AliExpress, eBay, etc.) use **OMNIBUSF4PRO_ ## SoftwareSerial +### Omnibus F4 v1/v2 SoftwareSerial Connections + This board allows for single **SoftwareSerial** port on small soldering pads located on the bottom side of the board. Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount pads CH5/CH6. @@ -148,7 +150,7 @@ Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount ### Omnibus F4 v3/v4/v5 SoftwareSerial Connections The SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin. -When enabled, the UART6 is still available as hardware port but it's then RX-only port (good for e.g. receiving S.BUS input). TX instead is controlled in software (can be used for e.g. transmitting one-way telemetry). +When enabled, the UART6 is still available as hardware port but it's then RX-only port (good for e.g. receiving S.BUS input). TX instead is controlled in software and can be used for transmitting one-way telemetry (e.g. LTM). Please note that UART6-TX line passes programmable inverter on those boards, so it is a pure output-only port. SmartPort/FPort telemetry requires bi-directional communication, so it is not possible on this port without hardware modification (bypassing the inverter). # Wiring diagrams for Omnibus F4 Pro From 5468d2fe6e432482cb3112661b2c8c9d814f9811 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Mon, 9 Jul 2018 00:30:58 +0100 Subject: [PATCH 31/57] Add support for retriving setting types and listing all of them This allows an external caller to retrieve and change any setting as well as listing all the available ones without any a priori knowledge. --- src/main/config/parameter_group_ids.h | 4 + src/main/fc/cli.c | 9 +- src/main/fc/fc_msp.c | 113 +++++++++++++++++++++++++- src/main/fc/settings.c | 39 +++++++++ src/main/fc/settings.h | 15 +++- src/main/msp/msp_protocol_v2_common.h | 11 ++- 6 files changed, 180 insertions(+), 11 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index a3e23b392b..dd37d7466d 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -113,3 +113,7 @@ #define PG_RESERVED_FOR_TESTING_2 4094 #define PG_RESERVED_FOR_TESTING_3 4093 +#define PG_ID_INVALID 0 +#define PG_ID_FIRST PG_CF_START +#define PG_ID_LAST PG_INAV_END + diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 908dbcfb0c..30beb2b319 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -352,14 +352,17 @@ static void printValuePointer(const setting_t *var, const void *valuePointer, ui } break; case MODE_LOOKUP: - if (var->config.lookup.tableIndex < LOOKUP_TABLE_COUNT) { - cliPrintf(settingLookupTables[var->config.lookup.tableIndex].values[value]); + { + const char *name = settingLookupValueName(var, value); + if (name) { + cliPrintf(name); } else { settingGetName(var, buf); - cliPrintLinef("VALUE %s OUT OF RANGE", buf); + cliPrintLinef("VALUE %d OUT OF RANGE FOR %s", (int)value, buf); } break; } + } } static bool valuePtrEqualsDefault(const setting_t *value, const void *ptr, const void *ptrDefault) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index aad30560fd..a3dd091d6c 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -35,6 +35,8 @@ #include "common/time.h" #include "common/utils.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro/accgyro.h" #include "drivers/bus_i2c.h" #include "drivers/compass/compass.h" @@ -2549,9 +2551,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ACK; } -static const setting_t *mspReadSettingName(sbuf_t *src) +static const setting_t *mspReadSetting(sbuf_t *src) { char name[SETTING_MAX_NAME_LENGTH]; + uint16_t id; uint8_t c; size_t s = 0; while (1) { @@ -2560,6 +2563,14 @@ static const setting_t *mspReadSettingName(sbuf_t *src) } name[s++] = c; if (c == '\0') { + if (s == 1) { + // Payload starts with a zero, setting index + // as uint16_t follows + if (sbufReadU16Safe(&id, src)) { + return settingGet(id); + } + return NULL; + } break; } if (s == SETTING_MAX_NAME_LENGTH) { @@ -2572,7 +2583,7 @@ static const setting_t *mspReadSettingName(sbuf_t *src) static bool mspSettingCommand(sbuf_t *dst, sbuf_t *src) { - const setting_t *setting = mspReadSettingName(src); + const setting_t *setting = mspReadSetting(src); if (!setting) { return false; } @@ -2587,7 +2598,7 @@ static bool mspSetSettingCommand(sbuf_t *dst, sbuf_t *src) { UNUSED(dst); - const setting_t *setting = mspReadSettingName(src); + const setting_t *setting = mspReadSetting(src); if (!setting) { return false; } @@ -2679,6 +2690,94 @@ static bool mspSetSettingCommand(sbuf_t *dst, sbuf_t *src) return true; } +static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src) +{ + const setting_t *setting = mspReadSetting(src); + if (!setting) { + return false; + } + + // Parameter Group ID + sbufWriteU16(dst, settingGetPgn(setting)); + + // Type, section and mode + sbufWriteU8(dst, SETTING_TYPE(setting)); + sbufWriteU8(dst, SETTING_SECTION(setting)); + sbufWriteU8(dst, SETTING_MODE(setting)); + + // Min as int32_t + int32_t min = settingGetMin(setting); + sbufWriteU32(dst, (uint32_t)min); + // Max as uint32_t + uint32_t max = settingGetMax(setting); + sbufWriteU32(dst, max); + + // Absolute setting index + sbufWriteU16(dst, settingGetIndex(setting)); + + // If the setting is profile based, send the current one + // and the count, both as uint8_t. For MASTER_VALUE, we + // send two zeroes, so the MSP client can assume there + // will always be two bytes. + switch (SETTING_SECTION(setting)) { + case MASTER_VALUE: + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + break; + case PROFILE_VALUE: + FALLTHROUGH; + case CONTROL_RATE_VALUE: + sbufWriteU8(dst, getConfigProfile()); + sbufWriteU8(dst, MAX_PROFILE_COUNT); + break; + case BATTERY_CONFIG_VALUE: + sbufWriteU8(dst, getConfigBatteryProfile()); + sbufWriteU8(dst, MAX_BATTERY_PROFILE_COUNT); + break; + } + + // If the setting uses a table, send each possible string (null terminated) + if (SETTING_MODE(setting) == MODE_LOOKUP) { + for (int ii = (int)min; ii <= (int)max; ii++) { + const char *name = settingLookupValueName(setting, ii); + sbufWriteDataSafe(dst, name, strlen(name) + 1); + } + } + + // Finally, include the setting value. This way resource constrained callers + // (e.g. a script in the radio) don't need to perform another call to retrieve + // the value. + const void *ptr = settingGetValuePointer(setting); + size_t size = settingGetValueSize(setting); + sbufWriteDataSafe(dst, ptr, size); + + return true; +} + +static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src) +{ + uint16_t first; + uint16_t last; + uint16_t start; + uint16_t end; + + if (sbufReadU16Safe(&first, src)) { + last = first; + } else { + first = PG_ID_FIRST; + last = PG_ID_LAST; + } + + for (int ii = first; ii <= last; ii++) { + if (settingsGetParameterGroupIndexes(ii, &start, &end)) { + sbufWriteU16(dst, ii); + sbufWriteU16(dst, start); + sbufWriteU16(dst, end); + } + } + return true; +} + bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret) { switch (cmdMSP) { @@ -2705,6 +2804,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = mspSetSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR; break; + case MSP2_COMMON_SETTING_INFO: + *ret = mspSettingInfoCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR; + break; + + case MSP2_COMMON_PG_LIST: + *ret = mspParameterGroupsCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR; + break; + #if defined(USE_OSD) case MSP2_INAV_OSD_LAYOUTS: if (sbufBytesRemaining(src) >= 1) { diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8c66ebb1f3..d9fd778d89 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -77,6 +77,16 @@ const setting_t *settingFind(const char *name) return NULL; } +const setting_t *settingGet(unsigned index) +{ + return index < SETTINGS_TABLE_COUNT ? &settingsTable[index] : NULL; +} + +unsigned settingGetIndex(const setting_t *val) +{ + return val - settingsTable; +} + size_t settingGetValueSize(const setting_t *val) { switch (SETTING_TYPE(val)) { @@ -154,6 +164,17 @@ setting_max_t settingGetMax(const setting_t *val) return settingMinMaxTable[SETTING_INDEXES_GET_MAX(val)]; } +const char * settingLookupValueName(const setting_t *val, unsigned v) +{ + if (SETTING_MODE(val) == MODE_LOOKUP && val->config.lookup.tableIndex < LOOKUP_TABLE_COUNT) { + const lookupTableEntry_t *table = &settingLookupTables[val->config.lookup.tableIndex]; + if (v < table->valueCount) { + return table->values[v]; + } + } + return NULL; +} + const char * settingGetString(const setting_t *val) { if (SETTING_TYPE(val) == VAR_STRING) { @@ -180,3 +201,21 @@ setting_max_t settingGetStringMaxLength(const setting_t *val) } return 0; } + +bool settingsGetParameterGroupIndexes(pgn_t pg, uint16_t *start, uint16_t *end) +{ + unsigned acc = 0; + for (int ii = 0; ii < SETTINGS_PGN_COUNT; ii++) { + if (settingsPgn[ii] == pg) { + if (start) { + *start = acc; + } + if (end) { + *end = acc + settingsPgnCounts[ii] - 1; + } + return true; + } + acc += settingsPgnCounts[ii]; + } + return false; +} diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 6b26ee6445..0140be5816 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -81,6 +81,11 @@ bool settingNameIsExactMatch(const setting_t *val, char *buf, const char *cmdlin // Returns a setting_t with the exact name (case sensitive), or // NULL if no setting with that name exists. const setting_t *settingFind(const char *name); +// Returns the setting at the given index, or NULL if +// the index is greater than the total count. +const setting_t *settingGet(unsigned index); +// Returns the setting index for the given setting. +unsigned settingGetIndex(const setting_t *val); // Returns the size in bytes of the setting value. size_t settingGetValueSize(const setting_t *val); pgn_t settingGetPgn(const setting_t *val); @@ -100,6 +105,10 @@ setting_min_t settingGetMin(const setting_t *val); // depends on the target and build options, but will always be an unsigned // integer (e.g. uintxx_t,) setting_max_t settingGetMax(const setting_t *val); +// Returns the string in the table which corresponds to the value v +// for the given setting. If the setting mode is not MODE_LOOKUP or +// if the value is out of range, it returns NULL. +const char * settingLookupValueName(const setting_t *val, unsigned v); // Returns the setting value as a const char * iff the setting is of type // VAR_STRING. Otherwise it returns NULL. const char * settingGetString(const setting_t *val); @@ -109,4 +118,8 @@ const char * settingGetString(const setting_t *val); void settingSetString(const setting_t *val, const char *s, size_t size); // Returns the max string length (without counting the '\0' terminator) // for setting of type VAR_STRING. Otherwise it returns 0. -setting_max_t settingGetStringMaxLength(const setting_t *val); \ No newline at end of file +setting_max_t settingGetStringMaxLength(const setting_t *val); + +// Retrieve the setting indexes for the given PG. If the PG is not +// found, these function returns false. +bool settingsGetParameterGroupIndexes(pgn_t pg, uint16_t *start, uint16_t *end); \ No newline at end of file diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index 0f57056f42..98ebdf5e3f 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -15,10 +15,13 @@ * along with INAV. If not, see . */ -#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) -#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) -#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting -#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting +#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) +#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) +#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting +#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting #define MSP2_COMMON_MOTOR_MIXER 0x1005 #define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 + +#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). +#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings \ No newline at end of file From 9a88769317a04800e2a5196618705c43bf16c0f9 Mon Sep 17 00:00:00 2001 From: giacomo892 Date: Mon, 9 Jul 2018 14:58:49 +0200 Subject: [PATCH 32/57] Update README.md --- README.md | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 3056e1c4f7..c03f335c78 100644 --- a/README.md +++ b/README.md @@ -12,12 +12,8 @@ * Rangefinder support (sonar and laser) * Oneshot and Multishot ESC support. * Blackbox flight recorder logging (to onboard flash or external SD card). -* Support for more than 8 RC channels - (e.g. 16 Channels via FrSky X4RSB SBus). -* Support for N-Position switches via flexible channel ranges - not just 3 like baseflight or 3/6 in MultiWii * Lux's new PID (uses float values internally, resistant to looptime variation). * Simultaneous Bluetooth configuration and OSD. -* Better PWM and PPM input and failsafe detection than baseflight. -* Better FrSky Telemetry than baseflight. * LTM Telemetry. * Smartport Telemetry. * RSSI via ADC - Uses ADC to read PWM RSSI signals, tested with FrSky D4R-II and X8R. @@ -28,7 +24,7 @@ * Configurable serial ports for Serial RX, Telemetry, MSP, GPS - Use most devices on any port, softserial too. * Multi-color RGB LED Strip support (each LED can be a different color using variable length WS2811 Addressable RGB strips - use for Orientation Indicators, Low Battery Warning, Flight Mode Status, etc) * PIDs from CF/BF can be used in INAV, no need to retune for INAV -* And many more minor bug fixes. +* And many more! For a list of features, changes and some discussion please review the thread on RCGroups forums and consult the documentation. @@ -54,7 +50,9 @@ See: https://github.com/iNavFlight/inav/blob/master/docs/Installation.md * [Official documentation](https://github.com/iNavFlight/inav/tree/master/docs) * [Official Wiki](https://github.com/iNavFlight/inav/wiki) -* [Slack channel](https://publicslack.com/slacks/inavflight/invites/new) +* [INAV Official on Telegram](https://t.me/INAVFlight) +* [INAV Official on Facebook](https://www.facebook.com/groups/INAVOfficial) +* [INAV Official on Slack](https://publicslack.com/slacks/inavflight/invites/new) * [RC Groups Support](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-(navigation-rewrite)-project) * [Video series by Painless360](https://www.youtube.com/playlist?list=PLYsWjANuAm4qdXEGFSeUhOZ10-H8YTSnH) * [Video series by Paweł Spychalski](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLloACrA6f1_daCjhqY2x0fB) From 09b7c7f41ef43ff0bb8f1c3b25ce17d019dd4630 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 9 Jul 2018 20:56:16 +0200 Subject: [PATCH 33/57] Fix unreliable QMC5883L mag detection (kudos to MJ666) --- src/main/drivers/compass/compass_qmc5883l.c | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/compass/compass_qmc5883l.c b/src/main/drivers/compass/compass_qmc5883l.c index c48ce9cd02..381bb3b48d 100755 --- a/src/main/drivers/compass/compass_qmc5883l.c +++ b/src/main/drivers/compass/compass_qmc5883l.c @@ -122,18 +122,22 @@ static bool qmc5883Read(magDev_t * mag) #define DETECTION_MAX_RETRY_COUNT 5 static bool deviceDetect(magDev_t * mag) { - // Must write reset first - don't care about the result - busWrite(mag->busDev, QMC5883L_REG_CONF2, QMC5883L_RST); - delay(20); - for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { - delay(10); + // Must write reset first - don't care about the result + busWrite(mag->busDev, QMC5883L_REG_CONF2, QMC5883L_RST); + delay(30); uint8_t sig = 0; bool ack = busRead(mag->busDev, QMC5883L_REG_ID, &sig); if (ack && sig == QMC5883_ID_VAL) { - return true; + // Should be in standby mode after soft reset and sensor is really present + // Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present + + ack = busRead(mag->busDev, QMC5883L_REG_CONF1, &sig); + if (ack && sig == QMC5883L_MODE_STANDBY) { + return true; + } } } From aa8e46b2de4f3c4332c55fdaca9aefbd4c6fa36e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Mon, 9 Jul 2018 22:36:01 +0100 Subject: [PATCH 34/57] Use new setting accessor functions for retriving settings This reduces flash usage by 96 bytes, making the cost of the whole PR around 300 bytes. Also, the settings tables in settings_generated.c are now made static to make sure no code outside of settings.c can reference them. --- src/main/cms/cms.c | 12 +++--------- src/main/fc/cli.c | 14 +++++++------- src/main/fc/settings.c | 16 +++++++++++----- src/main/fc/settings.h | 7 +++---- src/utils/settings.rb | 6 +++--- 5 files changed, 27 insertions(+), 28 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index e85b37e745..375b7a82a6 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -428,7 +428,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t if (IS_PRINTVALUE(p, screenRow) && p->data) { buff[0] = '\0'; const OSD_SETTING_t *ptr = p->data; - const setting_t *var = &settingsTable[ptr->val]; + const setting_t *var = settingGet(ptr->val); int32_t value; const void *valuePointer = settingGetValuePointer(var); switch (SETTING_TYPE(var)) { @@ -476,13 +476,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t break; case MODE_LOOKUP: { - const char *str = NULL; - if (var->config.lookup.tableIndex < LOOKUP_TABLE_COUNT) { - const lookupTableEntry_t *tableEntry = &settingLookupTables[var->config.lookup.tableIndex]; - if (value < tableEntry->valueCount) { - str = tableEntry->values[value]; - } - } + const char *str = settingLookupValueName(var, value); strncpy(buff, str ? str : "INVALID", sizeof(buff) - 1); } break; @@ -1008,7 +1002,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_Setting: if (p->data) { const OSD_SETTING_t *ptr = p->data; - const setting_t *var = &settingsTable[ptr->val]; + const setting_t *var = settingGet(ptr->val); setting_min_t min = settingGetMin(var); setting_max_t max = settingGetMax(var); float step = ptr->step ?: 1; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 30beb2b319..cd78165788 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -428,8 +428,8 @@ static void dumpPgValue(const setting_t *value, uint8_t dumpMask) static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) { - for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) { - const setting_t *value = &settingsTable[i]; + for (unsigned i = 0; i < SETTINGS_TABLE_COUNT; i++) { + const setting_t *value = settingGet(i); bufWriterFlush(cliWriter); if (SETTING_SECTION(value) == valueSection) { dumpPgValue(value, dumpMask); @@ -456,7 +456,7 @@ static void cliPrintVarRange(const setting_t *var) break; case MODE_LOOKUP: { - const lookupTableEntry_t *tableEntry = &settingLookupTables[var->config.lookup.tableIndex]; + const lookupTableEntry_t *tableEntry = settingLookupTable(var); cliPrint("Allowed values:"); for (uint32_t i = 0; i < tableEntry->valueCount ; i++) { if (i > 0) @@ -2259,7 +2259,7 @@ static void cliGet(char *cmdline) char name[SETTING_MAX_NAME_LENGTH]; for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) { - val = &settingsTable[i]; + val = settingGet(i); if (settingNameContains(val, name, cmdline)) { cliPrintf("%s = ", name); cliPrintVar(val, 0); @@ -2291,7 +2291,7 @@ static void cliSet(char *cmdline) if (len == 0 || (len == 1 && cmdline[0] == '*')) { cliPrintLine("Current settings:"); for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) { - val = &settingsTable[i]; + val = settingGet(i); settingGetName(val, name); cliPrintf("%s = ", name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui @@ -2313,7 +2313,7 @@ static void cliSet(char *cmdline) } for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) { - val = &settingsTable[i]; + val = settingGet(i); // ensure exact match when setting to prevent setting variables with shorter names if (settingNameIsExactMatch(val, name, cmdline, variableNameLength)) { const setting_type_e type = SETTING_TYPE(val); @@ -2344,7 +2344,7 @@ static void cliSet(char *cmdline) } break; case MODE_LOOKUP: { - const lookupTableEntry_t *tableEntry = &settingLookupTables[settingsTable[i].config.lookup.tableIndex]; + const lookupTableEntry_t *tableEntry = settingLookupTable(val); bool matched = false; for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { matched = sl_strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index d9fd778d89..43375b02cc 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -164,13 +164,19 @@ setting_max_t settingGetMax(const setting_t *val) return settingMinMaxTable[SETTING_INDEXES_GET_MAX(val)]; } -const char * settingLookupValueName(const setting_t *val, unsigned v) +const lookupTableEntry_t * settingLookupTable(const setting_t *val) { if (SETTING_MODE(val) == MODE_LOOKUP && val->config.lookup.tableIndex < LOOKUP_TABLE_COUNT) { - const lookupTableEntry_t *table = &settingLookupTables[val->config.lookup.tableIndex]; - if (v < table->valueCount) { - return table->values[v]; - } + return &settingLookupTables[val->config.lookup.tableIndex]; + } + return NULL; +} + +const char * settingLookupValueName(const setting_t *val, unsigned v) +{ + const lookupTableEntry_t *table = settingLookupTable(val); + if (table && v < table->valueCount) { + return table->values[v]; } return NULL; } diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 0140be5816..172f960ce4 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -13,8 +13,6 @@ typedef struct lookupTableEntry_s { const uint8_t valueCount; } lookupTableEntry_t; -extern const lookupTableEntry_t settingLookupTables[]; - #define SETTING_TYPE_OFFSET 0 #define SETTING_SECTION_OFFSET 4 #define SETTING_MODE_OFFSET 6 @@ -69,8 +67,6 @@ typedef struct { } __attribute__((packed)) setting_t; -extern const setting_t settingsTable[]; - static inline setting_type_e SETTING_TYPE(const setting_t *s) { return s->type & SETTING_TYPE_MASK; } static inline setting_section_e SETTING_SECTION(const setting_t *s) { return s->type & SETTING_SECTION_MASK; } static inline setting_mode_e SETTING_MODE(const setting_t *s) { return s->type & SETTING_MODE_MASK; } @@ -105,6 +101,9 @@ setting_min_t settingGetMin(const setting_t *val); // depends on the target and build options, but will always be an unsigned // integer (e.g. uintxx_t,) setting_max_t settingGetMax(const setting_t *val); +// Returns the lookup table for the given setting. If the setting mode +// is not MODE_LOOKUP, it returns NULL; +const lookupTableEntry_t * settingLookupTable(const setting_t *val); // Returns the string in the table which corresponds to the value v // for the given setting. If the setting mode is not MODE_LOOKUP or // if the value is out of range, it returns NULL. diff --git a/src/utils/settings.rb b/src/utils/settings.rb index c42901c87f..b84c6f35b7 100644 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -468,7 +468,7 @@ class Generator buf << "};\n" end - buf << "const lookupTableEntry_t settingLookupTables[] = {\n" + buf << "static const lookupTableEntry_t settingLookupTables[] = {\n" table_names.each do |name| vn = table_variable_name(name) buf << "\t{ #{vn}, sizeof(#{vn}) / sizeof(char*) },\n" @@ -476,7 +476,7 @@ class Generator buf << "};\n" # Write min/max values table - buf << "const uint32_t settingMinMaxTable[] = {\n" + buf << "static const uint32_t settingMinMaxTable[] = {\n" @value_encoder.values.each do |v| buf << "\t#{v},\n" end @@ -492,7 +492,7 @@ class Generator end # Write setting_t values - buf << "const setting_t settingsTable[] = {\n" + buf << "static const setting_t settingsTable[] = {\n" last_group = nil foreach_enabled_member do |group, member| From e71725fe53c600f9072ff8b645fb57e54de6bde1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Tue, 10 Jul 2018 14:04:15 +0100 Subject: [PATCH 35/57] Draw missing AHI character when it overlaps with the crosshairs The crosshairs bounds were incorrectly calculated, causing the character at the right of the crosshairs to not be drawn. References #3449 --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0a2b5942f8..99c7eda519 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1601,7 +1601,7 @@ static bool osdDrawSingleElement(uint8_t item) osdCrosshairsBounds(&cx, &cy, &cl); crosshairsX = cx - elemPosX; crosshairsY = cy - elemPosY; - crosshairsXEnd = crosshairsX + cl; + crosshairsXEnd = crosshairsX + cl - 1; } for (int x = -4; x <= 4; x++) { From 1022ada9a766a2f9d57e3c7875775526b9664cf5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Tue, 10 Jul 2018 22:53:45 +0100 Subject: [PATCH 36/57] Reset the biquad filter used for servos to 1500 on startup Filter was initialized at zero, which delayed the servo centering by ~2 seconds. The filter is now reset to 1500 during initialization, so the servos receive 1500 from the start. Coupled with #3540, this completely eliminates all servo twitching during boot. Thanks to @shellixyz for testing! --- src/main/common/filter.c | 7 +++++++ src/main/common/filter.h | 1 + src/main/flight/servos.c | 1 + 3 files changed, 9 insertions(+) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 37f76fce6b..924383bef8 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -205,6 +205,13 @@ float biquadFilterApply(biquadFilter_t *filter, float input) return result; } +float biquadFilterReset(biquadFilter_t *filter, float value) +{ + filter->d1 = value - (value * filter->b0); + filter->d2 = (filter->b2 - filter->a2) * value; + return value; +} + /* * FIR filter */ diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 88f8cbd0aa..2811095ce1 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -73,6 +73,7 @@ void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs); void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float sample); +float biquadFilterReset(biquadFilter_t *filter, float value); float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 9a625b53c3..7489c8cb80 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -177,6 +177,7 @@ static void filterServos(void) if (!servoFilterIsSet) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, gyro.targetLooptime); + biquadFilterReset(&servoFilter[i], servo[i]); } servoFilterIsSet = true; } From c4605fe1634710c305be67f1e425b46fea80d3a1 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 11 Jul 2018 01:15:20 +0200 Subject: [PATCH 37/57] Fix roll attitude angle display --- docs/Cli.md | 1 - src/main/fc/settings.yaml | 4 ---- src/main/io/osd.c | 9 ++++----- src/main/io/osd.h | 1 - 4 files changed, 4 insertions(+), 11 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index fb0576308d..9c72852900 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -300,7 +300,6 @@ Re-apply any new defaults as desired. | osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | | osd_neg_alt_alarm | 5 | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_estimations_wind_compensation | ON | Use wind estimation for remaining flight time/distance estimation | -| osd_attitude_angle_decimals | 0 | Chose the numbers of decimals displayed by OSD attitude angle elements [0-1] | | magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | | magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | | magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1f50a04854..a625ae90af 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1553,10 +1553,6 @@ groups: field: main_voltage_decimals min: 1 max: 2 - - name: osd_attitude_angle_decimals - field: attitude_angle_decimals - min: 0 - max: 1 - name: osd_estimations_wind_compensation field: estimations_wind_compensation diff --git a/src/main/io/osd.c b/src/main/io/osd.c index bb6778d608..359e1d4eec 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1552,19 +1552,19 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_ATTITUDE_ROLL: buff[0] = SYM_ROLL_LEVEL; - if (ABS(attitude.values.roll) >= (osdConfig()->attitude_angle_decimals ? 1 : 10)) + if (ABS(attitude.values.roll) >= 1) buff[0] += (attitude.values.roll < 0 ? -1 : 1); - osdFormatCentiNumber(buff + 1, ABS(attitude.values.roll) * 10, 0, osdConfig()->attitude_angle_decimals, 0, 2 + osdConfig()->attitude_angle_decimals); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3); break; case OSD_ATTITUDE_PITCH: - if (ABS(attitude.values.pitch) < (osdConfig()->attitude_angle_decimals ? 1 : 10)) + if (ABS(attitude.values.pitch) < 1) buff[0] = 'P'; else if (attitude.values.pitch > 0) buff[0] = SYM_PITCH_DOWN; else if (attitude.values.pitch < 0) buff[0] = SYM_PITCH_UP; - osdFormatCentiNumber(buff + 1, ABS(attitude.values.pitch) * 10, 0, osdConfig()->attitude_angle_decimals, 0, 2 + osdConfig()->attitude_angle_decimals); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3); break; case OSD_ARTIFICIAL_HORIZON: @@ -2421,7 +2421,6 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->units = OSD_UNIT_METRIC; osdConfig->main_voltage_decimals = 1; - osdConfig->attitude_angle_decimals = 0; osdConfig->estimations_wind_compensation = true; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a47c1d4280..52d8c6458c 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -163,7 +163,6 @@ typedef struct osdConfig_s { // Preferences uint8_t main_voltage_decimals; - uint8_t attitude_angle_decimals; uint8_t ahi_reverse_roll; uint8_t crosshairs_style; // from osd_crosshairs_style_e uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e From 867e46ebc9c4b362ae19ca71e116945397e36f83 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Wed, 11 Jul 2018 00:54:57 +0100 Subject: [PATCH 38/57] Check for the correct display coordinate when drawing POI When checking the display, we were testing for (poiY, poiY) rather than (poiX, poiY), which was specially visible when drawing across the horizontal line over the map center, since the test was always for (0, 0), causing the scale to go to the maximum value since (0, 0) always contains the map center symbol. Fixes #3508 --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 28501d0f7a..e0dff00731 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -987,7 +987,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente } else { uint8_t c; - if (displayReadCharWithAttr(osdDisplayPort, poiY, poiY, &c, NULL) && c != SYM_BLANK) { + if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) { // Something else written here, increase scale. If the display doesn't support reading // back characters, we assume there's nothing. continue; From be2dd33dda558452dd930f1cf7008214e26924ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Wed, 11 Jul 2018 16:48:08 +0100 Subject: [PATCH 39/57] Fix VTX power levels to be in 1-5 range rather than 0-4 Drivers ignore zero, so the minimum power is actually 1. Also, remove some hardcoded values and use proper constants for them, fixing an issue with the number of power levels in Tramp (sizeof of an uint16_t array was used rather than its count). Fixes #3560 --- src/main/cms/cms_menu_vtx_tramp.c | 2 +- src/main/drivers/vtx_common.h | 2 +- src/main/io/vtx_smartaudio.c | 2 +- src/main/io/vtx_tramp.c | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index 735d480339..4c6a1345dc 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/src/main/cms/cms_menu_vtx_tramp.c @@ -89,7 +89,7 @@ static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, VTX_TRAMP_CHANNEL_COUN static uint8_t trampCmsPower = 1; -static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, sizeof(trampPowerTable), trampPowerNames }; +static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, VTX_TRAMP_POWER_COUNT, trampPowerNames }; static void trampCmsUpdateFreqRef(void) { diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index b8d9cec880..bd98501cb8 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -47,7 +47,7 @@ #define VTX_SETTINGS_POWER_COUNT 5 #define VTX_SETTINGS_DEFAULT_POWER 1 -#define VTX_SETTINGS_MIN_POWER 0 +#define VTX_SETTINGS_MIN_POWER 1 #define VTX_SETTINGS_MIN_USER_FREQ 5000 #define VTX_SETTINGS_MAX_USER_FREQ 5999 #define VTX_SETTINGS_FREQCMD diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index f9722affd1..fd5002b410 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -200,7 +200,7 @@ static void saPrintSettings(void) int saDacToPowerIndex(int dac) { - for (int idx = 3 ; idx >= 0 ; idx--) { + for (int idx = VTX_SMARTAUDIO_POWER_COUNT - 1 ; idx >= 0 ; idx--) { if (saPowerTable[idx].valueV1 <= dac) { return idx; } diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index e6a93eb462..9a984c9926 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -60,7 +60,7 @@ static vtxDevice_t vtxTramp = { .vTable = &trampVTable, .capability.bandCount = VTX_TRAMP_BAND_COUNT, .capability.channelCount = VTX_TRAMP_CHANNEL_COUNT, - .capability.powerCount = sizeof(trampPowerTable), + .capability.powerCount = VTX_TRAMP_POWER_COUNT, .bandNames = (char **)vtx58BandNames, .channelNames = (char **)vtx58ChannelNames, .powerNames = (char **)trampPowerNames, @@ -196,7 +196,7 @@ bool trampCommitChanges(void) // return false if index out of range static bool trampDevSetPowerByIndex(uint8_t index) { - if (index > 0 && index <= sizeof(trampPowerTable)) { + if (index > 0 && index <= VTX_TRAMP_POWER_COUNT) { trampSetRFPower(trampPowerTable[index - 1]); trampCommitChanges(); return true; From 726a51ecd2dd038f2b70abe0af3509cd4f266347 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sat, 7 Jul 2018 19:12:20 +0200 Subject: [PATCH 40/57] Improve readability --- src/main/navigation/navigation.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f4f199db6f..eb109f12a0 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2969,9 +2969,9 @@ void navigationUsePIDs(void) (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f, (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f); - navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 9.80665f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 9.80665f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 9.80665f); + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f); } void navigationInit(void) From a5a8eea30f0c008fdebcfacfb8a524f4e9c4e3e4 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 11 Jul 2018 22:15:33 +0200 Subject: [PATCH 41/57] Fix beeping when using RC alignment adjust with beeper -ON_USB --- src/main/sensors/battery.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 41b1ce4d51..57095e3c88 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -189,7 +189,13 @@ void setBatteryProfile(uint8_t profileIndex) void activateBatteryProfile(void) { - batteryInit(); + static int8_t previous_battery_profile_index = -1; + // Don't call batteryInit if the battery profile was not changed to prevent batteryCellCount to be reset while adjusting board alignment + // causing the beeper to be silent when it is disabled while the board is connected through USB (beeper -ON_USB) + if (systemConfig()->current_battery_profile_index != previous_battery_profile_index) { + batteryInit(); + previous_battery_profile_index = systemConfig()->current_battery_profile_index; + } } static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) From 0eded14827aa43f0e43169abc6cb8ae9112c3147 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Thu, 12 Jul 2018 00:42:07 +0200 Subject: [PATCH 42/57] Remove unnecessary batteryCellCount initialization --- src/main/sensors/battery.c | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 57095e3c88..13cf059175 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -56,19 +56,19 @@ #include "io/beeper.h" -#define ADCVREF 3300 // in mV (3300 = 3.3V) +#define ADCVREF 3300 // in mV (3300 = 3.3V) -#define VBATT_CELL_FULL_MAX_DIFF 10 // Max difference with cell max voltage for the battery to be considered full (10mV steps) -#define VBATT_PRESENT_THRESHOLD 100 // Minimum voltage to consider battery present -#define VBATT_STABLE_DELAY 40 // Delay after connecting battery to begin monitoring -#define VBATT_HYSTERESIS 10 // Batt Hysteresis of +/-100mV for changing battery state -#define VBATT_LPF_FREQ 1 // Battery voltage filtering cutoff -#define AMPERAGE_LPF_FREQ 1 // Battery current filtering cutoff +#define VBATT_CELL_FULL_MAX_DIFF 10 // Max difference with cell max voltage for the battery to be considered full (10mV steps) +#define VBATT_PRESENT_THRESHOLD 100 // Minimum voltage to consider battery present +#define VBATT_STABLE_DELAY 40 // Delay after connecting battery to begin monitoring +#define VBATT_HYSTERESIS 10 // Batt Hysteresis of +/-100mV for changing battery state +#define VBATT_LPF_FREQ 1 // Battery voltage filtering cutoff +#define AMPERAGE_LPF_FREQ 1 // Battery current filtering cutoff #define IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH 10 // Minimum sample count to consider calculated power supply impedance as stable // Battery monitoring stuff -static uint8_t batteryCellCount = 3; // cell count +static uint8_t batteryCellCount; // cell count static uint16_t batteryFullVoltage; static uint16_t batteryWarningVoltage; static uint16_t batteryCriticalVoltage; @@ -77,15 +77,15 @@ static bool batteryUseCapacityThresholds = false; static bool batteryFullWhenPluggedIn = false; static bool profileAutoswitchDisable = false; -static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) -static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm -static uint16_t sagCompensatedVBat = 0; // calculated no load vbat +static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) +static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm +static uint16_t sagCompensatedVBat = 0; // calculated no load vbat static bool powerSupplyImpedanceIsValid = false; -static int16_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) -static int32_t power = 0; // power draw in cW (0.01W resolution) -static int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start -static int32_t mWhDrawn = 0; // energy (milliWatt hours) drawn from the battery since start +static int16_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) +static int32_t power = 0; // power draw in cW (0.01W resolution) +static int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start +static int32_t mWhDrawn = 0; // energy (milliWatt hours) drawn from the battery since start batteryState_e batteryState; const batteryProfile_t *currentBatteryProfile; From 31840b3d44b46ef62ea5645a4e5c470ff2dcb2c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Wed, 11 Jul 2018 23:51:08 +0100 Subject: [PATCH 43/57] Teach the map drawing code to reduce scale When looking for another scale when drawing would overwrite a non-blank character, reduce the scale when we're close to the center and increase it when we're far from it. Otherwise there are some pathological cases when an indicator next to the map center would cause the map to not draw anything. --- src/main/io/osd.c | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ce07930a2e..5a89e23ca7 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -918,7 +918,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente char symUnscaled; char symScaled; int maxDecimals; - const int scaleMultiplier = 2; + const unsigned scaleMultiplier = 2; // We try to reduce the scale when the POI will be around half the distance // between the center and the closers map edge, to avoid too much jumping const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; @@ -961,19 +961,22 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente float poiCos = cos_approx(poiAngle); // Now start looking for a valid scale that lets us draw everything - for (int ii = 0; ii < 50; ii++, scale *= scaleMultiplier) { + int ii; + for (ii = 0; ii < 50; ii++) { // Calculate location of the aircraft in map - int points = poiDistance / (float)(scale / charHeight); + int points = poiDistance / ((float)scale / charHeight); float pointsX = points * poiSin; int poiX = midX - roundf(pointsX / charWidth); if (poiX < minX || poiX > maxX) { + scale *= scaleMultiplier; continue; } float pointsY = points * poiCos; int poiY = midY + roundf(pointsY / charHeight); if (poiY < minY || poiY > maxY) { + scale *= scaleMultiplier; continue; } @@ -990,6 +993,18 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) { // Something else written here, increase scale. If the display doesn't support reading // back characters, we assume there's nothing. + // + // If we're close to the center, decrease scale. Otherwise increase it. + uint8_t centerDeltaX = (maxX - minX) / (scaleMultiplier * 2); + uint8_t centerDeltaY = (maxY - minY) / (scaleMultiplier * 2); + if (poiX >= midX - centerDeltaX && poiX <= midX + centerDeltaX && + poiY >= midY - centerDeltaY && poiY <= midY + centerDeltaY && + scale > scaleMultiplier) { + + scale /= scaleMultiplier; + } else { + scale *= scaleMultiplier; + } continue; } } From d5af8ebf10e5b149a5fdab482a0072017809a811 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Thu, 12 Jul 2018 00:08:41 +0100 Subject: [PATCH 44/57] Avoid flickering when the AHI overlaps another element Make the AHI avoid drawing over other elements that might partially overlap with it. --- src/main/io/osd.c | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ce07930a2e..ea2d5fb895 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1618,25 +1618,32 @@ static bool osdDrawSingleElement(uint8_t item) int wx = x + 4; // map the -4 to the 1st element in the writtenY array int pwy = writtenY[wx]; // previously written Y at this X value int wy = (y / AH_SYMBOL_COUNT); - // Check if we're overlapping with the crosshairs. Saves a few - // trips to the video driver. + + unsigned chX = elemPosX + x; + unsigned chY = elemPosY + wy; + uint8_t c; + + // Check if we're overlapping with the crosshairs directly. Saves a few + // trips to the video driver. Also, test for other arbitrary overlapping + // elements if the display supports reading back characters. bool overlaps = (crosshairsVisible && crosshairsY == wy && - x >= crosshairsX && x <= crosshairsXEnd); + x >= crosshairsX && x <= crosshairsXEnd) || + (pwy != wy && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && c != SYM_BLANK); if (y >= 0 && y <= 80 && !overlaps) { if (pwy != -1 && pwy != wy) { // Erase previous character at pwy rows below elemPosY // iff we're writing at a different Y coordinate. Otherwise // we just overwrite the previous one. - displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + pwy, SYM_BLANK); + displayWriteChar(osdDisplayPort, chX, elemPosY + pwy, SYM_BLANK); } uint8_t ch = SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT); - displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + wy, ch); + displayWriteChar(osdDisplayPort, chX, chY, ch); writtenY[wx] = wy; } else { if (pwy != -1) { - displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + pwy, SYM_BLANK); + displayWriteChar(osdDisplayPort, chX, elemPosY + pwy, SYM_BLANK); writtenY[wx] = -1; } } From 8ca7f0686097f9c08f365ddfe546138291308dd0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Thu, 12 Jul 2018 00:48:33 +0100 Subject: [PATCH 45/57] Add support for configurable coordinate digits in OSD 8-11 are allowed values. This allows pilots to choose how much space they want to dedicate to the coordinates, while providing a consistent display with both latitude and longitude at the same length. --- src/main/fc/settings.yaml | 4 ++++ src/main/io/osd.c | 39 +++++++++++++++++++++++++-------------- src/main/io/osd.h | 1 + 3 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a625ae90af..f26b902293 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1553,6 +1553,10 @@ groups: field: main_voltage_decimals min: 1 max: 2 + - name: osd_coordinate_digits + field: coordinate_digits + min: 8 + max: 11 - name: osd_estimations_wind_compensation field: estimations_wind_compensation diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ce07930a2e..ac0a28e313 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -488,25 +488,35 @@ static uint16_t osdConvertRSSI(void) static void osdFormatCoordinate(char *buff, char sym, int32_t val) { - const int coordinateMaxLength = 12; // 11 for the number + 1 for the symbol + // This should be faster than pow(10, x). Make it available this + // via extern rather than _GNU_SOURCE, since it should be more + // portable. + extern float exp10f(float); + + // up to 4 for number + 1 for the symbol + null terminator + fill the rest with decimals + const int coordinateMaxLength = osdConfig()->coordinate_digits + 1; buff[0] = sym; int32_t integerPart = val / GPS_DEGREES_DIVIDER; - int32_t decimalPart = abs(val % GPS_DEGREES_DIVIDER); // Latitude maximum integer width is 3 (-90) while - // longitude maximum integer width is 4 (-180). We always - // show 7 decimals, so we need to use 11 spaces because - // we're embedding the decimal separator between the - // two numbers. - int written = tfp_sprintf(buff + 1, "%d", integerPart); - tfp_sprintf(buff + 1 + written, "%07d", decimalPart); + // longitude maximum integer width is 4 (-180). + int integerDigits = tfp_sprintf(buff + 1, "%d", integerPart); + // We can show up to 7 digits in decimalPart. Remove + // some if needed. + int32_t decimalPart = abs(val % GPS_DEGREES_DIVIDER); + int trim = 7 - MAX(coordinateMaxLength - 1 - integerDigits, 0); + if (trim > 0) { + decimalPart /= (int32_t)exp10f(trim); + } + int decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%d", decimalPart); // Embbed the decimal separator - buff[1+written-1] += SYM_ZERO_HALF_TRAILING_DOT - '0'; - buff[1+written] += SYM_ZERO_HALF_LEADING_DOT - '0'; - // Pad to 11 coordinateMaxLength - while(1 + 7 + written < coordinateMaxLength) { - buff[1 + 7 + written] = SYM_BLANK; - written++; + buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0'; + buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0'; + // Fill up to coordinateMaxLength with zeros + int total = 1 + integerDigits + decimalDigits; + while(total < coordinateMaxLength) { + buff[total] = '0'; + total++; } buff[coordinateMaxLength] = '\0'; } @@ -2423,6 +2433,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->main_voltage_decimals = 1; osdConfig->estimations_wind_compensation = true; + osdConfig->coordinate_digits = 9; } static void osdSetNextRefreshIn(uint32_t timeMs) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 52d8c6458c..cffa557eb0 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -173,6 +173,7 @@ typedef struct osdConfig_s { uint8_t stats_energy_unit; // from osd_stats_energy_unit_e bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance + uint8_t coordinate_digits; } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); From ad3b3284793d3ea35b70815e1b2e11ac6d0ea9f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Thu, 12 Jul 2018 10:29:14 +0100 Subject: [PATCH 46/57] Add menu for OSD settings to CMS Main voltage decimals, coordinate digits, etc... --- src/main/cms/cms_menu_builtin.c | 3 +- src/main/cms/cms_menu_osd.c | 59 ++++++++++++++++++++++++++++++--- src/main/cms/cms_menu_osd.h | 3 +- 3 files changed, 56 insertions(+), 9 deletions(-) diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 32a9dd68ab..32336c4788 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -148,8 +148,7 @@ static const OSD_Entry menuMainEntries[] = OSD_SUBMENU_ENTRY("PID TUNING", &cmsx_menuImu), OSD_SUBMENU_ENTRY("FEATURES", &menuFeatures), #if defined(USE_OSD) && defined(CMS_MENU_OSD) - OSD_SUBMENU_ENTRY("OSD LAYOUTS", &cmsx_menuOsdLayout), - OSD_SUBMENU_ENTRY("ALARMS", &cmsx_menuAlarms), + OSD_SUBMENU_ENTRY("OSD", &cmsx_menuOsd), #endif OSD_SUBMENU_ENTRY("BATTERY", &cmsx_menuBattery), OSD_SUBMENU_ENTRY("FC&FW INFO", &menuInfo), diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index b0058d04da..cd28fd9e55 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -61,9 +61,9 @@ static const OSD_Entry cmsx_menuAlarmsEntries[] = { OSD_END_ENTRY, }; -const CMS_Menu cmsx_menuAlarms = { +static const CMS_Menu cmsx_menuAlarms = { #ifdef CMS_MENU_DEBUG - .GUARD_text = "MENUALARMS", + .GUARD_text = "MENUOSDA", .GUARD_type = OME_MENU, #endif .onEnter = NULL, @@ -144,7 +144,7 @@ static long osdElemActionsOnEnter(const OSD_Entry *from) static const OSD_Entry menuOsdElemsEntries[] = { - OSD_LABEL_ENTRY("--- OSD ---"), + OSD_LABEL_ENTRY("--- OSD ITEMS ---"), OSD_ELEMENT_ENTRY("RSSI", OSD_RSSI_VALUE), OSD_ELEMENT_ENTRY("MAIN BATTERY", OSD_MAIN_BATT_VOLTAGE), @@ -257,7 +257,7 @@ _Static_assert(ARRAYLEN(menuOsdElemsEntries) - 3 + 1 == OSD_ITEM_COUNT, "missing const CMS_Menu menuOsdElements = { #ifdef CMS_MENU_DEBUG - .GUARD_text = "MENUOSDELEMS", + .GUARD_text = "MENUOSDE", .GUARD_type = OME_MENU, #endif .onEnter = osdElementsOnEnter, @@ -290,7 +290,7 @@ static const OSD_Entry cmsx_menuOsdLayoutEntries[] = const CMS_Menu cmsx_menuOsdLayout = { #ifdef CMS_MENU_DEBUG - .GUARD_text = "MENULAYOUT", + .GUARD_text = "MENUOSDL", .GUARD_type = OME_MENU, #endif .onEnter = NULL, @@ -317,4 +317,53 @@ static long osdElementsOnExit(const OSD_Entry *from) return 0; } +static const OSD_Entry menuOsdSettingsEntries[] = { + OSD_LABEL_ENTRY("--- OSD SETTINGS ---"), + + OSD_SETTING_ENTRY("VOLT. DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), + OSD_SETTING_ENTRY("COORD. DIGITS", SETTING_OSD_COORDINATE_DIGITS), + OSD_SETTING_ENTRY("CROSSHAIRS STYLE", SETTING_OSD_CROSSHAIRS_STYLE), + OSD_SETTING_ENTRY("LEFT SCROLL", SETTING_OSD_LEFT_SIDEBAR_SCROLL), + OSD_SETTING_ENTRY("RIGHT SCROLL", SETTING_OSD_RIGHT_SIDEBAR_SCROLL), + OSD_SETTING_ENTRY("SCROLL ARROWS", SETTING_OSD_SIDEBAR_SCROLL_ARROWS), + + + OSD_BACK_ENTRY, + OSD_END_ENTRY, +}; + +static const CMS_Menu cmsx_menuOsdSettings = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUOSDS", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuOsdSettingsEntries, +}; + +static const OSD_Entry menuOsdEntries[] = { + OSD_LABEL_ENTRY("--- OSD ---"), + + OSD_SUBMENU_ENTRY("LAYOUTS", &cmsx_menuOsdLayout), + OSD_SUBMENU_ENTRY("SETTINGS", &cmsx_menuOsdSettings), + OSD_SUBMENU_ENTRY("ALARMS", &cmsx_menuAlarms), + + OSD_BACK_ENTRY, + OSD_END_ENTRY, +}; + + +const CMS_Menu cmsx_menuOsd = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUOSD", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuOsdEntries, +}; + #endif // CMS diff --git a/src/main/cms/cms_menu_osd.h b/src/main/cms/cms_menu_osd.h index e78d4bff18..946f79211c 100644 --- a/src/main/cms/cms_menu_osd.h +++ b/src/main/cms/cms_menu_osd.h @@ -17,5 +17,4 @@ #pragma once -extern const CMS_Menu cmsx_menuAlarms; -extern const CMS_Menu cmsx_menuOsdLayout; +extern const CMS_Menu cmsx_menuOsd; From 5838984b2c999ac11bc2950337ca7bf8c75a06f7 Mon Sep 17 00:00:00 2001 From: Andrey Mironov Date: Thu, 12 Jul 2018 16:40:02 +0300 Subject: [PATCH 47/57] Ensured micros() doesn't return a smaller value on millisecond bound --- src/main/drivers/system.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index fdae14bcdc..7f505d3e0f 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -52,6 +52,7 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn) STATIC_UNIT_TESTED timeUs_t usTicks = 0; // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0; +STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0; // cached value of RCC->CSR uint32_t cachedRccCsrValue; @@ -81,6 +82,7 @@ void SysTick_Handler(void) { ATOMIC_BLOCK(NVIC_PRIO_MAX) { sysTickUptime++; + sysTickValStamp = SysTick->VAL; sysTickPending = 0; (void)(SysTick->CTRL); } @@ -148,12 +150,8 @@ timeUs_t micros(void) do { ms = sysTickUptime; cycle_cnt = SysTick->VAL; - /* - * If the SysTick timer expired during the previous instruction, we need to give it a little time for that - * interrupt to be delivered before we can recheck sysTickUptime: - */ - asm volatile("\tnop\n"); - } while (ms != sysTickUptime); + } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp); + return ((timeUs_t)ms * 1000LL) + (usTicks * 1000LL - (timeUs_t)cycle_cnt) / usTicks; } From b5d588634b0fcba266bc1080cf00e3cd811bdc1f Mon Sep 17 00:00:00 2001 From: junwoo091400 Date: Sat, 14 Jul 2018 00:27:06 +0900 Subject: [PATCH 48/57] Fixed wrong Idx value for States array from UpdateFunction --- src/main/fc/rc_adjustments.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 675c702f8a..e00379b4ca 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -643,7 +643,7 @@ void updateAdjustmentStates(bool canUseRxData) if (canUseRxData && isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) { configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); } else { - adjustmentState_t * const adjustmentState = &adjustmentStates[index]; + adjustmentState_t * const adjustmentState = &adjustmentStates[adjustmentRange->adjustmentIndex]; if (adjustmentState->config == adjustmentConfig) { adjustmentState->config = NULL; } From ca3e537815c1037bdafe4a455a5225782a8cee07 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Fri, 13 Jul 2018 20:31:44 +0200 Subject: [PATCH 49/57] Fix tests --- src/main/drivers/system.c | 2 +- src/test/unit/time_unittest.cc | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 7f505d3e0f..aa4bfcb2c9 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -51,7 +51,7 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn) // cycles per microsecond STATIC_UNIT_TESTED timeUs_t usTicks = 0; // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. -STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0; +STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0; STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0; // cached value of RCC->CSR uint32_t cachedRccCsrValue; diff --git a/src/test/unit/time_unittest.cc b/src/test/unit/time_unittest.cc index 976d0a85fb..8048601ac6 100644 --- a/src/test/unit/time_unittest.cc +++ b/src/test/unit/time_unittest.cc @@ -25,6 +25,7 @@ extern "C" { #include "drivers/time.h" extern timeUs_t usTicks; extern volatile timeMs_t sysTickUptime; + extern volatile timeMs_t sysTickValStamp; } #include "unittest_macros.h" @@ -44,7 +45,7 @@ TEST(TimeUnittest, TestMillis) TEST(TimeUnittest, TestMicros) { usTicks = 168; - SysTick->VAL = 1000 * usTicks; + sysTickValStamp = SysTick->VAL = 1000 * usTicks; sysTickUptime = 0; EXPECT_EQ(0, micros()); sysTickUptime = 1; @@ -58,7 +59,7 @@ TEST(TimeUnittest, TestMicros) EXPECT_EQ(500000000, micros()); sysTickUptime = 0; - SysTick->VAL = 0; + sysTickValStamp = SysTick->VAL = 0; EXPECT_EQ(1000, micros()); sysTickUptime = 1; EXPECT_EQ(2000, micros()); From ea84f16897a64083061b666995b0a691fdcf4a74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Fri, 13 Jul 2018 23:10:15 +0100 Subject: [PATCH 50/57] Show a message when arming with GPS but without HOME Useful for people using extra_arming_safety = OFF, so they can notice they armed without a home position recorded. --- src/main/io/osd.c | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 55ceaba480..c8d360e723 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2683,13 +2683,21 @@ static void osdShowArmed(void) displayWrite(osdDisplayPort, 12, y, "ARMED"); y += 2; - if (STATE(GPS_FIX)) { - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - osdFormatCoordinate(buf, SYM_LON, gpsSol.llh.lon); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); - y += 3; +#if defined(USE_GPS) + if (feature(FEATURE_GPS)) { + if (STATE(GPS_FIX_HOME)) { + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); + y += 3; + } else { + strcpy(buf, "!NO HOME POSITION!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + y += 1; + } } +#endif if (rtcGetDateTime(&dt)) { dateTimeFormatLocal(buf, &dt); From 3ffef998c80cd23bd7e88fde0ac64d20363a1e2d Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sat, 14 Jul 2018 09:42:41 +0200 Subject: [PATCH 51/57] Revert "PWM driver - startup value fix" This reverts commit 4514aad8242bc0d23742d4faa99b6eac6bff3c43. --- src/main/drivers/pwm_output.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index e09f98bfe9..96e9fe85cf 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -76,6 +76,8 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, const timerHardware_t *timerH p->ccr = timerCCR(timerHardware->tim, timerHardware->channel); p->period = period; p->tim = timerHardware->tim; + + *p->ccr = 0; } static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, bool enableOutput) From 48560383fa0701b82d2035a59f32b48cfcb4a8db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 14 Jul 2018 11:41:48 +0100 Subject: [PATCH 52/57] Make the home arrow show only when we know all needed data Home arrow will only be displayed if we have a GPS fix, we have a home direction recorded and we have a proper heading. Otherwise the indicator will blink and it will show a fixed arrow point up while unarmed (so users can see the arrow when configuring the OSD) and a '-' character when armed (so users can see if some sensor has failed during a flight). --- src/main/io/osd.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 55ceaba480..a31359088f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1235,14 +1235,23 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - // There are 16 orientations for the home direction arrow. - // so we use 22.5deg per image, where the 1st image is used - // for [349, 11], the 2nd for [12, 33], etc... - int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); - // Add 11 to the angle, so first character maps to [349, 11] - int homeArrowDir = osdGetHeadingAngle(homeDirection + 11); - unsigned arrowOffset = homeArrowDir * 2 / 45; - buff[0] = SYM_ARROW_UP + arrowOffset; + if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + // There are 16 orientations for the home direction arrow. + // so we use 22.5deg per image, where the 1st image is used + // for [349, 11], the 2nd for [12, 33], etc... + int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); + // Add 11 to the angle, so first character maps to [349, 11] + int homeArrowDir = osdGetHeadingAngle(homeDirection + 11); + unsigned arrowOffset = homeArrowDir * 2 / 45; + buff[0] = SYM_ARROW_UP + arrowOffset; + } else { + // No home or no fix or unknown heading, blink. + // If we're unarmed, show the arrow pointing up so users can see the arrow + // while configuring the OSD. If we're armed, show a '-' indicating that + // we don't know the direction to home. + buff[0] = ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP; + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } buff[1] = 0; break; } From b61d46a4551e347536a65b846156d745a74e78e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 14 Jul 2018 11:59:03 +0100 Subject: [PATCH 53/57] Correctly display coordinates in the (-1, 0) range Fixes #3592 --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 55ceaba480..6d37f4cabc 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -500,7 +500,7 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val) int32_t integerPart = val / GPS_DEGREES_DIVIDER; // Latitude maximum integer width is 3 (-90) while // longitude maximum integer width is 4 (-180). - int integerDigits = tfp_sprintf(buff + 1, "%d", integerPart); + int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", integerPart); // We can show up to 7 digits in decimalPart. Remove // some if needed. int32_t decimalPart = abs(val % GPS_DEGREES_DIVIDER); From a47b6c91d70c5b337dab14aafa0d896909d1728c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 14 Jul 2018 15:04:05 +0100 Subject: [PATCH 54/57] Fix RC adjustments triggering too fast - Make sure we don't read before the adjustmentStates[0] element - Prevent several adjustments using the same slot from resetting the slot state (and hence, its timeout for the next update) when another state using the same slot is active. --- src/main/fc/rc_adjustments.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index e00379b4ca..3f18bfacc5 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -638,12 +638,18 @@ void updateAdjustmentStates(bool canUseRxData) { for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) { const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index); + if (adjustmentRange->adjustmentFunction == ADJUSTMENT_NONE) { + // Range not set up + continue; + } const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; + adjustmentState_t * const adjustmentState = &adjustmentStates[adjustmentRange->adjustmentIndex]; if (canUseRxData && isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) { - configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); + if (!adjustmentState->config) { + configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); + } } else { - adjustmentState_t * const adjustmentState = &adjustmentStates[adjustmentRange->adjustmentIndex]; if (adjustmentState->config == adjustmentConfig) { adjustmentState->config = NULL; } From 5575617cd0a3694eb8f2519fb7aa64adac46b6c0 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Thu, 12 Jul 2018 00:39:25 +0200 Subject: [PATCH 55/57] Change default ALT PIDs for fixed wing --- src/main/flight/pid.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index acd972f436..5e90e5986e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -156,9 +156,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, }, [PID_HEADING] = { 60, 0, 0 }, [PID_POS_Z] = { - .P = 50, // FW_POS_Z_P * 100 - .I = 0, // not used - .D = 0, // not used + .P = 40, // FW_POS_Z_P * 100 + .I = 5, // not used + .D = 10, // not used }, [PID_POS_XY] = { .P = 75, // FW_NAV_P * 100 From fa49e84507ece2bf10a4029938608ab49526f495 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Fri, 13 Jul 2018 19:01:46 +0200 Subject: [PATCH 56/57] Update PID init comments --- src/main/flight/pid.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5e90e5986e..4803e53b7f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -137,8 +137,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .D = 0, // not used }, [PID_VEL_Z] = { - .P = 100, // NAV_VEL_Z_P * 100 - .I = 50, // NAV_VEL_Z_I * 100 + .P = 100, // NAV_VEL_Z_P * 66.7 + .I = 50, // NAV_VEL_Z_I * 20 .D = 10, // NAV_VEL_Z_D * 100 } } @@ -156,14 +156,14 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, }, [PID_HEADING] = { 60, 0, 0 }, [PID_POS_Z] = { - .P = 40, // FW_POS_Z_P * 100 - .I = 5, // not used - .D = 10, // not used + .P = 40, // FW_POS_Z_P * 10 + .I = 5, // FW_POS_Z_I * 10 + .D = 10, // FW_POS_Z_D * 10 }, [PID_POS_XY] = { - .P = 75, // FW_NAV_P * 100 - .I = 5, // FW_NAV_I * 100 - .D = 8, // FW_NAV_D * 100 + .P = 75, // FW_POS_XY_P * 100 + .I = 5, // FW_POS_XY_I * 100 + .D = 8, // FW_POS_XY_D * 100 } } }, From 8277edd2f31c833078357f8888a0926ad4ce9a71 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Sat, 14 Jul 2018 17:14:13 +0200 Subject: [PATCH 57/57] Make sbus sync delay configurable --- src/main/fc/settings.yaml | 4 ++++ src/main/rx/rx.c | 3 ++- src/main/rx/rx.h | 2 +- src/main/rx/sbus.c | 4 +--- src/main/rx/sbus.h | 2 ++ 5 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f26b902293..51a8604e4c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -349,6 +349,10 @@ groups: - name: rssi_invert field: rssiInvert type: bool + - name: sbus_sync_interval + field: sbusSyncInterval + min: 10000 + max: 500 - name: rc_smoothing field: rcSmoothing type: bool diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index d6abb4aeef..e851bd6dc3 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -100,7 +100,7 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; -PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 5); #ifndef RX_SPI_DEFAULT_PROTOCOL #define RX_SPI_DEFAULT_PROTOCOL 0 @@ -129,6 +129,7 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .rssi_channel = 0, .rssi_scale = RSSI_SCALE_DEFAULT, .rssiInvert = 0, + .sbusSyncInterval = SBUS_DEFAULT_INTERFRAME_DELAY_US, .rcSmoothing = 1, ); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index d577dd6362..ced4f56df5 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -121,7 +121,7 @@ typedef struct rxConfig_s { uint8_t rssi_channel; uint8_t rssi_scale; uint8_t rssiInvert; - uint16_t __reserved; // was micrd + uint16_t sbusSyncInterval; uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end uint16_t rx_min_usec; diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 3e30692e29..0682e4683a 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -52,8 +52,6 @@ * time to send frame: 3ms. */ -#define SBUS_MIN_INTERFRAME_DELAY_US 3000 // According to FrSky interframe is 6.67ms, we go smaller just in case - #define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2) #define SBUS_FRAME_BEGIN_BYTE 0x0F @@ -114,7 +112,7 @@ static void sbusDataReceive(uint16_t c, void *data) sbusFrameData->lastActivityTimeUs = currentTimeUs; // Handle inter-frame gap. We dwell in STATE_SBUS_WAIT_SYNC state ignoring all incoming bytes until we get long enough quite period on the wire - if (sbusFrameData->state == STATE_SBUS_WAIT_SYNC && timeSinceLastByteUs >= SBUS_MIN_INTERFRAME_DELAY_US) { + if (sbusFrameData->state == STATE_SBUS_WAIT_SYNC && timeSinceLastByteUs >= rxConfig()->sbusSyncInterval) { DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_INTERFRAME_TIME, timeSinceLastByteUs); sbusFrameData->state = STATE_SBUS_SYNC; } diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h index 1dbcc51942..b12a030f28 100644 --- a/src/main/rx/sbus.h +++ b/src/main/rx/sbus.h @@ -17,4 +17,6 @@ #pragma once +#define SBUS_DEFAULT_INTERFRAME_DELAY_US 3000 // According to FrSky interframe is 6.67ms, we go smaller just in case + bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);