From 9ad0e5b35794fae336d79f0bfe1572e1a353f84c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= <igy1000mb@gmail.com>
Date: Wed, 22 Sep 2021 12:37:28 +0200
Subject: [PATCH] mavlink_receiver: updated sens flow parameters to be
 dynamically handle

---
 src/modules/mavlink/mavlink_receiver.cpp | 35 +++++++++++++++++++++---
 src/modules/mavlink/mavlink_receiver.h   | 20 ++++++++++----
 2 files changed, 46 insertions(+), 9 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 6449bd9c52..d3528261d6 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -88,6 +88,10 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_parameters_manager(parent),
 	_mavlink_timesync(parent)
 {
+	_handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT");
+	_handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR");
+	_handle_sens_flow_minhgt = param_find("SENS_FLOW_MINHGT");
+	_handle_sens_flow_rot = param_find("SENS_FLOW_ROT");
 }
 
 void
@@ -693,12 +697,12 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 	f.ground_distance_m     = flow.distance;
 	f.quality               = flow.quality;
 	f.sensor_id             = flow.sensor_id;
-	f.max_flow_rate         = _param_sens_flow_maxr.get();
-	f.min_ground_distance   = _param_sens_flow_minhgt.get();
-	f.max_ground_distance   = _param_sens_flow_maxhgt.get();
+	f.max_flow_rate         = _param_sens_flow_maxr;
+	f.min_ground_distance   = _param_sens_flow_minhgt;
+	f.max_ground_distance   = _param_sens_flow_maxhgt;
 
 	/* read flow sensor parameters */
-	const Rotation flow_rot = (Rotation)_param_sens_flow_rot.get();
+	const Rotation flow_rot = (Rotation)_param_sens_flow_rot;
 
 	/* rotate measurements according to parameter */
 	float zero_val = 0.0f;
@@ -3339,6 +3343,29 @@ void MavlinkReceiver::start()
 	pthread_attr_destroy(&receiveloop_attr);
 }
 
+void
+MavlinkReceiver::updateParams()
+{
+	// update parameters from storage
+	ModuleParams::updateParams();
+
+	if (_handle_sens_flow_maxhgt != PARAM_INVALID) {
+		param_get(_handle_sens_flow_maxhgt, &_param_sens_flow_maxhgt);
+	}
+
+	if (_handle_sens_flow_maxr != PARAM_INVALID) {
+		param_get(_handle_sens_flow_maxr, &_param_sens_flow_maxr);
+	}
+
+	if (_handle_sens_flow_minhgt != PARAM_INVALID) {
+		param_get(_handle_sens_flow_minhgt, &_param_sens_flow_minhgt);
+	}
+
+	if (_handle_sens_flow_rot != PARAM_INVALID) {
+		param_get(_handle_sens_flow_rot, &_param_sens_flow_rot);
+	}
+}
+
 void *MavlinkReceiver::start_trampoline(void *context)
 {
 	MavlinkReceiver *self = reinterpret_cast<MavlinkReceiver *>(context);
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 5b7cf1f998..70957dd934 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -232,6 +232,10 @@ private:
 
 	px4::atomic_bool 	_should_exit{false};
 	pthread_t		_thread {};
+	/**
+	 * @brief Updates optical flow parameters.
+	 */
+	void updateParams() override;
 
 	Mavlink				*_mavlink;
 
@@ -375,14 +379,20 @@ private:
 	hrt_abstime _heartbeat_component_udp_bridge{0};
 	hrt_abstime _heartbeat_component_uart_bridge{0};
 
+	param_t _handle_sens_flow_maxhgt{PARAM_INVALID};
+	param_t _handle_sens_flow_maxr{PARAM_INVALID};
+	param_t _handle_sens_flow_minhgt{PARAM_INVALID};
+	param_t _handle_sens_flow_rot{PARAM_INVALID};
+
+	float _param_sens_flow_maxhgt{-1.0f};
+	float _param_sens_flow_maxr{-1.0f};
+	float _param_sens_flow_minhgt{-1.0f};
+	float _param_sens_flow_rot{-1.0f};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,
-		(ParamFloat<px4::params::BAT_LOW_THR>)      _param_bat_low_thr,
-		(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
-		(ParamFloat<px4::params::SENS_FLOW_MAXR>)   _param_sens_flow_maxr,
-		(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
-		(ParamInt<px4::params::SENS_FLOW_ROT>)      _param_sens_flow_rot
+		(ParamFloat<px4::params::BAT_LOW_THR>)      _param_bat_low_thr
 	);
 
 	// Disallow copy construction and move assignment.