From 07b3ad907535d5cd38808eb89ed2adc07091c86e Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Tue, 19 Dec 2017 15:46:15 +0530
Subject: [PATCH 01/16] follow_me example: add source and CMake file

---
 example/follow_me/CMakeLists.txt |  22 ++++++
 example/follow_me/follow_me.cpp  | 113 +++++++++++++++++++++++++++++++
 2 files changed, 135 insertions(+)
 create mode 100644 example/follow_me/CMakeLists.txt
 create mode 100644 example/follow_me/follow_me.cpp

diff --git a/example/follow_me/CMakeLists.txt b/example/follow_me/CMakeLists.txt
new file mode 100644
index 0000000000..1cd0c0d4fc
--- /dev/null
+++ b/example/follow_me/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 2.8.12)
+
+project(follow_me)
+
+add_definitions("-std=c++11 -Wall -Wextra -Werror")
+
+if(NOT MSVC)
+    add_definitions("-std=c++11 -Wall -Wextra -Werror")
+else()
+    add_definitions("-std=c++11 -WX -W2")
+    include_directories(${CMAKE_SOURCE_DIR}/../../install/include)
+    link_directories(${CMAKE_SOURCE_DIR}/../../install/lib)
+endif()
+
+add_executable(follow_me
+    follow_me.cpp
+)
+
+target_link_libraries(follow_me
+    dronecore
+)
+
diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
new file mode 100644
index 0000000000..bb3dbf37d8
--- /dev/null
+++ b/example/follow_me/follow_me.cpp
@@ -0,0 +1,113 @@
+#include <dronecore/dronecore.h>
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <cstdint>
+
+using namespace dronecore;
+
+#define ERROR_CONSOLE_TEXT "\033[31m" //Turn text on console red
+#define TELEMETRY_CONSOLE_TEXT "\033[34m" //Turn text on console blue
+#define NORMAL_CONSOLE_TEXT "\033[0m"  //Restore normal console colour
+
+int main(int /*argc*/, char ** /*argv*/)
+{
+    DroneCore dc;
+
+    bool discovered_device = false;
+
+    DroneCore::ConnectionResult connection_result = dc.add_udp_connection();
+
+    if (connection_result != DroneCore::ConnectionResult::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT << "Connection failed: "
+                  << DroneCore::connection_result_str(connection_result)
+                  << NORMAL_CONSOLE_TEXT << std::endl;
+        return 1;
+    }
+
+    std::cout << "Waiting to discover device..." << std::endl;
+    dc.register_on_discover([&discovered_device](uint64_t uuid) {
+        std::cout << "Discovered device with UUID: " << uuid << std::endl;
+        discovered_device = true;
+    });
+
+    // We usually receive heartbeats at 1Hz, therefore we should find a device after around 2 seconds.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    if (!discovered_device) {
+        std::cout << ERROR_CONSOLE_TEXT << "No device found, exiting." << NORMAL_CONSOLE_TEXT << std::endl;
+        return 1;
+    }
+
+    // We don't need to specify the UUID if it's only one device anyway.
+    // If there were multiple, we could specify it with:
+    // dc.device(uint64_t uuid);
+    Device &device = dc.device();
+
+    // Check if vehicle is ready to arm
+    if (device.telemetry().health_all_ok() != true) {
+        std::cout << ERROR_CONSOLE_TEXT << "Vehicle not ready to arm" << NORMAL_CONSOLE_TEXT << std::endl;
+        return 1;
+    }
+
+    // Arm vehicle
+    std::cout << "Arming..." << std::endl;
+    const Action::Result arm_result = device.action().arm();
+
+    if (arm_result != Action::Result::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT << "Arming failed:" << Action::result_str(
+                      arm_result) << NORMAL_CONSOLE_TEXT << std::endl;
+        return 1;
+    }
+
+#if 0
+    device.telemetry().flight_mode_async(
+    std::bind([&](Telemetry::FlightMode flight_mode) {
+        auto last_location = device.follow_me().get_last_location();
+
+        std::cout << "[FlightMode: " << Telemetry::flight_mode_str(flight_mode)
+                 << "] Vehicle is at Lat: " << last_location.latitude_deg << " deg, "  <<
+                  "Lon: " << last_location.longitude_deg << " deg." << std::endl;
+    }, std::placeholders::_1));
+#endif
+
+	//std::string mode = Telemetry::flight_mode_str(Telemetry::FlightMode::TAKEOFF);
+//	Telemetry::FlightMode mode = device.telemetry().flight_mode();
+
+	//std::cout << "Result: " << FollowMe::result_str(FollowMe::Result::SUCCESS);
+//	std::cout << "Result: " << Mission::result_str(Mission::Result::SUCCESS);
+
+    // Take off
+std::cout << "Taking off..." << std::endl;
+    const Action::Result takeoff_result = device.action().takeoff();
+    if (takeoff_result != Action::Result::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT << "Takeoff failed:" << Action::result_str(
+                      takeoff_result) << NORMAL_CONSOLE_TEXT << std::endl;
+        return 1;
+    }
+
+    // Let it hover for a bit before landing again.
+    std::this_thread::sleep_for(std::chrono::seconds(10));
+
+//device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
+device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
+FollowMe::Result follow_me_result = device.follow_me().start();
+ if(follow_me_result!=FollowMe::Result::SUCCESS){
+  std::cout<<ERROR_CONSOLE_TEXT<<"failed to initiate follow me mode"<<std::endl;
+
+}
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    std::cout << "Landing..." << std::endl;
+    const Action::Result land_result = device.action().land();
+    if (land_result != Action::Result::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT << "Land failed:" << Action::result_str(
+                      land_result) << NORMAL_CONSOLE_TEXT << std::endl;
+        return 1;
+    }
+
+    // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
+    std::this_thread::sleep_for(std::chrono::seconds(5));
+    std::cout << "Finished..." << std::endl;
+    return 0;
+}

From bd603635a55f8bad13475c4a9128d01dd8528e5b Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 29 Dec 2017 10:39:01 +0530
Subject: [PATCH 02/16] Add Follow Me example

---
 example/follow_me/follow_me.cpp      |  32 +++-----
 integration_tests/follow_me.cpp      | 108 ++++++++++++++++++++++++++-
 plugins/follow_me/follow_me_impl.cpp |   6 +-
 3 files changed, 120 insertions(+), 26 deletions(-)

diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
index bb3dbf37d8..d15b6c2dcb 100644
--- a/example/follow_me/follow_me.cpp
+++ b/example/follow_me/follow_me.cpp
@@ -5,6 +5,7 @@
 #include <cstdint>
 
 using namespace dronecore;
+using namespace std::placeholders;
 
 #define ERROR_CONSOLE_TEXT "\033[31m" //Turn text on console red
 #define TELEMETRY_CONSOLE_TEXT "\033[34m" //Turn text on console blue
@@ -60,43 +61,34 @@ int main(int /*argc*/, char ** /*argv*/)
         return 1;
     }
 
-#if 0
     device.telemetry().flight_mode_async(
     std::bind([&](Telemetry::FlightMode flight_mode) {
         auto last_location = device.follow_me().get_last_location();
 
         std::cout << "[FlightMode: " << Telemetry::flight_mode_str(flight_mode)
-                 << "] Vehicle is at Lat: " << last_location.latitude_deg << " deg, "  <<
+                  << "] Vehicle is at Lat: " << last_location.latitude_deg << " deg, "  <<
                   "Lon: " << last_location.longitude_deg << " deg." << std::endl;
-    }, std::placeholders::_1));
-#endif
+    }, _1));
 
-	//std::string mode = Telemetry::flight_mode_str(Telemetry::FlightMode::TAKEOFF);
-//	Telemetry::FlightMode mode = device.telemetry().flight_mode();
-
-	//std::cout << "Result: " << FollowMe::result_str(FollowMe::Result::SUCCESS);
-//	std::cout << "Result: " << Mission::result_str(Mission::Result::SUCCESS);
 
     // Take off
-std::cout << "Taking off..." << std::endl;
+    std::cout << "Taking off..." << std::endl;
     const Action::Result takeoff_result = device.action().takeoff();
     if (takeoff_result != Action::Result::SUCCESS) {
         std::cout << ERROR_CONSOLE_TEXT << "Takeoff failed:" << Action::result_str(
                       takeoff_result) << NORMAL_CONSOLE_TEXT << std::endl;
         return 1;
     }
+    std::this_thread::sleep_for(std::chrono::seconds(5));
 
-    // Let it hover for a bit before landing again.
-    std::this_thread::sleep_for(std::chrono::seconds(10));
-
-//device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
-device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
-FollowMe::Result follow_me_result = device.follow_me().start();
- if(follow_me_result!=FollowMe::Result::SUCCESS){
-  std::cout<<ERROR_CONSOLE_TEXT<<"failed to initiate follow me mode"<<std::endl;
+    // Start Follow Me
+    device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
+    FollowMe::Result follow_me_result = device.follow_me().start();
+    if (follow_me_result != FollowMe::Result::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT << "failed to initiate follow me mode" << std::endl;
 
-}
-    std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+    std::this_thread::sleep_for(std::chrono::seconds(10));
 
     std::cout << "Landing..." << std::endl;
     const Action::Result land_result = device.action().land();
diff --git a/integration_tests/follow_me.cpp b/integration_tests/follow_me.cpp
index 34abf47438..9f4e8fc4fa 100644
--- a/integration_tests/follow_me.cpp
+++ b/integration_tests/follow_me.cpp
@@ -15,7 +15,7 @@ using namespace std::placeholders;
 void print(const FollowMe::Config &config);
 void send_location_updates(FollowMe &follow_me_handle, size_t count = 25ul, float rate = 1.f);
 
-const size_t N_LOCATIONS = 100ul;
+const size_t N_LOCATIONS = 200ul;
 
 TEST_F(SitlTest, FollowMeOneLocation)
 {
@@ -54,15 +54,15 @@ TEST_F(SitlTest, FollowMeOneLocation)
     print(curr_config);
 
     // Set just a single location before starting FollowMe (optional)
-    device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
+    device.follow_me().set_target_location({47.3978439, 8.5452824, 0.0, 0.f, 0.f, 0.f});
 
     // Start following with default configuration
     FollowMe::Result follow_me_result = device.follow_me().start();
     ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
     sleep_for(seconds(1));
 
-    std::cout << "We're waiting (for 5s) to see the drone moving target location set." << std::endl;
-    sleep_for(seconds(5));
+    std::cout << "We're waiting (for 10s) to see the drone moving target location set." << std::endl;
+    sleep_for(seconds(10));
 
     // stop following
     follow_me_result = device.follow_me().stop();
@@ -160,6 +160,106 @@ void send_location_updates(FollowMe &follow_me, size_t count, float rate)
     // Altitude here is ignored by PX4, as we've set min altitude in configuration.
     std::array<FollowMe::TargetLocation, N_LOCATIONS> spiral_path = {
         {
+            { 47.3997373, 8.5426978, 490.0, 0.f, 0.f, 0.f },
+            { 47.39973730, 8.54269780, 510.0, 0.f, 0.f, 0.f },
+            { 47.39780291, 8.54557048, 490.0, 0.f, 0.f, 0.f },
+            { 47.39779838, 8.54555174, 492.0, 0.f, 0.f, 0.f },
+            { 47.39778748, 8.54554499, 493.0, 0.f, 0.f, 0.f },
+            { 47.39777659, 8.54553561, 494.0, 0.f, 0.f, 0.f },
+            { 47.39776569, 8.54553292, 494.0, 0.f, 0.f, 0.f },
+            { 47.39774663, 8.54552622, 501.0, 0.f, 0.f, 0.f },
+            { 47.39772938, 8.54552488, 501.0, 0.f, 0.f, 0.f },
+            { 47.39771304, 8.54554231, 502.0, 0.f, 0.f, 0.f },
+            { 47.39770578, 8.54557445, 504.0, 0.f, 0.f, 0.f },
+            { 47.39770487, 8.54559596, 502.0, 0.f, 0.f, 0.f },
+            { 47.39770578, 8.54561741, 502.0, 0.f, 0.f, 0.f },
+            { 47.39770669, 8.54563887, 502.0, 0.f, 0.f, 0.f },
+            { 47.39771486, 8.54565765, 493.0, 0.f, 0.f, 0.f },
+            { 47.39773029, 8.54567642, 496.0, 0.f, 0.f, 0.f },
+            { 47.39775026, 8.54568447, 502.0, 0.f, 0.f, 0.f },
+            { 47.39776751, 8.54569118, 502.0, 0.f, 0.f, 0.f },
+            { 47.39778203, 8.54569118, 502.0, 0.f, 0.f, 0.f },
+            { 47.39779838, 8.54568447, 502.0, 0.f, 0.f, 0.f },
+            { 47.39781835, 8.54566972, 502.0, 0.f, 0.f, 0.f },
+            { 47.39782107, 8.54564692, 502.0, 0.f, 0.f, 0.f },
+            { 47.39782474, 8.54561876, 502.0, 0.f, 0.f, 0.f },
+            { 47.39782475, 8.54559193, 501.0, 0.f, 0.f, 0.f },
+            { 47.39782474, 8.54556511, 501.0, 0.f, 0.f, 0.f },
+            { 47.39782107, 8.54553427, 503.0, 0.f, 0.f, 0.f },
+            { 47.39780836, 8.54552756, 502.0, 0.f, 0.f, 0.f },
+            { 47.39779656, 8.54551549, 501.0, 0.f, 0.f, 0.f },
+            { 47.39777568, 8.54550342, 501.0, 0.f, 0.f, 0.f },
+            { 47.39775482, 8.54549671, 504.0, 0.f, 0.f, 0.f },
+            { 47.39773755, 8.54549671, 501.0, 0.f, 0.f, 0.f },
+            { 47.39771849, 8.54550208, 501.0, 0.f, 0.f, 0.f },
+            { 47.39770396, 8.54551415, 502.0, 0.f, 0.f, 0.f },
+            { 47.39769398, 8.54554097, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768762, 8.54556243, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768672, 8.54557852, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768493, 8.54559998, 502.0, 0.f, 0.f, 0.f },
+            { 47.39768399, 8.54562278, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768399, 8.54564155, 501.0, 0.f, 0.f, 0.f },
+            { 47.39769035, 8.54566569, 501.0, 0.f, 0.f, 0.f },
+            { 47.39770759, 8.54568983, 502.0, 0.f, 0.f, 0.f },
+            { 47.39772757, 8.54569922, 502.0, 0.f, 0.f, 0.f },
+            { 47.39774481, 8.54570727, 507.0, 0.f, 0.f, 0.f },
+            { 47.39776025, 8.54572202, 502.0, 0.f, 0.f, 0.f },
+            { 47.39778567, 8.54572336, 505.0, 0.f, 0.f, 0.f },
+            { 47.39780291, 8.54572202, 507.0, 0.f, 0.f, 0.f },
+            { 47.39782107, 8.54571263, 502.0, 0.f, 0.f, 0.f },
+            { 47.39783469, 8.54569788, 501.0, 0.f, 0.f, 0.f },
+            { 47.39783832, 8.54568179, 501.0, 0.f, 0.f, 0.f },
+            { 47.39784104, 8.54566569, 503.0, 0.f, 0.f, 0.f },
+            { 47.39784376, 8.54564424, 502.0, 0.f, 0.f, 0.f },
+            { 47.39784386, 8.54564435, 503.0, 0.f, 0.f, 0.f },
+            { 47.39784396, 8.54564444, 505.0, 0.f, 0.f, 0.f },
+            { 47.39784386, 8.54564454, 503.0, 0.f, 0.f, 0.f },
+            { 47.39784346, 8.54564464, 504.0, 0.f, 0.f, 0.f },
+            { 47.39784336, 8.54564424, 501.0, 0.f, 0.f, 0.f },
+            { 47.39772757, 8.54569922, 501.0, 0.f, 0.f, 0.f },
+            { 47.39774481, 8.54570727, 503.0, 0.f, 0.f, 0.f },
+            { 47.39776025, 8.54572202, 501.0, 0.f, 0.f, 0.f },
+            { 47.39778567, 8.54572336, 503.0, 0.f, 0.f, 0.f },
+            { 47.39770396, 8.54551415, 501.0, 0.f, 0.f, 0.f },
+            { 47.39769398, 8.54554097, 502.0, 0.f, 0.f, 0.f },
+            { 47.39768762, 8.54556243, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768672, 8.54557852, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768494, 8.54559998, 501.0, 0.f, 0.f, 0.f },
+            { 47.39779454, 8.54559464, 502.0, 0.f, 0.f, 0.f },
+            { 47.39780291, 8.54557048, 489.0, 0.f, 0.f, 0.f },
+            { 47.39779838, 8.54555173, 502.0, 0.f, 0.f, 0.f },
+            { 47.39778748, 8.54554499, 508.0, 0.f, 0.f, 0.f },
+            { 47.39777659, 8.54553561, 510.0, 0.f, 0.f, 0.f },
+            { 47.39776569, 8.54553292, 501.0, 0.f, 0.f, 0.f },
+            { 47.39774663, 8.54552622, 502.0, 0.f, 0.f, 0.f },
+            { 47.39772938, 8.54552488, 505.0, 0.f, 0.f, 0.f },
+            { 47.39771304, 8.54554231, 505.0, 0.f, 0.f, 0.f },
+
+
+
+            { 47.39784933,  8.54555596,  0.0, 0.f, 0.f, 0.f },
+            { 47.39784933, 8.54555596,  0.0, 0.f, 0.f, 0.f },
+            { 47.39784388, 8.54528237,  0.0, 0.f, 0.f, 0.f },
+            { 47.39776854, 8.54522604,  0.0, 0.f, 0.f, 0.f },
+            { 47.39764689, 8.54521666,  0.0, 0.f, 0.f, 0.f },
+            { 47.39754158, 8.54527566,  0.0, 0.f, 0.f, 0.f },
+            { 47.39752252, 8.54552511,  0.0, 0.f, 0.f, 0.f },
+            { 47.39750618, 8.5458805,  0.0, 0.f, 0.f, 0.f },
+            { 47.39758334, 8.54593281,  0.0, 0.f, 0.f, 0.f },
+            { 47.39775219, 8.54612861,  0.0, 0.f, 0.f, 0.f },
+            { 47.39785932, 8.546024,  0.0, 0.f, 0.f, 0.f },
+            { 47.39793739, 8.54574371,  0.0, 0.f, 0.f, 0.f },
+            { 47.39808445, 8.54564849,  0.0, 0.f, 0.f, 0.f },
+            { 47.39808899, 8.54552645,  0.0, 0.f, 0.f, 0.f },
+            { 47.39807174, 8.54531053,  0.0, 0.f, 0.f, 0.f },
+            { 47.39800456, 8.54527566,  0.0, 0.f, 0.f, 0.f },
+            { 47.39799912, 8.54549829,  0.0, 0.f, 0.f, 0.f },
+            { 47.39797188, 8.54559485,  0.0, 0.f, 0.f, 0.f },
+            { 47.39786839, 8.54575042,  0.0, 0.f, 0.f, 0.f },
+            { 47.39779668, 8.54575444,  0.0, 0.f, 0.f, 0.f },
+            { 47.39779486, 8.54564849,  0.0, 0.f, 0.f, 0.f },
+
+            /////////////////////////////////////////////////
             { 47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f },
             { 47.39769035, 8.54566569, 0.0, 0.f, 0.f, 0.f },
             { 47.39770759, 8.54568983, 0.0, 0.f, 0.f, 0.f },
diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index 9f6da380d4..218902a4e8 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -307,11 +307,13 @@ void FollowMeImpl::send_target_location()
     uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds
 
     _mutex.lock();
-//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg <<
-//	" Alt: " << _target_location.absolute_altitude_m;
+    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
+               <<
+               " Alt: " << _target_location.absolute_altitude_m;
     const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
     const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
     const float alt = static_cast<float>(_target_location.absolute_altitude_m);
+    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
     _mutex.unlock();
 
     const float pos_std_dev[] = { NAN, NAN, NAN };

From f809bbf37019ca594122759604182c0a77b16c3f Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Thu, 4 Jan 2018 15:06:43 +0530
Subject: [PATCH 03/16] Simplify Follow Me example

1. Added methods for error handling.
2. Improved periodic location update logic.
3. Removed redundant header `cstdint`.
4. Removed log from FollowMe plugin.
5. Improved overall FollowMe example.
---
 example/follow_me/follow_me.cpp      | 172 +++++++++++++++++----------
 plugins/follow_me/follow_me_impl.cpp |   8 +-
 2 files changed, 110 insertions(+), 70 deletions(-)

diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
index d15b6c2dcb..7401716533 100644
--- a/example/follow_me/follow_me.cpp
+++ b/example/follow_me/follow_me.cpp
@@ -1,105 +1,145 @@
+/**
+* @file follow_me.cpp
+* @brief Example that demonstrates the usage of Follow Me plugin.
+* @author Shakthi Prashanth <shakthi.prashanth.m@intel.com>
+* @date 2018-01-03
+*/
+
 #include <dronecore/dronecore.h>
 #include <iostream>
 #include <thread>
 #include <chrono>
-#include <cstdint>
 
 using namespace dronecore;
-using namespace std::placeholders;
+using namespace std::placeholders; // for `_1`
+using namespace std::chrono; // for seconds(), milliseconds(), etc
+using namespace std::this_thread;  // for sleep_for()
 
+// For coloring output
 #define ERROR_CONSOLE_TEXT "\033[31m" //Turn text on console red
 #define TELEMETRY_CONSOLE_TEXT "\033[34m" //Turn text on console blue
 #define NORMAL_CONSOLE_TEXT "\033[0m"  //Restore normal console colour
 
-int main(int /*argc*/, char ** /*argv*/)
+inline void action_error_exit(Action::Result result, const std::string &message);
+inline void follow_me_error_exit(FollowMe::Result result, const std::string &message);
+inline void connection_error_exit(DroneCore::ConnectionResult result, const std::string &message);
+void send_location_updates(FollowMe &follow_me, size_t count = 50ul, float rate = 1.f);
+
+int main(int, char **)
 {
     DroneCore dc;
 
-    bool discovered_device = false;
-
-    DroneCore::ConnectionResult connection_result = dc.add_udp_connection();
+    DroneCore::ConnectionResult conn_result = dc.add_udp_connection();
+    connection_error_exit(conn_result, "Connection failed");
 
-    if (connection_result != DroneCore::ConnectionResult::SUCCESS) {
-        std::cout << ERROR_CONSOLE_TEXT << "Connection failed: "
-                  << DroneCore::connection_result_str(connection_result)
-                  << NORMAL_CONSOLE_TEXT << std::endl;
-        return 1;
+    // Wait for the device to connect via heartbeat
+    while (!dc.is_connected()) {
+        std::cout << "Wait for device to connect via heartbeat" << std::endl;
+        sleep_for(seconds(1));
     }
 
-    std::cout << "Waiting to discover device..." << std::endl;
-    dc.register_on_discover([&discovered_device](uint64_t uuid) {
-        std::cout << "Discovered device with UUID: " << uuid << std::endl;
-        discovered_device = true;
-    });
-
-    // We usually receive heartbeats at 1Hz, therefore we should find a device after around 2 seconds.
-    std::this_thread::sleep_for(std::chrono::seconds(2));
-
-    if (!discovered_device) {
-        std::cout << ERROR_CONSOLE_TEXT << "No device found, exiting." << NORMAL_CONSOLE_TEXT << std::endl;
-        return 1;
-    }
-
-    // We don't need to specify the UUID if it's only one device anyway.
-    // If there were multiple, we could specify it with:
-    // dc.device(uint64_t uuid);
+    // Device got discovered.
     Device &device = dc.device();
-
-    // Check if vehicle is ready to arm
-    if (device.telemetry().health_all_ok() != true) {
-        std::cout << ERROR_CONSOLE_TEXT << "Vehicle not ready to arm" << NORMAL_CONSOLE_TEXT << std::endl;
-        return 1;
+    while (!device.telemetry().health_all_ok()) {
+        std::cout << "Waiting for device to be ready" << std::endl;
+        sleep_for(seconds(1));
     }
+    std::cout << "Device is ready" << std::endl;
 
-    // Arm vehicle
-    std::cout << "Arming..." << std::endl;
-    const Action::Result arm_result = device.action().arm();
-
-    if (arm_result != Action::Result::SUCCESS) {
-        std::cout << ERROR_CONSOLE_TEXT << "Arming failed:" << Action::result_str(
-                      arm_result) << NORMAL_CONSOLE_TEXT << std::endl;
-        return 1;
-    }
+    // Arm
+    Action::Result arm_result = device.action().arm();
+    action_error_exit(arm_result, "Arming failed");
+    std::cout << "Armed" << std::endl;
 
+    // Subscribe to receive updates on flight mode. You can find out whether FollowMe is active.
     device.telemetry().flight_mode_async(
     std::bind([&](Telemetry::FlightMode flight_mode) {
-        auto last_location = device.follow_me().get_last_location();
-
+        const FollowMe::TargetLocation last_location = device.follow_me().get_last_location();
         std::cout << "[FlightMode: " << Telemetry::flight_mode_str(flight_mode)
                   << "] Vehicle is at Lat: " << last_location.latitude_deg << " deg, "  <<
                   "Lon: " << last_location.longitude_deg << " deg." << std::endl;
     }, _1));
 
-
-    // Take off
-    std::cout << "Taking off..." << std::endl;
-    const Action::Result takeoff_result = device.action().takeoff();
-    if (takeoff_result != Action::Result::SUCCESS) {
-        std::cout << ERROR_CONSOLE_TEXT << "Takeoff failed:" << Action::result_str(
-                      takeoff_result) << NORMAL_CONSOLE_TEXT << std::endl;
-        return 1;
-    }
-    std::this_thread::sleep_for(std::chrono::seconds(5));
+    // Takeoff
+    Action::Result takeoff_result = device.action().takeoff();
+    action_error_exit(takeoff_result, "Takeoff failed");
+    std::cout << "In Air..." << std::endl;
+    sleep_for(seconds(5));
 
     // Start Follow Me
-    device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
     FollowMe::Result follow_me_result = device.follow_me().start();
-    if (follow_me_result != FollowMe::Result::SUCCESS) {
-        std::cout << ERROR_CONSOLE_TEXT << "failed to initiate follow me mode" << std::endl;
+    follow_me_error_exit(follow_me_result, "Failed to start FollowMe mode");
 
-    }
-    std::this_thread::sleep_for(std::chrono::seconds(10));
+    // Keep sending location updates to Drone at rate 1Hz.
+    send_location_updates(device.follow_me());
 
-    std::cout << "Landing..." << std::endl;
+    // Land
     const Action::Result land_result = device.action().land();
-    if (land_result != Action::Result::SUCCESS) {
-        std::cout << ERROR_CONSOLE_TEXT << "Land failed:" << Action::result_str(
-                      land_result) << NORMAL_CONSOLE_TEXT << std::endl;
-        return 1;
-    }
+    action_error_exit(land_result, "Landing failed");
 
     // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
-    std::this_thread::sleep_for(std::chrono::seconds(5));
+    sleep_for(seconds(5));
     std::cout << "Finished..." << std::endl;
     return 0;
 }
+
+// Handles Action's result
+inline void action_error_exit(Action::Result result, const std::string &message)
+{
+    if (result != Action::Result::SUCCESS) {
+        std::cerr << ERROR_CONSOLE_TEXT << message << Action::result_str(
+                      result) << NORMAL_CONSOLE_TEXT << std::endl;
+        exit(EXIT_FAILURE);
+    }
+}
+// Handles FollowMe's result
+inline void follow_me_error_exit(FollowMe::Result result, const std::string &message)
+{
+    if (result != FollowMe::Result::SUCCESS) {
+        std::cerr << ERROR_CONSOLE_TEXT << message << FollowMe::result_str(
+                      result) << NORMAL_CONSOLE_TEXT << std::endl;
+        exit(EXIT_FAILURE);
+    }
+}
+// Handles connection result
+inline void connection_error_exit(DroneCore::ConnectionResult result, const std::string &message)
+{
+    if (result != DroneCore::ConnectionResult::SUCCESS) {
+        std::cerr << ERROR_CONSOLE_TEXT << message
+                  << DroneCore::connection_result_str(result)
+                  << NORMAL_CONSOLE_TEXT << std::endl;
+        exit(EXIT_FAILURE);
+    }
+}
+
+void send_location_updates(FollowMe &follow_me, size_t count, float rate)
+{
+    FollowMe::TargetLocation location = { 47.3977419, 8.5455938, 0.0, 0.f, 0.f, 0.f };
+    const auto LATITUDE_IN_DEG_PER_METER = 0.000009044;
+    const auto LONGITUDE_IN_DEG_PER_METER = 0.000008985;
+
+    for (auto i = 1u; i < count / 5; i++) {
+        follow_me.set_target_location(location);
+        auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
+        sleep_for(milliseconds(sleep_duration_ms));
+        location.latitude_deg -= LATITUDE_IN_DEG_PER_METER * 4;
+    }
+    for (auto i = 1u; i < count / 5; i++) {
+        follow_me.set_target_location(location);
+        auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
+        sleep_for(milliseconds(sleep_duration_ms));
+        location.longitude_deg += LONGITUDE_IN_DEG_PER_METER * 4;
+    }
+    for (auto i = 1u; i < count / 5; i++) {
+        follow_me.set_target_location(location);
+        auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
+        sleep_for(milliseconds(sleep_duration_ms));
+        location.latitude_deg += LATITUDE_IN_DEG_PER_METER * 4;
+    }
+    for (auto i = 1u; i < count / 5; i++) {
+        follow_me.set_target_location(location);
+        auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
+        sleep_for(milliseconds(sleep_duration_ms));
+        location.longitude_deg -= LONGITUDE_IN_DEG_PER_METER * 4;
+    }
+}
diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index 218902a4e8..63ffc660e7 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -307,13 +307,13 @@ void FollowMeImpl::send_target_location()
     uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds
 
     _mutex.lock();
-    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
-               <<
-               " Alt: " << _target_location.absolute_altitude_m;
+//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
+//               <<
+//               " Alt: " << _target_location.absolute_altitude_m;
     const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
     const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
     const float alt = static_cast<float>(_target_location.absolute_altitude_m);
-    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
+//    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
     _mutex.unlock();
 
     const float pos_std_dev[] = { NAN, NAN, NAN };

From c8e9eb92fb62362bded923b4a7cd675498992284 Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 5 Jan 2018 13:13:06 +0530
Subject: [PATCH 04/16] Add `FakeLocationProvider` class for Follow Me example

This change uses Boost APIs for registering with `FakeLocationProvider`.
Removed location generator logic in example.
Also, adds stop Follow Me before landing.
---
 example/follow_me/CMakeLists.txt             |  8 ++-
 example/follow_me/fake_location_provider.cpp | 60 ++++++++++++++++++++
 example/follow_me/fake_location_provider.h   | 42 ++++++++++++++
 example/follow_me/follow_me.cpp              | 52 +++++------------
 4 files changed, 122 insertions(+), 40 deletions(-)
 create mode 100644 example/follow_me/fake_location_provider.cpp
 create mode 100644 example/follow_me/fake_location_provider.h

diff --git a/example/follow_me/CMakeLists.txt b/example/follow_me/CMakeLists.txt
index 1cd0c0d4fc..4598137241 100644
--- a/example/follow_me/CMakeLists.txt
+++ b/example/follow_me/CMakeLists.txt
@@ -2,8 +2,10 @@ cmake_minimum_required(VERSION 2.8.12)
 
 project(follow_me)
 
-add_definitions("-std=c++11 -Wall -Wextra -Werror")
+find_package(Boost 1.66 COMPONENTS REQUIRED system)
+include_directories(${Boost_INCLUDE_DIR})
 
+add_definitions("-std=c++11 -g -Wall -Wextra -Werror")
 if(NOT MSVC)
     add_definitions("-std=c++11 -Wall -Wextra -Werror")
 else()
@@ -14,9 +16,11 @@ endif()
 
 add_executable(follow_me
     follow_me.cpp
+    fake_location_provider.cpp
 )
 
 target_link_libraries(follow_me
+    LINK_PUBLIC ${Boost_LIBRARIES}
     dronecore
+    pthread
 )
-
diff --git a/example/follow_me/fake_location_provider.cpp b/example/follow_me/fake_location_provider.cpp
new file mode 100644
index 0000000000..16ff5a68c5
--- /dev/null
+++ b/example/follow_me/fake_location_provider.cpp
@@ -0,0 +1,60 @@
+
+#include "fake_location_provider.h"
+
+void FakeLocationProvider::request_location_updates(location_callback_t callback)
+{
+    location_callback_ = callback;
+    timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+}
+
+// Rudimentary location provider whose successive lat, lon combination
+// makes Drone revolve in a semi-circular path.
+void FakeLocationProvider::compute_next_location()
+{
+    if (count_++ < 10) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        latitude_deg_ -= LATITUDE_DEG_PER_METER * 4;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < 20) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        longitude_deg_ += LONGITUDE_DEG_PER_METER * 4;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < 30) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        latitude_deg_ += LATITUDE_DEG_PER_METER * 4;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < 40) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        longitude_deg_ -= LONGITUDE_DEG_PER_METER * 4;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < 50) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        latitude_deg_ -= LATITUDE_DEG_PER_METER * 3;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < MAX_LOCATIONS) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        longitude_deg_ += LONGITUDE_DEG_PER_METER * 3;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+}
+
+const size_t FakeLocationProvider::MAX_LOCATIONS = 60u;
+const double FakeLocationProvider::LATITUDE_DEG_PER_METER = 0.000009044;
+const double FakeLocationProvider::LONGITUDE_DEG_PER_METER = 0.000008985;
diff --git a/example/follow_me/fake_location_provider.h b/example/follow_me/fake_location_provider.h
new file mode 100644
index 0000000000..f7f00137bf
--- /dev/null
+++ b/example/follow_me/fake_location_provider.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <functional>
+#include <boost/asio.hpp>
+#include <boost/bind.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+
+/**
+ * @brief The FakeLocationProvider class
+ * This class provides periodic reports on the fake location of the device.
+ */
+class FakeLocationProvider
+{
+public:
+    typedef std::function<void(double lat, double lon)> location_callback_t;
+
+    FakeLocationProvider(boost::asio::io_context &io)
+        : timer_(io, boost::posix_time::seconds(1))
+    {
+    }
+
+    ~FakeLocationProvider()
+    {
+        std::cout << "FakeLocationProvider: Done" << std::endl;
+    }
+
+    void request_location_updates(location_callback_t callback);
+
+private:
+
+    void compute_next_location();
+
+    boost::asio::deadline_timer timer_;
+    location_callback_t location_callback_ = nullptr;
+    double latitude_deg_ = 47.3977419;
+    double longitude_deg_ = 8.5455938;
+    size_t count_ = 0u;
+
+    static const size_t MAX_LOCATIONS;
+    static const double LATITUDE_DEG_PER_METER;
+    static const double LONGITUDE_DEG_PER_METER;
+};
diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
index 7401716533..dd21ad9579 100644
--- a/example/follow_me/follow_me.cpp
+++ b/example/follow_me/follow_me.cpp
@@ -9,6 +9,7 @@
 #include <iostream>
 #include <thread>
 #include <chrono>
+#include "fake_location_provider.h"
 
 using namespace dronecore;
 using namespace std::placeholders; // for `_1`
@@ -23,7 +24,6 @@ using namespace std::this_thread;  // for sleep_for()
 inline void action_error_exit(Action::Result result, const std::string &message);
 inline void follow_me_error_exit(FollowMe::Result result, const std::string &message);
 inline void connection_error_exit(DroneCore::ConnectionResult result, const std::string &message);
-void send_location_updates(FollowMe &follow_me, size_t count = 50ul, float rate = 1.f);
 
 int main(int, char **)
 {
@@ -56,9 +56,9 @@ int main(int, char **)
     std::bind([&](Telemetry::FlightMode flight_mode) {
         const FollowMe::TargetLocation last_location = device.follow_me().get_last_location();
         std::cout << "[FlightMode: " << Telemetry::flight_mode_str(flight_mode)
-                  << "] Vehicle is at Lat: " << last_location.latitude_deg << " deg, "  <<
-                  "Lon: " << last_location.longitude_deg << " deg." << std::endl;
-    }, _1));
+                  << "] Vehicle is at: " << last_location.latitude_deg << ", "
+                  << last_location.longitude_deg << " degrees." << std::endl;
+    }, std::placeholders::_1));
 
     // Takeoff
     Action::Result takeoff_result = device.action().takeoff();
@@ -70,8 +70,16 @@ int main(int, char **)
     FollowMe::Result follow_me_result = device.follow_me().start();
     follow_me_error_exit(follow_me_result, "Failed to start FollowMe mode");
 
-    // Keep sending location updates to Drone at rate 1Hz.
-    send_location_updates(device.follow_me());
+    boost::asio::io_context io; // for event loop
+    // Register for platform-specific Location provider. We're using FakeLocationProvider for the example.
+    (new FakeLocationProvider(io))->request_location_updates([&device](double lat, double lon) {
+        device.follow_me().set_target_location({lat, lon, 0.0, 0.f, 0.f, 0.f});
+    });
+    io.run(); // will run as long as location updates continue to happen.
+
+    // Stop Follow Me
+    follow_me_result = device.follow_me().stop();
+    follow_me_error_exit(follow_me_result, "Failed to stop FollowMe mode");
 
     // Land
     const Action::Result land_result = device.action().land();
@@ -111,35 +119,3 @@ inline void connection_error_exit(DroneCore::ConnectionResult result, const std:
         exit(EXIT_FAILURE);
     }
 }
-
-void send_location_updates(FollowMe &follow_me, size_t count, float rate)
-{
-    FollowMe::TargetLocation location = { 47.3977419, 8.5455938, 0.0, 0.f, 0.f, 0.f };
-    const auto LATITUDE_IN_DEG_PER_METER = 0.000009044;
-    const auto LONGITUDE_IN_DEG_PER_METER = 0.000008985;
-
-    for (auto i = 1u; i < count / 5; i++) {
-        follow_me.set_target_location(location);
-        auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
-        sleep_for(milliseconds(sleep_duration_ms));
-        location.latitude_deg -= LATITUDE_IN_DEG_PER_METER * 4;
-    }
-    for (auto i = 1u; i < count / 5; i++) {
-        follow_me.set_target_location(location);
-        auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
-        sleep_for(milliseconds(sleep_duration_ms));
-        location.longitude_deg += LONGITUDE_IN_DEG_PER_METER * 4;
-    }
-    for (auto i = 1u; i < count / 5; i++) {
-        follow_me.set_target_location(location);
-        auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
-        sleep_for(milliseconds(sleep_duration_ms));
-        location.latitude_deg += LATITUDE_IN_DEG_PER_METER * 4;
-    }
-    for (auto i = 1u; i < count / 5; i++) {
-        follow_me.set_target_location(location);
-        auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
-        sleep_for(milliseconds(sleep_duration_ms));
-        location.longitude_deg -= LONGITUDE_IN_DEG_PER_METER * 4;
-    }
-}

From 76a7ed48996ff6b17e7b76d6e5bd42abd01f1cf0 Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 5 Jan 2018 13:57:57 +0530
Subject: [PATCH 05/16] Restore Follow Me integration test

Experimental changes in Follow Me integration test
was accidentally committed. This commit restore original.
---
 integration_tests/follow_me.cpp      | 108 +--------------------------
 plugins/follow_me/follow_me_impl.cpp |   6 +-
 2 files changed, 6 insertions(+), 108 deletions(-)

diff --git a/integration_tests/follow_me.cpp b/integration_tests/follow_me.cpp
index 9f4e8fc4fa..34abf47438 100644
--- a/integration_tests/follow_me.cpp
+++ b/integration_tests/follow_me.cpp
@@ -15,7 +15,7 @@ using namespace std::placeholders;
 void print(const FollowMe::Config &config);
 void send_location_updates(FollowMe &follow_me_handle, size_t count = 25ul, float rate = 1.f);
 
-const size_t N_LOCATIONS = 200ul;
+const size_t N_LOCATIONS = 100ul;
 
 TEST_F(SitlTest, FollowMeOneLocation)
 {
@@ -54,15 +54,15 @@ TEST_F(SitlTest, FollowMeOneLocation)
     print(curr_config);
 
     // Set just a single location before starting FollowMe (optional)
-    device.follow_me().set_target_location({47.3978439, 8.5452824, 0.0, 0.f, 0.f, 0.f});
+    device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
 
     // Start following with default configuration
     FollowMe::Result follow_me_result = device.follow_me().start();
     ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
     sleep_for(seconds(1));
 
-    std::cout << "We're waiting (for 10s) to see the drone moving target location set." << std::endl;
-    sleep_for(seconds(10));
+    std::cout << "We're waiting (for 5s) to see the drone moving target location set." << std::endl;
+    sleep_for(seconds(5));
 
     // stop following
     follow_me_result = device.follow_me().stop();
@@ -160,106 +160,6 @@ void send_location_updates(FollowMe &follow_me, size_t count, float rate)
     // Altitude here is ignored by PX4, as we've set min altitude in configuration.
     std::array<FollowMe::TargetLocation, N_LOCATIONS> spiral_path = {
         {
-            { 47.3997373, 8.5426978, 490.0, 0.f, 0.f, 0.f },
-            { 47.39973730, 8.54269780, 510.0, 0.f, 0.f, 0.f },
-            { 47.39780291, 8.54557048, 490.0, 0.f, 0.f, 0.f },
-            { 47.39779838, 8.54555174, 492.0, 0.f, 0.f, 0.f },
-            { 47.39778748, 8.54554499, 493.0, 0.f, 0.f, 0.f },
-            { 47.39777659, 8.54553561, 494.0, 0.f, 0.f, 0.f },
-            { 47.39776569, 8.54553292, 494.0, 0.f, 0.f, 0.f },
-            { 47.39774663, 8.54552622, 501.0, 0.f, 0.f, 0.f },
-            { 47.39772938, 8.54552488, 501.0, 0.f, 0.f, 0.f },
-            { 47.39771304, 8.54554231, 502.0, 0.f, 0.f, 0.f },
-            { 47.39770578, 8.54557445, 504.0, 0.f, 0.f, 0.f },
-            { 47.39770487, 8.54559596, 502.0, 0.f, 0.f, 0.f },
-            { 47.39770578, 8.54561741, 502.0, 0.f, 0.f, 0.f },
-            { 47.39770669, 8.54563887, 502.0, 0.f, 0.f, 0.f },
-            { 47.39771486, 8.54565765, 493.0, 0.f, 0.f, 0.f },
-            { 47.39773029, 8.54567642, 496.0, 0.f, 0.f, 0.f },
-            { 47.39775026, 8.54568447, 502.0, 0.f, 0.f, 0.f },
-            { 47.39776751, 8.54569118, 502.0, 0.f, 0.f, 0.f },
-            { 47.39778203, 8.54569118, 502.0, 0.f, 0.f, 0.f },
-            { 47.39779838, 8.54568447, 502.0, 0.f, 0.f, 0.f },
-            { 47.39781835, 8.54566972, 502.0, 0.f, 0.f, 0.f },
-            { 47.39782107, 8.54564692, 502.0, 0.f, 0.f, 0.f },
-            { 47.39782474, 8.54561876, 502.0, 0.f, 0.f, 0.f },
-            { 47.39782475, 8.54559193, 501.0, 0.f, 0.f, 0.f },
-            { 47.39782474, 8.54556511, 501.0, 0.f, 0.f, 0.f },
-            { 47.39782107, 8.54553427, 503.0, 0.f, 0.f, 0.f },
-            { 47.39780836, 8.54552756, 502.0, 0.f, 0.f, 0.f },
-            { 47.39779656, 8.54551549, 501.0, 0.f, 0.f, 0.f },
-            { 47.39777568, 8.54550342, 501.0, 0.f, 0.f, 0.f },
-            { 47.39775482, 8.54549671, 504.0, 0.f, 0.f, 0.f },
-            { 47.39773755, 8.54549671, 501.0, 0.f, 0.f, 0.f },
-            { 47.39771849, 8.54550208, 501.0, 0.f, 0.f, 0.f },
-            { 47.39770396, 8.54551415, 502.0, 0.f, 0.f, 0.f },
-            { 47.39769398, 8.54554097, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768762, 8.54556243, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768672, 8.54557852, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768493, 8.54559998, 502.0, 0.f, 0.f, 0.f },
-            { 47.39768399, 8.54562278, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768399, 8.54564155, 501.0, 0.f, 0.f, 0.f },
-            { 47.39769035, 8.54566569, 501.0, 0.f, 0.f, 0.f },
-            { 47.39770759, 8.54568983, 502.0, 0.f, 0.f, 0.f },
-            { 47.39772757, 8.54569922, 502.0, 0.f, 0.f, 0.f },
-            { 47.39774481, 8.54570727, 507.0, 0.f, 0.f, 0.f },
-            { 47.39776025, 8.54572202, 502.0, 0.f, 0.f, 0.f },
-            { 47.39778567, 8.54572336, 505.0, 0.f, 0.f, 0.f },
-            { 47.39780291, 8.54572202, 507.0, 0.f, 0.f, 0.f },
-            { 47.39782107, 8.54571263, 502.0, 0.f, 0.f, 0.f },
-            { 47.39783469, 8.54569788, 501.0, 0.f, 0.f, 0.f },
-            { 47.39783832, 8.54568179, 501.0, 0.f, 0.f, 0.f },
-            { 47.39784104, 8.54566569, 503.0, 0.f, 0.f, 0.f },
-            { 47.39784376, 8.54564424, 502.0, 0.f, 0.f, 0.f },
-            { 47.39784386, 8.54564435, 503.0, 0.f, 0.f, 0.f },
-            { 47.39784396, 8.54564444, 505.0, 0.f, 0.f, 0.f },
-            { 47.39784386, 8.54564454, 503.0, 0.f, 0.f, 0.f },
-            { 47.39784346, 8.54564464, 504.0, 0.f, 0.f, 0.f },
-            { 47.39784336, 8.54564424, 501.0, 0.f, 0.f, 0.f },
-            { 47.39772757, 8.54569922, 501.0, 0.f, 0.f, 0.f },
-            { 47.39774481, 8.54570727, 503.0, 0.f, 0.f, 0.f },
-            { 47.39776025, 8.54572202, 501.0, 0.f, 0.f, 0.f },
-            { 47.39778567, 8.54572336, 503.0, 0.f, 0.f, 0.f },
-            { 47.39770396, 8.54551415, 501.0, 0.f, 0.f, 0.f },
-            { 47.39769398, 8.54554097, 502.0, 0.f, 0.f, 0.f },
-            { 47.39768762, 8.54556243, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768672, 8.54557852, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768494, 8.54559998, 501.0, 0.f, 0.f, 0.f },
-            { 47.39779454, 8.54559464, 502.0, 0.f, 0.f, 0.f },
-            { 47.39780291, 8.54557048, 489.0, 0.f, 0.f, 0.f },
-            { 47.39779838, 8.54555173, 502.0, 0.f, 0.f, 0.f },
-            { 47.39778748, 8.54554499, 508.0, 0.f, 0.f, 0.f },
-            { 47.39777659, 8.54553561, 510.0, 0.f, 0.f, 0.f },
-            { 47.39776569, 8.54553292, 501.0, 0.f, 0.f, 0.f },
-            { 47.39774663, 8.54552622, 502.0, 0.f, 0.f, 0.f },
-            { 47.39772938, 8.54552488, 505.0, 0.f, 0.f, 0.f },
-            { 47.39771304, 8.54554231, 505.0, 0.f, 0.f, 0.f },
-
-
-
-            { 47.39784933,  8.54555596,  0.0, 0.f, 0.f, 0.f },
-            { 47.39784933, 8.54555596,  0.0, 0.f, 0.f, 0.f },
-            { 47.39784388, 8.54528237,  0.0, 0.f, 0.f, 0.f },
-            { 47.39776854, 8.54522604,  0.0, 0.f, 0.f, 0.f },
-            { 47.39764689, 8.54521666,  0.0, 0.f, 0.f, 0.f },
-            { 47.39754158, 8.54527566,  0.0, 0.f, 0.f, 0.f },
-            { 47.39752252, 8.54552511,  0.0, 0.f, 0.f, 0.f },
-            { 47.39750618, 8.5458805,  0.0, 0.f, 0.f, 0.f },
-            { 47.39758334, 8.54593281,  0.0, 0.f, 0.f, 0.f },
-            { 47.39775219, 8.54612861,  0.0, 0.f, 0.f, 0.f },
-            { 47.39785932, 8.546024,  0.0, 0.f, 0.f, 0.f },
-            { 47.39793739, 8.54574371,  0.0, 0.f, 0.f, 0.f },
-            { 47.39808445, 8.54564849,  0.0, 0.f, 0.f, 0.f },
-            { 47.39808899, 8.54552645,  0.0, 0.f, 0.f, 0.f },
-            { 47.39807174, 8.54531053,  0.0, 0.f, 0.f, 0.f },
-            { 47.39800456, 8.54527566,  0.0, 0.f, 0.f, 0.f },
-            { 47.39799912, 8.54549829,  0.0, 0.f, 0.f, 0.f },
-            { 47.39797188, 8.54559485,  0.0, 0.f, 0.f, 0.f },
-            { 47.39786839, 8.54575042,  0.0, 0.f, 0.f, 0.f },
-            { 47.39779668, 8.54575444,  0.0, 0.f, 0.f, 0.f },
-            { 47.39779486, 8.54564849,  0.0, 0.f, 0.f, 0.f },
-
-            /////////////////////////////////////////////////
             { 47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f },
             { 47.39769035, 8.54566569, 0.0, 0.f, 0.f, 0.f },
             { 47.39770759, 8.54568983, 0.0, 0.f, 0.f, 0.f },
diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index 63ffc660e7..9f6da380d4 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -307,13 +307,11 @@ void FollowMeImpl::send_target_location()
     uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds
 
     _mutex.lock();
-//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
-//               <<
-//               " Alt: " << _target_location.absolute_altitude_m;
+//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg <<
+//	" Alt: " << _target_location.absolute_altitude_m;
     const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
     const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
     const float alt = static_cast<float>(_target_location.absolute_altitude_m);
-//    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
     _mutex.unlock();
 
     const float pos_std_dev[] = { NAN, NAN, NAN };

From 9b69f0a4a69f7fca29cd71546a477f1ef6ad6cf0 Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 5 Jan 2018 15:22:06 +0530
Subject: [PATCH 06/16] Rename `follow_dist_m` to `follow_distance_m` for
 consistency

---
 integration_tests/follow_me.cpp      |  4 ++--
 plugins/follow_me/follow_me.h        |  2 +-
 plugins/follow_me/follow_me_impl.cpp | 12 ++++++------
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/integration_tests/follow_me.cpp b/integration_tests/follow_me.cpp
index 34abf47438..13322e3880 100644
--- a/integration_tests/follow_me.cpp
+++ b/integration_tests/follow_me.cpp
@@ -111,7 +111,7 @@ TEST_F(SitlTest, FollowMeMultiLocationWithConfig)
     // configure follow me behaviour
     FollowMe::Config config;
     config.min_height_m = 12.f; // increase min height
-    config.follow_dist_m = 20.f; // set distance b/w device and target during FollowMe mode
+    config.follow_distance_m = 20.f; // set distance b/w device and target during FollowMe mode
     config.responsiveness = 0.2f; // set to higher responsiveness
     config.follow_direction =
         FollowMe::Config::FollowDirection::FRONT; // Device follows target from FRONT side
@@ -148,7 +148,7 @@ void print(const FollowMe::Config &config)
     std::cout << "Current FollowMe configuration of the device" << std::endl;
     std::cout << "---------------------------" << std::endl;
     std::cout << "Min Height: " << config.min_height_m << "m" << std::endl;
-    std::cout << "Distance: " << config.follow_dist_m << "m" << std::endl;
+    std::cout << "Distance: " << config.follow_distance_m << "m" << std::endl;
     std::cout << "Responsiveness: " << config.responsiveness << std::endl;
     std::cout << "Following from: " << FollowMe::Config::to_str(config.follow_direction) << std::endl;
     std::cout << "---------------------------" << std::endl;
diff --git a/plugins/follow_me/follow_me.h b/plugins/follow_me/follow_me.h
index 70f1cbd07c..d978cef327 100644
--- a/plugins/follow_me/follow_me.h
+++ b/plugins/follow_me/follow_me.h
@@ -70,7 +70,7 @@ class FollowMe
         constexpr static const float MAX_RESPONSIVENESS = 1.0f; /**< @brief Max responsiveness. */
 
         float min_height_m = 8.0f; /**< @brief Min follow height, in meters. */
-        float follow_dist_m = 8.0f; /**< @brief Horizontal follow distance to target, in meters. */
+        float follow_distance_m = 8.0f; /**< @brief Horizontal follow distance to target, in meters. */
         FollowDirection follow_direction =
             FollowDirection::BEHIND; /**< @brief Relative position of the following vehicle. */
         float responsiveness = 0.5f; /**< @brief Responsiveness, Range (0.0-1.0) */
diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index 9f6da380d4..377d544981 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -50,7 +50,7 @@ FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config &config)
     }
 
     auto height = config.min_height_m;
-    auto distance = config.follow_dist_m;
+    auto distance = config.follow_distance_m;
     int32_t direction = static_cast<int32_t>(config.follow_direction);
     auto responsiveness = config.responsiveness;
 
@@ -179,7 +179,7 @@ void FollowMeImpl::set_default_config()
     FollowMe::Config default_config {};
 
     auto height = default_config.min_height_m;
-    auto distance = default_config.follow_dist_m;
+    auto distance = default_config.follow_distance_m;
     int32_t direction = static_cast<int32_t>(default_config.follow_direction);
     auto responsiveness = default_config.responsiveness;
 
@@ -204,7 +204,7 @@ bool FollowMeImpl::is_config_ok(const FollowMe::Config &config) const
 
     if (config.min_height_m < FollowMe::Config::MIN_HEIGHT_M) {
         LogErr() << "Err: Min height must be atleast 8.0 meters";
-    } else if (config.follow_dist_m < FollowMe::Config::MIN_FOLLOW_DIST_M) {
+    } else if (config.follow_distance_m < FollowMe::Config::MIN_FOLLOW_DIST_M) {
         LogErr() << "Err: Min Follow distance must be atleast 1.0 meter";
     } else if (config.follow_direction < FollowMe::Config::FollowDirection::FRONT_RIGHT ||
                config.follow_direction > FollowMe::Config::FollowDirection::NONE) {
@@ -228,12 +228,12 @@ void FollowMeImpl::receive_param_min_height(bool success, float min_height_m)
     }
 }
 
-void FollowMeImpl::receive_param_follow_distance(bool success, float follow_dist_m)
+void FollowMeImpl::receive_param_follow_distance(bool success, float follow_distance_m)
 {
     if (success) {
-        _config.follow_dist_m = follow_dist_m;
+        _config.follow_distance_m = follow_distance_m;
     } else {
-        LogErr() << "Failed to set NAV_FT_DST: " << follow_dist_m << "m";
+        LogErr() << "Failed to set NAV_FT_DST: " << follow_distance_m << "m";
     }
 }
 

From baece82c0381c1cb21b2cc707a1f51a47a35af0d Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 5 Jan 2018 15:27:17 +0530
Subject: [PATCH 07/16] Remove unused variables in Follow Me plugin impl

Removed unused variables `curr_direction_s` & `new_direction_s` in
`FollowMeImpl::receive_param_follow_direction()`.
---
 plugins/follow_me/follow_me_impl.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index 377d544981..23e63b02cc 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -247,8 +247,6 @@ void FollowMeImpl::receive_param_follow_direction(bool success, int32_t directio
         case 3: new_direction = FollowMe::Config::FollowDirection::FRONT_LEFT; break;
         default: break;
     }
-    auto curr_direction_s = FollowMe::Config::to_str(_config.follow_direction);
-    auto new_direction_s = FollowMe::Config::to_str(new_direction);
     if (success) {
         if (new_direction != FollowMe::Config::FollowDirection::NONE) {
             _config.follow_direction = new_direction;

From 1252e52e92bea9dd552cb22131f89608fabcd4a8 Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 5 Jan 2018 15:50:47 +0530
Subject: [PATCH 08/16] Make location provider as smart pointer in Follow Me
 example.

This is done for auto-deletion of the location object when
control goes out of scope.
---
 example/follow_me/follow_me.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
index dd21ad9579..cba589d636 100644
--- a/example/follow_me/follow_me.cpp
+++ b/example/follow_me/follow_me.cpp
@@ -8,6 +8,7 @@
 #include <dronecore/dronecore.h>
 #include <iostream>
 #include <thread>
+#include <memory>
 #include <chrono>
 #include "fake_location_provider.h"
 
@@ -71,8 +72,9 @@ int main(int, char **)
     follow_me_error_exit(follow_me_result, "Failed to start FollowMe mode");
 
     boost::asio::io_context io; // for event loop
+    std::unique_ptr<FakeLocationProvider> location_provider(new FakeLocationProvider(io));
     // Register for platform-specific Location provider. We're using FakeLocationProvider for the example.
-    (new FakeLocationProvider(io))->request_location_updates([&device](double lat, double lon) {
+    location_provider->request_location_updates([&device](double lat, double lon) {
         device.follow_me().set_target_location({lat, lon, 0.0, 0.f, 0.f, 0.f});
     });
     io.run(); // will run as long as location updates continue to happen.

From 5d2a4d4729758e70b5dcf79d0f422b0085bdda5e Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Thu, 11 Jan 2018 11:24:26 +0530
Subject: [PATCH 09/16] Rebase with develop

---
 example/follow_me/CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/example/follow_me/CMakeLists.txt b/example/follow_me/CMakeLists.txt
index 4598137241..1f348007cb 100644
--- a/example/follow_me/CMakeLists.txt
+++ b/example/follow_me/CMakeLists.txt
@@ -5,7 +5,6 @@ project(follow_me)
 find_package(Boost 1.66 COMPONENTS REQUIRED system)
 include_directories(${Boost_INCLUDE_DIR})
 
-add_definitions("-std=c++11 -g -Wall -Wextra -Werror")
 if(NOT MSVC)
     add_definitions("-std=c++11 -Wall -Wextra -Werror")
 else()

From 092ab0a9e407d5336fb5a22669537a98300a372e Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 29 Dec 2017 10:39:01 +0530
Subject: [PATCH 10/16] Add Follow Me example

---
 integration_tests/follow_me.cpp      | 108 ++++++++++++++++++++++++++-
 plugins/follow_me/follow_me_impl.cpp |   6 +-
 2 files changed, 108 insertions(+), 6 deletions(-)

diff --git a/integration_tests/follow_me.cpp b/integration_tests/follow_me.cpp
index 13322e3880..5a81de8eab 100644
--- a/integration_tests/follow_me.cpp
+++ b/integration_tests/follow_me.cpp
@@ -15,7 +15,7 @@ using namespace std::placeholders;
 void print(const FollowMe::Config &config);
 void send_location_updates(FollowMe &follow_me_handle, size_t count = 25ul, float rate = 1.f);
 
-const size_t N_LOCATIONS = 100ul;
+const size_t N_LOCATIONS = 200ul;
 
 TEST_F(SitlTest, FollowMeOneLocation)
 {
@@ -54,15 +54,15 @@ TEST_F(SitlTest, FollowMeOneLocation)
     print(curr_config);
 
     // Set just a single location before starting FollowMe (optional)
-    device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
+    device.follow_me().set_target_location({47.3978439, 8.5452824, 0.0, 0.f, 0.f, 0.f});
 
     // Start following with default configuration
     FollowMe::Result follow_me_result = device.follow_me().start();
     ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
     sleep_for(seconds(1));
 
-    std::cout << "We're waiting (for 5s) to see the drone moving target location set." << std::endl;
-    sleep_for(seconds(5));
+    std::cout << "We're waiting (for 10s) to see the drone moving target location set." << std::endl;
+    sleep_for(seconds(10));
 
     // stop following
     follow_me_result = device.follow_me().stop();
@@ -160,6 +160,106 @@ void send_location_updates(FollowMe &follow_me, size_t count, float rate)
     // Altitude here is ignored by PX4, as we've set min altitude in configuration.
     std::array<FollowMe::TargetLocation, N_LOCATIONS> spiral_path = {
         {
+            { 47.3997373, 8.5426978, 490.0, 0.f, 0.f, 0.f },
+            { 47.39973730, 8.54269780, 510.0, 0.f, 0.f, 0.f },
+            { 47.39780291, 8.54557048, 490.0, 0.f, 0.f, 0.f },
+            { 47.39779838, 8.54555174, 492.0, 0.f, 0.f, 0.f },
+            { 47.39778748, 8.54554499, 493.0, 0.f, 0.f, 0.f },
+            { 47.39777659, 8.54553561, 494.0, 0.f, 0.f, 0.f },
+            { 47.39776569, 8.54553292, 494.0, 0.f, 0.f, 0.f },
+            { 47.39774663, 8.54552622, 501.0, 0.f, 0.f, 0.f },
+            { 47.39772938, 8.54552488, 501.0, 0.f, 0.f, 0.f },
+            { 47.39771304, 8.54554231, 502.0, 0.f, 0.f, 0.f },
+            { 47.39770578, 8.54557445, 504.0, 0.f, 0.f, 0.f },
+            { 47.39770487, 8.54559596, 502.0, 0.f, 0.f, 0.f },
+            { 47.39770578, 8.54561741, 502.0, 0.f, 0.f, 0.f },
+            { 47.39770669, 8.54563887, 502.0, 0.f, 0.f, 0.f },
+            { 47.39771486, 8.54565765, 493.0, 0.f, 0.f, 0.f },
+            { 47.39773029, 8.54567642, 496.0, 0.f, 0.f, 0.f },
+            { 47.39775026, 8.54568447, 502.0, 0.f, 0.f, 0.f },
+            { 47.39776751, 8.54569118, 502.0, 0.f, 0.f, 0.f },
+            { 47.39778203, 8.54569118, 502.0, 0.f, 0.f, 0.f },
+            { 47.39779838, 8.54568447, 502.0, 0.f, 0.f, 0.f },
+            { 47.39781835, 8.54566972, 502.0, 0.f, 0.f, 0.f },
+            { 47.39782107, 8.54564692, 502.0, 0.f, 0.f, 0.f },
+            { 47.39782474, 8.54561876, 502.0, 0.f, 0.f, 0.f },
+            { 47.39782475, 8.54559193, 501.0, 0.f, 0.f, 0.f },
+            { 47.39782474, 8.54556511, 501.0, 0.f, 0.f, 0.f },
+            { 47.39782107, 8.54553427, 503.0, 0.f, 0.f, 0.f },
+            { 47.39780836, 8.54552756, 502.0, 0.f, 0.f, 0.f },
+            { 47.39779656, 8.54551549, 501.0, 0.f, 0.f, 0.f },
+            { 47.39777568, 8.54550342, 501.0, 0.f, 0.f, 0.f },
+            { 47.39775482, 8.54549671, 504.0, 0.f, 0.f, 0.f },
+            { 47.39773755, 8.54549671, 501.0, 0.f, 0.f, 0.f },
+            { 47.39771849, 8.54550208, 501.0, 0.f, 0.f, 0.f },
+            { 47.39770396, 8.54551415, 502.0, 0.f, 0.f, 0.f },
+            { 47.39769398, 8.54554097, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768762, 8.54556243, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768672, 8.54557852, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768493, 8.54559998, 502.0, 0.f, 0.f, 0.f },
+            { 47.39768399, 8.54562278, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768399, 8.54564155, 501.0, 0.f, 0.f, 0.f },
+            { 47.39769035, 8.54566569, 501.0, 0.f, 0.f, 0.f },
+            { 47.39770759, 8.54568983, 502.0, 0.f, 0.f, 0.f },
+            { 47.39772757, 8.54569922, 502.0, 0.f, 0.f, 0.f },
+            { 47.39774481, 8.54570727, 507.0, 0.f, 0.f, 0.f },
+            { 47.39776025, 8.54572202, 502.0, 0.f, 0.f, 0.f },
+            { 47.39778567, 8.54572336, 505.0, 0.f, 0.f, 0.f },
+            { 47.39780291, 8.54572202, 507.0, 0.f, 0.f, 0.f },
+            { 47.39782107, 8.54571263, 502.0, 0.f, 0.f, 0.f },
+            { 47.39783469, 8.54569788, 501.0, 0.f, 0.f, 0.f },
+            { 47.39783832, 8.54568179, 501.0, 0.f, 0.f, 0.f },
+            { 47.39784104, 8.54566569, 503.0, 0.f, 0.f, 0.f },
+            { 47.39784376, 8.54564424, 502.0, 0.f, 0.f, 0.f },
+            { 47.39784386, 8.54564435, 503.0, 0.f, 0.f, 0.f },
+            { 47.39784396, 8.54564444, 505.0, 0.f, 0.f, 0.f },
+            { 47.39784386, 8.54564454, 503.0, 0.f, 0.f, 0.f },
+            { 47.39784346, 8.54564464, 504.0, 0.f, 0.f, 0.f },
+            { 47.39784336, 8.54564424, 501.0, 0.f, 0.f, 0.f },
+            { 47.39772757, 8.54569922, 501.0, 0.f, 0.f, 0.f },
+            { 47.39774481, 8.54570727, 503.0, 0.f, 0.f, 0.f },
+            { 47.39776025, 8.54572202, 501.0, 0.f, 0.f, 0.f },
+            { 47.39778567, 8.54572336, 503.0, 0.f, 0.f, 0.f },
+            { 47.39770396, 8.54551415, 501.0, 0.f, 0.f, 0.f },
+            { 47.39769398, 8.54554097, 502.0, 0.f, 0.f, 0.f },
+            { 47.39768762, 8.54556243, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768672, 8.54557852, 501.0, 0.f, 0.f, 0.f },
+            { 47.39768494, 8.54559998, 501.0, 0.f, 0.f, 0.f },
+            { 47.39779454, 8.54559464, 502.0, 0.f, 0.f, 0.f },
+            { 47.39780291, 8.54557048, 489.0, 0.f, 0.f, 0.f },
+            { 47.39779838, 8.54555173, 502.0, 0.f, 0.f, 0.f },
+            { 47.39778748, 8.54554499, 508.0, 0.f, 0.f, 0.f },
+            { 47.39777659, 8.54553561, 510.0, 0.f, 0.f, 0.f },
+            { 47.39776569, 8.54553292, 501.0, 0.f, 0.f, 0.f },
+            { 47.39774663, 8.54552622, 502.0, 0.f, 0.f, 0.f },
+            { 47.39772938, 8.54552488, 505.0, 0.f, 0.f, 0.f },
+            { 47.39771304, 8.54554231, 505.0, 0.f, 0.f, 0.f },
+
+
+
+            { 47.39784933,  8.54555596,  0.0, 0.f, 0.f, 0.f },
+            { 47.39784933, 8.54555596,  0.0, 0.f, 0.f, 0.f },
+            { 47.39784388, 8.54528237,  0.0, 0.f, 0.f, 0.f },
+            { 47.39776854, 8.54522604,  0.0, 0.f, 0.f, 0.f },
+            { 47.39764689, 8.54521666,  0.0, 0.f, 0.f, 0.f },
+            { 47.39754158, 8.54527566,  0.0, 0.f, 0.f, 0.f },
+            { 47.39752252, 8.54552511,  0.0, 0.f, 0.f, 0.f },
+            { 47.39750618, 8.5458805,  0.0, 0.f, 0.f, 0.f },
+            { 47.39758334, 8.54593281,  0.0, 0.f, 0.f, 0.f },
+            { 47.39775219, 8.54612861,  0.0, 0.f, 0.f, 0.f },
+            { 47.39785932, 8.546024,  0.0, 0.f, 0.f, 0.f },
+            { 47.39793739, 8.54574371,  0.0, 0.f, 0.f, 0.f },
+            { 47.39808445, 8.54564849,  0.0, 0.f, 0.f, 0.f },
+            { 47.39808899, 8.54552645,  0.0, 0.f, 0.f, 0.f },
+            { 47.39807174, 8.54531053,  0.0, 0.f, 0.f, 0.f },
+            { 47.39800456, 8.54527566,  0.0, 0.f, 0.f, 0.f },
+            { 47.39799912, 8.54549829,  0.0, 0.f, 0.f, 0.f },
+            { 47.39797188, 8.54559485,  0.0, 0.f, 0.f, 0.f },
+            { 47.39786839, 8.54575042,  0.0, 0.f, 0.f, 0.f },
+            { 47.39779668, 8.54575444,  0.0, 0.f, 0.f, 0.f },
+            { 47.39779486, 8.54564849,  0.0, 0.f, 0.f, 0.f },
+
+            /////////////////////////////////////////////////
             { 47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f },
             { 47.39769035, 8.54566569, 0.0, 0.f, 0.f, 0.f },
             { 47.39770759, 8.54568983, 0.0, 0.f, 0.f, 0.f },
diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index 23e63b02cc..a04fb705a2 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -305,11 +305,13 @@ void FollowMeImpl::send_target_location()
     uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds
 
     _mutex.lock();
-//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg <<
-//	" Alt: " << _target_location.absolute_altitude_m;
+    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
+               <<
+               " Alt: " << _target_location.absolute_altitude_m;
     const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
     const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
     const float alt = static_cast<float>(_target_location.absolute_altitude_m);
+    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
     _mutex.unlock();
 
     const float pos_std_dev[] = { NAN, NAN, NAN };

From 3d5256d75cd5793217fd15809ef60f48b755b00b Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Thu, 4 Jan 2018 15:06:43 +0530
Subject: [PATCH 11/16] Simplify Follow Me example

1. Added methods for error handling.
2. Improved periodic location update logic.
3. Removed redundant header `cstdint`.
4. Removed log from FollowMe plugin.
5. Improved overall FollowMe example.
---
 plugins/follow_me/follow_me_impl.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index a04fb705a2..99742b0eb8 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -305,13 +305,13 @@ void FollowMeImpl::send_target_location()
     uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds
 
     _mutex.lock();
-    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
-               <<
-               " Alt: " << _target_location.absolute_altitude_m;
+//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
+//               <<
+//               " Alt: " << _target_location.absolute_altitude_m;
     const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
     const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
     const float alt = static_cast<float>(_target_location.absolute_altitude_m);
-    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
+//    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
     _mutex.unlock();
 
     const float pos_std_dev[] = { NAN, NAN, NAN };

From 586c3c560508fe2ce19eaf7c0105e755294ef6eb Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 5 Jan 2018 13:13:06 +0530
Subject: [PATCH 12/16] Add `FakeLocationProvider` class for Follow Me example

This change uses Boost APIs for registering with `FakeLocationProvider`.
Removed location generator logic in example.
Also, adds stop Follow Me before landing.
---
 example/follow_me/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/example/follow_me/CMakeLists.txt b/example/follow_me/CMakeLists.txt
index 1f348007cb..4598137241 100644
--- a/example/follow_me/CMakeLists.txt
+++ b/example/follow_me/CMakeLists.txt
@@ -5,6 +5,7 @@ project(follow_me)
 find_package(Boost 1.66 COMPONENTS REQUIRED system)
 include_directories(${Boost_INCLUDE_DIR})
 
+add_definitions("-std=c++11 -g -Wall -Wextra -Werror")
 if(NOT MSVC)
     add_definitions("-std=c++11 -Wall -Wextra -Werror")
 else()

From 5837ab81b49ab7c816112926472afa8958e73307 Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Fri, 5 Jan 2018 13:57:57 +0530
Subject: [PATCH 13/16] Restore Follow Me integration test

Experimental changes in Follow Me integration test
was accidentally committed. This commit restore original.
---
 integration_tests/follow_me.cpp      | 108 +--------------------------
 plugins/follow_me/follow_me_impl.cpp |   6 +-
 2 files changed, 6 insertions(+), 108 deletions(-)

diff --git a/integration_tests/follow_me.cpp b/integration_tests/follow_me.cpp
index 5a81de8eab..13322e3880 100644
--- a/integration_tests/follow_me.cpp
+++ b/integration_tests/follow_me.cpp
@@ -15,7 +15,7 @@ using namespace std::placeholders;
 void print(const FollowMe::Config &config);
 void send_location_updates(FollowMe &follow_me_handle, size_t count = 25ul, float rate = 1.f);
 
-const size_t N_LOCATIONS = 200ul;
+const size_t N_LOCATIONS = 100ul;
 
 TEST_F(SitlTest, FollowMeOneLocation)
 {
@@ -54,15 +54,15 @@ TEST_F(SitlTest, FollowMeOneLocation)
     print(curr_config);
 
     // Set just a single location before starting FollowMe (optional)
-    device.follow_me().set_target_location({47.3978439, 8.5452824, 0.0, 0.f, 0.f, 0.f});
+    device.follow_me().set_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});
 
     // Start following with default configuration
     FollowMe::Result follow_me_result = device.follow_me().start();
     ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
     sleep_for(seconds(1));
 
-    std::cout << "We're waiting (for 10s) to see the drone moving target location set." << std::endl;
-    sleep_for(seconds(10));
+    std::cout << "We're waiting (for 5s) to see the drone moving target location set." << std::endl;
+    sleep_for(seconds(5));
 
     // stop following
     follow_me_result = device.follow_me().stop();
@@ -160,106 +160,6 @@ void send_location_updates(FollowMe &follow_me, size_t count, float rate)
     // Altitude here is ignored by PX4, as we've set min altitude in configuration.
     std::array<FollowMe::TargetLocation, N_LOCATIONS> spiral_path = {
         {
-            { 47.3997373, 8.5426978, 490.0, 0.f, 0.f, 0.f },
-            { 47.39973730, 8.54269780, 510.0, 0.f, 0.f, 0.f },
-            { 47.39780291, 8.54557048, 490.0, 0.f, 0.f, 0.f },
-            { 47.39779838, 8.54555174, 492.0, 0.f, 0.f, 0.f },
-            { 47.39778748, 8.54554499, 493.0, 0.f, 0.f, 0.f },
-            { 47.39777659, 8.54553561, 494.0, 0.f, 0.f, 0.f },
-            { 47.39776569, 8.54553292, 494.0, 0.f, 0.f, 0.f },
-            { 47.39774663, 8.54552622, 501.0, 0.f, 0.f, 0.f },
-            { 47.39772938, 8.54552488, 501.0, 0.f, 0.f, 0.f },
-            { 47.39771304, 8.54554231, 502.0, 0.f, 0.f, 0.f },
-            { 47.39770578, 8.54557445, 504.0, 0.f, 0.f, 0.f },
-            { 47.39770487, 8.54559596, 502.0, 0.f, 0.f, 0.f },
-            { 47.39770578, 8.54561741, 502.0, 0.f, 0.f, 0.f },
-            { 47.39770669, 8.54563887, 502.0, 0.f, 0.f, 0.f },
-            { 47.39771486, 8.54565765, 493.0, 0.f, 0.f, 0.f },
-            { 47.39773029, 8.54567642, 496.0, 0.f, 0.f, 0.f },
-            { 47.39775026, 8.54568447, 502.0, 0.f, 0.f, 0.f },
-            { 47.39776751, 8.54569118, 502.0, 0.f, 0.f, 0.f },
-            { 47.39778203, 8.54569118, 502.0, 0.f, 0.f, 0.f },
-            { 47.39779838, 8.54568447, 502.0, 0.f, 0.f, 0.f },
-            { 47.39781835, 8.54566972, 502.0, 0.f, 0.f, 0.f },
-            { 47.39782107, 8.54564692, 502.0, 0.f, 0.f, 0.f },
-            { 47.39782474, 8.54561876, 502.0, 0.f, 0.f, 0.f },
-            { 47.39782475, 8.54559193, 501.0, 0.f, 0.f, 0.f },
-            { 47.39782474, 8.54556511, 501.0, 0.f, 0.f, 0.f },
-            { 47.39782107, 8.54553427, 503.0, 0.f, 0.f, 0.f },
-            { 47.39780836, 8.54552756, 502.0, 0.f, 0.f, 0.f },
-            { 47.39779656, 8.54551549, 501.0, 0.f, 0.f, 0.f },
-            { 47.39777568, 8.54550342, 501.0, 0.f, 0.f, 0.f },
-            { 47.39775482, 8.54549671, 504.0, 0.f, 0.f, 0.f },
-            { 47.39773755, 8.54549671, 501.0, 0.f, 0.f, 0.f },
-            { 47.39771849, 8.54550208, 501.0, 0.f, 0.f, 0.f },
-            { 47.39770396, 8.54551415, 502.0, 0.f, 0.f, 0.f },
-            { 47.39769398, 8.54554097, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768762, 8.54556243, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768672, 8.54557852, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768493, 8.54559998, 502.0, 0.f, 0.f, 0.f },
-            { 47.39768399, 8.54562278, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768399, 8.54564155, 501.0, 0.f, 0.f, 0.f },
-            { 47.39769035, 8.54566569, 501.0, 0.f, 0.f, 0.f },
-            { 47.39770759, 8.54568983, 502.0, 0.f, 0.f, 0.f },
-            { 47.39772757, 8.54569922, 502.0, 0.f, 0.f, 0.f },
-            { 47.39774481, 8.54570727, 507.0, 0.f, 0.f, 0.f },
-            { 47.39776025, 8.54572202, 502.0, 0.f, 0.f, 0.f },
-            { 47.39778567, 8.54572336, 505.0, 0.f, 0.f, 0.f },
-            { 47.39780291, 8.54572202, 507.0, 0.f, 0.f, 0.f },
-            { 47.39782107, 8.54571263, 502.0, 0.f, 0.f, 0.f },
-            { 47.39783469, 8.54569788, 501.0, 0.f, 0.f, 0.f },
-            { 47.39783832, 8.54568179, 501.0, 0.f, 0.f, 0.f },
-            { 47.39784104, 8.54566569, 503.0, 0.f, 0.f, 0.f },
-            { 47.39784376, 8.54564424, 502.0, 0.f, 0.f, 0.f },
-            { 47.39784386, 8.54564435, 503.0, 0.f, 0.f, 0.f },
-            { 47.39784396, 8.54564444, 505.0, 0.f, 0.f, 0.f },
-            { 47.39784386, 8.54564454, 503.0, 0.f, 0.f, 0.f },
-            { 47.39784346, 8.54564464, 504.0, 0.f, 0.f, 0.f },
-            { 47.39784336, 8.54564424, 501.0, 0.f, 0.f, 0.f },
-            { 47.39772757, 8.54569922, 501.0, 0.f, 0.f, 0.f },
-            { 47.39774481, 8.54570727, 503.0, 0.f, 0.f, 0.f },
-            { 47.39776025, 8.54572202, 501.0, 0.f, 0.f, 0.f },
-            { 47.39778567, 8.54572336, 503.0, 0.f, 0.f, 0.f },
-            { 47.39770396, 8.54551415, 501.0, 0.f, 0.f, 0.f },
-            { 47.39769398, 8.54554097, 502.0, 0.f, 0.f, 0.f },
-            { 47.39768762, 8.54556243, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768672, 8.54557852, 501.0, 0.f, 0.f, 0.f },
-            { 47.39768494, 8.54559998, 501.0, 0.f, 0.f, 0.f },
-            { 47.39779454, 8.54559464, 502.0, 0.f, 0.f, 0.f },
-            { 47.39780291, 8.54557048, 489.0, 0.f, 0.f, 0.f },
-            { 47.39779838, 8.54555173, 502.0, 0.f, 0.f, 0.f },
-            { 47.39778748, 8.54554499, 508.0, 0.f, 0.f, 0.f },
-            { 47.39777659, 8.54553561, 510.0, 0.f, 0.f, 0.f },
-            { 47.39776569, 8.54553292, 501.0, 0.f, 0.f, 0.f },
-            { 47.39774663, 8.54552622, 502.0, 0.f, 0.f, 0.f },
-            { 47.39772938, 8.54552488, 505.0, 0.f, 0.f, 0.f },
-            { 47.39771304, 8.54554231, 505.0, 0.f, 0.f, 0.f },
-
-
-
-            { 47.39784933,  8.54555596,  0.0, 0.f, 0.f, 0.f },
-            { 47.39784933, 8.54555596,  0.0, 0.f, 0.f, 0.f },
-            { 47.39784388, 8.54528237,  0.0, 0.f, 0.f, 0.f },
-            { 47.39776854, 8.54522604,  0.0, 0.f, 0.f, 0.f },
-            { 47.39764689, 8.54521666,  0.0, 0.f, 0.f, 0.f },
-            { 47.39754158, 8.54527566,  0.0, 0.f, 0.f, 0.f },
-            { 47.39752252, 8.54552511,  0.0, 0.f, 0.f, 0.f },
-            { 47.39750618, 8.5458805,  0.0, 0.f, 0.f, 0.f },
-            { 47.39758334, 8.54593281,  0.0, 0.f, 0.f, 0.f },
-            { 47.39775219, 8.54612861,  0.0, 0.f, 0.f, 0.f },
-            { 47.39785932, 8.546024,  0.0, 0.f, 0.f, 0.f },
-            { 47.39793739, 8.54574371,  0.0, 0.f, 0.f, 0.f },
-            { 47.39808445, 8.54564849,  0.0, 0.f, 0.f, 0.f },
-            { 47.39808899, 8.54552645,  0.0, 0.f, 0.f, 0.f },
-            { 47.39807174, 8.54531053,  0.0, 0.f, 0.f, 0.f },
-            { 47.39800456, 8.54527566,  0.0, 0.f, 0.f, 0.f },
-            { 47.39799912, 8.54549829,  0.0, 0.f, 0.f, 0.f },
-            { 47.39797188, 8.54559485,  0.0, 0.f, 0.f, 0.f },
-            { 47.39786839, 8.54575042,  0.0, 0.f, 0.f, 0.f },
-            { 47.39779668, 8.54575444,  0.0, 0.f, 0.f, 0.f },
-            { 47.39779486, 8.54564849,  0.0, 0.f, 0.f, 0.f },
-
-            /////////////////////////////////////////////////
             { 47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f },
             { 47.39769035, 8.54566569, 0.0, 0.f, 0.f, 0.f },
             { 47.39770759, 8.54568983, 0.0, 0.f, 0.f, 0.f },
diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index 99742b0eb8..23e63b02cc 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -305,13 +305,11 @@ void FollowMeImpl::send_target_location()
     uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds
 
     _mutex.lock();
-//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
-//               <<
-//               " Alt: " << _target_location.absolute_altitude_m;
+//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg <<
+//	" Alt: " << _target_location.absolute_altitude_m;
     const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
     const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
     const float alt = static_cast<float>(_target_location.absolute_altitude_m);
-//    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
     _mutex.unlock();
 
     const float pos_std_dev[] = { NAN, NAN, NAN };

From 3f1f06e0a74701cc57de76fe6686c1961bd47f0d Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Wed, 10 Jan 2018 17:19:50 +0530
Subject: [PATCH 14/16] Fix plugin issues and improve example

1. This commit address DroneCore issue #221.
2. Corresponding changes (from #1 above) in integration tests & example.
3. Replaces boost::bind by std::bind.
4. Adds important note about using Boost in the example.
5. Adds debug_str to distinguish FollowMe plugin debug messages.
---
 example/follow_me/fake_location_provider.cpp | 14 ++--
 example/follow_me/fake_location_provider.h   | 18 ++--
 example/follow_me/follow_me.cpp              | 22 +++--
 integration_tests/follow_me.cpp              |  6 --
 plugins/follow_me/follow_me_impl.cpp         | 87 ++++++++++++++------
 plugins/follow_me/follow_me_impl.h           | 35 ++++++++
 6 files changed, 130 insertions(+), 52 deletions(-)

diff --git a/example/follow_me/fake_location_provider.cpp b/example/follow_me/fake_location_provider.cpp
index 16ff5a68c5..ffb68f0ad2 100644
--- a/example/follow_me/fake_location_provider.cpp
+++ b/example/follow_me/fake_location_provider.cpp
@@ -4,7 +4,7 @@
 void FakeLocationProvider::request_location_updates(location_callback_t callback)
 {
     location_callback_ = callback;
-    timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+    timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
 }
 
 // Rudimentary location provider whose successive lat, lon combination
@@ -15,42 +15,42 @@ void FakeLocationProvider::compute_next_location()
         location_callback_(latitude_deg_, longitude_deg_);
         latitude_deg_ -= LATITUDE_DEG_PER_METER * 4;
         timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
-        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
         sleep(1);
     }
     if (count_++ < 20) {
         location_callback_(latitude_deg_, longitude_deg_);
         longitude_deg_ += LONGITUDE_DEG_PER_METER * 4;
         timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
-        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
         sleep(1);
     }
     if (count_++ < 30) {
         location_callback_(latitude_deg_, longitude_deg_);
         latitude_deg_ += LATITUDE_DEG_PER_METER * 4;
         timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
-        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
         sleep(1);
     }
     if (count_++ < 40) {
         location_callback_(latitude_deg_, longitude_deg_);
         longitude_deg_ -= LONGITUDE_DEG_PER_METER * 4;
         timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
-        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
         sleep(1);
     }
     if (count_++ < 50) {
         location_callback_(latitude_deg_, longitude_deg_);
         latitude_deg_ -= LATITUDE_DEG_PER_METER * 3;
         timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
-        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
         sleep(1);
     }
     if (count_++ < MAX_LOCATIONS) {
         location_callback_(latitude_deg_, longitude_deg_);
         longitude_deg_ += LONGITUDE_DEG_PER_METER * 3;
         timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
-        timer_.async_wait(boost::bind(&FakeLocationProvider::compute_next_location, this));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
         sleep(1);
     }
 }
diff --git a/example/follow_me/fake_location_provider.h b/example/follow_me/fake_location_provider.h
index f7f00137bf..47aef133bd 100644
--- a/example/follow_me/fake_location_provider.h
+++ b/example/follow_me/fake_location_provider.h
@@ -1,8 +1,17 @@
 #pragma once
 
 #include <functional>
+/**
+  ********************************************************************************************
+  ********************************************************************************************
+  Important note: Boost isn't a dependency for DroneCore library.
+  We're using Boost::Asio in this example ONLY to simulate asynchronous Fake location provider.
+  Applications on platforms Android, Windows, Apple, etc should make use their platform-specific
+  Location Provider in place of FakeLocationProvider.
+  ********************************************************************************************
+  ********************************************************************************************
+  */
 #include <boost/asio.hpp>
-#include <boost/bind.hpp>
 #include <boost/date_time/posix_time/posix_time.hpp>
 
 /**
@@ -16,13 +25,10 @@ class FakeLocationProvider
 
     FakeLocationProvider(boost::asio::io_context &io)
         : timer_(io, boost::posix_time::seconds(1))
-    {
-    }
+    {}
 
     ~FakeLocationProvider()
-    {
-        std::cout << "FakeLocationProvider: Done" << std::endl;
-    }
+    {}
 
     void request_location_updates(location_callback_t callback);
 
diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
index cba589d636..11da6e25fc 100644
--- a/example/follow_me/follow_me.cpp
+++ b/example/follow_me/follow_me.cpp
@@ -65,10 +65,16 @@ int main(int, char **)
     Action::Result takeoff_result = device.action().takeoff();
     action_error_exit(takeoff_result, "Takeoff failed");
     std::cout << "In Air..." << std::endl;
-    sleep_for(seconds(5));
+    sleep_for(seconds(5)); // Wait for drone to reach takeoff altitude
+
+    // Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front right".
+    FollowMe::Config config;
+    config.min_height_m = 20.0;
+    config.follow_direction = FollowMe::Config::FollowDirection::FRONT_RIGHT;
+    FollowMe::Result follow_me_result = device.follow_me().set_config(config);
 
     // Start Follow Me
-    FollowMe::Result follow_me_result = device.follow_me().start();
+    follow_me_result = device.follow_me().start();
     follow_me_error_exit(follow_me_result, "Failed to start FollowMe mode");
 
     boost::asio::io_context io; // for event loop
@@ -83,13 +89,17 @@ int main(int, char **)
     follow_me_result = device.follow_me().stop();
     follow_me_error_exit(follow_me_result, "Failed to stop FollowMe mode");
 
+    // Stop flight mode updates.
+    device.telemetry().flight_mode_async(nullptr);
+
     // Land
     const Action::Result land_result = device.action().land();
     action_error_exit(land_result, "Landing failed");
-
-    // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
-    sleep_for(seconds(5));
-    std::cout << "Finished..." << std::endl;
+    while (device.telemetry().in_air()) {
+        std::cout << "waiting until landed" << std::endl;
+        sleep_for(seconds(1));
+    }
+    std::cout << "Landed..." << std::endl;
     return 0;
 }
 
diff --git a/integration_tests/follow_me.cpp b/integration_tests/follow_me.cpp
index 13322e3880..66e501d4c9 100644
--- a/integration_tests/follow_me.cpp
+++ b/integration_tests/follow_me.cpp
@@ -120,12 +120,6 @@ TEST_F(SitlTest, FollowMeMultiLocationWithConfig)
     FollowMe::Result config_result = device.follow_me().set_config(config);
     ASSERT_EQ(FollowMe::Result::SUCCESS, config_result);
 
-    sleep_for(seconds(2)); // until config is applied
-
-    // Verify your configuration
-    auto curr_config = device.follow_me().get_config();
-    print(curr_config);
-
     // Start following
     FollowMe::Result follow_me_result = device.follow_me().start();
     ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
index 23e63b02cc..166870403a 100644
--- a/plugins/follow_me/follow_me_impl.cpp
+++ b/plugins/follow_me/follow_me_impl.cpp
@@ -45,7 +45,7 @@ FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config &config)
 {
     // Valdidate configuration
     if (!is_config_ok(config)) {
-        LogErr() << "set_config() failed. Last configuration is preserved.";
+        LogErr() << debug_str << "set_config() failed. Last configuration is preserved.";
         return FollowMe::Result::SET_CONFIG_FAILED;
     }
 
@@ -54,23 +54,52 @@ FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config &config)
     int32_t direction = static_cast<int32_t>(config.follow_direction);
     auto responsiveness = config.responsiveness;
 
+    _config_change_requested = 0;
+
     // Send configuration to Vehicle
-    _parent->set_param_float_async("NAV_MIN_FT_HT", height,
-                                   std::bind(&FollowMeImpl::receive_param_min_height,
-                                             this, _1, height));
-    _parent->set_param_float_async("NAV_FT_DST", distance,
-                                   std::bind(&FollowMeImpl::receive_param_follow_distance,
-                                             this, _1, distance));
-    _parent->set_param_int_async("NAV_FT_FS", direction,
-                                 std::bind(&FollowMeImpl::receive_param_follow_direction,
-                                           this, _1, direction));
-    _parent->set_param_float_async("NAV_FT_RS", responsiveness,
-                                   std::bind(&FollowMeImpl::receive_param_responsiveness,
-                                             this, _1, responsiveness));
+    if (_config.min_height_m != height) {
+        _parent->set_param_float_async("NAV_MIN_FT_HT", height,
+                                       std::bind(&FollowMeImpl::receive_param_min_height,
+                                                 this, _1, height));
+        _config_change_requested |= ConfigParameter::MIN_HEIGHT;
+    }
+    if (_config.follow_distance_m != distance) {
+        _parent->set_param_float_async("NAV_FT_DST", distance,
+                                       std::bind(&FollowMeImpl::receive_param_follow_distance,
+                                                 this, _1, distance));
+        _config_change_requested |= ConfigParameter::DISTANCE;
+    }
+    if (_config.follow_direction != config.follow_direction) {
+        _parent->set_param_int_async("NAV_FT_FS", direction,
+                                     std::bind(&FollowMeImpl::receive_param_follow_direction,
+                                               this, _1, direction));
+        _config_change_requested |= ConfigParameter::FOLLOW_DIRECTION;
+    }
+    if (_config.responsiveness != responsiveness) {
+        _parent->set_param_float_async("NAV_FT_RS", responsiveness,
+                                       std::bind(&FollowMeImpl::receive_param_responsiveness,
+                                                 this, _1, responsiveness));
+        _config_change_requested |= ConfigParameter::RESPONSIVENESS;
+    }
+
+    if (_config_change_requested == 0) {
+        LogDebug() << debug_str <<  "Requested configuration is NO different from existing one!";
+    } else {
+        using namespace std::this_thread; // for sleep_for()
+        using namespace std::chrono; // for milliseconds()
+        // Lets wait for confirmation from Vehicle about configuration change.
+        while (_config_change_requested) {
+            LogDebug() << debug_str <<  "Waiting for the device confirmation of the new configuration..";
+            sleep_for(milliseconds(10));
+        }
+        LogInfo() << debug_str <<  "Configured: " << ANSI_COLOR_BLUE << "Min height: " <<
+                  _config.min_height_m <<
+                  " meters, Follow distance: " <<
+                  _config.follow_distance_m << " meters, Follow direction: " <<
+                  FollowMe::Config::to_str(_config.follow_direction) << ", Responsiveness: " <<
+                  _config.responsiveness << ANSI_COLOR_RESET;
+    }
 
-    // FIXME: We've sent valid configuration to Vehicle.
-    // But that doesn't mean configuration is applied, untill we receive confirmation.
-    // For now we're hoping that it is applied successfully.
     return FollowMe::Result::SUCCESS;
 }
 
@@ -175,7 +204,7 @@ FollowMe::Result FollowMeImpl::stop()
 // Applies default FollowMe configuration to the device
 void FollowMeImpl::set_default_config()
 {
-    LogInfo() << "Applying default FollowMe configuration FollowMe to the device...";
+    LogInfo() << debug_str <<  "Applying default FollowMe configuration FollowMe to the device...";
     FollowMe::Config default_config {};
 
     auto height = default_config.min_height_m;
@@ -203,15 +232,15 @@ bool FollowMeImpl::is_config_ok(const FollowMe::Config &config) const
     auto config_ok = false;
 
     if (config.min_height_m < FollowMe::Config::MIN_HEIGHT_M) {
-        LogErr() << "Err: Min height must be atleast 8.0 meters";
+        LogErr() << debug_str <<  "Err: Min height must be atleast 8.0 meters";
     } else if (config.follow_distance_m < FollowMe::Config::MIN_FOLLOW_DIST_M) {
-        LogErr() << "Err: Min Follow distance must be atleast 1.0 meter";
+        LogErr() << debug_str <<  "Err: Min Follow distance must be atleast 1.0 meter";
     } else if (config.follow_direction < FollowMe::Config::FollowDirection::FRONT_RIGHT ||
                config.follow_direction > FollowMe::Config::FollowDirection::NONE) {
-        LogErr() << "Err: Invalid Follow direction";
+        LogErr() << debug_str <<  "Err: Invalid Follow direction";
     } else if (config.responsiveness < FollowMe::Config::MIN_RESPONSIVENESS ||
                config.responsiveness > FollowMe::Config::MAX_RESPONSIVENESS) {
-        LogErr() << "Err: Responsiveness must be in range (0.0 to 1.0)";
+        LogErr() << debug_str <<  "Err: Responsiveness must be in range (0.0 to 1.0)";
     } else { // Config is OK
         config_ok = true;
     }
@@ -223,8 +252,9 @@ void FollowMeImpl::receive_param_min_height(bool success, float min_height_m)
 {
     if (success) {
         _config.min_height_m = min_height_m;
+        _config_change_requested &= ~(ConfigParameter::MIN_HEIGHT);
     } else {
-        LogErr() << "Failed to set NAV_MIN_FT_HT: " << min_height_m << "m";
+        LogErr() << debug_str <<  "Failed to set NAV_MIN_FT_HT: " << min_height_m << "m";
     }
 }
 
@@ -232,8 +262,9 @@ void FollowMeImpl::receive_param_follow_distance(bool success, float follow_dist
 {
     if (success) {
         _config.follow_distance_m = follow_distance_m;
+        _config_change_requested &= ~(ConfigParameter::DISTANCE);
     } else {
-        LogErr() << "Failed to set NAV_FT_DST: " << follow_distance_m << "m";
+        LogErr() << debug_str <<  "Failed to set NAV_FT_DST: " << follow_distance_m << "m";
     }
 }
 
@@ -250,9 +281,10 @@ void FollowMeImpl::receive_param_follow_direction(bool success, int32_t directio
     if (success) {
         if (new_direction != FollowMe::Config::FollowDirection::NONE) {
             _config.follow_direction = new_direction;
+            _config_change_requested &= ~(ConfigParameter::FOLLOW_DIRECTION);
         }
     } else {
-        LogErr() << "Failed to set NAV_FT_FS: " <<  FollowMe::Config::to_str(new_direction);
+        LogErr() << debug_str <<  "Failed to set NAV_FT_FS: " <<  FollowMe::Config::to_str(new_direction);
     }
 }
 
@@ -260,8 +292,9 @@ void FollowMeImpl::receive_param_responsiveness(bool success, float responsivene
 {
     if (success) {
         _config.responsiveness = responsiveness;
+        _config_change_requested &= ~(ConfigParameter::RESPONSIVENESS);
     } else {
-        LogErr() << "Failed to set NAV_FT_RS: " << responsiveness;
+        LogErr() << debug_str <<  "Failed to set NAV_FT_RS: " << responsiveness;
     }
 }
 
@@ -305,7 +338,7 @@ void FollowMeImpl::send_target_location()
     uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds
 
     _mutex.lock();
-//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg <<
+//   LogDebug() << debug_str <<  "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg <<
 //	" Alt: " << _target_location.absolute_altitude_m;
     const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
     const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
@@ -336,7 +369,7 @@ void FollowMeImpl::send_target_location()
                                    custom_state);
 
     if (!_parent->send_message(msg)) {
-        LogErr() << "send_target_location() failed..";
+        LogErr() << debug_str <<  "send_target_location() failed..";
     } else {
         std::lock_guard<std::mutex> lock(_mutex);
         _last_location = _target_location;
diff --git a/plugins/follow_me/follow_me_impl.h b/plugins/follow_me/follow_me_impl.h
index 519ee586be..3b022524fc 100644
--- a/plugins/follow_me/follow_me_impl.h
+++ b/plugins/follow_me/follow_me_impl.h
@@ -6,6 +6,7 @@
 #include "device_impl.h"
 #include "timeout_handler.h"
 #include "global_include.h"
+#include "log.h"
 
 namespace dronecore {
 
@@ -33,8 +34,10 @@ class FollowMeImpl : public PluginImplBase
     FollowMe::Result stop();
 
 private:
+    typedef unsigned int config_val_t;
     void process_heartbeat(const mavlink_message_t &message);
 
+    enum class ConfigParameter;
     // Config methods
     void set_default_config();
     bool is_config_ok(const FollowMe::Config &config) const;
@@ -58,6 +61,35 @@ class FollowMeImpl : public PluginImplBase
         ACTIVE
     } _mode = Mode::NOT_ACTIVE;
 
+    enum class ConfigParameter {
+        NONE = 0,
+        FOLLOW_DIRECTION = 1 << 0,
+        MIN_HEIGHT = 1 << 1,
+        DISTANCE = 1 << 2,
+        RESPONSIVENESS = 1 << 3
+    };
+
+    friend config_val_t operator ~(ConfigParameter cfgp)
+    {
+        return ~static_cast<config_val_t>(cfgp);
+    }
+    friend config_val_t operator |(config_val_t config_val, ConfigParameter cfgp)
+    {
+        return (config_val) | static_cast<config_val_t>(cfgp);
+    }
+    friend config_val_t operator |=(config_val_t &config_val, ConfigParameter cfgp)
+    {
+        return config_val = config_val | static_cast<config_val_t>(cfgp);
+    }
+    friend bool operator !=(config_val_t config_val, ConfigParameter cfgp)
+    {
+        return config_val != static_cast<config_val_t>(cfgp);
+    }
+    friend bool operator ==(config_val_t config_val, ConfigParameter cfgp)
+    {
+        return config_val == static_cast<config_val_t>(cfgp);
+    }
+
     mutable std::mutex _mutex {};
     FollowMe::TargetLocation _target_location; // sent to vehicle
     FollowMe::TargetLocation _last_location; // sent to vehicle
@@ -66,8 +98,11 @@ class FollowMeImpl : public PluginImplBase
     Time _time {};
     uint8_t _estimatation_capabilities = 0; // sent to vehicle
     FollowMe::Config _config {}; // has FollowMe configuration settings
+    config_val_t _config_change_requested = 0;
 
     const float SENDER_RATE = 1.0f; // send location updates once in a second
+
+    std::string debug_str = "FollowMe: ";
 };
 
 } // namespace dronecore

From 88c7f9349cb90246ecb839d84a1a97c929aa40de Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Thu, 11 Jan 2018 12:06:10 +0530
Subject: [PATCH 15/16] Add example description and correct note

---
 example/follow_me/CMakeLists.txt           | 1 -
 example/follow_me/fake_location_provider.h | 2 +-
 example/follow_me/follow_me.cpp            | 3 +++
 3 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/example/follow_me/CMakeLists.txt b/example/follow_me/CMakeLists.txt
index 4598137241..1f348007cb 100644
--- a/example/follow_me/CMakeLists.txt
+++ b/example/follow_me/CMakeLists.txt
@@ -5,7 +5,6 @@ project(follow_me)
 find_package(Boost 1.66 COMPONENTS REQUIRED system)
 include_directories(${Boost_INCLUDE_DIR})
 
-add_definitions("-std=c++11 -g -Wall -Wextra -Werror")
 if(NOT MSVC)
     add_definitions("-std=c++11 -Wall -Wextra -Werror")
 else()
diff --git a/example/follow_me/fake_location_provider.h b/example/follow_me/fake_location_provider.h
index 47aef133bd..8d17b749d1 100644
--- a/example/follow_me/fake_location_provider.h
+++ b/example/follow_me/fake_location_provider.h
@@ -6,7 +6,7 @@
   ********************************************************************************************
   Important note: Boost isn't a dependency for DroneCore library.
   We're using Boost::Asio in this example ONLY to simulate asynchronous Fake location provider.
-  Applications on platforms Android, Windows, Apple, etc should make use their platform-specific
+  Applications on platforms Android, Windows, Apple, etc should make use of their platform-specific
   Location Provider in place of FakeLocationProvider.
   ********************************************************************************************
   ********************************************************************************************
diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
index 11da6e25fc..1037ad17b2 100644
--- a/example/follow_me/follow_me.cpp
+++ b/example/follow_me/follow_me.cpp
@@ -1,6 +1,9 @@
 /**
 * @file follow_me.cpp
 * @brief Example that demonstrates the usage of Follow Me plugin.
+* The example registers with FakeLocationProvider for location updates
+* and sends them to the Follow Me plugin which are sent to drone. You can observe
+* drone following you. We print last location of the drone in flight mode callback.
 * @author Shakthi Prashanth <shakthi.prashanth.m@intel.com>
 * @date 2018-01-03
 */

From bf1dc3861af4c45bf8463925baeccee4432881f9 Mon Sep 17 00:00:00 2001
From: Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
Date: Thu, 11 Jan 2018 12:09:42 +0530
Subject: [PATCH 16/16] Add spacing in the FollowMe example

---
 example/follow_me/follow_me.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
index 1037ad17b2..497d52feac 100644
--- a/example/follow_me/follow_me.cpp
+++ b/example/follow_me/follow_me.cpp
@@ -1,9 +1,11 @@
 /**
 * @file follow_me.cpp
+*
 * @brief Example that demonstrates the usage of Follow Me plugin.
 * The example registers with FakeLocationProvider for location updates
 * and sends them to the Follow Me plugin which are sent to drone. You can observe
 * drone following you. We print last location of the drone in flight mode callback.
+*
 * @author Shakthi Prashanth <shakthi.prashanth.m@intel.com>
 * @date 2018-01-03
 */