diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 12b74a2f3e..688eb700fb 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -328,6 +328,7 @@ enum FlipState {
 #define ERROR_SUBSYSTEM_EKFINAV_CHECK       16
 #define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV    17
 #define ERROR_SUBSYSTEM_BARO                18
+#define ERROR_SUBSYSTEM_CPU                 19
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
diff --git a/ArduCopter/failsafe.pde b/ArduCopter/failsafe.pde
index 7a64c5d16c..3b7bd25ef0 100644
--- a/ArduCopter/failsafe.pde
+++ b/ArduCopter/failsafe.pde
@@ -39,15 +39,20 @@ void failsafe_check()
         // the main loop is running, all is OK
         failsafe_last_mainLoop_count = mainLoop_count;
         failsafe_last_timestamp = tnow;
-        in_failsafe = false;
+        if (in_failsafe) {
+            in_failsafe = false;
+            Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_RESOLVED);
+        }
         return;
     }
 
-    if (failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
+    if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
         // motors are running but we have gone 2 second since the
         // main loop ran. That means we're in trouble and should
         // disarm the motors.
         in_failsafe = true;
+
+        Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_OCCURRED);
     }
 
     if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {