mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-06 15:48:53 +08:00
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).
This commit is contained in:
Jacob Dahl
@@ -29,6 +29,13 @@ fi
|
||||
# Start Multicopter Position Controller.
|
||||
#
|
||||
mc_hover_thrust_estimator start
|
||||
|
||||
# Bit 1 of SENS_BAR_AUTOCAL enables online thrust compensation (value > 1)
|
||||
if param greater -s SENS_BAR_AUTOCAL 1
|
||||
then
|
||||
baro_thrust_estimator start
|
||||
fi
|
||||
|
||||
flight_mode_manager start
|
||||
mc_pos_control start
|
||||
|
||||
|
||||
@@ -0,0 +1,153 @@
|
||||
# Barometer Thrust Compensation Analysis
|
||||
|
||||
Post-flight analysis tool for barometer thrust compensation. Works with the
|
||||
online `baro_thrust_estimator` module and/or a range sensor for ground truth.
|
||||
|
||||
## Background
|
||||
|
||||
Propwash from propellers changes the static pressure at the barometer sensor.
|
||||
This creates a thrust-dependent altitude error: as thrust increases, the baro
|
||||
reading shifts. The direction and magnitude depend on sensor placement relative
|
||||
to the propellers.
|
||||
|
||||
The `vehicle_air_data` module compensates for this by applying a correction to
|
||||
the barometer altitude before publishing:
|
||||
|
||||
```
|
||||
corrected_baro_alt = raw_baro_alt + SENS_BARO_PCOEF * abs(thrust_z)
|
||||
```
|
||||
|
||||
where `thrust_z` is the Z body-axis component (`vehicle_thrust_setpoint.xyz[2]`),
|
||||
which is signed in PX4 (negative for upward thrust in FRD). The correction uses
|
||||
its magnitude `|thrust_z|` in [0, 1]. Using the vertical thrust setpoint (rather
|
||||
than individual motor outputs) provides correct behavior for both multicopter and
|
||||
VTOL aircraft.
|
||||
|
||||
## How Calibration Works
|
||||
|
||||
### Online (Preferred)
|
||||
|
||||
The online estimator is disabled by default. Enable it by setting
|
||||
`SENS_BAR_AUTOCAL` bit 1 (e.g. set to 3 for both GNSS cal and thrust comp).
|
||||
The `baro_thrust_estimator` module identifies `SENS_BARO_PCOEF` automatically
|
||||
during flight using an accel-baro complementary filter and RLS estimation.
|
||||
Parameters are saved to flash on disarm once converged, and the estimator
|
||||
continues to refine PCOEF over subsequent flights. No range sensor required.
|
||||
|
||||
### Offline (This Tool)
|
||||
|
||||
This tool identifies PCOEF from a flight log using a range sensor as ground
|
||||
truth. Useful for:
|
||||
- Validating online estimator results against ground truth
|
||||
- Analyzing compensation effectiveness
|
||||
- Initial calibration when the online estimator hasn't converged yet
|
||||
|
||||
## Prerequisites
|
||||
|
||||
- **Python packages**: `pyulog`, `numpy`, `matplotlib`
|
||||
|
||||
```bash
|
||||
pip install pyulog numpy matplotlib
|
||||
```
|
||||
|
||||
## Analysis Modes
|
||||
|
||||
The tool automatically selects a mode based on what data is in the log:
|
||||
|
||||
### 1. Estimator Review (online estimator logged, no range sensor)
|
||||
|
||||
Shows what the online estimator did during the flight:
|
||||
- K estimate convergence with uncertainty band
|
||||
- Prediction error and thrust excitation
|
||||
- Convergence status timeline
|
||||
- Compensation effect (residual before/after correction)
|
||||
|
||||
### 2. Full Validation (online estimator + range sensor)
|
||||
|
||||
Cross-validates the online estimator against range-sensor ground truth:
|
||||
- All estimator review plots (above)
|
||||
- Side-by-side comparison of online vs offline compensation
|
||||
- Scatter plots comparing raw, online-corrected, and offline-corrected error
|
||||
|
||||
### 3. Standalone Calibration (range sensor, no estimator)
|
||||
|
||||
Identifies PCOEF from baro vs range error:
|
||||
- Cross-correlation delay analysis
|
||||
- Recommended PCOEF value
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
python3 baro_thrust_calibration.py <path/to/log.ulg> [--output-dir <dir>]
|
||||
```
|
||||
|
||||
## Output
|
||||
|
||||
### Console
|
||||
|
||||
Summary including parameters found in the log, online estimator convergence
|
||||
status, range-based error statistics, and cross-validation between online and
|
||||
offline identification (when both are available).
|
||||
|
||||
### PDF Report (`<log_name>.pdf`)
|
||||
|
||||
Pages vary by mode. When the online estimator is present:
|
||||
|
||||
**Altitude Overview** — Baro observation, EKF altitude, distance sensor (if
|
||||
available), and thrust over time.
|
||||
|
||||
**Estimator Convergence** — K estimate with variance band, error variance
|
||||
and thrust excitation, convergence flags over time.
|
||||
|
||||
**Compensation Effect** — CF residual before/after applying estimated K,
|
||||
scatter plots, and compensation statistics.
|
||||
|
||||
When a range sensor is present, additional pages show:
|
||||
|
||||
**Compensation Comparison** — Side-by-side baro/range altitude with online
|
||||
vs offline correction applied.
|
||||
|
||||
**Online vs Offline Scatter** — Error vs thrust scatter for raw, online-
|
||||
corrected, and offline-corrected data.
|
||||
|
||||
## Manual Calibration Procedure
|
||||
|
||||
If not using the online estimator, you can calibrate manually:
|
||||
|
||||
1. **Disable existing compensation**:
|
||||
```
|
||||
param set SENS_BARO_PCOEF 0.0
|
||||
```
|
||||
|
||||
2. **Fly a hover** at 2-5 m AGL for at least 60 seconds with some gentle
|
||||
altitude changes. A range sensor must be installed.
|
||||
|
||||
3. **Run this tool** on the log and apply the recommended parameter:
|
||||
```
|
||||
param set SENS_BARO_PCOEF <value>
|
||||
```
|
||||
|
||||
4. **Fly again** and re-run the tool to verify (it will auto-detect validation
|
||||
mode when compensation parameters are active).
|
||||
|
||||
## Interpreting Results
|
||||
|
||||
| Metric | Good | Marginal | Poor |
|
||||
|--------|------|----------|------|
|
||||
| Thrust correlation abs(r) | > 0.6 | 0.3 - 0.6 | < 0.3 |
|
||||
| Model R^2 | > 0.3 | 0.1 - 0.3 | < 0.1 |
|
||||
| Compensated abs(r) | < 0.2 | 0.2 - 0.4 | > 0.4 |
|
||||
|
||||
- **Low R^2**: Thrust is not the dominant baro error source. Consider thermal
|
||||
drift, ground effect, or sensor placement issues.
|
||||
- **Very large K (> 5 m)**: May indicate a sensor mounting issue. The baro
|
||||
should be shielded from direct propwash where possible.
|
||||
- **Online/offline K disagreement > 2 m**: The estimator may not have had
|
||||
enough excitation. Fly longer or with more altitude variation.
|
||||
|
||||
## Parameters Reference
|
||||
|
||||
| Parameter | Description | Range | Default |
|
||||
|-----------|-------------|-------|---------|
|
||||
| `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 |
|
||||
+1136
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,11 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # time of baro data last used for this estimate
|
||||
|
||||
float32 residual # CF residual: baro minus accel-predicted altitude [m]
|
||||
float32 k_estimate # estimated thrust-to-baro gain [m/unit_thrust]
|
||||
float32 k_estimate_var # variance of K estimate (P[0][0])
|
||||
float32 error_var # RLS prediction error variance
|
||||
float32 thrust_std # standard deviation of recent thrust excitation
|
||||
|
||||
bool converged # true when all convergence criteria are met
|
||||
bool estimation_active # true when RLS is updating (soft guards not active)
|
||||
@@ -47,6 +47,7 @@ set(msg_files
|
||||
Airspeed.msg
|
||||
AirspeedWind.msg
|
||||
AutotuneAttitudeControlStatus.msg
|
||||
BaroThrustEstimate.msg
|
||||
BatteryInfo.msg
|
||||
ButtonEvent.msg
|
||||
CameraCapture.msg
|
||||
|
||||
@@ -259,5 +259,15 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
|
||||
}
|
||||
}
|
||||
|
||||
// 2026-03-31: SENS_BAR_AUTOCAL boolean -> bitmask (bit 0: GNSS cal, bit 1: thrust comp)
|
||||
{
|
||||
if (strcmp("SENS_BAR_AUTOCAL", node->name) == 0 && node->type == bson_type_t::BSON_BOOL) {
|
||||
node->i32 = node->b ? 1 : 0;
|
||||
node->type = bson_type_t::BSON_INT32;
|
||||
PX4_INFO("migrating %s from bool to bitmask", node->name);
|
||||
return param_modify_on_import_ret::PARAM_MODIFIED;
|
||||
}
|
||||
}
|
||||
|
||||
return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,388 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file BaroThrustEstimator.cpp
|
||||
*
|
||||
* Online estimator for barometer thrust compensation parameters.
|
||||
*
|
||||
* Uses a complementary filter (accel-integrated altitude vs baro) to isolate
|
||||
* thrust-induced baro pressure errors. The CF residual (baro - accel_prediction)
|
||||
* captures propwash effects while rejecting real altitude changes and accel drift.
|
||||
* An RLS estimator identifies the gain K using the commanded collective thrust
|
||||
* magnitude from vehicle_thrust_setpoint (|xyz[2]|). Converged parameters are
|
||||
* saved on disarm.
|
||||
*/
|
||||
|
||||
#include "BaroThrustEstimator.hpp"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor BaroThrustEstimator::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
BaroThrustEstimator::BaroThrustEstimator() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
|
||||
{
|
||||
_estimator.reset();
|
||||
updateParams();
|
||||
}
|
||||
|
||||
BaroThrustEstimator::~BaroThrustEstimator()
|
||||
{
|
||||
perf_free(_cycle_perf);
|
||||
}
|
||||
|
||||
bool BaroThrustEstimator::init()
|
||||
{
|
||||
if (!_vehicle_air_data_sub.registerCallback()) {
|
||||
PX4_ERR("callback registration failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void BaroThrustEstimator::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
_estimator.setCfBandwidth(_param_sens_bar_cf_bw.get());
|
||||
}
|
||||
|
||||
void BaroThrustEstimator::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_vehicle_air_data_sub.unregisterCallback();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
// Handle parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
updateParams();
|
||||
}
|
||||
|
||||
// Track arm state for disarm edge detection
|
||||
if (_vehicle_status_sub.updated()) {
|
||||
vehicle_status_s status;
|
||||
|
||||
if (_vehicle_status_sub.copy(&status)) {
|
||||
const bool was_armed = _armed;
|
||||
_armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
|
||||
if (was_armed != _armed) {
|
||||
if (was_armed && !_armed) {
|
||||
saveParameters();
|
||||
}
|
||||
|
||||
_estimator.reset();
|
||||
_estimation_start_time = 0;
|
||||
_last_update_time = 0;
|
||||
_last_publish_time = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Track landed state
|
||||
if (_vehicle_land_detected_sub.updated()) {
|
||||
vehicle_land_detected_s ld;
|
||||
|
||||
if (_vehicle_land_detected_sub.copy(&ld)) {
|
||||
_landed = ld.landed;
|
||||
}
|
||||
}
|
||||
|
||||
// Get baro data (callback trigger)
|
||||
vehicle_air_data_s air_data{};
|
||||
|
||||
if (!_vehicle_air_data_sub.copy(&air_data)) {
|
||||
perf_end(_cycle_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// Hard guards: no estimation or CF when disarmed/landed
|
||||
if (!_armed || _landed) {
|
||||
perf_end(_cycle_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// Compute dt
|
||||
const hrt_abstime now = air_data.timestamp_sample;
|
||||
|
||||
if (_last_update_time == 0) {
|
||||
_last_update_time = now;
|
||||
_estimation_start_time = now;
|
||||
perf_end(_cycle_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
const float dt = math::constrain(static_cast<float>(now - _last_update_time) * 1e-6f, 0.001f, 0.5f);
|
||||
_last_update_time = now;
|
||||
|
||||
// Validate baro
|
||||
if (!PX4_ISFINITE(air_data.baro_alt_meter)) {
|
||||
perf_end(_cycle_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// Get acceleration in body frame
|
||||
vehicle_acceleration_s accel{};
|
||||
|
||||
if (!_vehicle_acceleration_sub.copy(&accel)
|
||||
|| hrt_elapsed_time(&accel.timestamp) > 100_ms
|
||||
|| !PX4_ISFINITE(accel.xyz[0]) || !PX4_ISFINITE(accel.xyz[1]) || !PX4_ISFINITE(accel.xyz[2])) {
|
||||
perf_end(_cycle_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// Get attitude quaternion
|
||||
vehicle_attitude_s att{};
|
||||
|
||||
if (!_vehicle_attitude_sub.copy(&att)
|
||||
|| hrt_elapsed_time(&att.timestamp) > 100_ms
|
||||
|| !PX4_ISFINITE(att.q[0])) {
|
||||
perf_end(_cycle_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// Compute upward linear acceleration and CF residual
|
||||
const float accel_up = BaroThrustCfRls::computeAccelUp(
|
||||
matrix::Vector3f{accel.xyz}, matrix::Quatf{att.q});
|
||||
|
||||
const float residual = _estimator.updateCf(air_data.baro_alt_meter, accel_up, dt);
|
||||
|
||||
if (!PX4_ISFINITE(residual)) {
|
||||
perf_end(_cycle_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// Soft guards: skip RLS update but keep CF current
|
||||
bool estimation_active = true;
|
||||
|
||||
vehicle_local_position_s local_pos{};
|
||||
|
||||
if (_vehicle_local_position_sub.copy(&local_pos)) {
|
||||
if (local_pos.v_z_valid && fabsf(local_pos.vz) > MAX_VZ) {
|
||||
estimation_active = false;
|
||||
}
|
||||
|
||||
if (local_pos.v_xy_valid
|
||||
&& (local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy) > MAX_VXY * MAX_VXY) {
|
||||
estimation_active = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Get vertical thrust setpoint — uses latest sample rather than time-matched
|
||||
// (unlike thrustCompensation). Fine for a statistical estimator; timing jitter is noise.
|
||||
float thrust = 0.f;
|
||||
|
||||
vehicle_thrust_setpoint_s thrust_sp{};
|
||||
|
||||
if (_vehicle_thrust_setpoint_sub.copy(&thrust_sp)
|
||||
&& hrt_elapsed_time(&thrust_sp.timestamp) < 500_ms
|
||||
&& PX4_ISFINITE(thrust_sp.xyz[2])) {
|
||||
thrust = fabsf(thrust_sp.xyz[2]);
|
||||
|
||||
} else {
|
||||
estimation_active = false;
|
||||
}
|
||||
|
||||
// Once converged, freeze RLS — the estimate is good and continued
|
||||
// updates during descent/landing would corrupt it with ground effect.
|
||||
// Note: when updateEstimator() stops being called, all internal
|
||||
// tracking state (thrust variance, K smoothed, RLS covariance) freezes,
|
||||
// so the convergence criteria checked by checkConvergence() remain
|
||||
// stable and converged() cannot flicker back to false.
|
||||
if (estimation_active && !_estimator.converged() && !_estimator.convergedLocked()) {
|
||||
_estimator.updateEstimator(residual, thrust, dt);
|
||||
}
|
||||
|
||||
// Convergence checking runs independently of soft guards so the hold
|
||||
// timer keeps ticking during descent. With RLS frozen the checked
|
||||
// values (variance, error, stability) remain stable.
|
||||
if (!_estimator.convergedLocked()) {
|
||||
const float elapsed_s = static_cast<float>(now - _estimation_start_time) * 1e-6f;
|
||||
_estimator.checkConvergence(elapsed_s, dt);
|
||||
|
||||
if (_estimator.convergedLocked()) {
|
||||
PX4_INFO("convergence locked (K=%.2f)", (double)_estimator.kEstimate());
|
||||
}
|
||||
}
|
||||
|
||||
if (now - _last_publish_time > 200_ms) {
|
||||
publishStatus(now, residual, estimation_active);
|
||||
_last_publish_time = now;
|
||||
}
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
void BaroThrustEstimator::saveParameters()
|
||||
{
|
||||
if (!_estimator.convergedLocked()) {
|
||||
PX4_INFO("baro thrust estimator did not converge (K=%.2f, var=%.1f, err=%.2f, thr_std=%.3f)",
|
||||
(double)_estimator.kEstimate(),
|
||||
(double)_estimator.kEstimateVar(),
|
||||
(double)_estimator.errorVar(),
|
||||
(double)_estimator.thrustStd());
|
||||
return;
|
||||
}
|
||||
|
||||
const float K_est = _estimator.kEstimate();
|
||||
|
||||
// Only update if the remaining error is significant
|
||||
if (fabsf(K_est) < MIN_K_UPDATE_THRESHOLD) {
|
||||
PX4_INFO("K_est=%.2f below threshold, calibration adequate", (double)K_est);
|
||||
return;
|
||||
}
|
||||
|
||||
// K_est is the residual gain the estimator sees *after* existing compensation,
|
||||
// so the corrected PCOEF shifts by -K_est to cancel it out.
|
||||
const float pcoef_new = _param_sens_baro_pcoef.get() - K_est;
|
||||
|
||||
if (!PX4_ISFINITE(pcoef_new)) {
|
||||
PX4_WARN("non-finite result, skipping save");
|
||||
return;
|
||||
}
|
||||
|
||||
if (fabsf(pcoef_new) > PCOEF_MAX) {
|
||||
PX4_WARN("result out of range (pcoef=%.1f)", (double)pcoef_new);
|
||||
return;
|
||||
}
|
||||
|
||||
PX4_INFO("saving SENS_BARO_PCOEF=%.1f (K_est=%.2f, prev_pcoef=%.1f)",
|
||||
(double)pcoef_new, (double)K_est,
|
||||
(double)_param_sens_baro_pcoef.get());
|
||||
|
||||
_param_sens_baro_pcoef.set(pcoef_new);
|
||||
_param_sens_baro_pcoef.commit_no_notification();
|
||||
}
|
||||
|
||||
void BaroThrustEstimator::publishStatus(hrt_abstime now, float residual, bool estimation_active)
|
||||
{
|
||||
baro_thrust_estimate_s status{};
|
||||
status.timestamp_sample = now;
|
||||
status.residual = residual;
|
||||
status.k_estimate = _estimator.kEstimate();
|
||||
status.k_estimate_var = _estimator.kEstimateVar();
|
||||
status.error_var = _estimator.errorVar();
|
||||
status.thrust_std = _estimator.thrustStd();
|
||||
status.converged = _estimator.converged();
|
||||
status.estimation_active = estimation_active && !_estimator.converged();
|
||||
status.timestamp = hrt_absolute_time();
|
||||
|
||||
_baro_thrust_estimate_pub.publish(status);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Module boilerplate
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
int BaroThrustEstimator::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
BaroThrustEstimator *instance = new BaroThrustEstimator();
|
||||
|
||||
if (instance) {
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int BaroThrustEstimator::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int BaroThrustEstimator::print_status()
|
||||
{
|
||||
PX4_INFO("converged: %s", _estimator.converged() ? "yes" : "no");
|
||||
PX4_INFO("K_est: %.3f K_var: %.4f error_var: %.4f",
|
||||
(double)_estimator.kEstimate(), (double)_estimator.kEstimateVar(),
|
||||
(double)_estimator.errorVar());
|
||||
PX4_INFO("thrust std: %.4f", (double)_estimator.thrustStd());
|
||||
PX4_INFO("current SENS_BARO_PCOEF: %.1f", (double)_param_sens_baro_pcoef.get());
|
||||
|
||||
perf_print_counter(_cycle_perf);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int BaroThrustEstimator::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
|
||||
Online estimator for barometer thrust compensation (SENS_BARO_PCOEF).
|
||||
|
||||
Uses an accel-baro complementary filter to isolate thrust-induced baro pressure errors.
|
||||
The CF residual captures propwash effects while rejecting real altitude changes and
|
||||
accelerometer drift. An RLS estimator identifies the thrust-to-baro gain K using the
|
||||
commanded collective thrust magnitude from vehicle_thrust_setpoint (|xyz[2]|).
|
||||
Converged parameters are saved on disarm and refined over subsequent flights.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("baro_thrust_estimator", "estimator");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int baro_thrust_estimator_main(int argc, char *argv[])
|
||||
{
|
||||
return ModuleBase::main(BaroThrustEstimator::desc, argc, argv);
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file BaroThrustEstimator.hpp
|
||||
*
|
||||
* Online estimator for barometer thrust compensation parameters.
|
||||
* Module wrapper around BaroThrustCfRls — handles uORB I/O,
|
||||
* parameter persistence, and lifecycle.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "baro_thrust_cf_rls.hpp"
|
||||
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/baro_thrust_estimate.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_acceleration.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class BaroThrustEstimator : public ModuleBase, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
BaroThrustEstimator();
|
||||
~BaroThrustEstimator() override;
|
||||
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
int print_status() override;
|
||||
|
||||
private:
|
||||
static constexpr float MAX_VZ = 2.f;
|
||||
static constexpr float MAX_VXY = 5.f;
|
||||
static constexpr float PCOEF_MAX = 30.f;
|
||||
static constexpr float MIN_K_UPDATE_THRESHOLD = 0.1f;
|
||||
|
||||
void Run() override;
|
||||
void updateParams() override;
|
||||
void saveParameters();
|
||||
void publishStatus(hrt_abstime now, float residual, bool estimation_active);
|
||||
|
||||
BaroThrustCfRls _estimator{};
|
||||
|
||||
// Subscriptions
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_air_data_sub{this, ORB_ID(vehicle_air_data)};
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
|
||||
// Publication
|
||||
uORB::Publication<baro_thrust_estimate_s> _baro_thrust_estimate_pub{ORB_ID(baro_thrust_estimate)};
|
||||
|
||||
// State
|
||||
bool _armed{false};
|
||||
bool _landed{true};
|
||||
hrt_abstime _estimation_start_time{0};
|
||||
hrt_abstime _last_update_time{0};
|
||||
hrt_abstime _last_publish_time{0};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::SENS_BARO_PCOEF>) _param_sens_baro_pcoef,
|
||||
(ParamFloat<px4::params::SENS_BAR_CF_BW>) _param_sens_bar_cf_bw
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,55 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(baro_thrust_cf_rls
|
||||
baro_thrust_cf_rls.cpp
|
||||
baro_thrust_cf_rls.hpp
|
||||
)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__baro_thrust_estimator
|
||||
MAIN baro_thrust_estimator
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
SRCS
|
||||
BaroThrustEstimator.cpp
|
||||
BaroThrustEstimator.hpp
|
||||
MODULE_CONFIG
|
||||
params.yaml
|
||||
DEPENDS
|
||||
baro_thrust_cf_rls
|
||||
mathlib
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
px4_add_unit_gtest(SRC baro_thrust_cf_rls_test.cpp LINKLIBS baro_thrust_cf_rls)
|
||||
@@ -0,0 +1,13 @@
|
||||
menuconfig MODULES_BARO_THRUST_ESTIMATOR
|
||||
bool "baro_thrust_estimator"
|
||||
default y
|
||||
depends on SENSORS_VEHICLE_AIR_DATA
|
||||
---help---
|
||||
Enable support for baro_thrust_estimator
|
||||
|
||||
menuconfig USER_BARO_THRUST_ESTIMATOR
|
||||
bool "baro_thrust_estimator running as userspace module"
|
||||
default y
|
||||
depends on BOARD_PROTECTED && MODULES_BARO_THRUST_ESTIMATOR
|
||||
---help---
|
||||
Put baro_thrust_estimator in userspace memory
|
||||
@@ -0,0 +1,223 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "baro_thrust_cf_rls.hpp"
|
||||
|
||||
void BaroThrustCfRls::reset()
|
||||
{
|
||||
_rls.reset(RLS_P_INIT);
|
||||
|
||||
_cf_alt = 0.f;
|
||||
_cf_vel = 0.f;
|
||||
_cf_initialized = false;
|
||||
|
||||
_converged = false;
|
||||
_converged_locked = false;
|
||||
_converged_elapsed_s = 0.f;
|
||||
_k_stable_elapsed_s = 0.f;
|
||||
_k_est_smoothed.reset(0.f);
|
||||
_thrust_mean.reset(0.f);
|
||||
_thrust_var.reset(0.f);
|
||||
}
|
||||
|
||||
float BaroThrustCfRls::computeAccelUp(const matrix::Vector3f &accel_body,
|
||||
const matrix::Quatf &attitude)
|
||||
{
|
||||
// Rotate body-frame specific force to NED, extract Z component
|
||||
const float specific_force_ned_z = (attitude.rotateVector(accel_body))(2);
|
||||
|
||||
// Convert to altitude-up coordinate acceleration
|
||||
// specific_force = a_inertial - g_vec, so a_inertial_z = specific_force_z + g
|
||||
// altitude = -NED_z, so accel_up = -(specific_force_z + g)
|
||||
return -(specific_force_ned_z + CONSTANTS_ONE_G);
|
||||
}
|
||||
|
||||
float BaroThrustCfRls::updateCf(float baro_alt, float accel_up, float dt)
|
||||
{
|
||||
if (!_cf_initialized) {
|
||||
_cf_alt = baro_alt;
|
||||
_cf_vel = 0.f;
|
||||
_cf_initialized = true;
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
// Predict altitude from accel integration
|
||||
const float alt_pred = _cf_alt + _cf_vel * dt + 0.5f * accel_up * dt * dt;
|
||||
const float vel_pred = _cf_vel + accel_up * dt;
|
||||
|
||||
// CF residual: what the baro says minus what accel predicts
|
||||
const float residual = baro_alt - alt_pred;
|
||||
|
||||
// Correct CF state with baro at low bandwidth to prevent accel drift
|
||||
// 2nd-order critically damped: K1 = 2*omega, K2 = omega^2
|
||||
const float omega = 2.f * static_cast<float>(M_PI) * _cf_bandwidth_hz;
|
||||
const float K1 = 2.f * omega;
|
||||
const float K2 = omega * omega;
|
||||
|
||||
_cf_alt = alt_pred + K1 * dt * residual;
|
||||
_cf_vel = vel_pred + K2 * dt * residual;
|
||||
|
||||
if (!std::isfinite(_cf_alt) || !std::isfinite(_cf_vel)) {
|
||||
_cf_alt = baro_alt;
|
||||
_cf_vel = 0.f;
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
return residual;
|
||||
}
|
||||
|
||||
void BaroThrustCfRls::updateEstimator(float residual, float thrust, float dt)
|
||||
{
|
||||
_rls.update(residual, thrust, dt, RLS_LAMBDA);
|
||||
|
||||
// Track thrust excitation (compute deviation before updating mean
|
||||
// so the current sample doesn't bias the mean used for deviation)
|
||||
_thrust_mean.setParameters(dt, 2.f);
|
||||
const float thrust_dev = thrust - _thrust_mean.getState();
|
||||
_thrust_mean.update(thrust);
|
||||
_thrust_var.setParameters(dt, 2.f);
|
||||
_thrust_var.update(thrust_dev * thrust_dev);
|
||||
|
||||
// Track K stability
|
||||
_k_est_smoothed.setParameters(dt, 5.f);
|
||||
_k_est_smoothed.update(_rls.theta[0]);
|
||||
}
|
||||
|
||||
void BaroThrustCfRls::checkConvergence(float elapsed_since_start_s, float dt)
|
||||
{
|
||||
if (_converged_locked) {
|
||||
return;
|
||||
}
|
||||
|
||||
const bool variance_ok = _rls.P[0][0] < CONVERGENCE_VAR_THR;
|
||||
const bool excitation_ok = fmaxf(_thrust_var.getState(), 0.f) > (MIN_THRUST_EXCITATION * MIN_THRUST_EXCITATION);
|
||||
|
||||
// Dual-path error check: absolute threshold works for refinement flights
|
||||
// (PCOEF already set), relative threshold allows first calibration where
|
||||
// the uncompensated CF residual is noisy but the model explains most of it.
|
||||
const float K = _rls.theta[0];
|
||||
const float explained_var = K * K * fmaxf(_thrust_var.getState(), 0.f);
|
||||
const float total_var = explained_var + _rls.error_var;
|
||||
const bool error_ok = _rls.error_var < CONVERGENCE_ERR_THR
|
||||
|| (total_var > 1.f
|
||||
&& _rls.error_var / total_var < CONVERGENCE_ERR_REL_THR
|
||||
&& _rls.error_var < CONVERGENCE_ERR_MAX_THR);
|
||||
const bool time_ok = elapsed_since_start_s > MIN_ESTIMATION_TIME_S;
|
||||
|
||||
const float k_current = _k_est_smoothed.getState();
|
||||
const float k_best = _rls.theta[0];
|
||||
|
||||
if (fabsf(k_current - k_best) > K_STABILITY_DIFF_THR) {
|
||||
_k_stable_elapsed_s = 0.f;
|
||||
|
||||
} else {
|
||||
_k_stable_elapsed_s += dt;
|
||||
}
|
||||
|
||||
const bool stability_ok = _k_stable_elapsed_s > K_STABILITY_TIME_S;
|
||||
|
||||
_converged = variance_ok && error_ok && excitation_ok && time_ok && stability_ok;
|
||||
|
||||
// Once converged, keep accumulating hold time even if excitation
|
||||
// momentarily dips — the estimate quality checks (variance, error,
|
||||
// stability) are sufficient to guard the hold period.
|
||||
const bool hold_ok = variance_ok && error_ok && time_ok && stability_ok;
|
||||
|
||||
if (_converged || (_converged_elapsed_s > 0.f && hold_ok)) {
|
||||
_converged_elapsed_s += dt;
|
||||
|
||||
if (_converged_elapsed_s > CONVERGENCE_HOLD_TIME_S) {
|
||||
_converged_locked = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
_converged_elapsed_s = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// RlsEstimator
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void BaroThrustCfRls::RlsEstimator::reset(float p_init)
|
||||
{
|
||||
theta[0] = 0.f;
|
||||
theta[1] = 0.f;
|
||||
P[0][0] = p_init;
|
||||
P[0][1] = 0.f;
|
||||
P[1][0] = 0.f;
|
||||
P[1][1] = p_init;
|
||||
error_var = ERROR_VAR_INIT;
|
||||
}
|
||||
|
||||
void BaroThrustCfRls::RlsEstimator::update(float residual, float thrust, float dt, float lambda)
|
||||
{
|
||||
const float phi[2] = {thrust, 1.f};
|
||||
const float e = residual - (theta[0] * phi[0] + theta[1] * phi[1]);
|
||||
|
||||
const float Pphi[2] = {
|
||||
P[0][0] * phi[0] + P[0][1] * phi[1],
|
||||
P[1][0] * phi[0] + P[1][1] * phi[1]
|
||||
};
|
||||
|
||||
const float phiPphi = phi[0] * Pphi[0] + phi[1] * Pphi[1];
|
||||
const float denom = lambda + phiPphi;
|
||||
|
||||
if (fabsf(denom) < 1e-10f) {
|
||||
return;
|
||||
}
|
||||
|
||||
const float inv_denom = 1.f / denom;
|
||||
|
||||
theta[0] += Pphi[0] * inv_denom * e;
|
||||
theta[1] += Pphi[1] * inv_denom * e;
|
||||
|
||||
const float inv_lambda = 1.f / lambda;
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
for (int j = 0; j < 2; j++) {
|
||||
P[i][j] = (P[i][j] - Pphi[i] * Pphi[j] * inv_denom) * inv_lambda;
|
||||
}
|
||||
}
|
||||
|
||||
constexpr float alpha_err = 0.01f;
|
||||
error_var = (1.f - alpha_err) * error_var + alpha_err * e * e;
|
||||
|
||||
// Guard against numerical instability
|
||||
if (!std::isfinite(theta[0]) || !std::isfinite(theta[1])
|
||||
|| !std::isfinite(P[0][0]) || !std::isfinite(P[0][1])
|
||||
|| !std::isfinite(P[1][0]) || !std::isfinite(P[1][1])
|
||||
|| !std::isfinite(error_var)) {
|
||||
reset(RLS_P_INIT);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file baro_thrust_cf_rls.hpp
|
||||
*
|
||||
* Complementary Filter + Recursive Least Squares (CF+RLS) estimator
|
||||
* for barometer thrust compensation. Pure math — no uORB, no PX4
|
||||
* module infrastructure.
|
||||
*
|
||||
* Problem: propwash creates a static pressure error at the baro sensor
|
||||
* proportional to motor output. We want to identify the gain K in:
|
||||
*
|
||||
* baro_error = K * mean_motor_output + bias
|
||||
*
|
||||
* But we can't directly observe baro_error in flight — the vehicle is
|
||||
* actually moving, so baro altitude changes for real reasons too.
|
||||
*
|
||||
* Solution — two stages:
|
||||
*
|
||||
* 1. Complementary Filter (CF) — isolate the baro error signal.
|
||||
* Fuses barometer altitude with double-integrated accelerometer to
|
||||
* produce an altitude estimate. The CF uses a very low crossover
|
||||
* frequency (0.05 Hz), trusting the accelerometer for fast changes
|
||||
* and the baro for DC. The residual (baro minus CF prediction)
|
||||
* contains the thrust-correlated pressure error plus noise, with
|
||||
* real vehicle motion removed.
|
||||
*
|
||||
* 2. Recursive Least Squares (RLS) — identify K from the residual.
|
||||
* Fits the linear model: residual = K * thrust + bias, updating
|
||||
* the estimate with each new sample. RLS is chosen over batch
|
||||
* least-squares because it runs online with O(1) memory, adapts
|
||||
* to changing conditions, and provides a covariance estimate (P)
|
||||
* used for convergence detection. The forgetting factor (lambda)
|
||||
* down-weights old data to track slow parameter drift.
|
||||
*
|
||||
* Convergence requires: low parameter variance (P[0][0]), acceptable
|
||||
* prediction error (absolute OR relative to explained variance),
|
||||
* sufficient thrust excitation, stable K estimate for 10s, and a 10s
|
||||
* hold period. The relative error path allows first-flight calibration
|
||||
* (PCOEF=0) where absolute residual is high but the model explains
|
||||
* most of the variance. Once locked, K is saved to SENS_BARO_PCOEF
|
||||
* on disarm.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <float.h>
|
||||
|
||||
class BaroThrustCfRls
|
||||
{
|
||||
public:
|
||||
// --- RLS tuning ---
|
||||
static constexpr float RLS_LAMBDA = 0.998f; ///< forgetting factor (1.0 = never forget, lower = adapt faster)
|
||||
static constexpr float RLS_P_INIT = 100.f; ///< initial covariance diagonal (high = uncertain, learns fast)
|
||||
|
||||
// --- Convergence criteria ---
|
||||
static constexpr float CONVERGENCE_VAR_THR = 3.f; ///< max K variance (P[0][0]) to consider converged
|
||||
static constexpr float CONVERGENCE_ERR_THR = 0.5f; ///< max prediction error variance (absolute) [m^2]
|
||||
static constexpr float CONVERGENCE_ERR_REL_THR = 0.4f; ///< max error/total variance ratio (model explains >60%)
|
||||
static constexpr float CONVERGENCE_ERR_MAX_THR = 4.0f; ///< absolute error cap for relative path [m^2]
|
||||
static constexpr float MIN_THRUST_EXCITATION = 0.05f; ///< min thrust std dev to trust the estimate
|
||||
static constexpr float MIN_ESTIMATION_TIME_S = 30.f; ///< min flight time before convergence allowed
|
||||
static constexpr float K_STABILITY_TIME_S = 10.f; ///< K must be stable within threshold for this long
|
||||
static constexpr float CONVERGENCE_HOLD_TIME_S = 10.f; ///< must stay converged this long before locking
|
||||
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 ERROR_VAR_INIT = 10.f; ///< initial prediction error variance
|
||||
|
||||
/**
|
||||
* 2-state RLS estimator: fits residual = theta[0]*thrust + theta[1]
|
||||
* where theta[0] = K (the gain we want) and theta[1] = bias.
|
||||
* P is the 2x2 parameter covariance matrix.
|
||||
*/
|
||||
struct RlsEstimator {
|
||||
float theta[2] {}; ///< [K, bias] parameter estimates
|
||||
float P[2][2] {}; ///< 2x2 parameter covariance
|
||||
float error_var{ERROR_VAR_INIT}; ///< exponentially-weighted prediction error variance
|
||||
|
||||
void reset(float p_init);
|
||||
void update(float residual, float thrust, float dt, float lambda);
|
||||
};
|
||||
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* Set CF crossover frequency. Call before first updateCf() or after reset().
|
||||
*/
|
||||
void setCfBandwidth(float hz) { _cf_bandwidth_hz = hz; }
|
||||
|
||||
/**
|
||||
* Convert body-frame specific force (including gravity, FRD) to
|
||||
* altitude-up linear acceleration.
|
||||
*/
|
||||
static float computeAccelUp(const matrix::Vector3f &accel_body, const matrix::Quatf &attitude);
|
||||
|
||||
/**
|
||||
* Update complementary filter and return residual (baro - accel prediction).
|
||||
*/
|
||||
float updateCf(float baro_alt, float accel_up, float dt);
|
||||
|
||||
/**
|
||||
* Update the RLS estimator with the CF residual and motor thrust.
|
||||
*/
|
||||
void updateEstimator(float residual, float thrust, float dt);
|
||||
|
||||
void checkConvergence(float elapsed_since_start_s, float dt);
|
||||
|
||||
bool converged() const { return _converged; }
|
||||
bool convergedLocked() const { return _converged_locked; }
|
||||
float kEstimate() const { return _rls.theta[0]; }
|
||||
float kEstimateVar() const { return _rls.P[0][0]; }
|
||||
float errorVar() const { return _rls.error_var; }
|
||||
float thrustStd() const { return sqrtf(fmaxf(_thrust_var.getState(), 0.f)); }
|
||||
|
||||
private:
|
||||
RlsEstimator _rls{};
|
||||
|
||||
// CF state: 2nd-order (position + velocity) integrator corrected by baro
|
||||
float _cf_bandwidth_hz{CF_BANDWIDTH_HZ_DEFAULT}; ///< crossover frequency [Hz]
|
||||
float _cf_alt{0.f}; ///< CF altitude estimate [m]
|
||||
float _cf_vel{0.f}; ///< CF vertical velocity estimate [m/s]
|
||||
bool _cf_initialized{false};
|
||||
|
||||
// Convergence tracking
|
||||
AlphaFilter<float> _k_est_smoothed{}; ///< low-pass filtered K for stability check
|
||||
float _k_stable_elapsed_s{0.f}; ///< time K has been within stability threshold
|
||||
bool _converged{false}; ///< all convergence criteria currently met
|
||||
bool _converged_locked{false}; ///< converged and held long enough — ready to save
|
||||
float _converged_elapsed_s{0.f}; ///< accumulated hold time while converged
|
||||
|
||||
// Thrust excitation tracking (need variation in thrust to observe K)
|
||||
AlphaFilter<float> _thrust_mean{}; ///< low-pass mean thrust
|
||||
AlphaFilter<float> _thrust_var{}; ///< low-pass thrust variance
|
||||
};
|
||||
@@ -0,0 +1,252 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Test code for BaroThrustCfRls
|
||||
* Run: make tests TESTFILTER=baro_thrust_cf_rls
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <random>
|
||||
#include <cmath>
|
||||
|
||||
#include "baro_thrust_cf_rls.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
class BaroThrustCfRlsTest : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
BaroThrustCfRls _estimator{};
|
||||
static constexpr float _dt = 0.02f; // 50 Hz
|
||||
|
||||
std::default_random_engine _rng{42};
|
||||
std::normal_distribution<float> _normal{0.f, 1.f};
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// computeAccelUp — gravity handling
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, AccelUpAtRestLevel)
|
||||
{
|
||||
// At rest, level: specific force in FRD body = {0, 0, -g}
|
||||
const Vector3f accel_body{0.f, 0.f, -CONSTANTS_ONE_G};
|
||||
const Quatf identity{};
|
||||
|
||||
const float accel_up = BaroThrustCfRls::computeAccelUp(accel_body, identity);
|
||||
|
||||
EXPECT_NEAR(accel_up, 0.f, 1e-4f);
|
||||
}
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, AccelUpAscending)
|
||||
{
|
||||
const float a_up_true = 2.f;
|
||||
const Vector3f accel_body{0.f, 0.f, -(CONSTANTS_ONE_G + a_up_true)};
|
||||
const Quatf identity{};
|
||||
|
||||
const float accel_up = BaroThrustCfRls::computeAccelUp(accel_body, identity);
|
||||
|
||||
EXPECT_NEAR(accel_up, a_up_true, 1e-4f);
|
||||
}
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, AccelUpTilted45Hover)
|
||||
{
|
||||
const float theta = 45.f * 3.14159265f / 180.f;
|
||||
const Vector3f accel_body{CONSTANTS_ONE_G * sinf(theta), 0.f, -CONSTANTS_ONE_G * cosf(theta)};
|
||||
const Quatf attitude{Eulerf{0.f, theta, 0.f}};
|
||||
|
||||
const float accel_up = BaroThrustCfRls::computeAccelUp(accel_body, attitude);
|
||||
|
||||
EXPECT_NEAR(accel_up, 0.f, 1e-3f);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// CF residual
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, CfResidualConstantAltitude)
|
||||
{
|
||||
_estimator.reset();
|
||||
|
||||
const float baro_alt = 100.f;
|
||||
const float accel_up = 0.f;
|
||||
float residual = 0.f;
|
||||
|
||||
for (int i = 0; i < 500; i++) { // 10 seconds
|
||||
residual = _estimator.updateCf(baro_alt, accel_up, _dt);
|
||||
}
|
||||
|
||||
EXPECT_NEAR(residual, 0.f, 1e-3f);
|
||||
}
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, CfResidualCapturesPropwash)
|
||||
{
|
||||
_estimator.reset();
|
||||
|
||||
const float true_alt = 100.f;
|
||||
|
||||
// Settle CF
|
||||
for (int i = 0; i < 500; i++) {
|
||||
_estimator.updateCf(true_alt, 0.f, _dt);
|
||||
}
|
||||
|
||||
// Apply a +3 m propwash error to baro
|
||||
const float propwash_error = 3.f;
|
||||
float residual = _estimator.updateCf(true_alt + propwash_error, 0.f, _dt);
|
||||
|
||||
EXPECT_NEAR(residual, propwash_error, 0.5f);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// RLS convergence
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, RlsConvergenceNoiseless)
|
||||
{
|
||||
// Feed residual = K_true * thrust + bias, verify K estimate converges.
|
||||
_estimator.reset();
|
||||
|
||||
const float K_true = -5.f;
|
||||
const float bias_true = 0.3f;
|
||||
|
||||
for (float t = 0.f; t < 30.f; t += _dt) {
|
||||
const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f);
|
||||
const float residual = K_true * thrust + bias_true;
|
||||
_estimator.updateEstimator(residual, thrust, _dt);
|
||||
}
|
||||
|
||||
EXPECT_NEAR(_estimator.kEstimate(), K_true, 0.1f);
|
||||
}
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, RlsConvergenceWithNoise)
|
||||
{
|
||||
_estimator.reset();
|
||||
|
||||
const float K_true = -3.f;
|
||||
const float bias_true = 0.5f;
|
||||
const float noise_std = 0.3f;
|
||||
|
||||
for (float t = 0.f; t < 60.f; t += _dt) {
|
||||
const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f);
|
||||
const float residual = K_true * thrust + bias_true + noise_std * _normal(_rng);
|
||||
_estimator.updateEstimator(residual, thrust, _dt);
|
||||
}
|
||||
|
||||
EXPECT_NEAR(_estimator.kEstimate(), K_true, 0.5f);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Reset
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, ResetClearsConvergence)
|
||||
{
|
||||
_estimator.reset();
|
||||
|
||||
const float K_true = -3.f;
|
||||
const float bias_true = 0.1f;
|
||||
|
||||
// Drive to convergence
|
||||
for (float t = 0.f; t < 80.f; t += _dt) {
|
||||
const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f);
|
||||
const float residual = K_true * thrust + bias_true;
|
||||
_estimator.updateEstimator(residual, thrust, _dt);
|
||||
_estimator.checkConvergence(t, _dt);
|
||||
}
|
||||
|
||||
EXPECT_TRUE(_estimator.convergedLocked());
|
||||
EXPECT_NEAR(_estimator.kEstimate(), K_true, 0.2f);
|
||||
|
||||
// Reset should clear all convergence state
|
||||
_estimator.reset();
|
||||
EXPECT_FALSE(_estimator.converged());
|
||||
EXPECT_FALSE(_estimator.convergedLocked());
|
||||
EXPECT_NEAR(_estimator.kEstimate(), 0.f, 1e-6f);
|
||||
EXPECT_NEAR(_estimator.kEstimateVar(), BaroThrustCfRls::RLS_P_INIT, 1e-6f);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Convergence gating
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, ConvergenceRequiresMinTime)
|
||||
{
|
||||
_estimator.reset();
|
||||
|
||||
const float K_true = -3.f;
|
||||
|
||||
for (float t = 0.f; t < 25.f; t += _dt) { // < 30s minimum
|
||||
const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f);
|
||||
const float residual = K_true * thrust;
|
||||
_estimator.updateEstimator(residual, thrust, _dt);
|
||||
_estimator.checkConvergence(t, _dt);
|
||||
}
|
||||
|
||||
EXPECT_FALSE(_estimator.convergedLocked());
|
||||
}
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, NoExcitationDoesNotConverge)
|
||||
{
|
||||
_estimator.reset();
|
||||
|
||||
// Constant thrust — no excitation. RLS can fit K but the excitation
|
||||
// gate should prevent convergence since we can't trust the estimate.
|
||||
for (float t = 0.f; t < 80.f; t += _dt) {
|
||||
const float thrust = 0.5f;
|
||||
const float residual = -3.f * thrust;
|
||||
_estimator.updateEstimator(residual, thrust, _dt);
|
||||
_estimator.checkConvergence(t, _dt);
|
||||
}
|
||||
|
||||
EXPECT_FALSE(_estimator.convergedLocked());
|
||||
}
|
||||
|
||||
TEST_F(BaroThrustCfRlsTest, ConvergenceLocksAfterHoldTime)
|
||||
{
|
||||
_estimator.reset();
|
||||
|
||||
const float K_true = -3.f;
|
||||
const float bias_true = 0.1f;
|
||||
|
||||
for (float t = 0.f; t < 80.f; t += _dt) {
|
||||
const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f);
|
||||
const float residual = K_true * thrust + bias_true;
|
||||
_estimator.updateEstimator(residual, thrust, _dt);
|
||||
_estimator.checkConvergence(t, _dt);
|
||||
}
|
||||
|
||||
EXPECT_TRUE(_estimator.convergedLocked());
|
||||
EXPECT_NEAR(_estimator.kEstimate(), K_true, 0.2f);
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
module_name: baro_thrust_estimator
|
||||
parameters:
|
||||
- group: Sensors
|
||||
definitions:
|
||||
SENS_BAR_CF_BW:
|
||||
description:
|
||||
short: Baro thrust estimator CF crossover frequency
|
||||
long: |-
|
||||
Complementary filter bandwidth for the baro-accel altitude
|
||||
estimator used to isolate thrust-induced baro errors.
|
||||
Lower values trust baro more at low frequencies (conservative),
|
||||
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
|
||||
min: 0.01
|
||||
max: 1.0
|
||||
unit: Hz
|
||||
decimal: 2
|
||||
@@ -82,6 +82,7 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic_multi("heater_status");
|
||||
add_topic("home_position");
|
||||
add_topic("hover_thrust_estimate", 100);
|
||||
add_topic("baro_thrust_estimate", 100);
|
||||
add_topic("input_rc", 500);
|
||||
add_optional_topic("internal_combustion_engine_control", 10);
|
||||
add_optional_topic("internal_combustion_engine_status", 10);
|
||||
|
||||
@@ -99,6 +99,63 @@ float VehicleAirData::AirTemperatureUpdate(const float temperature_baro, Tempera
|
||||
return math::constrain(temperature, TEMPERATURE_MIN_CELSIUS, TEMPERATURE_MAX_CELSIUS);
|
||||
}
|
||||
|
||||
void VehicleAirData::updateThrustBuffer()
|
||||
{
|
||||
vehicle_thrust_setpoint_s thrust_sp;
|
||||
|
||||
while (_vehicle_thrust_setpoint_sub.update(&thrust_sp)) {
|
||||
ThrustSample &sample = _thrust_buffer[_thrust_buffer_head];
|
||||
sample.timestamp = thrust_sp.timestamp;
|
||||
sample.thrust_z = PX4_ISFINITE(thrust_sp.xyz[2]) ? fabsf(thrust_sp.xyz[2]) : 0.f;
|
||||
|
||||
_thrust_buffer_head = (_thrust_buffer_head + 1) % THRUST_BUFFER_SIZE;
|
||||
|
||||
if (_thrust_buffer_count < THRUST_BUFFER_SIZE) {
|
||||
_thrust_buffer_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float VehicleAirData::thrustCompensation(hrt_abstime timestamp_sample)
|
||||
{
|
||||
const float pcoef = _param_sens_baro_pcoef.get();
|
||||
|
||||
if (fabsf(pcoef) < FLT_EPSILON || _thrust_buffer_count == 0) {
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
// Find the thrust sample closest to the baro measurement time.
|
||||
// Buffer is chronologically ordered (newest at head-1), so once
|
||||
// the time delta starts increasing we've passed the closest match.
|
||||
int best_idx = -1;
|
||||
hrt_abstime best_dt = UINT64_MAX;
|
||||
|
||||
for (int i = 0; i < _thrust_buffer_count; i++) {
|
||||
int idx = (_thrust_buffer_head - 1 - i + THRUST_BUFFER_SIZE) % THRUST_BUFFER_SIZE;
|
||||
const hrt_abstime ts = _thrust_buffer[idx].timestamp;
|
||||
|
||||
if (ts == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const hrt_abstime dt = (ts >= timestamp_sample) ? (ts - timestamp_sample) : (timestamp_sample - ts);
|
||||
|
||||
if (dt < best_dt) {
|
||||
best_dt = dt;
|
||||
best_idx = idx;
|
||||
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (best_idx < 0 || best_dt > 500_ms) {
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
return pcoef * _thrust_buffer[best_idx].thrust_z;
|
||||
}
|
||||
|
||||
bool VehicleAirData::ParametersUpdate(bool force)
|
||||
{
|
||||
// Check if parameters have changed
|
||||
@@ -144,6 +201,8 @@ void VehicleAirData::Run()
|
||||
|
||||
const bool parameter_update = ParametersUpdate();
|
||||
|
||||
updateThrustBuffer();
|
||||
|
||||
estimator_status_flags_s estimator_status_flags;
|
||||
const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags);
|
||||
|
||||
@@ -260,7 +319,7 @@ void VehicleAirData::Run()
|
||||
if (!_relative_calibration_done) {
|
||||
_relative_calibration_done = UpdateRelativeCalibrations(time_now_us);
|
||||
|
||||
} else if (!_baro_gnss_calibration_done && _param_sens_baro_autocal.get() && latest_estimator_status_flags.cs_gps_hgt) {
|
||||
} else if (!_baro_gnss_calibration_done && (_param_sens_baro_autocal.get() & 1) && latest_estimator_status_flags.cs_gps_hgt) {
|
||||
_baro_gnss_calibration_done = BaroGNSSAltitudeOffset();
|
||||
}
|
||||
}
|
||||
@@ -289,7 +348,8 @@ void VehicleAirData::Run()
|
||||
const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us);
|
||||
|
||||
const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f;
|
||||
const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa);
|
||||
float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa);
|
||||
altitude += thrustCompensation(timestamp_sample);
|
||||
|
||||
// calculate air density
|
||||
const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature);
|
||||
|
||||
@@ -57,6 +57,7 @@
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/estimator_status_flags.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
@@ -90,6 +91,24 @@ private:
|
||||
bool UpdateRelativeCalibrations(hrt_abstime time_now_us);
|
||||
bool BaroGNSSAltitudeOffset();
|
||||
|
||||
// Note: applies a single PCOEF to whichever baro the voter selects as primary.
|
||||
// If baros are at different locations (e.g., internal vs external CAN), they may
|
||||
// experience different propwash magnitudes — a per-instance PCOEF would be needed
|
||||
// to handle that correctly. For now this assumes co-located sensors.
|
||||
float thrustCompensation(hrt_abstime timestamp_sample);
|
||||
void updateThrustBuffer();
|
||||
|
||||
static constexpr int THRUST_BUFFER_SIZE = 8;
|
||||
|
||||
struct ThrustSample {
|
||||
hrt_abstime timestamp{0};
|
||||
float thrust_z{0.f};
|
||||
};
|
||||
|
||||
ThrustSample _thrust_buffer[THRUST_BUFFER_SIZE] {};
|
||||
int _thrust_buffer_head{0};
|
||||
int _thrust_buffer_count{0};
|
||||
|
||||
static constexpr int MAX_SENSOR_COUNT = 4;
|
||||
|
||||
uORB::Publication<sensors_status_s> _sensors_status_baro_pub{ORB_ID(sensors_status_baro)};
|
||||
@@ -111,6 +130,8 @@ private:
|
||||
|
||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||
|
||||
uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
|
||||
|
||||
calibration::Barometer _calibration[MAX_SENSOR_COUNT];
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
@@ -147,7 +168,8 @@ private:
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
|
||||
(ParamBool<px4::params::SENS_BAR_AUTOCAL>) _param_sens_baro_autocal
|
||||
(ParamInt<px4::params::SENS_BAR_AUTOCAL>) _param_sens_baro_autocal,
|
||||
(ParamFloat<px4::params::SENS_BARO_PCOEF>) _param_sens_baro_pcoef
|
||||
)
|
||||
};
|
||||
}; // namespace sensors
|
||||
|
||||
@@ -13,7 +13,29 @@ parameters:
|
||||
SENS_BAR_AUTOCAL:
|
||||
description:
|
||||
short: Barometer auto calibration
|
||||
long: Automatically calibrate barometer based on the GNSS height
|
||||
long: |
|
||||
Bitmask controlling automatic barometer calibrations.
|
||||
Bit 0: GNSS-based altitude offset calibration.
|
||||
Bit 1: Online thrust compensation estimation (identifies
|
||||
SENS_BARO_PCOEF during flight and saves on disarm).
|
||||
category: System
|
||||
type: boolean
|
||||
type: bitmask
|
||||
bit:
|
||||
0: GNSS altitude offset
|
||||
1: Thrust compensation
|
||||
default: 1
|
||||
min: 0
|
||||
max: 3
|
||||
SENS_BARO_PCOEF:
|
||||
description:
|
||||
short: Baro altitude correction per unit vertical thrust
|
||||
long: |-
|
||||
Corrects propwash-induced baro error proportional to vertical
|
||||
thrust setpoint magnitude. Sign depends on sensor placement
|
||||
relative to propellers. Set to 0 to disable.
|
||||
type: float
|
||||
default: 0.0
|
||||
min: -30.0
|
||||
max: 30.0
|
||||
unit: m
|
||||
decimal: 1
|
||||
|
||||
Reference in New Issue
Block a user