Fix plugin issues and improve example

1. This commit address DroneCore issue mavlink#221.
2. Corresponding changes (from mavlink#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

@@ -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);
     }
 }

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);
 

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;
 }
 

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);

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,17 +252,19 @@ 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";
     }
 }
 
 void FollowMeImpl::receive_param_follow_distance(bool success, float follow_distance_m)
 {
     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,18 +281,20 @@ 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);
     }
 }
 
 void FollowMeImpl::receive_param_responsiveness(bool success, float responsiveness)
 {
     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;

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