fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

At every consumation of AirspeedValidated, check for SOURCE

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan 2025-04-04 16:56:40 +02:00 committed by Silvan Fuhrer
parent 5842c991ec
commit f7bb5d13f7
4 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp
View File

@ -54,6 +54,12 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
reporter.setIsPresent(health_component_t::differential_pressure);
const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1
|| airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2
|| airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3;
const float airspeed_calibrated_from_sensor = airspeed_from_sensor ? airspeed_validated.calibrated_airspeed_m_s : NAN;
// Maximally allow the airspeed reading to be at FW_AIRSPD_MAX when arming. This is to catch very badly calibrated
// airspeed sensors, but also high wind conditions that prevent a forward flight of the vehicle.
float arming_max_airspeed_allowed = 20.f;
@ -62,7 +68,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
/*
* Check if airspeed is declared valid or not by airspeed selector.
*/
if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
if (!PX4_ISFINITE(airspeed_calibrated_from_sensor)) {
/* EVENT
*/
@ -75,7 +81,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
}
}
if (!context.isArmed() && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) {
if (!context.isArmed() && airspeed_calibrated_from_sensor > arming_max_airspeed_allowed) {
/* EVENT
* @description
* Current airspeed reading too high. Check if wind is below maximum airspeed and redo airspeed

4
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -214,8 +214,10 @@ FixedwingPositionControl::airspeed_poll()
_eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed
// do not use synthetic airspeed as this would create a thrust loop
if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
&& PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) {
&& PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
&& airspeed_validated.airspeed_source != airspeed_validated_s::SYNTHETIC) {
airspeed_valid = true;

9
src/modules/land_detector/FixedwingLandDetector.cpp
View File

@ -93,10 +93,15 @@ bool FixedwingLandDetector::_get_landed_state()
airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated);
const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1
|| airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2
|| airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3;
bool airspeed_invalid = false;
// set _airspeed_filtered to 0 if airspeed data is invalid
if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
// set _airspeed_filtered to 0 if airspeed data is invalid or not from an actual airspeed sensor
if (!airspeed_from_sensor || !PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
|| hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
_airspeed_filtered = 0.0f;
airspeed_invalid = true;

6
src/modules/mavlink/streams/VFR_HUD.hpp
View File

@ -82,9 +82,13 @@ private:
airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated);
const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1
|| airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2
|| airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3;
mavlink_vfr_hud_t msg{};
msg.airspeed = airspeed_validated.calibrated_airspeed_m_s;
// display NAN in case of source not being one of the sensors
msg.airspeed = airspeed_from_sensor ? airspeed_validated.calibrated_airspeed_m_s : NAN;
msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy);
msg.heading = math::degrees(matrix::wrap_2pi(lpos.heading));