Fixed failsafe feature (#70)

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;
     }
 

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