From 14c9b8ab0bb9eeddcf55182b3973dc95a86b898b Mon Sep 17 00:00:00 2001 From: Uzayr Hussaini Date: Mon, 25 Nov 2024 19:30:43 -0500 Subject: [PATCH] Fixed failsafe feature --- .../rc_receiver/inc/rcreceiver_datatypes.h | 1 + SystemManager/Src/SystemManager.cpp | 27 +++++++++---------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/Drivers/rc_receiver/inc/rcreceiver_datatypes.h b/Drivers/rc_receiver/inc/rcreceiver_datatypes.h index f4acb601..b2a9783c 100644 --- a/Drivers/rc_receiver/inc/rcreceiver_datatypes.h +++ b/Drivers/rc_receiver/inc/rcreceiver_datatypes.h @@ -41,6 +41,7 @@ struct RCControl{ RCControl operator=(const RCControl& other){ std::copy(other.ControlSignals, other.ControlSignals + SBUS_INPUT_CHANNELS, this->ControlSignals); + this->isDataNew = other.isDataNew; // Add this line to copy the isDataNew flag return *this; } diff --git a/SystemManager/Src/SystemManager.cpp b/SystemManager/Src/SystemManager.cpp index 919fba4a..e2e82631 100644 --- a/SystemManager/Src/SystemManager.cpp +++ b/SystemManager/Src/SystemManager.cpp @@ -23,6 +23,7 @@ extern "C" { #include "log_util.h" } +#define DATANEW_TIMEOUT 75 #define TIMEOUT_CYCLES 250000 // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec #define TIMOUT_MS 10000 // 10 sec @@ -147,21 +148,17 @@ void SystemManager::systemManagerTask() { this->rcInputs_ = rcController_->GetRCControl(); - // TO-DO: need to implement it using is_Data_New; - // boolean is true if data has not changed since the last cycle - bool is_unchanged{rcInputs_.throttle == prevthrottle && rcInputs_.yaw == prevyaw && - rcInputs_.roll == prevroll && rcInputs_.pitch == prevpitch}; - - if (is_unchanged) { - DisconnectionCount += 1; // if its not changed we increment the timeout counter - if (DisconnectionCount > TIMEOUT_CYCLES) { // if timeout has occured - DisconnectionCount = - TIMEOUT_CYCLES + 1; // avoid overflow but keep value above threshold - this->rcInputs_.arm = 0; // failsafe - } - } else { - DisconnectionCount = 0; // if the data has changed we want to reset out counter - } + //Is_Data_new implementation for failsafe + if (!this->rcInputs_.isDataNew){ //if the data is not new + + DisconnectionCount += 1; //increment the counter + if (DisconnectionCount > DATANEW_TIMEOUT){ //if the counter is greater than 75, then we can disarm + this->rcInputs_.arm = 0; //disarm the drone for failsafe + } + } + else{ + DisconnectionCount = 0; //if the data is new, then we reset to 0. + } watchdog_.refreshWatchdog(); // always hit the dog