From f41de2d49f9ea7e8143b3951035e60f70aa1b00a Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 5 Aug 2019 08:44:27 +0200 Subject: [PATCH 1/4] Maneuver/FollowReference: dispatch FollowReference before signalError If after, it is not dispatched --- src/Maneuver/FollowReference/UAV/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Maneuver/FollowReference/UAV/Task.cpp b/src/Maneuver/FollowReference/UAV/Task.cpp index 1d817147fd..84ba76b460 100644 --- a/src/Maneuver/FollowReference/UAV/Task.cpp +++ b/src/Maneuver/FollowReference/UAV/Task.cpp @@ -441,8 +441,8 @@ namespace Maneuver if (delta > m_spec.timeout && isActive()) { m_fref_state.state = IMC::FollowRefState::FR_TIMEOUT; - signalError("reference source timed out"); dispatch(m_fref_state); + signalError("reference source timed out"); } } From 9233527503c3f2be41047675b30d0331e150bf51 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 5 Aug 2019 08:45:41 +0200 Subject: [PATCH 2/4] Maneuvers/FollowReference: always consume EstimatedState Is needed upon consume FollowReference --- src/Maneuver/FollowReference/UAV/Task.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Maneuver/FollowReference/UAV/Task.cpp b/src/Maneuver/FollowReference/UAV/Task.cpp index 84ba76b460..b09973f442 100644 --- a/src/Maneuver/FollowReference/UAV/Task.cpp +++ b/src/Maneuver/FollowReference/UAV/Task.cpp @@ -99,7 +99,8 @@ namespace Maneuver bindToManeuver(); bind(this); - bind(this); + //Always consume EstimatedState, as it is needed by consume(FollowReference) + bind(this,true); } void From b59f9728b5a6a41b846ba5de610c3207eaf2ffae Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 5 Aug 2019 08:48:51 +0200 Subject: [PATCH 3/4] Maneuver/FollowReference: always consume Reference, and only use EstimatedState if not received Reference This fixes a bug where the reference would jump to the current position at the start of a FollowReference Maneuver --- src/Maneuver/FollowReference/UAV/Task.cpp | 34 +++++++++++++++-------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/src/Maneuver/FollowReference/UAV/Task.cpp b/src/Maneuver/FollowReference/UAV/Task.cpp index b09973f442..e7e8af8a69 100644 --- a/src/Maneuver/FollowReference/UAV/Task.cpp +++ b/src/Maneuver/FollowReference/UAV/Task.cpp @@ -98,7 +98,8 @@ namespace Maneuver m_last_ref_time = 0; bindToManeuver(); - bind(this); + //Always consume Reference, as it is needed by consume(FollowReference) + bind(this,true); //Always consume EstimatedState, as it is needed by consume(FollowReference) bind(this,true); } @@ -108,15 +109,21 @@ namespace Maneuver { // initialize maneuver definitions and time for timeout calculation. m_spec = *msg; - m_last_ref_time = Clock::get(); + // assume that we have received a Reference message already. + // If not, or if it came from a source that is now non-authorized, set ref from estate + if(((Clock::get() - m_last_ref_time) > m_spec.timeout) || (m_cur_ref.getSource() != msg->control_src)) + { + debug("Using estate as ref"); + m_last_ref_time = Clock::get(); - // Waiting for the first reference in the current position. - m_cur_ref.flags = Reference::FLAG_LOCATION; - m_cur_ref.lat = m_estate.lat; - m_cur_ref.lon = m_estate.lon; - m_cur_ref.radius = m_args.loitering_radius; + // Waiting for the first reference in the current position. + m_cur_ref.flags = Reference::FLAG_LOCATION; + m_cur_ref.lat = m_estate.lat; + m_cur_ref.lon = m_estate.lon; + m_cur_ref.radius = m_args.loitering_radius; - WGS84::displace(m_estate.x, m_estate.y, &(m_cur_ref.lat), &(m_cur_ref.lon)); + WGS84::displace(m_estate.x, m_estate.y, &(m_cur_ref.lat), &(m_cur_ref.lon)); + } debug("lat lon = 0 ? %f", m_cur_ref.lat); @@ -132,15 +139,15 @@ namespace Maneuver void consume(const IMC::Reference* msg) { - // accept control source or broadcast. - if (m_spec.control_src != 0xFFFF && m_spec.control_src != msg->getSource()) + // accept control source, broadcast, and all messages before start of maneuver + if (m_spec.control_src != 0xFFFF && m_spec.control_src != msg->getSource() && m_spec.control_src != 0) { debug("ignored reference from non-authorized source: %d", msg->getSource()); return; } - // accept control source entity or broadcast. - if (m_spec.control_ent != 0xFF && m_spec.control_ent != msg->getSourceEntity()) + // accept control source, broadcast, and all messages before start of maneuver + if (m_spec.control_ent != 0xFF && m_spec.control_ent != msg->getSourceEntity() && m_spec.control_ent != 0) { debug("ignored reference from non-authorized entity: %d", msg->getSourceEntity()); return; @@ -153,6 +160,9 @@ namespace Maneuver { m_fref_state.proximity = IMC::FollowRefState::PROX_FAR; m_fref_state.state = IMC::FollowRefState::FR_WAIT; + // reset, since we no longer know who will send Reference + m_spec.control_src = 0; + m_spec.control_ent = 0; signalCompletion("maneuver terminated by reference source"); } else From 9d0a5e3dbad990d602aafbf7b2c900fa0d53c636 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 5 Aug 2019 15:56:44 +0200 Subject: [PATCH 4/4] Maneuver/FollowReference/UAV: fix onlineOrWaiting, use or instead of and --- src/Maneuver/FollowReference/UAV/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Maneuver/FollowReference/UAV/Task.cpp b/src/Maneuver/FollowReference/UAV/Task.cpp index e7e8af8a69..a1eb4b1fa7 100644 --- a/src/Maneuver/FollowReference/UAV/Task.cpp +++ b/src/Maneuver/FollowReference/UAV/Task.cpp @@ -463,7 +463,7 @@ namespace Maneuver offlineOrWaiting(void) { return ((m_fref_state.state & IMC::FollowRefState::FR_TIMEOUT) - && (m_fref_state.state & IMC::FollowRefState::FR_WAIT)); + || (m_fref_state.state & IMC::FollowRefState::FR_WAIT)); } void