fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

AirspeedSelector: add synthetic airspeed option

Browse Source 
Synthetic airspeed is calculated based on the thrust setpoint
and the thrust<->airpseed model as configured in the parameters.
This commit is contained in:
Marco Hauswirth 2025-04-04 16:52:05 +02:00 committed by Silvan Fuhrer
parent c3c863ad95
commit 5842c991ec
14 changed files with 222 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
docs/assets/airframes/fw/reptile_dragon_2/reptile_dragon_2_params.params
View File

@ -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

2
docs/assets/airframes/fw/turbo_timber_evolution/tteparams.params
View File

@ -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

18
msg/px4_msgs_old/msg/AirspeedValidatedV0.msg Normal file
View File

@ -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]

1
msg/translation_node/translations/all_translations.h
View File

@ -11,3 +11,4 @@
//#include "example_translation_service_v1.h"
#include "translation_vehicle_status_v1.h"
#include "translation_airspeed_validated_v1.h"

51
msg/translation_node/translations/translation_airspeed_validated_v1.h Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
* Copyright (c) 2025 PX4 Development Team.
* SPDX-License-Identifier: BSD-3-Clause
****************************************************************************/
#pragma once
// Translate AirspeedValidated v0 <--> v1
#include <px4_msgs_old/msg/airspeed_validated_v0.hpp>
#include <px4_msgs/msg/airspeed_validated.hpp>
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);

24
msg/versioned/AirspeedValidated.msg
View File

@ -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]

4
src/drivers/osd/msp_osd/uorb_to_msp.cpp
View File

@ -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 {

5
src/lib/airspeed/airspeed.cpp
View File

@ -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);
}

2
src/lib/airspeed/airspeed.h
View File

@ -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

8
src/lib/parameters/param_translation.cpp
View File

@ -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;
}

9
src/modules/airspeed_selector/AirspeedValidator.cpp
View File

@ -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<float>(timestamp - _time_last_first_principle_check) / 1_s;
const float dt = static_cast<float>(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

4
src/modules/airspeed_selector/AirspeedValidator.hpp
View File

@ -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<float> _IAS_derivative; ///< indicated airspeed derivative for first principle check
AlphaFilter<float> _throttle_filtered; ///< filtered throttle for first principle check
AlphaFilter<float> _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)

130
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -44,6 +44,7 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/airspeed/airspeed.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
@ -65,12 +66,14 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/airspeed_wind.h>
#include <uORB/topics/flight_phase_estimation.h>
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_s, MAX_NUM_AIRSPEED_SENSORS> _airspeed_subs{ORB_ID::airspeed};
uORB::SubscriptionData<flight_phase_estimation_s> _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<float> _throttle_filtered{_kThrottleFilterTimeConstant};
uint64_t _t_last_throttle_fw{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ASPD_WIND_NSD>) _param_aspd_wind_nsd,
(ParamFloat<px4::params::ASPD_SCALE_NSD>) _param_aspd_scale_nsd,
@ -190,7 +197,7 @@ private:
(ParamFloat<px4::params::ASPD_SCALE_3>) _param_airspeed_scale_3,
(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
(ParamInt<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
(ParamInt<px4::params::ASPD_FALLBACK>) _param_airspeed_fallback,
(ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
@ -202,7 +209,12 @@ private:
// external parameters
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
(ParamFloat<px4::params::FW_THR_ASPD_MIN>) _param_fw_thr_aspd_min,
(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _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<int>(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<int>(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<uint8_t, uint8_t>(events::ID("airspeed_selector_estimation_regain"), events::Log::Info,
"Airspeed sensor healthy, start using again", _prev_airspeed_index,
_valid_airspeed_index);
events::send<uint8_t, uint8_t>(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<float>(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()) {

14
src/modules/airspeed_selector/airspeed_selector_params.c
View File

@ -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