From 5f6d03099e07e20abe9cec685fee1244d7c9a0d4 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Mon, 17 Nov 2014 19:58:18 -0500 Subject: fixed yaw/yawrate bit masking. fixed navigator overriding offboard setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bc092c7e9..c0a8d7322 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,10 +540,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } 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; + (OFB_IGN_BIT_YAW - 10); 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; + (OFB_IGN_BIT_YAWRATE - 11); offboard_control_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 10a4ee88f..5e9806499 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -468,9 +468,10 @@ Navigator::task_main() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } - /* if nothing is running, set position setpoint triplet invalid */ - if (_navigation_mode == nullptr) { - // TODO publish empty sp only once + /* if nothing is running, set position setpoint triplet invalid once */ + static bool published_once = false; + if (_navigation_mode == nullptr && !published_once) { + published_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; -- cgit v1.2.3 From 4eb930f704018253a4cff3604cb9faee8468283b Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Tue, 18 Nov 2014 08:26:29 -0500 Subject: 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 c0a8d7322..ca00d1a67 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 d550dcc4c..9cd609955 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 5e9806499..035b0d18c 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; -- cgit v1.2.3 From 69271a7251a0bcfd980c3534194b73fc45fa5f23 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Tue, 18 Nov 2014 08:40:52 -0500 Subject: made invalid setpoints publish once every time it enters an invalid state --- src/modules/navigator/navigator_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 035b0d18c..df620e5e7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -428,12 +428,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RCRECOVER: + _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { @@ -441,11 +444,13 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ + _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { @@ -453,9 +458,11 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: -- cgit v1.2.3