39 #include <px4_platform_common/module.h> 40 #include <px4_platform_common/module_params.h> 41 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 72 class AirspeedModule :
public ModuleBase<AirspeedModule>,
public ModuleParams,
73 public px4::ScheduledWorkItem
82 static int task_spawn(
int argc,
char *argv[]);
85 static int custom_command(
int argc,
char *argv[]);
88 static int print_usage(
const char *reason =
nullptr);
96 static constexpr
int MAX_NUM_AIRSPEED_SENSORS = 3;
132 int32_t _number_of_airspeed_sensors{0};
133 int32_t _prev_number_of_airspeed_sensors{0};
137 int _valid_airspeed_index{-2};
138 int _prev_airspeed_index{-2};
139 bool _initialized{
false};
140 bool _vehicle_local_position_valid{
false};
141 bool _in_takeoff_situation{
true};
142 float _ground_minus_wind_TAS{0.0f};
143 float _ground_minus_wind_EAS{0.0f};
145 bool _scale_estimation_previously_on{
false};
150 (ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
151 (ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
152 (ParamFloat<px4::params::ASPD_TAS_NOISE>) _param_west_tas_noise,
153 (ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
154 (ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
155 (ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
156 (ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
157 (ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
158 (ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
159 (ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
160 (ParamInt<px4::params::ASPD_FALLBACK>) _param_airspeed_fallback,
162 (ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold,
163 (ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold,
164 (ParamInt<px4::params::ASPD_FS_T1>) _checks_fail_delay,
165 (ParamInt<px4::params::ASPD_FS_T2>) _checks_clear_delay,
166 (ParamFloat<px4::params::ASPD_STALL>) _airspeed_stall
170 void check_for_connected_airspeed_sensors();
173 void update_wind_estimator_sideslip();
174 void update_ground_minus_wind_airspeed();
175 void select_airspeed_and_publish();
180 ModuleParams(nullptr),
181 ScheduledWorkItem(MODULE_NAME,
px4::wq_configurations::att_pos_ctrl)
204 PX4_ERR(
"alloc failed");
211 _task_id = task_id_is_work_queue;
227 "No airspeed sensor detected. Switch to non-airspeed mode.");
231 "Primary airspeed index bigger than number connected sensors. Take last sensor.");
236 _param_airspeed_primary_index.get();
246 int detected_airspeed_sensors = 0;
248 if (_param_airspeed_primary_index.get() > 0) {
255 detected_airspeed_sensors = i + 1;
259 detected_airspeed_sensors = 0;
336 if (!in_air || !fixed_wing) {
398 _param_west_scale_estimation_on.set(0);
399 _param_west_scale_estimation_on.commit_no_notification();
408 _param_west_airspeed_scale.commit_no_notification();
440 bool att_valid =
true;
485 bool airspeed_sensor_switching_necessary =
_prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
488 _param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get();
491 if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) {
505 if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
507 || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
512 || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
543 case airspeed_index::DISABLED_INDEX:
546 case airspeed_index::GROUND_MINUS_WIND_INDEX:
601 print_message(est.get());
609 PX4_WARN(
"%s\n", reason);
612 PRINT_MODULE_DESCRIPTION(
615 This module provides a single airspeed_validated topic, containing an indicated (IAS), 616 equivalent (EAS), true airspeed (TAS) and the information if the estimation currently 617 is invalid and if based sensor readings or on groundspeed minus windspeed. 618 Supporting the input of multiple "raw" airspeed inputs, this module automatically switches 619 to a valid sensor in case of failure detection. For failure detection as well as for 620 the estimation of a scale factor from IAS to EAS, it runs several wind estimators 621 and also publishes those. 625 PRINT_MODULE_USAGE_NAME("airspeed_estimator",
"estimator");
626 PRINT_MODULE_USAGE_COMMAND(
"start");
627 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
void set_wind_estimator_beta_noise(float beta_var)
void check_for_connected_airspeed_sensors()
check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors
void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma)
WindEstimator _wind_estimator_sideslip
wind estimator instance only fusing sideslip
float equivalent_airspeed_m_s
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
void set_wind_estimator_tas_noise(float tas_sigma)
void update(uint64_t time_now)
vehicle_acceleration_s _accel
measure the time elapsed performing an event
wind_estimate_s _wind_estimate_sideslip
wind estimate message for wind estimator instance only fusing sideslip
uORB::Publication< airspeed_validated_s > _airspeed_validated_pub
airspeed validated topic
void get_wind(float wind[2])
void set_wind_estimator_wind_p_noise(float wind_sigma)
int main(int argc, char **argv)
uORB::Subscription _vtol_vehicle_status_sub
float calc_EAS_from_TAS(float speed_true, float pressure_ambient, float temperature_celsius)
Calculate equivalent airspeed (EAS) from true airspeed (TAS).
int32_t _number_of_airspeed_sensors
number of airspeed sensors in use (detected during initialization)
float air_temperature_celsius
void set_checks_fail_delay(float checks_fail_delay)
int print_status() override
int32_t _prev_number_of_airspeed_sensors
number of airspeed sensors in previous loop (to detect a new added sensor)
perf_counter_t _perf_elapsed
void set_wind_estimator_tas_gate(uint8_t gate_size)
static void print_usage()
bool airspeed_sensor_measurement_valid
float true_ground_minus_wind_m_s
void update_params()
update parameters
void get_wind_var(float wind_var[2])
High-resolution timer with callouts and timekeeping.
uORB::Subscription _vehicle_air_data_sub
uORB::Subscription _vehicle_land_detected_sub
int8_t selected_airspeed_index
#define mavlink_and_console_log_info(_pub, _text,...)
Send a mavlink emergency message and print to console.
float get_beta_innov_var()
Global flash based parameter store.
void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
void select_airspeed_and_publish()
select airspeed sensor (or groundspeed-windspeed)
A wind and airspeed scale estimator.
int _valid_airspeed_index
index of currently chosen (valid) airspeed sensor
Calculates airspeed from differential pressure and checks if this airspeed is valid.
void set_airspeed_stall(float airspeed_stall)
uORB::Subscription _vehicle_attitude_sub
bool _initialized
module initialized
void set_tas_scale_p_noise(float tas_scale_sigma)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
float _ground_minus_wind_TAS
true airspeed from groundspeed minus windspeed
void set_beta_noise(float beta_var)
wind_estimate_s get_wind_estimator_states(uint64_t timestamp)
bool publish(const T &data)
Publish the struct.
void poll_topics()
poll all topics required beside airspeed (e.g.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
static sensor_accel_s _accel
accel report
bool _vehicle_local_position_valid
local position (from GPS) valid
orb_advert_t _mavlink_log_pub
mavlink log topic
void set_checks_clear_delay(float checks_clear_delay)
void update_airspeed_validator(const airspeed_validator_update_data &input_data)
void set_beta_gate(uint8_t gate_size)
float indicated_airspeed_m_s
void set_tas_noise(float tas_sigma)
perf_counter_t perf_alloc_once(enum perf_counter_type type, const char *name)
Get the reference to an existing counter or create a new one if it does not exist.
static struct actuator_armed_s armed
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Vector2< float > Vector2f
float indicated_airspeed_m_s
uint64_t airspeed_timestamp
vehicle_status_s _vehicle_status
vtol_vehicle_status_s _vtol_vehicle_status
void set_airspeed_scale_manual(float airspeed_scale_manual)
void perf_end(perf_counter_t handle)
End a performance event.
vehicle_attitude_s _vehicle_attitude
void update_wind_estimator_sideslip()
update the wind estimator instance only fusing sideslip
__EXPORT int airspeed_selector_main(int argc, char *argv[])
uORB::Subscription _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS]
raw airspeed topics subscriptions.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
constexpr _Tp min(_Tp a, _Tp b)
float airspeed_indicated_raw
static constexpr int MAX_NUM_AIRSPEED_SENSORS
Support max 3 airspeed sensors.
static void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &got_changes)
Vector3< float > Vector3f
AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS]
airspeedValidator instances (one for each sensor)
int _prev_airspeed_index
previously chosen airspeed sensor index
bool get_airspeed_valid()
uORB::Subscription _vehicle_local_position_sub
static int print_usage(const char *reason=nullptr)
bool _in_takeoff_situation
in takeoff situation (defined as not yet stall speed reached)
Quaternion< float > Quatf
uORB::Subscription _estimator_status_sub
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
vehicle_land_detected_s _vehicle_land_detected
static int task_spawn(int argc, char *argv[])
void set_tas_innov_threshold(float tas_innov_threshold)
vehicle_local_position_s _vehicle_local_position
~AirspeedModule() override
bool in_fixed_wing_flight
float air_temperature_celsius
void set_tas_innov_integ_threshold(float tas_innov_integ_threshold)
uORB::PublicationMulti< wind_estimate_s > _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS+1]
wind estimate topic (for each airspeed validator + purely sideslip fusion)
float _ground_minus_wind_EAS
equivalent airspeed from groundspeed minus windspeed
bool update(void *dst)
Update the struct.
void set_tas_gate(uint8_t gate_size)
float get_tas_innov_var()
void set_wind_p_noise(float wind_sigma)
hrt_abstime _time_now_usec
bool _scale_estimation_previously_on
scale_estimation was on in the last cycle
bool publish(const T &data)
Publish the struct.
bool copy(void *dst)
Copy the struct.
vehicle_air_data_s _vehicle_air_data
estimator_status_s _estimator_status
void update_ground_minus_wind_airspeed()
update airspeed estimate based on groundspeed minus windspeed
void set_wind_estimator_scale_estimation_on(bool scale_estimation_on)
void perf_begin(perf_counter_t handle)
Begin a performance event.
float equivalent_ground_minus_wind_m_s
static int custom_command(int argc, char *argv[])
uORB::Subscription _param_sub
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uORB::Subscription _vehicle_acceleration_sub
uORB::Subscription _vehicle_status_sub
void set_wind_estimator_beta_gate(uint8_t gate_size)
Performance measuring tools.
static constexpr uint32_t SCHEDULE_INTERVAL
The schedule interval in usec (10 Hz)