Files
PX4-Autopilot/src/lib/sensor_calibration/DifferentialPressure.cpp
T
Mahima Yoga f16f99d4b5 feat(sensors): support multiple airspeed sensors (#28832)
* feat(lib/sensor_calibration): add differential pressure calibration

Airspeed sensors had a single global zero offset, so a second or third
sensor could not be calibrated.

Link the calibration to the sensor's device ID, following the existing
barometer and magnetometer pattern, so the offsets stay bound to physical
sensors rather than to uORB instance order.

Signed-off-by: mahima-yoga <mahima@auterion.com>

* feat(sensors): support multiple airspeed sensors

The airspeed selector has always been able to validate and fail over
between three airspeed sensors, but nothing ever produced more than one.

Give each differential pressure sensor its own VehicleAirspeed item,
which owns its calibration, validator and airspeed publication, following the
VehicleIMU pattern.

Airspeed carries the device ID of the sensor it came from, so consumers
can tell the instances apart. The calibration routine now calibrates every
connected sensor.

Signed-off-by: mahima-yoga <mahima@auterion.com>

* refactor(sensors)!: replace SENS_DPRES_OFF with per-sensor calibration

SENS_DPRES_OFF applied one zero offset to whichever airspeed sensor
happened to come first, which cannot work once there is more than one
sensor. It is superseded by CAL_DPRESx_OFF.

BREAKING CHANGE: SENS_DPRES_OFF is removed. Ground stations that read it
to decide whether airspeed has been calibrated must check CAL_DPRES0_ID
instead, matching how CAL_MAG0_ID, CAL_GYRO0_ID and CAL_ACC0_ID are
already used.

Signed-off-by: mahima-yoga <mahima@auterion.com>

* feat(logger): log all airspeed and differential pressure instances

The airspeed selector supports up to three airspeed sensors, but the
logger only recorded one airspeed instance and two differential pressure
instances.

Signed-off-by: mahima-yoga <mahima@auterion.com>

* fix(commander): stop airspeed calibration consuming a new slot each time

Signed-off-by: mahima-yoga <mahima@auterion.com>

* fix(sensors): give the analogue airspeed sensor a device ID

Signed-off-by: mahima-yoga <mahima@auterion.com>

* fix(boards): keep the airspeed conversion in ITCM

Signed-off-by: mahima-yoga <mahima@auterion.com>

* refactor(sensors): initialise airspeed instances outside the main loop

Signed-off-by: mahima-yoga <mahima@auterion.com>

* chore(sensors): correct the copyright year on the new CMakeLists

Signed-off-by: mahima-yoga <mahima@auterion.com>

* refactor(airspeed_selector): share the airspeed sensor count constant

Signed-off-by: mahima-yoga <mahima@auterion.com>

* docs(sensor_calibration): explain the unusable calibration slot guard

Signed-off-by: mahima-yoga <mahima@auterion.com>

* fix(sensors): keep SENS_DPRES_OFF as a mirror of the first sensor

Signed-off-by: mahima-yoga <mahima@auterion.com>

---------

Signed-off-by: mahima-yoga <mahima@auterion.com>
2026-10-12 10:51:22 +02:00

222 lines
6.1 KiB
C++

/****************************************************************************
*
* Copyright (c) 2026 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 "DifferentialPressure.hpp"
#include "Utilities.hpp"
#include <lib/drivers/device/Device.hpp>
#include <lib/parameters/param.h>
#include <float.h>
namespace calibration
{
DifferentialPressure::DifferentialPressure()
{
Reset();
}
DifferentialPressure::DifferentialPressure(uint32_t device_id)
{
set_device_id(device_id);
}
void DifferentialPressure::set_device_id(uint32_t device_id)
{
const bool external = device::device_is_external(device_id);
if (_device_id != device_id || _external != external) {
_device_id = device_id;
_external = external;
Reset();
ParametersUpdate();
}
}
bool DifferentialPressure::set_offset(const float &offset)
{
if (fabsf(_offset - offset) > 0.01f) {
if (PX4_ISFINITE(offset)) {
_offset = offset;
_calibration_count++;
return true;
}
}
return false;
}
bool DifferentialPressure::set_calibration_index(int calibration_index)
{
if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
_calibration_index = calibration_index;
return true;
}
return false;
}
void DifferentialPressure::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
if (_calibration_index == -1) {
// no saved calibration available
Reset();
} else {
ParametersLoad();
}
}
bool DifferentialPressure::ParametersLoad()
{
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// CAL_DPRESx_OFF
set_offset(GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index));
return true;
}
return false;
}
bool DifferentialPressure::AdoptLegacyOffset()
{
if ((_device_id == 0) || calibrated()) {
return false;
}
// slot 0 must still be free, otherwise another sensor already owns the legacy offset
if (GetCalibrationParamInt32(SensorString(), "ID", kLegacyIndex) != 0) {
return false;
}
float legacy_offset = 0.f;
if (param_get(param_find(kLegacyOffsetParam), &legacy_offset) != PX4_OK) {
return false;
}
if (!PX4_ISFINITE(legacy_offset) || (fabsf(legacy_offset) < FLT_EPSILON)) {
return false;
}
_offset = legacy_offset;
_calibration_index = kLegacyIndex;
PX4_INFO("%s %" PRIu32 " adopted %s %.3f Pa", SensorString(), _device_id, kLegacyOffsetParam, (double)_offset);
return true;
}
void DifferentialPressure::UpdateLegacyOffset() const
{
// keep SENS_DPRES_OFF in step with slot 0 so that ground stations reading it can still tell
// whether airspeed has been calibrated
if (_calibration_index != kLegacyIndex) {
return;
}
const param_t handle = param_find(kLegacyOffsetParam);
if (handle == PARAM_INVALID) {
return;
}
float offset = _offset;
param_set_no_notification(handle, &offset);
}
void DifferentialPressure::Reset()
{
_offset = 0;
_calibration_index = -1;
_calibration_count = 0;
}
bool DifferentialPressure::ParametersSave(int desired_calibration_index, bool force)
{
if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
_calibration_index = desired_calibration_index;
} else if (!force || (_calibration_index < 0)
|| (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
// ensure we have a valid calibration slot (matching existing or first available slot)
int8_t calibration_index_prev = _calibration_index;
_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
calibration_index_prev, _calibration_index);
}
}
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// save calibration
bool success = true;
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
success &= SetCalibrationParam(SensorString(), "OFF", _calibration_index, _offset);
UpdateLegacyOffset();
return success;
}
// index 3 is possible: FindAvailableCalibrationIndex() searches four slots, we define three
PX4_ERR("%s %" PRIu32 " no free calibration slot (got %" PRIi8 ")", SensorString(), _device_id, _calibration_index);
return false;
}
void DifferentialPressure::PrintStatus()
{
PX4_INFO_RAW("%s %" PRIu32 " offset: %05.3f Pa, %s\n",
SensorString(), device_id(), (double)_offset,
external() ? "Ext" : "Internal");
}
} // namespace calibration