mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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>
This commit is contained in:
1 parent
849873ca7f
commit
f16f99d4b5
23 files changed
+969
-238
No files matched your search
@@ -302,7 +302,7 @@
|
||||
*(.text.imxrt_progressep)
|
||||
*(.text.imxrt_gpio_configinput)
|
||||
*(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s)
|
||||
*(.text._ZN7Sensors14diff_pres_pollEv)
|
||||
*(.text._ZN7sensors15VehicleAirspeed3RunEv)
|
||||
*(.text._ZN21MavlinkStreamAttitude4sendEv)
|
||||
*(.text._ZN4EKF220UpdateMagCalibrationERKy)
|
||||
*(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv)
|
||||
|
||||
@@ -294,7 +294,7 @@
|
||||
*(.text.imxrt_progressep)
|
||||
*(.text.imxrt_gpio_configinput)
|
||||
*(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s)
|
||||
*(.text._ZN7Sensors14diff_pres_pollEv)
|
||||
*(.text._ZN7sensors15VehicleAirspeed3RunEv)
|
||||
*(.text._ZN21MavlinkStreamAttitude4sendEv)
|
||||
*(.text._ZN4EKF220UpdateMagCalibrationERKy)
|
||||
*(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv)
|
||||
|
||||
@@ -298,7 +298,7 @@
|
||||
*(.text.imxrt_progressep)
|
||||
*(.text.imxrt_gpio_configinput)
|
||||
*(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s)
|
||||
*(.text._ZN7Sensors14diff_pres_pollEv)
|
||||
*(.text._ZN7sensors15VehicleAirspeed3RunEv)
|
||||
*(.text._ZN21MavlinkStreamAttitude4sendEv)
|
||||
*(.text._ZN4EKF220UpdateMagCalibrationERKy)
|
||||
*(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv)
|
||||
|
||||
@@ -298,7 +298,7 @@
|
||||
*(.text.imxrt_progressep)
|
||||
*(.text.imxrt_gpio_configinput)
|
||||
*(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s)
|
||||
*(.text._ZN7Sensors14diff_pres_pollEv)
|
||||
*(.text._ZN7sensors15VehicleAirspeed3RunEv)
|
||||
*(.text._ZN21MavlinkStreamAttitude4sendEv)
|
||||
*(.text._ZN4EKF220UpdateMagCalibrationERKy)
|
||||
*(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv)
|
||||
|
||||
@@ -302,7 +302,7 @@
|
||||
*(.text.imxrt_progressep)
|
||||
*(.text.imxrt_gpio_configinput)
|
||||
*(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s)
|
||||
*(.text._ZN7Sensors14diff_pres_pollEv)
|
||||
*(.text._ZN7sensors15VehicleAirspeed3RunEv)
|
||||
*(.text._ZN21MavlinkStreamAttitude4sendEv)
|
||||
*(.text._ZN4EKF220UpdateMagCalibrationERKy)
|
||||
*(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv)
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
|
||||
uint64 timestamp # [us] Time since system start
|
||||
uint64 timestamp_sample # [us] Timestamp of the raw data
|
||||
uint32 device_id # [-] Unique device ID of the sensor this measurement came from, 0 if unknown
|
||||
float32 indicated_airspeed_m_s # [m/s] Indicated airspeed
|
||||
float32 true_airspeed_m_s # [m/s] True airspeed
|
||||
float32 confidence # [@range 0,1] Confidence value for this sensor
|
||||
@@ -136,6 +136,7 @@
|
||||
#define DRV_BARO_DEVTYPE_MPL3115A2 0x51
|
||||
#define DRV_ACC_DEVTYPE_FXOS8701C 0x52
|
||||
#define DRV_DIFF_PRESS_DEVTYPE_DLVR 0x53
|
||||
#define DRV_DIFF_PRESS_DEVTYPE_ANALOG 0x55
|
||||
#define DRV_GYR_DEVTYPE_FXAS2100C 0x54
|
||||
|
||||
#define DRV_IMU_DEVTYPE_ADIS16448 0x57
|
||||
|
||||
@@ -102,6 +102,7 @@ UavcanAirspeedBridge::ias_sub_cb(const
|
||||
* The proper solution is to be developed.
|
||||
*/
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.device_id = make_uavcan_device_id(msg);
|
||||
report.indicated_airspeed_m_s = msg.indicated_airspeed;
|
||||
report.true_airspeed_m_s = _last_tas_m_s;
|
||||
|
||||
|
||||
@@ -36,6 +36,8 @@ px4_add_library(sensor_calibration
|
||||
Accelerometer.hpp
|
||||
Barometer.cpp
|
||||
Barometer.hpp
|
||||
DifferentialPressure.cpp
|
||||
DifferentialPressure.hpp
|
||||
Gyroscope.cpp
|
||||
Gyroscope.hpp
|
||||
Magnetometer.cpp
|
||||
|
||||
@@ -0,0 +1,221 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
@@ -0,0 +1,104 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <math.h>
|
||||
|
||||
namespace calibration
|
||||
{
|
||||
class DifferentialPressure
|
||||
{
|
||||
public:
|
||||
static constexpr int MAX_SENSOR_COUNT = 3; ///< also the airspeed selector's sensor limit
|
||||
|
||||
static constexpr const char *SensorString() { return "DPRES"; }
|
||||
|
||||
DifferentialPressure();
|
||||
explicit DifferentialPressure(uint32_t device_id);
|
||||
|
||||
~DifferentialPressure() = default;
|
||||
|
||||
void PrintStatus();
|
||||
|
||||
bool set_calibration_index(int calibration_index);
|
||||
void set_device_id(uint32_t device_id);
|
||||
bool set_offset(const float &offset);
|
||||
|
||||
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
|
||||
uint8_t calibration_count() const { return _calibration_count; }
|
||||
int8_t calibration_index() const { return _calibration_index; }
|
||||
uint32_t device_id() const { return _device_id; }
|
||||
bool external() const { return _external; }
|
||||
const float &offset() const { return _offset; }
|
||||
|
||||
/// remove the zero offset from a raw differential pressure reading [Pa]
|
||||
inline float Correct(const float &data) const { return data - _offset; }
|
||||
|
||||
inline float Uncorrect(const float &corrected_data) const { return corrected_data + _offset; }
|
||||
|
||||
bool ParametersLoad();
|
||||
bool ParametersSave(int desired_calibration_index = -1, bool force = false);
|
||||
void ParametersUpdate();
|
||||
|
||||
/**
|
||||
* Take over the offset left in SENS_DPRES_OFF, using slot 0.
|
||||
* Does nothing if slot 0 is already claimed.
|
||||
*
|
||||
* @return true if the offset was adopted (the caller should save)
|
||||
*/
|
||||
bool AdoptLegacyOffset();
|
||||
|
||||
/** mirror slot 0's offset into SENS_DPRES_OFF for ground stations that still read it */
|
||||
void UpdateLegacyOffset() const;
|
||||
|
||||
void Reset();
|
||||
|
||||
private:
|
||||
static constexpr int kLegacyIndex{0};
|
||||
static constexpr const char *kLegacyOffsetParam{"SENS_DPRES_OFF"};
|
||||
|
||||
float _offset{0};
|
||||
|
||||
int8_t _calibration_index{-1};
|
||||
uint32_t _device_id{0};
|
||||
uint8_t _calibration_count{0};
|
||||
|
||||
bool _external{false};
|
||||
};
|
||||
} // namespace calibration
|
||||
@@ -43,6 +43,7 @@
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/airspeed/airspeed.h>
|
||||
#include <lib/sensor_calibration/DifferentialPressure.hpp>
|
||||
#include <lib/systemlib/mavlink_log.h>
|
||||
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
|
||||
|
||||
@@ -104,7 +105,7 @@ private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
|
||||
static constexpr int MAX_NUM_AIRSPEED_SENSORS = calibration::DifferentialPressure::MAX_SENSOR_COUNT;
|
||||
enum class AirspeedSource : int {
|
||||
DISABLED = -1,
|
||||
GROUND_MINUS_WIND,
|
||||
|
||||
@@ -49,6 +49,7 @@
|
||||
#include <fcntl.h>
|
||||
#include <math.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/sensor_calibration/DifferentialPressure.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
@@ -65,25 +66,27 @@ static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
|
||||
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
||||
static constexpr int MAX_SENSORS = calibration::DifferentialPressure::MAX_SENSOR_COUNT;
|
||||
|
||||
struct AirspeedSensor {
|
||||
uORB::SubscriptionData<differential_pressure_s> sub{ORB_ID(differential_pressure)};
|
||||
uint32_t device_id{0};
|
||||
float offset{0.f};
|
||||
int instance{0};
|
||||
bool connected{false};
|
||||
};
|
||||
|
||||
/**
|
||||
* Collect the zero reading of every connected sensor simultaneously.
|
||||
*/
|
||||
static int collect_zero_offsets(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started,
|
||||
AirspeedSensor sensors[MAX_SENSORS], int num_sensors)
|
||||
{
|
||||
const hrt_abstime calibration_started = hrt_absolute_time();
|
||||
static constexpr unsigned calibration_count = (500 * 2) / 3;
|
||||
|
||||
int result = PX4_OK;
|
||||
unsigned calibration_counter = 0;
|
||||
const unsigned maxcount = 500;
|
||||
|
||||
/* give directions */
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
|
||||
|
||||
const unsigned calibration_count = (maxcount * 2) / 3;
|
||||
|
||||
float diff_pres_offset = 0.0f;
|
||||
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
|
||||
px4_usleep(500 * 1000);
|
||||
|
||||
uORB::SubscriptionData<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
|
||||
float sums[MAX_SENSORS] {};
|
||||
unsigned counts[MAX_SENSORS] {};
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
|
||||
@@ -91,11 +94,17 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (diff_pres_sub.update()) {
|
||||
bool any_update = false;
|
||||
|
||||
const differential_pressure_s &diff_pres = diff_pres_sub.get();
|
||||
for (int i = 0; i < num_sensors; i++) {
|
||||
if (sensors[i].sub.update()) {
|
||||
sums[i] += sensors[i].sub.get().differential_pressure_pa;
|
||||
counts[i]++;
|
||||
any_update = true;
|
||||
}
|
||||
}
|
||||
|
||||
diff_pres_offset += diff_pres.differential_pressure_pa;
|
||||
if (any_update) {
|
||||
calibration_counter++;
|
||||
|
||||
if (calibration_counter % (calibration_count / 20) == 0) {
|
||||
@@ -111,77 +120,97 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
||||
px4_usleep(10000);
|
||||
}
|
||||
|
||||
diff_pres_offset = diff_pres_offset / calibration_count;
|
||||
|
||||
if (PX4_ISFINITE(diff_pres_offset)) {
|
||||
// Prevent a completely zero param
|
||||
// since this is used to detect a missing calibration
|
||||
// This value is numerically down in the noise and has
|
||||
// no effect on the sensor performance.
|
||||
if (fabsf(diff_pres_offset) < 0.00000001f) {
|
||||
diff_pres_offset = 0.00000001f;
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset)) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
|
||||
for (int i = 0; i < num_sensors; i++) {
|
||||
if (counts[i] == 0) {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor %d stopped reporting", i + 1);
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
sensors[i].offset = sums[i] / counts[i];
|
||||
|
||||
if (!PX4_ISFINITE(sensors[i].offset)) {
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
/* wait 500 ms to ensure parameter propagated through the system */
|
||||
px4_usleep(500 * 1000);
|
||||
/** Store (or clear, with an offset of 0) the zero offset of one sensor. */
|
||||
static bool save_offset(uint32_t device_id, float offset, int instance)
|
||||
{
|
||||
calibration::DifferentialPressure calibration{device_id};
|
||||
calibration.set_offset(offset);
|
||||
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");
|
||||
// claim the slot matching the sensor instance, as the other sensor calibrations do, so that
|
||||
// recalibrating reuses a slot instead of consuming a new one every time
|
||||
return calibration.ParametersSave(instance, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Only the sensor that failed is cleared
|
||||
*/
|
||||
static void clear_offset(const AirspeedSensor &sensor)
|
||||
{
|
||||
save_offset(sensor.device_id, 0.f, sensor.instance);
|
||||
|
||||
param_notify_changes();
|
||||
}
|
||||
|
||||
/**
|
||||
* Verify that positive pressure reaches the sensor, which catches swapped static and dynamic
|
||||
* ports. Each sensor has its own pitot, so this has to be repeated for each of them.
|
||||
*/
|
||||
static int check_pitot_direction(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started,
|
||||
AirspeedSensor sensors[MAX_SENSORS], int num_sensors, int index)
|
||||
{
|
||||
static constexpr unsigned maxcount = 500;
|
||||
|
||||
AirspeedSensor &sensor = sensors[index];
|
||||
|
||||
if (num_sensors > 1) {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot %d without touching", index + 1);
|
||||
|
||||
} else {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");
|
||||
}
|
||||
|
||||
const hrt_abstime sensor_started = hrt_absolute_time();
|
||||
|
||||
float differential_pressure_sum = 0.f;
|
||||
int differential_pressure_sum_count = 0;
|
||||
unsigned calibration_counter = 0;
|
||||
|
||||
calibration_counter = 0;
|
||||
|
||||
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
|
||||
while (calibration_counter < maxcount) {
|
||||
|
||||
if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (diff_pres_sub.update()) {
|
||||
const differential_pressure_s &diff_pres = diff_pres_sub.get();
|
||||
|
||||
differential_pressure_sum += diff_pres.differential_pressure_pa;
|
||||
if (sensor.sub.update()) {
|
||||
differential_pressure_sum += sensor.sub.get().differential_pressure_pa;
|
||||
differential_pressure_sum_count++;
|
||||
|
||||
const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) - diff_pres_offset;
|
||||
const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) -
|
||||
sensor.offset;
|
||||
|
||||
if ((differential_pressure_sum_count > 10) && (fabsf(differential_pressure_pa) > 50.f)) {
|
||||
if (differential_pressure_pa > 0) {
|
||||
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)differential_pressure_pa);
|
||||
break;
|
||||
|
||||
} else {
|
||||
/* do not allow negative values */
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
|
||||
(int)differential_pressure_pa);
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports or set SENS_DPRES_REV");
|
||||
|
||||
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
|
||||
diff_pres_offset = 0.0f;
|
||||
|
||||
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
/* do not allow negative values */
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
|
||||
(int)differential_pressure_pa);
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports or set SENS_DPRES_REV");
|
||||
|
||||
clear_offset(sensor);
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (calibration_counter % 300 == 0) {
|
||||
@@ -197,14 +226,8 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
||||
calibration_counter++;
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&calibration_started) > 90_s) {
|
||||
diff_pres_offset = 0.0f;
|
||||
|
||||
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&sensor_started) > 90_s) {
|
||||
clear_offset(sensor);
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
@@ -213,17 +236,70 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
||||
px4_usleep(10000);
|
||||
}
|
||||
|
||||
if (calibration_counter == maxcount) {
|
||||
diff_pres_offset = 0.0f;
|
||||
clear_offset(sensor);
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
||||
{
|
||||
const hrt_abstime calibration_started = hrt_absolute_time();
|
||||
|
||||
/* give directions */
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
|
||||
|
||||
// find every connected differential pressure sensor.
|
||||
AirspeedSensor sensors[MAX_SENSORS] {};
|
||||
int num_sensors = 0;
|
||||
|
||||
for (int i = 0; i < MAX_SENSORS; i++) {
|
||||
uORB::SubscriptionData<differential_pressure_s> sub{ORB_ID(differential_pressure), (uint8_t)i};
|
||||
|
||||
if (sub.advertised() && sub.get().timestamp != 0) {
|
||||
sensors[num_sensors].sub.ChangeInstance(i);
|
||||
sensors[num_sensors].instance = i;
|
||||
sensors[num_sensors].device_id = sub.get().device_id;
|
||||
sensors[num_sensors].connected = true;
|
||||
num_sensors++;
|
||||
}
|
||||
}
|
||||
|
||||
if (num_sensors == 0) {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (num_sensors > 1) {
|
||||
calibration_log_info(mavlink_log_pub, "[cal] Calibrating %d airspeed sensors", num_sensors);
|
||||
}
|
||||
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
|
||||
px4_usleep(500 * 1000);
|
||||
|
||||
if (collect_zero_offsets(mavlink_log_pub, calibration_started, sensors, num_sensors) != PX4_OK) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
for (int i = 0; i < num_sensors; i++) {
|
||||
if (!save_offset(sensors[i].device_id, sensors[i].offset, sensors[i].instance)) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
return PX4_ERROR;
|
||||
calibration_log_info(mavlink_log_pub, "[cal] Sensor %d offset of %d Pascal", i + 1, (int)sensors[i].offset);
|
||||
}
|
||||
|
||||
param_notify_changes();
|
||||
|
||||
/* wait 500 ms to ensure parameter propagated through the system */
|
||||
px4_usleep(500 * 1000);
|
||||
|
||||
for (int i = 0; i < num_sensors; i++) {
|
||||
if (check_pitot_direction(mavlink_log_pub, calibration_started, sensors, num_sensors, i) != PX4_OK) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
|
||||
@@ -234,5 +310,5 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
||||
// This give a chance for the log messages to go out of the queue before someone else stomps on then
|
||||
px4_usleep(200000);
|
||||
|
||||
return result;
|
||||
return PX4_OK;
|
||||
}
|
||||
@@ -48,7 +48,6 @@ void LoggedTopics::add_default_topics()
|
||||
add_topic("action_request");
|
||||
add_topic("actuator_armed");
|
||||
add_optional_topic("actuator_controls_status_0", 300);
|
||||
add_topic("airspeed", 1000);
|
||||
add_optional_topic("airspeed_validated", 200);
|
||||
add_optional_topic("autotune_attitude_control_status", 100);
|
||||
add_topic_multi("battery_info", 5000, 3);
|
||||
@@ -170,6 +169,7 @@ void LoggedTopics::add_default_topics()
|
||||
|
||||
// multi topics
|
||||
add_optional_topic_multi("actuator_outputs", 100, 3);
|
||||
add_topic_multi("airspeed", 1000, 3);
|
||||
add_optional_topic_multi("airspeed_wind", 1000, 4);
|
||||
add_optional_topic_multi("control_allocator_status", 200, 2);
|
||||
add_topic_multi("fuel_tank_status", 200, 3);
|
||||
@@ -238,7 +238,7 @@ void LoggedTopics::add_default_topics()
|
||||
|
||||
// log all raw sensors at minimal rate (at least 1 Hz)
|
||||
add_topic_multi("battery_status", 200, 3);
|
||||
add_topic_multi("differential_pressure", 1000, 2);
|
||||
add_topic_multi("differential_pressure", 1000, 3);
|
||||
add_topic_multi("distance_sensor", 1000, 2);
|
||||
add_optional_topic_multi("sensor_accel", 1000, 4);
|
||||
add_topic_multi("sensor_baro", 1000, 4);
|
||||
|
||||
@@ -45,6 +45,10 @@ if(CONFIG_SENSORS_VEHICLE_AIR_DATA)
|
||||
add_subdirectory(vehicle_air_data)
|
||||
endif()
|
||||
|
||||
if(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
add_subdirectory(vehicle_airspeed)
|
||||
endif()
|
||||
|
||||
if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
|
||||
add_subdirectory(vehicle_angular_velocity)
|
||||
endif()
|
||||
@@ -99,7 +103,7 @@ if(CONFIG_SENSORS_VEHICLE_AIR_DATA)
|
||||
endif()
|
||||
|
||||
if(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
target_link_libraries(modules__sensors PRIVATE airspeed)
|
||||
target_link_libraries(modules__sensors PRIVATE vehicle_airspeed)
|
||||
endif()
|
||||
|
||||
if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
|
||||
|
||||
@@ -210,6 +210,29 @@ parameters:
|
||||
num_instances: *max_num_sensor_instances
|
||||
instance_start: 0
|
||||
|
||||
# Differential pressure (airspeed) calibration
|
||||
CAL_DPRES${i}_ID:
|
||||
description:
|
||||
short: Differential pressure sensor ${i} calibration device ID
|
||||
long: Device ID of the differential pressure sensor this calibration applies to.
|
||||
category: System
|
||||
type: int32
|
||||
default: 0
|
||||
num_instances: 3
|
||||
instance_start: 0
|
||||
|
||||
CAL_DPRES${i}_OFF:
|
||||
description:
|
||||
short: Differential pressure sensor ${i} offset
|
||||
long: The zero-reading of this differential pressure sensor, in Pascal.
|
||||
category: System
|
||||
type: float
|
||||
default: 0.0
|
||||
decimal: 3
|
||||
volatile: true
|
||||
num_instances: 3
|
||||
instance_start: 0
|
||||
|
||||
# Gyroscope calibration
|
||||
CAL_GYRO${i}_ID:
|
||||
description:
|
||||
|
||||
@@ -4,8 +4,12 @@ parameters:
|
||||
definitions:
|
||||
SENS_DPRES_OFF:
|
||||
description:
|
||||
short: Differential pressure sensor offset
|
||||
long: The offset (zero-reading) in Pascal
|
||||
short: Differential pressure sensor offset of the first airspeed sensor
|
||||
long: |-
|
||||
The offset (zero-reading) in Pascal of the sensor holding calibration slot 0.
|
||||
|
||||
Kept as a mirror of CAL_DPRES0_OFF so that ground stations can still detect whether
|
||||
airspeed has been calibrated. The firmware applies CAL_DPRESx_OFF, not this value.
|
||||
category: System
|
||||
type: float
|
||||
default: 0.0
|
||||
|
||||
+64
-104
@@ -59,18 +59,9 @@ Sensors::Sensors(bool hil_enabled) :
|
||||
#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
/* Differential pressure offset */
|
||||
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
|
||||
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
|
||||
|
||||
_parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
|
||||
_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
|
||||
_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
|
||||
|
||||
_airspeed_validator.set_timeout(300000);
|
||||
_airspeed_validator.set_equal_value_threshold(100);
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
|
||||
@@ -115,6 +106,17 @@ Sensors::~Sensors()
|
||||
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
|
||||
for (auto &airspeed : _vehicle_airspeed_list) {
|
||||
if (airspeed) {
|
||||
airspeed->Stop();
|
||||
delete airspeed;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
|
||||
|
||||
if (_vehicle_gps_position) {
|
||||
@@ -169,15 +171,9 @@ int Sensors::parameters_update()
|
||||
}
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
/* Airspeed offset */
|
||||
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
|
||||
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
|
||||
|
||||
param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel);
|
||||
param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length);
|
||||
param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm);
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
_voted_sensors_update.parametersUpdate();
|
||||
@@ -250,6 +246,10 @@ int Sensors::parameters_update()
|
||||
InitializeVehicleAirData();
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
InitializeVehicleAirspeed();
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
|
||||
InitializeVehicleGPSPosition();
|
||||
#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
|
||||
@@ -266,93 +266,6 @@ int Sensors::parameters_update()
|
||||
}
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
void Sensors::diff_pres_poll()
|
||||
{
|
||||
differential_pressure_s diff_pres{};
|
||||
|
||||
if (_diff_pres_sub.update(&diff_pres)) {
|
||||
|
||||
if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
|
||||
// ignore invalid data and reset accumulated
|
||||
|
||||
// reset
|
||||
_diff_pres_timestamp_sum = 0;
|
||||
_diff_pres_pressure_sum = 0;
|
||||
_diff_pres_temperature_sum = 0;
|
||||
_baro_pressure_sum = 0;
|
||||
_diff_pres_count = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
vehicle_air_data_s air_data{};
|
||||
_vehicle_air_data_sub.copy(&air_data);
|
||||
const float temperature = air_data.ambient_temperature;
|
||||
|
||||
// push raw data into validator
|
||||
float airspeed_input[3] { diff_pres.differential_pressure_pa, 0.0f, 0.0f };
|
||||
_airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
|
||||
|
||||
// accumulate average for publication
|
||||
_diff_pres_timestamp_sum += diff_pres.timestamp_sample;
|
||||
_diff_pres_pressure_sum += diff_pres.differential_pressure_pa;
|
||||
_baro_pressure_sum += air_data.baro_pressure_pa;
|
||||
_diff_pres_count++;
|
||||
|
||||
if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 50_ms) {
|
||||
|
||||
// average data and apply calibration offset (SENS_DPRES_OFF)
|
||||
const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count;
|
||||
const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa;
|
||||
const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count;
|
||||
|
||||
// reset
|
||||
_diff_pres_timestamp_sum = 0;
|
||||
_diff_pres_pressure_sum = 0;
|
||||
_baro_pressure_sum = 0;
|
||||
_diff_pres_count = 0;
|
||||
|
||||
|
||||
enum AIRSPEED_SENSOR_MODEL smodel;
|
||||
|
||||
switch ((diff_pres.device_id >> 16) & 0xFF) {
|
||||
case DRV_DIFF_PRESS_DEVTYPE_SDP31:
|
||||
|
||||
// fallthrough
|
||||
case DRV_DIFF_PRESS_DEVTYPE_SDP32:
|
||||
|
||||
// fallthrough
|
||||
case DRV_DIFF_PRESS_DEVTYPE_SDP33:
|
||||
smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
|
||||
break;
|
||||
|
||||
default:
|
||||
smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
|
||||
break;
|
||||
}
|
||||
|
||||
float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
|
||||
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
|
||||
differential_pressure_pa, baro_pressure_pa, temperature);
|
||||
|
||||
// assume that CAS = IAS as we don't have an CAS-scale here
|
||||
float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature);
|
||||
|
||||
if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
|
||||
|
||||
airspeed_s airspeed;
|
||||
airspeed.timestamp_sample = timestamp_sample;
|
||||
airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s;
|
||||
airspeed.true_airspeed_m_s = true_airspeed_m_s;
|
||||
airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
|
||||
airspeed.timestamp = hrt_absolute_time();
|
||||
_airspeed_pub.publish(airspeed);
|
||||
|
||||
_airspeed_last_publish = airspeed.timestamp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Sensors::adc_poll()
|
||||
{
|
||||
/* only read if not in HIL mode */
|
||||
@@ -388,8 +301,14 @@ void Sensors::adc_poll()
|
||||
if (voltage > 0.4f) {
|
||||
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale;
|
||||
|
||||
// the analogue sensor is not on a bus, but it still needs an identity so that it
|
||||
// can hold a calibration slot like any other airspeed sensor
|
||||
device::Device::DeviceId device_id{};
|
||||
device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_ANALOG;
|
||||
|
||||
differential_pressure_s diff_pres{};
|
||||
diff_pres.timestamp_sample = adc.timestamp;
|
||||
diff_pres.device_id = device_id.devid;
|
||||
diff_pres.differential_pressure_pa = diff_pres_pa_raw;
|
||||
diff_pres.temperature = NAN;
|
||||
diff_pres.timestamp = hrt_absolute_time();
|
||||
@@ -420,6 +339,32 @@ void Sensors::InitializeVehicleAirData()
|
||||
}
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
void Sensors::InitializeVehicleAirspeed()
|
||||
{
|
||||
// create a VehicleAirspeed instance for each advertised differential pressure sensor.
|
||||
for (uint8_t i = 0; i < calibration::DifferentialPressure::MAX_SENSOR_COUNT; i++) {
|
||||
if (_vehicle_airspeed_list[i] == nullptr) {
|
||||
|
||||
uORB::Subscription differential_pressure_sub{ORB_ID(differential_pressure), i};
|
||||
|
||||
if (differential_pressure_sub.advertised()) {
|
||||
VehicleAirspeed *airspeed = new VehicleAirspeed(i);
|
||||
|
||||
if (airspeed != nullptr) {
|
||||
if (airspeed->Start()) {
|
||||
_vehicle_airspeed_list[i] = airspeed;
|
||||
|
||||
} else {
|
||||
delete airspeed;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
|
||||
void Sensors::InitializeVehicleGPSPosition()
|
||||
{
|
||||
@@ -547,6 +492,16 @@ void Sensors::Run()
|
||||
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
const int n_diff_pres = orb_group_count(ORB_ID(differential_pressure));
|
||||
|
||||
if (n_diff_pres != _n_diff_pres) {
|
||||
_n_diff_pres = n_diff_pres;
|
||||
updated = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
|
||||
const int n_gnss = orb_group_count(ORB_ID(sensor_gnss));
|
||||
|
||||
@@ -619,7 +574,6 @@ void Sensors::Run()
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
// check analog airspeed
|
||||
adc_poll();
|
||||
diff_pres_poll();
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
@@ -704,7 +658,13 @@ int Sensors::print_status()
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
PX4_INFO_RAW("\n");
|
||||
PX4_INFO_RAW("Airspeed status:\n");
|
||||
_airspeed_validator.print();
|
||||
|
||||
for (auto &airspeed : _vehicle_airspeed_list) {
|
||||
if (airspeed) {
|
||||
airspeed->PrintStatus();
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
|
||||
|
||||
@@ -58,11 +58,10 @@
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
# include <drivers/drv_sensor.h>
|
||||
# include <lib/drivers/device/Device.hpp>
|
||||
# include <drivers/drv_adc.h>
|
||||
# include <lib/airspeed/airspeed.h>
|
||||
# include <uORB/topics/airspeed.h>
|
||||
# include <uORB/topics/differential_pressure.h>
|
||||
# include <uORB/topics/vehicle_air_data.h>
|
||||
# include "vehicle_airspeed/VehicleAirspeed.hpp"
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
|
||||
@@ -162,14 +161,6 @@ private:
|
||||
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
|
||||
/**
|
||||
* Poll the differential pressure sensor for updated data.
|
||||
*
|
||||
* @param raw Combined sensor data structure into which
|
||||
* data should be returned.
|
||||
*/
|
||||
void diff_pres_poll();
|
||||
|
||||
/**
|
||||
* Poll the ADC and update readings to suit.
|
||||
*
|
||||
@@ -178,48 +169,23 @@ private:
|
||||
*/
|
||||
void adc_poll();
|
||||
|
||||
uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)};
|
||||
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
|
||||
void InitializeVehicleAirspeed();
|
||||
|
||||
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
|
||||
|
||||
DataValidator _airspeed_validator; /**< data validator to monitor airspeed */
|
||||
|
||||
float _diff_pres_pressure_sum{0.f};
|
||||
float _diff_pres_temperature_sum{0.f};
|
||||
float _baro_pressure_sum{0.f};
|
||||
|
||||
int _diff_pres_count{0};
|
||||
|
||||
uint64_t _airspeed_last_publish{0};
|
||||
uint64_t _diff_pres_timestamp_sum{0};
|
||||
VehicleAirspeed *_vehicle_airspeed_list[calibration::DifferentialPressure::MAX_SENSOR_COUNT] {};
|
||||
uint8_t _n_diff_pres{0};
|
||||
|
||||
# ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};
|
||||
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)};
|
||||
# endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
|
||||
struct Parameters {
|
||||
float diff_pres_offset_pa;
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
float diff_pres_analog_scale;
|
||||
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
|
||||
|
||||
int32_t air_cmodel;
|
||||
float air_tube_length;
|
||||
float air_tube_diameter_mm;
|
||||
} _parameters{}; /**< local copies of interesting parameters */
|
||||
} _parameters {}; /**< local copies of interesting parameters */
|
||||
|
||||
struct ParameterHandles {
|
||||
param_t diff_pres_offset_pa;
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
param_t diff_pres_analog_scale;
|
||||
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
|
||||
|
||||
param_t air_cmodel;
|
||||
param_t air_tube_length;
|
||||
param_t air_tube_diameter_mm;
|
||||
} _parameter_handles{}; /**< handles for interesting parameters */
|
||||
} _parameter_handles {}; /**< handles for interesting parameters */
|
||||
# endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION)
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(vehicle_airspeed
|
||||
VehicleAirspeed.cpp
|
||||
VehicleAirspeed.hpp
|
||||
)
|
||||
target_link_libraries(vehicle_airspeed
|
||||
PRIVATE
|
||||
airspeed
|
||||
data_validator
|
||||
px4_work_queue
|
||||
sensor_calibration
|
||||
)
|
||||
@@ -0,0 +1,202 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "VehicleAirspeed.hpp"
|
||||
|
||||
#include <drivers/drv_sensor.h>
|
||||
#include <lib/airspeed/airspeed.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
VehicleAirspeed::VehicleAirspeed(uint8_t instance) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
||||
_instance(instance),
|
||||
_sensor_sub{this, ORB_ID(differential_pressure), instance}
|
||||
{
|
||||
_airspeed_validator.set_timeout(300000);
|
||||
_airspeed_validator.set_equal_value_threshold(100);
|
||||
|
||||
_airspeed_pub.advertise();
|
||||
}
|
||||
|
||||
VehicleAirspeed::~VehicleAirspeed()
|
||||
{
|
||||
Stop();
|
||||
perf_free(_cycle_perf);
|
||||
}
|
||||
|
||||
bool VehicleAirspeed::Start()
|
||||
{
|
||||
ParametersUpdate(true);
|
||||
|
||||
return _sensor_sub.registerCallback();
|
||||
}
|
||||
|
||||
void VehicleAirspeed::Stop()
|
||||
{
|
||||
Deinit();
|
||||
|
||||
_sensor_sub.unregisterCallback();
|
||||
}
|
||||
|
||||
void VehicleAirspeed::ResetAccumulation()
|
||||
{
|
||||
_timestamp_sample_sum = 0;
|
||||
_differential_pressure_sum = 0.f;
|
||||
_baro_pressure_sum = 0.f;
|
||||
_temperature_sum = 0.f;
|
||||
_sample_count = 0;
|
||||
}
|
||||
|
||||
void VehicleAirspeed::ParametersUpdate(bool force)
|
||||
{
|
||||
if (_parameter_update_sub.updated() || force) {
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
updateParams();
|
||||
|
||||
_calibration.ParametersUpdate();
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleAirspeed::Run()
|
||||
{
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
ParametersUpdate();
|
||||
|
||||
differential_pressure_s diff_pres;
|
||||
|
||||
while (_sensor_sub.update(&diff_pres)) {
|
||||
|
||||
if (_calibration.device_id() != diff_pres.device_id) {
|
||||
_calibration.set_device_id(diff_pres.device_id);
|
||||
|
||||
// the global SENS_DPRES_OFF only ever applied to the first sensor, so only that one
|
||||
// may take it over
|
||||
if ((_instance == 0) && _calibration.AdoptLegacyOffset()) {
|
||||
_calibration.ParametersSave();
|
||||
param_notify_changes();
|
||||
}
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
|
||||
// ignore invalid data and reset accumulated
|
||||
ResetAccumulation();
|
||||
continue;
|
||||
}
|
||||
|
||||
vehicle_air_data_s air_data{};
|
||||
_vehicle_air_data_sub.copy(&air_data);
|
||||
|
||||
// push raw data into validator
|
||||
float airspeed_input[3] { diff_pres.differential_pressure_pa, 0.0f, 0.0f };
|
||||
_airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100);
|
||||
|
||||
// accumulate average for publication
|
||||
_timestamp_sample_sum += diff_pres.timestamp_sample;
|
||||
_differential_pressure_sum += diff_pres.differential_pressure_pa;
|
||||
_baro_pressure_sum += air_data.baro_pressure_pa;
|
||||
_temperature_sum += air_data.ambient_temperature;
|
||||
_sample_count++;
|
||||
}
|
||||
|
||||
if ((_sample_count > 0) && (hrt_elapsed_time(&_last_publish) >= kPublishInterval)) {
|
||||
|
||||
// average the accumulated data and remove this sensor's zero offset (CAL_DPRESx_OFF)
|
||||
const uint64_t timestamp_sample = _timestamp_sample_sum / _sample_count;
|
||||
const float differential_pressure_pa = _calibration.Correct(_differential_pressure_sum / _sample_count);
|
||||
const float baro_pressure_pa = _baro_pressure_sum / _sample_count;
|
||||
const float temperature = _temperature_sum / _sample_count;
|
||||
|
||||
ResetAccumulation();
|
||||
|
||||
enum AIRSPEED_SENSOR_MODEL smodel;
|
||||
|
||||
switch ((_calibration.device_id() >> 16) & 0xFF) {
|
||||
case DRV_DIFF_PRESS_DEVTYPE_SDP31:
|
||||
|
||||
// fallthrough
|
||||
case DRV_DIFF_PRESS_DEVTYPE_SDP32:
|
||||
|
||||
// fallthrough
|
||||
case DRV_DIFF_PRESS_DEVTYPE_SDP33:
|
||||
smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
|
||||
break;
|
||||
|
||||
default:
|
||||
smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
|
||||
break;
|
||||
}
|
||||
|
||||
const float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_param_cal_air_cmodel.get(),
|
||||
smodel, _param_cal_air_tubelen.get(), _param_cal_air_tubed_mm.get(),
|
||||
differential_pressure_pa, baro_pressure_pa, temperature);
|
||||
|
||||
// assume that CAS = IAS as we don't have an CAS-scale here
|
||||
const float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature);
|
||||
|
||||
if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
|
||||
airspeed_s airspeed{};
|
||||
airspeed.timestamp_sample = timestamp_sample;
|
||||
airspeed.device_id = _calibration.device_id();
|
||||
airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s;
|
||||
airspeed.true_airspeed_m_s = true_airspeed_m_s;
|
||||
airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
|
||||
airspeed.timestamp = hrt_absolute_time();
|
||||
_airspeed_pub.publish(airspeed);
|
||||
|
||||
_last_publish = airspeed.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
void VehicleAirspeed::PrintStatus()
|
||||
{
|
||||
PX4_INFO_RAW("[vehicle_airspeed] instance %" PRIu8 ", airspeed instance %d\n", _instance,
|
||||
_airspeed_pub.get_instance());
|
||||
|
||||
_calibration.PrintStatus();
|
||||
_airspeed_validator.print();
|
||||
PX4_INFO_RAW("\n");
|
||||
|
||||
perf_print_counter(_cycle_perf);
|
||||
}
|
||||
|
||||
} // namespace sensors
|
||||
@@ -0,0 +1,116 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "data_validator/DataValidator.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/sensor_calibration/DifferentialPressure.hpp>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
/**
|
||||
* Converts the differential pressure of a single sensor into an airspeed measurement.
|
||||
*
|
||||
* One instance is created per advertised differential_pressure instance.
|
||||
* The airspeed selector is responsible for validating and choosing between them.
|
||||
*/
|
||||
class VehicleAirspeed : public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
explicit VehicleAirspeed(uint8_t instance);
|
||||
~VehicleAirspeed() override;
|
||||
|
||||
bool Start();
|
||||
void Stop();
|
||||
|
||||
void PrintStatus();
|
||||
|
||||
uint32_t device_id() const { return _calibration.device_id(); }
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
void ParametersUpdate(bool force = false);
|
||||
void ResetAccumulation();
|
||||
|
||||
static constexpr hrt_abstime kPublishInterval{50_ms};
|
||||
|
||||
const uint8_t _instance;
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_sub;
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
|
||||
|
||||
uORB::PublicationMulti<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
|
||||
|
||||
calibration::DifferentialPressure _calibration;
|
||||
|
||||
DataValidator _airspeed_validator;
|
||||
|
||||
// accumulated between publications so that the published airspeed is an average
|
||||
uint64_t _timestamp_sample_sum{0};
|
||||
float _differential_pressure_sum{0.f};
|
||||
float _baro_pressure_sum{0.f};
|
||||
float _temperature_sum{0.f};
|
||||
int _sample_count{0};
|
||||
|
||||
hrt_abstime _last_publish{0};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::CAL_AIR_CMODEL>) _param_cal_air_cmodel,
|
||||
(ParamFloat<px4::params::CAL_AIR_TUBELEN>) _param_cal_air_tubelen,
|
||||
(ParamFloat<px4::params::CAL_AIR_TUBED_MM>) _param_cal_air_tubed_mm
|
||||
)
|
||||
};
|
||||
|
||||
} // namespace sensors
|
||||
@@ -772,8 +772,13 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
|
||||
}
|
||||
|
||||
// TODO: send differential pressure instead?
|
||||
device::Device::DeviceId device_id{};
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
|
||||
device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM;
|
||||
|
||||
airspeed_s airspeed{};
|
||||
airspeed.timestamp_sample = time_now_us;
|
||||
airspeed.device_id = device_id.devid;
|
||||
|
||||
const Vector3f v_apparent_B = _q.rotateVectorInverse(_v_apparent_N);
|
||||
const float v_pitot = (_vehicle == VehicleType::TailsitterVTOL) ? -v_apparent_B(2) : v_apparent_B(0);
|
||||
|
||||
Reference in new issue
Block a user