From b11b606ddc8d092c3a5707cf321b4f247091bada Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 4 Jun 2012 10:21:01 -0500 Subject: [PATCH] Fix RTH mode. Now works in simulation when GCSReceiver goes invalid. There is a small issue that when it is in RTH failsafe mode and you regain signal and your throttle is low you will disarm due to the timeout. That means you'd suddenly get it back and plumet ouf of the sky. If you are still holding throttle (which most people might do?) you will have control. The timeout needs to probably start when you regain signal. --- flight/Modules/ManualControl/manualcontrol.c | 21 +++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 16f9e664a..a6ee3b29a 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -363,14 +363,24 @@ static void manualControlTask(void *parameters) } - processFlightMode(&settings, flightMode); } - if (processFailsafe(valid_input_detected, &settings) == false) { + if (processFailsafe(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE, &settings) == false) { // Process arming outside conditional so system will disarm when disconnected. However don't do // it when in failsafe mode + + // Only update the flight mode when we got valid data + if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE) + processFlightMode(&settings, flightMode); + + // However process arming all the time so it can time out processArm(&cmd, &settings); + + } else { + // In the case of failsafe don't process the channels to update the flight mode + ManualControlCommandSet(&cmd); + continue; } // Update cmd object @@ -641,6 +651,11 @@ static void setRTH(bool changed) } } +/** + * Determine based on flight settings if we should enter failsafe when there is no valid input + * it returns true when the failsafe is engaged and the informs the rest of the system not to + * use the inputs + */ static bool processFailsafe(bool valid_input_detected, ManualControlSettingsData *settings) { static bool failsafe_enaged = false; @@ -666,9 +681,9 @@ static bool processFailsafe(bool valid_input_detected, ManualControlSettingsData // exclude fixed wing. if (guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_TRUE && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { - setRTH(true); flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_RTH; + FlightStatusSet(&flightStatus); failsafe_enaged = true; } }