fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 02:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
796db86121cab916970b82a03401a013ba1fb033
PX4-Autopilot/src/modules/sensors/airspeed_selector/AirspeedSelector.cpp
T

723 lines
28 KiB
C++
Raw Blame History

/****************************************************************************
*
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "AirspeedSelector.hpp"
using namespace time_literals;
using matrix::Dcmf;
using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector3f;
AirspeedModule::AirspeedModule():
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_param_handle_pitch_sp_offset = param_find("FW_PSP_OFF");
_param_handle_fw_thr_max = param_find("FW_THR_MAX");
// initialise parameters
update_params();
_perf_elapsed = perf_alloc(PC_ELAPSED, MODULE_NAME": elapsed");
_airspeed_validated_pub.advertise();
_wind_est_pub[0].advertise();
_wind_est_pub[1].advertise();
}
AirspeedModule::~AirspeedModule()
{
ScheduleClear();
perf_free(_perf_elapsed);
}
int
AirspeedModule::task_spawn(int argc, char *argv[])
{
AirspeedModule *dev = new AirspeedModule();
// check if the trampoline is called for the first time
if (!dev) {
PX4_ERR("alloc failed");
return PX4_ERROR;
}
_object.store(dev);
dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
_task_id = task_id_is_work_queue;
return PX4_OK;
}
void
AirspeedModule::init()
{
check_for_connected_airspeed_sensors();
// Set the default sensor
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_src = static_cast<AirspeedSource>(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");
events::send(events::ID("airspeed_selector_switch"), events::Log::Info,
"No airspeed sensor detected, switching to non-airspeed mode");
} else {
mavlink_log_info(&_mavlink_log_pub, "Primary airspeed index bigger than number connected sensors. Take last sensor.\t");
events::send(events::ID("airspeed_selector_prim_too_high"), events::Log::Info,
"Primary airspeed index bigger than number connected sensors, taking last sensor");
}
} else {
// set index to the one provided in the parameter ASPD_PRIMARY
_valid_airspeed_src = static_cast<AirspeedSource>(_param_airspeed_primary_index.get());
}
_prev_airspeed_src = _valid_airspeed_src;
}
void
AirspeedModule::check_for_connected_airspeed_sensors()
{
// check for new connected airspeed sensor
int detected_airspeed_sensors = 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()) {
break;
}
detected_airspeed_sensors = i + 1;
}
} else {
// user has selected groundspeed-windspeed as primary source, or disabled airspeed
detected_airspeed_sensors = 0;
}
_number_of_airspeed_sensors = detected_airspeed_sensors;
}
void
AirspeedModule::Run()
{
_time_now_usec = hrt_absolute_time(); // hrt time of the current cycle
// do not run the airspeed selector until 2s after system boot,
// as data from airspeed sensor and estimator may not be valid yet
if (_time_now_usec < 2_s) {
return;
}
perf_begin(_perf_elapsed);
if (!_initialized) {
init(); // initialize airspeed validator instances
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
_airspeed_validator[i].set_CAS_scale_validated(_param_airspeed_scale[i]);
_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
}
_initialized = true;
}
parameter_update_s update;
if (_parameter_update_sub.update(&update)) {
update_params();
}
const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
// check for new connected airspeed sensors as long as we're disarmed
if (!armed) {
check_for_connected_airspeed_sensors();
}
poll_topics();
update_wind_estimator_sideslip();
update_ground_minus_wind_airspeed();
update_throttle_filter(_time_now_usec);
if (_number_of_airspeed_sensors > 0) {
// disable checks if not a fixed-wing or the vehicle is landing/landed, as then airspeed can fall below stall speed
// and wind estimate isn't accurate anymore. Even better would be to have a reliable "ground_contact" detection
// for fixed-wing landings.
const bool in_air_fixed_wing = !_vehicle_land_detected.landed &&
_position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
const matrix::Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
// Prepare data for airspeed_validator
struct airspeed_validator_update_data input_data = {};
input_data.timestamp = _time_now_usec;
input_data.ground_velocity = vI;
input_data.gnss_valid = _gnss_lpos_valid;
input_data.lpos_evh = _vehicle_local_position.evh;
input_data.lpos_evv = _vehicle_local_position.evv;
input_data.q_att = _q_att;
input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa;
input_data.accel_z = _accel.xyz[2];
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_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
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
// poll raw airspeed topic of the i-th sensor
airspeed_s airspeed_raw;
if (_airspeed_subs[i].update(&airspeed_raw)) {
input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
input_data.airspeed_timestamp = airspeed_raw.timestamp;
input_data.air_temperature_celsius = _vehicle_air_data.ambient_temperature;
if (_in_takeoff_situation) {
// set flag to false if either speed is above stall speed,
// or launch detection + land detection indicate flying
const bool speed_above_stall = airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get();
airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get()
|| (PX4_ISFINITE(_ground_minus_wind_CAS) && _ground_minus_wind_CAS > _param_fw_airspd_stall.get());
launch_detection_status_s launch_detection_status{};
_launch_detection_status_sub.copy(&launch_detection_status);
const bool launch_detection_flying = launch_detection_status.launch_detection_state ==
launch_detection_status_s::STATE_FLYING
&& !_vehicle_land_detected.landed;
_in_takeoff_situation = !(speed_above_stall || launch_detection_flying);
} else {
// reset takeoff_situation to true when not in air and not in fixed-wing mode
_in_takeoff_situation = !in_air_fixed_wing;
}
input_data.in_fixed_wing_flight = (in_air_fixed_wing && !_in_takeoff_situation);
// push input data into airspeed validator
_airspeed_validator[i].update_airspeed_validator(input_data);
_time_last_airspeed_update[i] = _time_now_usec;
} else if (_time_now_usec - _time_last_airspeed_update[i] > 1_s) {
// declare airspeed invalid if more then 1s since last raw airspeed update
_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);
}
// save estimated airspeed scale after disarm if airspeed is valid and scale has changed
if (!armed && _armed_prev) {
if (_param_aspd_scale_apply.get() > 0 && _airspeed_validator[i].get_airspeed_valid()
&& fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _param_airspeed_scale[i]) > FLT_EPSILON) {
mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.4f --> %.4f", i + 1,
(double)_param_airspeed_scale[i],
(double)_airspeed_validator[i].get_CAS_scale_validated());
switch (i) {
case 0:
_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated());
_param_airspeed_scale_1.commit_no_notification();
break;
case 1:
_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated());
_param_airspeed_scale_2.commit_no_notification();
break;
case 2:
_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_validated());
_param_airspeed_scale_3.commit_no_notification();
break;
}
}
_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
}
}
}
select_airspeed_and_publish();
_armed_prev = armed;
perf_end(_perf_elapsed);
if (should_exit()) {
exit_and_cleanup();
}
}
void AirspeedModule::update_params()
{
updateParams();
if (_param_handle_pitch_sp_offset != PARAM_INVALID) {
param_get(_param_handle_pitch_sp_offset, &_param_pitch_sp_offset);
}
if (_param_handle_fw_thr_max != PARAM_INVALID) {
param_get(_param_handle_fw_thr_max, &_param_fw_thr_max);
}
const float prev_scale[MAX_NUM_AIRSPEED_SENSORS] = {
_param_airspeed_scale[0],
_param_airspeed_scale[1],
_param_airspeed_scale[2]
};
_param_airspeed_scale[0] = _param_airspeed_scale_1.get();
_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
if (fabsf(_param_airspeed_scale[i] - prev_scale[i]) > FLT_EPSILON) {
_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
_airspeed_validator[i].reset_scale_estimator();
_airspeed_validator[i].set_CAS_scale_validated(_param_airspeed_scale[i]);
}
}
_wind_estimator_sideslip.set_wind_process_noise_spectral_density(_param_aspd_wind_nsd.get());
_wind_estimator_sideslip.set_tas_scale_process_noise_spectral_density(_param_aspd_scale_nsd.get());
_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
_wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get());
_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
_airspeed_validator[i].set_wind_estimator_wind_process_noise_spectral_density(_param_aspd_wind_nsd.get());
_airspeed_validator[i].set_wind_estimator_tas_scale_process_noise_spectral_density(_param_aspd_scale_nsd.get());
_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
_airspeed_validator[i].set_tas_scale_apply(_param_aspd_scale_apply.get());
_airspeed_validator[i].set_wind_estimator_tas_scale_init(_param_airspeed_scale[i]);
_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
_airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
_airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
_airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get());
_airspeed_validator[i].set_enable_data_stuck_check(_param_airspeed_checks_on.get() &
CheckTypeBits::CHECK_TYPE_DATA_STUCK_BIT);
_airspeed_validator[i].set_enable_innovation_check(_param_airspeed_checks_on.get() &
CheckTypeBits::CHECK_TYPE_INNOVATION_BIT);
_airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() &
CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT);
_airspeed_validator[i].set_enable_first_principle_check(_param_airspeed_checks_on.get() &
CheckTypeBits::CHECK_TYPE_FIRST_PRINCIPLE_BIT);
_airspeed_validator[i].set_psp_off_param(math::radians(_param_pitch_sp_offset));
_airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max);
_airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get());
}
}
void AirspeedModule::poll_topics()
{
// use primary estimator_status
if (_estimator_selector_status_sub.updated()) {
estimator_selector_status_s estimator_selector_status;
if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
}
}
}
_estimator_status_sub.update(&_estimator_status);
_vehicle_acceleration_sub.update(&_accel);
_vehicle_air_data_sub.update(&_vehicle_air_data);
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
_vehicle_status_sub.update(&_vehicle_status);
_vehicle_local_position_sub.update(&_vehicle_local_position);
_position_setpoint_sub.update(&_position_setpoint);
_tecs_status_sub.update(&_tecs_status);
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude;
_vehicle_attitude_sub.update(&vehicle_attitude);
if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// if the vehicle is a tailsitter we have to rotate the attitude by 90° to get to the airspeed frame
_q_att = Quatf(vehicle_attitude.q) * Quatf(matrix::Eulerf(0.f, M_PI_2_F, 0.f));
} else {
_q_att = Quatf(vehicle_attitude.q);
}
}
_gnss_lpos_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
&& (_vehicle_local_position.timestamp > 0)
&& _vehicle_local_position.v_xy_valid
&& (_estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS)
|| _estimator_status.control_mode_flags & (static_cast<uint64_t>(1) << estimator_status_s::CS_GNSS_VEL));
}
void AirspeedModule::update_wind_estimator_sideslip()
{
// update wind and airspeed estimator
_wind_estimator_sideslip.update(_time_now_usec);
if (_gnss_lpos_valid
&& _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& !_vehicle_land_detected.landed) {
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
const float hor_vel_variance = _vehicle_local_position.evh * _vehicle_local_position.evh;
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, _q_att);
}
_wind_estimate_sideslip.timestamp = _time_now_usec;
_wind_estimate_sideslip.windspeed_north = _wind_estimator_sideslip.get_wind()(0);
_wind_estimate_sideslip.windspeed_east = _wind_estimator_sideslip.get_wind()(1);
_wind_estimate_sideslip.variance_north = _wind_estimator_sideslip.get_wind_var()(0);
_wind_estimate_sideslip.variance_east = _wind_estimator_sideslip.get_wind_var()(1);
_wind_estimate_sideslip.tas_innov = NAN;
_wind_estimate_sideslip.tas_innov_var = NAN;
_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
_wind_estimate_sideslip.tas_scale_raw = NAN;
_wind_estimate_sideslip.tas_scale_raw_var = NAN;
_wind_estimate_sideslip.tas_scale_validated = NAN;
_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
}
void AirspeedModule::update_ground_minus_wind_airspeed()
{
const float wind_uncertainty = sqrtf(_wind_estimate_sideslip.variance_north + _wind_estimate_sideslip.variance_east);
if (_wind_estimator_sideslip.is_estimate_valid() && wind_uncertainty < _param_wind_sigma_max_synth_tas.get()) {
// calculate airspeed estimate based on groundspeed-windspeed
const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
_ground_minus_wind_CAS = calc_calibrated_from_true_airspeed(_ground_minus_wind_TAS, _vehicle_air_data.rho);
} else {
_ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN;
}
}
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<int>(_prev_airspeed_src);
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();
}
const bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
_param_airspeed_primary_index.get() > static_cast<int>(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_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_src = static_cast<AirspeedSource>(i + 1);
break;
}
}
}
// check if airspeed based on ground-wind speed is valid and can be published
if (_valid_airspeed_src < AirspeedSource::SENSOR_1) {
// _gnss_lpos_valid determines if ground-wind estimate is valid
if (_gnss_lpos_valid &&
(_param_airspeed_fallback.get() == 1
|| _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;
}
}
const int valid_airspeed_index = static_cast<int>(_valid_airspeed_src);
// print warning or info, depending of whether airspeed got declared invalid or healthy
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_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_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_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_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_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::Critical,
"Airspeed sensor healthy, start using again", 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.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_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:
break;
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.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.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
airspeed_validated.calibraded_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered);
break;
}
_airspeed_validated_pub.publish(airspeed_validated);
_wind_est_pub[0].publish(_wind_estimate_sideslip);
// publish the wind estimator states from all airspeed validators
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
airspeed_wind_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
if (i == 0) {
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_1;
} else if (i == 1) {
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_2;
} else {
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_3;
}
_wind_est_pub[i + 1].publish(wind_est);
}
}
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_rates_setpoint_s vehicle_rates_setpoint{};
_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint);
float forward_thrust = vehicle_rates_setpoint.thrust_body[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_rates_setpoint.thrust_body[0] * vehicle_rates_setpoint.thrust_body[0] +
vehicle_rates_setpoint.thrust_body[1] * vehicle_rates_setpoint.thrust_body[1] +
vehicle_rates_setpoint.thrust_body[2] * vehicle_rates_setpoint.thrust_body[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()) {
int ret = AirspeedModule::task_spawn(argc, argv);
if (ret) {
return ret;
}
}
return print_usage("unknown command");
}
int AirspeedModule::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module provides a single airspeed_validated topic, containing indicated (IAS),
calibrated (CAS), true airspeed (TAS) and the information if the estimation currently
is invalid and if based sensor readings or on groundspeed minus windspeed.
Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
to a valid sensor in case of failure detection. For failure detection as well as for
the estimation of a scale factor from IAS to CAS, it runs several wind estimators
and also publishes those.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[])
{
return AirspeedModule::main(argc, argv);
}
Reference in New Issue View Git Blame Copy Permalink