diff --git a/example/follow_me/fake_location_provider.cpp b/example/follow_me/fake_location_provider.cpp index 83050f2e35..02c9914920 100644 --- 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 index 04e273ed69..718c1343cd 100644 --- 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 index 8592d6eee3..bfbee47f5c 100644 --- 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 index 5034d47f5e..aeba2b81b9 100644 --- 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(this)); + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_HEARTBEAT, + std::bind(&FollowMeImpl::process_heartbeat, this, std::placeholders::_1), + static_cast(this)); } void FollowMeImpl::deinit()