From 22180a2639bd679d868563b5fa4fde8674f5249c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Dec 2021 14:59:16 +0100
Subject: [PATCH] mavlink: make range params optional

If there is no range sensor compiled in, these two params do not seem to
be available. Hence, we need to make them optional.
---
 src/modules/mavlink/mavlink_receiver.cpp | 14 ++++++++++++--
 src/modules/mavlink/mavlink_receiver.h   |  8 +++++---
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 24ee4bb608..f29a8bac13 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -94,6 +94,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_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");
+	_handle_ekf2_min_rng = param_find("EKF2_MIN_RNG");
+	_handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX");
 }
 
 void
@@ -829,8 +831,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 		device_id.devid_s.address = msg->sysid;
 
 		d.timestamp = f.timestamp;
-		d.min_distance = _param_ekf2_min_rng.get();
-		d.max_distance = _param_ekf2_rng_a_hmax.get();
+		d.min_distance = _param_ekf2_min_rng;
+		d.max_distance = _param_ekf2_rng_a_hmax;
 		d.current_distance = flow.distance; /* both are in m */
 		d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
 		d.device_id = device_id.devid;
@@ -3448,6 +3450,14 @@ MavlinkReceiver::updateParams()
 	if (_handle_sens_flow_rot != PARAM_INVALID) {
 		param_get(_handle_sens_flow_rot, &_param_sens_flow_rot);
 	}
+
+	if (_handle_ekf2_min_rng != PARAM_INVALID) {
+		param_get(_handle_ekf2_min_rng, &_param_ekf2_min_rng);
+	}
+
+	if (_handle_ekf2_rng_a_hmax != PARAM_INVALID) {
+		param_get(_handle_ekf2_rng_a_hmax, &_param_ekf2_rng_a_hmax);
+	}
 }
 
 void *MavlinkReceiver::start_trampoline(void *context)
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index d81c361d53..2a929ea594 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -394,18 +394,20 @@ private:
 	param_t _handle_sens_flow_maxr{PARAM_INVALID};
 	param_t _handle_sens_flow_minhgt{PARAM_INVALID};
 	param_t _handle_sens_flow_rot{PARAM_INVALID};
+	param_t _handle_ekf2_min_rng{PARAM_INVALID};
+	param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID};
 
 	float _param_sens_flow_maxhgt{-1.0f};
 	float _param_sens_flow_maxr{-1.0f};
 	float _param_sens_flow_minhgt{-1.0f};
 	int32_t _param_sens_flow_rot{0};
+	float _param_ekf2_min_rng{NAN};
+	float _param_ekf2_rng_a_hmax{NAN};
 
 	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::EKF2_MIN_RNG>)     _param_ekf2_min_rng,
-		(ParamFloat<px4::params::EKF2_RNG_A_HMAX>)  _param_ekf2_rng_a_hmax
+		(ParamFloat<px4::params::BAT_LOW_THR>)      _param_bat_low_thr
 	);
 
 	// Disallow copy construction and move assignment.