From a7dd6b50e302cd8aea23b7775cc120febf896fbc Mon Sep 17 00:00:00 2001 From: Artur Khylskyi Date: Sun, 29 Mar 2026 02:36:45 +0200 Subject: [PATCH] Add lowairspeed binding and logic for airspeed monitoring --- ExtLibs/ArduPilot/CurrentState.cs | 33 +++++++++++++++++++++++++++++++ GCSViews/FlightData.Designer.cs | 1 + 2 files changed, 34 insertions(+) diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs index 70e05a1e90..d8a7e42e88 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -503,6 +503,8 @@ public float airspeed public float targetairspeed { get; private set; } public bool lowairspeed { get; set; } + private float _cachedAirspeedMin = 0; + private DateTime _lastAirspeedMinCheck = DateTime.MinValue; [DisplayFieldName("asratio.Field")] [DisplayText("Airspeed Ratio")] @@ -3835,6 +3837,37 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi //This comes from the EKF, so it supposed to be correct climbrate = vfr.climb; gotVFR = true; // we have a vfr packet + + if ((timeSinceArmInAir > 0) && (DateTime.Now - _lastAirspeedMinCheck).TotalSeconds > 5) + { + try + { + if (parent?.param != null) + { + if (parent.param.ContainsKey("AIRSPEED_MIN")) + { + _cachedAirspeedMin = (float)parent.param["AIRSPEED_MIN"].Value; + } + else if (parent.param.ContainsKey("ARSPD_FBW_MIN")) + { + _cachedAirspeedMin = (float)parent.param["ARSPD_FBW_MIN"].Value; + } + } + } + catch (Exception ex) + { + log.Debug("Error getting AIRSPEED_MIN or ARSPD_FBW_MIN from param for lowairspeed handling", ex); + } + + _lastAirspeedMinCheck = DateTime.Now; + } + + lowairspeed = armed + && (timeSinceArmInAir > 0) + && (_cachedAirspeedMin > 0) + && sensors_enabled.differential_pressure + && sensors_health.differential_pressure + && (vfr.airspeed < _cachedAirspeedMin); } break; diff --git a/GCSViews/FlightData.Designer.cs b/GCSViews/FlightData.Designer.cs index 3eeea28fc8..3795864449 100644 --- a/GCSViews/FlightData.Designer.cs +++ b/GCSViews/FlightData.Designer.cs @@ -395,6 +395,7 @@ private void InitializeComponent() this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("AOA", this.bindingSourceHud, "AOA", true)); this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("SSA", this.bindingSourceHud, "SSA", true)); this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("critAOA", this.bindingSourceHud, "crit_AOA", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("lowairspeed", this.bindingSourceHud, "lowairspeed", true)); this.hud1.datetime = new System.DateTime(((long)(0))); this.hud1.displayAOASSA = false; this.hud1.displayCellVoltage = false;