From 4eb930f704018253a4cff3604cb9faee8468283b Mon Sep 17 00:00:00 2001
From: Steven Blass <steven.r.blass@gmail.com>
Date: Tue, 18 Nov 2014 08:26:29 -0500
Subject: [PATCH] improved readability of offboard fixes
---
src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++++----
src/modules/navigator/navigator.h | 1 +
src/modules/navigator/navigator_main.cpp | 6 +++---
3 files changed, 12 insertions(+), 7 deletions(-)
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c0a8d7322f..ca00d1a67d 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -538,12 +538,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
offboard_control_sp.ignore &= ~(1 << i);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i));
}
+
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW);
- offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) <<
- (OFB_IGN_BIT_YAW - 10);
+ if (set_position_target_local_ned.type_mask & (1 << 10)) {
+ offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW);
+ }
+
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
- offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) <<
- (OFB_IGN_BIT_YAWRATE - 11);
+ if (set_position_target_local_ned.type_mask & (1 << 11)) {
+ offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE);
+ }
offboard_control_sp.timestamp = hrt_absolute_time();
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d550dcc4cb..9cd609955b 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -205,6 +205,7 @@ private:
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
+ bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 5e98064996..035b0d18c8 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -137,6 +137,7 @@ Navigator::Navigator() :
_gpsFailure(this, "GPSF"),
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
+ _pos_sp_triplet_published_invalid_once(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -469,9 +470,8 @@ Navigator::task_main()
}
/* if nothing is running, set position setpoint triplet invalid once */
- static bool published_once = false;
- if (_navigation_mode == nullptr && !published_once) {
- published_once = true;
+ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
+ _pos_sp_triplet_published_invalid_once = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
--
GitLab