diff --git a/examples/follow_me/follow_me.cpp b/examples/follow_me/follow_me.cpp index 0329873141..b350a4e18e 100644 --- a/examples/follow_me/follow_me.cpp +++ b/examples/follow_me/follow_me.cpp @@ -128,11 +128,11 @@ int main(int argc, char** argv) std::cout << "In Air...\n"; 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". + // Configure Follow height of the drone to be "20 meters" + // And Follow direction as "Behind" (180 degrees) FollowMe::Config config; - config.min_height_m = 10.0; - config.follow_direction = FollowMe::Config::FollowDirection::Behind; + config.follow_height_m = 10.0; + config.follow_angle_deg = 180.0f; FollowMe::Result follow_me_result = follow_me.set_config(config); if (follow_me_result != FollowMe::Result::Success) { diff --git a/proto b/proto index 7d8baab0d6..abec623802 160000 --- a/proto +++ b/proto @@ -1 +1 @@ -Subproject commit 7d8baab0d62637b0d752d22c30a708ad2295aea3 +Subproject commit abec6238027996ae388afd8854804ded41fe6213 diff --git a/src/integration_tests/follow_me.cpp b/src/integration_tests/follow_me.cpp index 7087ca0db1..dc75f1892d 100644 --- a/src/integration_tests/follow_me.cpp +++ b/src/integration_tests/follow_me.cpp @@ -3,22 +3,33 @@ #include #include #include + #include "integration_test_helper.h" + #include "mavsdk.h" #include "plugins/telemetry/telemetry.h" #include "plugins/action/action.h" #include "plugins/follow_me/follow_me.h" +#include "plugins/info/info.h" using namespace mavsdk; using namespace std::chrono; using namespace std::this_thread; +/* Updated FollowMe only works from PX4 v1.13, as it went through refactoring */ +const int32_t PX4_SW_MAJOR_MINIMUM = 1; +const int32_t PX4_SW_MINOR_MINIMUM = 13; +bool autopilot_sw_ver_minimum_satisfied(const std::shared_ptr info); + +/* Auxilary Functions */ void print(const FollowMe::Config& config); + void send_location_updates( std::shared_ptr follow_me, size_t count = 25ul, float rate = 2.f); const size_t N_LOCATIONS = 100ul; +/* Test FollowMe with a stationary target at one location */ TEST_F(SitlTest, PX4FollowMeOneLocation) { Mavsdk mavsdk; @@ -34,6 +45,11 @@ TEST_F(SitlTest, PX4FollowMeOneLocation) auto telemetry = std::make_shared(system); auto follow_me = std::make_shared(system); auto action = std::make_shared(system); + auto info = std::make_shared(system); + + if (!autopilot_sw_ver_minimum_satisfied(info)) { + GTEST_SKIP(); + } LogInfo() << "Waiting for system to be ready"; ASSERT_TRUE(poll_condition_with_timeout( @@ -98,6 +114,7 @@ TEST_F(SitlTest, PX4FollowMeOneLocation) telemetry->subscribe_flight_mode(nullptr); } +/* Test FollowMe with a dynamically moving target */ TEST_F(SitlTest, PX4FollowMeMultiLocationWithConfig) { Mavsdk mavsdk; @@ -124,6 +141,11 @@ TEST_F(SitlTest, PX4FollowMeMultiLocationWithConfig) auto telemetry = std::make_shared(system); auto follow_me = std::make_shared(system); auto action = std::make_shared(system); + auto info = std::make_shared(system); + + if (!autopilot_sw_ver_minimum_satisfied(info)) { + GTEST_SKIP(); + } LogInfo() << "Waiting for system to be ready"; ASSERT_TRUE(poll_condition_with_timeout( @@ -151,11 +173,10 @@ TEST_F(SitlTest, PX4FollowMeMultiLocationWithConfig) // configure follow me behaviour FollowMe::Config config; - config.min_height_m = 12.f; // increase min height + config.follow_height_m = 12.f; // increase min height config.follow_distance_m = 20.f; // set distance b/w system and target during FollowMe mode - config.responsiveness = 0.2f; // set to higher responsiveness - config.follow_direction = - FollowMe::Config::FollowDirection::Front; // System follows target from FRONT side + config.responsiveness = 0.2f; // Make it less responsive (higher value for the setting) + config.follow_angle_deg = 0.0; // System follows target from FRONT side // Apply configuration FollowMe::Result config_result = follow_me->set_config(config); @@ -190,10 +211,10 @@ void print(const FollowMe::Config& config) { std::cout << "Current FollowMe configuration of the system" << '\n'; std::cout << "---------------------------" << '\n'; - std::cout << "Min Height: " << config.min_height_m << "m" << '\n'; + std::cout << "Height: " << config.follow_height_m << "m" << '\n'; std::cout << "Distance: " << config.follow_distance_m << "m" << '\n'; + std::cout << "Following angle: " << config.follow_angle_deg << "[deg]" << '\n'; std::cout << "Responsiveness: " << config.responsiveness << '\n'; - std::cout << "Following from: " << config.follow_direction << '\n'; std::cout << "---------------------------" << '\n'; } @@ -209,6 +230,29 @@ FollowMe::TargetLocation create_target_location(double latitude_deg, double long return location; } +bool autopilot_sw_ver_minimum_satisfied(const std::shared_ptr info) +{ + EXPECT_TRUE(poll_condition_with_timeout( + [&]() { return info->get_version().first == Info::Result::Success; }, + std::chrono::seconds(5))); + + // Check PX4 version running and if too low, skip the test + std::pair version_result = info->get_version(); + EXPECT_EQ(version_result.first, Info::Result::Success); + + if (version_result.second.flight_sw_major < PX4_SW_MAJOR_MINIMUM) { + return false; // Major version not satisfied + + } else if (version_result.second.flight_sw_major > PX4_SW_MAJOR_MINIMUM) { + return true; // Major version satisfied + + } else if (version_result.second.flight_sw_minor >= PX4_SW_MINOR_MINIMUM) { + return true; // Major version same, minor version satisfied + } + + return false; +} + void send_location_updates(std::shared_ptr follow_me, size_t count, float rate) { // TODO: Generate these co-ordinates from an algorithm diff --git a/src/mavsdk/plugins/follow_me/follow_me.cpp b/src/mavsdk/plugins/follow_me/follow_me.cpp index 73f8dc4c07..69a3dd5911 100644 --- a/src/mavsdk/plugins/follow_me/follow_me.cpp +++ b/src/mavsdk/plugins/follow_me/follow_me.cpp @@ -57,42 +57,44 @@ FollowMe::Result FollowMe::stop() const } std::ostream& -operator<<(std::ostream& str, FollowMe::Config::FollowDirection const& follow_direction) +operator<<(std::ostream& str, FollowMe::Config::FollowAltitudeMode const& follow_altitude_mode) { - switch (follow_direction) { - case FollowMe::Config::FollowDirection::None: - return str << "None"; - case FollowMe::Config::FollowDirection::Behind: - return str << "Behind"; - case FollowMe::Config::FollowDirection::Front: - return str << "Front"; - case FollowMe::Config::FollowDirection::FrontRight: - return str << "Front Right"; - case FollowMe::Config::FollowDirection::FrontLeft: - return str << "Front Left"; + switch (follow_altitude_mode) { + case FollowMe::Config::FollowAltitudeMode::Constant: + return str << "Constant"; + case FollowMe::Config::FollowAltitudeMode::Terrain: + return str << "Terrain"; + case FollowMe::Config::FollowAltitudeMode::TargetGps: + return str << "Target Gps"; default: return str << "Unknown"; } } bool operator==(const FollowMe::Config& lhs, const FollowMe::Config& rhs) { - return ((std::isnan(rhs.min_height_m) && std::isnan(lhs.min_height_m)) || - rhs.min_height_m == lhs.min_height_m) && + return ((std::isnan(rhs.follow_height_m) && std::isnan(lhs.follow_height_m)) || + rhs.follow_height_m == lhs.follow_height_m) && ((std::isnan(rhs.follow_distance_m) && std::isnan(lhs.follow_distance_m)) || rhs.follow_distance_m == lhs.follow_distance_m) && - (rhs.follow_direction == lhs.follow_direction) && ((std::isnan(rhs.responsiveness) && std::isnan(lhs.responsiveness)) || - rhs.responsiveness == lhs.responsiveness); + rhs.responsiveness == lhs.responsiveness) && + (rhs.altitude_mode == lhs.altitude_mode) && + ((std::isnan(rhs.max_tangential_vel_m_s) && std::isnan(lhs.max_tangential_vel_m_s)) || + rhs.max_tangential_vel_m_s == lhs.max_tangential_vel_m_s) && + ((std::isnan(rhs.follow_angle_deg) && std::isnan(lhs.follow_angle_deg)) || + rhs.follow_angle_deg == lhs.follow_angle_deg); } std::ostream& operator<<(std::ostream& str, FollowMe::Config const& config) { str << std::setprecision(15); str << "config:" << '\n' << "{\n"; - str << " min_height_m: " << config.min_height_m << '\n'; + str << " follow_height_m: " << config.follow_height_m << '\n'; str << " follow_distance_m: " << config.follow_distance_m << '\n'; - str << " follow_direction: " << config.follow_direction << '\n'; str << " responsiveness: " << config.responsiveness << '\n'; + str << " altitude_mode: " << config.altitude_mode << '\n'; + str << " max_tangential_vel_m_s: " << config.max_tangential_vel_m_s << '\n'; + str << " follow_angle_deg: " << config.follow_angle_deg << '\n'; str << '}'; return str; } diff --git a/src/mavsdk/plugins/follow_me/follow_me_impl.cpp b/src/mavsdk/plugins/follow_me/follow_me_impl.cpp index 275acf5c27..653252333a 100644 --- a/src/mavsdk/plugins/follow_me/follow_me_impl.cpp +++ b/src/mavsdk/plugins/follow_me/follow_me_impl.cpp @@ -43,37 +43,53 @@ void FollowMeImpl::deinit() void FollowMeImpl::enable() { _parent->get_param_float_async( - "NAV_MIN_FT_HT", + "FLW_TGT_HT", [this](MAVLinkParameters::Result result, float value) { if (result == MAVLinkParameters::Result::Success) { - _config.min_height_m = value; + _config.follow_height_m = value; } }, this); _parent->get_param_float_async( - "NAV_FT_DST", + "FLW_TGT_DST", [this](MAVLinkParameters::Result result, float value) { if (result == MAVLinkParameters::Result::Success) { _config.follow_distance_m = value; } }, this); - _parent->get_param_int_async( - "NAV_FT_FS", - [this](MAVLinkParameters::Result result, int32_t value) { + _parent->get_param_float_async( + "FLW_TGT_FA", + [this](MAVLinkParameters::Result result, float value) { if (result == MAVLinkParameters::Result::Success) { - _config.follow_direction = static_cast(value); + _config.follow_angle_deg = value; } }, this); _parent->get_param_float_async( - "NAV_FT_RS", + "FLW_TGT_RS", [this](MAVLinkParameters::Result result, float value) { if (result == MAVLinkParameters::Result::Success) { _config.responsiveness = value; } }, this); + _parent->get_param_int_async( + "FLW_TGT_ALT_M", + [this](MAVLinkParameters::Result result, int value) { + if (result == MAVLinkParameters::Result::Success) { + _config.altitude_mode = static_cast(value); + } + }, + this); + _parent->get_param_float_async( + "FLW_TGT_MAX_VEL", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::Success) { + _config.max_tangential_vel_m_s = value; + } + }, + this); } void FollowMeImpl::disable() @@ -94,42 +110,45 @@ FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config& config) return FollowMe::Result::SetConfigFailed; } - auto height = config.min_height_m; + auto height = config.follow_height_m; auto distance = config.follow_distance_m; - int32_t direction = static_cast(config.follow_direction); auto responsiveness = config.responsiveness; + auto altitude_mode = config.altitude_mode; + auto max_tangential_vel_m_s = config.max_tangential_vel_m_s; LogDebug() << "Waiting for the system confirmation of the new configuration.."; bool success = true; // Send configuration to Vehicle - if (_config.min_height_m != height) { - if (_parent->set_param_float("NAV_MIN_FT_HT", height) == - MAVLinkParameters::Result::Success) { - _config.min_height_m = height; + if (_config.follow_height_m != height) { + if (_parent->set_param_float("FLW_TGT_HT", height) == MAVLinkParameters::Result::Success) { + _config.follow_height_m = height; } else { success = false; } } + if (_config.follow_distance_m != distance) { - if (_parent->set_param_float("NAV_FT_DST", distance) == + if (_parent->set_param_float("FLW_TGT_DST", distance) == MAVLinkParameters::Result::Success) { _config.follow_distance_m = distance; } else { success = false; } } - if (_config.follow_direction != config.follow_direction) { - if (_parent->set_param_int("NAV_FT_FS", direction) == MAVLinkParameters::Result::Success) { - _config.follow_direction = static_cast(direction); + if (_config.follow_angle_deg != config.follow_angle_deg) { + if (_parent->set_param_float("FLW_TGT_FA", config.follow_angle_deg) == + MAVLinkParameters::Result::Success) { + _config.follow_angle_deg = config.follow_angle_deg; } else { success = false; } } + if (_config.responsiveness != responsiveness) { - if (_parent->set_param_float("NAV_FT_RS", responsiveness) == + if (_parent->set_param_float("FLW_TGT_RS", responsiveness) == MAVLinkParameters::Result::Success) { _config.responsiveness = responsiveness; } else { @@ -137,6 +156,24 @@ FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config& config) } } + if (_config.altitude_mode != altitude_mode) { + if (_parent->set_param_int("FLW_TGT_ALT_M", static_cast(altitude_mode)) == + MAVLinkParameters::Result::Success) { + _config.altitude_mode = altitude_mode; + } else { + success = false; + } + } + + if (_config.max_tangential_vel_m_s != max_tangential_vel_m_s) { + if (_parent->set_param_float("FLW_TGT_MAX_VEL", max_tangential_vel_m_s) == + MAVLinkParameters::Result::Success) { + _config.max_tangential_vel_m_s = max_tangential_vel_m_s; + } else { + success = false; + } + } + return (success ? FollowMe::Result::Success : FollowMe::Result::SetConfigFailed); } @@ -215,18 +252,22 @@ bool FollowMeImpl::is_config_ok(const FollowMe::Config& config) const { auto config_ok = false; - if (config.min_height_m < CONFIG_MIN_HEIGHT_M) { - LogErr() << debug_str << "Err: Min height must be atleast 8.0 meters"; + if (config.follow_height_m < CONFIG_MIN_HEIGHT_M) { + LogErr() << debug_str << "Err: Min height must be at least " << CONFIG_MIN_HEIGHT_M + << " meters"; } else if (config.follow_distance_m < CONFIG_MIN_FOLLOW_DIST_M) { - LogErr() << debug_str << "Err: Min Follow distance must be atleast 1.0 meter"; - } else if ( - config.follow_direction < FollowMe::Config::FollowDirection::None || - config.follow_direction > FollowMe::Config::FollowDirection::FrontLeft) { - LogErr() << debug_str << "Err: Invalid Follow direction"; + LogErr() << debug_str << "Err: Min Follow distance must be at least " + << CONFIG_MIN_FOLLOW_DIST_M << " meters"; } else if ( config.responsiveness < CONFIG_MIN_RESPONSIVENESS || config.responsiveness > CONFIG_MAX_RESPONSIVENESS) { - LogErr() << debug_str << "Err: Responsiveness must be in range (0.0 to 1.0)"; + LogErr() << debug_str << "Err: Responsiveness must be in range (" + << CONFIG_MIN_RESPONSIVENESS << " to " << CONFIG_MAX_RESPONSIVENESS << ")"; + } else if ( + config.follow_angle_deg < CONFIG_MIN_FOLLOW_ANGLE || + config.follow_angle_deg > CONFIG_MAX_FOLLOW_ANGLE) { + LogErr() << debug_str << "Err: Follow Angle must be in range " << CONFIG_MIN_FOLLOW_ANGLE + << " to " << CONFIG_MAX_FOLLOW_ANGLE << " degrees!"; } else { // Config is OK config_ok = true; } diff --git a/src/mavsdk/plugins/follow_me/follow_me_impl.h b/src/mavsdk/plugins/follow_me/follow_me_impl.h index 87239bd906..1ab3f7357a 100644 --- a/src/mavsdk/plugins/follow_me/follow_me_impl.h +++ b/src/mavsdk/plugins/follow_me/follow_me_impl.h @@ -40,8 +40,10 @@ class FollowMeImpl : public PluginImplBase { void process_heartbeat(const mavlink_message_t& message); enum class ConfigParameter; + // Config methods bool is_config_ok(const FollowMe::Config& config) const; + FollowMe::Result to_follow_me_result(MavlinkCommandSender::Result result) const; bool is_target_location_set() const; @@ -61,10 +63,24 @@ class FollowMeImpl : public PluginImplBase { }; constexpr static const float CONFIG_MIN_HEIGHT_M = 8.0f; - constexpr static const float CONFIG_MIN_FOLLOW_DIST_M = 1.0f; + constexpr static const float CONFIG_MIN_FOLLOW_DIST_M = 2.0f; + constexpr static const float CONFIG_MIN_RESPONSIVENESS = 0.f; constexpr static const float CONFIG_MAX_RESPONSIVENESS = 1.0f; + constexpr static const float CONFIG_MIN_FOLLOW_ANGLE = -180.0f; + constexpr static const float CONFIG_MAX_FOLLOW_ANGLE = 180.0f; + + /* + * NOTE : This MUST be up to date with the upstream PX4's enum values!! */ + + // Follow Altitude modes set by the parameter FLW_TGT_ALT_M + enum kFollowAltitudeMode { + kFollowAltitudeModeConstant, + kFollowAltitudeModeTerrain, + kFollowAltitudeModeTrackTarget + }; + friend config_val_t operator~(ConfigParameter cfgp) { return ~static_cast(cfgp); } friend config_val_t operator|(config_val_t config_val, ConfigParameter cfgp) { diff --git a/src/mavsdk/plugins/follow_me/include/plugins/follow_me/follow_me.h b/src/mavsdk/plugins/follow_me/include/plugins/follow_me/follow_me.h index bf0aec5d57..848c3e94cb 100644 --- a/src/mavsdk/plugins/follow_me/include/plugins/follow_me/follow_me.h +++ b/src/mavsdk/plugins/follow_me/include/plugins/follow_me/follow_me.h @@ -62,31 +62,40 @@ class FollowMe : public PluginBase { */ struct Config { /** - * @brief Direction relative to the target that the vehicle should follow + * @brief Altitude mode to configure which altitude the follow me will assume the target to + * be at. */ - enum class FollowDirection { - None, /**< @brief Do not follow. */ - Behind, /**< @brief Follow from behind. */ - Front, /**< @brief Follow from front. */ - FrontRight, /**< @brief Follow from front right. */ - FrontLeft, /**< @brief Follow from front left. */ + enum class FollowAltitudeMode { + Constant, /**< @brief Target assumed to be mobing at a constant altitude of home + position (where the vehicle armed). */ + Terrain, /**< @brief Target assumed to be at the terrain level sensed by the distance + sensor. */ + TargetGps, /**< @brief Target GPS altitude taken into account to do 3D tracking. */ }; /** - * @brief Stream operator to print information about a `FollowMe::FollowDirection`. + * @brief Stream operator to print information about a `FollowMe::FollowAltitudeMode`. * * @return A reference to the stream. */ - friend std::ostream& - operator<<(std::ostream& str, FollowMe::Config::FollowDirection const& follow_direction); - - float min_height_m{8.0}; /**< @brief Minimum height for the vehicle in meters (recommended - minimum 8 meters) */ - float follow_distance_m{8.0}; /**< @brief Distance from target for vehicle to follow in - meters (recommended minimum 1 meter) */ - FollowDirection follow_direction{}; /**< @brief Direction to follow in */ - float responsiveness{0.5}; /**< @brief How responsive the vehicle is to the motion of the - target (range 0.0 to 1.0) */ + friend std::ostream& operator<<( + std::ostream& str, FollowMe::Config::FollowAltitudeMode const& follow_altitude_mode); + + float follow_height_m{ + 8.0f}; /**< @brief [m] Follow height in meters (recommended minimum 8 meters) */ + float follow_distance_m{8.0f}; /**< @brief [m] Follow distance to target in meters + (recommended minimum 4 meter) */ + float responsiveness{0.1f}; /**< @brief How responsive the vehicle is to the motion of the + target, Lower value = More responsive (range 0.0 to 1.0) */ + FollowAltitudeMode altitude_mode{}; /**< @brief Follow Altitude control mode */ + float max_tangential_vel_m_s{ + 8.0f}; /**< @brief [m/s] Maximum orbit tangential velocity relative to the target, in + meters per second. Higher value = More aggressive follow angle tracking. */ + float follow_angle_deg{ + 180.0f}; /**< @brief [deg] Follow Angle relative to the target. 0 equals following in + front of the target's direction. Angle increases in Clockwise direction, so + following from right would be 90 degrees, from the left is -90 degrees, and + so on. */ }; /** diff --git a/src/mavsdk_server/src/generated/follow_me/follow_me.pb.cc b/src/mavsdk_server/src/generated/follow_me/follow_me.pb.cc index 7ebd2efce6..c56eea207b 100644 --- a/src/mavsdk_server/src/generated/follow_me/follow_me.pb.cc +++ b/src/mavsdk_server/src/generated/follow_me/follow_me.pb.cc @@ -25,11 +25,13 @@ namespace rpc { namespace follow_me { PROTOBUF_CONSTEXPR Config::Config( ::_pbi::ConstantInitialized) - : min_height_m_(0) + : follow_height_m_(0) , follow_distance_m_(0) - , follow_direction_(0) + , responsiveness_(0) + , altitude_mode_(0) - , responsiveness_(0){} + , max_tangential_vel_m_s_(0) + , follow_angle_deg_(0){} struct ConfigDefaultTypeInternal { PROTOBUF_CONSTEXPR ConfigDefaultTypeInternal() : _instance(::_pbi::ConstantInitialized{}) {} @@ -247,10 +249,12 @@ const uint32_t TableStruct_follow_5fme_2ffollow_5fme_2eproto::offsets[] PROTOBUF ~0u, // no _oneof_case_ ~0u, // no _weak_field_map_ ~0u, // no _inlined_string_donated_ - PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::Config, min_height_m_), + PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::Config, follow_height_m_), PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::Config, follow_distance_m_), - PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::Config, follow_direction_), PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::Config, responsiveness_), + PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::Config, altitude_mode_), + PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::Config, max_tangential_vel_m_s_), + PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::Config, follow_angle_deg_), ~0u, // no _has_bits_ PROTOBUF_FIELD_OFFSET(::mavsdk::rpc::follow_me::TargetLocation, _internal_metadata_), ~0u, // no _extensions_ @@ -367,22 +371,22 @@ const uint32_t TableStruct_follow_5fme_2ffollow_5fme_2eproto::offsets[] PROTOBUF }; static const ::_pbi::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = { { 0, -1, -1, sizeof(::mavsdk::rpc::follow_me::Config)}, - { 10, -1, -1, sizeof(::mavsdk::rpc::follow_me::TargetLocation)}, - { 22, -1, -1, sizeof(::mavsdk::rpc::follow_me::GetConfigRequest)}, - { 28, -1, -1, sizeof(::mavsdk::rpc::follow_me::GetConfigResponse)}, - { 35, -1, -1, sizeof(::mavsdk::rpc::follow_me::SetConfigRequest)}, - { 42, -1, -1, sizeof(::mavsdk::rpc::follow_me::SetConfigResponse)}, - { 49, -1, -1, sizeof(::mavsdk::rpc::follow_me::IsActiveRequest)}, - { 55, -1, -1, sizeof(::mavsdk::rpc::follow_me::IsActiveResponse)}, - { 62, -1, -1, sizeof(::mavsdk::rpc::follow_me::SetTargetLocationRequest)}, - { 69, -1, -1, sizeof(::mavsdk::rpc::follow_me::SetTargetLocationResponse)}, - { 76, -1, -1, sizeof(::mavsdk::rpc::follow_me::GetLastLocationRequest)}, - { 82, -1, -1, sizeof(::mavsdk::rpc::follow_me::GetLastLocationResponse)}, - { 89, -1, -1, sizeof(::mavsdk::rpc::follow_me::StartRequest)}, - { 95, -1, -1, sizeof(::mavsdk::rpc::follow_me::StartResponse)}, - { 102, -1, -1, sizeof(::mavsdk::rpc::follow_me::StopRequest)}, - { 108, -1, -1, sizeof(::mavsdk::rpc::follow_me::StopResponse)}, - { 115, -1, -1, sizeof(::mavsdk::rpc::follow_me::FollowMeResult)}, + { 12, -1, -1, sizeof(::mavsdk::rpc::follow_me::TargetLocation)}, + { 24, -1, -1, sizeof(::mavsdk::rpc::follow_me::GetConfigRequest)}, + { 30, -1, -1, sizeof(::mavsdk::rpc::follow_me::GetConfigResponse)}, + { 37, -1, -1, sizeof(::mavsdk::rpc::follow_me::SetConfigRequest)}, + { 44, -1, -1, sizeof(::mavsdk::rpc::follow_me::SetConfigResponse)}, + { 51, -1, -1, sizeof(::mavsdk::rpc::follow_me::IsActiveRequest)}, + { 57, -1, -1, sizeof(::mavsdk::rpc::follow_me::IsActiveResponse)}, + { 64, -1, -1, sizeof(::mavsdk::rpc::follow_me::SetTargetLocationRequest)}, + { 71, -1, -1, sizeof(::mavsdk::rpc::follow_me::SetTargetLocationResponse)}, + { 78, -1, -1, sizeof(::mavsdk::rpc::follow_me::GetLastLocationRequest)}, + { 84, -1, -1, sizeof(::mavsdk::rpc::follow_me::GetLastLocationResponse)}, + { 91, -1, -1, sizeof(::mavsdk::rpc::follow_me::StartRequest)}, + { 97, -1, -1, sizeof(::mavsdk::rpc::follow_me::StartResponse)}, + { 104, -1, -1, sizeof(::mavsdk::rpc::follow_me::StopRequest)}, + { 110, -1, -1, sizeof(::mavsdk::rpc::follow_me::StopResponse)}, + { 117, -1, -1, sizeof(::mavsdk::rpc::follow_me::FollowMeResult)}, }; static const ::_pb::Message* const file_default_instances[] = { @@ -407,75 +411,76 @@ static const ::_pb::Message* const file_default_instances[] = { const char descriptor_table_protodef_follow_5fme_2ffollow_5fme_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = "\n\031follow_me/follow_me.proto\022\024mavsdk.rpc." - "follow_me\032\024mavsdk_options.proto\"\337\002\n\006Conf" - "ig\022\035\n\014min_height_m\030\001 \001(\002B\007\202\265\030\0038.0\022\"\n\021fol" - "low_distance_m\030\002 \001(\002B\007\202\265\030\0038.0\022F\n\020follow_" - "direction\030\003 \001(\0162,.mavsdk.rpc.follow_me.C" - "onfig.FollowDirection\022\037\n\016responsiveness\030" - "\004 \001(\002B\007\202\265\030\0030.5\"\250\001\n\017FollowDirection\022\031\n\025FO" - "LLOW_DIRECTION_NONE\020\000\022\033\n\027FOLLOW_DIRECTIO" - "N_BEHIND\020\001\022\032\n\026FOLLOW_DIRECTION_FRONT\020\002\022 " - "\n\034FOLLOW_DIRECTION_FRONT_RIGHT\020\003\022\037\n\033FOLL" - "OW_DIRECTION_FRONT_LEFT\020\004\"\330\001\n\016TargetLoca" - "tion\022\035\n\014latitude_deg\030\001 \001(\001B\007\202\265\030\003NaN\022\036\n\rl" - "ongitude_deg\030\002 \001(\001B\007\202\265\030\003NaN\022$\n\023absolute_" - "altitude_m\030\003 \001(\002B\007\202\265\030\003NaN\022\037\n\016velocity_x_" - "m_s\030\004 \001(\002B\007\202\265\030\003NaN\022\037\n\016velocity_y_m_s\030\005 \001" - "(\002B\007\202\265\030\003NaN\022\037\n\016velocity_z_m_s\030\006 \001(\002B\007\202\265\030" - "\003NaN\"\022\n\020GetConfigRequest\"A\n\021GetConfigRes" - "ponse\022,\n\006config\030\001 \001(\0132\034.mavsdk.rpc.follo" - "w_me.Config\"@\n\020SetConfigRequest\022,\n\006confi" - "g\030\001 \001(\0132\034.mavsdk.rpc.follow_me.Config\"S\n" - "\021SetConfigResponse\022>\n\020follow_me_result\030\001" - " \001(\0132$.mavsdk.rpc.follow_me.FollowMeResu" - "lt\"\021\n\017IsActiveRequest\"%\n\020IsActiveRespons" - "e\022\021\n\tis_active\030\001 \001(\010\"R\n\030SetTargetLocatio" - "nRequest\0226\n\010location\030\001 \001(\0132$.mavsdk.rpc." - "follow_me.TargetLocation\"[\n\031SetTargetLoc" - "ationResponse\022>\n\020follow_me_result\030\001 \001(\0132" - "$.mavsdk.rpc.follow_me.FollowMeResult\"\030\n" - "\026GetLastLocationRequest\"Q\n\027GetLastLocati" - "onResponse\0226\n\010location\030\001 \001(\0132$.mavsdk.rp" - "c.follow_me.TargetLocation\"\016\n\014StartReque" - "st\"O\n\rStartResponse\022>\n\020follow_me_result\030" - "\001 \001(\0132$.mavsdk.rpc.follow_me.FollowMeRes" - "ult\"\r\n\013StopRequest\"N\n\014StopResponse\022>\n\020fo" - "llow_me_result\030\001 \001(\0132$.mavsdk.rpc.follow" - "_me.FollowMeResult\"\274\002\n\016FollowMeResult\022;\n" - "\006result\030\001 \001(\0162+.mavsdk.rpc.follow_me.Fol" - "lowMeResult.Result\022\022\n\nresult_str\030\002 \001(\t\"\330" - "\001\n\006Result\022\022\n\016RESULT_UNKNOWN\020\000\022\022\n\016RESULT_" - "SUCCESS\020\001\022\024\n\020RESULT_NO_SYSTEM\020\002\022\033\n\027RESUL" - "T_CONNECTION_ERROR\020\003\022\017\n\013RESULT_BUSY\020\004\022\031\n" - "\025RESULT_COMMAND_DENIED\020\005\022\022\n\016RESULT_TIMEO" - "UT\020\006\022\025\n\021RESULT_NOT_ACTIVE\020\007\022\034\n\030RESULT_SE" - "T_CONFIG_FAILED\020\0102\331\005\n\017FollowMeService\022b\n" - "\tGetConfig\022&.mavsdk.rpc.follow_me.GetCon" - "figRequest\032\'.mavsdk.rpc.follow_me.GetCon" - "figResponse\"\004\200\265\030\001\022b\n\tSetConfig\022&.mavsdk." - "rpc.follow_me.SetConfigRequest\032\'.mavsdk." - "rpc.follow_me.SetConfigResponse\"\004\200\265\030\001\022_\n" - "\010IsActive\022%.mavsdk.rpc.follow_me.IsActiv" - "eRequest\032&.mavsdk.rpc.follow_me.IsActive" - "Response\"\004\200\265\030\001\022z\n\021SetTargetLocation\022..ma" - "vsdk.rpc.follow_me.SetTargetLocationRequ" - "est\032/.mavsdk.rpc.follow_me.SetTargetLoca" - "tionResponse\"\004\200\265\030\001\022t\n\017GetLastLocation\022,." - "mavsdk.rpc.follow_me.GetLastLocationRequ" - "est\032-.mavsdk.rpc.follow_me.GetLastLocati" - "onResponse\"\004\200\265\030\001\022V\n\005Start\022\".mavsdk.rpc.f" - "ollow_me.StartRequest\032#.mavsdk.rpc.follo" - "w_me.StartResponse\"\004\200\265\030\001\022S\n\004Stop\022!.mavsd" - "k.rpc.follow_me.StopRequest\032\".mavsdk.rpc" - ".follow_me.StopResponse\"\004\200\265\030\001B$\n\023io.mavs" - "dk.follow_meB\rFollowMeProtob\006proto3" + "follow_me\032\024mavsdk_options.proto\"\212\003\n\006Conf" + "ig\022!\n\017follow_height_m\030\001 \001(\002B\010\202\265\030\0048.0f\022#\n" + "\021follow_distance_m\030\002 \001(\002B\010\202\265\030\0048.0f\022 \n\016re" + "sponsiveness\030\004 \001(\002B\010\202\265\030\0040.1f\022F\n\raltitude" + "_mode\030\005 \001(\0162/.mavsdk.rpc.follow_me.Confi" + "g.FollowAltitudeMode\022(\n\026max_tangential_v" + "el_m_s\030\006 \001(\002B\010\202\265\030\0048.0f\022$\n\020follow_angle_d" + "eg\030\007 \001(\002B\n\202\265\030\006180.0f\"~\n\022FollowAltitudeMo" + "de\022!\n\035FOLLOW_ALTITUDE_MODE_CONSTANT\020\000\022 \n" + "\034FOLLOW_ALTITUDE_MODE_TERRAIN\020\001\022#\n\037FOLLO" + "W_ALTITUDE_MODE_TARGET_GPS\020\002\"\330\001\n\016TargetL" + "ocation\022\035\n\014latitude_deg\030\001 \001(\001B\007\202\265\030\003NaN\022\036" + "\n\rlongitude_deg\030\002 \001(\001B\007\202\265\030\003NaN\022$\n\023absolu" + "te_altitude_m\030\003 \001(\002B\007\202\265\030\003NaN\022\037\n\016velocity" + "_x_m_s\030\004 \001(\002B\007\202\265\030\003NaN\022\037\n\016velocity_y_m_s\030" + "\005 \001(\002B\007\202\265\030\003NaN\022\037\n\016velocity_z_m_s\030\006 \001(\002B\007" + "\202\265\030\003NaN\"\022\n\020GetConfigRequest\"A\n\021GetConfig" + "Response\022,\n\006config\030\001 \001(\0132\034.mavsdk.rpc.fo" + "llow_me.Config\"@\n\020SetConfigRequest\022,\n\006co" + "nfig\030\001 \001(\0132\034.mavsdk.rpc.follow_me.Config" + "\"S\n\021SetConfigResponse\022>\n\020follow_me_resul" + "t\030\001 \001(\0132$.mavsdk.rpc.follow_me.FollowMeR" + "esult\"\021\n\017IsActiveRequest\"%\n\020IsActiveResp" + "onse\022\021\n\tis_active\030\001 \001(\010\"R\n\030SetTargetLoca" + "tionRequest\0226\n\010location\030\001 \001(\0132$.mavsdk.r" + "pc.follow_me.TargetLocation\"[\n\031SetTarget" + "LocationResponse\022>\n\020follow_me_result\030\001 \001" + "(\0132$.mavsdk.rpc.follow_me.FollowMeResult" + "\"\030\n\026GetLastLocationRequest\"Q\n\027GetLastLoc" + "ationResponse\0226\n\010location\030\001 \001(\0132$.mavsdk" + ".rpc.follow_me.TargetLocation\"\016\n\014StartRe" + "quest\"O\n\rStartResponse\022>\n\020follow_me_resu" + "lt\030\001 \001(\0132$.mavsdk.rpc.follow_me.FollowMe" + "Result\"\r\n\013StopRequest\"N\n\014StopResponse\022>\n" + "\020follow_me_result\030\001 \001(\0132$.mavsdk.rpc.fol" + "low_me.FollowMeResult\"\274\002\n\016FollowMeResult" + "\022;\n\006result\030\001 \001(\0162+.mavsdk.rpc.follow_me." + "FollowMeResult.Result\022\022\n\nresult_str\030\002 \001(" + "\t\"\330\001\n\006Result\022\022\n\016RESULT_UNKNOWN\020\000\022\022\n\016RESU" + "LT_SUCCESS\020\001\022\024\n\020RESULT_NO_SYSTEM\020\002\022\033\n\027RE" + "SULT_CONNECTION_ERROR\020\003\022\017\n\013RESULT_BUSY\020\004" + "\022\031\n\025RESULT_COMMAND_DENIED\020\005\022\022\n\016RESULT_TI" + "MEOUT\020\006\022\025\n\021RESULT_NOT_ACTIVE\020\007\022\034\n\030RESULT" + "_SET_CONFIG_FAILED\020\0102\331\005\n\017FollowMeService" + "\022b\n\tGetConfig\022&.mavsdk.rpc.follow_me.Get" + "ConfigRequest\032\'.mavsdk.rpc.follow_me.Get" + "ConfigResponse\"\004\200\265\030\001\022b\n\tSetConfig\022&.mavs" + "dk.rpc.follow_me.SetConfigRequest\032\'.mavs" + "dk.rpc.follow_me.SetConfigResponse\"\004\200\265\030\001" + "\022_\n\010IsActive\022%.mavsdk.rpc.follow_me.IsAc" + "tiveRequest\032&.mavsdk.rpc.follow_me.IsAct" + "iveResponse\"\004\200\265\030\001\022z\n\021SetTargetLocation\022." + ".mavsdk.rpc.follow_me.SetTargetLocationR" + "equest\032/.mavsdk.rpc.follow_me.SetTargetL" + "ocationResponse\"\004\200\265\030\001\022t\n\017GetLastLocation" + "\022,.mavsdk.rpc.follow_me.GetLastLocationR" + "equest\032-.mavsdk.rpc.follow_me.GetLastLoc" + "ationResponse\"\004\200\265\030\001\022V\n\005Start\022\".mavsdk.rp" + "c.follow_me.StartRequest\032#.mavsdk.rpc.fo" + "llow_me.StartResponse\"\004\200\265\030\001\022S\n\004Stop\022!.ma" + "vsdk.rpc.follow_me.StopRequest\032\".mavsdk." + "rpc.follow_me.StopResponse\"\004\200\265\030\001B$\n\023io.m" + "avsdk.follow_meB\rFollowMeProtob\006proto3" ; static const ::_pbi::DescriptorTable* const descriptor_table_follow_5fme_2ffollow_5fme_2eproto_deps[1] = { &::descriptor_table_mavsdk_5foptions_2eproto, }; static ::_pbi::once_flag descriptor_table_follow_5fme_2ffollow_5fme_2eproto_once; const ::_pbi::DescriptorTable descriptor_table_follow_5fme_2ffollow_5fme_2eproto = { - false, false, 2515, descriptor_table_protodef_follow_5fme_2ffollow_5fme_2eproto, + false, false, 2558, descriptor_table_protodef_follow_5fme_2ffollow_5fme_2eproto, "follow_me/follow_me.proto", &descriptor_table_follow_5fme_2ffollow_5fme_2eproto_once, descriptor_table_follow_5fme_2ffollow_5fme_2eproto_deps, 1, 17, schemas, file_default_instances, TableStruct_follow_5fme_2ffollow_5fme_2eproto::offsets, @@ -491,17 +496,15 @@ PROTOBUF_ATTRIBUTE_INIT_PRIORITY2 static ::_pbi::AddDescriptorsRunner dynamic_in namespace mavsdk { namespace rpc { namespace follow_me { -const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* Config_FollowDirection_descriptor() { +const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* Config_FollowAltitudeMode_descriptor() { ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&descriptor_table_follow_5fme_2ffollow_5fme_2eproto); return file_level_enum_descriptors_follow_5fme_2ffollow_5fme_2eproto[0]; } -bool Config_FollowDirection_IsValid(int value) { +bool Config_FollowAltitudeMode_IsValid(int value) { switch (value) { case 0: case 1: case 2: - case 3: - case 4: return true; default: return false; @@ -509,14 +512,12 @@ bool Config_FollowDirection_IsValid(int value) { } #if (__cplusplus < 201703) && (!defined(_MSC_VER) || (_MSC_VER >= 1900 && _MSC_VER < 1912)) -constexpr Config_FollowDirection Config::FOLLOW_DIRECTION_NONE; -constexpr Config_FollowDirection Config::FOLLOW_DIRECTION_BEHIND; -constexpr Config_FollowDirection Config::FOLLOW_DIRECTION_FRONT; -constexpr Config_FollowDirection Config::FOLLOW_DIRECTION_FRONT_RIGHT; -constexpr Config_FollowDirection Config::FOLLOW_DIRECTION_FRONT_LEFT; -constexpr Config_FollowDirection Config::FollowDirection_MIN; -constexpr Config_FollowDirection Config::FollowDirection_MAX; -constexpr int Config::FollowDirection_ARRAYSIZE; +constexpr Config_FollowAltitudeMode Config::FOLLOW_ALTITUDE_MODE_CONSTANT; +constexpr Config_FollowAltitudeMode Config::FOLLOW_ALTITUDE_MODE_TERRAIN; +constexpr Config_FollowAltitudeMode Config::FOLLOW_ALTITUDE_MODE_TARGET_GPS; +constexpr Config_FollowAltitudeMode Config::FollowAltitudeMode_MIN; +constexpr Config_FollowAltitudeMode Config::FollowAltitudeMode_MAX; +constexpr int Config::FollowAltitudeMode_ARRAYSIZE; #endif // (__cplusplus < 201703) && (!defined(_MSC_VER) || (_MSC_VER >= 1900 && _MSC_VER < 1912)) const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* FollowMeResult_Result_descriptor() { ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&descriptor_table_follow_5fme_2ffollow_5fme_2eproto); @@ -569,17 +570,17 @@ Config::Config(::PROTOBUF_NAMESPACE_ID::Arena* arena, Config::Config(const Config& from) : ::PROTOBUF_NAMESPACE_ID::Message() { _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_); - ::memcpy(&min_height_m_, &from.min_height_m_, - static_cast(reinterpret_cast(&responsiveness_) - - reinterpret_cast(&min_height_m_)) + sizeof(responsiveness_)); + ::memcpy(&follow_height_m_, &from.follow_height_m_, + static_cast(reinterpret_cast(&follow_angle_deg_) - + reinterpret_cast(&follow_height_m_)) + sizeof(follow_angle_deg_)); // @@protoc_insertion_point(copy_constructor:mavsdk.rpc.follow_me.Config) } inline void Config::SharedCtor() { ::memset(reinterpret_cast(this) + static_cast( - reinterpret_cast(&min_height_m_) - reinterpret_cast(this)), - 0, static_cast(reinterpret_cast(&responsiveness_) - - reinterpret_cast(&min_height_m_)) + sizeof(responsiveness_)); + reinterpret_cast(&follow_height_m_) - reinterpret_cast(this)), + 0, static_cast(reinterpret_cast(&follow_angle_deg_) - + reinterpret_cast(&follow_height_m_)) + sizeof(follow_angle_deg_)); } Config::~Config() { @@ -605,9 +606,9 @@ void Config::Clear() { // Prevent compiler warnings about cached_has_bits being unused (void) cached_has_bits; - ::memset(&min_height_m_, 0, static_cast( - reinterpret_cast(&responsiveness_) - - reinterpret_cast(&min_height_m_)) + sizeof(responsiveness_)); + ::memset(&follow_height_m_, 0, static_cast( + reinterpret_cast(&follow_angle_deg_) - + reinterpret_cast(&follow_height_m_)) + sizeof(follow_angle_deg_)); _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(); } @@ -617,15 +618,15 @@ const char* Config::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx) { uint32_t tag; ptr = ::_pbi::ReadTag(ptr, &tag); switch (tag >> 3) { - // float min_height_m = 1 [(.mavsdk.options.default_value) = "8.0"]; + // float follow_height_m = 1 [(.mavsdk.options.default_value) = "8.0f"]; case 1: if (PROTOBUF_PREDICT_TRUE(static_cast(tag) == 13)) { - min_height_m_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad(ptr); + follow_height_m_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad(ptr); ptr += sizeof(float); } else goto handle_unusual; continue; - // float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0"]; + // float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0f"]; case 2: if (PROTOBUF_PREDICT_TRUE(static_cast(tag) == 21)) { follow_distance_m_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad(ptr); @@ -633,19 +634,35 @@ const char* Config::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx) { } else goto handle_unusual; continue; - // .mavsdk.rpc.follow_me.Config.FollowDirection follow_direction = 3; - case 3: - if (PROTOBUF_PREDICT_TRUE(static_cast(tag) == 24)) { + // float responsiveness = 4 [(.mavsdk.options.default_value) = "0.1f"]; + case 4: + if (PROTOBUF_PREDICT_TRUE(static_cast(tag) == 37)) { + responsiveness_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad(ptr); + ptr += sizeof(float); + } else + goto handle_unusual; + continue; + // .mavsdk.rpc.follow_me.Config.FollowAltitudeMode altitude_mode = 5; + case 5: + if (PROTOBUF_PREDICT_TRUE(static_cast(tag) == 40)) { uint64_t val = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr); CHK_(ptr); - _internal_set_follow_direction(static_cast<::mavsdk::rpc::follow_me::Config_FollowDirection>(val)); + _internal_set_altitude_mode(static_cast<::mavsdk::rpc::follow_me::Config_FollowAltitudeMode>(val)); } else goto handle_unusual; continue; - // float responsiveness = 4 [(.mavsdk.options.default_value) = "0.5"]; - case 4: - if (PROTOBUF_PREDICT_TRUE(static_cast(tag) == 37)) { - responsiveness_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad(ptr); + // float max_tangential_vel_m_s = 6 [(.mavsdk.options.default_value) = "8.0f"]; + case 6: + if (PROTOBUF_PREDICT_TRUE(static_cast(tag) == 53)) { + max_tangential_vel_m_s_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad(ptr); + ptr += sizeof(float); + } else + goto handle_unusual; + continue; + // float follow_angle_deg = 7 [(.mavsdk.options.default_value) = "180.0f"]; + case 7: + if (PROTOBUF_PREDICT_TRUE(static_cast(tag) == 61)) { + follow_angle_deg_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad(ptr); ptr += sizeof(float); } else goto handle_unusual; @@ -679,17 +696,17 @@ uint8_t* Config::_InternalSerialize( uint32_t cached_has_bits = 0; (void) cached_has_bits; - // float min_height_m = 1 [(.mavsdk.options.default_value) = "8.0"]; + // float follow_height_m = 1 [(.mavsdk.options.default_value) = "8.0f"]; static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); - float tmp_min_height_m = this->_internal_min_height_m(); - uint32_t raw_min_height_m; - memcpy(&raw_min_height_m, &tmp_min_height_m, sizeof(tmp_min_height_m)); - if (raw_min_height_m != 0) { + float tmp_follow_height_m = this->_internal_follow_height_m(); + uint32_t raw_follow_height_m; + memcpy(&raw_follow_height_m, &tmp_follow_height_m, sizeof(tmp_follow_height_m)); + if (raw_follow_height_m != 0) { target = stream->EnsureSpace(target); - target = ::_pbi::WireFormatLite::WriteFloatToArray(1, this->_internal_min_height_m(), target); + target = ::_pbi::WireFormatLite::WriteFloatToArray(1, this->_internal_follow_height_m(), target); } - // float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0"]; + // float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0f"]; static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); float tmp_follow_distance_m = this->_internal_follow_distance_m(); uint32_t raw_follow_distance_m; @@ -699,14 +716,7 @@ uint8_t* Config::_InternalSerialize( target = ::_pbi::WireFormatLite::WriteFloatToArray(2, this->_internal_follow_distance_m(), target); } - // .mavsdk.rpc.follow_me.Config.FollowDirection follow_direction = 3; - if (this->_internal_follow_direction() != 0) { - target = stream->EnsureSpace(target); - target = ::_pbi::WireFormatLite::WriteEnumToArray( - 3, this->_internal_follow_direction(), target); - } - - // float responsiveness = 4 [(.mavsdk.options.default_value) = "0.5"]; + // float responsiveness = 4 [(.mavsdk.options.default_value) = "0.1f"]; static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); float tmp_responsiveness = this->_internal_responsiveness(); uint32_t raw_responsiveness; @@ -716,6 +726,33 @@ uint8_t* Config::_InternalSerialize( target = ::_pbi::WireFormatLite::WriteFloatToArray(4, this->_internal_responsiveness(), target); } + // .mavsdk.rpc.follow_me.Config.FollowAltitudeMode altitude_mode = 5; + if (this->_internal_altitude_mode() != 0) { + target = stream->EnsureSpace(target); + target = ::_pbi::WireFormatLite::WriteEnumToArray( + 5, this->_internal_altitude_mode(), target); + } + + // float max_tangential_vel_m_s = 6 [(.mavsdk.options.default_value) = "8.0f"]; + static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); + float tmp_max_tangential_vel_m_s = this->_internal_max_tangential_vel_m_s(); + uint32_t raw_max_tangential_vel_m_s; + memcpy(&raw_max_tangential_vel_m_s, &tmp_max_tangential_vel_m_s, sizeof(tmp_max_tangential_vel_m_s)); + if (raw_max_tangential_vel_m_s != 0) { + target = stream->EnsureSpace(target); + target = ::_pbi::WireFormatLite::WriteFloatToArray(6, this->_internal_max_tangential_vel_m_s(), target); + } + + // float follow_angle_deg = 7 [(.mavsdk.options.default_value) = "180.0f"]; + static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); + float tmp_follow_angle_deg = this->_internal_follow_angle_deg(); + uint32_t raw_follow_angle_deg; + memcpy(&raw_follow_angle_deg, &tmp_follow_angle_deg, sizeof(tmp_follow_angle_deg)); + if (raw_follow_angle_deg != 0) { + target = stream->EnsureSpace(target); + target = ::_pbi::WireFormatLite::WriteFloatToArray(7, this->_internal_follow_angle_deg(), target); + } + if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) { target = ::_pbi::WireFormat::InternalSerializeUnknownFieldsToArray( _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream); @@ -732,16 +769,16 @@ size_t Config::ByteSizeLong() const { // Prevent compiler warnings about cached_has_bits being unused (void) cached_has_bits; - // float min_height_m = 1 [(.mavsdk.options.default_value) = "8.0"]; + // float follow_height_m = 1 [(.mavsdk.options.default_value) = "8.0f"]; static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); - float tmp_min_height_m = this->_internal_min_height_m(); - uint32_t raw_min_height_m; - memcpy(&raw_min_height_m, &tmp_min_height_m, sizeof(tmp_min_height_m)); - if (raw_min_height_m != 0) { + float tmp_follow_height_m = this->_internal_follow_height_m(); + uint32_t raw_follow_height_m; + memcpy(&raw_follow_height_m, &tmp_follow_height_m, sizeof(tmp_follow_height_m)); + if (raw_follow_height_m != 0) { total_size += 1 + 4; } - // float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0"]; + // float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0f"]; static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); float tmp_follow_distance_m = this->_internal_follow_distance_m(); uint32_t raw_follow_distance_m; @@ -750,13 +787,7 @@ size_t Config::ByteSizeLong() const { total_size += 1 + 4; } - // .mavsdk.rpc.follow_me.Config.FollowDirection follow_direction = 3; - if (this->_internal_follow_direction() != 0) { - total_size += 1 + - ::_pbi::WireFormatLite::EnumSize(this->_internal_follow_direction()); - } - - // float responsiveness = 4 [(.mavsdk.options.default_value) = "0.5"]; + // float responsiveness = 4 [(.mavsdk.options.default_value) = "0.1f"]; static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); float tmp_responsiveness = this->_internal_responsiveness(); uint32_t raw_responsiveness; @@ -765,6 +796,30 @@ size_t Config::ByteSizeLong() const { total_size += 1 + 4; } + // .mavsdk.rpc.follow_me.Config.FollowAltitudeMode altitude_mode = 5; + if (this->_internal_altitude_mode() != 0) { + total_size += 1 + + ::_pbi::WireFormatLite::EnumSize(this->_internal_altitude_mode()); + } + + // float max_tangential_vel_m_s = 6 [(.mavsdk.options.default_value) = "8.0f"]; + static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); + float tmp_max_tangential_vel_m_s = this->_internal_max_tangential_vel_m_s(); + uint32_t raw_max_tangential_vel_m_s; + memcpy(&raw_max_tangential_vel_m_s, &tmp_max_tangential_vel_m_s, sizeof(tmp_max_tangential_vel_m_s)); + if (raw_max_tangential_vel_m_s != 0) { + total_size += 1 + 4; + } + + // float follow_angle_deg = 7 [(.mavsdk.options.default_value) = "180.0f"]; + static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); + float tmp_follow_angle_deg = this->_internal_follow_angle_deg(); + uint32_t raw_follow_angle_deg; + memcpy(&raw_follow_angle_deg, &tmp_follow_angle_deg, sizeof(tmp_follow_angle_deg)); + if (raw_follow_angle_deg != 0) { + total_size += 1 + 4; + } + return MaybeComputeUnknownFieldsSize(total_size, &_cached_size_); } @@ -788,11 +843,11 @@ void Config::MergeFrom(const Config& from) { (void) cached_has_bits; static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); - float tmp_min_height_m = from._internal_min_height_m(); - uint32_t raw_min_height_m; - memcpy(&raw_min_height_m, &tmp_min_height_m, sizeof(tmp_min_height_m)); - if (raw_min_height_m != 0) { - _internal_set_min_height_m(from._internal_min_height_m()); + float tmp_follow_height_m = from._internal_follow_height_m(); + uint32_t raw_follow_height_m; + memcpy(&raw_follow_height_m, &tmp_follow_height_m, sizeof(tmp_follow_height_m)); + if (raw_follow_height_m != 0) { + _internal_set_follow_height_m(from._internal_follow_height_m()); } static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); float tmp_follow_distance_m = from._internal_follow_distance_m(); @@ -801,9 +856,6 @@ void Config::MergeFrom(const Config& from) { if (raw_follow_distance_m != 0) { _internal_set_follow_distance_m(from._internal_follow_distance_m()); } - if (from._internal_follow_direction() != 0) { - _internal_set_follow_direction(from._internal_follow_direction()); - } static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); float tmp_responsiveness = from._internal_responsiveness(); uint32_t raw_responsiveness; @@ -811,6 +863,23 @@ void Config::MergeFrom(const Config& from) { if (raw_responsiveness != 0) { _internal_set_responsiveness(from._internal_responsiveness()); } + if (from._internal_altitude_mode() != 0) { + _internal_set_altitude_mode(from._internal_altitude_mode()); + } + static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); + float tmp_max_tangential_vel_m_s = from._internal_max_tangential_vel_m_s(); + uint32_t raw_max_tangential_vel_m_s; + memcpy(&raw_max_tangential_vel_m_s, &tmp_max_tangential_vel_m_s, sizeof(tmp_max_tangential_vel_m_s)); + if (raw_max_tangential_vel_m_s != 0) { + _internal_set_max_tangential_vel_m_s(from._internal_max_tangential_vel_m_s()); + } + static_assert(sizeof(uint32_t) == sizeof(float), "Code assumes uint32_t and float are the same size."); + float tmp_follow_angle_deg = from._internal_follow_angle_deg(); + uint32_t raw_follow_angle_deg; + memcpy(&raw_follow_angle_deg, &tmp_follow_angle_deg, sizeof(tmp_follow_angle_deg)); + if (raw_follow_angle_deg != 0) { + _internal_set_follow_angle_deg(from._internal_follow_angle_deg()); + } _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_); } @@ -829,11 +898,11 @@ void Config::InternalSwap(Config* other) { using std::swap; _internal_metadata_.InternalSwap(&other->_internal_metadata_); ::PROTOBUF_NAMESPACE_ID::internal::memswap< - PROTOBUF_FIELD_OFFSET(Config, responsiveness_) - + sizeof(Config::responsiveness_) - - PROTOBUF_FIELD_OFFSET(Config, min_height_m_)>( - reinterpret_cast(&min_height_m_), - reinterpret_cast(&other->min_height_m_)); + PROTOBUF_FIELD_OFFSET(Config, follow_angle_deg_) + + sizeof(Config::follow_angle_deg_) + - PROTOBUF_FIELD_OFFSET(Config, follow_height_m_)>( + reinterpret_cast(&follow_height_m_), + reinterpret_cast(&other->follow_height_m_)); } ::PROTOBUF_NAMESPACE_ID::Metadata Config::GetMetadata() const { diff --git a/src/mavsdk_server/src/generated/follow_me/follow_me.pb.h b/src/mavsdk_server/src/generated/follow_me/follow_me.pb.h index 30eb629080..6795f71bde 100644 --- a/src/mavsdk_server/src/generated/follow_me/follow_me.pb.h +++ b/src/mavsdk_server/src/generated/follow_me/follow_me.pb.h @@ -127,33 +127,31 @@ namespace mavsdk { namespace rpc { namespace follow_me { -enum Config_FollowDirection : int { - Config_FollowDirection_FOLLOW_DIRECTION_NONE = 0, - Config_FollowDirection_FOLLOW_DIRECTION_BEHIND = 1, - Config_FollowDirection_FOLLOW_DIRECTION_FRONT = 2, - Config_FollowDirection_FOLLOW_DIRECTION_FRONT_RIGHT = 3, - Config_FollowDirection_FOLLOW_DIRECTION_FRONT_LEFT = 4, - Config_FollowDirection_Config_FollowDirection_INT_MIN_SENTINEL_DO_NOT_USE_ = std::numeric_limits::min(), - Config_FollowDirection_Config_FollowDirection_INT_MAX_SENTINEL_DO_NOT_USE_ = std::numeric_limits::max() +enum Config_FollowAltitudeMode : int { + Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_CONSTANT = 0, + Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TERRAIN = 1, + Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TARGET_GPS = 2, + Config_FollowAltitudeMode_Config_FollowAltitudeMode_INT_MIN_SENTINEL_DO_NOT_USE_ = std::numeric_limits::min(), + Config_FollowAltitudeMode_Config_FollowAltitudeMode_INT_MAX_SENTINEL_DO_NOT_USE_ = std::numeric_limits::max() }; -bool Config_FollowDirection_IsValid(int value); -constexpr Config_FollowDirection Config_FollowDirection_FollowDirection_MIN = Config_FollowDirection_FOLLOW_DIRECTION_NONE; -constexpr Config_FollowDirection Config_FollowDirection_FollowDirection_MAX = Config_FollowDirection_FOLLOW_DIRECTION_FRONT_LEFT; -constexpr int Config_FollowDirection_FollowDirection_ARRAYSIZE = Config_FollowDirection_FollowDirection_MAX + 1; +bool Config_FollowAltitudeMode_IsValid(int value); +constexpr Config_FollowAltitudeMode Config_FollowAltitudeMode_FollowAltitudeMode_MIN = Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_CONSTANT; +constexpr Config_FollowAltitudeMode Config_FollowAltitudeMode_FollowAltitudeMode_MAX = Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TARGET_GPS; +constexpr int Config_FollowAltitudeMode_FollowAltitudeMode_ARRAYSIZE = Config_FollowAltitudeMode_FollowAltitudeMode_MAX + 1; -const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* Config_FollowDirection_descriptor(); +const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* Config_FollowAltitudeMode_descriptor(); template -inline const std::string& Config_FollowDirection_Name(T enum_t_value) { - static_assert(::std::is_same::value || +inline const std::string& Config_FollowAltitudeMode_Name(T enum_t_value) { + static_assert(::std::is_same::value || ::std::is_integral::value, - "Incorrect type passed to function Config_FollowDirection_Name."); + "Incorrect type passed to function Config_FollowAltitudeMode_Name."); return ::PROTOBUF_NAMESPACE_ID::internal::NameOfEnum( - Config_FollowDirection_descriptor(), enum_t_value); + Config_FollowAltitudeMode_descriptor(), enum_t_value); } -inline bool Config_FollowDirection_Parse( - ::PROTOBUF_NAMESPACE_ID::ConstStringParam name, Config_FollowDirection* value) { - return ::PROTOBUF_NAMESPACE_ID::internal::ParseNamedEnum( - Config_FollowDirection_descriptor(), name, value); +inline bool Config_FollowAltitudeMode_Parse( + ::PROTOBUF_NAMESPACE_ID::ConstStringParam name, Config_FollowAltitudeMode* value) { + return ::PROTOBUF_NAMESPACE_ID::internal::ParseNamedEnum( + Config_FollowAltitudeMode_descriptor(), name, value); } enum FollowMeResult_Result : int { FollowMeResult_Result_RESULT_UNKNOWN = 0, @@ -305,60 +303,58 @@ class Config final : // nested types ---------------------------------------------------- - typedef Config_FollowDirection FollowDirection; - static constexpr FollowDirection FOLLOW_DIRECTION_NONE = - Config_FollowDirection_FOLLOW_DIRECTION_NONE; - static constexpr FollowDirection FOLLOW_DIRECTION_BEHIND = - Config_FollowDirection_FOLLOW_DIRECTION_BEHIND; - static constexpr FollowDirection FOLLOW_DIRECTION_FRONT = - Config_FollowDirection_FOLLOW_DIRECTION_FRONT; - static constexpr FollowDirection FOLLOW_DIRECTION_FRONT_RIGHT = - Config_FollowDirection_FOLLOW_DIRECTION_FRONT_RIGHT; - static constexpr FollowDirection FOLLOW_DIRECTION_FRONT_LEFT = - Config_FollowDirection_FOLLOW_DIRECTION_FRONT_LEFT; - static inline bool FollowDirection_IsValid(int value) { - return Config_FollowDirection_IsValid(value); - } - static constexpr FollowDirection FollowDirection_MIN = - Config_FollowDirection_FollowDirection_MIN; - static constexpr FollowDirection FollowDirection_MAX = - Config_FollowDirection_FollowDirection_MAX; - static constexpr int FollowDirection_ARRAYSIZE = - Config_FollowDirection_FollowDirection_ARRAYSIZE; + typedef Config_FollowAltitudeMode FollowAltitudeMode; + static constexpr FollowAltitudeMode FOLLOW_ALTITUDE_MODE_CONSTANT = + Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_CONSTANT; + static constexpr FollowAltitudeMode FOLLOW_ALTITUDE_MODE_TERRAIN = + Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TERRAIN; + static constexpr FollowAltitudeMode FOLLOW_ALTITUDE_MODE_TARGET_GPS = + Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TARGET_GPS; + static inline bool FollowAltitudeMode_IsValid(int value) { + return Config_FollowAltitudeMode_IsValid(value); + } + static constexpr FollowAltitudeMode FollowAltitudeMode_MIN = + Config_FollowAltitudeMode_FollowAltitudeMode_MIN; + static constexpr FollowAltitudeMode FollowAltitudeMode_MAX = + Config_FollowAltitudeMode_FollowAltitudeMode_MAX; + static constexpr int FollowAltitudeMode_ARRAYSIZE = + Config_FollowAltitudeMode_FollowAltitudeMode_ARRAYSIZE; static inline const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* - FollowDirection_descriptor() { - return Config_FollowDirection_descriptor(); + FollowAltitudeMode_descriptor() { + return Config_FollowAltitudeMode_descriptor(); } template - static inline const std::string& FollowDirection_Name(T enum_t_value) { - static_assert(::std::is_same::value || + static inline const std::string& FollowAltitudeMode_Name(T enum_t_value) { + static_assert(::std::is_same::value || ::std::is_integral::value, - "Incorrect type passed to function FollowDirection_Name."); - return Config_FollowDirection_Name(enum_t_value); + "Incorrect type passed to function FollowAltitudeMode_Name."); + return Config_FollowAltitudeMode_Name(enum_t_value); } - static inline bool FollowDirection_Parse(::PROTOBUF_NAMESPACE_ID::ConstStringParam name, - FollowDirection* value) { - return Config_FollowDirection_Parse(name, value); + static inline bool FollowAltitudeMode_Parse(::PROTOBUF_NAMESPACE_ID::ConstStringParam name, + FollowAltitudeMode* value) { + return Config_FollowAltitudeMode_Parse(name, value); } // accessors ------------------------------------------------------- enum : int { - kMinHeightMFieldNumber = 1, + kFollowHeightMFieldNumber = 1, kFollowDistanceMFieldNumber = 2, - kFollowDirectionFieldNumber = 3, kResponsivenessFieldNumber = 4, + kAltitudeModeFieldNumber = 5, + kMaxTangentialVelMSFieldNumber = 6, + kFollowAngleDegFieldNumber = 7, }; - // float min_height_m = 1 [(.mavsdk.options.default_value) = "8.0"]; - void clear_min_height_m(); - float min_height_m() const; - void set_min_height_m(float value); + // float follow_height_m = 1 [(.mavsdk.options.default_value) = "8.0f"]; + void clear_follow_height_m(); + float follow_height_m() const; + void set_follow_height_m(float value); private: - float _internal_min_height_m() const; - void _internal_set_min_height_m(float value); + float _internal_follow_height_m() const; + void _internal_set_follow_height_m(float value); public: - // float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0"]; + // float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0f"]; void clear_follow_distance_m(); float follow_distance_m() const; void set_follow_distance_m(float value); @@ -367,16 +363,7 @@ class Config final : void _internal_set_follow_distance_m(float value); public: - // .mavsdk.rpc.follow_me.Config.FollowDirection follow_direction = 3; - void clear_follow_direction(); - ::mavsdk::rpc::follow_me::Config_FollowDirection follow_direction() const; - void set_follow_direction(::mavsdk::rpc::follow_me::Config_FollowDirection value); - private: - ::mavsdk::rpc::follow_me::Config_FollowDirection _internal_follow_direction() const; - void _internal_set_follow_direction(::mavsdk::rpc::follow_me::Config_FollowDirection value); - public: - - // float responsiveness = 4 [(.mavsdk.options.default_value) = "0.5"]; + // float responsiveness = 4 [(.mavsdk.options.default_value) = "0.1f"]; void clear_responsiveness(); float responsiveness() const; void set_responsiveness(float value); @@ -385,6 +372,33 @@ class Config final : void _internal_set_responsiveness(float value); public: + // .mavsdk.rpc.follow_me.Config.FollowAltitudeMode altitude_mode = 5; + void clear_altitude_mode(); + ::mavsdk::rpc::follow_me::Config_FollowAltitudeMode altitude_mode() const; + void set_altitude_mode(::mavsdk::rpc::follow_me::Config_FollowAltitudeMode value); + private: + ::mavsdk::rpc::follow_me::Config_FollowAltitudeMode _internal_altitude_mode() const; + void _internal_set_altitude_mode(::mavsdk::rpc::follow_me::Config_FollowAltitudeMode value); + public: + + // float max_tangential_vel_m_s = 6 [(.mavsdk.options.default_value) = "8.0f"]; + void clear_max_tangential_vel_m_s(); + float max_tangential_vel_m_s() const; + void set_max_tangential_vel_m_s(float value); + private: + float _internal_max_tangential_vel_m_s() const; + void _internal_set_max_tangential_vel_m_s(float value); + public: + + // float follow_angle_deg = 7 [(.mavsdk.options.default_value) = "180.0f"]; + void clear_follow_angle_deg(); + float follow_angle_deg() const; + void set_follow_angle_deg(float value); + private: + float _internal_follow_angle_deg() const; + void _internal_set_follow_angle_deg(float value); + public: + // @@protoc_insertion_point(class_scope:mavsdk.rpc.follow_me.Config) private: class _Internal; @@ -392,10 +406,12 @@ class Config final : template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; typedef void InternalArenaConstructable_; typedef void DestructorSkippable_; - float min_height_m_; + float follow_height_m_; float follow_distance_m_; - int follow_direction_; float responsiveness_; + int altitude_mode_; + float max_tangential_vel_m_s_; + float follow_angle_deg_; mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; friend struct ::TableStruct_follow_5fme_2ffollow_5fme_2eproto; }; @@ -2750,27 +2766,27 @@ class FollowMeResult final : #endif // __GNUC__ // Config -// float min_height_m = 1 [(.mavsdk.options.default_value) = "8.0"]; -inline void Config::clear_min_height_m() { - min_height_m_ = 0; +// float follow_height_m = 1 [(.mavsdk.options.default_value) = "8.0f"]; +inline void Config::clear_follow_height_m() { + follow_height_m_ = 0; } -inline float Config::_internal_min_height_m() const { - return min_height_m_; +inline float Config::_internal_follow_height_m() const { + return follow_height_m_; } -inline float Config::min_height_m() const { - // @@protoc_insertion_point(field_get:mavsdk.rpc.follow_me.Config.min_height_m) - return _internal_min_height_m(); +inline float Config::follow_height_m() const { + // @@protoc_insertion_point(field_get:mavsdk.rpc.follow_me.Config.follow_height_m) + return _internal_follow_height_m(); } -inline void Config::_internal_set_min_height_m(float value) { +inline void Config::_internal_set_follow_height_m(float value) { - min_height_m_ = value; + follow_height_m_ = value; } -inline void Config::set_min_height_m(float value) { - _internal_set_min_height_m(value); - // @@protoc_insertion_point(field_set:mavsdk.rpc.follow_me.Config.min_height_m) +inline void Config::set_follow_height_m(float value) { + _internal_set_follow_height_m(value); + // @@protoc_insertion_point(field_set:mavsdk.rpc.follow_me.Config.follow_height_m) } -// float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0"]; +// float follow_distance_m = 2 [(.mavsdk.options.default_value) = "8.0f"]; inline void Config::clear_follow_distance_m() { follow_distance_m_ = 0; } @@ -2790,27 +2806,7 @@ inline void Config::set_follow_distance_m(float value) { // @@protoc_insertion_point(field_set:mavsdk.rpc.follow_me.Config.follow_distance_m) } -// .mavsdk.rpc.follow_me.Config.FollowDirection follow_direction = 3; -inline void Config::clear_follow_direction() { - follow_direction_ = 0; -} -inline ::mavsdk::rpc::follow_me::Config_FollowDirection Config::_internal_follow_direction() const { - return static_cast< ::mavsdk::rpc::follow_me::Config_FollowDirection >(follow_direction_); -} -inline ::mavsdk::rpc::follow_me::Config_FollowDirection Config::follow_direction() const { - // @@protoc_insertion_point(field_get:mavsdk.rpc.follow_me.Config.follow_direction) - return _internal_follow_direction(); -} -inline void Config::_internal_set_follow_direction(::mavsdk::rpc::follow_me::Config_FollowDirection value) { - - follow_direction_ = value; -} -inline void Config::set_follow_direction(::mavsdk::rpc::follow_me::Config_FollowDirection value) { - _internal_set_follow_direction(value); - // @@protoc_insertion_point(field_set:mavsdk.rpc.follow_me.Config.follow_direction) -} - -// float responsiveness = 4 [(.mavsdk.options.default_value) = "0.5"]; +// float responsiveness = 4 [(.mavsdk.options.default_value) = "0.1f"]; inline void Config::clear_responsiveness() { responsiveness_ = 0; } @@ -2830,6 +2826,66 @@ inline void Config::set_responsiveness(float value) { // @@protoc_insertion_point(field_set:mavsdk.rpc.follow_me.Config.responsiveness) } +// .mavsdk.rpc.follow_me.Config.FollowAltitudeMode altitude_mode = 5; +inline void Config::clear_altitude_mode() { + altitude_mode_ = 0; +} +inline ::mavsdk::rpc::follow_me::Config_FollowAltitudeMode Config::_internal_altitude_mode() const { + return static_cast< ::mavsdk::rpc::follow_me::Config_FollowAltitudeMode >(altitude_mode_); +} +inline ::mavsdk::rpc::follow_me::Config_FollowAltitudeMode Config::altitude_mode() const { + // @@protoc_insertion_point(field_get:mavsdk.rpc.follow_me.Config.altitude_mode) + return _internal_altitude_mode(); +} +inline void Config::_internal_set_altitude_mode(::mavsdk::rpc::follow_me::Config_FollowAltitudeMode value) { + + altitude_mode_ = value; +} +inline void Config::set_altitude_mode(::mavsdk::rpc::follow_me::Config_FollowAltitudeMode value) { + _internal_set_altitude_mode(value); + // @@protoc_insertion_point(field_set:mavsdk.rpc.follow_me.Config.altitude_mode) +} + +// float max_tangential_vel_m_s = 6 [(.mavsdk.options.default_value) = "8.0f"]; +inline void Config::clear_max_tangential_vel_m_s() { + max_tangential_vel_m_s_ = 0; +} +inline float Config::_internal_max_tangential_vel_m_s() const { + return max_tangential_vel_m_s_; +} +inline float Config::max_tangential_vel_m_s() const { + // @@protoc_insertion_point(field_get:mavsdk.rpc.follow_me.Config.max_tangential_vel_m_s) + return _internal_max_tangential_vel_m_s(); +} +inline void Config::_internal_set_max_tangential_vel_m_s(float value) { + + max_tangential_vel_m_s_ = value; +} +inline void Config::set_max_tangential_vel_m_s(float value) { + _internal_set_max_tangential_vel_m_s(value); + // @@protoc_insertion_point(field_set:mavsdk.rpc.follow_me.Config.max_tangential_vel_m_s) +} + +// float follow_angle_deg = 7 [(.mavsdk.options.default_value) = "180.0f"]; +inline void Config::clear_follow_angle_deg() { + follow_angle_deg_ = 0; +} +inline float Config::_internal_follow_angle_deg() const { + return follow_angle_deg_; +} +inline float Config::follow_angle_deg() const { + // @@protoc_insertion_point(field_get:mavsdk.rpc.follow_me.Config.follow_angle_deg) + return _internal_follow_angle_deg(); +} +inline void Config::_internal_set_follow_angle_deg(float value) { + + follow_angle_deg_ = value; +} +inline void Config::set_follow_angle_deg(float value) { + _internal_set_follow_angle_deg(value); + // @@protoc_insertion_point(field_set:mavsdk.rpc.follow_me.Config.follow_angle_deg) +} + // ------------------------------------------------------------------- // TargetLocation @@ -3868,10 +3924,10 @@ inline void FollowMeResult::set_allocated_result_str(std::string* result_str) { PROTOBUF_NAMESPACE_OPEN -template <> struct is_proto_enum< ::mavsdk::rpc::follow_me::Config_FollowDirection> : ::std::true_type {}; +template <> struct is_proto_enum< ::mavsdk::rpc::follow_me::Config_FollowAltitudeMode> : ::std::true_type {}; template <> -inline const EnumDescriptor* GetEnumDescriptor< ::mavsdk::rpc::follow_me::Config_FollowDirection>() { - return ::mavsdk::rpc::follow_me::Config_FollowDirection_descriptor(); +inline const EnumDescriptor* GetEnumDescriptor< ::mavsdk::rpc::follow_me::Config_FollowAltitudeMode>() { + return ::mavsdk::rpc::follow_me::Config_FollowAltitudeMode_descriptor(); } template <> struct is_proto_enum< ::mavsdk::rpc::follow_me::FollowMeResult_Result> : ::std::true_type {}; template <> diff --git a/src/mavsdk_server/src/plugins/follow_me/follow_me_service_impl.h b/src/mavsdk_server/src/plugins/follow_me/follow_me_service_impl.h index de77c7128e..a72bd3eb2b 100644 --- a/src/mavsdk_server/src/plugins/follow_me/follow_me_service_impl.h +++ b/src/mavsdk_server/src/plugins/follow_me/follow_me_service_impl.h @@ -41,45 +41,37 @@ class FollowMeServiceImpl final : public rpc::follow_me::FollowMeService::Servic response->set_allocated_follow_me_result(rpc_follow_me_result); } - static rpc::follow_me::Config::FollowDirection - translateToRpcFollowDirection(const mavsdk::FollowMe::Config::FollowDirection& follow_direction) + static rpc::follow_me::Config::FollowAltitudeMode translateToRpcFollowAltitudeMode( + const mavsdk::FollowMe::Config::FollowAltitudeMode& follow_altitude_mode) { - switch (follow_direction) { + switch (follow_altitude_mode) { default: - LogErr() << "Unknown follow_direction enum value: " - << static_cast(follow_direction); + LogErr() << "Unknown follow_altitude_mode enum value: " + << static_cast(follow_altitude_mode); // FALLTHROUGH - case mavsdk::FollowMe::Config::FollowDirection::None: - return rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_NONE; - case mavsdk::FollowMe::Config::FollowDirection::Behind: - return rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_BEHIND; - case mavsdk::FollowMe::Config::FollowDirection::Front: - return rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_FRONT; - case mavsdk::FollowMe::Config::FollowDirection::FrontRight: - return rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_FRONT_RIGHT; - case mavsdk::FollowMe::Config::FollowDirection::FrontLeft: - return rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_FRONT_LEFT; + case mavsdk::FollowMe::Config::FollowAltitudeMode::Constant: + return rpc::follow_me::Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_CONSTANT; + case mavsdk::FollowMe::Config::FollowAltitudeMode::Terrain: + return rpc::follow_me::Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TERRAIN; + case mavsdk::FollowMe::Config::FollowAltitudeMode::TargetGps: + return rpc::follow_me::Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TARGET_GPS; } } - static mavsdk::FollowMe::Config::FollowDirection - translateFromRpcFollowDirection(const rpc::follow_me::Config::FollowDirection follow_direction) + static mavsdk::FollowMe::Config::FollowAltitudeMode translateFromRpcFollowAltitudeMode( + const rpc::follow_me::Config::FollowAltitudeMode follow_altitude_mode) { - switch (follow_direction) { + switch (follow_altitude_mode) { default: - LogErr() << "Unknown follow_direction enum value: " - << static_cast(follow_direction); + LogErr() << "Unknown follow_altitude_mode enum value: " + << static_cast(follow_altitude_mode); // FALLTHROUGH - case rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_NONE: - return mavsdk::FollowMe::Config::FollowDirection::None; - case rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_BEHIND: - return mavsdk::FollowMe::Config::FollowDirection::Behind; - case rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_FRONT: - return mavsdk::FollowMe::Config::FollowDirection::Front; - case rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_FRONT_RIGHT: - return mavsdk::FollowMe::Config::FollowDirection::FrontRight; - case rpc::follow_me::Config_FollowDirection_FOLLOW_DIRECTION_FRONT_LEFT: - return mavsdk::FollowMe::Config::FollowDirection::FrontLeft; + case rpc::follow_me::Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_CONSTANT: + return mavsdk::FollowMe::Config::FollowAltitudeMode::Constant; + case rpc::follow_me::Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TERRAIN: + return mavsdk::FollowMe::Config::FollowAltitudeMode::Terrain; + case rpc::follow_me::Config_FollowAltitudeMode_FOLLOW_ALTITUDE_MODE_TARGET_GPS: + return mavsdk::FollowMe::Config::FollowAltitudeMode::TargetGps; } } @@ -88,14 +80,18 @@ class FollowMeServiceImpl final : public rpc::follow_me::FollowMeService::Servic { auto rpc_obj = std::make_unique(); - rpc_obj->set_min_height_m(config.min_height_m); + rpc_obj->set_follow_height_m(config.follow_height_m); rpc_obj->set_follow_distance_m(config.follow_distance_m); - rpc_obj->set_follow_direction(translateToRpcFollowDirection(config.follow_direction)); - rpc_obj->set_responsiveness(config.responsiveness); + rpc_obj->set_altitude_mode(translateToRpcFollowAltitudeMode(config.altitude_mode)); + + rpc_obj->set_max_tangential_vel_m_s(config.max_tangential_vel_m_s); + + rpc_obj->set_follow_angle_deg(config.follow_angle_deg); + return rpc_obj; } @@ -103,14 +99,18 @@ class FollowMeServiceImpl final : public rpc::follow_me::FollowMeService::Servic { mavsdk::FollowMe::Config obj; - obj.min_height_m = config.min_height_m(); + obj.follow_height_m = config.follow_height_m(); obj.follow_distance_m = config.follow_distance_m(); - obj.follow_direction = translateFromRpcFollowDirection(config.follow_direction()); - obj.responsiveness = config.responsiveness(); + obj.altitude_mode = translateFromRpcFollowAltitudeMode(config.altitude_mode()); + + obj.max_tangential_vel_m_s = config.max_tangential_vel_m_s(); + + obj.follow_angle_deg = config.follow_angle_deg(); + return obj; }