diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 8453f5d3ec..b6be6d5cef 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -2236,26 +2236,18 @@ bool ModeAuto::verify_nav_attitude_time(const AP_Mission::Mission_Command& cmd)
 bool ModeAuto::pause()
 {
     // do not pause if not in the WP sub mode or already reached to the destination
-    if(_mode != SubMode::WP || wp_nav->reached_wp_destination()) {
+    if (_mode != SubMode::WP || wp_nav->reached_wp_destination()) {
         return false;
     }
 
-    // do not pause if already paused
-    if (!wp_nav->paused()) {
-        wp_nav->set_pause();
-    }
-
+    wp_nav->set_pause();
     return true;
 }
 
 // resume - Allow aircraft to progress along the track
 bool ModeAuto::resume()
 {
-    // do not resume if not paused before
-    if(wp_nav->paused()) {
-        wp_nav->set_resume();
-    }
-
+    wp_nav->set_resume();
     return true;
 }
 
diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index b946b69a05..0b61e16010 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -8851,7 +8851,7 @@ class AutoTestCopter(AutoTest):
         self.send_pause_command()
         self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)
         self.send_resume_command()
-        
+
         # sending a pause, or resume, to the aircraft twice, doesn't result in reporting a failure
         self.wait_current_waypoint(wpnum=5, timeout=500)
         self.send_pause_command()