From b117fddb2188e686174244a98741a3b4deb9f425 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 19 Sep 2017 10:03:59 +1000
Subject: [PATCH] ekf2: publish estimator position control limits
---
src/modules/ekf2/ekf2_main.cpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 71504fd881..dea416a520 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -1022,6 +1022,14 @@ void Ekf2::run()
_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
+ // get control limit information
+ _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.limit_hagl);
+
+ // convert NaN to zero
+ if (!PX4_ISFINITE(lpos.vxy_max)) {
+ lpos.vxy_max = 0.0f;
+ }
+
// publish vehicle local position data
_vehicle_local_position_pub.update();
--
GitLab