From 63a53d48e7b91765e4eaef4ee4dc5d6b168b393d Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Thu, 11 Mar 2021 19:27:09 +0100
Subject: [PATCH] FW Position controller: improve parameter sanity checks
 (provide more feedback)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 .../FixedwingPositionControl.cpp              | 32 +++++++++++++------
 .../FixedwingPositionControl.hpp              |  1 +
 2 files changed, 23 insertions(+), 10 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index d75aeb2b2e..355a32f0fb 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -141,19 +141,31 @@ FixedwingPositionControl::parameters_update()
 
 	landing_status_publish();
 
+	int check_ret = PX4_OK;
+
 	// sanity check parameters
-	if ((_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) ||
-	    (_param_fw_airspd_max.get() < 5.0f) ||
-	    (_param_fw_airspd_min.get() > 100.0f) ||
-	    (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get()) ||
-	    (_param_fw_airspd_trim.get() > _param_fw_airspd_max.get())) {
-
-		mavlink_log_critical(&_mavlink_log_pub, "Airspeed parameters invalid");
-
-		return PX4_ERROR;
+	if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed min smaller than max");
+		check_ret = PX4_ERROR;
 	}
 
-	return PX4_OK;
+	if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s");
+		check_ret = PX4_ERROR;
+	}
+
+	if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() ||
+	    _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds");
+		check_ret = PX4_ERROR;
+	}
+
+	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get() * 0.9f) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min");
+		check_ret = PX4_ERROR;
+	}
+
+	return check_ret;
 }
 
 void
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index d6080394cd..b41181a8e6 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -353,6 +353,7 @@ private:
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 
 		(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff,