From aefbd80b5394e8926e3d7d7d62e7c8bb129fef9f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 9 Sep 2021 17:38:47 +0200 Subject: [PATCH] mavlink: use optical flow limits from params This makes more sense than hard-coding arbitrary values. --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/modules/mavlink/mavlink_receiver.h | 4 +++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6690821edb..713e2dc9d5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -829,8 +829,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 = 0.3f; - d.max_distance = 5.0f; + d.min_distance = _param_ekf2_min_rng.get(); + d.max_distance = _param_ekf2_rng_a_hmax.get(); d.current_distance = flow.distance; /* both are in m */ d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; d.device_id = device_id.devid; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 55fb16df96..d81c361d53 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -403,7 +403,9 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, - (ParamFloat) _param_bat_low_thr + (ParamFloat) _param_bat_low_thr, + (ParamFloat) _param_ekf2_min_rng, + (ParamFloat) _param_ekf2_rng_a_hmax ); // Disallow copy construction and move assignment.