From a9dae25e3a3d4c8b2870a4428d128103f6b88cf2 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 19 Dec 2025 10:37:47 -0600 Subject: [PATCH 01/22] msp: add minimum power index to MSP_VTX_CONFIG MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds minPowerIndex (byte 12) to MSP_VTX_CONFIG response to indicate the minimum valid power index for the VTX device. - MSP VTX: minPowerIndex = 0 (supports power off at index 0) - SmartAudio/Tramp: minPowerIndex = 1 (power off not supported) This allows configurator to correctly display all available power levels without hardcoding device-specific logic. Backward compatible: old configurators will ignore the extra byte. Related: iNavFlight/inav-configurator#2486 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude Sonnet 4.5 --- .gitignore | 6 ++++++ src/main/fc/fc_msp.c | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/.gitignore b/.gitignore index 9b72fa8305a..ccfda8cc1c3 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,9 @@ launch.json # Assitnow token and files for test script tokens.yaml *.ubx + +# Local development files +.semgrepignore +build_sitl/ +CLAUDE.md +brother/ diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 53daf9f524e..12c3f8268f5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1526,6 +1526,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, vtxDevice->capability.bandCount); sbufWriteU8(dst, vtxDevice->capability.channelCount); sbufWriteU8(dst, vtxDevice->capability.powerCount); + + uint8_t minPowerIndex = 1; + if (deviceType == VTXDEV_MSP) { + minPowerIndex = 0; + } + sbufWriteU8(dst, minPowerIndex); } else { sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured From cebc906a0d7dd3d5c5826bfe2205bd4ff9c2d432 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 22 Dec 2025 01:49:51 -0600 Subject: [PATCH 02/22] Refactor OMNIBUSF4 targets into separate directories Split OMNIBUSF4 mega-target into 4 directories to improve maintainability: - DYSF4: DYSF4PRO, DYSF4PROV2 - OMNIBUSF4: Base OMNIBUSF4 variant only - OMNIBUSF4PRO: OMNIBUSF4PRO, OMNIBUSF4V3, OMNIBUSF4V3_ICM - OMNIBUSF4V3_SS: Softserial variants (S6_SS, S5S6_SS, S5_S6_2SS) Each directory now contains only the code relevant to its variants, with irrelevant preprocessor conditionals removed. Changes: - Reduced DYSF4/target.h from 271 to 169 lines (37% reduction) - Reduced preprocessor conditionals from 18 to 4 in DYSF4 files - All 11 targets build successfully and produce identical binaries --- src/main/target/DYSF4/CMakeLists.txt | 2 + src/main/target/DYSF4/target.c | 47 ++++ src/main/target/DYSF4/target.h | 169 +++++++++++++ src/main/target/OMNIBUSF4/CMakeLists.txt | 10 - src/main/target/OMNIBUSF4PRO/CMakeLists.txt | 5 + src/main/target/OMNIBUSF4PRO/target.c | 61 +++++ src/main/target/OMNIBUSF4PRO/target.h | 232 ++++++++++++++++++ src/main/target/OMNIBUSF4V3_SS/CMakeLists.txt | 4 + src/main/target/OMNIBUSF4V3_SS/target.c | 60 +++++ src/main/target/OMNIBUSF4V3_SS/target.h | 209 ++++++++++++++++ 10 files changed, 789 insertions(+), 10 deletions(-) create mode 100644 src/main/target/DYSF4/CMakeLists.txt create mode 100644 src/main/target/DYSF4/target.c create mode 100644 src/main/target/DYSF4/target.h create mode 100644 src/main/target/OMNIBUSF4PRO/CMakeLists.txt create mode 100644 src/main/target/OMNIBUSF4PRO/target.c create mode 100644 src/main/target/OMNIBUSF4PRO/target.h create mode 100644 src/main/target/OMNIBUSF4V3_SS/CMakeLists.txt create mode 100644 src/main/target/OMNIBUSF4V3_SS/target.c create mode 100644 src/main/target/OMNIBUSF4V3_SS/target.h diff --git a/src/main/target/DYSF4/CMakeLists.txt b/src/main/target/DYSF4/CMakeLists.txt new file mode 100644 index 00000000000..ec00b5acc49 --- /dev/null +++ b/src/main/target/DYSF4/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32f405xg(DYSF4PRO) +target_stm32f405xg(DYSF4PROV2) diff --git a/src/main/target/DYSF4/target.c b/src/main/target/DYSF4/target.c new file mode 100644 index 00000000000..2eed0188f97 --- /dev/null +++ b/src/main/target/DYSF4/target.c @@ -0,0 +1,47 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/bus.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 + + // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_OUTPUT_AUTO }, // MOTOR_3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT MOTOR, SERVO or LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT + + + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX + DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/DYSF4/target.h b/src/main/target/DYSF4/target.h new file mode 100644 index 00000000000..24d5aa0dac4 --- /dev/null +++ b/src/main/target/DYSF4/target.h @@ -0,0 +1,169 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +//Same target as OMNIBUSF4PRO with LED strip in M5 +//Same target as OMNIBUSF4V3 with softserial in M5 and M6 + +#if defined(DYSF4PRO) +#define TARGET_BOARD_IDENTIFIER "DYS4" +#elif defined(DYSF4PROV2) +#define TARGET_BOARD_IDENTIFIER "DY42" +#else +#define TARGET_BOARD_IDENTIFIER "OBF4" +#endif + +#if defined(DYSF4PRO) || defined(DYSF4PROV2) +#define USBD_PRODUCT_STRING "DysF4Pro" +#else +#define USBD_PRODUCT_STRING "Omnibus F4" +#endif + +#define LED0 PB5 + +#define BEEPER PB4 +#define BEEPER_INVERTED + +#if defined(DYSF4PROV2) +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define I2C_EXT_BUS BUS_I2C1 +#else +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE_2_SHARES_UART3 +#define I2C_EXT_BUS BUS_I2C2 +#endif + +#define UG2864_I2C_BUS I2C_EXT_BUS + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + + #define USE_IMU_MPU6000 + #define IMU_MPU6000_ALIGN CW180_DEG + +// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 + +#define USE_MAG +#define MAG_I2C_BUS I2C_EXT_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS I2C_EXT_BUS + +#define USE_BARO + + #define BARO_I2C_BUS I2C_EXT_BUS + #define USE_BARO_BMP085 + #define USE_BARO_BMP280 + #define USE_BARO_MS5611 + +#define PITOT_I2C_BUS I2C_EXT_BUS + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS I2C_EXT_BUS + +#define USE_VCP +#define VBUS_SENSING_PIN PC5 +#define VBUS_SENSING_ENABLED + + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO +#define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 + + +#define USE_SPI_DEVICE_3 + #define SPI3_NSS_PIN PB3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + #define M25P16_CS_PIN SPI3_NSS_PIN + #define M25P16_SPI_BUS BUS_SPI3 + #define USE_FLASHFS + #define USE_FLASH_M25P16 + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 + +#ifdef DYSF4PRO + #define ADC_CHANNEL_3_PIN PC3 +#else + #define ADC_CHANNEL_3_PIN PA0 +#endif + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) + +#define USE_LED_STRIP + #define WS2811_PIN PA1 + +#define DISABLE_RX_PWM_FEATURE +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) + +#define USE_SPEKTRUM_BIND +#define BIND_PIN PB11 // USART3 RX + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// Number of available PWM outputs +#define MAX_PWM_OUTPUT_PORTS 6 +#define TARGET_MOTOR_COUNT 6 +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + diff --git a/src/main/target/OMNIBUSF4/CMakeLists.txt b/src/main/target/OMNIBUSF4/CMakeLists.txt index a6ccb483bf5..28f13c7cd5b 100644 --- a/src/main/target/OMNIBUSF4/CMakeLists.txt +++ b/src/main/target/OMNIBUSF4/CMakeLists.txt @@ -1,11 +1 @@ -target_stm32f405xg(DYSF4PRO) -target_stm32f405xg(DYSF4PROV2) target_stm32f405xg(OMNIBUSF4) -# the OMNIBUSF4SD has an SDCARD instead of flash, a BMP280 baro and therefore a slightly different ppm/pwm and SPI mapping -target_stm32f405xg(OMNIBUSF4PRO) -target_stm32f405xg(OMNIBUSF4V3_S5_S6_2SS) -target_stm32f405xg(OMNIBUSF4V3_S5S6_SS) -target_stm32f405xg(OMNIBUSF4V3_S6_SS) -# OMNIBUSF4V3 is a (almost identical) variant of OMNIBUSF4PRO target, -# except for an inverter on UART6. -target_stm32f405xg(OMNIBUSF4V3) diff --git a/src/main/target/OMNIBUSF4PRO/CMakeLists.txt b/src/main/target/OMNIBUSF4PRO/CMakeLists.txt new file mode 100644 index 00000000000..e09984ac0a9 --- /dev/null +++ b/src/main/target/OMNIBUSF4PRO/CMakeLists.txt @@ -0,0 +1,5 @@ +# OMNIBUSF4PRO has SD card, BMP280 baro +target_stm32f405xg(OMNIBUSF4PRO) +# OMNIBUSF4V3 is similar to PRO but with UART6 inverter +target_stm32f405xg(OMNIBUSF4V3) +target_stm32f405xg(OMNIBUSF4V3_ICM SKIP_RELEASES) diff --git a/src/main/target/OMNIBUSF4PRO/target.c b/src/main/target/OMNIBUSF4PRO/target.c new file mode 100644 index 00000000000..9aad85a14af --- /dev/null +++ b/src/main/target/OMNIBUSF4PRO/target.c @@ -0,0 +1,61 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/bus.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 + + // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_OUTPUT_AUTO }, // MOTOR_3 +#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !(defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) + + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT +#else + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT MOTOR, SERVO or LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT +#endif + +#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) +#endif + +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) + // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN +#else + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN +#endif + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX + DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUSF4PRO/target.h b/src/main/target/OMNIBUSF4PRO/target.h new file mode 100644 index 00000000000..74829342c2c --- /dev/null +++ b/src/main/target/OMNIBUSF4PRO/target.h @@ -0,0 +1,232 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +//Same target as OMNIBUSF4PRO with LED strip in M5 +#ifdef OMNIBUSF4PRO_LEDSTRIPM5 +#define OMNIBUSF4PRO +#endif +//Same target as OMNIBUSF4V3 with softserial in M5 and M6 + +#ifdef OMNIBUSF4PRO +#define TARGET_BOARD_IDENTIFIER "OBSD" +#elif defined(OMNIBUSF4V3) +#define TARGET_BOARD_IDENTIFIER "OB43" +#else +#define TARGET_BOARD_IDENTIFIER "OBF4" +#endif + +#define USBD_PRODUCT_STRING "Omnibus F4" + +#define LED0 PB5 + +#define BEEPER PB4 +#define BEEPER_INVERTED + +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE_2_SHARES_UART3 +#define I2C_EXT_BUS BUS_I2C2 + +#define UG2864_I2C_BUS I2C_EXT_BUS + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) + #define USE_IMU_MPU6000 + #define IMU_MPU6000_ALIGN CW270_DEG +#else + #define USE_IMU_MPU6000 + #define IMU_MPU6000_ALIGN CW180_DEG +#endif + +// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) + #define MPU6500_CS_PIN MPU6000_CS_PIN + #define MPU6500_SPI_BUS MPU6000_SPI_BUS + #define USE_IMU_MPU6500 + #define IMU_MPU6500_ALIGN IMU_MPU6000_ALIGN + + //BMI270 + #define USE_IMU_BMI270 + #define IMU_BMI270_ALIGN IMU_MPU6000_ALIGN + #define BMI270_SPI_BUS MPU6000_SPI_BUS + #define BMI270_CS_PIN MPU6000_CS_PIN +#endif + +#define USE_MAG +#define MAG_I2C_BUS I2C_EXT_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS I2C_EXT_BUS + +#define USE_BARO + +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) + #define USE_BARO_BMP280 + #define BMP280_SPI_BUS BUS_SPI3 + #define BMP280_CS_PIN PB3 // v1 + // Support external barometers + #define BARO_I2C_BUS I2C_EXT_BUS + #define USE_BARO_BMP085 + #define USE_BARO_MS5611 +#else + #define BARO_I2C_BUS I2C_EXT_BUS + #define USE_BARO_BMP085 + #define USE_BARO_BMP280 + #define USE_BARO_MS5611 +#endif + +#define PITOT_I2C_BUS I2C_EXT_BUS + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS I2C_EXT_BUS + +#define USE_VCP +#define VBUS_SENSING_PIN PC5 +#define VBUS_SENSING_ENABLED + +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#define USE_UART_INVERTER +#endif + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#if defined(OMNIBUSF4PRO) +#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 +#define UART3_TX_PIN PB10 + +#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) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX +#define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 + +#else // One softserial on versions other than OMNIBUSF4V3 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO +#define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 +#endif + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 + +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) + #define USE_SPI_DEVICE_2 + #define SPI2_NSS_PIN PB12 + #define SPI2_SCK_PIN PB13 + #define SPI2_MISO_PIN PB14 + #define SPI2_MOSI_PIN PB15 +#endif + +#define USE_SPI_DEVICE_3 +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) + #define SPI3_NSS_PIN PA15 +#else + #define SPI3_NSS_PIN PB3 +#endif +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + #define USE_SDCARD + #define USE_SDCARD_SPI + + #define SDCARD_SPI_BUS BUS_SPI2 + #define SDCARD_CS_PIN SPI2_NSS_PIN + + #define SDCARD_DETECT_PIN PB7 + #define SDCARD_DETECT_INVERTED +#else + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + #define M25P16_CS_PIN SPI3_NSS_PIN + #define M25P16_SPI_BUS BUS_SPI3 + #define USE_FLASHFS + #define USE_FLASH_M25P16 +#endif + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 + + #define ADC_CHANNEL_3_PIN PA0 + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) + +#define USE_LED_STRIP +#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) + #define WS2811_PIN PB6 +#else + #define WS2811_PIN PA1 +#endif + +#define DISABLE_RX_PWM_FEATURE +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) + +#define USE_SPEKTRUM_BIND +#define BIND_PIN PB11 // USART3 RX + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// Number of available PWM outputs +#define MAX_PWM_OUTPUT_PORTS 6 +#define TARGET_MOTOR_COUNT 6 +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#ifdef OMNIBUSF4PRO +#define CURRENT_METER_SCALE 265 +#endif diff --git a/src/main/target/OMNIBUSF4V3_SS/CMakeLists.txt b/src/main/target/OMNIBUSF4V3_SS/CMakeLists.txt new file mode 100644 index 00000000000..321eeb211d1 --- /dev/null +++ b/src/main/target/OMNIBUSF4V3_SS/CMakeLists.txt @@ -0,0 +1,4 @@ +# OMNIBUSF4V3 softserial variants with different S5/S6 timer configurations +target_stm32f405xg(OMNIBUSF4V3_S6_SS) +target_stm32f405xg(OMNIBUSF4V3_S5S6_SS) +target_stm32f405xg(OMNIBUSF4V3_S5_S6_2SS) diff --git a/src/main/target/OMNIBUSF4V3_SS/target.c b/src/main/target/OMNIBUSF4V3_SS/target.c new file mode 100644 index 00000000000..ed38a77a51a --- /dev/null +++ b/src/main/target/OMNIBUSF4V3_SS/target.c @@ -0,0 +1,60 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/bus.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 + + // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_OUTPUT_AUTO }, // MOTOR_3 +#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !(defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) + + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT +#elif defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) + DEF_TIM(TIM5, CH2, PA1, TIM_USE_ANY, 0, 0), // S5_OUT SOFTSERIAL + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL +#elif defined(OMNIBUSF4V3_S6_SS) + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL +#else + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT MOTOR, SERVO or LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT +#endif + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) + + // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX + DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUSF4V3_SS/target.h b/src/main/target/OMNIBUSF4V3_SS/target.h new file mode 100644 index 00000000000..4d3355eed9c --- /dev/null +++ b/src/main/target/OMNIBUSF4V3_SS/target.h @@ -0,0 +1,209 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +//Same target as OMNIBUSF4PRO with LED strip in M5 +//Same target as OMNIBUSF4V3 with softserial in M5 and M6 +#if defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) +#define OMNIBUSF4V3 +#endif + +#define TARGET_BOARD_IDENTIFIER "OB43" + +#define USBD_PRODUCT_STRING "Omnibus F4" + +#define LED0 PB5 + +#define BEEPER PB4 +#define BEEPER_INVERTED + +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE_2_SHARES_UART3 +#define I2C_EXT_BUS BUS_I2C2 + +#define UG2864_I2C_BUS I2C_EXT_BUS + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + + #define USE_IMU_MPU6000 + #define IMU_MPU6000_ALIGN CW270_DEG + +// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 + #define MPU6500_CS_PIN MPU6000_CS_PIN + #define MPU6500_SPI_BUS MPU6000_SPI_BUS + #define USE_IMU_MPU6500 + #define IMU_MPU6500_ALIGN IMU_MPU6000_ALIGN + + //BMI270 + #define USE_IMU_BMI270 + #define IMU_BMI270_ALIGN IMU_MPU6000_ALIGN + #define BMI270_SPI_BUS MPU6000_SPI_BUS + #define BMI270_CS_PIN MPU6000_CS_PIN + +#define USE_MAG +#define MAG_I2C_BUS I2C_EXT_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS I2C_EXT_BUS + +#define USE_BARO + + #define USE_BARO_BMP280 + #define BMP280_SPI_BUS BUS_SPI3 + #define BMP280_CS_PIN PB3 // v1 + // Support external barometers + #define BARO_I2C_BUS I2C_EXT_BUS + #define USE_BARO_BMP085 + #define USE_BARO_MS5611 + +#define PITOT_I2C_BUS I2C_EXT_BUS + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS I2C_EXT_BUS + +#define USE_VCP +#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 USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + #define INVERTER_PIN_UART6_RX PC8 + #define INVERTER_PIN_UART6_TX PC9 + +#if defined(OMNIBUSF4V3) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX +#define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 + +#elif defined(OMNIBUSF4V3_S6_SS) // one softserial on S6 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PA8 // S6 output +#define SOFTSERIAL_1_TX_PIN PA8 // S6 output + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 + +#elif defined(OMNIBUSF4V3_S5S6_SS) // one softserial on S5/RX S6/TX +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PA1 // S5 output +#define SOFTSERIAL_1_TX_PIN PA8 // S6 output + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 + +#elif defined(OMNIBUSF4V3_S5_S6_2SS) // two softserials, one on S5 and one on S6 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PA1 // S5 output +#define SOFTSERIAL_1_TX_PIN PA1 // S5 output + +#define USE_SOFTSERIAL2 +#define SOFTSERIAL_2_RX_PIN PA8 // S6 output +#define SOFTSERIAL_2_TX_PIN PA8 // S6 output + +#define SERIAL_PORT_COUNT 6 // VCP, USART1, USART3, USART6, SOFTSERIAL1, SOFTSERIAL2 + +#else // One softserial on versions other than OMNIBUSF4V3 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO +#define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 +#endif + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 + + #define USE_SPI_DEVICE_2 + #define SPI2_NSS_PIN PB12 + #define SPI2_SCK_PIN PB13 + #define SPI2_MISO_PIN PB14 + #define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 + #define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + #define USE_SDCARD + #define USE_SDCARD_SPI + + #define SDCARD_SPI_BUS BUS_SPI2 + #define SDCARD_CS_PIN SPI2_NSS_PIN + + #define SDCARD_DETECT_PIN PB7 + #define SDCARD_DETECT_INVERTED + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 + + #define ADC_CHANNEL_3_PIN PA0 + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) + +#define USE_LED_STRIP + #define WS2811_PIN PB6 + +#define DISABLE_RX_PWM_FEATURE +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) + +#define USE_SPEKTRUM_BIND +#define BIND_PIN PB11 // USART3 RX + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// Number of available PWM outputs +#define MAX_PWM_OUTPUT_PORTS 6 +#define TARGET_MOTOR_COUNT 6 +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + From 476a0cbcf94f232b610fa45944821b0c7b6d15ca Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 22 Dec 2025 02:15:55 -0600 Subject: [PATCH 03/22] Clean up dead code and obsolete comments from OMNIBUS split Remove preprocessor conditionals that can never be true in split directories: - OMNIBUSF4PRO: Remove checks for _S6_SS variants (in different directory) - OMNIBUSF4V3_SS: Remove checks for OMNIBUSF4PRO/V3 (in different directory) - Remove OMNIBUSF4PRO_LEDSTRIPM5 references (target no longer exists) Update comments to accurately describe what targets each directory contains. All affected targets build successfully. --- src/main/target/DYSF4/target.h | 3 +-- src/main/target/OMNIBUSF4PRO/target.c | 6 ------ src/main/target/OMNIBUSF4PRO/target.h | 17 +++++------------ src/main/target/OMNIBUSF4V3_SS/target.c | 9 +-------- src/main/target/OMNIBUSF4V3_SS/target.h | 13 +++---------- 5 files changed, 10 insertions(+), 38 deletions(-) diff --git a/src/main/target/DYSF4/target.h b/src/main/target/DYSF4/target.h index 24d5aa0dac4..2578eb6b424 100644 --- a/src/main/target/DYSF4/target.h +++ b/src/main/target/DYSF4/target.h @@ -17,8 +17,7 @@ #pragma once -//Same target as OMNIBUSF4PRO with LED strip in M5 -//Same target as OMNIBUSF4V3 with softserial in M5 and M6 +// This directory contains: DYSF4PRO, DYSF4PROV2 #if defined(DYSF4PRO) #define TARGET_BOARD_IDENTIFIER "DYS4" diff --git a/src/main/target/OMNIBUSF4PRO/target.c b/src/main/target/OMNIBUSF4PRO/target.c index 9aad85a14af..10a1aaabf5a 100644 --- a/src/main/target/OMNIBUSF4PRO/target.c +++ b/src/main/target/OMNIBUSF4PRO/target.c @@ -31,14 +31,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_OUTPUT_AUTO }, // MOTOR_3 -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !(defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) - DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT -#else - DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT MOTOR, SERVO or LED - DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT -#endif #if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) diff --git a/src/main/target/OMNIBUSF4PRO/target.h b/src/main/target/OMNIBUSF4PRO/target.h index 74829342c2c..6e7ed8aa9b3 100644 --- a/src/main/target/OMNIBUSF4PRO/target.h +++ b/src/main/target/OMNIBUSF4PRO/target.h @@ -17,11 +17,8 @@ #pragma once -//Same target as OMNIBUSF4PRO with LED strip in M5 -#ifdef OMNIBUSF4PRO_LEDSTRIPM5 -#define OMNIBUSF4PRO -#endif -//Same target as OMNIBUSF4V3 with softserial in M5 and M6 +// This directory contains: OMNIBUSF4PRO, OMNIBUSF4V3, OMNIBUSF4V3_ICM +// Softserial variants are in separate OMNIBUSF4V3_SS/ directory #ifdef OMNIBUSF4PRO #define TARGET_BOARD_IDENTIFIER "OBSD" @@ -126,14 +123,14 @@ #define INVERTER_PIN_UART6_TX PC9 #endif -#if defined(OMNIBUSF4V3) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) +#if defined(OMNIBUSF4V3) #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX #define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX #define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 -#else // One softserial on versions other than OMNIBUSF4V3 +#else // OMNIBUSF4PRO #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO #define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO @@ -202,11 +199,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_LED_STRIP -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) - #define WS2811_PIN PB6 -#else - #define WS2811_PIN PA1 -#endif +#define WS2811_PIN PB6 #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) diff --git a/src/main/target/OMNIBUSF4V3_SS/target.c b/src/main/target/OMNIBUSF4V3_SS/target.c index ed38a77a51a..3aa99b0ea09 100644 --- a/src/main/target/OMNIBUSF4V3_SS/target.c +++ b/src/main/target/OMNIBUSF4V3_SS/target.c @@ -31,19 +31,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_OUTPUT_AUTO }, // MOTOR_3 -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !(defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) - - DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT -#elif defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) +#if defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) DEF_TIM(TIM5, CH2, PA1, TIM_USE_ANY, 0, 0), // S5_OUT SOFTSERIAL DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL #elif defined(OMNIBUSF4V3_S6_SS) DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL -#else - DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT MOTOR, SERVO or LED - DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT #endif DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) diff --git a/src/main/target/OMNIBUSF4V3_SS/target.h b/src/main/target/OMNIBUSF4V3_SS/target.h index 4d3355eed9c..973ccdf3920 100644 --- a/src/main/target/OMNIBUSF4V3_SS/target.h +++ b/src/main/target/OMNIBUSF4V3_SS/target.h @@ -17,8 +17,8 @@ #pragma once -//Same target as OMNIBUSF4PRO with LED strip in M5 -//Same target as OMNIBUSF4V3 with softserial in M5 and M6 +// This directory contains: OMNIBUSF4V3_S6_SS, OMNIBUSF4V3_S5S6_SS, OMNIBUSF4V3_S5_S6_2SS +// Softserial variants of OMNIBUSF4V3 with different S5/S6 timer configurations #if defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) #define OMNIBUSF4V3 #endif @@ -99,14 +99,7 @@ #define INVERTER_PIN_UART6_RX PC8 #define INVERTER_PIN_UART6_TX PC9 -#if defined(OMNIBUSF4V3) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX -#define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX - -#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 - -#elif defined(OMNIBUSF4V3_S6_SS) // one softserial on S6 +#if defined(OMNIBUSF4V3_S6_SS) // one softserial on S6 #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_RX_PIN PA8 // S6 output #define SOFTSERIAL_1_TX_PIN PA8 // S6 output From c79e53a61358b8d34a02d59b7c62c726c4b9188f Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 22 Dec 2025 02:21:32 -0600 Subject: [PATCH 04/22] Remove all dead code from OMNIBUSF4 directory MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The OMNIBUSF4 directory now only contains the base OMNIBUSF4 target. All other variants (OMNIBUSF4PRO, OMNIBUSF4V3, DYSF4, etc.) are in separate directories. Used unifdef to remove all conditionals for non-OMNIBUSF4 targets: - Removed checks for OMNIBUSF4PRO, OMNIBUSF4V3, DYSF4PRO, DYSF4PROV2 - Removed checks for softserial variants (_S6_SS, etc.) - Removed LEDSTRIPM5 references Results: - target.h: 280 → 147 lines (47% reduction, 133 lines removed) - target.c: cleaned (7 conditionals removed) - All conditionals removed: 0 remaining in both files - OMNIBUSF4 target builds successfully The OMNIBUSF4 directory now contains only the code specific to the base OMNIBUSF4 variant with no dead branches. --- src/main/target/OMNIBUSF4/target.c | 20 ----- src/main/target/OMNIBUSF4/target.h | 135 +---------------------------- 2 files changed, 1 insertion(+), 154 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index be85e9fbce3..2eed0188f97 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -31,32 +31,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_OUTPUT_AUTO }, // MOTOR_3 -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !(defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) - - DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT -#elif defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) - DEF_TIM(TIM5, CH2, PA1, TIM_USE_ANY, 0, 0), // S5_OUT SOFTSERIAL - DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL -#elif defined(OMNIBUSF4V3_S6_SS) - DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL -#else DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT MOTOR, SERVO or LED DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT -#endif -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) -#endif -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN -#else // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN -#endif DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 2c5a27afbb6..1d8188beadc 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -18,76 +18,31 @@ #pragma once //Same target as OMNIBUSF4PRO with LED strip in M5 -#ifdef OMNIBUSF4PRO_LEDSTRIPM5 -#define OMNIBUSF4PRO -#endif //Same target as OMNIBUSF4V3 with softserial in M5 and M6 -#if defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) -#define OMNIBUSF4V3 -#endif - -#ifdef OMNIBUSF4PRO -#define TARGET_BOARD_IDENTIFIER "OBSD" -#elif defined(OMNIBUSF4V3) -#define TARGET_BOARD_IDENTIFIER "OB43" -#elif defined(DYSF4PRO) -#define TARGET_BOARD_IDENTIFIER "DYS4" -#elif defined(DYSF4PROV2) -#define TARGET_BOARD_IDENTIFIER "DY42" -#else + #define TARGET_BOARD_IDENTIFIER "OBF4" -#endif -#if defined(DYSF4PRO) || defined(DYSF4PROV2) -#define USBD_PRODUCT_STRING "DysF4Pro" -#else #define USBD_PRODUCT_STRING "Omnibus F4" -#endif #define LED0 PB5 #define BEEPER PB4 #define BEEPER_INVERTED -#if defined(DYSF4PROV2) -#define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 -#define I2C_EXT_BUS BUS_I2C1 -#else #define USE_I2C #define USE_I2C_DEVICE_2 #define I2C_DEVICE_2_SHARES_UART3 #define I2C_EXT_BUS BUS_I2C2 -#endif #define UG2864_I2C_BUS I2C_EXT_BUS #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define USE_IMU_MPU6000 - #define IMU_MPU6000_ALIGN CW270_DEG -#else #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG -#endif // Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define MPU6500_CS_PIN MPU6000_CS_PIN - #define MPU6500_SPI_BUS MPU6000_SPI_BUS - #define USE_IMU_MPU6500 - #define IMU_MPU6500_ALIGN IMU_MPU6000_ALIGN - - //BMI270 - #define USE_IMU_BMI270 - #define IMU_BMI270_ALIGN IMU_MPU6000_ALIGN - #define BMI270_SPI_BUS MPU6000_SPI_BUS - #define BMI270_CS_PIN MPU6000_CS_PIN -#endif #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS @@ -97,20 +52,10 @@ #define USE_BARO -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define USE_BARO_BMP280 - #define BMP280_SPI_BUS BUS_SPI3 - #define BMP280_CS_PIN PB3 // v1 - // Support external barometers - #define BARO_I2C_BUS I2C_EXT_BUS - #define USE_BARO_BMP085 - #define USE_BARO_MS5611 -#else #define BARO_I2C_BUS I2C_EXT_BUS #define USE_BARO_BMP085 #define USE_BARO_BMP280 #define USE_BARO_MS5611 -#endif #define PITOT_I2C_BUS I2C_EXT_BUS @@ -121,17 +66,11 @@ #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) -#define USE_UART_INVERTER -#endif #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 -#if defined(OMNIBUSF4PRO) -#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 @@ -140,50 +79,12 @@ #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) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX -#define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX - -#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 - -#elif defined(OMNIBUSF4V3_S6_SS) // one softserial on S6 -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PA8 // S6 output -#define SOFTSERIAL_1_TX_PIN PA8 // S6 output - -#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 - -#elif defined(OMNIBUSF4V3_S5S6_SS) // one softserial on S5/RX S6/TX -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PA1 // S5 output -#define SOFTSERIAL_1_TX_PIN PA8 // S6 output - -#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 - -#elif defined(OMNIBUSF4V3_S5_S6_2SS) // two softserials, one on S5 and one on S6 -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PA1 // S5 output -#define SOFTSERIAL_1_TX_PIN PA1 // S5 output - -#define USE_SOFTSERIAL2 -#define SOFTSERIAL_2_RX_PIN PA8 // S6 output -#define SOFTSERIAL_2_TX_PIN PA8 // S6 output - -#define SERIAL_PORT_COUNT 6 // VCP, USART1, USART3, USART6, SOFTSERIAL1, SOFTSERIAL2 - -#else // One softserial on versions other than OMNIBUSF4V3 #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO #define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO #define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 -#endif #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -193,20 +94,9 @@ #define USE_SPI_DEVICE_1 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define USE_SPI_DEVICE_2 - #define SPI2_NSS_PIN PB12 - #define SPI2_SCK_PIN PB13 - #define SPI2_MISO_PIN PB14 - #define SPI2_MOSI_PIN PB15 -#endif #define USE_SPI_DEVICE_3 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define SPI3_NSS_PIN PA15 -#else #define SPI3_NSS_PIN PB3 -#endif #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 @@ -215,33 +105,17 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - #define USE_SDCARD - #define USE_SDCARD_SPI - - #define SDCARD_SPI_BUS BUS_SPI2 - #define SDCARD_CS_PIN SPI2_NSS_PIN - - #define SDCARD_DETECT_PIN PB7 - #define SDCARD_DETECT_INVERTED -#else #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define M25P16_CS_PIN SPI3_NSS_PIN #define M25P16_SPI_BUS BUS_SPI3 #define USE_FLASHFS #define USE_FLASH_M25P16 -#endif #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 #define ADC_CHANNEL_2_PIN PC2 -#ifdef DYSF4PRO - #define ADC_CHANNEL_3_PIN PC3 -#else #define ADC_CHANNEL_3_PIN PA0 -#endif #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 #define VBAT_ADC_CHANNEL ADC_CHN_2 @@ -250,11 +124,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_LED_STRIP -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) - #define WS2811_PIN PB6 -#else #define WS2811_PIN PA1 -#endif #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) @@ -275,6 +145,3 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#ifdef OMNIBUSF4PRO -#define CURRENT_METER_SCALE 265 -#endif From e3d0bda8830bd77b93008ed0a59d9ae834b6f97f Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 22 Dec 2025 02:34:18 -0600 Subject: [PATCH 05/22] Fix critical OMNIBUSF4V3_ICM configuration bug and remove dead code MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1. Added alias for OMNIBUSF4V3_ICM → OMNIBUSF4V3 in OMNIBUSF4PRO/target.h This fixes a critical bug where OMNIBUSF4V3_ICM would get wrong configuration: - Wrong IMU alignment (CW180 instead of CW270) - Missing MPU6500/BMI270 support - Wrong barometer (I2C-only instead of SPI BMP280) - Missing UART inverter support - Wrong SPI3_NSS_PIN (PB3 instead of PA15) - Wrong blackbox (SPI flash instead of SD card) - Wrong timer configuration 2. Removed dead #else clauses after OMNIBUS split: - DYSF4/target.h: Removed unreachable fallback cases - OMNIBUSF4PRO/target.h: All #else branches now dead after alias fix - OMNIBUSF4V3_SS/target.h: Removed unreachable softserial fallback All affected targets verified to build successfully. --- src/main/target/DYSF4/target.h | 8 +- src/main/target/OMNIBUSF4PRO/target.h | 105 +++++++++--------------- src/main/target/OMNIBUSF4V3_SS/target.h | 7 -- 3 files changed, 39 insertions(+), 81 deletions(-) diff --git a/src/main/target/DYSF4/target.h b/src/main/target/DYSF4/target.h index 2578eb6b424..41212fdb18d 100644 --- a/src/main/target/DYSF4/target.h +++ b/src/main/target/DYSF4/target.h @@ -19,19 +19,13 @@ // This directory contains: DYSF4PRO, DYSF4PROV2 -#if defined(DYSF4PRO) +#if defined(DYSF4PRO) #define TARGET_BOARD_IDENTIFIER "DYS4" #elif defined(DYSF4PROV2) #define TARGET_BOARD_IDENTIFIER "DY42" -#else -#define TARGET_BOARD_IDENTIFIER "OBF4" #endif -#if defined(DYSF4PRO) || defined(DYSF4PROV2) #define USBD_PRODUCT_STRING "DysF4Pro" -#else -#define USBD_PRODUCT_STRING "Omnibus F4" -#endif #define LED0 PB5 diff --git a/src/main/target/OMNIBUSF4PRO/target.h b/src/main/target/OMNIBUSF4PRO/target.h index 6e7ed8aa9b3..582dfe08d13 100644 --- a/src/main/target/OMNIBUSF4PRO/target.h +++ b/src/main/target/OMNIBUSF4PRO/target.h @@ -20,12 +20,14 @@ // This directory contains: OMNIBUSF4PRO, OMNIBUSF4V3, OMNIBUSF4V3_ICM // Softserial variants are in separate OMNIBUSF4V3_SS/ directory +#ifdef OMNIBUSF4V3_ICM +#define OMNIBUSF4V3 +#endif + #ifdef OMNIBUSF4PRO #define TARGET_BOARD_IDENTIFIER "OBSD" #elif defined(OMNIBUSF4V3) #define TARGET_BOARD_IDENTIFIER "OB43" -#else -#define TARGET_BOARD_IDENTIFIER "OBF4" #endif #define USBD_PRODUCT_STRING "Omnibus F4" @@ -45,27 +47,20 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define USE_IMU_MPU6000 - #define IMU_MPU6000_ALIGN CW270_DEG -#else - #define USE_IMU_MPU6000 - #define IMU_MPU6000_ALIGN CW180_DEG -#endif +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG // Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define MPU6500_CS_PIN MPU6000_CS_PIN - #define MPU6500_SPI_BUS MPU6000_SPI_BUS - #define USE_IMU_MPU6500 - #define IMU_MPU6500_ALIGN IMU_MPU6000_ALIGN - - //BMI270 - #define USE_IMU_BMI270 - #define IMU_BMI270_ALIGN IMU_MPU6000_ALIGN - #define BMI270_SPI_BUS MPU6000_SPI_BUS - #define BMI270_CS_PIN MPU6000_CS_PIN -#endif +#define MPU6500_CS_PIN MPU6000_CS_PIN +#define MPU6500_SPI_BUS MPU6000_SPI_BUS +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN IMU_MPU6000_ALIGN + +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN IMU_MPU6000_ALIGN +#define BMI270_SPI_BUS MPU6000_SPI_BUS +#define BMI270_CS_PIN MPU6000_CS_PIN #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS @@ -74,21 +69,13 @@ #define TEMPERATURE_I2C_BUS I2C_EXT_BUS #define USE_BARO - -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define USE_BARO_BMP280 - #define BMP280_SPI_BUS BUS_SPI3 - #define BMP280_CS_PIN PB3 // v1 - // Support external barometers - #define BARO_I2C_BUS I2C_EXT_BUS - #define USE_BARO_BMP085 - #define USE_BARO_MS5611 -#else - #define BARO_I2C_BUS I2C_EXT_BUS - #define USE_BARO_BMP085 - #define USE_BARO_BMP280 - #define USE_BARO_MS5611 -#endif +#define USE_BARO_BMP280 +#define BMP280_SPI_BUS BUS_SPI3 +#define BMP280_CS_PIN PB3 // v1 +// Support external barometers +#define BARO_I2C_BUS I2C_EXT_BUS +#define USE_BARO_BMP085 +#define USE_BARO_MS5611 #define PITOT_I2C_BUS I2C_EXT_BUS @@ -99,9 +86,7 @@ #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) #define USE_UART_INVERTER -#endif #define USE_UART1 #define UART1_RX_PIN PA10 @@ -146,20 +131,14 @@ #define USE_SPI_DEVICE_1 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define USE_SPI_DEVICE_2 - #define SPI2_NSS_PIN PB12 - #define SPI2_SCK_PIN PB13 - #define SPI2_MISO_PIN PB14 - #define SPI2_MOSI_PIN PB15 -#endif +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 #define USE_SPI_DEVICE_3 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define SPI3_NSS_PIN PA15 -#else - #define SPI3_NSS_PIN PB3 -#endif +#define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 @@ -168,23 +147,15 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - #define USE_SDCARD - #define USE_SDCARD_SPI - - #define SDCARD_SPI_BUS BUS_SPI2 - #define SDCARD_CS_PIN SPI2_NSS_PIN - - #define SDCARD_DETECT_PIN PB7 - #define SDCARD_DETECT_INVERTED -#else - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - #define M25P16_CS_PIN SPI3_NSS_PIN - #define M25P16_SPI_BUS BUS_SPI3 - #define USE_FLASHFS - #define USE_FLASH_M25P16 -#endif +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_SDCARD +#define USE_SDCARD_SPI + +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN + +#define SDCARD_DETECT_PIN PB7 +#define SDCARD_DETECT_INVERTED #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 diff --git a/src/main/target/OMNIBUSF4V3_SS/target.h b/src/main/target/OMNIBUSF4V3_SS/target.h index 973ccdf3920..65c87d6f7db 100644 --- a/src/main/target/OMNIBUSF4V3_SS/target.h +++ b/src/main/target/OMNIBUSF4V3_SS/target.h @@ -123,13 +123,6 @@ #define SOFTSERIAL_2_TX_PIN PA8 // S6 output #define SERIAL_PORT_COUNT 6 // VCP, USART1, USART3, USART6, SOFTSERIAL1, SOFTSERIAL2 - -#else // One softserial on versions other than OMNIBUSF4V3 -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO -#define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO - -#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 #endif #define DEFAULT_RX_TYPE RX_TYPE_SERIAL From dce7687a98886844e67661bbf85f9cc56a1a9c10 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 22 Dec 2025 02:44:02 -0600 Subject: [PATCH 06/22] Remove outdated and misleading comments from OMNIBUS split - DYSF4/target.h: Removed ICM20608 comment (no ICM support in this directory) - OMNIBUSF4/target.h: Removed ICM20608 comment (no ICM support in this directory) - OMNIBUSF4/target.h: Removed obsolete relationship comments from consolidated file ICM20608 support only exists in OMNIBUSF4PRO and OMNIBUSF4V3_SS directories. --- src/main/target/DYSF4/target.h | 2 -- src/main/target/OMNIBUSF4/target.h | 5 ----- 2 files changed, 7 deletions(-) diff --git a/src/main/target/DYSF4/target.h b/src/main/target/DYSF4/target.h index 41212fdb18d..a54f195b95b 100644 --- a/src/main/target/DYSF4/target.h +++ b/src/main/target/DYSF4/target.h @@ -53,8 +53,6 @@ #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG -// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 - #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS #define USE_MAG_ALL diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 1d8188beadc..0a1ecb2fec7 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -17,9 +17,6 @@ #pragma once -//Same target as OMNIBUSF4PRO with LED strip in M5 -//Same target as OMNIBUSF4V3 with softserial in M5 and M6 - #define TARGET_BOARD_IDENTIFIER "OBF4" #define USBD_PRODUCT_STRING "Omnibus F4" @@ -42,8 +39,6 @@ #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG -// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 - #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS #define USE_MAG_ALL From 2331e01e62c2364155a9600e8a9e5966048261dd Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 22 Dec 2025 02:48:22 -0600 Subject: [PATCH 07/22] Remove dead code from OMNIBUSF4PRO/target.c The #else branch at lines 44-46 was unreachable after adding OMNIBUSF4V3_ICM aliasing. All three targets (OMNIBUSF4PRO, OMNIBUSF4V3, OMNIBUSF4V3_ICM) always use TIM4/PB9, never TIM12/PB15. --- src/main/target/OMNIBUSF4PRO/target.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/target/OMNIBUSF4PRO/target.c b/src/main/target/OMNIBUSF4PRO/target.c index 10a1aaabf5a..39750241d97 100644 --- a/src/main/target/OMNIBUSF4PRO/target.c +++ b/src/main/target/OMNIBUSF4PRO/target.c @@ -38,13 +38,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) #endif -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN -#else - // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN -#endif DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO From 0fead0f6efe74942788fdc2d0c99cd5292cce697 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 22 Dec 2025 10:45:46 -0600 Subject: [PATCH 08/22] Remove outdated comments and redundant conditional from OMNIBUS split 1. Removed outdated 'pad labelled CH5/CH6 on OMNIBUSF4PRO' comments from: - OMNIBUSF4/target.h and target.c (wrong target) - DYSF4/target.h and target.c (wrong target) - OMNIBUSF4V3_SS/target.c (wrong target) These comments are only accurate in the OMNIBUSF4PRO directory where PC8/PC9 really are labelled CH5/CH6 on the physical board. 2. Removed redundant conditional from OMNIBUSF4PRO/target.c: #if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) This is always true since all 3 targets in this directory (OMNIBUSF4PRO, OMNIBUSF4V3, OMNIBUSF4V3_ICM) have one of these defined. --- src/main/target/DYSF4/target.c | 4 ++-- src/main/target/DYSF4/target.h | 4 ++-- src/main/target/OMNIBUSF4/target.c | 4 ++-- src/main/target/OMNIBUSF4/target.h | 4 ++-- src/main/target/OMNIBUSF4PRO/target.c | 2 -- src/main/target/OMNIBUSF4V3_SS/target.c | 4 ++-- 6 files changed, 10 insertions(+), 12 deletions(-) diff --git a/src/main/target/DYSF4/target.c b/src/main/target/DYSF4/target.c index 2eed0188f97..d6de2e9bf25 100644 --- a/src/main/target/DYSF4/target.c +++ b/src/main/target/DYSF4/target.c @@ -39,8 +39,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO - DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN }; diff --git a/src/main/target/DYSF4/target.h b/src/main/target/DYSF4/target.h index a54f195b95b..efce0d1b233 100644 --- a/src/main/target/DYSF4/target.h +++ b/src/main/target/DYSF4/target.h @@ -90,8 +90,8 @@ #define UART6_TX_PIN PC6 #define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO -#define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO +#define SOFTSERIAL_1_RX_PIN PC8 +#define SOFTSERIAL_1_TX_PIN PC9 #define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 2eed0188f97..d6de2e9bf25 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -39,8 +39,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO - DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN }; diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 0a1ecb2fec7..e3a209cca74 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -76,8 +76,8 @@ #define UART6_TX_PIN PC6 #define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO -#define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO +#define SOFTSERIAL_1_RX_PIN PC8 +#define SOFTSERIAL_1_TX_PIN PC9 #define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 diff --git a/src/main/target/OMNIBUSF4PRO/target.c b/src/main/target/OMNIBUSF4PRO/target.c index 39750241d97..29cb945af85 100644 --- a/src/main/target/OMNIBUSF4PRO/target.c +++ b/src/main/target/OMNIBUSF4PRO/target.c @@ -34,9 +34,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) -#endif // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN diff --git a/src/main/target/OMNIBUSF4V3_SS/target.c b/src/main/target/OMNIBUSF4V3_SS/target.c index 3aa99b0ea09..95b496e8bfb 100644 --- a/src/main/target/OMNIBUSF4V3_SS/target.c +++ b/src/main/target/OMNIBUSF4V3_SS/target.c @@ -45,8 +45,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO - DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN }; From e6dc927d46f59b78975eca44ca218f4c7f130fcd Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 22 Dec 2025 10:54:52 -0600 Subject: [PATCH 09/22] Fix inconsistent indentation remnants in OMNIBUS split MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Remove orphaned indentation from preprocessor defines that were previously inside conditional blocks but are now at top level after the target split. Files changed: - OMNIBUSF4/target.h: Fixed 2-space and 4-space orphaned indents - DYSF4/target.h: Fixed 2-space orphaned indents - OMNIBUSF4V3_SS/target.h: Fixed extensive 2-space orphaned indents - OMNIBUSF4PRO/target.h: Fixed 4-space orphaned indent on ADC_CHANNEL_3_PIN Valid indentation inside active #if blocks was preserved. 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude Opus 4.5 --- src/main/target/DYSF4/target.h | 27 +++++----- src/main/target/OMNIBUSF4/target.h | 30 +++++------ src/main/target/OMNIBUSF4PRO/target.h | 3 +- src/main/target/OMNIBUSF4V3_SS/target.h | 72 ++++++++++++------------- 4 files changed, 63 insertions(+), 69 deletions(-) diff --git a/src/main/target/DYSF4/target.h b/src/main/target/DYSF4/target.h index efce0d1b233..76e79106afa 100644 --- a/src/main/target/DYSF4/target.h +++ b/src/main/target/DYSF4/target.h @@ -50,8 +50,8 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 - #define USE_IMU_MPU6000 - #define IMU_MPU6000_ALIGN CW180_DEG +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS @@ -60,11 +60,10 @@ #define TEMPERATURE_I2C_BUS I2C_EXT_BUS #define USE_BARO - - #define BARO_I2C_BUS I2C_EXT_BUS - #define USE_BARO_BMP085 - #define USE_BARO_BMP280 - #define USE_BARO_MS5611 +#define BARO_I2C_BUS I2C_EXT_BUS +#define USE_BARO_BMP085 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 #define PITOT_I2C_BUS I2C_EXT_BUS @@ -105,7 +104,7 @@ #define USE_SPI_DEVICE_3 - #define SPI3_NSS_PIN PB3 +#define SPI3_NSS_PIN PB3 #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 @@ -114,11 +113,11 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - #define M25P16_CS_PIN SPI3_NSS_PIN - #define M25P16_SPI_BUS BUS_SPI3 - #define USE_FLASHFS - #define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 @@ -137,7 +136,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_LED_STRIP - #define WS2811_PIN PA1 +#define WS2811_PIN PA1 #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index e3a209cca74..48bdfb7f910 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -36,8 +36,8 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 - #define USE_IMU_MPU6000 - #define IMU_MPU6000_ALIGN CW180_DEG +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS @@ -46,11 +46,10 @@ #define TEMPERATURE_I2C_BUS I2C_EXT_BUS #define USE_BARO - - #define BARO_I2C_BUS I2C_EXT_BUS - #define USE_BARO_BMP085 - #define USE_BARO_BMP280 - #define USE_BARO_MS5611 +#define BARO_I2C_BUS I2C_EXT_BUS +#define USE_BARO_BMP085 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 #define PITOT_I2C_BUS I2C_EXT_BUS @@ -91,7 +90,7 @@ #define USE_SPI_DEVICE_3 - #define SPI3_NSS_PIN PB3 +#define SPI3_NSS_PIN PB3 #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 @@ -100,17 +99,16 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - #define M25P16_CS_PIN SPI3_NSS_PIN - #define M25P16_SPI_BUS BUS_SPI3 - #define USE_FLASHFS - #define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 #define ADC_CHANNEL_2_PIN PC2 - - #define ADC_CHANNEL_3_PIN PA0 +#define ADC_CHANNEL_3_PIN PA0 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 #define VBAT_ADC_CHANNEL ADC_CHN_2 @@ -119,7 +117,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_LED_STRIP - #define WS2811_PIN PA1 +#define WS2811_PIN PA1 #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) diff --git a/src/main/target/OMNIBUSF4PRO/target.h b/src/main/target/OMNIBUSF4PRO/target.h index 582dfe08d13..32bac107f96 100644 --- a/src/main/target/OMNIBUSF4PRO/target.h +++ b/src/main/target/OMNIBUSF4PRO/target.h @@ -160,8 +160,7 @@ #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 #define ADC_CHANNEL_2_PIN PC2 - - #define ADC_CHANNEL_3_PIN PA0 +#define ADC_CHANNEL_3_PIN PA0 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 #define VBAT_ADC_CHANNEL ADC_CHN_2 diff --git a/src/main/target/OMNIBUSF4V3_SS/target.h b/src/main/target/OMNIBUSF4V3_SS/target.h index 65c87d6f7db..f3cbf6c4a49 100644 --- a/src/main/target/OMNIBUSF4V3_SS/target.h +++ b/src/main/target/OMNIBUSF4V3_SS/target.h @@ -42,20 +42,20 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 - #define USE_IMU_MPU6000 - #define IMU_MPU6000_ALIGN CW270_DEG +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG // Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 - #define MPU6500_CS_PIN MPU6000_CS_PIN - #define MPU6500_SPI_BUS MPU6000_SPI_BUS - #define USE_IMU_MPU6500 - #define IMU_MPU6500_ALIGN IMU_MPU6000_ALIGN +#define MPU6500_CS_PIN MPU6000_CS_PIN +#define MPU6500_SPI_BUS MPU6000_SPI_BUS +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN IMU_MPU6000_ALIGN - //BMI270 - #define USE_IMU_BMI270 - #define IMU_BMI270_ALIGN IMU_MPU6000_ALIGN - #define BMI270_SPI_BUS MPU6000_SPI_BUS - #define BMI270_CS_PIN MPU6000_CS_PIN +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN IMU_MPU6000_ALIGN +#define BMI270_SPI_BUS MPU6000_SPI_BUS +#define BMI270_CS_PIN MPU6000_CS_PIN #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS @@ -64,14 +64,13 @@ #define TEMPERATURE_I2C_BUS I2C_EXT_BUS #define USE_BARO - - #define USE_BARO_BMP280 - #define BMP280_SPI_BUS BUS_SPI3 - #define BMP280_CS_PIN PB3 // v1 - // Support external barometers - #define BARO_I2C_BUS I2C_EXT_BUS - #define USE_BARO_BMP085 - #define USE_BARO_MS5611 +#define USE_BARO_BMP280 +#define BMP280_SPI_BUS BUS_SPI3 +#define BMP280_CS_PIN PB3 // v1 +// Support external barometers +#define BARO_I2C_BUS I2C_EXT_BUS +#define USE_BARO_BMP085 +#define USE_BARO_MS5611 #define PITOT_I2C_BUS I2C_EXT_BUS @@ -96,8 +95,8 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 - #define INVERTER_PIN_UART6_RX PC8 - #define INVERTER_PIN_UART6_TX PC9 +#define INVERTER_PIN_UART6_RX PC8 +#define INVERTER_PIN_UART6_TX PC9 #if defined(OMNIBUSF4V3_S6_SS) // one softserial on S6 #define USE_SOFTSERIAL1 @@ -133,14 +132,14 @@ #define USE_SPI_DEVICE_1 - #define USE_SPI_DEVICE_2 - #define SPI2_NSS_PIN PB12 - #define SPI2_SCK_PIN PB13 - #define SPI2_MISO_PIN PB14 - #define SPI2_MOSI_PIN PB15 +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 #define USE_SPI_DEVICE_3 - #define SPI3_NSS_PIN PA15 +#define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 @@ -149,21 +148,20 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - #define USE_SDCARD - #define USE_SDCARD_SPI +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_SDCARD +#define USE_SDCARD_SPI - #define SDCARD_SPI_BUS BUS_SPI2 - #define SDCARD_CS_PIN SPI2_NSS_PIN +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN - #define SDCARD_DETECT_PIN PB7 - #define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_PIN PB7 +#define SDCARD_DETECT_INVERTED #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 #define ADC_CHANNEL_2_PIN PC2 - - #define ADC_CHANNEL_3_PIN PA0 +#define ADC_CHANNEL_3_PIN PA0 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 #define VBAT_ADC_CHANNEL ADC_CHN_2 @@ -172,7 +170,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_LED_STRIP - #define WS2811_PIN PB6 +#define WS2811_PIN PB6 #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) From cba6d290dc9e3fc0ce74530ab67f40821dbc68d1 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Tue, 23 Dec 2025 09:31:02 -0600 Subject: [PATCH 10/22] Reduce CPU overhead by rate-limiting updateArmingStatus() to 200 Hz The updateArmingStatus() function performs 20+ safety checks (GPS fix, battery voltage, sensor health, LED updates, etc.) that don't need millisecond-level updates. Previously called every PID loop iteration (2000 Hz), now rate-limited to 200 Hz. Testing on AT32F435 hardware showed 10-13% PID loop performance improvement with no functional impact. 200 Hz (5ms response time) is well within human reaction time (~200ms) and exceeds the update rate of the sensors being checked (GPS: 5-10 Hz, battery: slow). --- src/main/fc/fc_core.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index ce874c8aedc..c96979edd0f 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -943,7 +943,12 @@ void taskMainPidLoop(timeUs_t currentTimeUs) processPilotAndFailSafeActions(dT); - updateArmingStatus(); + // Check battery, GPS signal, arming status etc @ 200 Hz + static uint8_t armingStatusDivider = 0; + if (++armingStatusDivider >= 10) { + armingStatusDivider = 0; + updateArmingStatus(); + } if (rxConfig()->rcFilterFrequency) { rcInterpolationApply(isRXDataNew, currentTimeUs); From 798a61202bd3982fb11d7c51219ca9facfb310b6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 17 Jan 2026 02:18:01 -0600 Subject: [PATCH 11/22] Fix AGL velocity estimation using squared acceleration weight factor The AGL velocity update was incorrectly using squared acceleration weight factor (sq(accWeightFactor)) while the altitude update uses linear weight. This creates physical inconsistency where position and velocity respond differently to the same acceleration. With accWeightFactor=0.5 (moderate confidence): - Altitude receives 50% of acceleration (correct) - Velocity was receiving 25% of acceleration (wrong - squared) This fix removes sq() from the velocity update to match the altitude update, making both integrations physically consistent with standard kinematic equations. May fix https://github.com/iNavFlight/inav/pull/10314 May be related to https://github.com/iNavFlight/inav/issues/10567 --- src/main/navigation/navigation_pos_estimator_agl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index 03b5bd8ccf0..38788e20a77 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -149,7 +149,7 @@ void estimationCalculateAGL(estimationContext_t * ctx) // Update estimate posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt; posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * posEstimator.imu.accWeightFactor; - posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(posEstimator.imu.accWeightFactor); + posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * posEstimator.imu.accWeightFactor; // Apply correction if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { From 5cf56e8d463bb7f755a753e9560c4e578befc8f6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Tue, 20 Jan 2026 23:42:34 -0600 Subject: [PATCH 12/22] Fix comment/code mismatch in position estimator weight factors Comments incorrectly stated 0.3 when code uses 0.5f. Updated comments to match actual implementation. --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cd861dacd6c..b696813393c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -354,7 +354,7 @@ static bool gravityCalibrationComplete(void) static void updateIMUEstimationWeight(const float dt) { static float acc_clip_factor = 1.0f; - // If accelerometer measurement is clipped - drop the acc weight to 0.3 + // If accelerometer measurement is clipped - drop the acc weight to 0.5 // and gradually restore weight back to 1.0 over time if (accIsClipped()) { acc_clip_factor = 0.5f; From 43fe3ab0e14feb0f8188006f85d9dea38cb2e1af Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 7 Feb 2026 22:53:39 -0600 Subject: [PATCH 13/22] Remove CLAUDE.md and brother/ from .gitignore These entries are not relevant to this PR and should not be included in upstream changes. --- .gitignore | 2 -- 1 file changed, 2 deletions(-) diff --git a/.gitignore b/.gitignore index ccfda8cc1c3..6d7138e6bd5 100644 --- a/.gitignore +++ b/.gitignore @@ -50,5 +50,3 @@ tokens.yaml # Local development files .semgrepignore build_sitl/ -CLAUDE.md -brother/ From f2f6e0e80843f38160df3e6fcf7143289bc5739a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 8 Feb 2026 23:52:40 +0000 Subject: [PATCH 14/22] improve pos est sensor corrections --- .../navigation/navigation_pos_estimator.c | 115 +++++++++++------- .../navigation_pos_estimator_flow.c | 26 ++-- .../navigation_pos_estimator_private.h | 5 + src/main/sensors/opflow.c | 1 + src/main/sensors/opflow.h | 1 + 5 files changed, 95 insertions(+), 53 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cd861dacd6c..0898ce3bb6c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -228,6 +228,7 @@ void onNewGPSData(void) /* If not the first update - calculate velocities */ if (!isFirstGPSUpdate) { float dT = US2S(getGPSDeltaTimeFilter(currentTimeUs - lastGPSNewDataTime)); + posEstimator.gps.updateDt = dT; /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */ float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); @@ -299,6 +300,8 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) posEstimator.baro.lastUpdateTime = currentTimeUs; if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { + posEstimator.baro.updateDt = US2S(baroDtUs); + posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs)); // baro altitude rate @@ -576,6 +579,12 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_BARO_VALID && wBaro) { + if (!posEstimator.baro.updateDt) { + return true; + } + const float dt = posEstimator.baro.updateDt; + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; + bool isAirCushionEffectDetected = false; static float baroGroundAlt = 0.0f; @@ -609,14 +618,14 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; - ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += baroVelZResidual * w_z_baro_v * ctx->dt; + ctx->estPosCorr.z = constrainf(baroAltResidual * w_z_baro_p * dt, -corrLimit, corrLimit); + ctx->estVelCorr.z = constrainf(baroVelZResidual * w_z_baro_v * dt, -corrLimit, corrLimit); - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z += (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); + ctx->accBiasCorr.z = dt * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -629,19 +638,23 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z; ctx->newEPV = posEstimator.gps.epv; } - else { + else if (posEstimator.gps.updateDt) { + const float dt = posEstimator.gps.updateDt; + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; + // Altitude const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; - ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt; - ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); + ctx->estPosCorr.z += constrainf(gpsAltResidual * w_z_gps_p * dt, -corrLimit, corrLimit); + ctx->estVelCorr.z += constrainf(gpsVelZResidual * w_z_gps_v * dt, -corrLimit, corrLimit); + + ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z += (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); + ctx->accBiasCorr.z += dt * (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -652,7 +665,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estVelCorr.z *= 2.0f / (wGps + wBaro); ctx->accBiasCorr.z *= 2.0f / (wGps + wBaro); - return correctOK; + return ctx->applyCorrections = correctOK; } static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) @@ -666,7 +679,10 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) ctx->estVelCorr.y += posEstimator.gps.vel.y - posEstimator.est.vel.y; ctx->newEPH = posEstimator.gps.eph; } - else { + else if (posEstimator.gps.updateDt) { + const float dt = posEstimator.gps.updateDt; + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; + const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x; @@ -680,19 +696,21 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler); // Coordinates - ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt; - ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt; + ctx->estPosCorr.x = constrainf(gpsPosXResidual * w_xy_gps_p * dt, -corrLimit, corrLimit); + ctx->estPosCorr.y = constrainf(gpsPosYResidual * w_xy_gps_p * dt, -corrLimit, corrLimit); // Velocity from direct measurement - ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt; - ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt; + ctx->estVelCorr.x = constrainf(gpsVelXResidual * w_xy_gps_v * dt, -corrLimit, corrLimit); + ctx->estVelCorr.y = constrainf(gpsVelYResidual * w_xy_gps_v * dt, -corrLimit, corrLimit); // Accelerometer bias - ctx->accBiasCorr.x += (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); - ctx->accBiasCorr.y += (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.x = dt * (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.y = dt * (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); /* Adjust EPH */ - ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); + ctx->newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); + + ctx->applyCorrections = true; } return true; @@ -736,9 +754,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) return; } - /* Calculate new EPH and EPV for the case we didn't update position */ - ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); - ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + /* Calculate new EPH and EPV for the case we didn't update position - linear degradation in around 10s */ + ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); + ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); + ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); vectorZero(&ctx.estPosCorr); vectorZero(&ctx.estVelCorr); @@ -751,43 +770,49 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) estimationPredict(&ctx); /* Correction stage: Z */ - const bool estZCorrectOk = - estimationCalculateCorrection_Z(&ctx); + const bool estZCorrectOk = estimationCalculateCorrection_Z(&ctx); /* Correction stage: XY: GPS, FLOW */ // FIXME: Handle transition from FLOW to GPS and back - seamlessly fly indoor/outdoor - const bool estXYCorrectOk = - estimationCalculateCorrection_XY_GPS(&ctx) || - estimationCalculateCorrection_XY_FLOW(&ctx); + const bool estXYCorrectOk = estimationCalculateCorrection_XY_GPS(&ctx) || estimationCalculateCorrection_XY_FLOW(&ctx); // If we can't apply correction or accuracy is off the charts - decay velocity to zero if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) { ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt; ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt; + ctx.applyCorrections = true; } if (!estZCorrectOk || ctx.newEPV > max_eph_epv) { ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; + ctx.applyCorrections = true; } - // Boost the corrections based on accWeight - vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); - vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); - - // Apply corrections - vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); - vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); - - /* Correct accelerometer bias */ - const float w_acc_bias = positionEstimationConfig()->w_acc_bias; - if (w_acc_bias > 0.0f) { - /* Correct accel bias */ - posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt; - - posEstimator.imu.accelBias.x = constrainf(posEstimator.imu.accelBias.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); - posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); - posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + + if (ctx.applyCorrections) { + posEstimator.gps.updateDt = 0.0f; + posEstimator.baro.updateDt = 0.0f; + posEstimator.flow.updateDt = 0.0f; + + // Boost the corrections based on accWeight + vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); + vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); + + // Apply corrections + vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); + vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); + + /* Correct accelerometer bias */ + const float w_acc_bias = positionEstimationConfig()->w_acc_bias; + if (w_acc_bias > 0.0f) { + /* Correct accel bias */ + posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias; + posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias; + posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias; + + posEstimator.imu.accelBias.x = constrainf(posEstimator.imu.accelBias.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + } } /* Update ground course */ diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c index 8656c4919f4..d192ec1dd19 100644 --- a/src/main/navigation/navigation_pos_estimator_flow.c +++ b/src/main/navigation/navigation_pos_estimator_flow.c @@ -73,6 +73,14 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) return false; } + if (!opflow.updateDt) { + return true; + } + posEstimator.flow.updateDt = opflow.updateDt; + + const float dt = posEstimator.flow.updateDt; + const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; + // Calculate linear velocity based on angular velocity and altitude // Technically we should calculate arc length here, but for fast sampling this is accurate enough fpVector3_t flowVel = { @@ -88,8 +96,9 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) const float flowVelXInnov = flowVel.x - posEstimator.est.vel.x; const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y; - ctx->estVelCorr.x = flowVelXInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt; - ctx->estVelCorr.y = flowVelYInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt; + const float w_xy_flow_v = positionEstimationConfig()->w_xy_flow_v; + ctx->estVelCorr.x = constrainf(flowVelXInnov * w_xy_flow_v * dt, -corrLimit, corrLimit); + ctx->estVelCorr.y = constrainf(flowVelYInnov * w_xy_flow_v * dt, -corrLimit, corrLimit); // Calculate position correction if possible/allowed if ((ctx->newFlags & EST_GPS_XY_VALID)) { @@ -98,16 +107,17 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) posEstimator.est.flowCoordinates[Y] = posEstimator.gps.pos.y; } else if (positionEstimationConfig()->allow_dead_reckoning) { - posEstimator.est.flowCoordinates[X] += flowVel.x * ctx->dt; - posEstimator.est.flowCoordinates[Y] += flowVel.y * ctx->dt; + posEstimator.est.flowCoordinates[X] += flowVel.x * dt; + posEstimator.est.flowCoordinates[Y] += flowVel.y * dt; const float flowResidualX = posEstimator.est.flowCoordinates[X] - posEstimator.est.pos.x; const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y; - ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt; - ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt; + const float w_xy_flow_p = positionEstimationConfig()->w_xy_flow_p; + ctx->estPosCorr.x = constrainf(flowResidualX * w_xy_flow_p * dt, -corrLimit, corrLimit); + ctx->estPosCorr.y = constrainf(flowResidualY * w_xy_flow_p * dt, -corrLimit, corrLimit); - ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, calc_length_pythagorean_2D(flowResidualX, flowResidualY), positionEstimationConfig()->w_xy_flow_p); + ctx->newEPH = updateEPE(posEstimator.est.eph, dt, calc_length_pythagorean_2D(flowResidualX, flowResidualY), w_xy_flow_p); } DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X])); @@ -115,7 +125,7 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) DEBUG_SET(DEBUG_FLOW, 2, posEstimator.est.flowCoordinates[X]); DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]); - return true; + return ctx->applyCorrections = true; #else UNUSED(ctx); return false; diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index abb389bddcb..6f4c738e10d 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -33,6 +33,7 @@ #define INAV_GPS_ACCEPTANCE_EPE 500.0f // 5m acceptance radius #define INAV_ACC_BIAS_ACCEPTANCE_VALUE (GRAVITY_CMSS * 0.25f) // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway +#define INAV_EST_CORR_LIMIT_VALUE 4000.0f #define INAV_GPS_GLITCH_RADIUS 250.0f // 2.5m GPS glitch radius #define INAV_GPS_GLITCH_ACCEL 1000.0f // 10m/s/s max possible acceleration for GPS glitch detection @@ -69,6 +70,7 @@ typedef struct { fpVector3_t vel; // GPS velocity (cms) float eph; float epv; + float updateDt; } navPositionEstimatorGPS_t; typedef struct { @@ -77,6 +79,7 @@ typedef struct { float alt; // Raw barometric altitude (cm) float epv; float baroAltRate; // Baro altitude rate of change (cm/s) + float updateDt; } navPositionEstimatorBARO_t; typedef struct { @@ -103,6 +106,7 @@ typedef struct { float quality; float flowRate[2]; float bodyRate[2]; + float updateDt; } navPositionEstimatorFLOW_t; typedef struct { @@ -178,6 +182,7 @@ typedef struct { fpVector3_t estPosCorr; fpVector3_t estVelCorr; fpVector3_t accBiasCorr; + bool applyCorrections; } estimationContext_t; extern navigationPosEstimator_t posEstimator; diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index f82d8f16a58..024c5d403cf 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -171,6 +171,7 @@ void opflowUpdate(timeUs_t currentTimeUs) if (opflow.dev.updateFn(&opflow.dev)) { // Indicate valid update opflow.isHwHealty = true; + opflow.updateDt = US2S(currentTimeUs - opflow.lastValidUpdate); opflow.lastValidUpdate = currentTimeUs; opflow.rawQuality = opflow.dev.rawData.quality; diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h index afe94ec79ac..a2f82745816 100755 --- a/src/main/sensors/opflow.h +++ b/src/main/sensors/opflow.h @@ -53,6 +53,7 @@ typedef struct opflow_s { opflowQuality_e flowQuality; timeUs_t lastValidUpdate; + float updateDt; bool isHwHealty; float flowRate[2]; // optical flow angular rate in rad/sec measured about the X and Y body axis float bodyRate[2]; // body inertial angular rate in rad/sec measured about the X and Y body axis From 13e56b74952a2c062c829eb8d2a4c411567c213e Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 9 Feb 2026 14:14:54 +0000 Subject: [PATCH 15/22] fixes and improvements --- .../navigation/navigation_pos_estimator.c | 47 +++++++++++-------- .../navigation_pos_estimator_flow.c | 11 ++--- 2 files changed, 32 insertions(+), 26 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 0898ce3bb6c..6b275d1809c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -300,13 +300,14 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) posEstimator.baro.lastUpdateTime = currentTimeUs; if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { - posEstimator.baro.updateDt = US2S(baroDtUs); + const float baroDtSec = US2S(baroDtUs); + posEstimator.baro.updateDt = baroDtSec; - posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs)); + posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, baroDtSec); // baro altitude rate static float baroAltPrevious = 0; - posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / US2S(baroDtUs); + posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / baroDtSec; baroAltPrevious = posEstimator.baro.alt; updateBaroAltitudeRate(posEstimator.baro.baroAltRate); } @@ -583,7 +584,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) return true; } const float dt = posEstimator.baro.updateDt; - const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; bool isAirCushionEffectDetected = false; static float baroGroundAlt = 0.0f; @@ -618,8 +618,8 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; - ctx->estPosCorr.z = constrainf(baroAltResidual * w_z_baro_p * dt, -corrLimit, corrLimit); - ctx->estVelCorr.z = constrainf(baroVelZResidual * w_z_baro_v * dt, -corrLimit, corrLimit); + ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dt; + ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dt; ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); @@ -640,7 +640,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } else if (posEstimator.gps.updateDt) { const float dt = posEstimator.gps.updateDt; - const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; // Altitude const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); @@ -648,8 +647,8 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; - ctx->estPosCorr.z += constrainf(gpsAltResidual * w_z_gps_p * dt, -corrLimit, corrLimit); - ctx->estVelCorr.z += constrainf(gpsVelZResidual * w_z_gps_v * dt, -corrLimit, corrLimit); + ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * dt; + ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * dt; ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); @@ -681,7 +680,6 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) } else if (posEstimator.gps.updateDt) { const float dt = posEstimator.gps.updateDt; - const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; @@ -696,12 +694,12 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler); // Coordinates - ctx->estPosCorr.x = constrainf(gpsPosXResidual * w_xy_gps_p * dt, -corrLimit, corrLimit); - ctx->estPosCorr.y = constrainf(gpsPosYResidual * w_xy_gps_p * dt, -corrLimit, corrLimit); + ctx->estPosCorr.x = gpsPosXResidual * w_xy_gps_p * dt; + ctx->estPosCorr.y = gpsPosYResidual * w_xy_gps_p * dt; // Velocity from direct measurement - ctx->estVelCorr.x = constrainf(gpsVelXResidual * w_xy_gps_v * dt, -corrLimit, corrLimit); - ctx->estVelCorr.y = constrainf(gpsVelYResidual * w_xy_gps_v * dt, -corrLimit, corrLimit); + ctx->estVelCorr.x = gpsVelXResidual * w_xy_gps_v * dt; + ctx->estVelCorr.y = gpsVelYResidual * w_xy_gps_v * dt; // Accelerometer bias ctx->accBiasCorr.x = dt * (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); @@ -754,7 +752,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) return; } - /* Calculate new EPH and EPV for the case we didn't update position - linear degradation in around 10s */ + /* Calculate new EPH and EPV for the case we didn't update position - linear degradation in max 10s */ ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); @@ -778,17 +776,19 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) // If we can't apply correction or accuracy is off the charts - decay velocity to zero if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) { - ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt; - ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt; - ctx.applyCorrections = true; + const float w_xy_res_v = positionEstimationConfig()->w_xy_res_v; + posEstimator.est.vel.x -= posEstimator.est.vel.x * w_xy_res_v * ctx.dt; + posEstimator.est.vel.y -= posEstimator.est.vel.y * w_xy_res_v * ctx.dt; } if (!estZCorrectOk || ctx.newEPV > max_eph_epv) { - ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; - ctx.applyCorrections = true; + posEstimator.est.vel.z -= posEstimator.est.vel.z * positionEstimationConfig()->w_z_res_v * ctx.dt; } if (ctx.applyCorrections) { + float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); + maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); + posEstimator.gps.updateDt = 0.0f; posEstimator.baro.updateDt = 0.0f; posEstimator.flow.updateDt = 0.0f; @@ -797,6 +797,13 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); + // Constrain corrections to prevent instability + float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt; + for (uint8_t axis = 0; axis < 3; axis++) { + ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -correctionLimit, correctionLimit); + ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -correctionLimit, correctionLimit); + } + // Apply corrections vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c index d192ec1dd19..36325f5dc1a 100644 --- a/src/main/navigation/navigation_pos_estimator_flow.c +++ b/src/main/navigation/navigation_pos_estimator_flow.c @@ -77,9 +77,8 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) return true; } posEstimator.flow.updateDt = opflow.updateDt; - + opflow.updateDt = 0.0f; const float dt = posEstimator.flow.updateDt; - const float corrLimit = INAV_EST_CORR_LIMIT_VALUE * dt; // Calculate linear velocity based on angular velocity and altitude // Technically we should calculate arc length here, but for fast sampling this is accurate enough @@ -97,8 +96,8 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y; const float w_xy_flow_v = positionEstimationConfig()->w_xy_flow_v; - ctx->estVelCorr.x = constrainf(flowVelXInnov * w_xy_flow_v * dt, -corrLimit, corrLimit); - ctx->estVelCorr.y = constrainf(flowVelYInnov * w_xy_flow_v * dt, -corrLimit, corrLimit); + ctx->estVelCorr.x = flowVelXInnov * w_xy_flow_v * dt; + ctx->estVelCorr.y = flowVelYInnov * w_xy_flow_v * dt; // Calculate position correction if possible/allowed if ((ctx->newFlags & EST_GPS_XY_VALID)) { @@ -114,8 +113,8 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y; const float w_xy_flow_p = positionEstimationConfig()->w_xy_flow_p; - ctx->estPosCorr.x = constrainf(flowResidualX * w_xy_flow_p * dt, -corrLimit, corrLimit); - ctx->estPosCorr.y = constrainf(flowResidualY * w_xy_flow_p * dt, -corrLimit, corrLimit); + ctx->estPosCorr.x = flowResidualX * w_xy_flow_p * dt; + ctx->estPosCorr.y = flowResidualY * w_xy_flow_p * dt; ctx->newEPH = updateEPE(posEstimator.est.eph, dt, calc_length_pythagorean_2D(flowResidualX, flowResidualY), w_xy_flow_p); } From 1c39cd89da1075be57f303df973c612e4e30b36c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 10 Feb 2026 10:59:07 +0000 Subject: [PATCH 16/22] Update navigation_pos_estimator.c --- .../navigation/navigation_pos_estimator.c | 81 ++++++++++--------- 1 file changed, 42 insertions(+), 39 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 43bd57acad7..b0f8bba4c8c 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -580,52 +580,53 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_BARO_VALID && wBaro) { - if (!posEstimator.baro.updateDt) { - return true; - } - const float dt = posEstimator.baro.updateDt; + if (posEstimator.baro.updateDt) { + ctx->applyCorrections = true; + const float dt = posEstimator.baro.updateDt; - bool isAirCushionEffectDetected = false; - static float baroGroundAlt = 0.0f; + bool isAirCushionEffectDetected = false; + static float baroGroundAlt = 0.0f; - if (STATE(MULTIROTOR)) { - static bool isBaroGroundValid = false; + if (STATE(MULTIROTOR)) { + static bool isBaroGroundValid = false; - if (!ARMING_FLAG(ARMED)) { - baroGroundAlt = posEstimator.baro.alt; - isBaroGroundValid = true; - } - else if (isBaroGroundValid) { - // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it - if (isMulticopterThrottleAboveMidHover()) { - // Disable ground effect detection at lift off when est alt and baro alt converge. Always disable if baro alt > 1m. - isBaroGroundValid = fabsf(posEstimator.est.pos.z - posEstimator.baro.alt) > 20.0f && posEstimator.baro.alt < 100.0f; + if (!ARMING_FLAG(ARMED)) { + baroGroundAlt = posEstimator.baro.alt; + isBaroGroundValid = true; + } + else if (isBaroGroundValid) { + // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it + if (isMulticopterThrottleAboveMidHover()) { + // Disable ground effect detection at lift off when est alt and baro alt converge. Always disable if baro alt > 1m. + isBaroGroundValid = fabsf(posEstimator.est.pos.z - posEstimator.baro.alt) > 20.0f && posEstimator.baro.alt < 100.0f; + } + + isAirCushionEffectDetected = (isEstimatedAglTrusted() && posEstimator.surface.alt < 20.0f) || + posEstimator.baro.alt < baroGroundAlt + 20.0f; } - - isAirCushionEffectDetected = (isEstimatedAglTrusted() && posEstimator.surface.alt < 20.0f) || posEstimator.baro.alt < baroGroundAlt + 20.0f; } - } - // Altitude - float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); + // Altitude + float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); - // Disable alt pos correction at point of lift off if ground effect active - if (isAirCushionEffectDetected && isMulticopterThrottleAboveMidHover()) { - baroAltResidual = 0.0f; - } + // Disable alt pos correction at point of lift off if ground effect active + if (isAirCushionEffectDetected && isMulticopterThrottleAboveMidHover()) { + baroAltResidual = 0.0f; + } - const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); - const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; - const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; + const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); + const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; + const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; - ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dt; - ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dt; + ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dt; + ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); - // Accelerometer bias - if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z = dt * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); + // Accelerometer bias + if (!isAirCushionEffectDetected) { + ctx->accBiasCorr.z = dt * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); + } } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -639,6 +640,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->newEPV = posEstimator.gps.epv; } else if (posEstimator.gps.updateDt) { + ctx->applyCorrections = true; const float dt = posEstimator.gps.updateDt; // Altitude @@ -650,7 +652,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * dt; ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); + ctx->newEPV = updateEPE(ctx->newEPV, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); // Accelerometer bias ctx->accBiasCorr.z += dt * (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); @@ -664,7 +666,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estVelCorr.z *= 2.0f / (wGps + wBaro); ctx->accBiasCorr.z *= 2.0f / (wGps + wBaro); - return ctx->applyCorrections = correctOK; + return correctOK; } static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) @@ -679,6 +681,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) ctx->newEPH = posEstimator.gps.eph; } else if (posEstimator.gps.updateDt) { + ctx->applyCorrections = true; const float dt = posEstimator.gps.updateDt; const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; @@ -707,8 +710,6 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) /* Adjust EPH */ ctx->newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); - - ctx->applyCorrections = true; } return true; @@ -786,6 +787,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) } if (ctx.applyCorrections) { + ctx.applyCorrections = false; + float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); From ffd4b9ebcf0776af0df5bf3b5dc3a58ea1751a04 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 10 Feb 2026 23:23:41 +0000 Subject: [PATCH 17/22] Update navigation_pos_estimator.c --- .../navigation/navigation_pos_estimator.c | 92 ++++++++++--------- 1 file changed, 49 insertions(+), 43 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index b0f8bba4c8c..cdfb3c87ed6 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -580,9 +580,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_BARO_VALID && wBaro) { - if (posEstimator.baro.updateDt) { + if (posEstimator.baro.updateDt) { // only update corrections once every sensor update ctx->applyCorrections = true; - const float dt = posEstimator.baro.updateDt; + const float dT = posEstimator.baro.updateDt; bool isAirCushionEffectDetected = false; static float baroGroundAlt = 0.0f; @@ -618,14 +618,14 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; - ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dt; - ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dt; + ctx->estPosCorr.z = baroAltResidual * w_z_baro_p * dT; + ctx->estVelCorr.z = baroVelZResidual * w_z_baro_v * dT; - ctx->newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, dT, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z = dt * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); + ctx->accBiasCorr.z = dT * (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); } } @@ -633,29 +633,31 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_GPS_Z_VALID && (wGps || !(ctx->newFlags & EST_Z_VALID))) { - // Reset current estimate to GPS altitude if estimate not valid (used for GPS and Baro) - if (!(ctx->newFlags & EST_Z_VALID)) { - posEstimator.est.pos.z = posEstimator.gps.pos.z; - posEstimator.est.vel.z = posEstimator.gps.vel.z; - ctx->newEPV = posEstimator.gps.epv; - } - else if (posEstimator.gps.updateDt) { - ctx->applyCorrections = true; - const float dt = posEstimator.gps.updateDt; + if (posEstimator.gps.updateDt) { // only update corrections once every sensor update + // Reset current estimate to GPS altitude if estimate not valid (used for GPS and Baro) + if (!(ctx->newFlags & EST_Z_VALID)) { + posEstimator.est.pos.z = posEstimator.gps.pos.z; + posEstimator.est.vel.z = posEstimator.gps.vel.z; + ctx->newEPV = posEstimator.gps.epv; + } + else { + ctx->applyCorrections = true; + const float dT = posEstimator.gps.updateDt; - // Altitude - const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); - const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); - const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; - const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; + // Altitude + const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); + const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); + const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; + const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; - ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * dt; - ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * dt; + ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * dT; + ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * dT; - ctx->newEPV = updateEPE(ctx->newEPV, dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); + ctx->newEPV = updateEPE(ctx->newEPV, dT, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); - // Accelerometer bias - ctx->accBiasCorr.z += dt * (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); + // Accelerometer bias + ctx->accBiasCorr.z += dT * (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); + } } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -672,6 +674,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) { if (ctx->newFlags & EST_GPS_XY_VALID) { + if (!posEstimator.gps.updateDt) { // only update corrections once every sensor update + return true; + } /* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */ if (!(ctx->newFlags & EST_XY_VALID)) { posEstimator.est.pos.x = posEstimator.gps.pos.x; @@ -680,9 +685,9 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) posEstimator.est.vel.y = posEstimator.gps.vel.y; ctx->newEPH = posEstimator.gps.eph; } - else if (posEstimator.gps.updateDt) { + else { ctx->applyCorrections = true; - const float dt = posEstimator.gps.updateDt; + const float dT = posEstimator.gps.updateDt; const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; @@ -697,19 +702,19 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler); // Coordinates - ctx->estPosCorr.x = gpsPosXResidual * w_xy_gps_p * dt; - ctx->estPosCorr.y = gpsPosYResidual * w_xy_gps_p * dt; + ctx->estPosCorr.x = gpsPosXResidual * w_xy_gps_p * dT; + ctx->estPosCorr.y = gpsPosYResidual * w_xy_gps_p * dT; // Velocity from direct measurement - ctx->estVelCorr.x = gpsVelXResidual * w_xy_gps_v * dt; - ctx->estVelCorr.y = gpsVelYResidual * w_xy_gps_v * dt; + ctx->estVelCorr.x = gpsVelXResidual * w_xy_gps_v * dT; + ctx->estVelCorr.y = gpsVelYResidual * w_xy_gps_v * dT; // Accelerometer bias - ctx->accBiasCorr.x = dt * (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); - ctx->accBiasCorr.y = dt * (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.x = dT * (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.y = dT * (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); /* Adjust EPH */ - ctx->newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); + ctx->newEPH = updateEPE(posEstimator.est.eph, dT, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); } return true; @@ -753,7 +758,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) return; } - /* Calculate new EPH and EPV for the case we didn't update position - linear degradation in max 10s */ + /* Calculate new degraded EPH and EPV for the case we didn't update estimation from sensors - linear degradation in max 10s */ ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); @@ -786,22 +791,18 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) posEstimator.est.vel.z -= posEstimator.est.vel.z * positionEstimationConfig()->w_z_res_v * ctx.dt; } + // Only apply corrections if new sensor update available if (ctx.applyCorrections) { ctx.applyCorrections = false; - float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); - maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); - - posEstimator.gps.updateDt = 0.0f; - posEstimator.baro.updateDt = 0.0f; - posEstimator.flow.updateDt = 0.0f; - // Boost the corrections based on accWeight vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); // Constrain corrections to prevent instability - float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt; + float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); + maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); + const float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt; for (uint8_t axis = 0; axis < 3; axis++) { ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -correctionLimit, correctionLimit); ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -correctionLimit, correctionLimit); @@ -823,6 +824,11 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); } + + // Reset sensor update time deltas once sensor corrections applied after sensor update + posEstimator.gps.updateDt = 0.0f; + posEstimator.baro.updateDt = 0.0f; + posEstimator.flow.updateDt = 0.0f; } /* Update ground course */ From 7cdf9546360587432eb99317d8de1c16703b7f9c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 11 Feb 2026 20:52:29 +0000 Subject: [PATCH 18/22] fixes --- src/main/navigation/navigation_pos_estimator.c | 2 +- src/main/navigation/navigation_pos_estimator_private.h | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cdfb3c87ed6..3db5aa2ae43 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -299,7 +299,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) posEstimator.baro.epv = positionEstimationConfig()->baro_epv; posEstimator.baro.lastUpdateTime = currentTimeUs; - if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { + if (baroDtUs > 0 && baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { const float baroDtSec = US2S(baroDtUs); posEstimator.baro.updateDt = baroDtSec; diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 34133d52b1f..8a91afe268b 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -33,7 +33,6 @@ #define INAV_GPS_ACCEPTANCE_EPE 500.0f // 5m acceptance radius #define INAV_ACC_BIAS_ACCEPTANCE_VALUE (GRAVITY_CMSS * 0.25f) // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway -#define INAV_EST_CORR_LIMIT_VALUE 4000.0f #define INAV_EST_CORR_LIMIT_VALUE 4000.0f // Sanity constraint limit for pos/vel estimate correction value (max 40m correction per s) From 1a854b6b8984cf00db899e3867ef7ac6ce62e047 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 12 Feb 2026 16:44:23 -0600 Subject: [PATCH 19/22] Bump firmware version to 9.0.1 Prepare for 9.0.1 patch release. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dc34f9da32b..14db3259720 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,7 +52,7 @@ else() endif() -project(INAV VERSION 9.0.0) +project(INAV VERSION 9.0.1) enable_language(ASM) From c50b5efe137fc46f331a31b60de49315e4f8f781 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 13 Feb 2026 14:20:46 -0600 Subject: [PATCH 20/22] Fix GPS capability poll blocking for 500ms every 5 seconds The periodic MON-VER and MON-GNSS polls in the main GPS loop waited on _ack_state for ACK/NAK. MON-class messages never produce ACK/NAK (only CFG-class messages do), so ptWaitTimeout always hit the full 500ms timeout. This blocked gpsProcessNewSolutionData from being called, causing a data processing stall every GPS_CAPA_INTERVAL (5s). Wait on actual response data instead, matching the pattern already used in the initialization code: hwVersion change for MON-VER, and lastCapaUpdMs timestamp for MON-GNSS. --- src/main/io/gps_ublox.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index f0899aa69d5..5cc805ac4ae 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1228,14 +1228,15 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { gpsState.lastCapaPoolMs = millis(); + // MON-class messages respond with data, not ACK/NAK - wait on response data if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) { pollVersion(); - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); } pollGnssCapabilities(); - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + ptWaitTimeout((gpsState.lastCapaUpdMs > gpsState.lastCapaPoolMs), GPS_CFG_CMD_TIMEOUT_MS); } } } From dff5011bb2c50cb82c2241dda4de0ddbe1855db6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 13 Feb 2026 16:29:43 -0600 Subject: [PATCH 21/22] Remove unnecessary ptWaitTimeout blocking from capability polls The receiver thread parses MON-VER/MON-GNSS responses asynchronously. Nothing between the polls and the next loop iteration depends on the updated data, so the waits just block position fix processing for no benefit. The 5-second GPS_CAPA_INTERVAL already prevents re-polling too frequently. --- src/main/io/gps_ublox.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 5cc805ac4ae..82e1a4f655d 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1228,15 +1228,12 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { gpsState.lastCapaPoolMs = millis(); - // MON-class messages respond with data, not ACK/NAK - wait on response data if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) { pollVersion(); - ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); } pollGnssCapabilities(); - ptWaitTimeout((gpsState.lastCapaUpdMs > gpsState.lastCapaPoolMs), GPS_CFG_CMD_TIMEOUT_MS); } } } From 682e55a1045647a853c462672b4c6268d6d530f0 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 15 Feb 2026 11:10:27 -0600 Subject: [PATCH 22/22] Fix NEXUSX default IMU orientation The default IMU_ICM42605_ALIGN was set to CW180_DEG, which is 180 degrees off from the physical direction arrow on the RadioMaster NEXUS-X board. Users had to manually apply YAW-180 correction in the configurator to get correct orientation. Change alignment to CW0_DEG to match the board silkscreen. Fixes #11325 --- src/main/target/NEXUSX/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/NEXUSX/target.h b/src/main/target/NEXUSX/target.h index 24c035b4cae..abe7bbaef28 100644 --- a/src/main/target/NEXUSX/target.h +++ b/src/main/target/NEXUSX/target.h @@ -32,7 +32,7 @@ #define SPI1_MOSI_PIN PA7 #define USE_IMU_ICM42605 // is actually ICM42688P -#define IMU_ICM42605_ALIGN CW180_DEG +#define IMU_ICM42605_ALIGN CW0_DEG #define ICM42605_CS_PIN PA4 #define ICM42605_EXTI_PIN PB8 #define ICM42605_SPI_BUS BUS_SPI1