diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 56f014d248..a19395b6f2 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -249,6 +249,9 @@ private:
     control_mode_t control_mode;
     mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
 
+    control_mode_t prev_control_mode;
+    mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN;
+
     // Structure used to detect changes in the flight mode control switch
     struct {
         int8_t debounced_switch_position;   // currently used switch position
diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp
index 934d2daa9a..f8e9cc2478 100644
--- a/ArduCopter/flight_mode.cpp
+++ b/ArduCopter/flight_mode.cpp
@@ -19,6 +19,9 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
 
     // return immediately if we are already in the desired mode
     if (mode == control_mode) {
+        prev_control_mode = control_mode;
+        prev_control_mode_reason = control_mode_reason;
+
         control_mode_reason = reason;
         return true;
     }
@@ -109,6 +112,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
     if (success) {
         // perform any cleanup required by previous flight mode
         exit_mode(control_mode, (control_mode_t)mode);
+        
+        prev_control_mode = control_mode;
+        prev_control_mode_reason = control_mode_reason;
+
         control_mode = (control_mode_t)mode;
         control_mode_reason = reason;
         DataFlash.Log_Write_Mode(control_mode, control_mode_reason);