Merge pull request #566 from Dronecode/fix-follow-me

Some follow-me fixes
mavlink · Oct 1, 2018 · c74692d · c74692d
2 parents 704ab33 + 3211597
commit c74692d
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 26 deletions.
diff --git a/example/follow_me/fake_location_provider.cpp b/example/follow_me/fake_location_provider.cpp
@@ -37,39 +37,30 @@ void FakeLocationProvider::stop()
     }
 }
 
-// Rudimentary location provider whose successive lat, lon combination
-// makes Drone revolve in a semi-circular path.
+// Rudimentary location provider to draw a square.
 void FakeLocationProvider::compute_locations()
 {
     while (!should_exit_) {
-        if (count_++ < 10) {
+        if (count_ < 10) {
             location_callback_(latitude_deg_, longitude_deg_);
             latitude_deg_ -= LATITUDE_DEG_PER_METER * 4;
-        }
-        if (count_++ < 20) {
+        } else if (count_ < 20) {
             location_callback_(latitude_deg_, longitude_deg_);
             longitude_deg_ += LONGITUDE_DEG_PER_METER * 4;
-        }
-        if (count_++ < 30) {
+        } else if (count_ < 30) {
             location_callback_(latitude_deg_, longitude_deg_);
             latitude_deg_ += LATITUDE_DEG_PER_METER * 4;
-        }
-        if (count_++ < 40) {
+        } else if (count_ < 40) {
             location_callback_(latitude_deg_, longitude_deg_);
             longitude_deg_ -= LONGITUDE_DEG_PER_METER * 4;
-        }
-        if (count_++ < 50) {
-            location_callback_(latitude_deg_, longitude_deg_);
-            latitude_deg_ -= LATITUDE_DEG_PER_METER * 3;
-        }
-        if (count_++ < MAX_LOCATIONS) {
-            location_callback_(latitude_deg_, longitude_deg_);
-            longitude_deg_ += LONGITUDE_DEG_PER_METER * 3;
+        } else {
+            // We're done.
+            should_exit_ = true;
         }
         sleep_for(seconds(1));
+        ++count_;
     }
 }
 
-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
@@ -17,6 +17,7 @@ class FakeLocationProvider {
     ~FakeLocationProvider();
 
     void request_location_updates(location_callback_t callback);
+    bool is_running() { return !should_exit_; };
 
 private:
     void start();

diff --git a/example/follow_me/follow_me.cpp b/example/follow_me/follow_me.cpp
@@ -108,8 +108,8 @@ int main(int argc, char **argv)
     // 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;
+    config.min_height_m = 10.0;
+    config.follow_direction = FollowMe::Config::FollowDirection::BEHIND;
     FollowMe::Result follow_me_result = follow_me->set_config(config);
 
     // Start Follow Me
@@ -119,10 +119,14 @@ int main(int argc, char **argv)
     FakeLocationProvider location_provider;
     // Register for platform-specific Location provider. We're using FakeLocationProvider for the
     // example.
-    location_provider.request_location_updates([&system, &follow_me](double lat, double lon) {
+    location_provider.request_location_updates([&follow_me](double lat, double lon) {
         follow_me->set_target_location({lat, lon, 0.0, 0.f, 0.f, 0.f});
     });
 
+    while (location_provider.is_running()) {
+        sleep_for(seconds(1));
+    }
+
     // Stop Follow Me
     follow_me_result = follow_me->stop();
     follow_me_error_exit(follow_me_result, "Failed to stop FollowMe mode");

diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp
@@ -5,8 +5,6 @@
 
 namespace dronecode_sdk {
 
-using namespace std::placeholders; // for `_1`
-
 FollowMeImpl::FollowMeImpl(System &system) : PluginImplBase(system)
 {
     // (Lat, Lon, Alt) => double, (vx, vy, vz) => float
@@ -22,9 +20,10 @@ FollowMeImpl::~FollowMeImpl()
 
 void FollowMeImpl::init()
 {
-    _parent->register_mavlink_message_handler(MAVLINK_MSG_ID_HEARTBEAT,
-                                              std::bind(&FollowMeImpl::process_heartbeat, this, _1),
-                                              static_cast<void *>(this));
+    _parent->register_mavlink_message_handler(
+        MAVLINK_MSG_ID_HEARTBEAT,
+        std::bind(&FollowMeImpl::process_heartbeat, this, std::placeholders::_1),
+        static_cast<void *>(this));
 }
 
 void FollowMeImpl::deinit()