fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 00:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: schedule airspeed selector

Browse Source
This commit is contained in:
Matthias Grob
2026-02-18 15:16:00 +01:00
parent 68428821a0
commit 7b7da2812e
4 changed files with 60 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/airspeed_selector/AirspeedSelector.cpp
+12 -73
View File
@@ -58,30 +58,20 @@ AirspeedSelector::AirspeedSelector():
AirspeedSelector::~AirspeedSelector()
{
ScheduleClear();
perf_free(_perf_elapsed);
}
int
AirspeedSelector::task_spawn(int argc, char *argv[])
void AirspeedSelector::Start()
{
AirspeedSelector *dev = new AirspeedSelector();
// check if the trampoline is called for the first time
if (!dev) {
PX4_ERR("alloc failed");
return PX4_ERROR;
}
_object.store(dev);
dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
_task_id = task_id_is_work_queue;
return PX4_OK;
ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
}
void
AirspeedSelector::init()
void AirspeedSelector::Stop()
{
ScheduleClear();
Deinit();
}
void AirspeedSelector::init()
{
check_for_connected_airspeed_sensors();
@@ -111,8 +101,7 @@ AirspeedSelector::init()
_prev_airspeed_src = _valid_airspeed_src;
}
void
AirspeedSelector::check_for_connected_airspeed_sensors()
void AirspeedSelector::check_for_connected_airspeed_sensors()
{
// check for new connected airspeed sensor
int detected_airspeed_sensors = 0;
@@ -135,9 +124,7 @@ AirspeedSelector::check_for_connected_airspeed_sensors()
_number_of_airspeed_sensors = detected_airspeed_sensors;
}
void
AirspeedSelector::Run()
void AirspeedSelector::Run()
{
_time_now_usec = hrt_absolute_time(); // hrt time of the current cycle
@@ -288,10 +275,6 @@ AirspeedSelector::Run()
_armed_prev = armed;
perf_end(_perf_elapsed);
if (should_exit()) {
exit_and_cleanup();
}
}
void AirspeedSelector::update_params()
@@ -676,47 +659,3 @@ void AirspeedSelector::update_throttle_filter(hrt_abstime now)
}
}
}
int AirspeedSelector::custom_command(int argc, char *argv[])
{
if (!is_running()) {
int ret = AirspeedSelector::task_spawn(argc, argv);
if (ret) {
return ret;
}
}
return print_usage("unknown command");
}
int AirspeedSelector::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module provides a single airspeed_validated topic, containing indicated (IAS),
calibrated (CAS), true airspeed (TAS) and the information if the estimation currently
is invalid and if based sensor readings or on groundspeed minus windspeed.
Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
to a valid sensor in case of failure detection. For failure detection as well as for
the estimation of a scale factor from IAS to CAS, it runs several wind estimators
and also publishes those.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[])
{
return AirspeedSelector::main(argc, argv);
}
src/modules/sensors/airspeed_selector/AirspeedSelector.hpp
+17 -13
View File
@@ -31,6 +31,20 @@
*
****************************************************************************/
/**
* @file AirspeedSelector.hpp
*
* This class provides a single airspeed_validated topic, containing indicated (IAS),
* calibrated (CAS), true airspeed (TAS) and the information if the estimation currently
* is invalid and if based sensor readings or on groundspeed minus windspeed.
* Supporting the input of multiple "raw" airspeed inputs, this class automatically switches
* to a valid sensor in case of failure detection. For failure detection as well as for
* the estimation of a scale factor from IAS to CAS, it runs several wind estimators
* and also publishes those.
*
* @author Silvan Fuhrer <silvan@auterion.com>
*/
#pragma once
#include "AirspeedValidator.hpp"
@@ -41,7 +55,6 @@
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/airspeed/airspeed.h>
@@ -72,23 +85,14 @@
#include <uORB/topics/airspeed_wind.h>
#include <uORB/topics/flight_phase_estimation.h>
class AirspeedSelector : public ModuleBase<AirspeedSelector>, public ModuleParams,
public px4::ScheduledWorkItem
class AirspeedSelector : public ModuleParams, public px4::ScheduledWorkItem
{
public:
AirspeedSelector();
~AirspeedSelector() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
void Start();
void Stop();
private:
static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */
src/modules/sensors/sensors.cpp
+24
View File
@@ -96,6 +96,15 @@ Sensors::~Sensors()
sub.unregisterCallback();
}
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
if (_airspeed_selector) {
_airspeed_selector->Stop();
delete _airspeed_selector;
}
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION)
_vehicle_acceleration.Stop();
#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION
@@ -397,6 +406,21 @@ void Sensors::adc_poll()
}
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED)
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
void Sensors::InitializeAirspeedSelector()
{
if (_param_sys_has_num_aspd.get()) {
if (_airspeed_selector == nullptr) {
_airspeed_selector = new AirspeedSelector();
if (_airspeed_selector) {
_airspeed_selector->Start();
}
}
}
}
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
void Sensors::InitializeVehicleAirData()
{
src/modules/sensors/sensors.hpp
+7 -5
View File
@@ -63,6 +63,7 @@
# include <uORB/topics/airspeed.h>
# include <uORB/topics/differential_pressure.h>
# include <uORB/topics/vehicle_air_data.h>
# include "airspeed_selector/AirspeedSelector.hpp"
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
@@ -115,17 +116,13 @@ public:
bool init();
private:
int parameters_update();
void InitializeAirspeedSelector();
void InitializeVehicleAirData();
void InitializeVehicleGPSPosition();
void InitializeVehicleIMU();
void InitializeVehicleMagnetometer();
void InitializeVehicleOpticalFlow();
const bool _hil_enabled; /**< if true, HIL is active */
@@ -159,6 +156,8 @@ private:
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
AirspeedSelector *_airspeed_selector {nullptr};
/**
* Poll the differential pressure sensor for updated data.
*
@@ -248,6 +247,9 @@ private:
#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
DEFINE_PARAMETERS(
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
(ParamBool<px4::params::SYS_HAS_NUM_ASPD>) _param_sys_has_num_aspd,
#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA