PX4 Firmware
PX4 Autopilot Software http://px4.io
|
Public Member Functions | |
AirspeedModule () | |
~AirspeedModule () override | |
void | Run () override |
int | print_status () override |
Static Public Member Functions | |
static int | task_spawn (int argc, char *argv[]) |
static int | custom_command (int argc, char *argv[]) |
static int | print_usage (const char *reason=nullptr) |
Private Types | |
enum | airspeed_index { DISABLED_INDEX = -1, GROUND_MINUS_WIND_INDEX, FIRST_SENSOR_INDEX, SECOND_SENSOR_INDEX, THIRD_SENSOR_INDEX } |
Private Member Functions | |
DEFINE_PARAMETERS ((ParamFloat< px4::params::ASPD_W_P_NOISE >) _param_west_w_p_noise,(ParamFloat< px4::params::ASPD_SC_P_NOISE >) _param_west_sc_p_noise,(ParamFloat< px4::params::ASPD_TAS_NOISE >) _param_west_tas_noise,(ParamFloat< px4::params::ASPD_BETA_NOISE >) _param_west_beta_noise,(ParamInt< px4::params::ASPD_TAS_GATE >) _param_west_tas_gate,(ParamInt< px4::params::ASPD_BETA_GATE >) _param_west_beta_gate,(ParamInt< px4::params::ASPD_SCALE_EST >) _param_west_scale_estimation_on,(ParamFloat< px4::params::ASPD_SCALE >) _param_west_airspeed_scale,(ParamInt< px4::params::ASPD_PRIMARY >) _param_airspeed_primary_index,(ParamInt< px4::params::ASPD_DO_CHECKS >) _param_airspeed_checks_on,(ParamInt< px4::params::ASPD_FALLBACK >) _param_airspeed_fallback,(ParamFloat< px4::params::ASPD_FS_INNOV >) _tas_innov_threshold,(ParamFloat< px4::params::ASPD_FS_INTEG >) _tas_innov_integ_threshold,(ParamInt< px4::params::ASPD_FS_T1 >) _checks_fail_delay,(ParamInt< px4::params::ASPD_FS_T2 >) _checks_clear_delay,(ParamFloat< px4::params::ASPD_STALL >) _airspeed_stall) void init() | |
initialization of the airspeed validator instances More... | |
void | check_for_connected_airspeed_sensors () |
check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors More... | |
void | update_params () |
update parameters More... | |
void | poll_topics () |
poll all topics required beside airspeed (e.g. More... | |
void | update_wind_estimator_sideslip () |
update the wind estimator instance only fusing sideslip More... | |
void | update_ground_minus_wind_airspeed () |
update airspeed estimate based on groundspeed minus windspeed More... | |
void | select_airspeed_and_publish () |
select airspeed sensor (or groundspeed-windspeed) More... | |
Static Private Attributes | |
static constexpr int | MAX_NUM_AIRSPEED_SENSORS = 3 |
Support max 3 airspeed sensors. More... | |
Definition at line 72 of file airspeed_selector_main.cpp.
|
private |
Enumerator | |
---|---|
DISABLED_INDEX | |
GROUND_MINUS_WIND_INDEX | |
FIRST_SENSOR_INDEX | |
SECOND_SENSOR_INDEX | |
THIRD_SENSOR_INDEX |
Definition at line 97 of file airspeed_selector_main.cpp.
AirspeedModule::AirspeedModule | ( | ) |
Definition at line 179 of file airspeed_selector_main.cpp.
References _perf_elapsed, PC_ELAPSED, perf_alloc_once(), and update_params().
Referenced by task_spawn().
|
override |
Definition at line 189 of file airspeed_selector_main.cpp.
References _perf_elapsed, and perf_free().
|
private |
check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors
Definition at line 243 of file airspeed_selector_main.cpp.
References _airspeed_sub, _number_of_airspeed_sensors, and MAX_NUM_AIRSPEED_SENSORS.
Referenced by Run(), and task_spawn().
|
static |
Definition at line 580 of file airspeed_selector_main.cpp.
References is_running(), print_usage(), and task_spawn().
|
private |
initialization of the airspeed validator instances
_tas_innov_threshold | innovation check threshold |
_tas_innov_integ_threshold | innovation check integrator threshold |
_checks_fail_delay | delay to declare airspeed invalid |
_checks_clear_delay | delay to declare airspeed valid again |
_airspeed_stall | stall speed |
|
private |
poll all topics required beside airspeed (e.g.
current temperature)
Definition at line 423 of file airspeed_selector_main.cpp.
References _accel, _estimator_status, _estimator_status_sub, _time_now_usec, _vehicle_acceleration_sub, _vehicle_air_data, _vehicle_air_data_sub, _vehicle_attitude, _vehicle_attitude_sub, _vehicle_land_detected, _vehicle_land_detected_sub, _vehicle_local_position, _vehicle_local_position_sub, _vehicle_local_position_valid, _vehicle_status, _vehicle_status_sub, _vtol_vehicle_status, _vtol_vehicle_status_sub, vehicle_local_position_s::timestamp, uORB::Subscription::update(), and vehicle_local_position_s::v_xy_valid.
Referenced by Run().
|
override |
Definition at line 593 of file airspeed_selector_main.cpp.
References _number_of_airspeed_sensors, _perf_elapsed, ll40ls::instance, ORB_ID, and perf_print_counter().
|
static |
Definition at line 606 of file airspeed_selector_main.cpp.
Referenced by custom_command().
|
override |
Definition at line 267 of file airspeed_selector_main.cpp.
References _accel, _airspeed_sub, _airspeed_validator, _estimator_status, _ground_minus_wind_EAS, _in_takeoff_situation, _initialized, _number_of_airspeed_sensors, _param_sub, _perf_elapsed, _time_now_usec, _vehicle_air_data, _vehicle_attitude, _vehicle_land_detected, _vehicle_local_position, _vehicle_local_position_valid, _vehicle_status, _vtol_vehicle_status, airspeed_validator_update_data::accel_z, airspeed_validator_update_data::air_pressure_pa, airspeed_s::air_temperature_celsius, airspeed_validator_update_data::air_temperature_celsius, airspeed_validator_update_data::airspeed_indicated_raw, airspeed_validator_update_data::airspeed_timestamp, airspeed_validator_update_data::airspeed_true_raw, armed, vehicle_status_s::arming_state, airspeed_validator_update_data::att_q, vehicle_air_data_s::baro_pressure_pa, check_for_connected_airspeed_sensors(), uORB::Subscription::copy(), vehicle_local_position_s::evh, vehicle_local_position_s::evv, hrt_absolute_time(), airspeed_validator_update_data::in_fixed_wing_flight, airspeed_s::indicated_airspeed_m_s, ToneAlarmInterface::init(), vehicle_land_detected_s::landed, airspeed_validator_update_data::lpos_evh, airspeed_validator_update_data::lpos_evv, airspeed_validator_update_data::lpos_valid, airspeed_validator_update_data::lpos_vx, airspeed_validator_update_data::lpos_vy, airspeed_validator_update_data::lpos_vz, airspeed_validator_update_data::mag_test_ratio, estimator_status_s::mag_test_ratio, perf_begin(), perf_end(), poll_topics(), vehicle_attitude_s::q, select_airspeed_and_publish(), airspeed_s::timestamp, airspeed_validator_update_data::timestamp, airspeed_s::true_airspeed_m_s, uORB::Subscription::update(), AirspeedValidator::update_airspeed_validator(), update_ground_minus_wind_airspeed(), update_params(), update_wind_estimator_sideslip(), airspeed_validator_update_data::vel_test_ratio, estimator_status_s::vel_test_ratio, vtol_vehicle_status_s::vtol_in_rw_mode, vehicle_local_position_s::vx, vehicle_local_position_s::vy, vehicle_local_position_s::vz, and vehicle_acceleration_s::xyz.
|
private |
select airspeed sensor (or groundspeed-windspeed)
Definition at line 482 of file airspeed_selector_main.cpp.
References _airspeed_validated_pub, _airspeed_validator, _ground_minus_wind_EAS, _ground_minus_wind_TAS, _mavlink_log_pub, _number_of_airspeed_sensors, _prev_airspeed_index, _prev_number_of_airspeed_sensors, _time_now_usec, _valid_airspeed_index, _vehicle_land_detected, _vehicle_local_position_valid, _wind_est_pub, _wind_estimate_sideslip, airspeed_validated_s::airspeed_sensor_measurement_valid, airspeed_validated_s::equivalent_airspeed_m_s, airspeed_validated_s::equivalent_ground_minus_wind_m_s, AirspeedValidator::get_airspeed_valid(), AirspeedValidator::get_EAS(), AirspeedValidator::get_IAS(), AirspeedValidator::get_TAS(), AirspeedValidator::get_wind_estimator_states(), airspeed_validated_s::indicated_airspeed_m_s, vehicle_land_detected_s::landed, mavlink_log_critical, uORB::Publication< T >::publish(), uORB::PublicationMulti< T >::publish(), airspeed_validated_s::selected_airspeed_index, airspeed_validated_s::timestamp, airspeed_validated_s::true_airspeed_m_s, and airspeed_validated_s::true_ground_minus_wind_m_s.
Referenced by Run().
|
static |
Definition at line 198 of file airspeed_selector_main.cpp.
References _mavlink_log_pub, _number_of_airspeed_sensors, _prev_airspeed_index, _valid_airspeed_index, AirspeedModule(), check_for_connected_airspeed_sensors(), ToneAlarmInterface::init(), mavlink_and_console_log_info, math::min(), and SCHEDULE_INTERVAL.
Referenced by custom_command().
|
private |
update airspeed estimate based on groundspeed minus windspeed
Definition at line 470 of file airspeed_selector_main.cpp.
References _ground_minus_wind_EAS, _ground_minus_wind_TAS, _vehicle_air_data, _vehicle_local_position, _wind_estimate_sideslip, vehicle_air_data_s::baro_pressure_pa, vehicle_air_data_s::baro_temp_celcius, calc_EAS_from_TAS(), vehicle_local_position_s::vx, vehicle_local_position_s::vy, vehicle_local_position_s::vz, wind_estimate_s::windspeed_east, and wind_estimate_s::windspeed_north.
Referenced by Run().
|
private |
update parameters
Definition at line 357 of file airspeed_selector_main.cpp.
References _airspeed_validator, _mavlink_log_pub, _number_of_airspeed_sensors, _scale_estimation_previously_on, _valid_airspeed_index, _wind_estimator_sideslip, f(), mavlink_and_console_log_info, AirspeedValidator::set_airspeed_scale_manual(), AirspeedValidator::set_airspeed_stall(), WindEstimator::set_beta_gate(), WindEstimator::set_beta_noise(), AirspeedValidator::set_checks_clear_delay(), AirspeedValidator::set_checks_fail_delay(), WindEstimator::set_tas_gate(), AirspeedValidator::set_tas_innov_integ_threshold(), AirspeedValidator::set_tas_innov_threshold(), WindEstimator::set_tas_noise(), WindEstimator::set_tas_scale_p_noise(), AirspeedValidator::set_wind_estimator_beta_gate(), AirspeedValidator::set_wind_estimator_beta_noise(), AirspeedValidator::set_wind_estimator_scale_estimation_on(), AirspeedValidator::set_wind_estimator_tas_gate(), AirspeedValidator::set_wind_estimator_tas_noise(), AirspeedValidator::set_wind_estimator_tas_scale_p_noise(), AirspeedValidator::set_wind_estimator_wind_p_noise(), and WindEstimator::set_wind_p_noise().
Referenced by AirspeedModule(), and Run().
|
private |
update the wind estimator instance only fusing sideslip
Definition at line 438 of file airspeed_selector_main.cpp.
References _time_now_usec, _vehicle_attitude, _vehicle_local_position, _vehicle_local_position_valid, _wind_estimate_sideslip, _wind_estimator_sideslip, wind_estimate_s::beta_innov, wind_estimate_s::beta_innov_var, WindEstimator::fuse_beta(), WindEstimator::get_beta_innov(), WindEstimator::get_beta_innov_var(), WindEstimator::get_tas_innov(), WindEstimator::get_tas_innov_var(), WindEstimator::get_tas_scale(), WindEstimator::get_wind(), WindEstimator::get_wind_var(), vehicle_attitude_s::q, wind_estimate_s::tas_innov, wind_estimate_s::tas_innov_var, wind_estimate_s::tas_scale, wind_estimate_s::timestamp, WindEstimator::update(), wind_estimate_s::variance_east, wind_estimate_s::variance_north, vehicle_local_position_s::vx, vehicle_local_position_s::vy, vehicle_local_position_s::vz, wind_estimate_s::windspeed_east, and wind_estimate_s::windspeed_north.
Referenced by Run().
|
private |
Definition at line 121 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), and Run().
|
private |
raw airspeed topics subscriptions.
Definition at line 118 of file airspeed_selector_main.cpp.
Referenced by check_for_connected_airspeed_sensors(), and Run().
|
private |
airspeed validated topic
Definition at line 105 of file airspeed_selector_main.cpp.
Referenced by select_airspeed_and_publish().
|
private |
airspeedValidator instances (one for each sensor)
Definition at line 134 of file airspeed_selector_main.cpp.
Referenced by Run(), select_airspeed_and_publish(), and update_params().
|
private |
Definition at line 120 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), and Run().
|
private |
Definition at line 109 of file airspeed_selector_main.cpp.
Referenced by poll_topics().
|
private |
equivalent airspeed from groundspeed minus windspeed
Definition at line 143 of file airspeed_selector_main.cpp.
Referenced by Run(), select_airspeed_and_publish(), and update_ground_minus_wind_airspeed().
|
private |
true airspeed from groundspeed minus windspeed
Definition at line 142 of file airspeed_selector_main.cpp.
Referenced by select_airspeed_and_publish(), and update_ground_minus_wind_airspeed().
|
private |
in takeoff situation (defined as not yet stall speed reached)
Definition at line 141 of file airspeed_selector_main.cpp.
Referenced by Run().
|
private |
|
private |
mavlink log topic
Definition at line 107 of file airspeed_selector_main.cpp.
Referenced by select_airspeed_and_publish(), task_spawn(), and update_params().
|
private |
number of airspeed sensors in use (detected during initialization)
Definition at line 132 of file airspeed_selector_main.cpp.
Referenced by check_for_connected_airspeed_sensors(), print_status(), Run(), select_airspeed_and_publish(), task_spawn(), and update_params().
|
private |
Definition at line 110 of file airspeed_selector_main.cpp.
Referenced by Run().
|
private |
Definition at line 147 of file airspeed_selector_main.cpp.
Referenced by AirspeedModule(), print_status(), Run(), and ~AirspeedModule().
|
private |
previously chosen airspeed sensor index
Definition at line 138 of file airspeed_selector_main.cpp.
Referenced by select_airspeed_and_publish(), and task_spawn().
|
private |
number of airspeed sensors in previous loop (to detect a new added sensor)
Definition at line 133 of file airspeed_selector_main.cpp.
Referenced by select_airspeed_and_publish().
|
private |
scale_estimation was on in the last cycle
Definition at line 145 of file airspeed_selector_main.cpp.
Referenced by update_params().
|
private |
Definition at line 136 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), Run(), select_airspeed_and_publish(), and update_wind_estimator_sideslip().
|
private |
index of currently chosen (valid) airspeed sensor
Definition at line 137 of file airspeed_selector_main.cpp.
Referenced by select_airspeed_and_publish(), task_spawn(), and update_params().
|
private |
Definition at line 111 of file airspeed_selector_main.cpp.
Referenced by poll_topics().
|
private |
Definition at line 122 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), Run(), and update_ground_minus_wind_airspeed().
|
private |
Definition at line 112 of file airspeed_selector_main.cpp.
Referenced by poll_topics().
|
private |
Definition at line 123 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), Run(), and update_wind_estimator_sideslip().
|
private |
Definition at line 113 of file airspeed_selector_main.cpp.
Referenced by poll_topics().
|
private |
Definition at line 124 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), Run(), and select_airspeed_and_publish().
|
private |
Definition at line 114 of file airspeed_selector_main.cpp.
Referenced by poll_topics().
|
private |
Definition at line 125 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), Run(), update_ground_minus_wind_airspeed(), and update_wind_estimator_sideslip().
|
private |
Definition at line 115 of file airspeed_selector_main.cpp.
Referenced by poll_topics().
|
private |
local position (from GPS) valid
Definition at line 140 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), Run(), select_airspeed_and_publish(), and update_wind_estimator_sideslip().
|
private |
Definition at line 126 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), and Run().
|
private |
Definition at line 116 of file airspeed_selector_main.cpp.
Referenced by poll_topics().
|
private |
Definition at line 127 of file airspeed_selector_main.cpp.
Referenced by poll_topics(), and Run().
|
private |
Definition at line 117 of file airspeed_selector_main.cpp.
Referenced by poll_topics().
|
private |
wind estimate topic (for each airspeed validator + purely sideslip fusion)
Definition at line 106 of file airspeed_selector_main.cpp.
Referenced by select_airspeed_and_publish().
|
private |
wind estimate message for wind estimator instance only fusing sideslip
Definition at line 130 of file airspeed_selector_main.cpp.
Referenced by select_airspeed_and_publish(), update_ground_minus_wind_airspeed(), and update_wind_estimator_sideslip().
|
private |
wind estimator instance only fusing sideslip
Definition at line 129 of file airspeed_selector_main.cpp.
Referenced by update_params(), and update_wind_estimator_sideslip().
|
staticprivate |
Support max 3 airspeed sensors.
Definition at line 96 of file airspeed_selector_main.cpp.
Referenced by check_for_connected_airspeed_sensors().