From 55e3f80bab1f12de0cbd17696ee5c699cb5971f4 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 1 Jan 2019 16:20:30 -0500
Subject: [PATCH] mavlink handle MAV_ODOM_LP parameter in main
- fixes #11125
---
src/modules/mavlink/mavlink_main.h | 9 +++++----
src/modules/mavlink/mavlink_messages.cpp | 6 +++---
src/modules/mavlink/mavlink_stream.cpp | 1 -
src/modules/mavlink/mavlink_stream.h | 4 ----
4 files changed, 8 insertions(+), 12 deletions(-)
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index e2c21e556e..d98d1ad743 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -494,9 +494,9 @@ public:
bool ftp_enabled() const { return _ftp_on; }
- bool hash_check_enabled() { return _param_hash_check_enabled.get(); }
-
- bool forward_heartbeats_enabled() { return _param_heartbeat_forwarding_enabled.get(); }
+ bool hash_check_enabled() const { return _param_hash_check_enabled.get(); }
+ bool forward_heartbeats_enabled() const { return _param_heartbeat_forwarding_enabled.get(); }
+ bool odometry_loopback_enabled() const { return _param_send_odom_loopback.get(); }
struct ping_statistics_s {
uint64_t last_ping_time;
@@ -642,7 +642,8 @@ private:
(ParamBool<px4::params::MAV_FWDEXTSP>) _param_forward_externalsp,
(ParamInt<px4::params::MAV_BROADCAST>) _param_broadcast_mode,
(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_hash_check_enabled,
- (ParamBool<px4::params::MAV_HB_FORW_EN>) _param_heartbeat_forwarding_enabled
+ (ParamBool<px4::params::MAV_HB_FORW_EN>) _param_heartbeat_forwarding_enabled,
+ (ParamBool<px4::params::MAV_ODOM_LP>) _param_send_odom_loopback
)
perf_counter_t _loop_perf; /**< loop performance counter */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 62b399f954..366e9b84f2 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2318,7 +2318,7 @@ protected:
mavlink_odometry_t msg = {};
- if (_send_odom_loopback.get()) {
+ if (_mavlink->odometry_loopback_enabled()) {
odom_updated = _vodom_sub->update(&_vodom_time, &odom);
// frame matches the external vision system
msg.frame_id = MAV_FRAME_VISION_NED;
@@ -2361,8 +2361,8 @@ protected:
msg.yawspeed = odom.yawspeed;
// get the covariance matrix size
- const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
- const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
+ static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
+ static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
static_assert(VEL_URT_SIZE == (sizeof(msg.twist_covariance) / sizeof(msg.twist_covariance[0])),
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index f3d6c2b2cc..f95587c833 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -58,7 +58,6 @@ int
MavlinkStream::update(const hrt_abstime &t)
{
update_data();
- ModuleParams::updateParams();
// If the message has never been sent before we want
// to send it immediately and can return right away
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index e982c61e97..327911f633 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -127,10 +127,6 @@ protected:
*/
virtual void update_data() { }
- DEFINE_PARAMETERS(
- (ParamBool<px4::params::MAV_ODOM_LP>) _send_odom_loopback
- )
-
private:
hrt_abstime _last_sent{0};
bool _first_message_sent{false};
--
GitLab