fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 02:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

AirspeedSelector: airspeed scale estimation improvements and robustification

Browse Source 
- run airspeed scale estimation always, not in dedicated mode
- add option to apply scale automatically, with extra feasibility check
- add airspeed scale for all 3 possible airspeed instances
- clean up parameters
- add check for data stuck (non-changing airspeed data)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-10-11 15:05:27 +02:00
parent 8e8c6efd66
commit 625f556b0e
7 changed files with 286 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+100 -16
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-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
@@ -39,6 +39,10 @@
#include "AirspeedValidator.hpp"
AirspeedValidator::AirspeedValidator()
{
reset_CAS_scale_check(); //this resets all elements of the Vectors to NAN
}
void
AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_data &input_data)
@@ -46,12 +50,14 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
// get indicated airspeed from input data (raw airspeed)
_IAS = input_data.airspeed_indicated_raw;
update_CAS_scale();
update_CAS_scale_estimated(input_data.lpos_valid, input_data.lpos_vx, input_data.lpos_vy, input_data.lpos_vz);
update_CAS_scale_applied();
update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
input_data.lpos_vy,
input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
check_airspeed_data_stuck(input_data.timestamp);
check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
check_load_factor(input_data.accel_z);
update_airspeed_valid_status(input_data.timestamp);
@@ -105,34 +111,112 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
wind_est.beta_innov = _wind_estimator.get_beta_innov();
wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
wind_est.tas_scale = _wind_estimator.get_tas_scale();
wind_est.tas_scale_var = _wind_estimator.get_tas_scale_var();
return wind_est;
}
void
AirspeedValidator::set_airspeed_scale_manual(float airspeed_scale_manual)
AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz)
{
_airspeed_scale_manual = airspeed_scale_manual;
_wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual); // scale is inverted inside the wind estimator
if (!_in_fixed_wing_flight) {
return;
}
// reset every 100s as we assume that all the samples for current check are in similar wind conditions
if (hrt_elapsed_time(&_begin_current_scale_check) > 100_s) {
reset_CAS_scale_check();
}
const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vy, vx));
const int segment_index = int(SCALE_CHECK_SAMPLES * course_over_ground_rad / (2.f * M_PI_F));
_scale_check_groundspeed(segment_index) = sqrt(vx * vx + vy * vy + vz * vz);
_scale_check_TAS(segment_index) = _TAS;
// run check if all segments are filled
if (PX4_ISFINITE(_scale_check_groundspeed.norm_squared())) {
float ground_speed_sum = 0.f;
float TAS_sum = 0.f;
for (int i = 0; i < SCALE_CHECK_SAMPLES; i++) {
ground_speed_sum += _scale_check_groundspeed(i);
TAS_sum += _scale_check_TAS(i);
}
const float TAS_to_grounspeed_error_current = ground_speed_sum - TAS_sum * _CAS_scale_estimated;
const float TAS_to_grounspeed_error_new = ground_speed_sum - TAS_sum * _wind_estimator.get_tas_scale();
// check passes if the average airspeed with the scale applied is closer to groundspeed than without
if (fabsf(TAS_to_grounspeed_error_new) < fabsf(TAS_to_grounspeed_error_current)) {
// constrain the scale update to max 0.01 at a time
const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_estimated - 0.01f,
_CAS_scale_estimated + 0.01f);
// PX4_INFO("_CAS_scale_estimated updated: %.2f --> %.2f", (double)_CAS_scale_estimated,
// (double)new_scale_constrained);
_CAS_scale_estimated = new_scale_constrained;
}
reset_CAS_scale_check();
}
}
void
AirspeedValidator::update_CAS_scale()
AirspeedValidator::reset_CAS_scale_check()
{
if (_wind_estimator.is_estimate_valid()) {
_CAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f);
} else {
_CAS_scale = _airspeed_scale_manual;
_scale_check_groundspeed.setAll(NAN);
_scale_check_TAS.setAll(NAN);
_begin_current_scale_check = hrt_absolute_time();
}
void
AirspeedValidator::update_CAS_scale_applied()
{
switch (_tas_scale_apply) {
default:
/* fallthrough */
case 0:
/* fallthrough */
case 1:
/* fallthrough */
case 2:
_CAS_scale_applied = _tas_scale_init;
break;
case 3:
_CAS_scale_applied = _CAS_scale_estimated;
break;
}
}
void
AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius)
{
_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale);
_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale_applied);
_TAS = calc_TAS_from_CAS(_CAS, air_pressure_pa, air_temperature_celsius);
}
void
AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
{
// data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s)
if (fabsf(_IAS - _IAS_prev) > FLT_EPSILON || _time_last_unequal_data == 0) {
_time_last_unequal_data = time_now;
_IAS_prev = _IAS;
}
_data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT;
}
void
AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
float estimator_status_mag_test_ratio)
@@ -205,12 +289,12 @@ AirspeedValidator::check_load_factor(float accel_z)
void
AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
{
if (_innovations_check_failed || _load_factor_check_failed) {
// either innovation or load factor check failed, so record timestamp
if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) {
// at least one check (data stuck, innovation or load factor) failed, so record timestamp
_time_checks_failed = timestamp;
} else if (!_innovations_check_failed && !_load_factor_check_failed) {
// both innovation or load factor checks must pass to declare airspeed good
} else if (! _data_stuck_test_failed && !_innovations_check_failed && !_load_factor_check_failed) {
// all checks(data stuck, innovation and load factor) must pass to declare airspeed good
_time_checks_passed = timestamp;
}
@@ -223,7 +307,7 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
// a timeout period is applied.
const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s;
if (both_checks_failed || single_check_fail_timeout) {
if (both_checks_failed || single_check_fail_timeout || _data_stuck_test_failed) {
_airspeed_valid = false;
}