fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 03:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added pitot tube icing detection (#23206)

Browse Source 
* lib: add FilteredDerivative class

* AirspeedValidator: add first principle check

- filters throttle, pitch and airspeed rate, and triggers
if the airspeed rate is negative even though the vehicle
is pitching down and giving high throttle.
Check has to fail for duration defined by ASPD_FP_T_WINDOW
to trigger an airspeed failure.

* AirspeedValidator: define constants for first principle check

* FilteredDerivative: set initialised to false if sample interval is invalid

* airspeed_selector: improved comment

* increase IAS derivative filter time constant from 4 to 5

* use legacy parameter handling for FW_PSP_OFF

* handle FW_THR_MAX as well

* ROMFS/airframes: exclude some airframes for v6x and v4pro to save flash on them

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Roman Bapst
2024-07-09 11:16:40 +02:00
committed by GitHub
co-authored by Silvan Fuhrer
parent a35cecece4
commit 8221940b60
11 changed files with 278 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+41 -2
View File
@@ -57,6 +57,7 @@
#include <uORB/topics/position_setpoint.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -112,6 +113,7 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
@@ -125,6 +127,7 @@ private:
uORB::SubscriptionMultiArray<airspeed_s, MAX_NUM_AIRSPEED_SENSORS> _airspeed_subs{ORB_ID::airspeed};
tecs_status_s _tecs_status {};
estimator_status_s _estimator_status {};
vehicle_acceleration_s _accel {};
vehicle_air_data_s _vehicle_air_data {};
@@ -162,9 +165,16 @@ private:
CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0),
CHECK_TYPE_DATA_STUCK_BIT = (1 << 1),
CHECK_TYPE_INNOVATION_BIT = (1 << 2),
CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3)
CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3),
CHECK_TYPE_FIRST_PRINCIPLE_BIT = (1 << 4)
};
param_t _param_handle_pitch_sp_offset{PARAM_INVALID};
float _param_pitch_sp_offset{0.0f};
param_t _param_handle_fw_thr_max{PARAM_INVALID};
float _param_fw_thr_max{0.0f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ASPD_WIND_NSD>) _param_aspd_wind_nsd,
(ParamFloat<px4::params::ASPD_SCALE_NSD>) _param_aspd_scale_nsd,
@@ -185,8 +195,12 @@ private:
(ParamFloat<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
(ParamFloat<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**< delay to declare airspeed valid again */
(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas,
(ParamFloat<px4::params::ASPD_FP_T_WINDOW>) _aspd_fp_t_window,
// external parameters
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim
)
void init(); /**< initialization of the airspeed validator instances */
@@ -203,6 +217,8 @@ AirspeedModule::AirspeedModule():
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_param_handle_pitch_sp_offset = param_find("FW_PSP_OFF");
_param_handle_fw_thr_max = param_find("FW_THR_MAX");
// initialise parameters
update_params();
@@ -355,6 +371,9 @@ AirspeedModule::Run()
input_data.accel_z = _accel.xyz[2];
input_data.vel_test_ratio = _estimator_status.vel_test_ratio;
input_data.mag_test_ratio = _estimator_status.mag_test_ratio;
input_data.tecs_timestamp = _tecs_status.timestamp;
input_data.fixed_wing_tecs_throttle = _tecs_status.throttle_sp;
input_data.fixed_wing_tecs_throttle_trim = _tecs_status.throttle_trim;
// iterate through all airspeed sensors, poll new data from them and update their validators
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
@@ -442,6 +461,14 @@ void AirspeedModule::update_params()
{
updateParams();
if (_param_handle_pitch_sp_offset != PARAM_INVALID) {
param_get(_param_handle_pitch_sp_offset, &_param_pitch_sp_offset);
}
if (_param_handle_fw_thr_max != PARAM_INVALID) {
param_get(_param_handle_fw_thr_max, &_param_fw_thr_max);
}
_param_airspeed_scale[0] = _param_airspeed_scale_1.get();
_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
@@ -476,6 +503,11 @@ void AirspeedModule::update_params()
CheckTypeBits::CHECK_TYPE_INNOVATION_BIT);
_airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() &
CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT);
_airspeed_validator[i].set_enable_first_principle_check(_param_airspeed_checks_on.get() &
CheckTypeBits::CHECK_TYPE_FIRST_PRINCIPLE_BIT);
_airspeed_validator[i].set_psp_off_param(math::radians(_param_pitch_sp_offset));
_airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max);
_airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get());
}
}
@@ -501,6 +533,8 @@ void AirspeedModule::poll_topics()
_vehicle_local_position_sub.update(&_vehicle_local_position);
_position_setpoint_sub.update(&_position_setpoint);
_tecs_status_sub.update(&_tecs_status);
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude;
_vehicle_attitude_sub.update(&vehicle_attitude);
@@ -661,6 +695,11 @@ void AirspeedModule::select_airspeed_and_publish()
airspeed_validated.airspeed_sensor_measurement_valid = false;
airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
airspeed_validated.airspeed_derivative_filtered = _airspeed_validator[_valid_airspeed_index -
1].get_airspeed_derivative();
airspeed_validated.throttle_filtered = _airspeed_validator[_valid_airspeed_index - 1].get_throttle_filtered();
airspeed_validated.pitch_filtered = _airspeed_validator[_valid_airspeed_index - 1].get_pitch_filtered();
switch (_valid_airspeed_index) {
case airspeed_index::DISABLED_INDEX:
break;