From 779b21e814008810cfc13bcc932bb38f04b53942 Mon Sep 17 00:00:00 2001 From: Raphael Hunziker Date: Thu, 10 Sep 2026 20:14:33 +0200 Subject: [PATCH 01/31] Refuse assigning SBUS output to a second serial port SBUS servo output is resolved with findSerialPortConfig(), which returns the first port that carries FUNCTION_SERVO_SERIAL. Assigning the function to a second port was accepted and reported back by the serial command, but that port was never opened. Reject such an assignment in the CLI instead of ignoring it silently. Fixes #11430 --- src/main/fc/cli.c | 6 ++++++ src/main/io/serial.c | 19 +++++++++++++++++++ src/main/io/serial.h | 1 + 3 files changed, 26 insertions(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 718dadca046..01be003f49a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -929,6 +929,12 @@ static void cliSerial(char *cmdline) return; } + const uint32_t duplicatedFunctions = serialDuplicatedSinglePortFunctions(&portConfig); + if (duplicatedFunctions) { + cliPrintErrorLinef("Function %d is already assigned to another port", (int)duplicatedFunctions); + return; + } + memcpy(currentConfig, &portConfig, sizeof(portConfig)); } diff --git a/src/main/io/serial.c b/src/main/io/serial.c index d88f071e7cd..9574ed363c2 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -313,6 +313,25 @@ bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) return true; } +// Functions that are resolved with findSerialPortConfig() and therefore only ever use the +// first port they are assigned to. Assigning one of them to a second port has no effect at +// all, so the configuration layer refuses it instead of ignoring it silently. +#define SERIAL_SINGLE_PORT_FUNCTIONS (FUNCTION_SERVO_SERIAL) + +uint32_t serialDuplicatedSinglePortFunctions(const serialPortConfig_t *portConfigToCheck) +{ + uint32_t functionMaskOfOtherPorts = 0; + + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { + const serialPortConfig_t *portConfig = &serialConfig()->portConfigs[index]; + if (portConfig->identifier != portConfigToCheck->identifier) { + functionMaskOfOtherPorts |= portConfig->functionMask; + } + } + + return portConfigToCheck->functionMask & functionMaskOfOtherPorts & SERIAL_SINGLE_PORT_FUNCTIONS; +} + serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier) { for (int index = 0; index < SERIAL_PORT_COUNT; index++) { diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 36f2e02328a..2274ec6e36e 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -159,6 +159,7 @@ void serialRemovePort(serialPortIdentifier_e identifier); uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); bool isSerialConfigValid(const serialConfig_t *serialConfig); +uint32_t serialDuplicatedSinglePortFunctions(const serialPortConfig_t *portConfigToCheck); serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier); bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier); serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function); From cd44927da8543dff49d9ebb7d40b614fb6fe99a5 Mon Sep 17 00:00:00 2001 From: Raphael Hunziker Date: Thu, 10 Sep 2026 20:43:45 +0200 Subject: [PATCH 02/31] Hide RX downlink power OSD element when the link does not report it The receiver transmit power is only written by the MSP and MAVLink RC paths. The CRSF link statistics frame 0x14 has no such field and no other CRSF frame is parsed for it, so with an ExpressLRS receiver the element showed a permanent 0 mW. ExpressLRS never sends the optional link statistics RX frame 0x1C that would carry a power value, so parsing it would not populate the element either. A power of 0 already means "not reported" on the other paths, so skip drawing the element in that case. Fixes #11136 --- docs/OSD.md | 2 +- src/main/io/osd.c | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/docs/OSD.md b/docs/OSD.md index 44084266f8f..7a976a2b0fc 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -192,7 +192,7 @@ Here are the OSD Elements provided by INAV. | 157 | OSD_CUSTOM_ELEMENT_7 | 8.0.0 | | | 158 | OSD_CUSTOM_ELEMENT_8 | 8.0.0 | | | 159 | OSD_LQ_DOWNLINK | 8.0.0 | | -| 160 | OSD_RX_POWER_DOWNLINK | 8.0.0 | | +| 160 | OSD_RX_POWER_DOWNLINK | 8.0.0 | The element will be hidden unless the RC link reports the transmit power of the receiver. CRSF does not send this value, it is available on MSP and MAVLink RC links. | | 161 | OSD_RX_BAND | 8.0.0 | | | 162 | OSD_RX_MODE | 8.0.0 | | | 163 | OSD_COURSE_TO_FENCE | 8.0.0 | | diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e5bbcf49037..38e395498c3 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2845,6 +2845,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_RX_POWER_DOWNLINK: { + // Only the MSP and MAVLink RC links report the transmit power of the receiver, the CRSF + // link statistics frame has no such field. Hide the element instead of showing a permanent 0. + if (rxLinkStatistics.downlinkTXPower == 0) + return false; + if (!failsafeIsReceivingRxData()) tfp_sprintf(buff, "%s%c%c", " ", SYM_MW, SYM_AH_DECORATION_DOWN); else From 5c4db1db56bd114c60256fe9d6a24bc342768bea Mon Sep 17 00:00:00 2001 From: Raphael Hunziker Date: Thu, 10 Sep 2026 21:40:31 +0200 Subject: [PATCH 03/31] servos: keep speed limit carry over when switching back The mixer switch carry over loop ended at the first INPUT_MIXER_SWITCH_HELPER rule. Switching a mixer profile back before the carry over of the previous switch had decayed therefore dropped its remaining filter state, the newly loaded rule restarted from zero and the servo jumped towards its mid position. Only the carry capacity ends the loop now, helper rules are carried over like any other speed limited rule. Fixes #11503 --- src/main/flight/servos.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 1c7c65b3899..010580edffa 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -231,10 +231,12 @@ void loadCustomServoMixer(void) // target-profile response. if (carryOverServoSpeedLimitsOnNextLoad) { for (int i = 0; i < servoRuleCount; i++) { - if(currentServoMixer[i].inputSource == INPUT_MIXER_SWITCH_HELPER || movefilterCount >= MAX_SERVO_RULES_SWITCH_CARRY) { - //will not carry over INPUT_MIXER_SWITCH_HELPER rules - break; + if (movefilterCount >= MAX_SERVO_RULES_SWITCH_CARRY) { + break; // no room left to carry more filter states } + // INPUT_MIXER_SWITCH_HELPER rules are carried over as well. They hold the + // still decaying output of an earlier switch, so dropping them would make + // the servo jump when switching back before that decay has finished. if(currentServoMixer[i].speed != 0 && fabsf(servoSpeedLimitFilter[i].state) > 0.01f) { servoMixerSwitchHelper[movefilterCount].targetChannel = currentServoMixer[i].targetChannel; servoMixerSwitchHelper[movefilterCount].speed = currentServoMixer[i].speed; From fe5ea8145af6e703d9f1c38db61689a54a291c55 Mon Sep 17 00:00:00 2001 From: Raffi1202 Date: Fri, 11 Sep 2026 18:02:07 +0200 Subject: [PATCH 04/31] Clear stale receiver power when telemetry becomes unavailable --- src/main/io/osd.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 38e395498c3..235a43a60c9 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2847,8 +2847,10 @@ static bool osdDrawSingleElement(uint8_t item) { // Only the MSP and MAVLink RC links report the transmit power of the receiver, the CRSF // link statistics frame has no such field. Hide the element instead of showing a permanent 0. - if (rxLinkStatistics.downlinkTXPower == 0) + if (rxLinkStatistics.downlinkTXPower == 0) { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); return false; + } if (!failsafeIsReceivingRxData()) tfp_sprintf(buff, "%s%c%c", " ", SYM_MW, SYM_AH_DECORATION_DOWN); From bec7f378d9308e88d97514d38c55aebc173f6beb Mon Sep 17 00:00:00 2001 From: Raphael Hunziker Date: Sun, 13 Sep 2026 21:21:12 +0200 Subject: [PATCH 05/31] docs: note that SBUS servo output takes only one port The constraints list already covers the analogous telemetry rule. Adds the SERVO_SERIAL case, including that the second assignment used to be accepted silently without the port ever being opened. --- docs/Serial.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Serial.md b/docs/Serial.md index ef10d4d0a3e..7450ec5e8a9 100644 --- a/docs/Serial.md +++ b/docs/Serial.md @@ -48,6 +48,7 @@ e.g. after configuring a port for GPS enable the GPS feature. * All telemetry systems except MSP will ignore any attempts to override the baudrate. * MSP/CLI can be shared with EITHER Blackbox OR telemetry (LTM or MAVlink, not RX telemetry). In shared mode blackbox or telemetry will be output only when armed. * Smartport telemetry cannot be shared with MSP. +* SBUS servo output (`SERVO_SERIAL`) can only be assigned to one port. The CLI refuses a second assignment; previously it was accepted and shown in the port list, but the second port was never opened. * No other serial port sharing combinations are valid. * You can use as many different telemetry systems as you like at the same time. * You can only use each telemetry system once. e.g. FrSky telemetry cannot be used on two port, but LTN Telemetry and FrSky on two different ports is fine. From bfcd366d24cd779d7d9f366510275dc46a9840f9 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 22 Sep 2026 23:04:06 +0200 Subject: [PATCH 06/31] Geozone: mark the nearest-safehome runtime zone as enabled The safehome zone appended in geoZoneInit() was counted in activeGeoZonesCount but never had enable set, unlike the configured zones. The following sort by enable could therefore move it behind the counted range and pull a zeroed slot (NULL vertices) in front of it, or keep a deliberately deactivated far inclusive zone active instead. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation_geozone.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index dfc7539859b..03dd2b4f382 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -1684,6 +1684,7 @@ static void geoZoneInit(void) activeGeoZones[activeGeoZonesCount].config = safeHomeGeozoneConfig; activeGeoZones[activeGeoZonesCount].verticesLocal = (fpVector2_t*)&posControl.safehomeState.nearestSafeHome; activeGeoZones[activeGeoZonesCount].radius = navConfig()->general.safehome_max_distance; + activeGeoZones[activeGeoZonesCount].enable = true; activeGeoZonesCount++; expectedVertices++; configuredVertices++; From e4aeeb4f9fd201308dd55c96c270acaf9d7f7c6b Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 22 Sep 2026 22:02:50 +0200 Subject: [PATCH 07/31] OSD: use atan2f for the telemetry tracker elevation atan2() promoted both integer arguments to double. F4/F7 have no double-precision FPU, so this single call pulled the libgcc soft-double arithmetic and compare routines plus the double atan/atan2 from libm into every image. The result is truncated to integer degrees, so float precision is more than enough. MATEKF722 -2792 B, MATEKF405 -1460 B, MATEKF765 -1392 B, MATEKH743 -1392 B, NEUTRONRCF435WING -1456 B flash; RAM unchanged. Co-Authored-By: Claude Fable 5.1 --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 961cb1ed309..897766eb766 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1538,7 +1538,7 @@ static void osdDisplayTelemetry(void) trk_bearing += 360 + 180; trk_bearing %= 360; int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); - float at = atan2(alt, GPS_distanceToHome); + float at = atan2f((float)alt, (float)GPS_distanceToHome); trk_elevation = at * 57.2957795f; // 57.2957795 = 1 rad trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 if (trk_elevation < 0) { From 2674c9e21a0f4ad404c4b2e55a7d36d6505dea57 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 22 Sep 2026 22:04:04 +0200 Subject: [PATCH 08/31] CLI/pitot: print floats as fixed point instead of %f A %f vararg promotes to double, which keeps libgcc's soft-double add/sub and int-to-double routines linked on MCUs without a double FPU. The four sites (GPS HDOP/EPH/EPV in "status", DLVR pitot debug log) only display already scaled integers. MATEKF722 -876 B, MATEKF765/H743/NEUTRONRCF435WING -44 B, MATEKF405 +20 B flash; RAM unchanged. The CLI status lines now print two decimals ("1.50") instead of ftoa's " 1.500". Co-Authored-By: Claude Fable 5.1 --- src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 2 +- src/main/fc/cli.c | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c index d899ac05a9e..4267874655c 100755 --- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c +++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c @@ -116,7 +116,7 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu // result must fit inside the max pressure range if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) { - LOG_DEBUG( PITOT,"DLVR: Out of range. pressure = %f", (double)(dP_inchH2O) ); + LOG_DEBUG( PITOT,"DLVR: Out of range. pressure = %d/1000 inH2O", (int)(dP_inchH2O * 1000.0f) ); return; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 73c65919b09..d0ce1e5d242 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4362,9 +4362,9 @@ static void cliStatus(char *cmdline) } cliPrintLinefeed(); cliPrintLinef(" SATS: %i", gpsSol.numSat); - cliPrintLinef(" HDOP: %f", (double)(gpsSol.hdop / (float)HDOP_SCALE)); - cliPrintLinef(" EPH : %f m", (double)(gpsSol.eph / 100.0f)); - cliPrintLinef(" EPV : %f m", (double)(gpsSol.epv / 100.0f)); + cliPrintLinef(" HDOP: %d.%02d", gpsSol.hdop / HDOP_SCALE, gpsSol.hdop % HDOP_SCALE); + cliPrintLinef(" EPH : %d.%02d m", gpsSol.eph / 100, gpsSol.eph % 100); + cliPrintLinef(" EPV : %d.%02d m", gpsSol.epv / 100, gpsSol.epv % 100); //cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate()); cliPrintLinef(" GNSS Capabilities:"); cliPrintLine(" GNSS Provider active/default"); From 95911d518c89e31273f798db0f0cb70c0d2d5567 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 22 Sep 2026 23:04:40 +0200 Subject: [PATCH 09/31] Replace newlib strstr/strnstr with a small own search newlib implements strstr and strnstr with the two-way algorithm and memmem, about 2.8 KB of code. INAV only searches short CLI tokens, setting names and u-blox version strings, so a plain scan is enough. cliSet's strstr(cmdline, "=") becomes strchr. SITL's private strnstr had no caller left and is removed. MATEKF722 -3008 B, MATEKF405 -3232 B, MATEKF765 -2672 B, MATEKH743 -2680 B, NEUTRONRCF435WING -2816 B flash. Co-Authored-By: Claude Fable 5.1 --- src/main/common/string_light.c | 26 ++++++++++++++++++++++++++ src/main/common/string_light.h | 3 +++ src/main/fc/cli.c | 6 +++--- src/main/fc/settings.c | 2 +- src/main/io/gps_ublox.c | 9 +++++---- src/main/target/SITL/target.c | 22 ---------------------- src/main/target/SITL/target.h | 1 - 7 files changed, 38 insertions(+), 31 deletions(-) diff --git a/src/main/common/string_light.c b/src/main/common/string_light.c index b99dbe7581c..692f3244c50 100755 --- a/src/main/common/string_light.c +++ b/src/main/common/string_light.c @@ -82,3 +82,29 @@ int sl_strncasecmp(const char * s1, const char * s2, int n) return d; } + +const char * sl_strnstr(const char * haystack, const char * needle, int n) +{ + if (*needle == '\0') { + return haystack; + } + for (; n > 0 && *haystack != '\0'; haystack++, n--) { + const char * h = haystack; + const char * nd = needle; + int left = n; + while (left > 0 && *h != '\0' && *h == *nd) { + h++; + nd++; + left--; + } + if (*nd == '\0') { + return haystack; + } + } + return NULL; +} + +const char * sl_strstr(const char * haystack, const char * needle) +{ + return sl_strnstr(haystack, needle, INT_MAX); +} diff --git a/src/main/common/string_light.h b/src/main/common/string_light.h index 6d0fd37d349..881edf50685 100755 --- a/src/main/common/string_light.h +++ b/src/main/common/string_light.h @@ -27,3 +27,6 @@ void sl_toupperptr(char * c); int sl_strcasecmp(const char * s1, const char * s2); int sl_strncasecmp(const char * s1, const char * s2, int n); +// newlib's strstr/strnstr link the two-way search machinery (~2.8 KB); plain scans are enough here +const char * sl_strstr(const char * haystack, const char * needle); +const char * sl_strnstr(const char * haystack, const char * needle, int n); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d0ce1e5d242..2fff85dda44 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -999,9 +999,9 @@ static void cliSerialPassthrough(char *cmdline) baud = fastA2I(tok); break; case 2: - if (strstr(tok, "rx") || strstr(tok, "RX")) + if (sl_strstr(tok, "rx") || sl_strstr(tok, "RX")) mode |= MODE_RX; - if (strstr(tok, "tx") || strstr(tok, "TX")) + if (sl_strstr(tok, "tx") || sl_strstr(tok, "TX")) mode |= MODE_TX; break; case 3: @@ -4014,7 +4014,7 @@ static void cliSet(char *cmdline) cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui cliPrintLinefeed(); } - } else if ((eqptr = strstr(cmdline, "=")) != NULL) { + } else if ((eqptr = strchr(cmdline, '=')) != NULL) { // has equals char *lastNonSpaceCharacter = eqptr; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index e3be177fb16..0676f88c8e0 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -102,7 +102,7 @@ void settingGetName(const setting_t *val, char *buf) bool settingNameContains(const setting_t *val, char *buf, const char *cmdline) { settingGetName(val, buf); - return strstr(buf, cmdline) != NULL; + return sl_strstr(buf, cmdline) != NULL; } bool settingNameIsExactMatch(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 703242d10dd..0d48216afec 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -35,6 +35,7 @@ #include "common/typeconversion.h" #include "common/gps_conversion.h" #include "common/maths.h" +#include "common/string_light.h" #include "common/utils.h" #include "drivers/serial.h" @@ -742,17 +743,17 @@ static bool gpsParseFrameUBLOX(void) for (int j = 40; j < _payload_length && !found; j += 30) { // Example content: GPS;GAL;BDS;GLO - if (strnstr((const char *)(_buffer.bytes + j), "GAL", 30)) + if (sl_strnstr((const char *)(_buffer.bytes + j), "GAL", 30)) { ubx_capabilities.supported |= UBX_MON_GNSS_GALILEO_MASK; found = true; } - if (strnstr((const char *)(_buffer.bytes + j), "BDS", 30)) + if (sl_strnstr((const char *)(_buffer.bytes + j), "BDS", 30)) { ubx_capabilities.supported |= UBX_MON_GNSS_BEIDOU_MASK; found = true; } - if (strnstr((const char *)(_buffer.bytes + j), "GLO", 30)) + if (sl_strnstr((const char *)(_buffer.bytes + j), "GLO", 30)) { ubx_capabilities.supported |= UBX_MON_GNSS_GLONASS_MASK; found = true; @@ -760,7 +761,7 @@ static bool gpsParseFrameUBLOX(void) } } for(int j = 40; j < _payload_length; j += 30) { - if (strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) { + if (sl_strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) { gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30); break; } diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 201977813d0..6cf758dba8a 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -480,28 +480,6 @@ bool isMPUSoftReset(void) return false; } -// Not in linux libs, but in arm-none-eabi ?!? -// https://github.com/lattera/freebsd/blob/master/lib/libc/string/strnstr.c -char * strnstr(const char *s, const char *find, size_t slen) -{ - char c, sc; - size_t len; - - if ((c = *find++) != '\0') { - len = strlen(find); - do { - do { - if (slen-- < 1 || (sc = *s++) == '\0') - return (NULL); - } while (sc != c); - if (len > slen) - return (NULL); - } while (strncmp(s, find, len) != 0); - s--; - } - return ((char *)s); -} - int lookupAddress (char *name, int port, int type, struct sockaddr *addr, socklen_t* len ) { struct addrinfo *servinfo, *p; diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 8156bbc8f7a..743c08bf919 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -220,7 +220,6 @@ typedef enum extern bool lockMainPID(void); extern void unlockMainPID(void); extern void parseArguments(int argc, char *argv[]); -extern char *strnstr(const char *s, const char *find, size_t slen); extern int lookupAddress (char *, int, int, struct sockaddr *, socklen_t*); #define IPADDRESS_PRINT_BUFLEN (INET6_ADDRSTRLEN + 16) From 214e1991f30c6f53f2a5d9d8a6bce43ca59c983d Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 22 Sep 2026 23:04:40 +0200 Subject: [PATCH 10/31] Use INAV's trig approximations in the wind estimator and OSD tracker These were the last callers of libm sinf/cosf/atan2f on targets without USE_TERRAIN. sinf/cosf link the large-argument range reduction (rem_pio2 plus its 792 B table) although the arguments here never exceed 2 pi. sin_approx/cos_approx are within 4e-7 of libm over that range and are the codebase standard; the results feed a smoothed wind estimate and an integer-degree telemetry value. The tracker elevation is now rounded instead of truncated, so the approximation's 3e-7 rad error at exact integer angles (45 deg) cannot shift the reported value by a degree. MATEKF722 -4808 B, MATEKF405 -4616 B, NEUTRONRCF435WING -4616 B, MATEKF765/H743 -760 B flash; RAM unchanged. Co-Authored-By: Claude Fable 5.1 --- src/main/flight/wind_estimator.c | 6 +++--- src/main/io/osd.c | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 8ba32ced26c..af12d1b7678 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -184,9 +184,9 @@ void updateWindEstimator(timeMs_t currentTimeMs) memcpy(lastFuselageDirection, fuselageDirection, sizeof(lastFuselageDirection)); memcpy(lastGroundVelocity, groundVelocity, sizeof(lastGroundVelocity)); - float theta = atan2f(groundVelocityDiff[Y], groundVelocityDiff[X]) - atan2f(fuselageDirectionDiff[Y], fuselageDirectionDiff[X]); // equation 9 - float sintheta = sinf(theta); - float costheta = cosf(theta); + float theta = atan2_approx(groundVelocityDiff[Y], groundVelocityDiff[X]) - atan2_approx(fuselageDirectionDiff[Y], fuselageDirectionDiff[X]); // equation 9 + float sintheta = sin_approx(theta); + float costheta = cos_approx(theta); float wind[XYZ_AXIS_COUNT]; wind[X] = (groundVelocitySum[X] - V * (costheta * fuselageDirectionSum[X] - sintheta * fuselageDirectionSum[Y])) * 0.5f; // equation 10 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 897766eb766..88f99c83d51 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1538,8 +1538,8 @@ static void osdDisplayTelemetry(void) trk_bearing += 360 + 180; trk_bearing %= 360; int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); - float at = atan2f((float)alt, (float)GPS_distanceToHome); - trk_elevation = at * 57.2957795f; // 57.2957795 = 1 rad + float at = atan2_approx((float)alt, (float)GPS_distanceToHome); + trk_elevation = lrintf(at * 57.2957795f); // 57.2957795 = 1 rad trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 if (trk_elevation < 0) { trk_elevation = 0; From 81b35634799b7c1c7079674ed4ec2d27cd81be69 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 22 Sep 2026 23:04:40 +0200 Subject: [PATCH 11/31] Drop qsort: sort battery profiles and geozones in place Both callers sort tiny arrays at init time: the three battery profiles by detect voltage, and the geozone runtime list into "enabled first". An insertion sort and a stable partition do the same and newlib's qsort (1.6 KB) leaves the image. Enabled geozones now keep their configured order instead of qsort's permutation; the battery order is unchanged (newlib already insertion-sorts fewer than seven elements). MATEKF722 -1472 B, MATEKF405 -1408 B, MATEKF765 -1288 B, MATEKH743 -1344 B, NEUTRONRCF435WING -1408 B flash. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation_geozone.c | 28 +++++++++++------------- src/main/sensors/battery.c | 22 +++++++++---------- 2 files changed, 23 insertions(+), 27 deletions(-) diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 03dd2b4f382..cfa845252c2 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -811,20 +811,6 @@ static uint8_t getCurrentZones(geoZoneRuntimeConfig_t *zones[], const bool ignor return getZonesForPos(zones, &navGetCurrentActualPositionAndVelocity()->pos, ignoreAltitude); } -static int geoZoneRTComp(const void *a, const void *b) -{ - geoZoneRuntimeConfig_t *zoneA = (geoZoneRuntimeConfig_t*)a; - geoZoneRuntimeConfig_t *zoneB = (geoZoneRuntimeConfig_t*)b; - - if (zoneA->enable == zoneB->enable) { - return 0; - } else if (zoneA->enable) { - return -1; - } else { - return 1; - } -} - // in cm and cms/s static uint32_t calcTime(const int32_t distance, const int32_t speed) { @@ -1726,7 +1712,19 @@ static void geoZoneInit(void) } geozoneIsEnabled = true; - qsort(activeGeoZones, MAX_GEOZONES, sizeof(geoZoneRuntimeConfig_t), geoZoneRTComp); + // Enabled zones first, both groups in their original order (stable, unlike qsort, and avoids linking it) + int firstDisabled = 0; + for (int i = 0; i < MAX_GEOZONES; i++) { + if (!activeGeoZones[i].enable) { + continue; + } + if (i != firstDisabled) { + geoZoneRuntimeConfig_t enabledZone = activeGeoZones[i]; + memmove(&activeGeoZones[firstDisabled + 1], &activeGeoZones[firstDisabled], (i - firstDisabled) * sizeof(geoZoneRuntimeConfig_t)); + activeGeoZones[firstDisabled] = enabledZone; + } + firstDisabled++; + } for (int i = 0; i < activeGeoZonesCount; i++) { if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 10cb6872af9..6db0d96c781 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -270,16 +270,6 @@ void batteryUpdateThresholdsAndCells(void) } #ifdef USE_ADC -// profileDetect() profile sorting compare function -static int profile_compare(profile_comp_t *a, profile_comp_t *b) { - if (a->max_voltage < b->max_voltage) - return -1; - else if (a->max_voltage > b->max_voltage) - return 1; - else - return 0; -} - // Find profile matching plugged battery for profile_autoselect static int8_t profileDetect(void) { profile_comp_t profile_comp_array[MAX_BATTERY_PROFILE_COUNT]; @@ -291,8 +281,16 @@ static int8_t profileDetect(void) { profile_comp_array[i].max_voltage = profile->cells * profile->voltage.cellDetect; } - // Sort profiles by max voltage - qsort(profile_comp_array, MAX_BATTERY_PROFILE_COUNT, sizeof(*profile_comp_array), (int (*)(const void *, const void *))profile_compare); + // Sort profiles by max voltage; insertion sort keeps equal profiles in index order and avoids linking qsort + for (uint8_t i = 1; i < MAX_BATTERY_PROFILE_COUNT; ++i) { + const profile_comp_t key = profile_comp_array[i]; + int8_t j = i - 1; + while (j >= 0 && profile_comp_array[j].max_voltage > key.max_voltage) { + profile_comp_array[j + 1] = profile_comp_array[j]; + --j; + } + profile_comp_array[j + 1] = key; + } // Return index of the first profile where vbat <= profile_max_voltage for (uint8_t i = 0; i < MAX_BATTERY_PROFILE_COUNT; ++i) From 88f350346a278882959f52dd3bbcf7ea84274667 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 22 Sep 2026 22:24:08 +0200 Subject: [PATCH 12/31] Navigation: order the FSM descriptor fields to avoid padding Three byte-sized members sat between word-sized ones, costing 4 B of padding per state in the const navFSM table. The table uses designated initialisers, so the order change has no other effect. MATEKF722 -196 B, the other reference targets -260 B flash. Co-Authored-By: Claude Fable 5.1 --- src/main/navigation/navigation_private.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 7f7fd84f481..7974f428e0f 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -377,14 +377,15 @@ typedef enum { NAV_CTL_SPEED = (1 << 18), // Auto speed allowed } navigationFSMStateFlags_t; +// Word-sized members first, then the byte-sized ones: no padding inside the const table typedef struct { - navigationPersistentId_e persistentId; navigationFSMEvent_t (*onEntry)(navigationFSMState_t previousState); uint32_t timeoutMs; - navSystemStatus_State_e mwState; - navSystemStatus_Error_e mwError; navigationFSMStateFlags_t stateFlags; flightModeFlags_e mapToFlightModes; + navigationPersistentId_e persistentId; + navSystemStatus_State_e mwState; + navSystemStatus_Error_e mwError; navigationFSMState_t onEvent[NAV_FSM_EVENT_COUNT]; } navigationFSMStateDescriptor_t; From dfcd7c7a7330551a421699e633f2a4d7869330f8 Mon Sep 17 00:00:00 2001 From: Raffi1202 <250872901+Raffi1202@users.noreply.github.com> Date: Thu, 24 Sep 2026 18:12:42 +0200 Subject: [PATCH 13/31] Return true after blanking the RX downlink power element Like the other OSD elements that hide themselves at runtime, the element now takes its slot in the per-cycle draw budget after blanking its field. No visible change. The receiver transmit power is carried by CRSF frame 0x1D, not 0x1C. INAV parses neither and ExpressLRS sends neither, so the docs no longer claim that CRSF cannot carry the value, and name mLRS only as MAVLink receiver. --- docs/OSD.md | 2 +- src/main/io/osd.c | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/docs/OSD.md b/docs/OSD.md index 7a976a2b0fc..d0459508e81 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -192,7 +192,7 @@ Here are the OSD Elements provided by INAV. | 157 | OSD_CUSTOM_ELEMENT_7 | 8.0.0 | | | 158 | OSD_CUSTOM_ELEMENT_8 | 8.0.0 | | | 159 | OSD_LQ_DOWNLINK | 8.0.0 | | -| 160 | OSD_RX_POWER_DOWNLINK | 8.0.0 | The element will be hidden unless the RC link reports the transmit power of the receiver. CRSF does not send this value, it is available on MSP and MAVLink RC links. | +| 160 | OSD_RX_POWER_DOWNLINK | 8.0.0 | The element will be hidden unless the RC link reports the transmit power of the receiver. MSP RC links and mLRS as MAVLink receiver report it, INAV does not read it from CRSF links such as ExpressLRS. | | 161 | OSD_RX_BAND | 8.0.0 | | | 162 | OSD_RX_MODE | 8.0.0 | | | 163 | OSD_COURSE_TO_FENCE | 8.0.0 | | diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 235a43a60c9..cafa5e81765 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2845,11 +2845,10 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_RX_POWER_DOWNLINK: { - // Only the MSP and MAVLink RC links report the transmit power of the receiver, the CRSF - // link statistics frame has no such field. Hide the element instead of showing a permanent 0. + // 0 means the link does not report receiver power (e.g. ExpressLRS over CRSF) if (rxLinkStatistics.downlinkTXPower == 0) { displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); - return false; + return true; } if (!failsafeIsReceivingRxData()) From a46881e923983aaf53ec1e3c3a6595033f619970 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Thu, 24 Sep 2026 08:03:12 +0300 Subject: [PATCH 14/31] gps: ask u-blox receivers for satellite details twice a second NAV-SIG (M9, M10) and NAV-SAT (M8) were requested at every navigation epoch. They feed only the satellite list in the CLI, yet they are most of what the receiver sends: NAV-SIG carries 16 bytes for each tracked signal, several hundred bytes per epoch on a multi-constellation receiver, against about 100 for the NAV-PVT that navigation runs on. They are now requested every (gps_ublox_nav_hz + 1) / 2 epochs, which is about twice a second at any navigation rate. --- src/main/io/gps_ublox.c | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 703242d10dd..d12dcbabea0 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -955,6 +955,15 @@ static uint16_t hz2rate(uint8_t hz) return 1000 / hz; } +// NAV-SIG and NAV-SAT feed only the satellite list in the CLI, yet at every epoch they are +// most of what the receiver sends: NAV-SIG carries 16 bytes for each tracked signal, several +// hundred bytes on a multi-constellation receiver. Twice a second is plenty. The rate is +// counted in navigation epochs, so it follows the configured navigation rate. +static uint8_t ubloxSatInfoRate(void) +{ + return MAX(1, (gpsState.gpsConfig->ubloxNavHz + 1) / 2); +} + STATIC_PROTOTHREAD(gpsConfigure) { ptBegin(gpsConfigure); @@ -1073,7 +1082,7 @@ STATIC_PROTOTHREAD(gpsConfigure) {UBLOX_CFG_MSGOUT_NAV_VELNED_UART1, 0}, // 2 {UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1, 0}, // 3 {UBLOX_CFG_MSGOUT_NAV_PVT_UART1, 1}, // 4 - {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, 1}, // 5 + {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, ubloxSatInfoRate()}, // 5 {UBLOX_CFG_MSGOUT_NAV_SAT_UART1, 0} // 6 }; @@ -1101,7 +1110,7 @@ STATIC_PROTOTHREAD(gpsConfigure) configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 1); + configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, ubloxSatInfoRate()); ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); } else { // Really old stuff, consider upgrading :), ols setting API, no PVT or NAV_SAT or NAV_SIG // TODO: remove in INAV 9.0.0 From f0fbc98fafa7e08ffba574c99f586755bdbca4c2 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Thu, 24 Sep 2026 08:03:12 +0300 Subject: [PATCH 15/31] gps: give the GPS port a 512 byte receive buffer A receive buffer has to hold what arrives between two runs of the task that drains it. For the GPS task, 20 ms apart, that is up to 230 bytes at 115200 and 460 at 230400, and a reply to a poll has to fit whole: MON-VER alone can be 258 bytes. With the 256 bytes every port has, a NEO-F10N at 230400 was never identified, because its MON-VER reply was always cut. The GPS port now gets a 512 byte buffer of its own, in FASTRAM, where there is room: CCM on F405, RAM1 on AT32F43x. serialSetRxBuffer() swaps it in right after the port is opened. Every other port keeps its 256 bytes. The note that occupied sizes are returned as uint8_t no longer held: the driver functions and the ring indices are all 32 bit. --- src/main/drivers/serial.c | 23 +++++++++++++++++++++++ src/main/drivers/serial.h | 1 + src/main/drivers/serial_uart.h | 6 +++--- src/main/io/gps.c | 9 +++++++++ src/test/unit/gps_null_port_unittest.cc | 7 +++++++ 5 files changed, 43 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index 1cba2609914..a372f321389 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -20,6 +20,10 @@ #include "platform.h" +#include "build/atomic.h" + +#include "drivers/nvic.h" + #include "serial.h" void serialPrint(serialPort_t *instance, const char *str) @@ -133,3 +137,22 @@ bool serialIsIdle(serialPort_t *instance) else return false; } + +// Gives a port a receive buffer larger than the one its driver owns, for the one consumer +// that needs more room than every port gets. Call it right after opening the port: whatever +// the old buffer held is dropped. The size must be a power of two. A port whose own buffer +// is already as large keeps it. +void serialSetRxBuffer(serialPort_t *instance, volatile uint8_t *buffer, uint32_t size) +{ + if (size <= instance->rxBufferSize) { + return; + } + + // The receive interrupt must never see the new buffer with the old size or indices + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + instance->rxBuffer = buffer; + instance->rxBufferSize = size; + instance->rxBufferHead = 0; + instance->rxBufferTail = 0; + } +} diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index 6ca0a7d1ec2..8f1b810a631 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -122,6 +122,7 @@ void serialPrint(serialPort_t *instance, const char *str); uint32_t serialGetBaudRate(serialPort_t *instance); bool serialIsConnected(const serialPort_t *instance); bool serialIsIdle(serialPort_t *instance); +void serialSetRxBuffer(serialPort_t *instance, volatile uint8_t *buffer, uint32_t size); // A shim that adapts the bufWriter API to the serialWriteBuf() API. void serialWriteBufShim(void *instance, const uint8_t *data, int count); diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 4c1e6d57508..d00715248bb 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -22,9 +22,9 @@ // Since serial ports can be used for any function these buffer sizes should be equal // The two largest things that need to be sent are: 1, MSP responses, 2, UBLOX SVINFO packet. -// Size must be a power of two due to various optimizations which use 'and' instead of 'mod' -// Various serial routines return the buffer occupied size as uint8_t which would need to be extended in order to -// increase size further. +// Size must be a power of two due to various optimizations which use 'and' instead of 'mod'. +// Occupied and free sizes are returned as uint32_t, so a buffer may exceed 256 bytes. The GPS +// port gets a larger receive buffer of its own, see gps.c. #define UART1_RX_BUFFER_SIZE 256 #define UART1_TX_BUFFER_SIZE 256 #define UART2_RX_BUFFER_SIZE 256 diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 5d34c97ec3c..4e59eb2608d 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -91,6 +91,13 @@ gpsStatistics_t gpsStats; gpsSolutionData_t gpsSolDRV; //filled by driver gpsSolutionData_t gpsSol; //used in the rest of the code +// The GPS port gets a receive buffer of its own, larger than the one every port has. It has +// to hold what arrives between two runs of the GPS task, 20 ms apart, which is up to 460 bytes +// at 230400, and a reply to a poll has to fit whole: MON-VER alone can be 258 bytes. In +// FASTRAM, which is CCM on F405 and RAM1 on AT32F43x, where there is room to spare. +#define GPS_RX_BUFFER_SIZE 512 +STATIC_FASTRAM volatile uint8_t gpsRxBuffer[GPS_RX_BUFFER_SIZE]; + // Map gpsBaudRate_e index to baudRate_e baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 }; @@ -526,6 +533,8 @@ void gpsInit(void) return; } + serialSetRxBuffer(gpsState.gpsPort, gpsRxBuffer, sizeof(gpsRxBuffer)); + gpsSetState(GPS_INITIALIZING); } diff --git a/src/test/unit/gps_null_port_unittest.cc b/src/test/unit/gps_null_port_unittest.cc index 288b68995c9..cd41bb2e6ff 100644 --- a/src/test/unit/gps_null_port_unittest.cc +++ b/src/test/unit/gps_null_port_unittest.cc @@ -189,6 +189,13 @@ void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate) UNUSED(baudRate); } +void serialSetRxBuffer(serialPort_t *instance, volatile uint8_t *buffer, uint32_t size) +{ + UNUSED(instance); + UNUSED(buffer); + UNUSED(size); +} + serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, serialReceiveCallbackPtr rxCallback, From 3a0ced5e7d40371c8f7a06acbce721a737af4730 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Fri, 25 Sep 2026 13:40:25 +0200 Subject: [PATCH 16/31] gps: shorter comments, the buffer's limit, and u-blox only The satellite details also feed the OSD's GPS extra stats, not only gpssats, so the comment now says both. The comments are one line each, the buffer states the rate it covers (230400; at 460800 and above a NAV-PVT plus a large NAV-SIG can still overflow it), its size is asserted to be a power of two because softserial masks with size - 1, and it only exists where the u-blox provider does, the only one that opens a port. The note in serial_uart.h about sending a UBLOX SVINFO was stale. --- src/main/drivers/serial.c | 5 +---- src/main/drivers/serial_uart.h | 2 +- src/main/io/gps.c | 10 ++++++---- src/main/io/gps_ublox.c | 5 +---- 4 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index a372f321389..a822715f60f 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -138,10 +138,7 @@ bool serialIsIdle(serialPort_t *instance) return false; } -// Gives a port a receive buffer larger than the one its driver owns, for the one consumer -// that needs more room than every port gets. Call it right after opening the port: whatever -// the old buffer held is dropped. The size must be a power of two. A port whose own buffer -// is already as large keeps it. +// A larger receive buffer, swapped in right after opening: what the old one held is dropped void serialSetRxBuffer(serialPort_t *instance, volatile uint8_t *buffer, uint32_t size) { if (size <= instance->rxBufferSize) { diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index d00715248bb..ea7f5d7b251 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -20,7 +20,7 @@ #define UART_AF(uart, af) CONCAT3(GPIO_AF, af, _ ## uart) // Since serial ports can be used for any function these buffer sizes should be equal -// The two largest things that need to be sent are: 1, MSP responses, 2, UBLOX SVINFO packet. +// The largest thing that needs to be sent is an MSP response. // Size must be a power of two due to various optimizations which use 'and' instead of 'mod'. // Occupied and free sizes are returned as uint32_t, so a buffer may exceed 256 bytes. The GPS diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 4e59eb2608d..6fa715bcd36 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -91,12 +91,12 @@ gpsStatistics_t gpsStats; gpsSolutionData_t gpsSolDRV; //filled by driver gpsSolutionData_t gpsSol; //used in the rest of the code -// The GPS port gets a receive buffer of its own, larger than the one every port has. It has -// to hold what arrives between two runs of the GPS task, 20 ms apart, which is up to 460 bytes -// at 230400, and a reply to a poll has to fit whole: MON-VER alone can be 258 bytes. In -// FASTRAM, which is CCM on F405 and RAM1 on AT32F43x, where there is room to spare. +#ifdef USE_GPS_PROTO_UBLOX +// Covers 230400; at 460800 and above a NAV-PVT plus a large NAV-SIG can still overflow it #define GPS_RX_BUFFER_SIZE 512 +STATIC_ASSERT((GPS_RX_BUFFER_SIZE & (GPS_RX_BUFFER_SIZE - 1)) == 0, gps_rx_buffer_size_not_power_of_2); STATIC_FASTRAM volatile uint8_t gpsRxBuffer[GPS_RX_BUFFER_SIZE]; +#endif // Map gpsBaudRate_e index to baudRate_e baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 }; @@ -533,7 +533,9 @@ void gpsInit(void) return; } +#ifdef USE_GPS_PROTO_UBLOX serialSetRxBuffer(gpsState.gpsPort, gpsRxBuffer, sizeof(gpsRxBuffer)); +#endif gpsSetState(GPS_INITIALIZING); } diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index d12dcbabea0..193ccf3e998 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -955,10 +955,7 @@ static uint16_t hz2rate(uint8_t hz) return 1000 / hz; } -// NAV-SIG and NAV-SAT feed only the satellite list in the CLI, yet at every epoch they are -// most of what the receiver sends: NAV-SIG carries 16 bytes for each tracked signal, several -// hundred bytes on a multi-constellation receiver. Twice a second is plenty. The rate is -// counted in navigation epochs, so it follows the configured navigation rate. +// Satellite details only feed gpssats and the OSD's GPS extra stats: about 2 Hz, in epochs static uint8_t ubloxSatInfoRate(void) { return MAX(1, (gpsState.gpsConfig->ubloxNavHz + 1) / 2); From 5b310b4f77bd33fd778c063dbcdb3e5c6444ae67 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Mon, 28 Sep 2026 00:18:42 +0200 Subject: [PATCH 17/31] Trim comments to the reasons the code doesn't show --- src/main/drivers/serial.c | 2 +- src/main/drivers/serial_uart.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index a822715f60f..f09704fa00e 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -138,7 +138,7 @@ bool serialIsIdle(serialPort_t *instance) return false; } -// A larger receive buffer, swapped in right after opening: what the old one held is dropped +// Swaps in a larger receive buffer; what the old one held is dropped void serialSetRxBuffer(serialPort_t *instance, volatile uint8_t *buffer, uint32_t size) { if (size <= instance->rxBufferSize) { diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index ea7f5d7b251..006fca03d05 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -23,8 +23,7 @@ // The largest thing that needs to be sent is an MSP response. // Size must be a power of two due to various optimizations which use 'and' instead of 'mod'. -// Occupied and free sizes are returned as uint32_t, so a buffer may exceed 256 bytes. The GPS -// port gets a larger receive buffer of its own, see gps.c. +// The GPS port gets a larger receive buffer of its own, see gps.c. #define UART1_RX_BUFFER_SIZE 256 #define UART1_TX_BUFFER_SIZE 256 #define UART2_RX_BUFFER_SIZE 256 From f6c16c8a4f299851472ead537780c4a74cd671c7 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Mon, 28 Sep 2026 13:12:01 +0200 Subject: [PATCH 18/31] gps: stub serialSetRxBuffer in the heartbeat test --- src/test/unit/gps_heartbeat_unittest.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/unit/gps_heartbeat_unittest.cc b/src/test/unit/gps_heartbeat_unittest.cc index 75505ec1ff0..e79dc19a9ae 100644 --- a/src/test/unit/gps_heartbeat_unittest.cc +++ b/src/test/unit/gps_heartbeat_unittest.cc @@ -72,6 +72,7 @@ void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { UN void serialPrint(serialPort_t *instance, const char *str) { UNUSED(instance); UNUSED(str); } void serialSetMode(serialPort_t *instance, portMode_t mode) { UNUSED(instance); UNUSED(mode); } void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate) { UNUSED(instance); UNUSED(baudRate); } +void serialSetRxBuffer(serialPort_t *instance, volatile uint8_t *buffer, uint32_t size) { UNUSED(instance); UNUSED(buffer); UNUSED(size); } serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, From e24b2cbf7cb851eb5fdf93bfbffd3804e7967896 Mon Sep 17 00:00:00 2001 From: Raffi1202 <250872901+Raffi1202@users.noreply.github.com> Date: Fri, 2 Oct 2026 16:04:31 +0200 Subject: [PATCH 19/31] serial: name the SBUS duplicate check as a getter, trim comment and docs Rename serialDuplicatedSinglePortFunctions() to serialGetDuplicatedSinglePortFunctions() so the helper has a verb name like the other serialGet*/serialFind* functions (Development.md #5). Cut the three-line comment above SERIAL_SINGLE_PORT_FUNCTIONS to one line that states why the list exists. docs/Serial.md: drop the "previously it was accepted" history from the constraint and say what happens to a saved configuration that already has SBUS output on several ports: only the first port in the serial list is used, the configuration is not reset. No functional change. --- docs/Serial.md | 2 +- src/main/fc/cli.c | 2 +- src/main/io/serial.c | 6 ++---- src/main/io/serial.h | 2 +- 4 files changed, 5 insertions(+), 7 deletions(-) diff --git a/docs/Serial.md b/docs/Serial.md index 7450ec5e8a9..96aa9a6b347 100644 --- a/docs/Serial.md +++ b/docs/Serial.md @@ -48,7 +48,7 @@ e.g. after configuring a port for GPS enable the GPS feature. * All telemetry systems except MSP will ignore any attempts to override the baudrate. * MSP/CLI can be shared with EITHER Blackbox OR telemetry (LTM or MAVlink, not RX telemetry). In shared mode blackbox or telemetry will be output only when armed. * Smartport telemetry cannot be shared with MSP. -* SBUS servo output (`SERVO_SERIAL`) can only be assigned to one port. The CLI refuses a second assignment; previously it was accepted and shown in the port list, but the second port was never opened. +* SBUS servo output (`SERVO_SERIAL`) can only be used on one port. The `serial` CLI command refuses to assign it to a second port. If a saved configuration already has it on several ports, only the first one in the `serial` list outputs SBUS. * No other serial port sharing combinations are valid. * You can use as many different telemetry systems as you like at the same time. * You can only use each telemetry system once. e.g. FrSky telemetry cannot be used on two port, but LTN Telemetry and FrSky on two different ports is fine. diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 01be003f49a..ef9617849fa 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -929,7 +929,7 @@ static void cliSerial(char *cmdline) return; } - const uint32_t duplicatedFunctions = serialDuplicatedSinglePortFunctions(&portConfig); + const uint32_t duplicatedFunctions = serialGetDuplicatedSinglePortFunctions(&portConfig); if (duplicatedFunctions) { cliPrintErrorLinef("Function %d is already assigned to another port", (int)duplicatedFunctions); return; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 9574ed363c2..752fac3f2ed 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -313,12 +313,10 @@ bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) return true; } -// Functions that are resolved with findSerialPortConfig() and therefore only ever use the -// first port they are assigned to. Assigning one of them to a second port has no effect at -// all, so the configuration layer refuses it instead of ignoring it silently. +// Only the first port carrying one of these is ever opened (findSerialPortConfig) #define SERIAL_SINGLE_PORT_FUNCTIONS (FUNCTION_SERVO_SERIAL) -uint32_t serialDuplicatedSinglePortFunctions(const serialPortConfig_t *portConfigToCheck) +uint32_t serialGetDuplicatedSinglePortFunctions(const serialPortConfig_t *portConfigToCheck) { uint32_t functionMaskOfOtherPorts = 0; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 2274ec6e36e..b9e8176ba19 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -159,7 +159,7 @@ void serialRemovePort(serialPortIdentifier_e identifier); uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); bool isSerialConfigValid(const serialConfig_t *serialConfig); -uint32_t serialDuplicatedSinglePortFunctions(const serialPortConfig_t *portConfigToCheck); +uint32_t serialGetDuplicatedSinglePortFunctions(const serialPortConfig_t *portConfigToCheck); serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier); bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier); serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function); From 0da3929e1b22db5c5eb98ae7f388a0d0aaf7e558 Mon Sep 17 00:00:00 2001 From: Raffi1202 <250872901+Raffi1202@users.noreply.github.com> Date: Fri, 2 Oct 2026 16:07:25 +0200 Subject: [PATCH 20/31] docs: state when the RX downlink power element is hidden Only MSP receivers that send MSP2_COMMON_SET_MSP_RC_INFO report the receiver transmit power, and mLRS reports 0 dBm or less as 0 mW, so the element is also hidden for mLRS at 1 mW and below. Say so in the OSD element table. --- docs/OSD.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/OSD.md b/docs/OSD.md index d0459508e81..78e821a6d60 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -192,7 +192,7 @@ Here are the OSD Elements provided by INAV. | 157 | OSD_CUSTOM_ELEMENT_7 | 8.0.0 | | | 158 | OSD_CUSTOM_ELEMENT_8 | 8.0.0 | | | 159 | OSD_LQ_DOWNLINK | 8.0.0 | | -| 160 | OSD_RX_POWER_DOWNLINK | 8.0.0 | The element will be hidden unless the RC link reports the transmit power of the receiver. MSP RC links and mLRS as MAVLink receiver report it, INAV does not read it from CRSF links such as ExpressLRS. | +| 160 | OSD_RX_POWER_DOWNLINK | 8.0.0 | The element is hidden while the receiver transmit power is 0 mW, which is also the value when the RC link does not report it. MSP receivers that send `MSP2_COMMON_SET_MSP_RC_INFO` and mLRS as MAVLink receiver report it (mLRS at 0 dBm or less reads as 0 mW). INAV does not read it from CRSF links such as ExpressLRS. | | 161 | OSD_RX_BAND | 8.0.0 | | | 162 | OSD_RX_MODE | 8.0.0 | | | 163 | OSD_COURSE_TO_FENCE | 8.0.0 | | From b25e907498f538eac5ce123c294ea22a8dd55c4a Mon Sep 17 00:00:00 2001 From: Raffi1202 <250872901+Raffi1202@users.noreply.github.com> Date: Sun, 4 Oct 2026 19:44:01 +0200 Subject: [PATCH 21/31] servos: shorten the carry over comment to one line Development.md asks for comments that explain why, not what. No functional change. --- src/main/flight/servos.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 010580edffa..464e050c17a 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -232,11 +232,9 @@ void loadCustomServoMixer(void) if (carryOverServoSpeedLimitsOnNextLoad) { for (int i = 0; i < servoRuleCount; i++) { if (movefilterCount >= MAX_SERVO_RULES_SWITCH_CARRY) { - break; // no room left to carry more filter states + break; } - // INPUT_MIXER_SWITCH_HELPER rules are carried over as well. They hold the - // still decaying output of an earlier switch, so dropping them would make - // the servo jump when switching back before that decay has finished. + // carry helper rules too, they still hold the decaying output of an earlier switch if(currentServoMixer[i].speed != 0 && fabsf(servoSpeedLimitFilter[i].state) > 0.01f) { servoMixerSwitchHelper[movefilterCount].targetChannel = currentServoMixer[i].targetChannel; servoMixerSwitchHelper[movefilterCount].speed = currentServoMixer[i].speed; From ba39efa2a49b5a37133ed657290cacd9d7075a6f Mon Sep 17 00:00:00 2001 From: JLP Date: Mon, 5 Oct 2026 19:47:42 -0400 Subject: [PATCH 22/31] F4 uart: mask received data to 8 bits, align with F7/H7 With parity enabled the F4 data register holds the parity bit in bit 8, and it was passed through to the rx callback. F7/H7 drivers already mask to 8 bits. Protocol handlers comparing the received value directly, e.g. the SBUS 0x2F start byte, never matched on F4. --- src/main/drivers/serial_uart_stm32f4xx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 0f54fb69142..6b5bc7fddcf 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -217,7 +217,7 @@ void uartIrqHandler(uartPort_t *s) { if (USART_GetITStatus(s->USARTx, USART_IT_RXNE) == SET) { if (s->port.rxCallback) { - s->port.rxCallback(s->USARTx->DR, s->port.rxCallbackData); + s->port.rxCallback(s->USARTx->DR & 0xFF, s->port.rxCallbackData); // mask out parity bit } else { s->port.rxBuffer[s->port.rxBufferHead] = s->USARTx->DR; s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize; From 75f345502923f490f277b5d098973bdfb5df7d86 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Thu, 8 Oct 2026 13:03:29 +0200 Subject: [PATCH 23/31] pwm_mapping: a timer output on UART1's pins is skipped when UART1 has a function besides MSP checkPwmTimerConflicts() kept outputs off the pins of UART2 to UART8 but not UART1. On NEXUSX and VANTAC_RF007 the pads labelled AUX and SBUS are both UART1 and two timer outputs, and SPRACINGF7DUAL has the same on PWM 3 and 4: with a receiver on UART1, a motor or servo could be assigned to the receiver's pin and drive nothing once the receiver took it back. UART1 gets MSP by default next to the VCP, and MSP opens before the outputs and has always given them these pins, so MSP alone still does; any other function on UART1 now keeps them, as on UART2 to UART8. --- src/main/drivers/pwm_mapping.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 06cdf5ea279..15bc4787761 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -97,6 +97,15 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) { serialPortPins_t uartPins; +#if defined(USE_UART1) + // MSP is UART1's default next to the VCP and has always given these pins up to the outputs + const serialPortConfig_t *uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1); + uartGetPortPins(UARTDEV_1, &uartPins); + if (uart1Config && (uart1Config->functionMask & ~FUNCTION_MSP) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { + return true; + } +#endif + #if defined(USE_UART2) uartGetPortPins(UARTDEV_2, &uartPins); if (doesConfigurationUsePort(SERIAL_PORT_USART2) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { From f8af477c8021cb0d9720b5e68fa1f2229a033084 Mon Sep 17 00:00:00 2001 From: N1netyNine99 Date: Mon, 12 Oct 2026 11:58:17 +0800 Subject: [PATCH 24/31] =?UTF-8?q?fix:=E4=BF=AE=E6=AD=A3fcSetArmState?= =?UTF-8?q?=E7=9A=84=E8=A7=A3=E9=94=81=E7=BB=93=E6=9E=9C=E5=88=A4=E6=96=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/fc/fc_core.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0b0a20a729f..41fadff6094 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -678,7 +678,7 @@ bool fcSetArmState(bool arm) disarm(DISARM_SWITCH); } - return ARMING_FLAG(ARMED) == arm; + return (ARMING_FLAG(ARMED) != 0) == arm; } #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) From 3b3c9bc9ff870feaae3ec2d3af000c88f987cb68 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Wed, 14 Oct 2026 02:06:32 +0200 Subject: [PATCH 25/31] ist8310: reset the chip at init and use single measurements A reboot without a power cycle (CLI save or exit, a Configurator reboot, a DFU flash) left the IST8310 as it was. In the continuous mode the driver set, a value the datasheet lists as reserved, it then never raised DRDY again: the compass kept its last reading, or zeros right after boot, with MAG=OK in status until the next power cycle. Following the datasheet and PX4: soft reset at init (50 ms power-on reset), then single measurement mode, each read starting the next measurement and restarting one that never completed. STAT1 and the data registers come in one transfer, which cannot tear since the data only changes after a trigger. --- src/main/drivers/compass/compass_ist8310.c | 45 ++++++++-------------- 1 file changed, 17 insertions(+), 28 deletions(-) diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c index 85476d318e8..474e10b82f2 100644 --- a/src/main/drivers/compass/compass_ist8310.c +++ b/src/main/drivers/compass/compass_ist8310.c @@ -84,7 +84,6 @@ * This bit will be set to zero after POR routine */ -#define IST8310_REG_DATA 0x03 #define IST8310_REG_STAT1 0x02 #define IST8310_DRDY_MASK 0x01 #define IST8310_REG_WHOAMI 0x00 @@ -96,12 +95,7 @@ #define IST8310_REG_PDCNTL 0x42 // Parameter -// ODR = Output Data Rate, we use single measure mode for getting more data. #define IST8310_ODR_SINGLE 0x01 -#define IST8310_ODR_10_HZ 0x03 -#define IST8310_ODR_20_HZ 0x05 -#define IST8310_ODR_50_HZ 0x07 -#define IST8310_ODR_100_HZ 0x06 // Device ID (ist8310 -> 0x10) #define IST8310_CHIP_ID 0x10 @@ -114,8 +108,9 @@ static bool ist8310Init(magDev_t * mag) { - busWrite(mag->busDev, IST8310_REG_CNTRL1, IST8310_ODR_50_HZ); - delay(5); + // A reboot without a power cycle leaves the chip as it was, and it may never measure again + busWrite(mag->busDev, IST8310_REG_CNTRL2, IST8310_CNTRL2_RESET); + delay(50); // power-on reset routine: 50 ms max busWrite(mag->busDev, IST8310_REG_AVERAGE, IST8310_AVG_16); delay(5); @@ -123,37 +118,31 @@ static bool ist8310Init(magDev_t * mag) busWrite(mag->busDev, IST8310_REG_PDCNTL, IST8310_PULSE_DURATION_NORMAL); delay(5); + // Single measurement is the only mode the datasheet defines besides stand-by + busWrite(mag->busDev, IST8310_REG_CNTRL1, IST8310_ODR_SINGLE); + return true; } static bool ist8310Read(magDev_t * mag) { - uint8_t status = 0; - - if (!busRead(mag->busDev, IST8310_REG_STAT1, &status)) { + // STAT1 and the data in one read: in single measurement mode the data only changes after a trigger + uint8_t buf[7]; + if (!busReadBuf(mag->busDev, IST8310_REG_STAT1, buf, 7)) { return false; } - // The IST8310 updates its data registers once per output-data period. Only read - // them once DRDY is set; a read racing an update would mix old and new samples - // across axes, corrupting the mag vector and calibration. When no fresh sample - // is ready, return success so the caller keeps the previous values. - if (!(status & IST8310_DRDY_MASK)) { - return true; - } - - uint8_t buf[6]; - uint8_t LSB2FSV = 3; // 3mG - 14 bit + if (buf[0] & IST8310_DRDY_MASK) { + uint8_t LSB2FSV = 3; // 3mG - 14 bit - bool ack = busReadBuf(mag->busDev, IST8310_REG_DATA, buf, 6); - if (!ack) { - return false; + // Invert Y axis to co convert from left to right coordinate system + mag->magADCRaw[X] = (int16_t)(buf[2] << 8 | buf[1]) * LSB2FSV; + mag->magADCRaw[Y] = -(int16_t)(buf[4] << 8 | buf[3]) * LSB2FSV; + mag->magADCRaw[Z] = (int16_t)(buf[6] << 8 | buf[5]) * LSB2FSV; } - // Invert Y axis to co convert from left to right coordinate system - mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV; - mag->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV; - mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV; + // Also restarts a measurement that never completed: the task is far slower than its 6 ms + busWrite(mag->busDev, IST8310_REG_CNTRL1, IST8310_ODR_SINGLE); return true; } From 58059b219cbb05c07848f692b2dd729878ce0335 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Wed, 14 Oct 2026 02:45:11 +0200 Subject: [PATCH 26/31] ist8310: drop samples outside the measuring range Beyond 1600 uT on X and Y, or 2500 uT on Z, a sample is not a field: it can only be a corrupted transfer or a chip in a bad state, and one such sample is enough to throw off the sphere fit of a compass calibration. The previous sample is kept, as when no measurement is ready. ArduPilot's IST8310 driver makes the same check. --- src/main/drivers/compass/compass_ist8310.c | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c index 474e10b82f2..f6390abdca6 100644 --- a/src/main/drivers/compass/compass_ist8310.c +++ b/src/main/drivers/compass/compass_ist8310.c @@ -101,6 +101,8 @@ #define IST8310_CHIP_ID 0x10 #define IST8310_AVG_16 0x24 #define IST8310_PULSE_DURATION_NORMAL 0xC0 +#define IST8310_RANGE_XY 16000 // mG, 1600 uT +#define IST8310_RANGE_Z 25000 // mG, 2500 uT #define IST8310_CNTRL2_RESET 0x01 #define IST8310_CNTRL2_DRPOL 0x04 @@ -132,13 +134,19 @@ static bool ist8310Read(magDev_t * mag) return false; } - if (buf[0] & IST8310_DRDY_MASK) { - uint8_t LSB2FSV = 3; // 3mG - 14 bit + uint8_t LSB2FSV = 3; // 3mG - 14 bit + const int x = (int16_t)(buf[2] << 8 | buf[1]) * LSB2FSV; + const int y = (int16_t)(buf[4] << 8 | buf[3]) * LSB2FSV; + const int z = (int16_t)(buf[6] << 8 | buf[5]) * LSB2FSV; - // Invert Y axis to co convert from left to right coordinate system - mag->magADCRaw[X] = (int16_t)(buf[2] << 8 | buf[1]) * LSB2FSV; - mag->magADCRaw[Y] = -(int16_t)(buf[4] << 8 | buf[3]) * LSB2FSV; - mag->magADCRaw[Z] = (int16_t)(buf[6] << 8 | buf[5]) * LSB2FSV; + // Past the measuring range a sample can only be corrupt, and one spoils a compass calibration + const bool inRange = ABS(x) <= IST8310_RANGE_XY && ABS(y) <= IST8310_RANGE_XY && ABS(z) <= IST8310_RANGE_Z; + + if ((buf[0] & IST8310_DRDY_MASK) && inRange) { + // Invert Y axis to convert from left to right coordinate system + mag->magADCRaw[X] = x; + mag->magADCRaw[Y] = -y; + mag->magADCRaw[Z] = z; } // Also restarts a measurement that never completed: the task is far slower than its 6 ms From 69be40034a13f9494a56bc2b17ec3286debc78c5 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Sun, 25 Oct 2026 10:47:15 +0100 Subject: [PATCH 27/31] fw pid: an I-term that opposes a big stick input fades while the lock holds it The fixed-wing I-term lock damps P and D and freezes the I-term while the stick asks for a big rate, so the feed-forward flies the manoeuvre. An I-term built in a motion the pilot did not ask for is then held against the stick and eats that feed-forward: a trainer pulling out of a spiral dive at 34 m/s got +30 us of elevator from a demand of 111 deg/s, the I-term at +87 against a feed-forward of -115, and hit the ground level, a second short. Now an I-term of the opposite sign to the rate target fades with a 0.5 s time constant, scaled by how far the lock attenuates the I-term; one of the same sign, the trim the lock is meant to keep, is left alone. Same in the auto-transition controller. Unit test on the trainer's pitch loop. --- src/main/flight/pid.c | 13 ++ src/test/unit/CMakeLists.txt | 5 + src/test/unit/pid_fw_iterm_lock_fade_access.c | 46 ++++ .../unit/pid_fw_iterm_lock_fade_unittest.cc | 211 ++++++++++++++++++ 4 files changed, 275 insertions(+) create mode 100644 src/test/unit/pid_fw_iterm_lock_fade_access.c create mode 100644 src/test/unit/pid_fw_iterm_lock_fade_unittest.cc diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6cfd68eedae..a41932b6977 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -160,6 +160,9 @@ static EXTENDED_FASTRAM int16_t headingHoldTarget; static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter; static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter; +// Time constant of an I-term that opposes a big stick input while the lock holds it (see iTermLockApply) +#define FW_ITERM_LOCK_FADE_S 0.5f + // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied STATIC_FASTRAM bool pidGainsUpdateRequired= true; FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; @@ -1116,6 +1119,10 @@ static int16_t applyAutoTransitionTargetFixedWingRateController(autoTransitionTa pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI; } + if (pidState->errorGyroIf * rateTarget < 0.0f) { + pidState->errorGyroIf *= 1.0f - MIN((1.0f - pidState->attenuation.aI) * dT / FW_ITERM_LOCK_FADE_S, 1.0f); + } + applyAutoTransitionTargetItermLimiting(pidState, itermLimitActive); const uint16_t limit = 500; @@ -1709,6 +1716,12 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI; } + // The lock holds the I-term at whatever it had when the stick went out. Built in a motion the pilot did not ask + // for, it can stand against the stick and eat the feed-forward that is left: then it fades while the stick is out + if (pidState->errorGyroIf * rateTarget < 0.0f) { + pidState->errorGyroIf *= 1.0f - MIN((1.0f - pidState->attenuation.aI) * dT / FW_ITERM_LOCK_FADE_S, 1.0f); + } + applyItermLimiting(pidState); const uint16_t limit = pidState->pidSumLimit; diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index de97812ddcc..6c1ae6e4cc8 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -176,6 +176,11 @@ set_property(SOURCE pid_apa_iterm_update_unittest.cc PROPERTY depends set_property(SOURCE pid_apa_iterm_update_unittest.cc PROPERTY extra_sources "../test/unit/pid_apa_iterm_update_access.c") +set_property(SOURCE pid_fw_iterm_lock_fade_unittest.cc PROPERTY depends + "common/maths.c" "common/filter.c" "common/lulu.c") +set_property(SOURCE pid_fw_iterm_lock_fade_unittest.cc PROPERTY extra_sources + "../test/unit/pid_fw_iterm_lock_fade_access.c") + set_property(SOURCE mixer_transition_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) set_property(SOURCE mixer_transition_policy_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) set_property(SOURCE mixer_transition_scenarios_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) diff --git a/src/test/unit/pid_fw_iterm_lock_fade_access.c b/src/test/unit/pid_fw_iterm_lock_fade_access.c new file mode 100644 index 00000000000..e5bebc1f970 --- /dev/null +++ b/src/test/unit/pid_fw_iterm_lock_fade_access.c @@ -0,0 +1,46 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify this software + * 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. + * + * INAV is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + */ + +// Test-only access shim for pid_fw_iterm_lock_fade_unittest.cc: pidState[] and the fixed-wing rate controller are +// file-static in flight/pid.c, so the real, unmodified pid.c is #included here and the accessors below drive one axis +// of it with the values the test chooses. + +#include "flight/pid.c" + +void testPidFwSetAxis(int axis, float kP, float kI, float kFF, float iTerm, float gyroRate, float rateTarget) +{ + pidState_t *s = &pidState[axis]; + s->axis = axis; + s->kP = kP; + s->kI = kI; + s->kD = 0.0f; + s->kFF = kFF; + s->errorGyroIf = iTerm; + s->gyroRate = gyroRate; + s->rateTarget = rateTarget; + s->pidSumLimit = 500; + s->itermLimitActive = false; + s->itermFreezeActive = false; + s->ptermFilterApplyFn = (filterApply3FnPtr)nullFilterApply3; + s->attenuation.targetOverThresholdTimeMs = 0; +} + +void testPidFwStep(int axis, float dT) +{ + pidLoopNowMs += (timeMs_t)(dT * 1000.0f); + pidApplyFixedWingRateController(&pidState[axis], dT, 1.0f / dT); +} + +float testPidFwGetIterm(int axis) { return pidState[axis].errorGyroIf; } +float testPidFwGetAttenuationI(int axis) { return pidState[axis].attenuation.aI; } +int16_t testPidFwGetOutput(int axis) { return axisPID[axis]; } diff --git a/src/test/unit/pid_fw_iterm_lock_fade_unittest.cc b/src/test/unit/pid_fw_iterm_lock_fade_unittest.cc new file mode 100644 index 00000000000..18723158c20 --- /dev/null +++ b/src/test/unit/pid_fw_iterm_lock_fade_unittest.cc @@ -0,0 +1,211 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify this software + * 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. + * + * INAV is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + */ + +// The fixed-wing I-term lock (iTermLockApply) damps P and D and freezes the I-term while the stick asks for a big +// rate, leaving the feed-forward to fly the manoeuvre. An I-term built in a motion the pilot did not ask for (a +// pitch rate coupled from a full-aileron roll, say) is then held against the stick and eats that feed-forward: a +// trainer's pull-out at 34 m/s got +30 us of elevator from a demand of 111 deg/s, with the I-term at +87 against a +// feed-forward of -115, and hit the ground one second short. The fix fades an I-term that opposes the rate target +// while the lock attenuates the I-term (time constant FW_ITERM_LOCK_FADE_S), and leaves one that helps it alone. +// +// Links the real flight/pid.c through an access shim and drives its fixed-wing rate controller on one axis; the +// dependencies of the rest of pid.c are stubbed as in pid_apa_iterm_update_unittest.cc. + +#include "gtest/gtest.h" + +extern "C" { + #include + #include + #include + #include + + #include "build/debug.h" + #include "common/axis.h" + #include "common/filter.h" + #include "common/fp_pid.h" + #include "common/maths.h" + #include "common/vector.h" + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + #include "fc/config.h" + #include "fc/control_profile.h" + #include "fc/rc_controls.h" + #include "fc/rc_modes.h" + #include "fc/runtime_config.h" + #include "flight/adaptive_filter.h" + #include "flight/imu.h" + #include "flight/kalman.h" + #include "flight/mixer.h" + #include "flight/mixer_profile.h" + #include "flight/pid.h" + #include "flight/smith_predictor.h" + #include "navigation/navigation.h" + #include "programming/logic_condition.h" + #include "rx/rx.h" + #include "sensors/battery.h" + #include "sensors/gyro.h" + #include "sensors/pitotmeter.h" +} + +// Test-controlled inputs +static float g_airspeedCmS = 0.0f; +static bool g_pitotValid = true; + +extern "C" { + // Globals normally provided by other translation units + uint32_t stateFlags = 0; + uint32_t flightModeFlags = 0; + uint32_t armingFlags = 0; + int16_t rcCommand[4] = {}; + int32_t debug[DEBUG32_VALUE_COUNT] = {}; + uint8_t debugMode = 0; + attitudeEulerAngles_t attitude = {}; + fpVector3_t HeadVecEFFiltered = {}; + gyro_t gyro = {}; + const controlConfig_t *currentControlProfile = nullptr; + const batteryProfile_t *currentBatteryProfile = nullptr; + mixerConfig_t currentMixerConfig; + bool isMixerTransitionMixing = false; + navConfig_t navConfig_System; + navConfig_t navConfig_Copy; + + // Airspeed source: the one thing this test actually drives + float getAirspeedEstimate(void) { return g_airspeedCmS; } + bool pitotGetValidForAirspeed(void) { return g_pitotValid; } + bool pitotIsHealthy(void) { return g_pitotValid; } + + // Everything below is only referenced from code paths this test never runs + void adaptiveFilterPushRate(const flight_dynamics_index_t, const float, const uint8_t) {} + float applySmithPredictor(uint8_t, smithPredictor_t *, float sample) { return sample; } + void smithPredictorInit(smithPredictor_t *, float, float, uint16_t, uint32_t) {} + bool areSticksDeflected(void) { return false; } + void autotuneFixedWingUpdate(const flight_dynamics_index_t, float, float, float) {} + float calculateCosTiltAngle(void) { return 1.0f; } + rollPitchStatus_e calculateRollPitchCenterStatus(void) { return CENTERED; } + float getEstimatedActualVelocity(int) { return 0.0f; } + float getFlightAxisAngleOverride(uint8_t, float angle) { return angle; } + float getFlightAxisRateOverride(uint8_t, float rate) { return rate; } + bool isFlightAxisAngleOverrideActive(uint8_t) { return false; } + int16_t getRcCommandOverride(int16_t command[], uint8_t axis) { return command[axis]; } + uint32_t getLooptime(void) { return 1000; } + uint16_t getMaxThrottle(void) { return 2000; } + int getThrottleIdleValue(void) { return 1150; } + float getMotorMixRange(void) { return 0.0f; } + bool mixerIsOutputSaturated(void) { return false; } + int32_t getRcStickDeflection(int32_t) { return 0; } + void gyroKalmanUpdateSetpoint(uint8_t, float) {} + void imuTransformVectorEarthToBody(fpVector3_t *) {} + bool isAdjustingPosition(void) { return false; } + bool IS_RC_MODE_ACTIVE(boxId_e) { return false; } + bool isFwAutoModeActive(boxId_e) { return false; } + timeMs_t millis(void) { return 0; } + int8_t navCheckActiveAngleHoldAxis(void) { return 0; } + int8_t navigationGetHeadingControlState(void) { return 0; } + bool navigationIsControllingAltitude(void) { return false; } + bool navigationIsControllingThrottle(void) { return false; } + float navPidApply3(pidController_t *, const float, const float, const float, const float, const float, const pidControllerFlags_e, const float, const float) { return 0.0f; } + void navPidInit(pidController_t *, float, float, float, float, float, float) {} + void navPidReset(pidController_t *) {} + int16_t rxGetChannelValue(unsigned) { return 1500; } // sticks centred + bool sensors(uint32_t) { return false; } + + // Defined by PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE in pid.c + extern pidProfile_t pidProfile_Storage[]; + extern const pidProfile_t pgResetTemplate_pidProfile; + + // Provided by pid_fw_iterm_lock_fade_access.c (wraps the real flight/pid.c) + void testPidFwSetAxis(int axis, float kP, float kI, float kFF, float iTerm, float gyroRate, float rateTarget); + void testPidFwStep(int axis, float dT); + float testPidFwGetIterm(int axis); + float testPidFwGetAttenuationI(int axis); + int16_t testPidFwGetOutput(int axis); +} + +namespace { + +// The trainer's pitch loop: P 10, I 10, FF 85 (fw_p/i/ff_pitch), pitch_rate 12 = 120 deg/s, lock at its defaults +constexpr float KP = 10.0f / FP_PID_RATE_P_MULTIPLIER; +constexpr float KI = 10.0f / FP_PID_RATE_I_MULTIPLIER; +constexpr float KFF = 85.0f / FP_PID_RATE_FF_MULTIPLIER; +constexpr float FULL_PULL = -111.0f; // deg/s, the stick at its stop +constexpr float DT = 0.001f; + +class PidFwItermLockFadeTest : public ::testing::Test { +protected: + controlConfig_t controlCfg; + + void SetUp() override + { + memset(&controlCfg, 0, sizeof(controlCfg)); + controlCfg.stabilized.rates[FD_PITCH] = 12; + currentControlProfile = &controlCfg; + pidProfile_Storage[0] = pgResetTemplate_pidProfile; + pidProfile_ProfileCurrent = &pidProfile_Storage[0]; + flightModeFlags = 0; + } + + void run(float seconds) + { + for (int i = 0; i < (int)(seconds / DT + 0.5f); i++) { + testPidFwStep(FD_PITCH, DT); + } + } +}; + +// The crash case: +87 of I-term against a full pull. Held, it left -28 of output; fading, the feed-forward comes back +TEST_F(PidFwItermLockFadeTest, OpposingItermFadesWhileTheStickIsOut) +{ + testPidFwSetAxis(FD_PITCH, KP, KI, KFF, 87.0f, -20.0f, FULL_PULL); + testPidFwStep(FD_PITCH, DT); + ASSERT_NEAR(0.0f, testPidFwGetAttenuationI(FD_PITCH), 1e-3f); // the lock holds the I-term at a full stick + EXPECT_GT(testPidFwGetOutput(FD_PITCH), -250); // most of the feed-forward is eaten at first + + run(0.5f); + EXPECT_NEAR(87.0f * expf(-1.0f), testPidFwGetIterm(FD_PITCH), 3.0f); + run(0.5f); + EXPECT_NEAR(87.0f * expf(-2.0f), testPidFwGetIterm(FD_PITCH), 3.0f); + EXPECT_LT(testPidFwGetOutput(FD_PITCH), -270); // FF -304 plus what is left of the I-term +} + +// A trim that helps the pull (same sign as the demand) is what the lock is meant to keep: untouched +TEST_F(PidFwItermLockFadeTest, HelpingItermIsKept) +{ + testPidFwSetAxis(FD_PITCH, KP, KI, KFF, -87.0f, -20.0f, FULL_PULL); + run(1.0f); + EXPECT_NEAR(-87.0f, testPidFwGetIterm(FD_PITCH), 0.5f); +} + +// With the stick centred nothing fades, whatever the I-term holds +TEST_F(PidFwItermLockFadeTest, NothingFadesAtACentredStick) +{ + testPidFwSetAxis(FD_PITCH, KP, KI, KFF, 87.0f, 0.0f, 0.0f); + run(1.0f); + EXPECT_FLOAT_EQ(87.0f, testPidFwGetIterm(FD_PITCH)); +} + +// A mild stick attenuates the I-term only in part, and the fade follows the attenuation +TEST_F(PidFwItermLockFadeTest, MildStickFadesSlower) +{ + // 36 deg/s is 30 % of the maximum rate: the lock's Gaussian leaves the I-term about a fifth alive + testPidFwSetAxis(FD_PITCH, KP, KI, KFF, 87.0f, -36.0f, -36.0f); + testPidFwStep(FD_PITCH, DT); + const float aI = testPidFwGetAttenuationI(FD_PITCH); + ASSERT_GT(aI, 0.1f); + ASSERT_LT(aI, 0.4f); + run(0.5f); + const float expected = 87.0f * expf(-(1.0f - aI)); + EXPECT_NEAR(expected, testPidFwGetIterm(FD_PITCH), 3.0f); + EXPECT_GT(testPidFwGetIterm(FD_PITCH), 87.0f * expf(-1.0f) + 3.0f); +} + +} // namespace From 793aa6eb03e724c8215a3f5aa61ce990a5dc7955 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Sun, 25 Oct 2026 11:21:53 +0100 Subject: [PATCH 28/31] fw pid: the I-term fade only above the lock's own stick threshold, and not on yaw A small correction of the opposite sign to the trim would have erased it in a few seconds, and the rudder trim would have faded in a banked turn, where the yaw rate target is the turn's. The fade now applies on roll and pitch, and only when the rate target is above 20 % of the maximum rate, the threshold the lock itself uses to count a stick input as big. Two unit tests. --- src/main/flight/pid.c | 16 +++++++++----- .../unit/pid_fw_iterm_lock_fade_unittest.cc | 21 +++++++++++++++++-- 2 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a41932b6977..9b9a59da595 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -162,6 +162,8 @@ static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter; // Time constant of an I-term that opposes a big stick input while the lock holds it (see iTermLockApply) #define FW_ITERM_LOCK_FADE_S 0.5f +// Rate target, as a fraction of the maximum rate, from which the lock counts a stick input as big +#define FW_ITERM_LOCK_BIG_STICK 0.2f // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied STATIC_FASTRAM bool pidGainsUpdateRequired= true; @@ -1066,7 +1068,7 @@ static void applyAutoTransitionTargetItermLock(autoTransitionTargetPidState_t *p const float dampingFactor = attenuation(rateTarget, maxRate * targetPidProfile->fwItermLockRateLimit / 100.0f); const bool errorThresholdReached = fabsf(rateError) > maxRate * targetPidProfile->fwItermLockEngageThreshold / 100.0f; - if (fabsf(rateTarget) > maxRate * 0.2f) { + if (fabsf(rateTarget) > maxRate * FW_ITERM_LOCK_BIG_STICK) { pidState->attenuation.targetOverThresholdTimeMs = pidLoopNowMs; } @@ -1119,7 +1121,8 @@ static int16_t applyAutoTransitionTargetFixedWingRateController(autoTransitionTa pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI; } - if (pidState->errorGyroIf * rateTarget < 0.0f) { + if (axis != FD_YAW && pidState->errorGyroIf * rateTarget < 0.0f + && fabsf(rateTarget) > controlProfile->stabilized.rates[axis] * 10.0f * FW_ITERM_LOCK_BIG_STICK) { pidState->errorGyroIf *= 1.0f - MIN((1.0f - pidState->attenuation.aI) * dT / FW_ITERM_LOCK_FADE_S, 1.0f); } @@ -1676,7 +1679,7 @@ static void iTermLockApply(pidState_t *pidState, const float rateTarget, const f const bool errorThresholdReached = fabsf(rateError) > maxRate * pidProfile()->fwItermLockEngageThreshold / 100.0f; //When abs of rate target is above 20% of max rate, we start tracking time - if (fabsf(rateTarget) > maxRate * 0.2f) { + if (fabsf(rateTarget) > maxRate * FW_ITERM_LOCK_BIG_STICK) { pidState->attenuation.targetOverThresholdTimeMs = pidLoopNowMs; } @@ -1717,8 +1720,11 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float } // The lock holds the I-term at whatever it had when the stick went out. Built in a motion the pilot did not ask - // for, it can stand against the stick and eat the feed-forward that is left: then it fades while the stick is out - if (pidState->errorGyroIf * rateTarget < 0.0f) { + // for, it can stand against the stick and eat the feed-forward that is left: then it fades while the stick is out. + // Only for the inputs the lock counts as big, so a small correction keeps the trim, and not on yaw, whose rate + // target in a banked turn is the turn's and its I-term the rudder trim + if (pidState->axis != FD_YAW && pidState->errorGyroIf * rateTarget < 0.0f + && fabsf(rateTarget) > currentControlProfile->stabilized.rates[pidState->axis] * 10.0f * FW_ITERM_LOCK_BIG_STICK) { pidState->errorGyroIf *= 1.0f - MIN((1.0f - pidState->attenuation.aI) * dT / FW_ITERM_LOCK_FADE_S, 1.0f); } diff --git a/src/test/unit/pid_fw_iterm_lock_fade_unittest.cc b/src/test/unit/pid_fw_iterm_lock_fade_unittest.cc index 18723158c20..e83b4c7bd13 100644 --- a/src/test/unit/pid_fw_iterm_lock_fade_unittest.cc +++ b/src/test/unit/pid_fw_iterm_lock_fade_unittest.cc @@ -154,10 +154,10 @@ class PidFwItermLockFadeTest : public ::testing::Test { flightModeFlags = 0; } - void run(float seconds) + void run(float seconds, int axis = FD_PITCH) { for (int i = 0; i < (int)(seconds / DT + 0.5f); i++) { - testPidFwStep(FD_PITCH, DT); + testPidFwStep(axis, DT); } } }; @@ -208,4 +208,21 @@ TEST_F(PidFwItermLockFadeTest, MildStickFadesSlower) EXPECT_GT(testPidFwGetIterm(FD_PITCH), 87.0f * expf(-1.0f) + 3.0f); } +// A small correction (under 20 % of the maximum rate, where the lock does not count the stick as big) keeps the trim +TEST_F(PidFwItermLockFadeTest, SmallCorrectionKeepsTheTrim) +{ + testPidFwSetAxis(FD_PITCH, KP, KI, KFF, 87.0f, -18.0f, -18.0f); + run(2.0f); + EXPECT_FLOAT_EQ(87.0f, testPidFwGetIterm(FD_PITCH)); +} + +// The yaw axis is left out: in a banked turn its rate target is the turn's and its I-term is the rudder trim +TEST_F(PidFwItermLockFadeTest, YawTrimIsKeptInATurn) +{ + controlCfg.stabilized.rates[FD_YAW] = 3; + testPidFwSetAxis(FD_YAW, KP, KI, KFF, 40.0f, -25.0f, -25.0f); + run(2.0f, FD_YAW); + EXPECT_FLOAT_EQ(40.0f, testPidFwGetIterm(FD_YAW)); +} + } // namespace From 68bb04e0aa084e60b977cca4ea5d829e08ee67f0 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 9 Nov 2026 22:33:06 +0000 Subject: [PATCH 29/31] filter cut off fix --- src/main/sensors/pitotmeter.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index c380064c757..4cc31215ecb 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -246,9 +246,9 @@ STATIC_PROTOTHREAD(pitotThread) // Init filter pitot.lastMeasurementUs = micros(); - const uint16_t pitot_lpf_milli_hz = pitotmeterConfig()->pitot_lpf_milli_hz; - if (pitot_lpf_milli_hz > 0) { - pt1FilterSetCutoff(&pitot.lpfState, pitot_lpf_milli_hz); + const float pitot_lpf_hz = pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f; + if (pitot_lpf_hz > 0) { + pt1FilterSetCutoff(&pitot.lpfState, pitot_lpf_hz); } while(1) { From c1a5a8fb762e4a978ae4b917a5a0a997234c6503 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 9 Nov 2026 22:42:02 +0000 Subject: [PATCH 30/31] Update pitotmeter.c --- src/main/sensors/pitotmeter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 4cc31215ecb..0f3bd231179 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -246,7 +246,7 @@ STATIC_PROTOTHREAD(pitotThread) // Init filter pitot.lastMeasurementUs = micros(); - const float pitot_lpf_hz = pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f; + const float pitot_lpf_hz = 0.001f * pitotmeterConfig()->pitot_lpf_milli_hz; if (pitot_lpf_hz > 0) { pt1FilterSetCutoff(&pitot.lpfState, pitot_lpf_hz); } From 861564b3069362f2bc699cadf0576c98c0d25294 Mon Sep 17 00:00:00 2001 From: Sensei Date: Wed, 11 Nov 2026 00:10:35 -0600 Subject: [PATCH 31/31] Fix typo in flash chip management section --- docs/Cli.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Cli.md b/docs/Cli.md index be9def8c302..3082ead9115 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -245,7 +245,7 @@ Note that the usual caveat that one should not share a timer with both a motor a ## Flash chip management -For targets that have a flash data chip, typically used for blackbox logs, the following additional comamnds are provided. +For targets that have a flash data chip, typically used for blackbox logs, the following additional commands are provided. | Command | Effect | | ------- | ------ |