From 5842c991ecc1564f3ca96ab40560684a6352b8f8 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Fri, 4 Apr 2025 16:52:05 +0200 Subject: [PATCH] AirspeedSelector: add synthetic airspeed option Synthetic airspeed is calculated based on the thrust setpoint and the thrust<->airpseed model as configured in the parameters. --- .../reptile_dragon_2_params.params | 4 +- .../turbo_timber_evolution/tteparams.params | 4 +- msg/px4_msgs_old/msg/AirspeedValidatedV0.msg | 18 +++ .../translations/all_translations.h | 1 + .../translation_airspeed_validated_v1.h | 51 +++++++ msg/versioned/AirspeedValidated.msg | 24 ++-- src/drivers/osd/msp_osd/uorb_to_msp.cpp | 4 +- src/lib/airspeed/airspeed.cpp | 5 + src/lib/airspeed/airspeed.h | 2 + src/lib/parameters/param_translation.cpp | 8 ++ .../airspeed_selector/AirspeedValidator.cpp | 9 +- .../airspeed_selector/AirspeedValidator.hpp | 4 +- .../airspeed_selector_main.cpp | 130 ++++++++++++++---- .../airspeed_selector_params.c | 14 +- 14 files changed, 222 insertions(+), 56 deletions(-) create mode 100644 msg/px4_msgs_old/msg/AirspeedValidatedV0.msg create mode 100644 msg/translation_node/translations/translation_airspeed_validated_v1.h diff --git a/docs/assets/airframes/fw/reptile_dragon_2/reptile_dragon_2_params.params b/docs/assets/airframes/fw/reptile_dragon_2/reptile_dragon_2_params.params index a683709db7..e2956fa478 100644 --- a/docs/assets/airframes/fw/reptile_dragon_2/reptile_dragon_2_params.params +++ b/docs/assets/airframes/fw/reptile_dragon_2/reptile_dragon_2_params.params @@ -1,5 +1,5 @@ param set-default ASPD_DO_CHECKS 19 -param set-default ASPD_FALLBACK_GW 1 +param set-default ASPD_FALLBACK 1 param set-default BAT1_N_CELLS 4 param set-default BAT1_V_CHARGED 4.2000 param set-default BAT1_V_EMPTY 2.9000 @@ -61,4 +61,4 @@ param set-default PWM_AUX_TIM3 100 param set-default SDLOG_PROFILE 17 param set-default SENS_EN_MS4525DO 1 param set-default TRIM_PITCH 0.1000 -# Make sure to add all params from the current airframe (ID=2100) as well \ No newline at end of file +# Make sure to add all params from the current airframe (ID=2100) as well diff --git a/docs/assets/airframes/fw/turbo_timber_evolution/tteparams.params b/docs/assets/airframes/fw/turbo_timber_evolution/tteparams.params index 3632f1d45e..5d1d81b84b 100644 --- a/docs/assets/airframes/fw/turbo_timber_evolution/tteparams.params +++ b/docs/assets/airframes/fw/turbo_timber_evolution/tteparams.params @@ -1,5 +1,5 @@ param set-default ASPD_DO_CHECKS 19 -param set-default ASPD_FALLBACK_GW 1 +param set-default ASPD_FALLBACK 1 param set-default BAT1_N_CELLS 4 param set-default BAT1_V_CHARGED 4.2000 param set-default BAT1_V_EMPTY 2.5000 @@ -80,4 +80,4 @@ param set-default SENS_EN_MS4525DO 1 param set-default SER_TEL1_BAUD 115200 param set-default TRIM_PITCH -0.4000 param set-default UAVCAN_ENABLE 0 -# Make sure to add all params from the current airframe (ID=2100) as well \ No newline at end of file +# Make sure to add all params from the current airframe (ID=2100) as well diff --git a/msg/px4_msgs_old/msg/AirspeedValidatedV0.msg b/msg/px4_msgs_old/msg/AirspeedValidatedV0.msg new file mode 100644 index 0000000000..cc46e193bf --- /dev/null +++ b/msg/px4_msgs_old/msg/AirspeedValidatedV0.msg @@ -0,0 +1,18 @@ +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid +float32 calibrated_airspeed_m_s # calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid +float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid + +float32 calibrated_ground_minus_wind_m_s # CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid +float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid + +bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid. + +int8 selected_airspeed_index # 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid + +float32 airspeed_derivative_filtered # filtered indicated airspeed derivative [m/s/s] +float32 throttle_filtered # filtered fixed-wing throttle [-] +float32 pitch_filtered # filtered pitch [rad] diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 4369a3c58a..7564a9f83d 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -11,3 +11,4 @@ //#include "example_translation_service_v1.h" #include "translation_vehicle_status_v1.h" +#include "translation_airspeed_validated_v1.h" diff --git a/msg/translation_node/translations/translation_airspeed_validated_v1.h b/msg/translation_node/translations/translation_airspeed_validated_v1.h new file mode 100644 index 0000000000..005c280378 --- /dev/null +++ b/msg/translation_node/translations/translation_airspeed_validated_v1.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate AirspeedValidated v0 <--> v1 +#include +#include + +class AirspeedValidatedV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::AirspeedValidatedV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::AirspeedValidated; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/airspeed_validated"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + msg_newer.timestamp = msg_older.timestamp; + msg_newer.indicated_airspeed_m_s = msg_older.indicated_airspeed_m_s; + msg_newer.calibrated_airspeed_m_s = msg_older.calibrated_airspeed_m_s; + msg_newer.true_airspeed_m_s = msg_older.true_airspeed_m_s; + msg_newer.airspeed_source = msg_older.selected_airspeed_index; + msg_newer.calibrated_ground_minus_wind_m_s = msg_older.calibrated_ground_minus_wind_m_s; + msg_newer.calibraded_airspeed_synth_m_s = NAN; + msg_newer.airspeed_derivative_filtered = msg_older.airspeed_derivative_filtered; + msg_newer.throttle_filtered = msg_older.throttle_filtered; + msg_newer.pitch_filtered = msg_older.pitch_filtered; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + msg_older.timestamp = msg_newer.timestamp; + msg_older.indicated_airspeed_m_s = msg_newer.indicated_airspeed_m_s; + msg_older.calibrated_airspeed_m_s = msg_newer.calibrated_airspeed_m_s; + msg_older.true_airspeed_m_s = msg_newer.true_airspeed_m_s; + msg_older.calibrated_ground_minus_wind_m_s = msg_newer.calibrated_ground_minus_wind_m_s; + msg_older.true_ground_minus_wind_m_s = msg_newer.calibrated_ground_minus_wind_m_s; + msg_older.airspeed_sensor_measurement_valid = msg_newer.airspeed_source > 0 && msg_newer.airspeed_source <= 3; + msg_older.selected_airspeed_index = msg_newer.airspeed_source; + msg_older.airspeed_derivative_filtered = msg_newer.airspeed_derivative_filtered; + msg_older.throttle_filtered = msg_newer.throttle_filtered; + msg_older.pitch_filtered = msg_newer.pitch_filtered; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(AirspeedValidatedV1Translation); diff --git a/msg/versioned/AirspeedValidated.msg b/msg/versioned/AirspeedValidated.msg index cc46e193bf..a54e1510b6 100644 --- a/msg/versioned/AirspeedValidated.msg +++ b/msg/versioned/AirspeedValidated.msg @@ -1,18 +1,22 @@ -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) -float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid -float32 calibrated_airspeed_m_s # calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid -float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid +float32 indicated_airspeed_m_s # [m/s] Indicated airspeed (IAS), set to NAN if invalid +float32 calibrated_airspeed_m_s # [m/s] Calibrated airspeed (CAS), set to NAN if invalid +float32 true_airspeed_m_s # [m/s] True airspeed (TAS), set to NAN if invalid +int8 airspeed_source # Source of currently published airspeed values +int8 DISABLED = -1 +int8 GROUND_MINUS_WIND = 0 +int8 SENSOR_1 = 1 +int8 SENSOR_2 = 2 +int8 SENSOR_3 = 3 +int8 SYNTHETIC = 4 + +# debug states float32 calibrated_ground_minus_wind_m_s # CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid -float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid - -bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid. - -int8 selected_airspeed_index # 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid - +float32 calibraded_airspeed_synth_m_s # synthetic airspeed in m/s, set to NAN if invalid float32 airspeed_derivative_filtered # filtered indicated airspeed derivative [m/s/s] float32 throttle_filtered # filtered fixed-wing throttle [-] float32 pitch_filtered # filtered pitch [rad] diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index 36adc16e8e..121b9ee778 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -267,9 +267,9 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, //raw_gps.hdop = vehicle_gps_position_struct.hdop raw_gps.numSat = vehicle_gps_position.satellites_used; - if (airspeed_validated.airspeed_sensor_measurement_valid + if (airspeed_validated.airspeed_source >= airspeed_validated_s::GROUND_MINUS_WIND && PX4_ISFINITE(airspeed_validated.indicated_airspeed_m_s) - && airspeed_validated.indicated_airspeed_m_s > 0) { + && airspeed_validated.indicated_airspeed_m_s > 0.f) { raw_gps.groundSpeed = airspeed_validated.indicated_airspeed_m_s * 100; } else { diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 1179fe6fd6..40677569a2 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -214,3 +214,8 @@ float calc_calibrated_from_true_airspeed(float speed_true, float air_density) { return speed_true * sqrtf(air_density / kAirDensitySeaLevelStandardAtmos); } + +float calc_true_from_calibrated_airspeed(float speed_calibrated, float air_density) +{ + return speed_calibrated * sqrtf(kAirDensitySeaLevelStandardAtmos / air_density); +} diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index 6b5161c5dd..a5c0c14fde 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -137,6 +137,8 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe */ __EXPORT float calc_calibrated_from_true_airspeed(float speed_true, float air_density); +__EXPORT float calc_true_from_calibrated_airspeed(float speed_calibrated, float air_density); + __END_DECLS #endif diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 490ad02f22..01879113d6 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -142,5 +142,13 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node) } } + // 2025-03-19: translate ASPD_FALLBACK_GW to ASPD_FALLBACK + { + if (strcmp("ASPD_FALLBACK_GW", node->name) == 0) { + strcpy(node->name, "ASPD_FALLBACK"); + PX4_INFO("copying %s -> %s", "ASPD_FALLBACK_GW", "ASPD_FALLBACK"); + } + } + return param_modify_on_import_ret::PARAM_NOT_MODIFIED; } diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index acdc88ef22..bbd907ff52 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -60,7 +60,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat check_load_factor(input_data.accel_z); check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.hdg_test_ratio, input_data.ground_velocity, input_data.gnss_valid); - check_first_principle(input_data.timestamp, input_data.fixed_wing_tecs_throttle, + check_first_principle(input_data.timestamp, input_data.fixed_wing_throttle_filtered, input_data.fixed_wing_tecs_throttle_trim, input_data.tecs_timestamp, input_data.q_att); update_airspeed_valid_status(input_data.timestamp); } @@ -298,31 +298,28 @@ AirspeedValidator::check_first_principle(const uint64_t timestamp, const float t return; } - const float dt = static_cast(timestamp - _time_last_first_principle_check) / 1_s; + const float dt = static_cast(timestamp - _time_last_first_principle_check) * 1e-6f; _time_last_first_principle_check = timestamp; // update filters if (dt < FLT_EPSILON || dt > 1.f) { // reset if dt is too large _IAS_derivative.reset(0.f); - _throttle_filtered.reset(throttle_fw); _pitch_filtered.reset(pitch); _time_last_first_principle_check_passing = timestamp; } else { // update filters, with different time constant _IAS_derivative.setParameters(dt, 5.f); - _throttle_filtered.setParameters(dt, 0.5f); _pitch_filtered.setParameters(dt, 1.5f); _IAS_derivative.update(_IAS); - _throttle_filtered.update(throttle_fw); _pitch_filtered.update(pitch); } // declare high throttle if more than 5% above trim const float high_throttle_threshold = math::min(throttle_trim + kHighThrottleDelta, _param_throttle_max); - const bool high_throttle = _throttle_filtered.getState() > high_throttle_threshold; + const bool high_throttle = throttle_fw > high_throttle_threshold; const bool pitching_down = _pitch_filtered.getState() < _param_psp_off; // check if the airspeed derivative is too low given the throttle and pitch diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 74f5747989..ec78b9ef8f 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -68,7 +68,7 @@ struct airspeed_validator_update_data { float vel_test_ratio; float hdg_test_ratio; bool in_fixed_wing_flight; - float fixed_wing_tecs_throttle; + float fixed_wing_throttle_filtered; float fixed_wing_tecs_throttle_trim; uint64_t tecs_timestamp; }; @@ -89,7 +89,6 @@ public: bool get_airspeed_valid() { return _airspeed_valid; } float get_CAS_scale_validated() {return _CAS_scale_validated;} float get_airspeed_derivative() { return _IAS_derivative.getState(); } - float get_throttle_filtered() { return _throttle_filtered.getState(); } float get_pitch_filtered() { return _pitch_filtered.getState(); } airspeed_wind_s get_wind_estimator_states(uint64_t timestamp); @@ -181,7 +180,6 @@ private: bool _first_principle_check_failed{false}; ///< first principle check has detected failure float _aspd_fp_t_window{0.f}; ///< time window for first principle check FilteredDerivative _IAS_derivative; ///< indicated airspeed derivative for first principle check - AlphaFilter _throttle_filtered; ///< filtered throttle for first principle check AlphaFilter _pitch_filtered; ///< filtered pitch for first principle check hrt_abstime _time_last_first_principle_check{0}; ///< time airspeed first principle was last checked (uSec) hrt_abstime _time_last_first_principle_check_passing{0}; ///< time airspeed first principle was last passing (uSec) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 9304398db7..b3209e2f33 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -65,12 +66,14 @@ #include #include #include -#include +#include #include +#include using namespace time_literals; static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */ +static constexpr float _kThrottleFilterTimeConstant{0.5f}; using matrix::Dcmf; using matrix::Quatf; @@ -128,6 +131,7 @@ private: uORB::Subscription _position_setpoint_sub{ORB_ID(position_setpoint)}; uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; uORB::SubscriptionMultiArray _airspeed_subs{ORB_ID::airspeed}; + uORB::SubscriptionData _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)}; tecs_status_s _tecs_status {}; @@ -177,6 +181,9 @@ private: param_t _param_handle_fw_thr_max{PARAM_INVALID}; float _param_fw_thr_max{0.0f}; + AlphaFilter _throttle_filtered{_kThrottleFilterTimeConstant}; + uint64_t _t_last_throttle_fw{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_aspd_wind_nsd, (ParamFloat) _param_aspd_scale_nsd, @@ -190,7 +197,7 @@ private: (ParamFloat) _param_airspeed_scale_3, (ParamInt) _param_airspeed_primary_index, (ParamInt) _param_airspeed_checks_on, - (ParamInt) _param_airspeed_fallback_gw, + (ParamInt) _param_airspeed_fallback, (ParamFloat) _tas_innov_threshold, /**< innovation check threshold */ (ParamFloat) _tas_innov_integ_threshold, /**< innovation check integrator threshold */ @@ -202,7 +209,12 @@ private: // external parameters (ParamFloat) _param_fw_airspd_stall, - (ParamFloat) _param_fw_airspd_trim + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_trim, + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_thr_aspd_min, + (ParamFloat) _param_fw_thr_trim, + (ParamFloat) _param_fw_thr_aspd_max ) void init(); /**< initialization of the airspeed validator instances */ @@ -212,6 +224,8 @@ private: void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */ void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */ void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */ + float get_synthetic_airspeed(float throttle); + void update_throttle_filter(hrt_abstime t_now); }; AirspeedModule::AirspeedModule(): @@ -350,6 +364,7 @@ AirspeedModule::Run() poll_topics(); update_wind_estimator_sideslip(); update_ground_minus_wind_airspeed(); + update_throttle_filter(_time_now_usec); if (_number_of_airspeed_sensors > 0) { @@ -375,7 +390,7 @@ AirspeedModule::Run() input_data.vel_test_ratio = _estimator_status.vel_test_ratio; input_data.hdg_test_ratio = _estimator_status.hdg_test_ratio; input_data.tecs_timestamp = _tecs_status.timestamp; - input_data.fixed_wing_tecs_throttle = _tecs_status.throttle_sp; + input_data.fixed_wing_throttle_filtered = _throttle_filtered.getState(); 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 @@ -653,6 +668,11 @@ void AirspeedModule::select_airspeed_and_publish() || _param_airspeed_primary_index.get() == static_cast(AirspeedSource::GROUND_MINUS_WIND))) { _valid_airspeed_src = AirspeedSource::GROUND_MINUS_WIND; + } else if ((_param_airspeed_fallback.get() == 2 + || _param_airspeed_primary_index.get() == static_cast(AirspeedSource::SYNTHETIC)) + && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _valid_airspeed_src = AirspeedSource::SYNTHETIC; + } else { _valid_airspeed_src = AirspeedSource::DISABLED; @@ -691,35 +711,34 @@ void AirspeedModule::select_airspeed_and_publish() "Airspeed estimation valid"); } else { - mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor healthy, start using again (%i, %i)\t", _prev_airspeed_index, - _valid_airspeed_index); + mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor healthy, start using again (%i, %i)\t", prev_airspeed_index, + valid_airspeed_index); /* EVENT * @description Previously selected sensor index: {1}, current sensor index: {2}. */ - events::send(events::ID("airspeed_selector_estimation_regain"), events::Log::Info, - "Airspeed sensor healthy, start using again", _prev_airspeed_index, - _valid_airspeed_index); + events::send(events::ID("airspeed_selector_estimation_regain"), events::Log::Critical, + "Airspeed sensor healthy, start using again", prev_airspeed_index, + valid_airspeed_index); } } - _prev_airspeed_index = _valid_airspeed_index; _prev_number_of_airspeed_sensors = _number_of_airspeed_sensors; airspeed_validated_s airspeed_validated = {}; airspeed_validated.timestamp = _time_now_usec; - airspeed_validated.true_ground_minus_wind_m_s = NAN; airspeed_validated.calibrated_ground_minus_wind_m_s = NAN; + airspeed_validated.calibraded_airspeed_synth_m_s = NAN; airspeed_validated.indicated_airspeed_m_s = NAN; airspeed_validated.calibrated_airspeed_m_s = NAN; airspeed_validated.true_airspeed_m_s = NAN; - 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(); + airspeed_validated.airspeed_derivative_filtered = _airspeed_validator[valid_airspeed_index - + 1].get_airspeed_derivative(); + airspeed_validated.throttle_filtered = _throttle_filtered.getState(); + airspeed_validated.pitch_filtered = _airspeed_validator[valid_airspeed_index - 1].get_pitch_filtered(); + airspeed_validated.airspeed_source = valid_airspeed_index; + _prev_airspeed_src = _valid_airspeed_src; switch (_valid_airspeed_src) { case AirspeedSource::DISABLED: @@ -730,17 +749,28 @@ void AirspeedModule::select_airspeed_and_publish() airspeed_validated.calibrated_airspeed_m_s = _ground_minus_wind_CAS; airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS; airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS; - airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS; + airspeed_validated.calibraded_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered); break; + case AirspeedSource::SYNTHETIC: { + airspeed_validated.throttle_filtered = _throttle_filtered.getState(); + airspeed_validated.pitch_filtered = _airspeed_validator[0].get_pitch_filtered(); + float synthetic_airspeed = get_synthetic_airspeed(airspeed_validated.throttle_filtered); + airspeed_validated.calibrated_airspeed_m_s = synthetic_airspeed; + airspeed_validated.indicated_airspeed_m_s = synthetic_airspeed; + airspeed_validated.calibraded_airspeed_synth_m_s = synthetic_airspeed; + airspeed_validated.true_airspeed_m_s = + calc_true_from_calibrated_airspeed(synthetic_airspeed, _vehicle_air_data.rho); + break; + } + default: - airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_IAS(); - airspeed_validated.calibrated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_CAS(); - airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_TAS(); + airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[valid_airspeed_index - 1].get_IAS(); + airspeed_validated.calibrated_airspeed_m_s = _airspeed_validator[valid_airspeed_index - 1].get_CAS(); + airspeed_validated.true_airspeed_m_s = _airspeed_validator[valid_airspeed_index - 1].get_TAS(); airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS; - airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS; - airspeed_validated.airspeed_sensor_measurement_valid = true; + airspeed_validated.calibraded_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered); break; } @@ -767,6 +797,60 @@ void AirspeedModule::select_airspeed_and_publish() } +float AirspeedModule::get_synthetic_airspeed(float throttle) +{ + float synthetic_airspeed; + _flight_phase_estimation_sub.update(); + flight_phase_estimation_s flight_phase_estimation = _flight_phase_estimation_sub.get(); + + if (flight_phase_estimation.flight_phase != flight_phase_estimation_s::FLIGHT_PHASE_LEVEL + || _time_now_usec - flight_phase_estimation.timestamp > 1_s) { + synthetic_airspeed = _param_fw_airspd_trim.get(); + + } else if (throttle < _param_fw_thr_trim.get() && _param_fw_thr_aspd_min.get() > 0.f) { + synthetic_airspeed = interpolate(throttle, _param_fw_thr_aspd_min.get(), + _param_fw_thr_trim.get(), + _param_fw_airspd_min.get(), _param_fw_airspd_trim.get()); + + } else if (throttle > _param_fw_thr_trim.get() && _param_fw_thr_aspd_max.get() > 0.f) { + synthetic_airspeed = interpolate(throttle, _param_fw_thr_trim.get(), + _param_fw_thr_aspd_max.get(), + _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()); + + } else { + synthetic_airspeed = _param_fw_airspd_trim.get(); + } + + return synthetic_airspeed; +} + +void AirspeedModule::update_throttle_filter(hrt_abstime now) +{ + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint_0{}; + _vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint_0); + + float forward_thrust = vehicle_thrust_setpoint_0.xyz[0]; + + // if VTOL, use the total thrust vector length (otherwise needs special handling for tailsitters and tiltrotors) + if (_vehicle_status.is_vtol) { + forward_thrust = sqrtf(vehicle_thrust_setpoint_0.xyz[0] * vehicle_thrust_setpoint_0.xyz[0] + + vehicle_thrust_setpoint_0.xyz[1] * vehicle_thrust_setpoint_0.xyz[1] + + vehicle_thrust_setpoint_0.xyz[2] * vehicle_thrust_setpoint_0.xyz[2]); + } + + const float dt = static_cast(now - _t_last_throttle_fw) * 1e-6f; + _t_last_throttle_fw = now; + + if (dt < FLT_EPSILON || dt > 1.f) { + _throttle_filtered.reset(forward_thrust); + + } else { + _throttle_filtered.update(forward_thrust, dt); + } + } +} + int AirspeedModule::custom_command(int argc, char *argv[]) { if (!is_running()) { diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 0462fbbc34..a5e479a9f2 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -136,6 +136,7 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_3, 1.0f); * @value 1 First airspeed sensor * @value 2 Second airspeed sensor * @value 3 Third airspeed sensor + * @value 4 Thrust based airspeed * * @reboot_required true * @group Airspeed Validator @@ -160,17 +161,14 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1); PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7); /** - * Enable fallback to sensor-less airspeed estimation + * Fallback options * - * If set to true and airspeed checks are enabled, it will use a sensor-less airspeed estimation based on groundspeed - * minus windspeed if no other airspeed sensor available to fall back to. - * - * @value 0 Disable fallback to sensor-less estimation - * @value 1 Enable fallback to sensor-less estimation - * @boolean + * @value 0 Fallback only to other airspeed sensors + * @value 1 Fallback to groundspeed-minus-windspeed airspeed estimation + * @value 2 Fallback to thrust based airspeed estimation * @group Airspeed Validator */ -PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0); +PARAM_DEFINE_INT32(ASPD_FALLBACK, 0); /** * Airspeed failure innovation threshold