Skip to content

Commit 7c460fc

Browse files
committed
feat(sensors): barometer thrust compensation with online estimator
Add propwash-induced barometer error compensation. Propellers create static pressure changes at the baro sensor proportional to motor output. Correction: baro_alt += SENS_BARO_PCOEF * mean_motor_output. Thrust compensation in VehicleAirData uses a motor ring buffer (8 samples) with time-matched lookup against baro timestamp_sample. BMP388 timestamp_sample corrected from read time to integration midpoint (- measurement_time/2), eliminating 38ms baro-thrust lag. Online estimator module (baro_thrust_estimator) uses a complementary filter to isolate thrust-correlated baro error at 0.05Hz crossover, and recursive least squares to identify gain K online. Saves to SENS_BARO_PCOEF on disarm. Controlled by SENS_BAR_AUTOCAL bit 1. SENS_BAR_AUTOCAL migrated from boolean to bitmask (bit 0: GNSS cal, bit 1: thrust comp). Includes offline calibration/validation tool (Tools/baro_compensation/baro_thrust_calibration.py).
1 parent d494a6c commit 7c460fc

File tree

17 files changed

+3131
-5
lines changed

17 files changed

+3131
-5
lines changed

ROMFS/px4fmu_common/init.d/rc.mc_apps

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,13 @@ fi
2929
# Start Multicopter Position Controller.
3030
#
3131
mc_hover_thrust_estimator start
32+
33+
# Bit 1 of SENS_BAR_AUTOCAL enables online thrust compensation (value > 1)
34+
if param greater -s SENS_BAR_AUTOCAL 1
35+
then
36+
baro_thrust_estimator start
37+
fi
38+
3239
flight_mode_manager start
3340
mc_pos_control start
3441

Tools/baro_compensation/README.md

Lines changed: 153 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,153 @@
1+
# Barometer Thrust Compensation Analysis
2+
3+
Post-flight analysis tool for barometer thrust compensation. Works with the
4+
online `baro_thrust_estimator` module and/or a range sensor for ground truth.
5+
6+
## Background
7+
8+
Propwash from propellers changes the static pressure at the barometer sensor.
9+
This creates a thrust-dependent altitude error: as thrust increases, the baro
10+
reading shifts. The direction and magnitude depend on sensor placement relative
11+
to the propellers.
12+
13+
The `vehicle_air_data` module compensates for this by applying a correction to
14+
the barometer altitude before publishing:
15+
16+
```
17+
corrected_baro_alt = raw_baro_alt + SENS_BARO_PCOEF * abs(thrust_z)
18+
```
19+
20+
where `thrust_z` is the Z body-axis component (`vehicle_thrust_setpoint.xyz[2]`),
21+
which is signed in PX4 (negative for upward thrust in FRD). The correction uses
22+
its magnitude `|thrust_z|` in [0, 1]. Using the vertical thrust setpoint (rather
23+
than individual motor outputs) provides correct behavior for both multicopter and
24+
VTOL aircraft.
25+
26+
## How Calibration Works
27+
28+
### Online (Preferred)
29+
30+
The online estimator is disabled by default. Enable it by setting
31+
`SENS_BAR_AUTOCAL` bit 1 (e.g. set to 3 for both GNSS cal and thrust comp).
32+
The `baro_thrust_estimator` module identifies `SENS_BARO_PCOEF` automatically
33+
during flight using an accel-baro complementary filter and RLS estimation.
34+
Parameters are saved to flash on disarm once converged, and the estimator
35+
continues to refine PCOEF over subsequent flights. No range sensor required.
36+
37+
### Offline (This Tool)
38+
39+
This tool identifies PCOEF from a flight log using a range sensor as ground
40+
truth. Useful for:
41+
- Validating online estimator results against ground truth
42+
- Analyzing compensation effectiveness
43+
- Initial calibration when the online estimator hasn't converged yet
44+
45+
## Prerequisites
46+
47+
- **Python packages**: `pyulog`, `numpy`, `matplotlib`
48+
49+
```bash
50+
pip install pyulog numpy matplotlib
51+
```
52+
53+
## Analysis Modes
54+
55+
The tool automatically selects a mode based on what data is in the log:
56+
57+
### 1. Estimator Review (online estimator logged, no range sensor)
58+
59+
Shows what the online estimator did during the flight:
60+
- K estimate convergence with uncertainty band
61+
- Prediction error and thrust excitation
62+
- Convergence status timeline
63+
- Compensation effect (residual before/after correction)
64+
65+
### 2. Full Validation (online estimator + range sensor)
66+
67+
Cross-validates the online estimator against range-sensor ground truth:
68+
- All estimator review plots (above)
69+
- Side-by-side comparison of online vs offline compensation
70+
- Scatter plots comparing raw, online-corrected, and offline-corrected error
71+
72+
### 3. Standalone Calibration (range sensor, no estimator)
73+
74+
Identifies PCOEF from baro vs range error:
75+
- Cross-correlation delay analysis
76+
- Recommended PCOEF value
77+
78+
## Usage
79+
80+
```bash
81+
python3 baro_thrust_calibration.py <path/to/log.ulg> [--output-dir <dir>]
82+
```
83+
84+
## Output
85+
86+
### Console
87+
88+
Summary including parameters found in the log, online estimator convergence
89+
status, range-based error statistics, and cross-validation between online and
90+
offline identification (when both are available).
91+
92+
### PDF Report (`<log_name>.pdf`)
93+
94+
Pages vary by mode. When the online estimator is present:
95+
96+
**Altitude Overview** — Baro observation, EKF altitude, distance sensor (if
97+
available), and thrust over time.
98+
99+
**Estimator Convergence** — K estimate with variance band, error variance
100+
and thrust excitation, convergence flags over time.
101+
102+
**Compensation Effect** — CF residual before/after applying estimated K,
103+
scatter plots, and compensation statistics.
104+
105+
When a range sensor is present, additional pages show:
106+
107+
**Compensation Comparison** — Side-by-side baro/range altitude with online
108+
vs offline correction applied.
109+
110+
**Online vs Offline Scatter** — Error vs thrust scatter for raw, online-
111+
corrected, and offline-corrected data.
112+
113+
## Manual Calibration Procedure
114+
115+
If not using the online estimator, you can calibrate manually:
116+
117+
1. **Disable existing compensation**:
118+
```
119+
param set SENS_BARO_PCOEF 0.0
120+
```
121+
122+
2. **Fly a hover** at 2-5 m AGL for at least 60 seconds with some gentle
123+
altitude changes. A range sensor must be installed.
124+
125+
3. **Run this tool** on the log and apply the recommended parameter:
126+
```
127+
param set SENS_BARO_PCOEF <value>
128+
```
129+
130+
4. **Fly again** and re-run the tool to verify (it will auto-detect validation
131+
mode when compensation parameters are active).
132+
133+
## Interpreting Results
134+
135+
| Metric | Good | Marginal | Poor |
136+
|--------|------|----------|------|
137+
| Thrust correlation abs(r) | > 0.6 | 0.3 - 0.6 | < 0.3 |
138+
| Model R^2 | > 0.3 | 0.1 - 0.3 | < 0.1 |
139+
| Compensated abs(r) | < 0.2 | 0.2 - 0.4 | > 0.4 |
140+
141+
- **Low R^2**: Thrust is not the dominant baro error source. Consider thermal
142+
drift, ground effect, or sensor placement issues.
143+
- **Very large K (> 5 m)**: May indicate a sensor mounting issue. The baro
144+
should be shielded from direct propwash where possible.
145+
- **Online/offline K disagreement > 2 m**: The estimator may not have had
146+
enough excitation. Fly longer or with more altitude variation.
147+
148+
## Parameters Reference
149+
150+
| Parameter | Description | Range | Default |
151+
|-----------|-------------|-------|---------|
152+
| `SENS_BARO_PCOEF` | Baro altitude correction per unit vertical thrust [m] | -30 to 30 | 0.0 |
153+
| `SENS_BAR_AUTOCAL` | Bitmask: bit 0 = GNSS offset, bit 1 = online thrust cal | 0 to 3 | 1 |

0 commit comments

Comments
 (0)