mavlink: use optical flow limits from params

This makes more sense than hard-coding arbitrary values.
This commit is contained in:
Julian Oes 2021-09-09 17:38:47 +02:00 committed by Daniel Agar
parent f48c3a2cc6
commit aefbd80b53
2 changed files with 5 additions and 3 deletions

View File

@ -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;

View File

@ -403,7 +403,9 @@ private:
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::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
);
// Disallow copy construction and move assignment.