feat: 新增接收机偏航角速度漂移辅助 / Add receiver-side yaw-rate drift assistance

docs/drift-assist.md

@@ -0,0 +1,54 @@
+# Drift assistance / 漂移辅助
+
+Experimental throttle reduction for forward-only cars with a yaw-rate steering loop. This uses yaw-rate feedback, not a wheel-slip or sideslip-angle estimate. Vehicle testing is still required.
+
+The receiver runs assistance locally from standard ELRS channel input and its onboard gyro. LetsFly and the configurator do not need to remain open. Saved settings are loaded on receiver startup; CH3 is forward throttle, CH4 is steering and the default activation switch is CH8. The zero-throttle endpoint follows the existing 988us mapping. Both ESC and steering outputs must use the compatible mixer described below.
+
+## 使用
+
+1. 在新版 GLRS 配置器的“飞控 → 漂移辅助”展开设置。已解锁时，CH3 油门归零（988μs）后可保存，无需关闭解锁开关或 CH7 共存开关；未解锁时也可保存。遥控端无法到达此值时，断开遥控链路、进入仅 Wi-Fi 模式后保存。
+2. 保持 CH3 油门、CH4 偏航；油门使用正系数纯油门电机混控，转向使用纯偏航舵机混控，并将相应 PWM 输出设为混控来源。
+3. 总开关默认关闭。启用后，默认由 CH8 的 `[1700, 2100)` μs 区间控制，仅 RATE 角速度模式下生效。开关不能占用已启用的解锁或 Wi-Fi 通道。
+4. 默认介入偏差 20°/s、渐进区间 80°/s、最大减油 15%、介入时间常数 120ms、恢复时间常数 300ms。先架空核对停车值和方向，再低速试车。
+
+The controller compares absolute measured yaw rate with the maximum absolute target in the previous 250 ms. The short window avoids excessive intervention when the stick crosses center during a reversal. Above the configured excess-rate threshold, the reduction rises linearly over the configured span and is smoothed with separate attack/recovery time constants. Each frame uses the original throttle; zero throttle stops immediately, and assistance never adds throttle above the manual request. Turning the remote switch off smoothly restores manual throttle. Disarm, RC loss/Wi-Fi-only operation, mode changes and configuration changes clear the history.
+
+## Configuration and status
+
+`GET/POST /config` adds `fc_drift`:
+
+```json
+{
+  "enabled": false,
+  "channel": 8,
+  "range": [1700, 2100],
+  "threshold_dps": 20,
+  "ramp_dps": 80,
+  "max_reduction_percent": 15,
+  "attack_ms": 120,
+  "recovery_ms": 300
+}
+```
+
+Allowed ranges: channel 5–16; activation endpoints 900–2100 μs with start < end; threshold 5–100°/s; span 20–300°/s; reduction 0–30%; attack 50–500ms; recovery 100–1000ms. With the RC link connected, changes require valid recent state. Both explicit and automatic arming allow changes at zero CH3 throttle (988 μs, the existing control-loop endpoint), so the arm and CH7 coexistence switches can stay on. Armed nonzero throttle is rejected; disarmed changes remain allowed. This check uses the current receiver state, not the requested configuration. Unchanged drift settings and disconnected/Wi-Fi-only configuration remain allowed. Old configuration files default to assistance off. A missing field in a configuration request preserves the existing setting.
+
+`GET /status.json` adds `drift` with the reason/state, remote request, target/measured yaw rates, manual/assisted throttle percentages and actual reduction percentage. Existing MSP packet layouts are unchanged.
+
+## Validation
+
+Run `pio test -e native -f test_drift` from `src`. The Native environment requires a host GCC compiler. On Windows, the installed PlatformIO toolchain can be added for the current terminal:
+
+```powershell
+$env:PATH = "$env:USERPROFILE\.platformio\packages\toolchain-gccmingw32\bin;" + $env:PATH
+pio test -e native -f test_drift
+```
+
+Tests cover threshold, manual throttle priority, zero stop, symmetry, reduction limits, reset, time consistency, switch-off recovery, reversal handling, timer rollover, JSON validation and mixer compatibility.
+
+The 409 save-guard fix additionally covers armed zero/nonzero throttle regardless of arming method, disarmed changes, unavailable state, unchanged settings and disconnected configuration. Hardware coexistence behavior still needs checking on the receiver.
+
+Build `Unified_ESP32C3_2400_RX_via_WIFI` and bundle the `LightFin Nano 2.4GHz RX` hardware layout. A noninteractive build may produce a bare image; verify the hardware layout before flashing.
+
+2026-10-12 validation: five drift tests passed. The full Windows Native run passed 57 cases; two existing MSP assertions failed, and CRC/OTA builds failed on the OTA8 struct-size assertion. All three affected suites reproduced the same failures on pristine upstream `4316c4aa` with the same MinGW toolchain. A pre-existing empty local `test_crash_detection` directory also reports “Nothing to build”. These are not a full-suite pass. No on-vehicle validation or flashing was performed.
+
+2026-10-15 PR review: 56 selected Native regression cases passed, including all six drift cases; the known failing/empty suites above were excluded. ESP32-C3 firmware built and the LightFin hardware layout was packaged and verified. The JSON helper include is now inside the existing RX/flight-control compilation guard; a feature-disabled syntax check passed without ArduinoJson include paths. RF input, CRSF serialization and the existing flight-control scheduler remain unchanged. Vehicle behavior was not tested in this review.

src/lib/FlightControl/DriftAssist.h

@@ -0,0 +1,93 @@
+#pragma once
+
+#include <cmath>
+#include <stdint.h>
+
+struct DriftAssistSettings {
+    bool enabled = false;
+    uint8_t channel = 8; // One-based RC channel, like the HTTP configuration.
+    uint16_t startUs = 1700;
+    uint16_t endUs = 2100;
+    float thresholdDps = 20.0f;
+    float rampDps = 80.0f;
+    float maxReductionPercent = 15.0f;
+    uint16_t attackMs = 120;
+    uint16_t recoveryMs = 300;
+};
+
+inline bool driftSettingsValid(const DriftAssistSettings &s)
+{
+    return s.channel >= 5 && s.channel <= 16 && s.startUs >= 900 &&
+        s.endUs <= 2100 && s.startUs < s.endUs &&
+        std::isfinite(s.thresholdDps) && s.thresholdDps >= 5 && s.thresholdDps <= 100 &&
+        std::isfinite(s.rampDps) && s.rampDps >= 20 && s.rampDps <= 300 &&
+        std::isfinite(s.maxReductionPercent) && s.maxReductionPercent >= 0 && s.maxReductionPercent <= 30 &&
+        s.attackMs >= 50 && s.attackMs <= 500 && s.recoveryMs >= 100 && s.recoveryMs <= 1000;
+}
+
+// Allow zero-throttle changes while armed so Wi-Fi/RC coexistence can stay on.
+// Use loadStickTargets()' endpoint and reject unavailable/stale RC state.
+inline const char *driftSettingsChangeError(bool changed, bool connected,
+    bool stateValid, bool armed, uint16_t throttleUs)
+{
+    if (!changed || !connected) return nullptr;
+    if (!stateValid) return "Wait for valid RC state before changing drift assistance";
+    if (!armed) return nullptr;
+    if (throttleUs > 988) return "Lower throttle to zero before changing drift assistance";
+    return nullptr;
+}
+
+inline bool driftMixerCompatible(const float *mix, const bool *servos, unsigned count)
+{
+    if (!mix || !servos || !count || count > 32 || count % 4) return false;
+    bool throttle = false, steering = false;
+    for (unsigned i = 0; i < count / 4; ++i) {
+        const float *row = mix + i * 4;
+        for (unsigned j = 0; j < 4; ++j) if (!std::isfinite(row[j])) return false;
+        const bool idle = row[0] == 0 && row[1] == 0 && row[2] == 0 && row[3] == 0;
+        if (idle) continue;
+        if (!servos[i] && row[0] > 0 && row[1] == 0 && row[2] == 0 && row[3] == 0) throttle = true;
+        else if (servos[i] && row[0] == 0 && row[1] == 0 && row[2] == 0 && row[3] != 0) steering = true;
+        else return false;
+    }
+    return throttle && steering;
+}
+
+// Uses the original throttle on every call; no feedback into RC channel state.
+// The bounded monotonic queue gives an exact 250 ms maximum at the 250 Hz loop.
+class DriftAssist {
+public:
+    void reset() { _head = _count = 0; _reduction = 0; }
+    float reduction() const { return _reduction; }
+    float update(float throttle, float targetDps, float measuredDps, float dt,
+        uint32_t nowUs, bool requested, const DriftAssistSettings &s)
+    {
+        if (!std::isfinite(throttle) || !std::isfinite(targetDps) || !std::isfinite(measuredDps) ||
+            !std::isfinite(dt) || dt <= 0 || dt > 0.1f || !driftSettingsValid(s)) {
+            reset();
+            return std::isfinite(throttle) ? throttle : 0.0f;
+        }
+        if (throttle <= 0) { reset(); return 0; }
+        const float target = fabsf(targetDps);
+        while (_count && (uint32_t)(nowUs - _samples[_head].timeUs) >= 250000U) {
+            _head = (_head + 1) % CAPACITY;
+            --_count;
+        }
+        while (_count && _samples[(_head + _count - 1) % CAPACITY].value <= target) --_count;
+        if (_count == CAPACITY) { _head = (_head + 1) % CAPACITY; --_count; }
+        _samples[(_head + _count) % CAPACITY] = {nowUs, target};
+        ++_count;
+        const float excess = fmaxf(0.0f, fabsf(measuredDps) - _samples[_head].value - s.thresholdDps);
+        const float desired = requested ? s.maxReductionPercent * 0.01f * fminf(1.0f, excess / s.rampDps) : 0;
+        const float tau = (desired > _reduction ? s.attackMs : s.recoveryMs) * 0.001f;
+        _reduction += (desired - _reduction) * (1.0f - expf(-dt / tau));
+        if (_reduction < 0.00001f) _reduction = 0;
+        return throttle * (1.0f - _reduction);
+    }
+private:
+    struct Sample { uint32_t timeUs; float value; };
+    static constexpr unsigned CAPACITY = 128;
+    Sample _samples[CAPACITY] = {};
+    unsigned _head = 0, _count = 0;
+    float _reduction = 0;
+};

src/lib/FlightControl/DriftAssistJson.h

@@ -0,0 +1,38 @@
+#pragma once
+#include "DriftAssist.h"
+#include <ArduinoJson.h>
+
+inline void driftSettingsToJson(JsonObject obj, const DriftAssistSettings &s)
+{
+    obj["enabled"] = s.enabled;
+    obj["channel"] = s.channel;
+    JsonArray range = obj["range"].to<JsonArray>();
+    range.add(s.startUs); range.add(s.endUs);
+    obj["threshold_dps"] = s.thresholdDps;
+    obj["ramp_dps"] = s.rampDps;
+    obj["max_reduction_percent"] = s.maxReductionPercent;
+    obj["attack_ms"] = s.attackMs;
+    obj["recovery_ms"] = s.recoveryMs;
+}
+
+inline bool driftSettingsFromJson(JsonVariantConst obj, DriftAssistSettings &s)
+{
+    if (!obj.is<JsonObjectConst>() || !obj["enabled"].is<bool>() ||
+        !obj["channel"].is<int>() || !obj["range"].is<JsonArrayConst>() ||
+        obj["range"].size() != 2 || !obj["range"][0].is<int>() || !obj["range"][1].is<int>() ||
+        !obj["threshold_dps"].is<float>() || !obj["ramp_dps"].is<float>() ||
+        !obj["max_reduction_percent"].is<float>() || !obj["attack_ms"].is<int>() || !obj["recovery_ms"].is<int>()) return false;
+    const int channel = obj["channel"], start = obj["range"][0], end = obj["range"][1];
+    const int attack = obj["attack_ms"], recovery = obj["recovery_ms"];
+    if (channel < 5 || channel > 16 || start < 900 || end > 2100 || start >= end ||
+        attack < 50 || attack > 500 || recovery < 100 || recovery > 1000) return false;
+    DriftAssistSettings next;
+    next.enabled = obj["enabled"]; next.channel = channel;
+    next.startUs = start; next.endUs = end;
+    next.thresholdDps = obj["threshold_dps"]; next.rampDps = obj["ramp_dps"];
+    next.maxReductionPercent = obj["max_reduction_percent"];
+    next.attackMs = attack; next.recoveryMs = recovery;
+    if (!driftSettingsValid(next)) return false;
+    s = next;
+    return true;
+}

src/lib/FlightControl/FlightControl.cpp

@@ -397,6 +397,10 @@ void FlightControlRuntime::resetAttitudeState()
 
 void FlightControlRuntime::resetPidState()
 {
+    _drift.reset();
+    _driftRequested = false;
+    _driftThrottleInput = _driftThrottleOutput = 0;
+    _driftReason = "inactive";
     _rollRatePid.reset();
     _pitchRatePid.reset();
     _yawRatePid.reset();
@@ -667,6 +671,12 @@ bool FlightControlRuntime::loadPidBank(FlightControlPid &rollPid, FlightControlP
 
 void FlightControlRuntime::updateAttitudeOnly(uint32_t nowUs)
 {
+    // Wi-Fi-only operation and missing RC input must not retain assistance
+    // history for the next connected session.
+    _drift.reset();
+    _driftRequested = false;
+    _driftThrottleInput = _driftThrottleOutput = 0;
+    _driftReason = "inactive";
     if (!_sensorsReady)
     {
         _sensorsReady = _sensors.begin();
@@ -741,6 +751,10 @@ void FlightControlRuntime::update(uint32_t nowUs)
         return;
     }
 
+    if (_driftConfigRevision != flightControlConfig.GetRevision()) {
+        _drift.reset();
+        _driftConfigRevision = flightControlConfig.GetRevision();
+    }
     updateArmState();
 
     // Timing is owned by FlightControl_device. A duplicate gate here caused
@@ -779,6 +793,7 @@ void FlightControlRuntime::update(uint32_t nowUs)
     _estimator.update(sample, dt);
     if (!_armed)
     {
+        _driftReason = "disarmed";
         return;
     }
     const FlightControlMode nextMode = readModeSwitch();
@@ -797,6 +812,9 @@ void FlightControlRuntime::update(uint32_t nowUs)
     _rollAngleTarget = rollAngleTarget;
     _pitchAngleTarget = pitchAngleTarget;
     _yawRateTarget = yawRateTarget;
+    _driftThrottleInput = _driftThrottleOutput = throttle;
+    _driftRequested = false;
+    _driftReason = flightControlConfig.GetDriftAssist().enabled ? "mode" : "disabled";
 
     if (_mode == FLIGHT_CONTROL_MODE_MANUAL)
     {
@@ -834,6 +852,27 @@ void FlightControlRuntime::update(uint32_t nowUs)
     const float pitchCorrection = _pitchRatePid.update(pitchRateTarget, gyroPitch, dt) + _rateFeedforwardGain.y * pitchRateTarget;
     const float yawCorrection = _yawRatePid.update(yawRateTarget, gyroYaw, dt) + _rateFeedforwardGain.z * yawRateTarget;
 
+    const auto &drift = flightControlConfig.GetDriftAssist();
+    if (drift.enabled && _mode == FLIGHT_CONTROL_MODE_RATE) {
+        const uint16_t switchRaw = ChannelData[drift.channel - 1];
+        const bool validSwitch = switchRaw >= CRSF_CHANNEL_VALUE_MIN && switchRaw <= CRSF_CHANNEL_VALUE_MAX;
+        const uint16_t switchUs = validSwitch ? CRSF_to_US(switchRaw) : 0;
+        const bool validMixer = flightControlConfig.DriftMixerCompatible();
+        const bool validChannel = flightControlConfig.DriftChannelCompatible();
+        const bool validSensor = sample.gyroValid && isfinite(gyroYaw) && _lastSampleAgeMs <= 20;
+        if (!validMixer) _driftReason = "mixer";
+        else if (!validChannel) _driftReason = "channel_conflict";
+        else if (!validSwitch) _driftReason = "switch_invalid";
+        else if (!validSensor) _driftReason = "sensor";
+        else {
+            _driftRequested = switchUs >= drift.startUs && switchUs < drift.endUs;
+            throttle = _drift.update(throttle, yawRateTarget, gyroYaw, dt, nowUs, _driftRequested, drift);
+            _driftThrottleOutput = throttle;
+            _driftReason = _driftRequested ? (throttle == 0 ? "idle" : "active")
+                : (_drift.reduction() > 0 ? "releasing" : "switch_off");
+        }
+        if (!validSwitch || !validMixer || !validChannel || !validSensor) _drift.reset();
+    } else _drift.reset();
     _mixerOutput = _mixer.mix(throttle, rollCorrection, pitchCorrection, yawCorrection);
 }
 
@@ -862,6 +901,11 @@ bool FlightControlRuntime::getDebugSnapshot(FlightControlDebugSnapshot &snapshot
     snapshot.pitchRateTarget = _pitchRateTarget;
     snapshot.yawRateTarget = _yawRateTarget;
     snapshot.armed = _armed;
+    snapshot.driftReason = _driftReason;
+    snapshot.driftRequested = _driftRequested;
+    snapshot.driftThrottleInput = _driftThrottleInput;
+    snapshot.driftThrottleOutput = _driftThrottleOutput;
+    snapshot.driftReduction = _drift.reduction();
     return _lastDebugUpdateMs != 0;
 }
 

src/lib/FlightControl/FlightControl.h

@@ -138,6 +138,11 @@ class FlightControlRuntime {
     float _rollRateTarget = 0.0f;
     float _pitchRateTarget = 0.0f;
     float _yawRateTarget = 0.0f;
+    DriftAssist _drift;
+    uint32_t _driftConfigRevision = 0;
+    const char *_driftReason = "disabled";
+    float _driftThrottleInput = 0, _driftThrottleOutput = 0;
+    bool _driftRequested = false;
     bool _armed = false;
 };
 

src/lib/FlightControl/FlightControlConfig.cpp

@@ -2,6 +2,7 @@
 
 #if defined(HAS_BASIC_FLIGHT_CONTROL) && defined(TARGET_RX)
 
+#include "DriftAssistJson.h"
 #include <ArduinoJson.h>
 #include <FS.h>
 #if defined(PLATFORM_ESP32)
@@ -17,6 +18,7 @@ FlightControlConfig flightControlConfig;
 
 void FlightControlConfig::SetDefaults()
 {
+    m_drift = DriftAssistSettings{};
     memset(m_ratePid, 0, sizeof(m_ratePid));
     memset(m_rateFeedforward, 0, sizeof(m_rateFeedforward));
     memset(m_anglePid, 0, sizeof(m_anglePid));
@@ -85,6 +87,8 @@ void FlightControlConfig::Load()
         return;
     }
 
+    // Missing or malformed drift settings never enable assistance.
+    driftSettingsFromJson(doc["drift"], m_drift);
     JsonArray ratePid = doc["rate_pid"].as<JsonArray>();
     for (uint8_t i = 0; i < min(ratePid.size(), (size_t)FC_PID_TERM_COUNT); ++i)
     {
@@ -261,6 +265,7 @@ bool FlightControlConfig::Commit()
     }
 
     JsonDocument doc;
+    driftSettingsToJson(doc["drift"].to<JsonObject>(), m_drift);
     copyArray(m_ratePid, doc["rate_pid"].to<JsonArray>());
     copyArray(m_rateFeedforward, doc["rate_ff"].to<JsonArray>());
     copyArray(m_anglePid, doc["angle_pid"].to<JsonArray>());
@@ -332,9 +337,35 @@ bool FlightControlConfig::Commit()
         return false;
     }
     m_modified = false;
+    ++m_revision;
     return true;
 }
 
+void FlightControlConfig::SetDriftAssist(const DriftAssistSettings &settings)
+{
+    if (!driftSettingsValid(settings)) return;
+    const auto &a = m_drift;
+    if (a.enabled == settings.enabled && a.channel == settings.channel &&
+        a.startUs == settings.startUs && a.endUs == settings.endUs &&
+        a.thresholdDps == settings.thresholdDps && a.rampDps == settings.rampDps &&
+        a.maxReductionPercent == settings.maxReductionPercent &&
+        a.attackMs == settings.attackMs && a.recoveryMs == settings.recoveryMs) return;
+    m_drift = settings;
+    m_modified = true;
+}
+
+bool FlightControlConfig::DriftMixerCompatible() const
+{
+    return driftMixerCompatible(m_mixer, m_mixerOutputServo, m_mixerCount);
+}
+
+bool FlightControlConfig::DriftChannelCompatible() const
+{
+    const unsigned channel = m_drift.channel - 1;
+    return !(m_armMode && channel == m_armChannel) &&
+        !(m_wifiCoexistEnabled && channel == m_wifiCoexistChannel);
+}
+
 void FlightControlConfig::SetRatePid(uint8_t index, int16_t value)
 {
     if (index < FC_PID_TERM_COUNT && m_ratePid[index] != value)

src/lib/FlightControl/FlightControlConfig.h

@@ -5,6 +5,7 @@
 #if defined(HAS_BASIC_FLIGHT_CONTROL) && defined(TARGET_RX)
 
 #include <stdint.h>
+#include "DriftAssist.h"
 
 constexpr uint8_t FC_PID_TERM_COUNT = 12;
 constexpr uint8_t FC_RATE_FF_TERM_COUNT = 3;
@@ -79,6 +80,11 @@ class FlightControlConfig
     void Load();
     bool Commit();
     void SetDefaults();
+    const DriftAssistSettings &GetDriftAssist() const { return m_drift; }
+    void SetDriftAssist(const DriftAssistSettings &settings);
+    uint32_t GetRevision() const { return m_revision; }
+    bool DriftMixerCompatible() const;
+    bool DriftChannelCompatible() const;
 
     const int16_t *GetRatePid() const { return m_ratePid; }
     const int16_t *GetRateFeedforward() const { return m_rateFeedforward; }
@@ -175,6 +181,8 @@ class FlightControlConfig
     bool m_armMode = false;
     uint8_t m_armChannel = FC_ARM_CHANNEL_DEFAULT;
     FlightControlChannelRange m_armRange = {};
+    DriftAssistSettings m_drift;
+    uint32_t m_revision = 0;
     bool m_modified = false;
 };