diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000000..92a8cca022 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,82 @@ +# PX4-Autopilot + +## Build + +```bash +make ark_fmu-v6x_default -j$(nproc) # ARK FPV target +make px4_sitl_default -j$(nproc) # SITL +``` + +## Conventions + +- Commits: `type(scope): description` (conventional commits). No `Co-Authored-By`. +- PR titles: same conventional commit format. No Claude attribution. +- Push to `jake` remote for fork PRs, `origin` for upstream PRs. + +## Active branch: `dakejahl/baro-thrust-estimator` + +PR: https://github.com/dakejahl/PX4-Autopilot/pull/41 + +### What it does + +Propwash-induced barometer error compensation. Propellers create static pressure changes at the baro sensor proportional to motor output (~7m error, r=0.91 on ARK FPV). Correction: `baro_alt += SENS_BARO_PCOEF * mean_motor_output`. + +### Components + +**Thrust compensation** (`src/modules/sensors/vehicle_air_data/VehicleAirData.cpp`) +- Motor ring buffer (8 samples) with time-matched lookup against baro `timestamp_sample` +- Param: `SENS_BARO_PCOEF` (m per unit motor output, identified as ~-20 to -22 on test vehicle) + +**BMP388 timestamp fix** (`src/drivers/barometer/bmp388/bmp388.cpp`) +- `timestamp_sample` corrected from read time to integration midpoint (`- measurement_time/2`) +- Eliminated 38ms baro-thrust lag. Other baro drivers not yet corrected. + +**Online estimator** (`src/modules/baro_thrust_estimator/`) +- CF (complementary filter) isolates thrust-correlated baro error at 0.05Hz crossover +- RLS (recursive least squares) identifies gain K online, saves to `SENS_BARO_PCOEF` on disarm +- Controlled by `SENS_BAR_AUTOCAL` bit 1 + +**Rate limiting refactor** +- Removed `SENS_BARO_RATE`/`SENS_MAG_RATE` from publishers (had aliasing bug) +- Added `EKF2_BARO_RATE`/`EKF2_MAG_RATE` in EKF2.cpp +- Param translations in `src/lib/parameters/param_translation.cpp` + +**Analysis tools** (`Tools/baro_compensation/`) +- `baro_thrust_calibration.py ` — offline calibration/validation, PDF output +- `motor_latency_analysis.py ` — command-to-RPM delay analysis + +### Parameters + +| Parameter | Default | Description | +|-----------|---------|-------------| +| `SENS_BARO_PCOEF` | 0.0 | Baro alt correction per unit motor output [m] | +| `SENS_BAR_AUTOCAL` | 3 | Bitmask: bit 0 = GNSS cal, bit 1 = thrust comp | +| `EKF2_BARO_RATE` | 20.0 | Max baro fusion rate in EKF [Hz] | +| `SENS_BAR_CF_BW` | 0.05 | CF crossover frequency for thrust estimator [Hz] | +| `EKF2_MAG_RATE` | 15.0 | Max mag fusion rate in EKF [Hz] | + +### Test vehicle + +ARK FPV: BMP390 baro (23Hz, 16x oversampling, 43ms forced-mode), 1404 4000KV motors, 3" triblade props, AM32 ESCs with BDShot, ARK Flow (range + optical flow) for validation. + +### Key findings + +- K ≈ 20-22m consistent across flights, online/offline agree within 0.5m +- Compensation: thrust r from 0.91 to 0.16, error std from 2.3m to 0.95m +- BMP390 integration window (37ms) smooths over motor response (~15ms), no delay param needed +- AM32 ESC RPM telemetry has ~6ms filtering latency (6-sample moving avg + IIR) + +### Motor delay analysis (ARK FPV: 1404 4000KV + 3" triblade + AM32) + +Measured 25ms command→RPM cross-correlation lag. Breakdown: +- ~15ms true electromechanical response (ESC commutation + prop inertia) +- ~6ms AM32 RPM filtering (6-sample commutation interval moving avg + IIR with alpha=0.25, see ~/code/ark/AM32/Src/main.c:1901,1626) +- ~4ms BDShot telemetry round-robin (4 motors at 800Hz output rate = 200Hz per channel) + +The motor delay is invisible to baro-thrust compensation because the BMP390's 37ms integration window low-passes over it. A `SENS_BARO_PTAU` delay param was prototyped and removed for this reason. On faster baro sensors (lower oversampling, shorter integration) the motor delay could resurface. + +### Next steps + +- Install ARK Flow for position hold +- Test on other vehicles/baro sensors +- Correct `timestamp_sample` in other baro drivers (MS5611, DPS310, BMP280) diff --git a/Tools/baro_compensation/README.md b/Tools/baro_compensation/README.md index fcee86e054..f4dc1375d9 100644 --- a/Tools/baro_compensation/README.md +++ b/Tools/baro_compensation/README.md @@ -152,7 +152,7 @@ If not using the online estimator, you can calibrate manually: ## CF Bandwidth Tuning -The default `SENS_BAR_CF_BW` (0.1 Hz) works well for most vehicles. Only +The default `SENS_BAR_CF_BW` (0.05 Hz) works well for most vehicles. Only adjust it if the online estimator consistently fails to converge or produces a K that disagrees with range-sensor ground truth. @@ -189,4 +189,4 @@ to IMU vibration but slower to converge. |-----------|-------------|-------|---------| | `SENS_BARO_PCOEF` | Baro altitude correction per unit vertical thrust [m] | -30 to 30 | 0.0 | | `SENS_BAR_AUTOCAL` | Bitmask: bit 0 = GNSS offset, bit 1 = online thrust cal | 0 to 3 | 1 | -| `SENS_BAR_CF_BW` | CF crossover frequency for the online estimator [Hz] | 0.01 to 1.0 | 0.1 | +| `SENS_BAR_CF_BW` | CF crossover frequency for the online estimator [Hz] | 0.01 to 1.0 | 0.05 | diff --git a/Tools/baro_compensation/baro_thrust_calibration.py b/Tools/baro_compensation/baro_thrust_calibration.py index 0c2dcf80ca..7a3d6aa188 100755 --- a/Tools/baro_compensation/baro_thrust_calibration.py +++ b/Tools/baro_compensation/baro_thrust_calibration.py @@ -227,7 +227,7 @@ class CfRls: """Python port of BaroThrustCfRls. Constants match baro_thrust_cf_rls.hpp.""" # Defaults (matching baro_thrust_cf_rls.hpp) - DEFAULT_CF_BANDWIDTH = 0.1 + DEFAULT_CF_BANDWIDTH = 0.05 DEFAULT_RLS_LAMBDA = 0.998 RLS_P_INIT = 100.0 ERROR_VAR_INIT = 10.0 diff --git a/docs/en/advanced_config/barometer_thrust_compensation.md b/docs/en/advanced_config/barometer_thrust_compensation.md index c914a23376..1ece756e16 100644 --- a/docs/en/advanced_config/barometer_thrust_compensation.md +++ b/docs/en/advanced_config/barometer_thrust_compensation.md @@ -25,7 +25,7 @@ The online estimator identifies `SENS_BARO_PCOEF` automatically during flight an ### How It Works -A complementary filter (CF) fuses barometer altitude with double-integrated accelerometer data at a very low crossover frequency (default 0.1 Hz). +A complementary filter (CF) fuses barometer altitude with double-integrated accelerometer data at a very low crossover frequency (default 0.05 Hz). The CF trusts the accelerometer for fast altitude changes and the barometer for slow drift, so the CF residual (baro minus accel prediction) isolates thrust-correlated pressure error while rejecting real altitude changes. A Recursive Least Squares (RLS) estimator then fits the linear model `residual = K * thrust + bias` to identify the gain K. diff --git a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp index f65f069360..ed4447bf7b 100644 --- a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp +++ b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp @@ -101,7 +101,7 @@ public: static constexpr float K_STABILITY_DIFF_THR = 0.5f; ///< max |K_smoothed - K_raw| for stability [m] // --- Complementary filter --- - static constexpr float CF_BANDWIDTH_HZ_DEFAULT = 0.1f; ///< default crossover frequency [Hz] + static constexpr float CF_BANDWIDTH_HZ_DEFAULT = 0.05f; ///< default crossover frequency [Hz] static constexpr float ERROR_VAR_INIT = 10.f; ///< initial prediction error variance /** diff --git a/src/modules/baro_thrust_estimator/params.yaml b/src/modules/baro_thrust_estimator/params.yaml index 2a09f7e860..e91135a94d 100644 --- a/src/modules/baro_thrust_estimator/params.yaml +++ b/src/modules/baro_thrust_estimator/params.yaml @@ -12,7 +12,7 @@ parameters: higher values let more baro error through to the RLS estimator (more accurate K identification but noisier with bad IMU). type: float - default: 0.1 + default: 0.05 min: 0.01 max: 1.0 unit: Hz