From b0dc3a4f5182e7bceac8d6037eff6a028c8246a9 Mon Sep 17 00:00:00 2001
From: Thomas Stastny <thomas.stastny@mavt.ethz.ch>
Date: Fri, 10 Sep 2021 13:32:02 +0200
Subject: [PATCH] add option to disable use of wind estimates with npfg

---
 msg/npfg_status.msg                                       | 2 +-
 .../fw_pos_control_l1/FixedwingPositionControl.cpp        | 6 ++++++
 .../fw_pos_control_l1/FixedwingPositionControl.hpp        | 1 +
 src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c  | 8 ++++++++
 4 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/msg/npfg_status.msg b/msg/npfg_status.msg
index 22df3d5fa6..0c7c29b5cd 100644
--- a/msg/npfg_status.msg
+++ b/msg/npfg_status.msg
@@ -1,6 +1,6 @@
 uint64 timestamp                # time since system start (microseconds)
 
-uint8 wind_est_valid            # (boolean) true = wind estimate is valid and being used by controller
+uint8 wind_est_valid            # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid)
 float32 lat_accel               # resultant lateral acceleration reference [m/s^2]
 float32 lat_accel_ff            # lateral acceleration demand only for maintaining curvature [m/s^2]
 float32 bearing_feas            # bearing feasibility [0,1]
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index e9d527cc08..015a49b9e3 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -269,6 +269,12 @@ FixedwingPositionControl::wind_poll()
 			_wind_vel(1) = 0.0f;
 		}
 	}
+
+	if (!_param_npfg_en_wind_estimates.get()) {
+		_wind_valid = false;
+		_wind_vel(0) = 0.0f;
+		_wind_vel(1) = 0.0f;
+	}
 }
 
 void
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index 6f35272913..1ccedbb59a 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -389,6 +389,7 @@ private:
 		(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
 		(ParamFloat<px4::params::NPFG_ASPD_BUF>) _param_npfg_airspeed_buffer,
 		(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
+		(ParamBool<px4::params::NPFG_EN_WIND_EST>) _param_npfg_en_wind_estimates,
 
 		(ParamFloat<px4::params::FW_LND_AIRSPD_SC>) _param_fw_lnd_airspd_sc,
 		(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index f837ad8ab0..a25b218a37 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -252,6 +252,14 @@ PARAM_DEFINE_FLOAT(NPFG_ASPD_BUF, 1.5f);
  */
 PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f);
 
+/**
+ * Enable use of wind estimates for NPFG. Disabling, controller assumes zero wind.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_EN_WIND_EST, 1);
+
 /**
  * Cruise throttle
  *