From 2c0fc686a751981105d38b1a0698b9e751b61634 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 16 Jun 2021 10:40:41 +0200 Subject: [PATCH] manual_control: don't accept outdated input --- src/modules/manual_control/ManualControlSelector.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index 689288ed3c..3bdd6e47a6 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -47,8 +47,14 @@ void ManualControlSelector::update_time_only(uint64_t now) void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance) { - // This method requires the current timestamp explicitly in order to prevent weird cases - // if the timestamp_sample of some source is invalid/wrong. + // This method requires the current timestamp explicitly in order to prevent the case + // where the timestamp_sample of some source is already outdated. + + if (now >= input.timestamp_sample && now - input.timestamp_sample > _timeout) { + _setpoint.valid = false; + _instance = -1; + return; + } // If previous setpoint is timed out, set it invalid, so it can get replaced below. update_time_only(now);