mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
AirspeedSelector: airspeed scale estimation improvements and robustification
- 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
parent
8e8c6efd66
commit
625f556b0e
@ -9,7 +9,9 @@ float32 variance_east # Wind estimate error variance in east / Y direction (m/s
|
||||
|
||||
float32 tas_innov # True airspeed innovation
|
||||
float32 tas_innov_var # True airspeed innovation variance
|
||||
|
||||
float32 tas_scale # Estimated true airspeed scale factor
|
||||
float32 tas_scale_var # True airspeed scale factor variance
|
||||
|
||||
float32 beta_innov # Sideslip measurement innovation
|
||||
float32 beta_innov_var # Sideslip measurement innovation variance
|
||||
|
||||
@ -56,7 +56,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
|
||||
// initilaise wind states assuming zero side slip and horizontal flight
|
||||
_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est);
|
||||
_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_est);
|
||||
_state(INDEX_TAS_SCALE) = 1.0f;
|
||||
_state(INDEX_TAS_SCALE) = _scale_init;
|
||||
|
||||
// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
|
||||
float L0 = v_e * v_e;
|
||||
@ -116,7 +116,7 @@ WindEstimator::update(uint64_t time_now)
|
||||
_time_last_update = time_now;
|
||||
|
||||
float q_w = _wind_p_var;
|
||||
float q_k_tas = _tas_scale_p_var;
|
||||
float q_k_tas = _disable_tas_scale_estimate ? 0.f : _tas_scale_p_var;
|
||||
|
||||
float SPP0 = dt * dt;
|
||||
float SPP1 = SPP0 * q_w;
|
||||
@ -160,28 +160,32 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
|
||||
const float v_e = velI(1);
|
||||
const float v_d = velI(2);
|
||||
|
||||
const float k_tas = _state(INDEX_TAS_SCALE);
|
||||
// calculate airspeed from ground speed and wind states (without scale)
|
||||
const float airspeed_predicted_raw = sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) +
|
||||
(v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d);
|
||||
|
||||
// compute kalman gain K
|
||||
const float HH0 = sqrtf(v_d * v_d + (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + (v_n - _state(
|
||||
INDEX_W_N)) * (v_n - _state(INDEX_W_N)));
|
||||
const float HH1 = k_tas / HH0;
|
||||
// compute state observation matrix H
|
||||
const float HH0 = airspeed_predicted_raw;
|
||||
const float HH1 = _state(INDEX_TAS_SCALE) / HH0;
|
||||
|
||||
matrix::Matrix<float, 1, 3> H_tas;
|
||||
H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N));
|
||||
H_tas(0, 1) = HH1 * (-v_e + _state(INDEX_W_E));
|
||||
H_tas(0, 2) = HH0;
|
||||
|
||||
matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
|
||||
|
||||
// compute innovation covariance S
|
||||
const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
|
||||
|
||||
// compute Kalman gain
|
||||
matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
|
||||
K /= S(0, 0);
|
||||
// compute innovation
|
||||
const float airspeed_pred = k_tas * sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + (v_e - _state(
|
||||
INDEX_W_E)) *
|
||||
(v_e - _state(INDEX_W_E)) + v_d * v_d);
|
||||
|
||||
if (_disable_tas_scale_estimate) {
|
||||
K(2, 0) = 0.f;
|
||||
}
|
||||
|
||||
// compute innovation
|
||||
const float airspeed_pred = _state(INDEX_TAS_SCALE) * airspeed_predicted_raw;
|
||||
_tas_innov = true_airspeed - airspeed_pred;
|
||||
|
||||
// innovation variance
|
||||
@ -259,13 +263,17 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
|
||||
H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5;
|
||||
H_beta(0, 2) = 0;
|
||||
|
||||
// compute kalman gain
|
||||
matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
|
||||
|
||||
// compute innovation covariance S
|
||||
const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
|
||||
|
||||
// compute Kalman gain
|
||||
matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
|
||||
K /= S(0, 0);
|
||||
|
||||
if (_disable_tas_scale_estimate) {
|
||||
K(2, 0) = 0.f;
|
||||
}
|
||||
|
||||
// compute predicted side slip angle
|
||||
matrix::Vector3f rel_wind(velI(0) - _state(INDEX_W_N), velI(1) - _state(INDEX_W_E), velI(2));
|
||||
matrix::Dcmf R_body_to_earth(q_att);
|
||||
@ -333,16 +341,6 @@ WindEstimator::run_sanity_checks()
|
||||
return;
|
||||
}
|
||||
|
||||
// check if we should inhibit learning of airspeed scale factor and rather use a pre-set value.
|
||||
// airspeed scale factor errors arise from sensor installation which does not change and only needs
|
||||
// to be computed once for a perticular installation.
|
||||
if (_enforced_airspeed_scale < 0) {
|
||||
_state(INDEX_TAS_SCALE) = math::max(0.0f, _state(INDEX_TAS_SCALE));
|
||||
|
||||
} else {
|
||||
_state(INDEX_TAS_SCALE) = _enforced_airspeed_scale;
|
||||
}
|
||||
|
||||
// attain symmetry
|
||||
for (unsigned row = 0; row < 3; row++) {
|
||||
for (unsigned column = 0; column < row; column++) {
|
||||
|
||||
@ -70,7 +70,9 @@ public:
|
||||
bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
|
||||
uint64_t &time_meas_rejected, bool &reinit_filter);
|
||||
|
||||
float get_tas_scale() { return _state(INDEX_TAS_SCALE); }
|
||||
// invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10]
|
||||
float get_tas_scale() { return math::constrain(1.f / (_state(INDEX_TAS_SCALE) + 0.0001f), 0.1f, 10.f); }
|
||||
float get_tas_scale_var() { return _P(2, 2); }
|
||||
float get_tas_innov() { return _tas_innov; }
|
||||
float get_tas_innov_var() { return _tas_innov_var; }
|
||||
float get_beta_innov() { return _beta_innov; }
|
||||
@ -88,10 +90,8 @@ public:
|
||||
void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
|
||||
void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
|
||||
void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; }
|
||||
|
||||
// Inhibit learning of the airspeed scale factor and force the estimator to use _enforced_airspeed_scale as scale factor.
|
||||
// Negative input values enable learning of the airspeed scale factor.
|
||||
void enforce_airspeed_scale(float scale) {_enforced_airspeed_scale = scale; }
|
||||
void set_scale_init(float scale_init) {_scale_init = math::constrain(1.f / (scale_init + 0.0001f), 0.1f, 10.f); }
|
||||
void set_disable_tas_scale_estimate(bool disable_tas_scale_estimate) {_disable_tas_scale_estimate = disable_tas_scale_estimate; }
|
||||
|
||||
private:
|
||||
enum {
|
||||
@ -100,7 +100,7 @@ private:
|
||||
INDEX_TAS_SCALE
|
||||
}; ///< enum which can be used to access state.
|
||||
|
||||
matrix::Vector3f _state; ///< state vector
|
||||
matrix::Vector3f _state{0.f, 0.f, 1.f};
|
||||
matrix::Matrix3f _P; ///< state covariance matrix
|
||||
|
||||
float _tas_innov{0.0f}; ///< true airspeed innovation
|
||||
@ -118,15 +118,18 @@ private:
|
||||
uint8_t _tas_gate{3}; ///< airspeed fusion gate size
|
||||
uint8_t _beta_gate{1}; ///< sideslip fusion gate size
|
||||
|
||||
float _scale_init{1.f};
|
||||
|
||||
uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion
|
||||
uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion
|
||||
uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction
|
||||
uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected
|
||||
uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion
|
||||
uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction
|
||||
uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected
|
||||
uint64_t _time_rejected_tas =
|
||||
0; ///<timestamp of when true airspeed measurements have consistently started to be rejected
|
||||
0; ///< timestamp of when true airspeed measurements have consistently started to be rejected
|
||||
|
||||
bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
|
||||
float _enforced_airspeed_scale{-1.0f}; ///< by default we want to estimate the true airspeed scale factor (see enforce_airspeed_scale(...) )
|
||||
|
||||
bool _disable_tas_scale_estimate{false};
|
||||
|
||||
// initialise state and state covariance matrix
|
||||
bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
@ -73,7 +73,7 @@ struct airspeed_validator_update_data {
|
||||
class AirspeedValidator
|
||||
{
|
||||
public:
|
||||
AirspeedValidator() = default;
|
||||
AirspeedValidator();
|
||||
~AirspeedValidator() = default;
|
||||
|
||||
void update_airspeed_validator(const airspeed_validator_update_data &input_data);
|
||||
@ -84,13 +84,18 @@ public:
|
||||
float get_CAS() { return _CAS; }
|
||||
float get_TAS() { return _TAS; }
|
||||
bool get_airspeed_valid() { return _airspeed_valid; }
|
||||
float get_CAS_scale() {return _CAS_scale;}
|
||||
float get_CAS_scale_estimated() {return _CAS_scale_estimated;}
|
||||
|
||||
airspeed_wind_s get_wind_estimator_states(uint64_t timestamp);
|
||||
|
||||
// setters wind estimator parameters
|
||||
void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); }
|
||||
void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma) { _wind_estimator.set_tas_scale_p_noise(tas_scale_sigma); }
|
||||
void set_wind_estimator_tas_scale_init(float tas_scale_init)
|
||||
{
|
||||
_tas_scale_init = tas_scale_init;
|
||||
}
|
||||
|
||||
void set_wind_estimator_tas_noise(float tas_sigma) { _wind_estimator.set_tas_noise(tas_sigma); }
|
||||
void set_wind_estimator_beta_noise(float beta_var) { _wind_estimator.set_beta_noise(beta_var); }
|
||||
void set_wind_estimator_tas_gate(uint8_t gate_size)
|
||||
@ -100,9 +105,6 @@ public:
|
||||
}
|
||||
|
||||
void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); }
|
||||
void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) { _wind_estimator_scale_estimation_on = scale_estimation_on;}
|
||||
|
||||
void set_airspeed_scale_manual(float airspeed_scale_manual);
|
||||
|
||||
// setters for failure detection tuning parameters
|
||||
void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; }
|
||||
@ -112,20 +114,31 @@ public:
|
||||
|
||||
void set_airspeed_stall(float airspeed_stall) { _airspeed_stall = airspeed_stall; }
|
||||
|
||||
void set_tas_scale_apply(int tas_scale_apply) { _tas_scale_apply = tas_scale_apply; }
|
||||
void set_CAS_scale_estimated(float scale) { _CAS_scale_estimated = scale; }
|
||||
void set_scale_init(float scale) { _wind_estimator.set_scale_init(scale); }
|
||||
void set_disable_tas_scale_estimate(bool disable_scale_est) {_wind_estimator.set_disable_tas_scale_estimate(disable_scale_est); }
|
||||
|
||||
private:
|
||||
|
||||
WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator
|
||||
|
||||
// wind estimator parameter
|
||||
bool _wind_estimator_scale_estimation_on{false}; ///< online scale estimation (IAS-->CAS) is on
|
||||
float _airspeed_scale_manual{1.0f}; ///< manually entered airspeed scale
|
||||
// airspeed scale validity check
|
||||
static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°)
|
||||
|
||||
// general states
|
||||
bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks
|
||||
float _IAS{0.0f}; ///< indicated airsped in m/s
|
||||
float _CAS{0.0f}; ///< calibrated airspeed in m/s
|
||||
float _TAS{0.0f}; ///< true airspeed in m/s
|
||||
float _CAS_scale{1.0f}; ///< scale factor from IAS to CAS
|
||||
float _CAS_scale_applied{1.0f}; ///< scale factor from IAS to CAS (currently applied value)
|
||||
float _CAS_scale_estimated{1.0f}; ///< scale factor from IAS to CAS (currently estimated value)
|
||||
|
||||
// data stuck check
|
||||
uint64_t _time_last_unequal_data{0};
|
||||
bool _data_stuck_test_failed{false};
|
||||
float _IAS_prev{0.f};
|
||||
static constexpr uint64_t DATA_STUCK_TIMEOUT{2_s}; ///< timeout after which data stuck check triggers when data is flat
|
||||
|
||||
// states of innovation check
|
||||
float _tas_gate{1.0f}; ///< gate size of airspeed innovation (to calculate tas_test_ratio)
|
||||
@ -146,22 +159,33 @@ private:
|
||||
// states of airspeed valid declaration
|
||||
bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed)
|
||||
int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec)
|
||||
int _checks_clear_delay{3}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
|
||||
int _checks_clear_delay{-1}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
|
||||
uint64_t _time_checks_passed{0}; ///< time the checks have last passed (uSec)
|
||||
uint64_t _time_checks_failed{0}; ///< time the checks have last not passed (uSec)
|
||||
|
||||
hrt_abstime _begin_current_scale_check{0};
|
||||
|
||||
int _tas_scale_apply{0};
|
||||
float _tas_scale_init{1.f};
|
||||
|
||||
matrix::Vector<float, SCALE_CHECK_SAMPLES> _scale_check_TAS {};
|
||||
matrix::Vector<float, SCALE_CHECK_SAMPLES> _scale_check_groundspeed {};
|
||||
|
||||
void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; }
|
||||
|
||||
void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, float lpos_vx,
|
||||
float lpos_vy,
|
||||
float lpos_vz,
|
||||
float lpos_evh, float lpos_evv, const float att_q[4]);
|
||||
void update_CAS_scale();
|
||||
void update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz);
|
||||
void update_CAS_scale_applied();
|
||||
void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
|
||||
void check_airspeed_data_stuck(uint64_t timestamp);
|
||||
void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
|
||||
float estimator_status_mag_test_ratio);
|
||||
void check_load_factor(float accel_z);
|
||||
void update_airspeed_valid_status(const uint64_t timestamp);
|
||||
void reset();
|
||||
void reset_CAS_scale_check();
|
||||
|
||||
};
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
@ -149,13 +149,14 @@ private:
|
||||
bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
|
||||
float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
|
||||
float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
|
||||
|
||||
bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
|
||||
bool _armed_prev{false};
|
||||
|
||||
hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
|
||||
|
||||
perf_counter_t _perf_elapsed{};
|
||||
|
||||
float _param_airspeed_scale[MAX_NUM_AIRSPEED_SENSORS] {}; /** array to save the airspeed scale params in */
|
||||
|
||||
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,
|
||||
@ -163,8 +164,10 @@ private:
|
||||
(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_SCALE_APPLY>) _param_aspd_scale_apply,
|
||||
(ParamFloat<px4::params::ASPD_SCALE_1>) _param_airspeed_scale_1,
|
||||
(ParamFloat<px4::params::ASPD_SCALE_2>) _param_airspeed_scale_2,
|
||||
(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,
|
||||
@ -291,6 +294,12 @@ AirspeedModule::Run()
|
||||
|
||||
if (!_initialized) {
|
||||
init(); // initialize airspeed validator instances
|
||||
|
||||
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
|
||||
_airspeed_validator[i].set_CAS_scale_estimated(_param_airspeed_scale[i]);
|
||||
_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
|
||||
}
|
||||
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
@ -300,7 +309,7 @@ AirspeedModule::Run()
|
||||
update_params();
|
||||
}
|
||||
|
||||
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
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) {
|
||||
@ -375,11 +384,44 @@ AirspeedModule::Run()
|
||||
_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);
|
||||
|
||||
}
|
||||
|
||||
// save estimated airspeed scale after disarm
|
||||
if (!armed && _armed_prev) {
|
||||
if (_param_aspd_scale_apply.get() > 1) {
|
||||
if (fabsf(_airspeed_validator[i].get_CAS_scale_estimated() - _param_airspeed_scale[i]) > 0.01f) {
|
||||
// apply the new scale if changed more than 0.01
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.2f --> %.2f", i + 1,
|
||||
(double)_param_airspeed_scale[i],
|
||||
(double)_airspeed_validator[i].get_CAS_scale_estimated());
|
||||
|
||||
switch (i) {
|
||||
case 0:
|
||||
_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_estimated());
|
||||
_param_airspeed_scale_1.commit_no_notification();
|
||||
break;
|
||||
|
||||
case 1:
|
||||
_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_estimated());
|
||||
_param_airspeed_scale_2.commit_no_notification();
|
||||
break;
|
||||
|
||||
case 2:
|
||||
_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_estimated());
|
||||
_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()) {
|
||||
@ -391,6 +433,10 @@ void AirspeedModule::update_params()
|
||||
{
|
||||
updateParams();
|
||||
|
||||
_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();
|
||||
|
||||
_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
|
||||
_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
|
||||
_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
|
||||
@ -398,62 +444,24 @@ void AirspeedModule::update_params()
|
||||
_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 < _number_of_airspeed_sensors; i++) {
|
||||
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
|
||||
_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
|
||||
_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.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_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
|
||||
|
||||
// only apply manual entered airspeed scale to first airspeed measurement
|
||||
// TODO: enable multiple airspeed sensors
|
||||
_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.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_disable_tas_scale_estimate(_param_aspd_scale_apply.get() == 0);
|
||||
}
|
||||
|
||||
// if the airspeed scale estimation is enabled and the airspeed is valid,
|
||||
// then set the scale inside the wind estimator to -1 such that it starts to estimate it
|
||||
if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
|
||||
if (_valid_airspeed_index > 0) {
|
||||
// set it to a negative value to start estimation inside wind estimator
|
||||
_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
|
||||
events::send(events::ID("airspeed_selector_cannot_est_scale"), events::Log::Error,
|
||||
"Airspeed: cannot estimate scale as there is no valid sensor");
|
||||
_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
|
||||
_param_west_scale_estimation_on.commit_no_notification();
|
||||
}
|
||||
|
||||
} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
|
||||
if (_valid_airspeed_index > 0) {
|
||||
|
||||
_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
|
||||
_param_west_airspeed_scale.commit_no_notification();
|
||||
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
|
||||
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f\t",
|
||||
(double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
|
||||
events::send<float>(events::ID("airspeed_selector_scale"), events::Log::Info,
|
||||
"Airspeed: estimated scale (ASPD_SCALE): {1:.2}", _airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
|
||||
events::send(events::ID("airspeed_selector_cannot_est_scale2"), events::Log::Error,
|
||||
"Airspeed: cannot estimate scale as there is no valid sensor");
|
||||
}
|
||||
}
|
||||
|
||||
_scale_estimation_previously_on = _param_west_scale_estimation_on.get();
|
||||
|
||||
}
|
||||
|
||||
void AirspeedModule::poll_topics()
|
||||
@ -505,11 +513,12 @@ void AirspeedModule::update_wind_estimator_sideslip()
|
||||
_wind_estimator_sideslip.get_wind_var(wind_cov);
|
||||
_wind_estimate_sideslip.variance_north = wind_cov[0];
|
||||
_wind_estimate_sideslip.variance_east = wind_cov[1];
|
||||
_wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov();
|
||||
_wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var();
|
||||
_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 = _wind_estimator_sideslip.get_tas_scale();
|
||||
_wind_estimate_sideslip.tas_scale = NAN;
|
||||
_wind_estimate_sideslip.tas_scale_var = NAN;
|
||||
_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
|
||||
}
|
||||
|
||||
|
||||
@ -7,6 +7,7 @@
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @unit m/s^2
|
||||
* @decimal 2
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
|
||||
@ -19,9 +20,10 @@ PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
|
||||
* @min 0
|
||||
* @max 0.1
|
||||
* @unit Hz
|
||||
* @decimal 5
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
|
||||
PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001f);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Wind estimator true airspeed measurement noise
|
||||
@ -31,9 +33,10 @@ PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
|
||||
* @min 0
|
||||
* @max 4
|
||||
* @unit m/s
|
||||
* @decimal 1
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
|
||||
PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Wind estimator sideslip measurement noise
|
||||
@ -43,9 +46,10 @@ PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @unit rad
|
||||
* @decimal 3
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3);
|
||||
PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Gate size for true airspeed fusion
|
||||
@ -72,27 +76,57 @@ PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3);
|
||||
PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1);
|
||||
|
||||
/**
|
||||
* Automatic airspeed scale estimation on
|
||||
* Controls when to apply the new esstimated airspeed scale
|
||||
*
|
||||
* Turns the automatic airspeed scale (scale from IAS to CAS) on or off. It is recommended to fly level
|
||||
* altitude while performing the estimation. Set to 1 to start estimation (best when already flying).
|
||||
* Set to 0 to end scale estimation. The estimated scale is then saved using the ASPD_SCALE parameter.
|
||||
*
|
||||
* @boolean
|
||||
* @value 0 Disable airspeed scale estimation completely
|
||||
* @value 1 Do not apply the new gains (logging and inside wind estimator)
|
||||
* @value 2 Apply the new scale after disarm
|
||||
* @value 3 Apply the new gains in air
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0);
|
||||
PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 2);
|
||||
|
||||
/**
|
||||
* Airspeed scale (scale from IAS to CAS)
|
||||
* Scale of airspeed sensor 1
|
||||
*
|
||||
* Scale can either be entered manually, or estimated in-flight by setting ASPD_SCALE_EST to 1.
|
||||
* This is the scale IAS --> CAS of the first airspeed sensor instance
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 1.5
|
||||
* @max 2.0
|
||||
* @decimal 2
|
||||
* @reboot_required true
|
||||
* @group Airspeed Validator
|
||||
* @volatile
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_SCALE, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(ASPD_SCALE_1, 1.0f);
|
||||
|
||||
/**
|
||||
* Scale of airspeed sensor 2
|
||||
*
|
||||
* This is the scale IAS --> CAS of the second airspeed sensor instance
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 2.0
|
||||
* @decimal 2
|
||||
* @reboot_required true
|
||||
* @group Airspeed Validator
|
||||
* @volatile
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_SCALE_2, 1.0f);
|
||||
|
||||
/**
|
||||
* Scale of airspeed sensor 3
|
||||
*
|
||||
* This is the scale IAS --> CAS of the third airspeed sensor instance
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 2.0
|
||||
* @decimal 2
|
||||
* @reboot_required true
|
||||
* @group Airspeed Validator
|
||||
* @volatile
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_SCALE_3, 1.0f);
|
||||
|
||||
/**
|
||||
* Index or primary airspeed measurement source
|
||||
@ -142,6 +176,7 @@ PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0);
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 3.0
|
||||
* @decimal 1
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
|
||||
@ -156,6 +191,7 @@ PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
|
||||
*
|
||||
* @unit s
|
||||
* @max 30.0
|
||||
* @decimal 1
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user