diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 16a94eec43..c21086ee49 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -79,7 +79,7 @@ public:
     virtual bool requires_terrain_failsafe() const { return false; }
 
     // functions for reporting to GCS
-    virtual bool get_wp(Location &loc) { return false; };
+    virtual bool get_wp(Location &loc) const { return false; };
     virtual int32_t wp_bearing() const { return 0; }
     virtual uint32_t wp_distance() const { return 0; }
     virtual float crosstrack_error() const { return 0.0f;}
@@ -419,7 +419,7 @@ protected:
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
     float crosstrack_error() const override { return wp_nav->crosstrack_error();}
-    bool get_wp(Location &loc) override;
+    bool get_wp(Location &loc) const override;
 
 private:
 
@@ -850,7 +850,7 @@ public:
     void set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust);
     bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false);
     bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
-    bool get_wp(Location &loc) override;
+    bool get_wp(Location &loc) const override;
     void set_accel(const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
     void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
     void set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
@@ -1148,7 +1148,7 @@ public:
     bool requires_terrain_failsafe() const override { return true; }
 
     // for reporting to GCS
-    bool get_wp(Location &loc) override;
+    bool get_wp(Location &loc) const override;
 
     // RTL states
     enum class SubMode : uint8_t {
@@ -1272,7 +1272,7 @@ protected:
     const char *name4() const override { return "SRTL"; }
 
     // for reporting to GCS
-    bool get_wp(Location &loc) override;
+    bool get_wp(Location &loc) const override;
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
     float crosstrack_error() const override { return wp_nav->crosstrack_error();}
@@ -1537,7 +1537,7 @@ protected:
     const char *name4() const override { return "FOLL"; }
 
     // for reporting to GCS
-    bool get_wp(Location &loc) override;
+    bool get_wp(Location &loc) const override;
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
 
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 520fb5ce03..e1ca19b978 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -657,7 +657,7 @@ int32_t ModeAuto::wp_bearing() const
     }
 }
 
-bool ModeAuto::get_wp(Location& destination)
+bool ModeAuto::get_wp(Location& destination) const
 {
     switch (_mode) {
     case SubMode::NAVGUIDED:
diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp
index 0d858c9009..64519e8ea7 100644
--- a/ArduCopter/mode_follow.cpp
+++ b/ArduCopter/mode_follow.cpp
@@ -168,7 +168,7 @@ int32_t ModeFollow::wp_bearing() const
 /*
   get target position for mavlink reporting
  */
-bool ModeFollow::get_wp(Location &loc)
+bool ModeFollow::get_wp(Location &loc) const
 {
     float dist = g2.follow.get_distance_to_target();
     float bearing = g2.follow.get_bearing_to_target();
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 848251babd..00f661fe5d 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -284,7 +284,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
     return true;
 }
 
-bool ModeGuided::get_wp(Location& destination)
+bool ModeGuided::get_wp(Location& destination) const
 {
     if (guided_mode != SubMode::WP) {
         return false;
diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp
index 19db54b1d2..b5a31198ce 100644
--- a/ArduCopter/mode_rtl.cpp
+++ b/ArduCopter/mode_rtl.cpp
@@ -544,7 +544,7 @@ void ModeRTL::compute_return_target()
     rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
 }
 
-bool ModeRTL::get_wp(Location& destination)
+bool ModeRTL::get_wp(Location& destination) const
 {
     // provide target in states which use wp_nav
     switch (_state) {
diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp
index 656a91932c..7704f7503c 100644
--- a/ArduCopter/mode_smart_rtl.cpp
+++ b/ArduCopter/mode_smart_rtl.cpp
@@ -180,7 +180,7 @@ void ModeSmartRTL::save_position()
     copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
 }
 
-bool ModeSmartRTL::get_wp(Location& destination)
+bool ModeSmartRTL::get_wp(Location& destination) const
 {
     // provide target in states which use wp_nav
     switch (smart_rtl_state) {