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

AirspeedSelector: only update with lpos if coming from GNSS (#23268)

Browse Source 
Compared to GNSS, alternate position observation methods are less accurate
and thus generally not good enough to do airspeed validation with.
Airspeed validation is thus disabled if no GNSS fusion is happening.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-06-14 17:10:08 +02:00
committed by GitHub
parent c0d6b67633
commit f646f1ba98
3 changed files with 22 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+9 -9
View File
@@ -50,16 +50,16 @@ 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_validated(input_data.lpos_valid, input_data.ground_velocity, input_data.airspeed_true_raw);
update_CAS_scale_validated(input_data.gnss_valid, input_data.ground_velocity, input_data.airspeed_true_raw);
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,
update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.gnss_valid,
input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.q_att);
update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
check_airspeed_data_stuck(input_data.timestamp);
check_load_factor(input_data.accel_z);
check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio,
input_data.ground_velocity, input_data.lpos_valid);
input_data.ground_velocity, input_data.gnss_valid);
update_airspeed_valid_status(input_data.timestamp);
}
@@ -71,12 +71,12 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp)
}
void
AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool gnss_valid,
const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const Quatf &q_att)
{
_wind_estimator.update(time_now_usec);
if (lpos_valid && _in_fixed_wing_flight) {
if (gnss_valid && _in_fixed_wing_flight) {
// airspeed fusion (with raw TAS)
const float hor_vel_variance = lpos_evh * lpos_evh;
@@ -109,9 +109,9 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
}
void
AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw)
AirspeedValidator::update_CAS_scale_validated(bool gnss_valid, const matrix::Vector3f &vI, float airspeed_true_raw)
{
if (!_in_fixed_wing_flight || !lpos_valid) {
if (!_in_fixed_wing_flight || !gnss_valid) {
return;
}
@@ -212,7 +212,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
void
AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid)
float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid)
{
// Check normalised innovation levels with requirement for continuous data and use of hysteresis
// to prevent false triggering.
@@ -226,7 +226,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
_innovations_check_failed = false;
_aspd_innov_integ_state = 0.f;
} else if (!lpos_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
} else if (!gnss_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
//nav velocity data is likely not good
//don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good
_aspd_innov_integ_state = 0.f;
src/modules/airspeed_selector/AirspeedValidator.hpp
+4 -4
View File
@@ -56,7 +56,7 @@ struct airspeed_validator_update_data {
float airspeed_true_raw;
uint64_t airspeed_timestamp;
matrix::Vector3f ground_velocity;
bool lpos_valid;
bool gnss_valid;
float lpos_evh;
float lpos_evv;
matrix::Quatf q_att;
@@ -175,15 +175,15 @@ private:
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,
void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool gnss_valid,
const matrix::Vector3f &vI,
float lpos_evh, float lpos_evv, const Quatf &q_att);
void update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw);
void update_CAS_scale_validated(bool gnss_valid, const matrix::Vector3f &vI, float airspeed_true_raw);
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, const matrix::Vector3f &vI, bool lpos_valid);
float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid);
void check_load_factor(float accel_z);
void update_airspeed_valid_status(const uint64_t timestamp);
void reset();
src/modules/airspeed_selector/airspeed_selector_main.cpp
+9 -9
View File
@@ -146,7 +146,7 @@ private:
int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */
int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */
bool _initialized{false}; /**< module initialized*/
bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
bool _gnss_lpos_valid{false}; /**< local position (from GNSS) valid */
bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */
float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */
@@ -347,7 +347,7 @@ AirspeedModule::Run()
struct airspeed_validator_update_data input_data = {};
input_data.timestamp = _time_now_usec;
input_data.ground_velocity = vI;
input_data.lpos_valid = _vehicle_local_position_valid;
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;
@@ -515,10 +515,10 @@ void AirspeedModule::poll_topics()
}
}
_vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
&& (_vehicle_local_position.timestamp > 0)
&& _vehicle_local_position.v_xy_valid
&& !_vehicle_local_position.dead_reckoning;
_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_GPS);
}
void AirspeedModule::update_wind_estimator_sideslip()
@@ -526,7 +526,7 @@ void AirspeedModule::update_wind_estimator_sideslip()
// update wind and airspeed estimator
_wind_estimator_sideslip.update(_time_now_usec);
if (_vehicle_local_position_valid
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);
@@ -603,8 +603,8 @@ void AirspeedModule::select_airspeed_and_publish()
if (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
|| _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX) {
// _vehicle_local_position_valid determines if ground-wind estimate is valid
if (_vehicle_local_position_valid &&
// _gnss_lpos_valid determines if ground-wind estimate is valid
if (_gnss_lpos_valid &&
(_param_airspeed_fallback_gw.get() || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
_valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;