Skip to content

Commit

Permalink
AirspeedSelector: only update with lpos if coming from GNSS (#23268)
Browse files Browse the repository at this point in the history 
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 <[email protected]>
  • Loading branch information
@sfuhrer
sfuhrer authored Jun 14, 2024
1 parent c0d6b67 commit f646f1b
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 22 deletions.
18 changes: 9 additions & 9 deletions src/modules/airspeed_selector/AirspeedValidator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand All @@ -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;
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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.
Expand All @@ -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;
Expand Down
8 changes: 4 additions & 4 deletions src/modules/airspeed_selector/AirspeedValidator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -175,15 +175,15 @@ class AirspeedValidator

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();
Expand Down
18 changes: 9 additions & 9 deletions src/modules/airspeed_selector/airspeed_selector_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
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 */
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -515,18 +515,18 @@ 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()
{
// 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);
Expand Down Expand Up @@ -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;

Expand Down

0 comments on commit f646f1b

Please sign in to comment.