diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index afc42a510f..aebfc4d55b 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -995,31 +995,24 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
 
 MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_command_int_t &packet)
 {
-#if MODE_AUTO_ENABLED
-    if (copter.flightmode->mode_number() != Mode::Number::AUTO) {
-        // only supported in AUTO mode
+    // requested pause
+    if ((uint8_t) packet.param1 == 0) {
+        if (copter.flightmode->pause()) {
+            return MAV_RESULT_ACCEPTED;
+        }
+        send_text(MAV_SEVERITY_INFO, "Failed to pause");
         return MAV_RESULT_FAILED;
     }
 
-    // requested pause from GCS
-    if ((int8_t) packet.param1 == 0) {
-        // copter.mode_auto.mission.stop();
-        copter.mode_auto.pause_mission();
-        gcs().send_text(MAV_SEVERITY_INFO, "Paused mission");
-        return MAV_RESULT_ACCEPTED;
+    // requested resume
+    if ((uint8_t) packet.param1 == 1) {
+        if (copter.flightmode->resume()) {
+            return MAV_RESULT_ACCEPTED;
+        }
+        send_text(MAV_SEVERITY_INFO, "Failed to resume");
+        return MAV_RESULT_FAILED;
     }
-
-    // requested resume from GCS
-    if ((int8_t) packet.param1 == 1) {
-        // copter.mode_auto.mission.resume();
-        copter.mode_auto.continue_mission();
-        gcs().send_text(MAV_SEVERITY_INFO, "Resumed mission");
-        return MAV_RESULT_ACCEPTED;
-    }
-#endif
-
-    // fail pause or continue
-    return MAV_RESULT_FAILED;
+    return MAV_RESULT_DENIED;
 }
 
 void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 8a2e1d4495..bf3e302a0b 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -110,6 +110,10 @@ public:
     // returns true if pilot's yaw input should be used to adjust vehicle's heading
     virtual bool use_pilot_yaw() const {return true; }
 
+    // pause and resume a mode
+    virtual bool pause() { return false; };
+    virtual bool resume() { return false; };
+
 protected:
 
     // helper functions
@@ -419,6 +423,10 @@ public:
     // Auto
     SubMode mode() const { return _mode; }
 
+    // pause continue in auto mode
+    bool pause() override;
+    bool resume() override;
+
     bool loiter_start();
     void rtl_start();
     void takeoff_start(const Location& dest_loc);
@@ -427,9 +435,6 @@ public:
     void circle_movetoedge_start(const Location &circle_center, float radius_m);
     void circle_start();
     void nav_guided_start();
-    
-    void pause_mission();
-    void continue_mission();
 
     bool is_landing() const override;
 
@@ -973,6 +978,10 @@ public:
 
     bool use_pilot_yaw() const override;
 
+    // pause continue in guided mode
+    bool pause() override;
+    bool resume() override;
+
 protected:
 
     const char *name() const override { return "GUIDED"; }
@@ -1016,6 +1025,9 @@ private:
     SubMode guided_mode = SubMode::TakeOff;
     bool send_notification;     // used to send one time notification to ground station
     bool takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
+
+    // guided mode is paused or not
+    bool _paused;
 };
 
 
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index b9490d8dd1..f3b084491f 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -673,18 +673,6 @@ void ModeAuto::exit_mission()
     }
 }
 
-// pause_mission - Prevent aircraft from progressing along the track
-void ModeAuto::pause_mission()
-{
-        wp_nav->set_pause();
-}
-
-// continue_mission - Allow aircraft to progress along the track
-void ModeAuto::continue_mission()
-{
-        wp_nav->set_continue();
-}
-
 // do_guided - start guided mode
 bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
 {
@@ -2078,4 +2066,28 @@ bool ModeAuto::verify_nav_script_time()
 }
 #endif
 
+// pause - Prevent aircraft from progressing along the track
+bool ModeAuto::pause()
+{
+    // do not pause if already paused or not in the WP sub mode or already reached to the destination
+    if(wp_nav->paused() || _mode != SubMode::WP || wp_nav->reached_wp_destination()) {
+        return false;
+    }
+
+    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()) {
+        return false;
+    }
+
+    wp_nav->set_resume();
+    return true;
+}
+
 #endif
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index a64d7e4f27..d48510efd3 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -40,6 +40,10 @@ bool ModeGuided::init(bool ignore_checks)
     guided_vel_target_cms.zero();
     guided_accel_target_cmss.zero();
     send_notification = false;
+
+    // clear pause state when entering guided mode
+    _paused = false;
+
     return true;
 }
 
@@ -47,10 +51,11 @@ bool ModeGuided::init(bool ignore_checks)
 // should be called at 100hz or more
 void ModeGuided::run()
 {
-    // if (paused) {
-    //     pause_control_run();
-    //     return;
-    // }
+    // run pause control if the vehicle is paused
+    if (_paused) {
+        pause_control_run();
+        return;
+    }
 
     // call the correct auto controller
     switch (guided_mode) {
@@ -854,7 +859,7 @@ void ModeGuided::velaccel_control_run()
     }
 }
 
-// velaccel_control_run - runs the guided velocity and acceleration controller
+// pause_control_run - runs the guided mode pause controller
 // called from guided_run
 void ModeGuided::pause_control_run()
 {
@@ -1199,4 +1204,18 @@ uint32_t ModeGuided::get_timeout_ms() const
     return MAX(copter.g2.guided_timeout, 0.1) * 1000;
 }
 
+// pause guide mode
+bool ModeGuided::pause()
+{
+    _paused = true;
+    return true;
+}
+
+// resume guided mode
+bool ModeGuided::resume()
+{
+    _paused = false;
+    return true;
+}
+
 #endif