diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index e63a9eef01bb..c1b3c9a9baeb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -7,6 +7,7 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board px4_fmu-v6x exclude # . ${R}etc/init.d/rc.mc_defaults 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 a683709db783..e2956fa478c2 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 3632f1d45ec9..5d1d81b84b75 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 000000000000..cc46e193bf5b --- /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 4369a3c58aca..7564a9f83d46 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 000000000000..005c280378c5 --- /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 cc46e193bf5b..a54e1510b6a3 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 -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 +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 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 36adc16e8eb4..121b9ee7782e 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 1179fe6fd6cc..40677569a23c 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 6b5161c5ddde..a5c0c14fdea6 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 490ad02f22c3..01879113d679 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 acdc88ef2219..bbd907ff5274 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 74f5747989f6..ec78b9ef8f24 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 @@ class AirspeedValidator 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 @@ class AirspeedValidator 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 5d998a58f2b9..b3209e2f3342 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; @@ -100,12 +103,13 @@ class AirspeedModule : public ModuleBase, public ModuleParams, void Run() override; static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */ - enum airspeed_index { - DISABLED_INDEX = -1, - GROUND_MINUS_WIND_INDEX, - FIRST_SENSOR_INDEX, - SECOND_SENSOR_INDEX, - THIRD_SENSOR_INDEX + enum class AirspeedSource : int { + DISABLED = -1, + GROUND_MINUS_WIND, + SENSOR_1, + SENSOR_2, + SENSOR_3, + SYNTHETIC }; uORB::Publication _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/ @@ -123,10 +127,11 @@ class AirspeedModule : public ModuleBase, public ModuleParams, uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0}; 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 {}; @@ -136,7 +141,6 @@ class AirspeedModule : public ModuleBase, public ModuleParams, vehicle_land_detected_s _vehicle_land_detected {}; vehicle_local_position_s _vehicle_local_position {}; vehicle_status_s _vehicle_status {}; - vtol_vehicle_status_s _vtol_vehicle_status {}; position_setpoint_s _position_setpoint {}; WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */ @@ -148,8 +152,8 @@ class AirspeedModule : public ModuleBase, public ModuleParams, matrix::Quatf _q_att; hrt_abstime _time_now_usec{0}; - int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */ - int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */ + AirspeedSource _valid_airspeed_src{AirspeedSource::DISABLED}; + AirspeedSource _prev_airspeed_src{AirspeedSource::DISABLED}; bool _initialized{false}; /**< module initialized*/ bool _gnss_lpos_valid{false}; /**< local position (from GNSS) valid */ bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ @@ -177,6 +181,9 @@ class AirspeedModule : public ModuleBase, public ModuleParams, 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 @@ class AirspeedModule : public ModuleBase, public ModuleParams, (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,17 +209,23 @@ class AirspeedModule : public ModuleBase, public ModuleParams, // 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 */ - void check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */ - void update_params(); /**< update parameters */ - void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */ - 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) */ - + void init(); /**< initialization of the airspeed validator instances */ + void check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */ + void update_params(); /**< update parameters */ + void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */ + 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(): @@ -261,9 +274,11 @@ AirspeedModule::init() check_for_connected_airspeed_sensors(); // Set the default sensor - if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) { + if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors + && _param_airspeed_primary_index.get() <= MAX_NUM_AIRSPEED_SENSORS) { // constrain the index to the number of sensors connected - _valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors); + _valid_airspeed_src = static_cast(math::min(_param_airspeed_primary_index.get(), + _number_of_airspeed_sensors)); if (_number_of_airspeed_sensors == 0) { mavlink_log_info(&_mavlink_log_pub, "No airspeed sensor detected. Switch to non-airspeed mode.\t"); @@ -278,10 +293,10 @@ AirspeedModule::init() } else { // set index to the one provided in the parameter ASPD_PRIMARY - _valid_airspeed_index = _param_airspeed_primary_index.get(); + _valid_airspeed_src = static_cast(_param_airspeed_primary_index.get()); } - _prev_airspeed_index = _valid_airspeed_index; // used to detect a sensor switching + _prev_airspeed_src = _valid_airspeed_src; } void @@ -290,7 +305,7 @@ AirspeedModule::check_for_connected_airspeed_sensors() // check for new connected airspeed sensor int detected_airspeed_sensors = 0; - if (_param_airspeed_primary_index.get() > 0) { + if (_param_airspeed_primary_index.get() > 0 && _param_airspeed_primary_index.get() <= MAX_NUM_AIRSPEED_SENSORS) { for (int i = 0; i < _airspeed_subs.size(); i++) { if (!_airspeed_subs[i].advertised()) { @@ -349,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) { @@ -374,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 @@ -538,7 +554,6 @@ void AirspeedModule::poll_topics() _vehicle_air_data_sub.update(&_vehicle_air_data); _vehicle_land_detected_sub.update(&_vehicle_land_detected); _vehicle_status_sub.update(&_vehicle_status); - _vtol_vehicle_status_sub.update(&_vtol_vehicle_status); _vehicle_local_position_sub.update(&_vehicle_local_position); _position_setpoint_sub.update(&_position_setpoint); @@ -614,122 +629,148 @@ void AirspeedModule::update_ground_minus_wind_airspeed() void AirspeedModule::select_airspeed_and_publish() { - // we need to re-evaluate the sensors if we're currently not on a phyisical sensor or the current sensor got invalid bool airspeed_sensor_switching_necessary = false; + const int prev_airspeed_index = static_cast(_prev_airspeed_src); - if (_prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX) { + if (_prev_airspeed_src < AirspeedSource::SENSOR_1) { airspeed_sensor_switching_necessary = true; } else { - airspeed_sensor_switching_necessary = !_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid(); + airspeed_sensor_switching_necessary = !_airspeed_validator[prev_airspeed_index - 1].get_airspeed_valid(); } const bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 && - _param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get(); + _param_airspeed_primary_index.get() > static_cast(AirspeedSource::GROUND_MINUS_WIND) + && _param_airspeed_checks_on.get(); const bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors; if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) { - _valid_airspeed_index = airspeed_index::DISABLED_INDEX; + _valid_airspeed_src = AirspeedSource::DISABLED; // loop through all sensors and take the first valid one for (int i = 0; i < _number_of_airspeed_sensors; i++) { if (_airspeed_validator[i].get_airspeed_valid()) { - _valid_airspeed_index = i + 1; + _valid_airspeed_src = static_cast(i + 1); break; } } } // check if airspeed based on ground-wind speed is valid and can be published - if (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX - || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX) { + if (_valid_airspeed_src < AirspeedSource::SENSOR_1) { // _gnss_lpos_valid determines if ground-wind estimate is valid if (_gnss_lpos_valid && - (_param_airspeed_fallback_gw.get() || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) { - _valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX; + (_param_airspeed_fallback.get() == 1 + || _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_index = airspeed_index::DISABLED_INDEX; + + _valid_airspeed_src = AirspeedSource::DISABLED; } } + const int valid_airspeed_index = static_cast(_valid_airspeed_src); + // print warning or info, depending of whether airspeed got declared invalid or healthy - if (_valid_airspeed_index != _prev_airspeed_index && + if (_valid_airspeed_src != _prev_airspeed_src && _number_of_airspeed_sensors > 0) { - if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED && _prev_airspeed_index > 0) { + if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED + && _prev_airspeed_src > AirspeedSource::GROUND_MINUS_WIND + && prev_airspeed_index <= MAX_NUM_AIRSPEED_SENSORS) { mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected. Check connection and reboot.\t"); events::send(events::ID("airspeed_selector_sensor_failure_disarmed"), events::Log::Critical, "Airspeed sensor failure detected. Check connection and reboot"); - } else if (_prev_airspeed_index > 0) { - mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected. Return to launch (RTL) is advised.\t"); + } else if (_prev_airspeed_src > AirspeedSource::GROUND_MINUS_WIND + && prev_airspeed_index <= MAX_NUM_AIRSPEED_SENSORS) { + mavlink_log_critical(&_mavlink_log_pub, + "Airspeed sensor failure detected. Return to launch (RTL) is advised.\t"); events::send(events::ID("airspeed_selector_sensor_failure"), events::Log::Critical, "Airspeed sensor failure detected. Return to launch (RTL) is advised"); - } else if (_prev_airspeed_index == 0 && _valid_airspeed_index == -1) { + } else if (_prev_airspeed_src == AirspeedSource::GROUND_MINUS_WIND + && _valid_airspeed_src == AirspeedSource::DISABLED) { mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation invalid\t"); events::send(events::ID("airspeed_selector_estimation_invalid"), events::Log::Error, "Airspeed estimation invalid"); - } else if (_prev_airspeed_index == -1 && _valid_airspeed_index == 0) { + } else if (_prev_airspeed_src == AirspeedSource::DISABLED + && _valid_airspeed_src == AirspeedSource::GROUND_MINUS_WIND) { mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation valid\t"); events::send(events::ID("airspeed_selector_estimation_valid"), events::Log::Info, "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(); - switch (_valid_airspeed_index) { - case airspeed_index::DISABLED_INDEX: + airspeed_validated.airspeed_source = valid_airspeed_index; + _prev_airspeed_src = _valid_airspeed_src; + + switch (_valid_airspeed_src) { + case AirspeedSource::DISABLED: break; - case airspeed_index::GROUND_MINUS_WIND_INDEX: + case AirspeedSource::GROUND_MINUS_WIND: airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_CAS; 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; } @@ -756,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 0462fbbc34f3..a5e479a9f2b9 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 diff --git a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp index a9af3ff0542d..0e15e856ca4e 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp @@ -54,6 +54,12 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) reporter.setIsPresent(health_component_t::differential_pressure); + const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3; + + const float airspeed_calibrated_from_sensor = airspeed_from_sensor ? airspeed_validated.calibrated_airspeed_m_s : NAN; + // Maximally allow the airspeed reading to be at FW_AIRSPD_MAX when arming. This is to catch very badly calibrated // airspeed sensors, but also high wind conditions that prevent a forward flight of the vehicle. float arming_max_airspeed_allowed = 20.f; @@ -62,7 +68,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) /* * Check if airspeed is declared valid or not by airspeed selector. */ - if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) { + if (!PX4_ISFINITE(airspeed_calibrated_from_sensor)) { /* EVENT */ @@ -75,7 +81,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) } } - if (!context.isArmed() && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) { + if (!context.isArmed() && airspeed_calibrated_from_sensor > arming_max_airspeed_allowed) { /* EVENT * @description * Current airspeed reading too high. Check if wind is below maximum airspeed and redo airspeed diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 075401e7a89f..ef64838f6bee 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -131,7 +131,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) { resetVelUsingAirspeed(airspeed_sample); - } else if (!_external_wind_init + } else if (!_external_wind_init && !_synthetic_airspeed && (!_control_status.flags.wind || getWindVelocityVariance().longerThan(sq(_params.initial_wind_uncertainty)))) { resetWindUsingAirspeed(airspeed_sample); @@ -212,6 +212,10 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour K.slice(State::wind_vel.idx, 0) = K_wind; } + if (_synthetic_airspeed) { + K.slice(State::wind_vel.idx, 0) = 0.f; + } + measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation); aid_src.fused = true; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index cee2fd05af39..ca01003eaea7 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1259,16 +1259,6 @@ void Ekf::clearInhibitedStateKalmanGains(VectorState &K) const } #endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_WIND) - - if (!_control_status.flags.wind) { - for (unsigned i = 0; i < State::wind_vel.dof; i++) { - K(State::wind_vel.idx + i) = 0.f; - } - } - -#endif // CONFIG_EKF2_WIND } float Ekf::getHeadingInnov() const diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5ff02ca7ab78..592b786a6e6f 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -105,6 +105,7 @@ class EstimatorInterface #if defined(CONFIG_EKF2_AIRSPEED) void setAirspeedData(const airspeedSample &airspeed_sample); + void setSyntheticAirspeed(const bool synthetic_airspeed) { _synthetic_airspeed = synthetic_airspeed; } #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -435,6 +436,7 @@ class EstimatorInterface #if defined(CONFIG_EKF2_AIRSPEED) RingBuffer *_airspeed_buffer {nullptr}; + bool _synthetic_airspeed{false}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_EXTERNAL_VISION) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 2e84439fb2d6..974e748719d3 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2066,8 +2066,11 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) if (_airspeed_validated_sub.update(&airspeed_validated)) { if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && (airspeed_validated.selected_airspeed_index > 0) + && (airspeed_validated.airspeed_source > airspeed_validated_s::GROUND_MINUS_WIND) ) { + + _ekf.setSyntheticAirspeed(airspeed_validated.airspeed_source == airspeed_validated_s::SYNTHETIC); + float cas2tas = 1.f; if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index d905fb85703c..25ff9935ca06 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -218,7 +218,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); const Vector3f vel = _ekf->getVelocity(); - EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-2f); + EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 0.05f); const Vector2f vel_wind_earth = _ekf->getWindVelocity(); EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f); EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index e939a56f549e..de00c74b597e 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -214,8 +214,10 @@ FixedwingPositionControl::airspeed_poll() _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed + // do not use synthetic airspeed as this would create a thrust loop if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) { + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) + && airspeed_validated.airspeed_source != airspeed_validated_s::SYNTHETIC) { airspeed_valid = true; diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index db077d650421..f54514ce5076 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -93,10 +93,15 @@ bool FixedwingLandDetector::_get_landed_state() airspeed_validated_s airspeed_validated{}; _airspeed_validated_sub.copy(&airspeed_validated); + const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3; + bool airspeed_invalid = false; - // set _airspeed_filtered to 0 if airspeed data is invalid - if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) { + // set _airspeed_filtered to 0 if airspeed data is invalid or not from an actual airspeed sensor + if (!airspeed_from_sensor || !PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) + || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) { _airspeed_filtered = 0.0f; airspeed_invalid = true; diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index bbdfe0d3c862..76af7fc39ef2 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -82,9 +82,13 @@ class MavlinkStreamVFRHUD : public MavlinkStream airspeed_validated_s airspeed_validated{}; _airspeed_validated_sub.copy(&airspeed_validated); + const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3; mavlink_vfr_hud_t msg{}; - msg.airspeed = airspeed_validated.calibrated_airspeed_m_s; + // display NAN in case of source not being one of the sensors + msg.airspeed = airspeed_from_sensor ? airspeed_validated.calibrated_airspeed_m_s : NAN; msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy); msg.heading = math::degrees(matrix::wrap_2pi(lpos.heading)); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 3681c2b17c5d..5ab114ba1682 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -104,8 +104,8 @@ void Standard::update_vtol_state() const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get(); - } else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) { - exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get(); + } else if (PX4_ISFINITE(_attc->get_calibrated_airspeed())) { + exit_backtransition_speed_condition = _attc->get_calibrated_airspeed() < _param_mpc_xy_cruise.get(); } const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get(); @@ -220,19 +220,18 @@ void Standard::update_transition_state() // do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed if (_airspeed_trans_blend_margin > 0.0f && - PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && - _airspeed_validated->calibrated_airspeed_m_s > 0.0f && - _airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed() && + PX4_ISFINITE(_attc->get_calibrated_airspeed()) && + _attc->get_calibrated_airspeed() > 0.0f && + _attc->get_calibrated_airspeed() >= getBlendAirspeed() && _time_since_trans_start > getMinimumFrontTransitionTime()) { - mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) / + mc_weight = 1.0f - fabsf(_attc->get_calibrated_airspeed() - getBlendAirspeed()) / _airspeed_trans_blend_margin; // time based blending when no airspeed sensor is set - } else if (!_param_fw_use_airspd.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) { + } else if (!PX4_ISFINITE(_attc->get_calibrated_airspeed())) { mc_weight = 1.0f - _time_since_trans_start / getMinimumFrontTransitionTime(); mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f); - } // ramp up FW_PSP_OFF diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index eaacbe68d611..8b159d24818f 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -336,15 +336,14 @@ void Tailsitter::fill_actuator_outputs() bool Tailsitter::isFrontTransitionCompletedBase() { - const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) - && _param_fw_use_airspd.get(); + const bool airspeed_triggers_transition = PX4_ISFINITE(_attc->get_calibrated_airspeed()); bool transition_to_fw = false; const float pitch = Eulerf(Quatf(_v_att->q)).theta(); if (pitch <= PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW) { if (airspeed_triggers_transition) { - transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ; + transition_to_fw = _attc->get_calibrated_airspeed() >= _param_vt_arsp_trans.get() ; } else { transition_to_fw = true; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 7fb52b4688da..1cc1e19088bf 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -104,8 +104,8 @@ void Tiltrotor::update_vtol_state() const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get() ; - } else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) { - exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ; + } else if (PX4_ISFINITE(_attc->get_calibrated_airspeed())) { + exit_backtransition_speed_condition = _attc->get_calibrated_airspeed() < _param_mpc_xy_cruise.get() ; } const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get() ; @@ -251,14 +251,14 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = 1.0f; _mc_yaw_weight = 1.0f; - if (_param_fw_use_airspd.get() && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && - _airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed()) { - _mc_roll_weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) / + if (PX4_ISFINITE(_attc->get_calibrated_airspeed()) && + _attc->get_calibrated_airspeed() >= getBlendAirspeed()) { + _mc_roll_weight = 1.0f - (_attc->get_calibrated_airspeed() - getBlendAirspeed()) / (getTransitionAirspeed() - getBlendAirspeed()); } // without airspeed do timed weight changes - if ((!_param_fw_use_airspd.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) && + if ((!PX4_ISFINITE(_attc->get_calibrated_airspeed())) && _time_since_trans_start > getMinimumFrontTransitionTime()) { _mc_roll_weight = 1.0f - (_time_since_trans_start - getMinimumFrontTransitionTime()) / (getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime()); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index de0ce8f78c65..353d236ce652 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -350,7 +350,6 @@ VtolAttitudeControl::Run() _local_pos_sub.update(&_local_pos); _local_pos_sp_sub.update(&_local_pos_sp); _pos_sp_triplet_sub.update(&_pos_sp_triplet); - _airspeed_validated_sub.update(&_airspeed_validated); _tecs_status_sub.update(&_tecs_status); _land_detected_sub.update(&_land_detected); @@ -365,6 +364,23 @@ VtolAttitudeControl::Run() } } + if (_airspeed_validated_sub.updated()) { + airspeed_validated_s airspeed_validated; + + if (_airspeed_validated_sub.copy(&airspeed_validated)) { + const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3; + const bool use_airspeed = _param_fw_use_airspd.get() && airspeed_from_sensor; + + _calibrated_airspeed = use_airspeed ? airspeed_validated.calibrated_airspeed_m_s : NAN; + _time_last_airspeed_update = airspeed_validated.timestamp; + + } else if (hrt_elapsed_time(&_time_last_airspeed_update) > 1_s) { + _calibrated_airspeed = NAN; + } + } + vehicle_status_poll(); action_request_poll(); vehicle_cmd_poll(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index ad08d23b5467..6040a2e71d1f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -128,7 +128,6 @@ class VtolAttitudeControl : public ModuleBase, public Modul struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_mc() {return &_vehicle_thrust_setpoint_virtual_mc;} struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_fw() {return &_vehicle_thrust_setpoint_virtual_fw;} - struct airspeed_validated_s *get_airspeed() {return &_airspeed_validated;} struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;} struct tecs_status_s *get_tecs_status() {return &_tecs_status;} struct vehicle_attitude_s *get_att() {return &_vehicle_attitude;} @@ -144,7 +143,9 @@ class VtolAttitudeControl : public ModuleBase, public Modul struct vehicle_thrust_setpoint_s *get_thrust_setpoint_0() {return &_thrust_setpoint_0;} struct vehicle_thrust_setpoint_s *get_thrust_setpoint_1() {return &_thrust_setpoint_1;} struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;} + float get_home_position_z() { return _home_position_z; } + float get_calibrated_airspeed() { return _calibrated_airspeed; } private: void Run() override; @@ -196,7 +197,6 @@ class VtolAttitudeControl : public ModuleBase, public Modul vehicle_thrust_setpoint_s _thrust_setpoint_0{}; vehicle_thrust_setpoint_s _thrust_setpoint_1{}; - airspeed_validated_s _airspeed_validated{}; position_setpoint_triplet_s _pos_sp_triplet{}; tecs_status_s _tecs_status{}; vehicle_attitude_s _vehicle_attitude{}; @@ -208,6 +208,8 @@ class VtolAttitudeControl : public ModuleBase, public Modul vtol_vehicle_status_s _vtol_vehicle_status{}; vtol_vehicle_status_s _prev_published_vtol_vehicle_status{}; float _home_position_z{NAN}; + float _calibrated_airspeed{NAN}; + hrt_abstime _time_last_airspeed_update{0}; float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // [kg/m^3] @@ -242,6 +244,7 @@ class VtolAttitudeControl : public ModuleBase, public Modul DEFINE_PARAMETERS( (ParamInt) _param_vt_type, - (ParamFloat) _param_vt_spoiler_mc_ld + (ParamFloat) _param_vt_spoiler_mc_ld, + (ParamBool) _param_fw_use_airspd ) }; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 7eb77e89d517..def70ece0743 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -78,7 +78,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _thrust_setpoint_1 = _attc->get_thrust_setpoint_1(); _local_pos = _attc->get_local_pos(); _local_pos_sp = _attc->get_local_pos_sp(); - _airspeed_validated = _attc->get_airspeed(); _tecs_status = _attc->get_tecs_status(); _land_detected = _attc->get_land_detected(); } @@ -175,8 +174,7 @@ bool VtolType::isFrontTransitionCompleted() bool VtolType::isFrontTransitionCompletedBase() { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) - && _param_fw_use_airspd.get(); + const bool airspeed_triggers_transition = PX4_ISFINITE(_attc->get_calibrated_airspeed()); const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime(); const bool openloop_trans_time_elapsed = _time_since_trans_start > getOpenLoopFrontTransitionTime(); @@ -184,7 +182,7 @@ bool VtolType::isFrontTransitionCompletedBase() if (airspeed_triggers_transition) { transition_to_fw = minimum_trans_time_elapsed - && _airspeed_validated->calibrated_airspeed_m_s >= getTransitionAirspeed(); + && _attc->get_calibrated_airspeed() >= getTransitionAirspeed(); } else { transition_to_fw = openloop_trans_time_elapsed; @@ -331,8 +329,8 @@ bool VtolType::isFrontTransitionTimeout() // check front transition timeout if (_common_vtol_mode == mode::TRANSITION_TO_FW) { // when we use airspeed, we can timeout earlier if airspeed is not increasing fast enough - if (_param_fw_use_airspd.get() && _time_since_trans_start > getOpenLoopFrontTransitionTime() - && _airspeed_validated->calibrated_airspeed_m_s < getBlendAirspeed()) { + if (PX4_ISFINITE(_attc->get_calibrated_airspeed()) && _time_since_trans_start > getOpenLoopFrontTransitionTime() + && _attc->get_calibrated_airspeed() < getBlendAirspeed()) { return true; } else if (_time_since_trans_start > getFrontTransitionTimeout()) { diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 592779ecfd24..69d1bed6f4da 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -338,7 +338,6 @@ class VtolType : public ModuleParams (ParamFloat) _param_vt_arsp_trans, (ParamFloat) _param_vt_f_trans_thr, (ParamFloat) _param_vt_arsp_blend, - (ParamBool) _param_fw_use_airspd, (ParamFloat) _param_vt_trans_timeout, (ParamFloat) _param_mpc_xy_cruise, (ParamInt) _param_vt_fw_difthr_en,