Copter: rewrite read_radio for clarity

ArduCopter/radio.cpp

@@ -107,19 +107,37 @@ void Copter::read_radio()
         // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
         radio_passthrough_to_motors();
 
-        float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
+        const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
         rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt);
         last_radio_update_ms = tnow_ms;
-    }else{
-        uint32_t elapsed = tnow_ms - last_radio_update_ms;
-        // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
-        const bool has_active_overrides = RC_Channels::has_active_overrides();
-        if (((!has_active_overrides && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (has_active_overrides && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) &&
-            (g.failsafe_throttle && (ap.rc_receiver_present||motors->armed()) && !failsafe.radio)) {
-            Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
-            set_failsafe_radio(true);
-        }
+        return;
     }
+
+    // No radio input this time
+    if (failsafe.radio) {
+        // already in failsafe!
+        return;
+    }
+
+    const uint32_t elapsed = tnow_ms - last_radio_update_ms;
+    // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
+    const uint32_t timeout = RC_Channels::has_active_overrides() ? FS_RADIO_RC_OVERRIDE_TIMEOUT_MS : FS_RADIO_TIMEOUT_MS;
+    if (elapsed < timeout) {
+        // not timed out yet
+        return;
+    }
+    if (!g.failsafe_throttle) {
+        // throttle failsafe not enabled
+        return;
+    }
+    if (!ap.rc_receiver_present && !motors->armed()) {
+        // we only failsafe if we are armed OR we have ever seen an RC receiver
+        return;
+    }
+
+    // Nobody ever talks to us.  Log an error and enter failsafe.
+    Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
+    set_failsafe_radio(true);
 }
 
 #define FS_COUNTER 3        // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value