Fixed failsafe

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

@@ -11,7 +11,7 @@
 #include "independent_watchdog.h"
 #include "tim.h"
 
-#define TIMEOUT_CYCLES 250000 // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
+#define DATANEW_TIMEOUT 75
 
 static uint32_t DisconnectionCount = 0;
 float prevthrottle;
@@ -35,24 +35,17 @@ void SystemManager::flyManually() {
     for(;;){
         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
-        };
+        //Is_Data_new implementation for failsafe
+		if (!this->rcInputs_.isDataNew){ //if the data is not new
 
-        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
-        }
+			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