From 996d4c245020ee44988cf1f20d30d192837a7ded Mon Sep 17 00:00:00 2001 From: Ahmed Elsaharti Date: Sun, 28 Feb 2021 17:08:55 -0600 Subject: [PATCH 1/5] Added movetoGPS API --- .../vehicles/multirotor/api/MultirotorApiBase.hpp | 3 +++ .../multirotor/api/MultirotorRpcLibClient.hpp | 3 +++ .../firmwares/arducopter/ArduCopterApi.hpp | 4 ++++ .../firmwares/mavlink/MavLinkMultirotorApi.hpp | 5 +++++ .../firmwares/simple_flight/SimpleFlightApi.hpp | 4 ++++ .../vehicles/multirotor/api/MultirotorApiBase.cpp | 12 ++++++++++++ .../multirotor/api/MultirotorRpcLibClient.cpp | 8 ++++++++ .../multirotor/api/MultirotorRpcLibServer.cpp | 5 +++++ PythonClient/airsim/client.py | 4 ++++ 9 files changed, 48 insertions(+) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index d0a0e1c86d..35a45813e6 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -43,6 +43,7 @@ class MultirotorApiBase : public VehicleApiBase { virtual LandedState getLandedState() const = 0; virtual GeoPoint getGpsLocation() const = 0; virtual const MultirotorApiParams& getMultirotorApiParams() const = 0; + virtual GeoPoint getHomeGpsLocation() const = 0; /************************* basic config APIs *********************************/ virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds @@ -89,6 +90,8 @@ class MultirotorApiBase : public VehicleApiBase { virtual bool land(float timeout_sec); virtual bool goHome(float timeout_sec); + virtual bool moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration); virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index 59eb996ac5..4fed449146 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -23,6 +23,9 @@ class MultirotorRpcLibClient : public RpcLibClientBase { MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name = ""); diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp index 400ac408ed..dd3d3cea23 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -87,6 +87,10 @@ class ArduCopterApi : public MultirotorApiBase { Utils::log("Not Implemented: getHomeGeoPoint", Utils::kLogLevelInfo); return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); } + virtual GeoPoint getHomeGpsLocation() const override + { + return AirSimSettings::singleton().origin_geopoint.home_geo_point; + } virtual void getStatusMessages(std::vector& messages) override { unused(messages); diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index f61aaeeac8..cd73720eae 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -400,6 +400,11 @@ class MavLinkMultirotorApi : public MultirotorApiBase return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); } + virtual GeoPoint getHomeGpsLocation() const override + { + return AirSimSettings::singleton().origin_geopoint.home_geo_point; + } + virtual GeoPoint getGpsLocation() const override { updateState(); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index a6a67dd52a..d41e3d9ef8 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -83,6 +83,10 @@ class SimpleFlightApi : public MultirotorApiBase { { return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint()); } + virtual GeoPoint getHomeGpsLocation() const override + { + return AirSimSettings::singleton().origin_geopoint.home_geo_point; + } virtual void getStatusMessages(std::vector& messages) override { comm_link_->getStatusMessages(messages); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 0dda206776..29bf6e017f 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -394,6 +394,18 @@ bool MultirotorApiBase::moveOnPath(const vector& path, float velocity, return waiter.isComplete(); } +bool MultirotorApiBase::moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) +{ + SingleTaskCall lock(this); + GeoPoint target; + target.latitude = latitude; + target.longitude = longitude; + target.altitude = altitude; + vector path{ msr::airlib::EarthUtils::GeodeticToNedFast(target, getHomeGpsLocation()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); +} + bool MultirotorApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) { diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 717106f232..3f70987f53 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -151,6 +151,14 @@ MultirotorRpcLibClient* MultirotorRpcLibClient::moveOnPathAsync(const vectorlast_future = static_cast(getClient())->async_call("movetoGPS", latitude, longitude, altitude, velocity, timeout_sec, + drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; +} + MultirotorRpcLibClient* MultirotorRpcLibClient::moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) { diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 10e650e620..e8b62cf78c 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -108,6 +108,11 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string MultirotorRpcLibAdapators::to(path, conv_path); return getVehicleApi(vehicle_name)->moveOnPath(conv_path, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); }); + (static_cast(getServer()))-> + bind("moveToGPS", [&](float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveToGPS(latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); + }); (static_cast(getServer()))-> bind("moveToPosition", [&](float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 27e6cc84b7..669082a902 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -948,6 +948,10 @@ def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + def moveToGPSAsync(self, latitude, longitude, altitude, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), + lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToGPS', latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) From 842787d6c8d35ba75561eb8a14fee7b190287dde Mon Sep 17 00:00:00 2001 From: Ahmed Elsaharti Date: Sun, 28 Feb 2021 23:04:55 -0600 Subject: [PATCH 2/5] added GeodeticToNed util function --- AirLib/include/common/EarthUtils.hpp | 41 +++++++++++++++++++ .../multirotor/api/MultirotorApiBase.cpp | 2 +- 2 files changed, 42 insertions(+), 1 deletion(-) diff --git a/AirLib/include/common/EarthUtils.hpp b/AirLib/include/common/EarthUtils.hpp index d578fac033..9f5eea97af 100644 --- a/AirLib/include/common/EarthUtils.hpp +++ b/AirLib/include/common/EarthUtils.hpp @@ -303,6 +303,47 @@ class EarthUtils { return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude - v.z()); } + static Vector3r GeodeticToEcef(const GeoPoint& geo) + { + // Convert geodetic coordinates to ECEF. + // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22 + double lat_rad = Utils::degreesToRadians(geo.latitude); + double lon_rad = Utils::degreesToRadians(geo.longitude); + double xi = sqrt(1 - (6.69437999014 * 0.001) * sin(lat_rad) * sin(lat_rad)); + real_T x = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * cos(lon_rad)); + real_T y = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * sin(lon_rad)); + real_T z = static_cast((EARTH_RADIUS / xi * (1 - (6.69437999014 * 0.001)) + geo.altitude) * sin(lat_rad)); + return Vector3r(x, y, z); + } + + static Vector3r EcefToNed(const Vector3r& ecefxyz, const Vector3r& ecefhome, const GeoPoint& geohome) + { + // Converts ECEF coordinate position into local-tangent-plane NED. + // Coordinates relative to given ECEF coordinate frame. + + Vector3r vect, ret; + Matrix3x3r ecef_to_ned_matrix_; + double lat_rad = Utils::degreesToRadians(geohome.latitude); + double lon_rad = Utils::degreesToRadians(geohome.longitude); + vect = ecefxyz - ecefhome; + ecef_to_ned_matrix_(0, 0) = static_cast(-sin(lat_rad) * cos(lon_rad)); + ecef_to_ned_matrix_(0, 1) = static_cast(-sin(lat_rad) * sin(lon_rad)); + ecef_to_ned_matrix_(0, 2) = static_cast(cos(lat_rad)); + ecef_to_ned_matrix_(1, 0) = static_cast(-sin(lon_rad)); + ecef_to_ned_matrix_(1, 1) = static_cast(cos(lon_rad)); + ecef_to_ned_matrix_(1, 2) = 0.0f; + ecef_to_ned_matrix_(2, 0) = static_cast(cos(lat_rad) * cos(lon_rad)); + ecef_to_ned_matrix_(2, 1) = static_cast(cos(lat_rad) * sin(lon_rad)); + ecef_to_ned_matrix_(2, 2) = static_cast(sin(lat_rad)); + ret = ecef_to_ned_matrix_ * vect; + return Vector3r(ret(0), ret(1), -ret(2)); + } + + static Vector3r GeodeticToNed(const GeoPoint& geo, const GeoPoint& geohome) + { + return EcefToNed(GeodeticToEcef(geo), GeodeticToEcef(geohome), geohome); + } + //below are approximate versions and would produce errors of more than 10m for points farther than 1km //for more accurate versions, please use the version in EarthUtils::nedToGeodetic static Vector3r GeodeticToNedFast(const GeoPoint& geo, const GeoPoint& home) diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 29bf6e017f..6b495ff931 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -402,7 +402,7 @@ bool MultirotorApiBase::moveToGPS(float latitude, float longitude, float altitud target.latitude = latitude; target.longitude = longitude; target.altitude = altitude; - vector path{ msr::airlib::EarthUtils::GeodeticToNedFast(target, getHomeGpsLocation()) }; + vector path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGpsLocation()) }; return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); } From 2a68cccb43592da9d486ae932f956257787f30ce Mon Sep 17 00:00:00 2001 From: Ahmed Elsaharti Date: Mon, 8 Mar 2021 20:55:09 -0600 Subject: [PATCH 3/5] use native getHomeGeoPoint() --- .../vehicles/multirotor/api/MultirotorApiBase.hpp | 1 - .../multirotor/firmwares/arducopter/ArduCopterApi.hpp | 4 ---- .../firmwares/mavlink/MavLinkMultirotorApi.hpp | 5 ----- .../firmwares/simple_flight/SimpleFlightApi.hpp | 4 ---- .../src/vehicles/multirotor/api/MultirotorApiBase.cpp | 10 ++++++++-- 5 files changed, 8 insertions(+), 16 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index 35a45813e6..512c285847 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -43,7 +43,6 @@ class MultirotorApiBase : public VehicleApiBase { virtual LandedState getLandedState() const = 0; virtual GeoPoint getGpsLocation() const = 0; virtual const MultirotorApiParams& getMultirotorApiParams() const = 0; - virtual GeoPoint getHomeGpsLocation() const = 0; /************************* basic config APIs *********************************/ virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp index dd3d3cea23..400ac408ed 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -87,10 +87,6 @@ class ArduCopterApi : public MultirotorApiBase { Utils::log("Not Implemented: getHomeGeoPoint", Utils::kLogLevelInfo); return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); } - virtual GeoPoint getHomeGpsLocation() const override - { - return AirSimSettings::singleton().origin_geopoint.home_geo_point; - } virtual void getStatusMessages(std::vector& messages) override { unused(messages); diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index cd73720eae..f61aaeeac8 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -400,11 +400,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); } - virtual GeoPoint getHomeGpsLocation() const override - { - return AirSimSettings::singleton().origin_geopoint.home_geo_point; - } - virtual GeoPoint getGpsLocation() const override { updateState(); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index d41e3d9ef8..a6a67dd52a 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -83,10 +83,6 @@ class SimpleFlightApi : public MultirotorApiBase { { return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint()); } - virtual GeoPoint getHomeGpsLocation() const override - { - return AirSimSettings::singleton().origin_geopoint.home_geo_point; - } virtual void getStatusMessages(std::vector& messages) override { comm_link_->getStatusMessages(messages); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 6b495ff931..3974bc426b 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -402,8 +402,14 @@ bool MultirotorApiBase::moveToGPS(float latitude, float longitude, float altitud target.latitude = latitude; target.longitude = longitude; target.altitude = altitude; - vector path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGpsLocation()) }; - return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + if (!std::isnan(getHomeGeoPoint().latitude) && !std::isnan(getHomeGeoPoint().longitude) && !std::isnan(getHomeGeoPoint().altitude)) { + vector path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGeoPoint()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } + else { + vector path{ Vector3r(getPosition().x(),getPosition().y(),getPosition().z()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } } bool MultirotorApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, From a8bbbfa20c442d3f20740004e76f4625444d664e Mon Sep 17 00:00:00 2001 From: zimmy87 Date: Thu, 2 Dec 2021 02:17:35 -0800 Subject: [PATCH 4/5] apply style from clang-format --- .clang-format | 56 + AirLib/include/common/EarthUtils.hpp | 644 ++++---- .../multirotor/api/MultirotorApiBase.hpp | 604 +++---- .../multirotor/api/MultirotorRpcLibClient.hpp | 108 +- .../multirotor/api/MultirotorApiBase.cpp | 1406 +++++++++-------- .../multirotor/api/MultirotorRpcLibClient.cpp | 438 +++-- .../multirotor/api/MultirotorRpcLibServer.cpp | 232 ++- 7 files changed, 1774 insertions(+), 1714 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000000..74f44951ab --- /dev/null +++ b/.clang-format @@ -0,0 +1,56 @@ +--- +# clang-format documentation +# http://clang.llvm.org/docs/ClangFormat.html +# http://clang.llvm.org/docs/ClangFormatStyleOptions.html + +# Preexisting formats: +# LLVM +# Google +# Chromium +# Mozilla +# WebKit + +Language: Cpp +BasedOnStyle: Mozilla + +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignTrailingComments: false +AllowShortBlocksOnASingleLine: false +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +BinPackArguments: false +BinPackParameters: true +BreakBeforeBraces: Custom +BraceWrapping: + AfterClass: true + AfterControlStatement: false + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterObjCDeclaration: false + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false +ContinuationIndentWidth: 4 +ConstructorInitializerIndentWidth: 4 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ColumnLimit: 0 +Cpp11BracedListStyle: false +IndentWidth: 4 +IndentCaseLabels: false +# KeepEmptyLinesAtTheStartOfBlocks: false +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: Inner +ReflowComments: false +PenaltyBreakBeforeFirstCallParameter: 100000 +PenaltyBreakComment: 100000 +SortIncludes: false +SpaceAfterTemplateKeyword: true +# Standard: Cpp11 # Broken +UseTab: Never +... diff --git a/AirLib/include/common/EarthUtils.hpp b/AirLib/include/common/EarthUtils.hpp index 9f5eea97af..798efc36b3 100644 --- a/AirLib/include/common/EarthUtils.hpp +++ b/AirLib/include/common/EarthUtils.hpp @@ -1,7 +1,6 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. - #ifndef air_Earth_hpp #define air_Earth_hpp @@ -10,202 +9,207 @@ #include "common/CommonStructs.hpp" #include "common/Common.hpp" -namespace msr { namespace airlib { - -class EarthUtils { -private: +namespace msr +{ +namespace airlib +{ - /** set this always to the sampling in degrees for the table below */ - static constexpr int MAG_SAMPLING_RES = 10; - static constexpr int MAG_SAMPLING_MIN_LAT = -60; - static constexpr int MAG_SAMPLING_MAX_LAT = 60; - static constexpr int MAG_SAMPLING_MIN_LON = -180; - static constexpr int MAG_SAMPLING_MAX_LON = 180; - - static constexpr int DECLINATION_TABLE[13][37] = + class EarthUtils { - { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, - { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, - { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, - { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, - { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, - { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, - { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, - { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, - { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, - { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, - { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, - { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, - { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, - }; -public: - //return declination in degrees - //Ref: https://github.com/PX4/ecl/blob/master/EKF/geo.cpp - static real_T getMagDeclination(real_T latitude, real_T longitude) - { - /* + private: + /** set this always to the sampling in degrees for the table below */ + static constexpr int MAG_SAMPLING_RES = 10; + static constexpr int MAG_SAMPLING_MIN_LAT = -60; + static constexpr int MAG_SAMPLING_MAX_LAT = 60; + static constexpr int MAG_SAMPLING_MIN_LON = -180; + static constexpr int MAG_SAMPLING_MAX_LON = 180; + + static constexpr int DECLINATION_TABLE[13][37] = { + { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, + }; + + public: + //return declination in degrees + //Ref: https://github.com/PX4/ecl/blob/master/EKF/geo.cpp + static real_T getMagDeclination(real_T latitude, real_T longitude) + { + /* * If the values exceed valid ranges, return zero as default * as we have no way of knowing what the closest real value * would be. */ - if (latitude < -90.0f || latitude > 90.0f || - longitude < -180.0f || longitude > 180.0f) { - throw std::out_of_range(Utils::stringf("invalid latitude (%f) or longitude (%f) value", latitude, longitude)); - } + if (latitude < -90.0f || latitude > 90.0f || + longitude < -180.0f || longitude > 180.0f) { + throw std::out_of_range(Utils::stringf("invalid latitude (%f) or longitude (%f) value", latitude, longitude)); + } - /* round down to nearest sampling resolution */ - int min_lat = static_cast(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES; - int min_lon = static_cast(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES; + /* round down to nearest sampling resolution */ + int min_lat = static_cast(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES; + int min_lon = static_cast(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES; - /* for the rare case of hitting the bounds exactly + /* for the rare case of hitting the bounds exactly * the rounding logic wouldn't fit, so enforce it. */ - /* limit to table bounds - required for maxima even when table spans full globe range */ - if (latitude <= MAG_SAMPLING_MIN_LAT) { - min_lat = MAG_SAMPLING_MIN_LAT; - } + /* limit to table bounds - required for maxima even when table spans full globe range */ + if (latitude <= MAG_SAMPLING_MIN_LAT) { + min_lat = MAG_SAMPLING_MIN_LAT; + } - if (latitude >= MAG_SAMPLING_MAX_LAT) { - min_lat = static_cast(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES; - } + if (latitude >= MAG_SAMPLING_MAX_LAT) { + min_lat = static_cast(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES; + } - if (longitude <= MAG_SAMPLING_MIN_LON) { - min_lon = MAG_SAMPLING_MIN_LON; - } + if (longitude <= MAG_SAMPLING_MIN_LON) { + min_lon = MAG_SAMPLING_MIN_LON; + } - if (longitude >= MAG_SAMPLING_MAX_LON) { - min_lon = static_cast(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES; - } + if (longitude >= MAG_SAMPLING_MAX_LON) { + min_lon = static_cast(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES; + } - /* find index of nearest low sampling point */ - int min_lat_index = (-(MAG_SAMPLING_MIN_LAT) + min_lat) / MAG_SAMPLING_RES; - int min_lon_index = (-(MAG_SAMPLING_MIN_LON) + min_lon) / MAG_SAMPLING_RES; + /* find index of nearest low sampling point */ + int min_lat_index = (-(MAG_SAMPLING_MIN_LAT) + min_lat) / MAG_SAMPLING_RES; + int min_lon_index = (-(MAG_SAMPLING_MIN_LON) + min_lon) / MAG_SAMPLING_RES; - real_T declination_sw = get_mag_lookup_table_val(min_lat_index, min_lon_index); - real_T declination_se = get_mag_lookup_table_val(min_lat_index, min_lon_index + 1); - real_T declination_ne = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index + 1); - real_T declination_nw = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index); + real_T declination_sw = get_mag_lookup_table_val(min_lat_index, min_lon_index); + real_T declination_se = get_mag_lookup_table_val(min_lat_index, min_lon_index + 1); + real_T declination_ne = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index + 1); + real_T declination_nw = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index); - /* perform bilinear interpolation on the four grid corners */ + /* perform bilinear interpolation on the four grid corners */ - real_T declination_min = ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; - real_T declination_max = ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; + real_T declination_min = ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; + real_T declination_max = ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; - return ((latitude - min_lat) / MAG_SAMPLING_RES) * (declination_max - declination_min) + declination_min; - } + return ((latitude - min_lat) / MAG_SAMPLING_RES) * (declination_max - declination_min) + declination_min; + } - //geopot_height = Earth_radius * altitude / (Earth_radius + altitude) /// all in kilometers - //temperature is in Kelvin = 273.15 + celcius - static real_T getStandardTemperature(real_T geopot_height) - { - //standard atmospheric pressure - //Below 51km: Practical Meteorology by Roland Stull, pg 12 - //Above 51km: http://www.braeunig.us/space/atmmodel.htm - if (geopot_height <= 11) //troposphere - return 288.15f - (6.5f * geopot_height); - else if (geopot_height <= 20) //Staroshere starts - return 216.65f; - else if (geopot_height <= 32) - return 196.65f + geopot_height; - else if (geopot_height <= 47) - return 228.65f + 2.8f * (geopot_height - 32); - else if (geopot_height <= 51) //Mesosphere starts - return 270.65f; - else if (geopot_height <= 71) - return 270.65f - 2.8f * (geopot_height - 51); - else if (geopot_height <= 84.85f) - return 214.65f - 2 * (geopot_height - 71); - else return 3; - //Thermospehere has high kinetic temperature (500c to 2000c) but temperature - //as measured by thermometer would be very low because of almost vacuum - //throw std::out_of_range("geopot_height must be less than 85km. Space domain is not supported yet!"); - } - - static real_T getGeopotential(real_T altitude_km) - { - static constexpr real_T radius_km = EARTH_RADIUS / 1000.0f; - return radius_km * altitude_km / (radius_km + altitude_km); - } + //geopot_height = Earth_radius * altitude / (Earth_radius + altitude) /// all in kilometers + //temperature is in Kelvin = 273.15 + celcius + static real_T getStandardTemperature(real_T geopot_height) + { + //standard atmospheric pressure + //Below 51km: Practical Meteorology by Roland Stull, pg 12 + //Above 51km: http://www.braeunig.us/space/atmmodel.htm + if (geopot_height <= 11) //troposphere + return 288.15f - (6.5f * geopot_height); + else if (geopot_height <= 20) //Staroshere starts + return 216.65f; + else if (geopot_height <= 32) + return 196.65f + geopot_height; + else if (geopot_height <= 47) + return 228.65f + 2.8f * (geopot_height - 32); + else if (geopot_height <= 51) //Mesosphere starts + return 270.65f; + else if (geopot_height <= 71) + return 270.65f - 2.8f * (geopot_height - 51); + else if (geopot_height <= 84.85f) + return 214.65f - 2 * (geopot_height - 71); + else + return 3; + //Thermospehere has high kinetic temperature (500c to 2000c) but temperature + //as measured by thermometer would be very low because of almost vacuum + //throw std::out_of_range("geopot_height must be less than 85km. Space domain is not supported yet!"); + } - static real_T getStandardPressure(real_T altitude /* meters */) //return Pa - { - real_T geopot_height = getGeopotential(altitude / 1000.0f); + static real_T getGeopotential(real_T altitude_km) + { + static constexpr real_T radius_km = EARTH_RADIUS / 1000.0f; + return radius_km * altitude_km / (radius_km + altitude_km); + } - real_T t = getStandardTemperature(geopot_height); + static real_T getStandardPressure(real_T altitude /* meters */) //return Pa + { + real_T geopot_height = getGeopotential(altitude / 1000.0f); - return getStandardPressure(geopot_height, t); - } + real_T t = getStandardTemperature(geopot_height); - static real_T getStandardPressure(real_T geopot_height, real_T std_temperature) //return Pa - { - //Below 51km: Practical Meteorology by Roland Stull, pg 12 - //Above 51km: http://www.braeunig.us/space/atmmodel.htm - //Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf - - //TODO: handle -ve altitude better (shouldn't grow indefinitely!) - - if (geopot_height <= 11) - //at alt 0, return sea level pressure - return SeaLevelPressure * powf(288.15f / std_temperature, -5.255877f); - else if (geopot_height <= 20) - return 22632.06f * expf(-0.1577f * (geopot_height - 11)); - else if (geopot_height <= 32) - return 5474.889f * powf(216.65f / std_temperature, 34.16319f); - else if (geopot_height <= 47) - return 868.0187f * powf(228.65f / std_temperature, 12.2011f); - else if (geopot_height <= 51) - return 110.9063f * exp(-0.1262f * (geopot_height - 47)); - else if (geopot_height <= 71) - return 66.93887f * powf(270.65f / std_temperature, -12.2011f); - else if (geopot_height <= 84.85f) - return 3.956420f * powf(214.65f / std_temperature, -17.0816f); - else return 1E-3f; - //throw std::out_of_range("altitude must be less than 86km. Space domain is not supported yet!"); - } - - static real_T getAirDensity(real_T std_pressure, real_T std_temperature) //kg / m^3 - { - //http://www.braeunig.us/space/atmmodel.htm - return std_pressure / 287.053f / std_temperature; - } + return getStandardPressure(geopot_height, t); + } - static real_T getAirDensity(real_T altitude) //kg / m^3 - { - real_T geo_pot = getGeopotential(altitude / 1000.0f); - real_T std_temperature = getStandardTemperature(geo_pot); - real_T std_pressure = getStandardPressure(geo_pot, std_temperature); - return getAirDensity(std_pressure, std_temperature); - } + static real_T getStandardPressure(real_T geopot_height, real_T std_temperature) //return Pa + { + //Below 51km: Practical Meteorology by Roland Stull, pg 12 + //Above 51km: http://www.braeunig.us/space/atmmodel.htm + //Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf + + //TODO: handle -ve altitude better (shouldn't grow indefinitely!) + + if (geopot_height <= 11) + //at alt 0, return sea level pressure + return SeaLevelPressure * powf(288.15f / std_temperature, -5.255877f); + else if (geopot_height <= 20) + return 22632.06f * expf(-0.1577f * (geopot_height - 11)); + else if (geopot_height <= 32) + return 5474.889f * powf(216.65f / std_temperature, 34.16319f); + else if (geopot_height <= 47) + return 868.0187f * powf(228.65f / std_temperature, 12.2011f); + else if (geopot_height <= 51) + return 110.9063f * exp(-0.1262f * (geopot_height - 47)); + else if (geopot_height <= 71) + return 66.93887f * powf(270.65f / std_temperature, -12.2011f); + else if (geopot_height <= 84.85f) + return 3.956420f * powf(214.65f / std_temperature, -17.0816f); + else + return 1E-3f; + //throw std::out_of_range("altitude must be less than 86km. Space domain is not supported yet!"); + } - static real_T getSpeedofSound(real_T altitude) // m/s - { - //http://www.braeunig.us/space/atmmodel.htm - return sqrt(1.400f * 287.053f * getStandardTemperature(getGeopotential(altitude))); - } + static real_T getAirDensity(real_T std_pressure, real_T std_temperature) //kg / m^3 + { + //http://www.braeunig.us/space/atmmodel.htm + return std_pressure / 287.053f / std_temperature; + } - static real_T getGravity(real_T altitude) - { - //derivation: http://www.citycollegiate.com/gravitation_XId.htm - if (altitude < 10000 && altitude > -10000) //up to 10 km, difference is too small - return EarthUtils::Gravity; - if (altitude < 100000 && altitude > -100000) //use first exproximation using binomial expansion - return EarthUtils::Gravity * (1 - 2 * altitude / EARTH_RADIUS); - else { - real_T factor = 1 + altitude / EARTH_RADIUS; - return EarthUtils::Gravity / factor / factor; + static real_T getAirDensity(real_T altitude) //kg / m^3 + { + real_T geo_pot = getGeopotential(altitude / 1000.0f); + real_T std_temperature = getStandardTemperature(geo_pot); + real_T std_pressure = getStandardPressure(geo_pot, std_temperature); + return getAirDensity(std_pressure, std_temperature); } - } - static Vector3r getMagField(const GeoPoint& geo_point) //return Tesla - { - double declination, inclination; - return getMagField(geo_point, declination, inclination); - } + static real_T getSpeedofSound(real_T altitude) // m/s + { + //http://www.braeunig.us/space/atmmodel.htm + return sqrt(1.400f * 287.053f * getStandardTemperature(getGeopotential(altitude))); + } - static Vector3r getMagField(const GeoPoint& geo_point, double& declination, double& inclination) //return Tesla - { - /* + static real_T getGravity(real_T altitude) + { + //derivation: http://www.citycollegiate.com/gravitation_XId.htm + if (altitude < 10000 && altitude > -10000) //up to 10 km, difference is too small + return EarthUtils::Gravity; + if (altitude < 100000 && altitude > -100000) //use first exproximation using binomial expansion + return EarthUtils::Gravity * (1 - 2 * altitude / EARTH_RADIUS); + else { + real_T factor = 1 + altitude / EARTH_RADIUS; + return EarthUtils::Gravity / factor / factor; + } + } + + static Vector3r getMagField(const GeoPoint& geo_point) //return Tesla + { + double declination, inclination; + return getMagField(geo_point, declination, inclination); + } + + static Vector3r getMagField(const GeoPoint& geo_point, double& declination, double& inclination) //return Tesla + { + /* We calculate magnetic field using simple dipol model of Earth, i.e., assume earth as perfect dipole sphere and ignoring all but first order terms. This obviously is inaccurate because of huge amount of irregularities, magnetic pole that is @@ -235,159 +239,159 @@ class EarthUtils { geo: 47.646 -122.134 -378 */ - //ref: The Earth's Magnetism: An Introduction for Geologists, Roberto Lanza, Antonio Meloni - //Sec 1.2.5, pg 27-30 https://goo.gl/bRm7wt - //some theory at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf - - double lat = Utils::degreesToRadians(geo_point.latitude); //geographic colatitude - double lon = Utils::degreesToRadians(geo_point.longitude); - double altitude = geo_point.altitude + EARTH_RADIUS; - - //cache value - double sin_MagPoleLat = sin(MagPoleLat); - double cos_MagPoleLat = cos(MagPoleLat); - double cos_lat = cos(lat); - double sin_lat = sin(lat); - - //find magnetic colatitude - double mag_clat = acos(cos_lat * cos_MagPoleLat + - sin_lat * sin_MagPoleLat * cos(lon - MagPoleLon) ); - - //calculation of magnetic longitude is not needed but just in case if someone wants it - //double mag_lon = asin( - // (sin(lon - MagPoleLon) * sin(lat)) / - // sin(mag_clat)); - - //field strength only depends on magnetic colatitude - //https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field - double cos_mag_clat = cos(mag_clat); - double field_mag = MeanMagField * pow(EARTH_RADIUS / altitude, 3) * - sqrt(1 + 3 * cos_mag_clat * cos_mag_clat); - - //find inclination and declination - //equation of declination in above referenced book is only partial - //full equation is (4a) at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf - double lat_test = sin_MagPoleLat * sin_lat; - double dec_factor = cos_MagPoleLat / sin(mag_clat); - if (cos_mag_clat > lat_test) - declination = asin(sin(lon - MagPoleLon) * dec_factor); - else - declination = asin(cos(lon - MagPoleLon) * dec_factor); - inclination = atan(2.0 / tan(mag_clat)); //do not use atan2 here - - //transform magnetic field vector to geographical coordinates - //ref: http://www.geo.mtu.edu/~jdiehl/magnotes.html - double field_xy = field_mag * cos(inclination); - return Vector3r( - static_cast(field_xy * cos(declination)), - static_cast(field_xy * sin(declination)), - static_cast(field_mag * sin(inclination)) - ); - } - - static GeoPoint nedToGeodetic(const Vector3r& v, const HomeGeoPoint& home_geo_point) - { - double x_rad = v.x() / EARTH_RADIUS; - double y_rad = v.y() / EARTH_RADIUS; - double c = sqrt(x_rad*x_rad + y_rad*y_rad); - double sin_c = sin(c), cos_c = cos(c); - double lat_rad, lon_rad; - if (!Utils::isApproximatelyZero(c)) { //avoids large changes? - lat_rad = asin(cos_c * home_geo_point.sin_lat + (x_rad * sin_c * home_geo_point.cos_lat) / c); - lon_rad = (home_geo_point.lon_rad + - atan2(y_rad * sin_c, c * home_geo_point.cos_lat * cos_c - x_rad * home_geo_point.sin_lat * sin_c)); - - return GeoPoint(Utils::radiansToDegrees(lat_rad), Utils::radiansToDegrees(lon_rad), - home_geo_point.home_geo_point.altitude - v.z()); - } else - return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude - v.z()); - } - - static Vector3r GeodeticToEcef(const GeoPoint& geo) - { - // Convert geodetic coordinates to ECEF. - // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22 - double lat_rad = Utils::degreesToRadians(geo.latitude); - double lon_rad = Utils::degreesToRadians(geo.longitude); - double xi = sqrt(1 - (6.69437999014 * 0.001) * sin(lat_rad) * sin(lat_rad)); - real_T x = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * cos(lon_rad)); - real_T y = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * sin(lon_rad)); - real_T z = static_cast((EARTH_RADIUS / xi * (1 - (6.69437999014 * 0.001)) + geo.altitude) * sin(lat_rad)); - return Vector3r(x, y, z); - } - - static Vector3r EcefToNed(const Vector3r& ecefxyz, const Vector3r& ecefhome, const GeoPoint& geohome) - { - // Converts ECEF coordinate position into local-tangent-plane NED. - // Coordinates relative to given ECEF coordinate frame. - - Vector3r vect, ret; - Matrix3x3r ecef_to_ned_matrix_; - double lat_rad = Utils::degreesToRadians(geohome.latitude); - double lon_rad = Utils::degreesToRadians(geohome.longitude); - vect = ecefxyz - ecefhome; - ecef_to_ned_matrix_(0, 0) = static_cast(-sin(lat_rad) * cos(lon_rad)); - ecef_to_ned_matrix_(0, 1) = static_cast(-sin(lat_rad) * sin(lon_rad)); - ecef_to_ned_matrix_(0, 2) = static_cast(cos(lat_rad)); - ecef_to_ned_matrix_(1, 0) = static_cast(-sin(lon_rad)); - ecef_to_ned_matrix_(1, 1) = static_cast(cos(lon_rad)); - ecef_to_ned_matrix_(1, 2) = 0.0f; - ecef_to_ned_matrix_(2, 0) = static_cast(cos(lat_rad) * cos(lon_rad)); - ecef_to_ned_matrix_(2, 1) = static_cast(cos(lat_rad) * sin(lon_rad)); - ecef_to_ned_matrix_(2, 2) = static_cast(sin(lat_rad)); - ret = ecef_to_ned_matrix_ * vect; - return Vector3r(ret(0), ret(1), -ret(2)); - } - - static Vector3r GeodeticToNed(const GeoPoint& geo, const GeoPoint& geohome) - { - return EcefToNed(GeodeticToEcef(geo), GeodeticToEcef(geohome), geohome); - } + //ref: The Earth's Magnetism: An Introduction for Geologists, Roberto Lanza, Antonio Meloni + //Sec 1.2.5, pg 27-30 https://goo.gl/bRm7wt + //some theory at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf + + double lat = Utils::degreesToRadians(geo_point.latitude); //geographic colatitude + double lon = Utils::degreesToRadians(geo_point.longitude); + double altitude = geo_point.altitude + EARTH_RADIUS; + + //cache value + double sin_MagPoleLat = sin(MagPoleLat); + double cos_MagPoleLat = cos(MagPoleLat); + double cos_lat = cos(lat); + double sin_lat = sin(lat); + + //find magnetic colatitude + double mag_clat = acos(cos_lat * cos_MagPoleLat + + sin_lat * sin_MagPoleLat * cos(lon - MagPoleLon)); + + //calculation of magnetic longitude is not needed but just in case if someone wants it + //double mag_lon = asin( + // (sin(lon - MagPoleLon) * sin(lat)) / + // sin(mag_clat)); + + //field strength only depends on magnetic colatitude + //https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field + double cos_mag_clat = cos(mag_clat); + double field_mag = MeanMagField * pow(EARTH_RADIUS / altitude, 3) * + sqrt(1 + 3 * cos_mag_clat * cos_mag_clat); + + //find inclination and declination + //equation of declination in above referenced book is only partial + //full equation is (4a) at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf + double lat_test = sin_MagPoleLat * sin_lat; + double dec_factor = cos_MagPoleLat / sin(mag_clat); + if (cos_mag_clat > lat_test) + declination = asin(sin(lon - MagPoleLon) * dec_factor); + else + declination = asin(cos(lon - MagPoleLon) * dec_factor); + inclination = atan(2.0 / tan(mag_clat)); //do not use atan2 here + + //transform magnetic field vector to geographical coordinates + //ref: http://www.geo.mtu.edu/~jdiehl/magnotes.html + double field_xy = field_mag * cos(inclination); + return Vector3r( + static_cast(field_xy * cos(declination)), + static_cast(field_xy * sin(declination)), + static_cast(field_mag * sin(inclination))); + } - //below are approximate versions and would produce errors of more than 10m for points farther than 1km - //for more accurate versions, please use the version in EarthUtils::nedToGeodetic - static Vector3r GeodeticToNedFast(const GeoPoint& geo, const GeoPoint& home) - { - double d_lat = geo.latitude - home.latitude; - double d_lon = geo.longitude - home.longitude; - real_T d_alt = static_cast(home.altitude - geo.altitude); - real_T x = static_cast(Utils::degreesToRadians(d_lat) * EARTH_RADIUS); - real_T y = static_cast(Utils::degreesToRadians(d_lon) * EARTH_RADIUS * cos(Utils::degreesToRadians(geo.latitude))); - return Vector3r(x, y, d_alt); - } - static GeoPoint nedToGeodeticFast(const Vector3r& local, const GeoPoint& home) - { - GeoPoint r; - double d_lat = local.x() / EARTH_RADIUS; - double d_lon = local.y() / (EARTH_RADIUS * cos(Utils::degreesToRadians(home.latitude))); - r.latitude = home.latitude + Utils::radiansToDegrees(d_lat); - r.longitude = home.longitude + Utils::radiansToDegrees(d_lon); - r.altitude = home.altitude - local.z(); - - return r; - } - -public: //consts - //ref: https://www.ngdc.noaa.gov/geomag/GeomagneticPoles.shtml - static constexpr double MagPoleLat = Utils::degreesToRadians(80.31f); - static constexpr double MagPoleLon = Utils::degreesToRadians(-72.62f); - static constexpr double MeanMagField = 3.12E-5; //Tesla, https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field - static constexpr float SeaLevelPressure = 101325.0f; //Pascal - static constexpr float SeaLevelAirDensity = 1.225f; //kg/m^3 - static constexpr float Gravity = 9.80665f; //m/s^2 - static constexpr float Radius = EARTH_RADIUS; //m - static constexpr float SpeedOfLight = 299792458.0f; //m - static constexpr float Obliquity = Utils::degreesToRadians(23.4397f); - static constexpr double Perihelion = Utils::degreesToRadians(102.9372); // perihelion of the Earth - static constexpr double DistanceFromSun = 149597870700.0; // meters - -private: - /* magnetic field */ - static float get_mag_lookup_table_val(int lat_index, int lon_index) - { - return static_cast(DECLINATION_TABLE[lat_index][lon_index]); - } -}; + static GeoPoint nedToGeodetic(const Vector3r& v, const HomeGeoPoint& home_geo_point) + { + double x_rad = v.x() / EARTH_RADIUS; + double y_rad = v.y() / EARTH_RADIUS; + double c = sqrt(x_rad * x_rad + y_rad * y_rad); + double sin_c = sin(c), cos_c = cos(c); + double lat_rad, lon_rad; + if (!Utils::isApproximatelyZero(c)) { //avoids large changes? + lat_rad = asin(cos_c * home_geo_point.sin_lat + (x_rad * sin_c * home_geo_point.cos_lat) / c); + lon_rad = (home_geo_point.lon_rad + + atan2(y_rad * sin_c, c * home_geo_point.cos_lat * cos_c - x_rad * home_geo_point.sin_lat * sin_c)); + + return GeoPoint(Utils::radiansToDegrees(lat_rad), Utils::radiansToDegrees(lon_rad), home_geo_point.home_geo_point.altitude - v.z()); + } + else + return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude - v.z()); + } + + static Vector3r GeodeticToEcef(const GeoPoint& geo) + { + // Convert geodetic coordinates to ECEF. + // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22 + double lat_rad = Utils::degreesToRadians(geo.latitude); + double lon_rad = Utils::degreesToRadians(geo.longitude); + double xi = sqrt(1 - (6.69437999014 * 0.001) * sin(lat_rad) * sin(lat_rad)); + real_T x = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * cos(lon_rad)); + real_T y = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * sin(lon_rad)); + real_T z = static_cast((EARTH_RADIUS / xi * (1 - (6.69437999014 * 0.001)) + geo.altitude) * sin(lat_rad)); + return Vector3r(x, y, z); + } + + static Vector3r EcefToNed(const Vector3r& ecefxyz, const Vector3r& ecefhome, const GeoPoint& geohome) + { + // Converts ECEF coordinate position into local-tangent-plane NED. + // Coordinates relative to given ECEF coordinate frame. + + Vector3r vect, ret; + Matrix3x3r ecef_to_ned_matrix_; + double lat_rad = Utils::degreesToRadians(geohome.latitude); + double lon_rad = Utils::degreesToRadians(geohome.longitude); + vect = ecefxyz - ecefhome; + ecef_to_ned_matrix_(0, 0) = static_cast(-sin(lat_rad) * cos(lon_rad)); + ecef_to_ned_matrix_(0, 1) = static_cast(-sin(lat_rad) * sin(lon_rad)); + ecef_to_ned_matrix_(0, 2) = static_cast(cos(lat_rad)); + ecef_to_ned_matrix_(1, 0) = static_cast(-sin(lon_rad)); + ecef_to_ned_matrix_(1, 1) = static_cast(cos(lon_rad)); + ecef_to_ned_matrix_(1, 2) = 0.0f; + ecef_to_ned_matrix_(2, 0) = static_cast(cos(lat_rad) * cos(lon_rad)); + ecef_to_ned_matrix_(2, 1) = static_cast(cos(lat_rad) * sin(lon_rad)); + ecef_to_ned_matrix_(2, 2) = static_cast(sin(lat_rad)); + ret = ecef_to_ned_matrix_ * vect; + return Vector3r(ret(0), ret(1), -ret(2)); + } + + static Vector3r GeodeticToNed(const GeoPoint& geo, const GeoPoint& geohome) + { + return EcefToNed(GeodeticToEcef(geo), GeodeticToEcef(geohome), geohome); + } + + //below are approximate versions and would produce errors of more than 10m for points farther than 1km + //for more accurate versions, please use the version in EarthUtils::nedToGeodetic + static Vector3r GeodeticToNedFast(const GeoPoint& geo, const GeoPoint& home) + { + double d_lat = geo.latitude - home.latitude; + double d_lon = geo.longitude - home.longitude; + real_T d_alt = static_cast(home.altitude - geo.altitude); + real_T x = static_cast(Utils::degreesToRadians(d_lat) * EARTH_RADIUS); + real_T y = static_cast(Utils::degreesToRadians(d_lon) * EARTH_RADIUS * cos(Utils::degreesToRadians(geo.latitude))); + return Vector3r(x, y, d_alt); + } + static GeoPoint nedToGeodeticFast(const Vector3r& local, const GeoPoint& home) + { + GeoPoint r; + double d_lat = local.x() / EARTH_RADIUS; + double d_lon = local.y() / (EARTH_RADIUS * cos(Utils::degreesToRadians(home.latitude))); + r.latitude = home.latitude + Utils::radiansToDegrees(d_lat); + r.longitude = home.longitude + Utils::radiansToDegrees(d_lon); + r.altitude = home.altitude - local.z(); + + return r; + } + + public: //consts + //ref: https://www.ngdc.noaa.gov/geomag/GeomagneticPoles.shtml + static constexpr double MagPoleLat = Utils::degreesToRadians(80.31f); + static constexpr double MagPoleLon = Utils::degreesToRadians(-72.62f); + static constexpr double MeanMagField = 3.12E-5; //Tesla, https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field + static constexpr float SeaLevelPressure = 101325.0f; //Pascal + static constexpr float SeaLevelAirDensity = 1.225f; //kg/m^3 + static constexpr float Gravity = 9.80665f; //m/s^2 + static constexpr float Radius = EARTH_RADIUS; //m + static constexpr float SpeedOfLight = 299792458.0f; //m + static constexpr float Obliquity = Utils::degreesToRadians(23.4397f); + static constexpr double Perihelion = Utils::degreesToRadians(102.9372); // perihelion of the Earth + static constexpr double DistanceFromSun = 149597870700.0; // meters + + private: + /* magnetic field */ + static float get_mag_lookup_table_val(int lat_index, int lon_index) + { + return static_cast(DECLINATION_TABLE[lat_index][lon_index]); + } + }; -}} //namespace +} +} //namespace #endif diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index 512c285847..4e29d2337e 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -17,333 +17,345 @@ using namespace msr::airlib; -namespace msr { namespace airlib { - -class MultirotorApiBase : public VehicleApiBase { - -protected: //must be implemented - - /************************* low level move APIs *********************************/ - virtual void commandMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm) = 0; - virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0; - virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0; - virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0; - virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0; - virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0; - virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 0; - virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; - virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; - virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; - - /************************* set Controller Gains APIs *********************************/ - virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) = 0; - - /************************* State APIs *********************************/ - virtual Kinematics::State getKinematicsEstimated() const = 0; - virtual LandedState getLandedState() const = 0; - virtual GeoPoint getGpsLocation() const = 0; - virtual const MultirotorApiParams& getMultirotorApiParams() const = 0; - - /************************* basic config APIs *********************************/ - virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds - virtual float getTakeoffZ() const = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). - //noise in difference of two position coordinates. This is not GPS or position accuracy which can be very low such as 1m. - //the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower. - virtual float getDistanceAccuracy() const = 0; - -protected: //optional overrides but recommended, default values may work - virtual float getAutoLookahead(float velocity, float adaptive_lookahead, - float max_factor = 40, float min_factor = 30) const; - virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const; - - //below methods gets called by default implementations of move-related commands that would use a long - //running loop. These can be used by derived classes to do some init/cleanup. - virtual void beforeTask() - { - //default is do nothing - } - virtual void afterTask() - { - //default is do nothing - } +namespace msr +{ +namespace airlib +{ -public: //optional overrides - virtual void moveByRC(const RCData& rc_data); - - //below method exist for any firmwares that may want to use ground truth for debugging purposes - virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) - { - unused(kinematics); - unused(environment); - } - - virtual void resetImplementation() override; - - -public: //these APIs uses above low level APIs - virtual ~MultirotorApiBase() = default; - - /************************* high level move APIs *********************************/ - //return value of these function is true if command was completed without interruption or timeouts - virtual bool takeoff(float timeout_sec); - virtual bool land(float timeout_sec); - virtual bool goHome(float timeout_sec); - - virtual bool moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); - virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); - virtual bool moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration); - virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); - virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); - virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); - virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration); - virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration); - virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration); - virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); - virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); - virtual bool moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, - float lookahead, float adaptive_lookahead); - virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); - virtual bool moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, - float lookahead, float adaptive_lookahead); - virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); - virtual bool rotateToYaw(float yaw, float timeout_sec, float margin); - virtual bool rotateByYawRate(float yaw_rate, float duration); - virtual bool hover(); - virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); - - /************************* set angle gain APIs *********************************/ - virtual void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd); - virtual void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd); - virtual void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd); - virtual void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd); - - /************************* Safety APIs *********************************/ - virtual void setSafetyEval(const shared_ptr safety_eval_ptr); - virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, - float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z); - - /************************* high level status APIs *********************************/ - MultirotorState getMultirotorState() const - { - MultirotorState state; - state.kinematics_estimated = getKinematicsEstimated(); - //TODO: add GPS health, accuracy in API - state.gps_location = getGpsLocation(); - state.timestamp = clock()->nowNanos(); - state.landed_state = getLandedState(); - state.rc_data = getRCData(); - state.ready = isReady(state.ready_message); - state.can_arm = canArm(); - return state; - } - - /******************* Task management Apis ********************/ - virtual void cancelLastTask() override - { - token_.cancel(); - } - -protected: //utility methods - typedef std::function WaitFunction; - - //*********************************safe wrapper around low level commands*************************************************** - virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z); - virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle); - virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle); - virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z); - virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z); - virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle); - virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); - virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); - virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); - - /************* safety checks & emergency maneuvers ************/ - virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); - virtual bool safetyCheckVelocity(const Vector3r& velocity); - virtual bool safetyCheckVelocityZ(float vx, float vy, float z); - virtual bool safetyCheckDestination(const Vector3r& dest_loc); - - /************* wait helpers ************/ - // helper function can wait for anything (as defined by the given function) up to the max_wait duration (in seconds). - // returns true if the wait function succeeded, or false if timeout occurred or the timeout is invalid. - Waiter waitForFunction(WaitFunction function, float max_wait); - - //useful for derived class to check after takeoff - bool waitForZ(float timeout_sec, float z, float margin); - - /************* other short hands ************/ - virtual Vector3r getPosition() const - { - return getKinematicsEstimated().pose.position; - } - virtual Vector3r getVelocity() const - { - return getKinematicsEstimated().twist.linear; - } - virtual Quaternionr getOrientation() const + class MultirotorApiBase : public VehicleApiBase { - return getKinematicsEstimated().pose.orientation; - } - CancelToken& getCancelToken() - { - return token_; - } - -public: //types - class UnsafeMoveException : public VehicleMoveException { - public: - const SafetyEval::EvalResult result; - - UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") - : VehicleMoveException(message), result(result_val) - {} - }; + protected: //must be implemented + /************************* low level move APIs *********************************/ + virtual void commandMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm) = 0; + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0; + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0; + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0; + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0; + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0; + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 0; + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; + + /************************* set Controller Gains APIs *********************************/ + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) = 0; + + /************************* State APIs *********************************/ + virtual Kinematics::State getKinematicsEstimated() const = 0; + virtual LandedState getLandedState() const = 0; + virtual GeoPoint getGpsLocation() const = 0; + virtual const MultirotorApiParams& getMultirotorApiParams() const = 0; + + /************************* basic config APIs *********************************/ + virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds + virtual float getTakeoffZ() const = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). + //noise in difference of two position coordinates. This is not GPS or position accuracy which can be very low such as 1m. + //the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower. + virtual float getDistanceAccuracy() const = 0; + + protected: //optional overrides but recommended, default values may work + virtual float getAutoLookahead(float velocity, float adaptive_lookahead, + float max_factor = 40, float min_factor = 30) const; + virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const; + + //below methods gets called by default implementations of move-related commands that would use a long + //running loop. These can be used by derived classes to do some init/cleanup. + virtual void beforeTask() + { + //default is do nothing + } + virtual void afterTask() + { + //default is do nothing + } -protected: //types - class SingleCall { - public: - SingleCall(MultirotorApiBase* api) - : api_(api) { - auto& token = api->getCancelToken(); - - //if we can't get lock, cancel previous call - if (!token.try_lock()) { - //TODO: should we worry about spurious failures in try_lock? - token.cancel(); - token.lock(); - } + public: //optional overrides + virtual void moveByRC(const RCData& rc_data); - if (isRootCall()) - token.reset(); - //else this is not the start of the call + //below method exist for any firmwares that may want to use ground truth for debugging purposes + virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) + { + unused(kinematics); + unused(environment); } - virtual ~SingleCall() + virtual void resetImplementation() override; + + public: //these APIs uses above low level APIs + virtual ~MultirotorApiBase() = default; + + /************************* high level move APIs *********************************/ + //return value of these function is true if command was completed without interruption or timeouts + virtual bool takeoff(float timeout_sec); + virtual bool land(float timeout_sec); + virtual bool goHome(float timeout_sec); + + virtual bool moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration); + virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); + virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); + virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); + virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration); + virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool rotateToYaw(float yaw, float timeout_sec, float margin); + virtual bool rotateByYawRate(float yaw_rate, float duration); + virtual bool hover(); + virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); + + /************************* set angle gain APIs *********************************/ + virtual void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd); + + /************************* Safety APIs *********************************/ + virtual void setSafetyEval(const shared_ptr safety_eval_ptr); + virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z); + + /************************* high level status APIs *********************************/ + MultirotorState getMultirotorState() const { - auto& token = api_->getCancelToken(); + MultirotorState state; + state.kinematics_estimated = getKinematicsEstimated(); + //TODO: add GPS health, accuracy in API + state.gps_location = getGpsLocation(); + state.timestamp = clock()->nowNanos(); + state.landed_state = getLandedState(); + state.rc_data = getRCData(); + state.ready = isReady(state.ready_message); + state.can_arm = canArm(); + return state; + } - if (isRootCall()) - token.reset(); - //else this is not the end of the call + /******************* Task management Apis ********************/ + virtual void cancelLastTask() override + { + token_.cancel(); + } - token.unlock(); + protected: //utility methods + typedef std::function WaitFunction; + + //*********************************safe wrapper around low level commands*************************************************** + virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z); + virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle); + virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle); + virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z); + virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z); + virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle); + virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); + virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); + virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); + + /************* safety checks & emergency maneuvers ************/ + virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); + virtual bool safetyCheckVelocity(const Vector3r& velocity); + virtual bool safetyCheckVelocityZ(float vx, float vy, float z); + virtual bool safetyCheckDestination(const Vector3r& dest_loc); + + /************* wait helpers ************/ + // helper function can wait for anything (as defined by the given function) up to the max_wait duration (in seconds). + // returns true if the wait function succeeded, or false if timeout occurred or the timeout is invalid. + Waiter waitForFunction(WaitFunction function, float max_wait); + + //useful for derived class to check after takeoff + bool waitForZ(float timeout_sec, float z, float margin); + + /************* other short hands ************/ + virtual Vector3r getPosition() const + { + return getKinematicsEstimated().pose.position; } - protected: - MultirotorApiBase * getVehicleApi() + virtual Vector3r getVelocity() const { - return api_; + return getKinematicsEstimated().twist.linear; } - - bool isRootCall() + virtual Quaternionr getOrientation() const { - return api_->getCancelToken().getRecursionCount() == 1; + return getKinematicsEstimated().pose.orientation; } - private: - MultirotorApiBase* api_; - }; - - class SingleTaskCall : public SingleCall - { - public: - SingleTaskCall(MultirotorApiBase* api) - : SingleCall(api) + CancelToken& getCancelToken() { - if (isRootCall()) - api->beforeTask(); + return token_; } - virtual ~SingleTaskCall() + public: //types + class UnsafeMoveException : public VehicleMoveException { - if (isRootCall()) - getVehicleApi()->afterTask(); - } - }; + public: + const SafetyEval::EvalResult result; + + UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") + : VehicleMoveException(message), result(result_val) + { + } + }; - //use this lock for vehicle status APIs - struct StatusLock { - //this const correctness gymnastic is required because most - //status update APIs are const - StatusLock(const MultirotorApiBase* api) - : lock_( - * const_cast(& api->status_mutex_) - ) + protected: //types + class SingleCall { - } + public: + SingleCall(MultirotorApiBase* api) + : api_(api) + { + auto& token = api->getCancelToken(); + + //if we can't get lock, cancel previous call + if (!token.try_lock()) { + //TODO: should we worry about spurious failures in try_lock? + token.cancel(); + token.lock(); + } + + if (isRootCall()) + token.reset(); + //else this is not the start of the call + } - private: - //we need mutable here because status APIs are const and shouldn't change data members - mutable std::lock_guard lock_; - }; + virtual ~SingleCall() + { + auto& token = api_->getCancelToken(); -private: //types - struct PathPosition { - uint seg_index; - float offset; - Vector3r position; - }; + if (isRootCall()) + token.reset(); + //else this is not the end of the call - struct PathSegment { - Vector3r seg_normalized; - Vector3r seg; - float seg_length; - float seg_velocity; - float start_z; - float seg_path_length; + token.unlock(); + } + + protected: + MultirotorApiBase* getVehicleApi() + { + return api_; + } - PathSegment(const Vector3r& start, const Vector3r& end, float velocity, float path_length) + bool isRootCall() + { + return api_->getCancelToken().getRecursionCount() == 1; + } + + private: + MultirotorApiBase* api_; + }; + + class SingleTaskCall : public SingleCall { - seg = end - start; - seg_length = seg.norm(); - seg_normalized = seg.normalized(); - start_z = start.z(); - seg_path_length = path_length; + public: + SingleTaskCall(MultirotorApiBase* api) + : SingleCall(api) + { + if (isRootCall()) + api->beforeTask(); + } - seg_velocity = velocity; - } - }; + virtual ~SingleTaskCall() + { + if (isRootCall()) + getVehicleApi()->afterTask(); + } + }; - //RAII - class ObsStrategyChanger { - private: - shared_ptr safety_eval_ptr_; - SafetyEval::ObsAvoidanceStrategy old_strategy_; - public: - ObsStrategyChanger(shared_ptr safety_eval_ptr, SafetyEval::ObsAvoidanceStrategy new_startegy) + //use this lock for vehicle status APIs + struct StatusLock { - safety_eval_ptr_ = safety_eval_ptr; - old_strategy_ = safety_eval_ptr_->getObsAvoidanceStrategy(); - safety_eval_ptr_->setObsAvoidanceStrategy(new_startegy); - } - ~ObsStrategyChanger() + //this const correctness gymnastic is required because most + //status update APIs are const + StatusLock(const MultirotorApiBase* api) + : lock_( + *const_cast(&api->status_mutex_)) + { + } + + private: + //we need mutable here because status APIs are const and shouldn't change data members + mutable std::lock_guard lock_; + }; + + private: //types + struct PathPosition { - safety_eval_ptr_->setObsAvoidanceStrategy(old_strategy_); - } + uint seg_index; + float offset; + Vector3r position; + }; + + struct PathSegment + { + Vector3r seg_normalized; + Vector3r seg; + float seg_length; + float seg_velocity; + float start_z; + float seg_path_length; + + PathSegment(const Vector3r& start, const Vector3r& end, float velocity, float path_length) + { + seg = end - start; + seg_length = seg.norm(); + seg_normalized = seg.normalized(); + start_z = start.z(); + seg_path_length = path_length; + + seg_velocity = velocity; + } + }; + + //RAII + class ObsStrategyChanger + { + private: + shared_ptr safety_eval_ptr_; + SafetyEval::ObsAvoidanceStrategy old_strategy_; + + public: + ObsStrategyChanger(shared_ptr safety_eval_ptr, SafetyEval::ObsAvoidanceStrategy new_startegy) + { + safety_eval_ptr_ = safety_eval_ptr; + old_strategy_ = safety_eval_ptr_->getObsAvoidanceStrategy(); + safety_eval_ptr_->setObsAvoidanceStrategy(new_startegy); + } + ~ObsStrategyChanger() + { + safety_eval_ptr_->setObsAvoidanceStrategy(old_strategy_); + } + }; + + private: //methods + float setNextPathPosition(const vector& path, const vector& path_segs, + const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc); + void adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode); + void adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode); + void moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z); + bool isYawWithinMargin(float yaw_target, float margin) const; + + private: //variables + CancelToken token_; + std::recursive_mutex status_mutex_; + RCData rc_data_trims_; + shared_ptr safety_eval_ptr_; + float obs_avoidance_vel_ = 0.5f; + + //TODO: make this configurable? + float landing_vel_ = 0.2f; //velocity to use for landing + float approx_zero_vel_ = 0.05f; + float approx_zero_angular_vel_ = 0.01f; }; -private: //methods - float setNextPathPosition(const vector& path, const vector& path_segs, - const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc); - void adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode); - void adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode); - void moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z); - bool isYawWithinMargin(float yaw_target, float margin) const; - -private: //variables - CancelToken token_; - std::recursive_mutex status_mutex_; - RCData rc_data_trims_; - shared_ptr safety_eval_ptr_; - float obs_avoidance_vel_ = 0.5f; - - //TODO: make this configurable? - float landing_vel_ = 0.2f; //velocity to use for landing - float approx_zero_vel_ = 0.05f; - float approx_zero_angular_vel_ = 0.01f; -}; - -}} //namespace +} +} //namespace #endif diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index 4fed449146..1883498bdc 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -12,66 +12,70 @@ #include "api/RpcLibClientBase.hpp" #include "vehicles/multirotor/api/MultirotorCommon.hpp" +namespace msr +{ +namespace airlib +{ -namespace msr { namespace airlib { + class MultirotorRpcLibClient : public RpcLibClientBase + { + public: + MultirotorRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); -class MultirotorRpcLibClient : public RpcLibClientBase { -public: - MultirotorRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); + MultirotorRpcLibClient* takeoffAsync(float timeout_sec = 20, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* takeoffAsync(float timeout_sec = 20, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveOnPathAsync(const vector& path, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveToZAsync(float z, float velocity, float timeout_sec = Utils::max(), + const YawMode& yaw_mode = YawMode(), float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + MultirotorRpcLibClient* rotateToYawAsync(float yaw, float timeout_sec = Utils::max(), float margin = 5, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* hoverAsync(const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec = Utils::max(), - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), - float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveOnPathAsync(const vector& path, float velocity, float timeout_sec = Utils::max(), - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), - float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec = Utils::max(), - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), - float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveToZAsync(float z, float velocity, float timeout_sec = Utils::max(), - const YawMode& yaw_mode = YawMode(), float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* rotateToYawAsync(float yaw, float timeout_sec = Utils::max(), float margin = 5, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* hoverAsync(const std::string& vehicle_name = ""); + void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void moveByRC(const RCData& rc_data, const std::string& vehicle_name = ""); - void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); - void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); - void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); - void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); - void moveByRC(const RCData& rc_data, const std::string& vehicle_name = ""); + MultirotorState getMultirotorState(const std::string& vehicle_name = ""); - MultirotorState getMultirotorState(const std::string& vehicle_name = ""); + bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name = ""); - bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, - float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name = ""); + virtual MultirotorRpcLibClient* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan()) override; - virtual MultirotorRpcLibClient* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan()) override; + virtual ~MultirotorRpcLibClient(); //required for pimpl - virtual ~MultirotorRpcLibClient(); //required for pimpl + private: + struct impl; + std::unique_ptr pimpl_; + }; -private: - struct impl; - std::unique_ptr pimpl_; -}; - -}} //namespace +} +} //namespace #endif diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 3974bc426b..d926fd9551 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -11,302 +11,332 @@ #include #include -namespace msr { namespace airlib { - -void MultirotorApiBase::resetImplementation() +namespace msr { - cancelLastTask(); - SingleTaskCall lock(this); //cancel previous tasks -} - -bool MultirotorApiBase::takeoff(float timeout_sec) +namespace airlib { - SingleTaskCall lock(this); - auto kinematics = getKinematicsEstimated(); - if (kinematics.twist.linear.norm() > approx_zero_vel_) { - throw VehicleMoveException(Utils::stringf( - "Cannot perform takeoff because vehicle is already moving with velocity %f m/s", - kinematics.twist.linear.norm())); + void MultirotorApiBase::resetImplementation() + { + cancelLastTask(); + SingleTaskCall lock(this); //cancel previous tasks } - bool ret = moveToPosition(kinematics.pose.position.x(), - kinematics.pose.position.y(), kinematics.pose.position.z() + getTakeoffZ(), - 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); + bool MultirotorApiBase::takeoff(float timeout_sec) + { + SingleTaskCall lock(this); - //last command is to hold on to position - //commandPosition(0, 0, getTakeoffZ(), YawMode::Zero()); + auto kinematics = getKinematicsEstimated(); + if (kinematics.twist.linear.norm() > approx_zero_vel_) { + throw VehicleMoveException(Utils::stringf( + "Cannot perform takeoff because vehicle is already moving with velocity %f m/s", + kinematics.twist.linear.norm())); + } - return ret; -} + bool ret = moveToPosition(kinematics.pose.position.x(), + kinematics.pose.position.y(), + kinematics.pose.position.z() + getTakeoffZ(), + 0.5f, + timeout_sec, + DrivetrainType::MaxDegreeOfFreedom, + YawMode::Zero(), + -1, + 1); -bool MultirotorApiBase::land(float timeout_sec) -{ - SingleTaskCall lock(this); + //last command is to hold on to position + //commandPosition(0, 0, getTakeoffZ(), YawMode::Zero()); - //after landing we detect if drone has stopped moving - int near_zero_vel_count = 0; + return ret; + } - return waitForFunction([&]() { - moveByVelocityInternal(0, 0, landing_vel_, YawMode::Zero()); + bool MultirotorApiBase::land(float timeout_sec) + { + SingleTaskCall lock(this); + + //after landing we detect if drone has stopped moving + int near_zero_vel_count = 0; + + return waitForFunction([&]() { + moveByVelocityInternal(0, 0, landing_vel_, YawMode::Zero()); + + float z_vel = getVelocity().z(); + if (z_vel <= approx_zero_vel_) + ++near_zero_vel_count; + else + near_zero_vel_count = 0; + + if (near_zero_vel_count > 10) + return true; + else { + moveByVelocityInternal(0, 0, landing_vel_, YawMode::Zero()); + return false; + } + }, + timeout_sec) + .isComplete(); + } - float z_vel = getVelocity().z(); - if (z_vel <= approx_zero_vel_) - ++near_zero_vel_count; - else - near_zero_vel_count = 0; + bool MultirotorApiBase::goHome(float timeout_sec) + { + SingleTaskCall lock(this); - if (near_zero_vel_count > 10) - return true; - else { - moveByVelocityInternal(0, 0, landing_vel_, YawMode::Zero()); - return false; - } - }, timeout_sec).isComplete(); -} + return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); + } -bool MultirotorApiBase::goHome(float timeout_sec) -{ - SingleTaskCall lock(this); + bool MultirotorApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); + float pitch, roll, yaw; + VectorMath::toEulerianAngle(getKinematicsEstimated().pose.orientation, pitch, roll, yaw); + float vx_new = (vx * (float)std::cos(yaw)) - (vy * (float)std::sin(yaw)); + float vy_new = (vx * (float)std::sin(yaw)) + (vy * (float)std::cos(yaw)); - return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); -} + if (duration <= 0) + return true; -bool MultirotorApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) -{ - SingleTaskCall lock(this); - float pitch, roll, yaw; - VectorMath::toEulerianAngle(getKinematicsEstimated().pose.orientation, pitch, roll, yaw); - float vx_new = (vx * (float)std::cos(yaw)) - (vy * (float)std::sin(yaw)); - float vy_new = (vx * (float)std::sin(yaw)) + (vy * (float)std::cos(yaw)); - - if (duration <= 0) - return true; - - YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); - adjustYaw(vx_new, vy_new, drivetrain, adj_yaw_mode); - - return waitForFunction([&]() { - moveByVelocityInternal(vx_new, vy_new, vz, adj_yaw_mode); - return false; //keep moving until timeout - }, duration).isTimeout(); -} -bool MultirotorApiBase::moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration) -{ - SingleTaskCall lock(this); + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adjustYaw(vx_new, vy_new, drivetrain, adj_yaw_mode); - if (duration <= 0) - return true; + return waitForFunction([&]() { + moveByVelocityInternal(vx_new, vy_new, vz, adj_yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + bool MultirotorApiBase::moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration) + { + SingleTaskCall lock(this); - return waitForFunction([&]() { - commandMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + if (duration <= 0) + return true; -bool MultirotorApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration) -{ - SingleTaskCall lock(this); + return waitForFunction([&]() { + commandMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } - if (duration <= 0) - return true; + bool MultirotorApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration) + { + SingleTaskCall lock(this); - return waitForFunction([&]() { - moveByRollPitchYawZInternal(roll, pitch, yaw, z); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + if (duration <= 0) + return true; -bool MultirotorApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration) -{ - SingleTaskCall lock(this); + return waitForFunction([&]() { + moveByRollPitchYawZInternal(roll, pitch, yaw, z); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } - if (duration <= 0) - return true; + bool MultirotorApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration) + { + SingleTaskCall lock(this); - return waitForFunction([&]() { - moveByRollPitchYawThrottleInternal(roll, pitch, yaw, throttle); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + if (duration <= 0) + return true; -bool MultirotorApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration) -{ - SingleTaskCall lock(this); + return waitForFunction([&]() { + moveByRollPitchYawThrottleInternal(roll, pitch, yaw, throttle); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } - if (duration <= 0) - return true; + bool MultirotorApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration) + { + SingleTaskCall lock(this); - return waitForFunction([&]() { - moveByRollPitchYawrateThrottleInternal(roll, pitch, yaw_rate, throttle); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + if (duration <= 0) + return true; -bool MultirotorApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration) -{ - SingleTaskCall lock(this); + return waitForFunction([&]() { + moveByRollPitchYawrateThrottleInternal(roll, pitch, yaw_rate, throttle); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } - if (duration <= 0) - return true; + bool MultirotorApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration) + { + SingleTaskCall lock(this); - return waitForFunction([&]() { - moveByRollPitchYawrateZInternal(roll, pitch, yaw_rate, z); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + if (duration <= 0) + return true; -bool MultirotorApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration) -{ - SingleTaskCall lock(this); + return waitForFunction([&]() { + moveByRollPitchYawrateZInternal(roll, pitch, yaw_rate, z); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } - if (duration <= 0) - return true; + bool MultirotorApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration) + { + SingleTaskCall lock(this); - return waitForFunction([&]() { - moveByAngleRatesZInternal(roll_rate, pitch_rate, yaw_rate, z); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + if (duration <= 0) + return true; -bool MultirotorApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration) -{ - SingleTaskCall lock(this); + return waitForFunction([&]() { + moveByAngleRatesZInternal(roll_rate, pitch_rate, yaw_rate, z); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } - if (duration <= 0) - return true; + bool MultirotorApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration) + { + SingleTaskCall lock(this); - return waitForFunction([&]() { - moveByAngleRatesThrottleInternal(roll_rate, pitch_rate, yaw_rate, throttle); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + if (duration <= 0) + return true; -bool MultirotorApiBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) -{ - SingleTaskCall lock(this); + return waitForFunction([&]() { + moveByAngleRatesThrottleInternal(roll_rate, pitch_rate, yaw_rate, throttle); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } - if (duration <= 0) - return true; + bool MultirotorApiBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); - YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); - adjustYaw(vx, vy, drivetrain, adj_yaw_mode); + if (duration <= 0) + return true; - return waitForFunction([&]() { - moveByVelocityInternal(vx, vy, vz, adj_yaw_mode); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adjustYaw(vx, vy, drivetrain, adj_yaw_mode); -bool MultirotorApiBase::moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) -{ - SingleTaskCall lock(this); - - if (duration <= 0) - return false; + return waitForFunction([&]() { + moveByVelocityInternal(vx, vy, vz, adj_yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } - YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); - adjustYaw(vx, vy, drivetrain, adj_yaw_mode); + bool MultirotorApiBase::moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); - return waitForFunction([&]() { - moveByVelocityZInternal(vx, vy, z, adj_yaw_mode); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + if (duration <= 0) + return false; -bool MultirotorApiBase::moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, - float lookahead, float adaptive_lookahead) -{ - SingleTaskCall lock(this); + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adjustYaw(vx, vy, drivetrain, adj_yaw_mode); - //validate path size - if (path.size() == 0) { - Utils::log("moveOnPath terminated because path has no points", Utils::kLogLevelWarn); - return true; + return waitForFunction([&]() { + moveByVelocityZInternal(vx, vy, z, adj_yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); } - //validate yaw mode - if (drivetrain == DrivetrainType::ForwardOnly && yaw_mode.is_rate) - throw std::invalid_argument("Yaw cannot be specified as rate if drivetrain is ForwardOnly"); + bool MultirotorApiBase::moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); - //validate and set auto-lookahead value - float command_period_dist = velocity * getCommandPeriod(); - if (lookahead == 0) - throw std::invalid_argument("lookahead distance cannot be 0"); //won't allow progress on path - else if (lookahead > 0) { - if (command_period_dist > lookahead) - throw std::invalid_argument(Utils::stringf("lookahead value %f is too small for velocity %f. It must be at least %f", lookahead, velocity, command_period_dist)); - if (getDistanceAccuracy() > lookahead) - throw std::invalid_argument(Utils::stringf("lookahead value %f is smaller than drone's distance accuracy %f.", lookahead, getDistanceAccuracy())); - } - else { - //if auto mode requested for lookahead then calculate based on velocity - lookahead = getAutoLookahead(velocity, adaptive_lookahead); - Utils::log(Utils::stringf("lookahead = %f, adaptive_lookahead = %f", lookahead, adaptive_lookahead)); - } - - //add current position as starting point - vector path3d; - vector path_segs; - path3d.push_back(getKinematicsEstimated().pose.position); - - Vector3r point; - float path_length = 0; - //append the input path and compute segments - for(uint i = 0; i < path.size(); ++i) { - point = path.at(i); - PathSegment path_seg(path3d.at(i), point, velocity, path_length); - path_length += path_seg.seg_length; - path_segs.push_back(path_seg); - path3d.push_back(point); - } - //add last segment as zero length segment so we have equal number of segments and points. - //path_segs[i] refers to segment that starts at point i - path_segs.push_back(PathSegment(point, point, velocity, path_length)); - - //when path ends, we want to slow down - float breaking_dist = 0; - if (velocity > getMultirotorApiParams().breaking_vel) { - breaking_dist = Utils::clip(velocity * getMultirotorApiParams().vel_to_breaking_dist, - getMultirotorApiParams().min_breaking_dist, getMultirotorApiParams().max_breaking_dist); - } - //else no need to change velocities for last segments - - //setup current position on path to 0 offset - PathPosition cur_path_loc, next_path_loc; - cur_path_loc.seg_index = 0; - cur_path_loc.offset = 0; - cur_path_loc.position = path3d[0]; - - float lookahead_error_increasing = 0; - float lookahead_error = 0; - Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); - - //initialize next path position - setNextPathPosition(path3d, path_segs, cur_path_loc, lookahead + lookahead_error, next_path_loc); - float overshoot = 0; - float goal_dist = 0; - - //until we are at the end of the path (last seg is always zero size) - while (!waiter.isTimeout() && (next_path_loc.seg_index < path_segs.size()-1 || goal_dist > 0) - ) { //current position is approximately at the last end point - - float seg_velocity = path_segs.at(next_path_loc.seg_index).seg_velocity; - float path_length_remaining = path_length - path_segs.at(cur_path_loc.seg_index).seg_path_length - cur_path_loc.offset; - if (seg_velocity > getMultirotorApiParams().min_vel_for_breaking && path_length_remaining <= breaking_dist) { - seg_velocity = getMultirotorApiParams().breaking_vel; - //Utils::logMessage("path_length_remaining = %f, Switched to breaking vel %f", path_length_remaining, seg_velocity); + //validate path size + if (path.size() == 0) { + Utils::log("moveOnPath terminated because path has no points", Utils::kLogLevelWarn); + return true; } - //send drone command to get to next lookahead - moveToPathPosition(next_path_loc.position, seg_velocity, drivetrain, - yaw_mode, path_segs.at(cur_path_loc.seg_index).start_z); + //validate yaw mode + if (drivetrain == DrivetrainType::ForwardOnly && yaw_mode.is_rate) + throw std::invalid_argument("Yaw cannot be specified as rate if drivetrain is ForwardOnly"); + + //validate and set auto-lookahead value + float command_period_dist = velocity * getCommandPeriod(); + if (lookahead == 0) + throw std::invalid_argument("lookahead distance cannot be 0"); //won't allow progress on path + else if (lookahead > 0) { + if (command_period_dist > lookahead) + throw std::invalid_argument(Utils::stringf("lookahead value %f is too small for velocity %f. It must be at least %f", lookahead, velocity, command_period_dist)); + if (getDistanceAccuracy() > lookahead) + throw std::invalid_argument(Utils::stringf("lookahead value %f is smaller than drone's distance accuracy %f.", lookahead, getDistanceAccuracy())); + } + else { + //if auto mode requested for lookahead then calculate based on velocity + lookahead = getAutoLookahead(velocity, adaptive_lookahead); + Utils::log(Utils::stringf("lookahead = %f, adaptive_lookahead = %f", lookahead, adaptive_lookahead)); + } - //sleep for rest of the cycle - if (!waiter.sleep()) - return false; + //add current position as starting point + vector path3d; + vector path_segs; + path3d.push_back(getKinematicsEstimated().pose.position); + + Vector3r point; + float path_length = 0; + //append the input path and compute segments + for (uint i = 0; i < path.size(); ++i) { + point = path.at(i); + PathSegment path_seg(path3d.at(i), point, velocity, path_length); + path_length += path_seg.seg_length; + path_segs.push_back(path_seg); + path3d.push_back(point); + } + //add last segment as zero length segment so we have equal number of segments and points. + //path_segs[i] refers to segment that starts at point i + path_segs.push_back(PathSegment(point, point, velocity, path_length)); + + //when path ends, we want to slow down + float breaking_dist = 0; + if (velocity > getMultirotorApiParams().breaking_vel) { + breaking_dist = Utils::clip(velocity * getMultirotorApiParams().vel_to_breaking_dist, + getMultirotorApiParams().min_breaking_dist, + getMultirotorApiParams().max_breaking_dist); + } + //else no need to change velocities for last segments + + //setup current position on path to 0 offset + PathPosition cur_path_loc, next_path_loc; + cur_path_loc.seg_index = 0; + cur_path_loc.offset = 0; + cur_path_loc.position = path3d[0]; + + float lookahead_error_increasing = 0; + float lookahead_error = 0; + Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); + + //initialize next path position + setNextPathPosition(path3d, path_segs, cur_path_loc, lookahead + lookahead_error, next_path_loc); + float overshoot = 0; + float goal_dist = 0; + + //until we are at the end of the path (last seg is always zero size) + while (!waiter.isTimeout() && (next_path_loc.seg_index < path_segs.size() - 1 || goal_dist > 0)) { //current position is approximately at the last end point + + float seg_velocity = path_segs.at(next_path_loc.seg_index).seg_velocity; + float path_length_remaining = path_length - path_segs.at(cur_path_loc.seg_index).seg_path_length - cur_path_loc.offset; + if (seg_velocity > getMultirotorApiParams().min_vel_for_breaking && path_length_remaining <= breaking_dist) { + seg_velocity = getMultirotorApiParams().breaking_vel; + //Utils::logMessage("path_length_remaining = %f, Switched to breaking vel %f", path_length_remaining, seg_velocity); + } - /* Below, P is previous position on path, N is next goal and C is our current position. + //send drone command to get to next lookahead + moveToPathPosition(next_path_loc.position, seg_velocity, drivetrain, yaw_mode, path_segs.at(cur_path_loc.seg_index).start_z); + + //sleep for rest of the cycle + if (!waiter.sleep()) + return false; + + /* Below, P is previous position on path, N is next goal and C is our current position. N ^ @@ -337,536 +367,542 @@ bool MultirotorApiBase::moveOnPath(const vector& path, float velocity, under no circumstances we should go back on the path (i.e. current pos on path can only move forward). */ - //how much have we moved towards last goal? - const Vector3r& goal_vect = next_path_loc.position - cur_path_loc.position; - - if (!goal_vect.isZero()) { //goal can only be zero if we are at the end of path - const Vector3r& actual_vect = getPosition() - cur_path_loc.position; - - //project actual vector on goal vector - const Vector3r& goal_normalized = goal_vect.normalized(); - goal_dist = actual_vect.dot(goal_normalized); //dist could be -ve if drone moves away from goal - - //if adaptive lookahead is enabled the calculate lookahead error (see above fig) - if (adaptive_lookahead) { - const Vector3r& actual_on_goal = goal_normalized * goal_dist; - float error = (actual_vect - actual_on_goal).norm() * adaptive_lookahead; - if (error > lookahead_error) { - lookahead_error_increasing++; - //TODO: below should be lower than 1E3 and configurable - //but lower values like 100 doesn't work for simple_flight + ScalableClock - if (lookahead_error_increasing > 1E5) { - throw std::runtime_error("lookahead error is continually increasing so we do not have safe control, aborting moveOnPath operation"); + //how much have we moved towards last goal? + const Vector3r& goal_vect = next_path_loc.position - cur_path_loc.position; + + if (!goal_vect.isZero()) { //goal can only be zero if we are at the end of path + const Vector3r& actual_vect = getPosition() - cur_path_loc.position; + + //project actual vector on goal vector + const Vector3r& goal_normalized = goal_vect.normalized(); + goal_dist = actual_vect.dot(goal_normalized); //dist could be -ve if drone moves away from goal + + //if adaptive lookahead is enabled the calculate lookahead error (see above fig) + if (adaptive_lookahead) { + const Vector3r& actual_on_goal = goal_normalized * goal_dist; + float error = (actual_vect - actual_on_goal).norm() * adaptive_lookahead; + if (error > lookahead_error) { + lookahead_error_increasing++; + //TODO: below should be lower than 1E3 and configurable + //but lower values like 100 doesn't work for simple_flight + ScalableClock + if (lookahead_error_increasing > 1E5) { + throw std::runtime_error("lookahead error is continually increasing so we do not have safe control, aborting moveOnPath operation"); + } } + else { + lookahead_error_increasing = 0; + } + lookahead_error = error; } - else { - lookahead_error_increasing = 0; - } - lookahead_error = error; } - } - else { - lookahead_error_increasing = 0; - goal_dist = 0; - lookahead_error = 0; //this is not really required because we will exit - waiter.complete(); - } + else { + lookahead_error_increasing = 0; + goal_dist = 0; + lookahead_error = 0; //this is not really required because we will exit + waiter.complete(); + } + + // Utils::logMessage("PF: cur=%s, goal_dist=%f, cur_path_loc=%s, next_path_loc=%s, lookahead_error=%f", + // VectorMath::toString(getPosition()).c_str(), goal_dist, VectorMath::toString(cur_path_loc.position).c_str(), + // VectorMath::toString(next_path_loc.position).c_str(), lookahead_error); + + //if drone moved backward, we don't want goal to move backward as well + //so only climb forward on the path, never back. Also note >= which means + //we climb path even if distance was 0 to take care of duplicated points on path + if (goal_dist >= 0) { + overshoot = setNextPathPosition(path3d, path_segs, cur_path_loc, goal_dist, cur_path_loc); + if (overshoot) + Utils::log(Utils::stringf("overshoot=%f", overshoot)); + } + //else + // Utils::logMessage("goal_dist was negative: %f", goal_dist); - // Utils::logMessage("PF: cur=%s, goal_dist=%f, cur_path_loc=%s, next_path_loc=%s, lookahead_error=%f", - // VectorMath::toString(getPosition()).c_str(), goal_dist, VectorMath::toString(cur_path_loc.position).c_str(), - // VectorMath::toString(next_path_loc.position).c_str(), lookahead_error); - - //if drone moved backward, we don't want goal to move backward as well - //so only climb forward on the path, never back. Also note >= which means - //we climb path even if distance was 0 to take care of duplicated points on path - if (goal_dist >= 0) { - overshoot = setNextPathPosition(path3d, path_segs, cur_path_loc, goal_dist, cur_path_loc); - if (overshoot) - Utils::log(Utils::stringf("overshoot=%f", overshoot)); + //compute next target on path + overshoot = setNextPathPosition(path3d, path_segs, cur_path_loc, lookahead + lookahead_error, next_path_loc); } - //else - // Utils::logMessage("goal_dist was negative: %f", goal_dist); - //compute next target on path - overshoot = setNextPathPosition(path3d, path_segs, cur_path_loc, lookahead + lookahead_error, next_path_loc); + return waiter.isComplete(); } - return waiter.isComplete(); -} - -bool MultirotorApiBase::moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) -{ - SingleTaskCall lock(this); - GeoPoint target; - target.latitude = latitude; - target.longitude = longitude; - target.altitude = altitude; - if (!std::isnan(getHomeGeoPoint().latitude) && !std::isnan(getHomeGeoPoint().longitude) && !std::isnan(getHomeGeoPoint().altitude)) { - vector path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGeoPoint()) }; - return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); - } - else { - vector path{ Vector3r(getPosition().x(),getPosition().y(),getPosition().z()) }; - return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + bool MultirotorApiBase::moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); + GeoPoint target; + target.latitude = latitude; + target.longitude = longitude; + target.altitude = altitude; + if (!std::isnan(getHomeGeoPoint().latitude) && !std::isnan(getHomeGeoPoint().longitude) && !std::isnan(getHomeGeoPoint().altitude)) { + vector path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGeoPoint()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } + else { + vector path{ Vector3r(getPosition().x(), getPosition().y(), getPosition().z()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } } -} -bool MultirotorApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) -{ - SingleTaskCall lock(this); + bool MultirotorApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); - vector path{ Vector3r(x, y, z) }; - return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); -} + vector path{ Vector3r(x, y, z) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } -bool MultirotorApiBase::moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, - float lookahead, float adaptive_lookahead) -{ - SingleTaskCall lock(this); + bool MultirotorApiBase::moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); - Vector2r cur_xy(getPosition().x(), getPosition().y()); - vector path { Vector3r(cur_xy.x(), cur_xy.y(), z) }; - return moveOnPath(path, velocity, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, yaw_mode, lookahead, adaptive_lookahead); -} + Vector2r cur_xy(getPosition().x(), getPosition().y()); + vector path{ Vector3r(cur_xy.x(), cur_xy.y(), z) }; + return moveOnPath(path, velocity, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, yaw_mode, lookahead, adaptive_lookahead); + } -bool MultirotorApiBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) -{ - SingleTaskCall lock(this); + bool MultirotorApiBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); - const float kMaxMessageAge = 0.1f /* 0.1 sec */, kMaxRCValue = 10000; + const float kMaxMessageAge = 0.1f /* 0.1 sec */, kMaxRCValue = 10000; - if (duration <= 0) - return true; + if (duration <= 0) + return true; - //freeze the quaternion - Quaternionr starting_quaternion = getKinematicsEstimated().pose.orientation; + //freeze the quaternion + Quaternionr starting_quaternion = getKinematicsEstimated().pose.orientation; - Waiter waiter(getCommandPeriod(), duration, getCancelToken()); - do { + Waiter waiter(getCommandPeriod(), duration, getCancelToken()); + do { - RCData rc_data = getRCData(); - TTimeDelta age = clock()->elapsedSince(rc_data.timestamp); - if (rc_data.is_valid && (rc_data.timestamp == 0 || age <= kMaxMessageAge)) { //if rc message timestamp is not set OR is not too old - if (rc_data_trims_.is_valid) - rc_data.subtract(rc_data_trims_); + RCData rc_data = getRCData(); + TTimeDelta age = clock()->elapsedSince(rc_data.timestamp); + if (rc_data.is_valid && (rc_data.timestamp == 0 || age <= kMaxMessageAge)) { //if rc message timestamp is not set OR is not too old + if (rc_data_trims_.is_valid) + rc_data.subtract(rc_data_trims_); - //convert RC commands to velocity vector - const Vector3r vel_word(rc_data.pitch * vy_max / kMaxRCValue, rc_data.roll * vx_max / kMaxRCValue, 0); - Vector3r vel_body = VectorMath::transformToBodyFrame(vel_word, starting_quaternion, true); + //convert RC commands to velocity vector + const Vector3r vel_word(rc_data.pitch * vy_max / kMaxRCValue, rc_data.roll * vx_max / kMaxRCValue, 0); + Vector3r vel_body = VectorMath::transformToBodyFrame(vel_word, starting_quaternion, true); - //find yaw as per terrain and remote setting - YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); - adj_yaw_mode.yaw_or_rate += rc_data.yaw * 100.0f / kMaxRCValue; - adjustYaw(vel_body, drivetrain, adj_yaw_mode); + //find yaw as per terrain and remote setting + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adj_yaw_mode.yaw_or_rate += rc_data.yaw * 100.0f / kMaxRCValue; + adjustYaw(vel_body, drivetrain, adj_yaw_mode); - //execute command - try { - float vz = (rc_data.throttle / kMaxRCValue) * z_min + getPosition().z(); - moveByVelocityZInternal(vel_body.x(), vel_body.y(), vz, adj_yaw_mode); - } - catch (const MultirotorApiBase::UnsafeMoveException& ex) { - Utils::log(Utils::stringf("Safety violation: %s", ex.result.message.c_str()), Utils::kLogLevelWarn); + //execute command + try { + float vz = (rc_data.throttle / kMaxRCValue) * z_min + getPosition().z(); + moveByVelocityZInternal(vel_body.x(), vel_body.y(), vz, adj_yaw_mode); + } + catch (const MultirotorApiBase::UnsafeMoveException& ex) { + Utils::log(Utils::stringf("Safety violation: %s", ex.result.message.c_str()), Utils::kLogLevelWarn); + } } - } - else - Utils::log(Utils::stringf("RCData had too old timestamp: %f", age)); + else + Utils::log(Utils::stringf("RCData had too old timestamp: %f", age)); - } while (waiter.sleep()); + } while (waiter.sleep()); - //if timeout occurred then command completed successfully otherwise it was interrupted - return waiter.isTimeout(); -} + //if timeout occurred then command completed successfully otherwise it was interrupted + return waiter.isTimeout(); + } -bool MultirotorApiBase::rotateToYaw(float yaw, float timeout_sec, float margin) -{ - SingleTaskCall lock(this); + bool MultirotorApiBase::rotateToYaw(float yaw, float timeout_sec, float margin) + { + SingleTaskCall lock(this); - if (timeout_sec <= 0) - return true; + if (timeout_sec <= 0) + return true; - auto start_pos = getPosition(); - float yaw_target = VectorMath::normalizeAngle(yaw); - YawMode move_yaw_mode(false, yaw_target); - YawMode stop_yaw_mode(true, 0); - - return waitForFunction([&]() { - if (isYawWithinMargin(yaw_target, margin)) { // yaw is within margin, then trying to stop rotation - moveToPositionInternal(start_pos, stop_yaw_mode); // let yaw rate be zero - auto yaw_rate = getKinematicsEstimated().twist.angular.z(); - if (abs(yaw_rate) <= approx_zero_angular_vel_) { // already sopped - return true; //stop all for stably achieving the goal - } - } - else { // yaw is not within margin, go on rotation - moveToPositionInternal(start_pos, move_yaw_mode); - } + auto start_pos = getPosition(); + float yaw_target = VectorMath::normalizeAngle(yaw); + YawMode move_yaw_mode(false, yaw_target); + YawMode stop_yaw_mode(true, 0); + + return waitForFunction([&]() { + if (isYawWithinMargin(yaw_target, margin)) { // yaw is within margin, then trying to stop rotation + moveToPositionInternal(start_pos, stop_yaw_mode); // let yaw rate be zero + auto yaw_rate = getKinematicsEstimated().twist.angular.z(); + if (abs(yaw_rate) <= approx_zero_angular_vel_) { // already sopped + return true; //stop all for stably achieving the goal + } + } + else { // yaw is not within margin, go on rotation + moveToPositionInternal(start_pos, move_yaw_mode); + } + + // yaw is not within margin + return false; //keep moving until timeout + }, + timeout_sec) + .isComplete(); + } - // yaw is not within margin - return false; //keep moving until timeout - }, timeout_sec).isComplete(); -} + bool MultirotorApiBase::rotateByYawRate(float yaw_rate, float duration) + { + SingleTaskCall lock(this); -bool MultirotorApiBase::rotateByYawRate(float yaw_rate, float duration) -{ - SingleTaskCall lock(this); + if (duration <= 0) + return true; - if (duration <= 0) - return true; + auto start_pos = getPosition(); + YawMode yaw_mode(true, yaw_rate); - auto start_pos = getPosition(); - YawMode yaw_mode(true, yaw_rate); - - return waitForFunction([&]() { - moveToPositionInternal(start_pos, yaw_mode); - return false; //keep moving until timeout - }, duration).isTimeout(); -} + return waitForFunction([&]() { + moveToPositionInternal(start_pos, yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } -void MultirotorApiBase::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd) -{ - uint8_t controller_type = 2; - setControllerGains(controller_type, kp, ki, kd); -} + void MultirotorApiBase::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd) + { + uint8_t controller_type = 2; + setControllerGains(controller_type, kp, ki, kd); + } -void MultirotorApiBase::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd) -{ - uint8_t controller_type = 3; - setControllerGains(controller_type, kp, ki, kd); -} + void MultirotorApiBase::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd) + { + uint8_t controller_type = 3; + setControllerGains(controller_type, kp, ki, kd); + } -void MultirotorApiBase::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd) -{ - uint8_t controller_type = 4; - setControllerGains(controller_type, kp, ki, kd); -} + void MultirotorApiBase::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd) + { + uint8_t controller_type = 4; + setControllerGains(controller_type, kp, ki, kd); + } -void MultirotorApiBase::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd) -{ - uint8_t controller_type = 5; - setControllerGains(controller_type, kp, ki, kd); -} + void MultirotorApiBase::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd) + { + uint8_t controller_type = 5; + setControllerGains(controller_type, kp, ki, kd); + } -bool MultirotorApiBase::hover() -{ - SingleTaskCall lock(this); + bool MultirotorApiBase::hover() + { + SingleTaskCall lock(this); - return moveToZ(getPosition().z(), 0.5f, Utils::max(), YawMode{ true,0 }, 1.0f, false); -} + return moveToZ(getPosition().z(), 0.5f, Utils::max(), YawMode{ true, 0 }, 1.0f, false); + } -void MultirotorApiBase::moveByRC(const RCData& rc_data) -{ - unused(rc_data); - //by default we say that this command is not supported - throw VehicleCommandNotImplementedException("moveByRC API is not implemented for this multirotor"); -} + void MultirotorApiBase::moveByRC(const RCData& rc_data) + { + unused(rc_data); + //by default we say that this command is not supported + throw VehicleCommandNotImplementedException("moveByRC API is not implemented for this multirotor"); + } -void MultirotorApiBase::moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode) -{ - if (safetyCheckVelocity(Vector3r(vx, vy, vz))) - commandVelocity(vx, vy, vz, yaw_mode); -} + void MultirotorApiBase::moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode) + { + if (safetyCheckVelocity(Vector3r(vx, vy, vz))) + commandVelocity(vx, vy, vz, yaw_mode); + } -void MultirotorApiBase::moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode) -{ - if (safetyCheckVelocityZ(vx, vy, z)) - commandVelocityZ(vx, vy, z, yaw_mode); -} + void MultirotorApiBase::moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode) + { + if (safetyCheckVelocityZ(vx, vy, z)) + commandVelocityZ(vx, vy, z, yaw_mode); + } -void MultirotorApiBase::moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode) -{ - if (safetyCheckDestination(dest)) - commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode); -} + void MultirotorApiBase::moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode) + { + if (safetyCheckDestination(dest)) + commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode); + } -void MultirotorApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z) -{ - if (safetyCheckVelocity(getVelocity())) - commandRollPitchYawZ(roll, pitch, yaw, z); -} + void MultirotorApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z) + { + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawZ(roll, pitch, yaw, z); + } -void MultirotorApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle) -{ - if (safetyCheckVelocity(getVelocity())) - commandRollPitchYawThrottle(roll, pitch, yaw, throttle); -} + void MultirotorApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle) + { + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawThrottle(roll, pitch, yaw, throttle); + } -void MultirotorApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle) -{ - if (safetyCheckVelocity(getVelocity())) - commandRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle); -} + void MultirotorApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle) + { + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle); + } -void MultirotorApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z) -{ - if (safetyCheckVelocity(getVelocity())) - commandRollPitchYawrateZ(roll, pitch, yaw_rate, z); -} + void MultirotorApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z) + { + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateZ(roll, pitch, yaw_rate, z); + } -void MultirotorApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z) -{ - if (safetyCheckVelocity(getVelocity())) - commandAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z); -} + void MultirotorApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z) + { + if (safetyCheckVelocity(getVelocity())) + commandAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z); + } -void MultirotorApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle) -{ - if (safetyCheckVelocity(getVelocity())) - commandAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle); -} + void MultirotorApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle) + { + if (safetyCheckVelocity(getVelocity())) + commandAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle); + } -//executes a given function until it returns true. Each execution is spaced apart at command period. -//return value is true if exit was due to given function returning true, otherwise false (due to timeout) -Waiter MultirotorApiBase::waitForFunction(WaitFunction function, float timeout_sec) -{ - Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); - if (timeout_sec <= 0) + //executes a given function until it returns true. Each execution is spaced apart at command period. + //return value is true if exit was due to given function returning true, otherwise false (due to timeout) + Waiter MultirotorApiBase::waitForFunction(WaitFunction function, float timeout_sec) + { + Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); + if (timeout_sec <= 0) + return waiter; + + do { + if (function()) { + waiter.complete(); + break; + } + } while (waiter.sleep()); return waiter; + } - do { - if (function()) { - waiter.complete(); - break; - } + bool MultirotorApiBase::waitForZ(float timeout_sec, float z, float margin) + { + float cur_z = 100000; + return waitForFunction([&]() { + cur_z = getPosition().z(); + return (std::abs(cur_z - z) <= margin); + }, + timeout_sec) + .isComplete(); } - while (waiter.sleep()); - return waiter; -} -bool MultirotorApiBase::waitForZ(float timeout_sec, float z, float margin) -{ - float cur_z = 100000; - return waitForFunction([&]() { - cur_z = getPosition().z(); - return (std::abs(cur_z - z) <= margin); - }, timeout_sec).isComplete(); -} + void MultirotorApiBase::setSafetyEval(const shared_ptr safety_eval_ptr) + { + SingleCall lock(this); + safety_eval_ptr_ = safety_eval_ptr; + } -void MultirotorApiBase::setSafetyEval(const shared_ptr safety_eval_ptr) -{ - SingleCall lock(this); - safety_eval_ptr_ = safety_eval_ptr; -} + RCData MultirotorApiBase::estimateRCTrims(float trimduration, float minCountForTrim, float maxTrim) + { + rc_data_trims_ = RCData(); -RCData MultirotorApiBase::estimateRCTrims(float trimduration, float minCountForTrim, float maxTrim) -{ - rc_data_trims_ = RCData(); + //get trims + Waiter waiter_trim(getCommandPeriod(), trimduration, getCancelToken()); + uint count = 0; + do { - //get trims - Waiter waiter_trim(getCommandPeriod(), trimduration, getCancelToken()); - uint count = 0; - do { + const RCData rc_data = getRCData(); + if (rc_data.is_valid) { + rc_data_trims_.add(rc_data); + count++; + } - const RCData rc_data = getRCData(); - if (rc_data.is_valid) { - rc_data_trims_.add(rc_data); - count++; - } + } while (waiter_trim.sleep()); - } while (waiter_trim.sleep()); + rc_data_trims_.is_valid = true; - rc_data_trims_.is_valid = true; + if (count < minCountForTrim) { + rc_data_trims_.is_valid = false; + Utils::log("Cannot compute RC trim because too few readings received"); + } + //take average + rc_data_trims_.divideBy(static_cast(count)); + if (rc_data_trims_.isAnyMoreThan(maxTrim)) { + rc_data_trims_.is_valid = false; + Utils::log(Utils::stringf("RC trims does not seem to be valid: %s", rc_data_trims_.toString().c_str())); + } - if (count < minCountForTrim) { - rc_data_trims_.is_valid = false; - Utils::log("Cannot compute RC trim because too few readings received"); - } + Utils::log(Utils::stringf("RCData Trims: %s", rc_data_trims_.toString().c_str())); - //take average - rc_data_trims_.divideBy(static_cast(count)); - if (rc_data_trims_.isAnyMoreThan(maxTrim)) { - rc_data_trims_.is_valid = false; - Utils::log(Utils::stringf("RC trims does not seem to be valid: %s", rc_data_trims_.toString().c_str())); + return rc_data_trims_; } - Utils::log(Utils::stringf("RCData Trims: %s", rc_data_trims_.toString().c_str())); - - return rc_data_trims_; -} + void MultirotorApiBase::moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z) + { + unused(last_z); + //validate dest + if (dest.hasNaN()) + throw std::invalid_argument(VectorMath::toString(dest, "dest vector cannot have NaN: ")); + + //what is the distance we will travel at this velocity? + float expected_dist = velocity * getCommandPeriod(); + + //get velocity vector + const Vector3r cur = getPosition(); + const Vector3r cur_dest = dest - cur; + float cur_dest_norm = cur_dest.norm(); + + //yaw for the direction of travel + adjustYaw(cur_dest, drivetrain, yaw_mode); + + //find velocity vector + Vector3r velocity_vect; + if (cur_dest_norm < getDistanceAccuracy()) //our dest is approximately same as current + velocity_vect = Vector3r::Zero(); + else if (cur_dest_norm >= expected_dist) { + velocity_vect = (cur_dest / cur_dest_norm) * velocity; + //Utils::logMessage("velocity_vect=%s", VectorMath::toString(velocity_vect).c_str()); + } + else { //cur dest is too close than the distance we would travel + //generate velocity vector that is same size as cur_dest_norm / command period + //this velocity vect when executed for command period would yield cur_dest_norm + Utils::log(Utils::stringf("Too close dest: cur_dest_norm=%f, expected_dist=%f", cur_dest_norm, expected_dist)); + velocity_vect = (cur_dest / cur_dest_norm) * (cur_dest_norm / getCommandPeriod()); + } -void MultirotorApiBase::moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z) -{ - unused(last_z); - //validate dest - if (dest.hasNaN()) - throw std::invalid_argument(VectorMath::toString(dest, "dest vector cannot have NaN: ")); - - //what is the distance we will travel at this velocity? - float expected_dist = velocity * getCommandPeriod(); - - //get velocity vector - const Vector3r cur = getPosition(); - const Vector3r cur_dest = dest - cur; - float cur_dest_norm = cur_dest.norm(); - - //yaw for the direction of travel - adjustYaw(cur_dest, drivetrain, yaw_mode); - - //find velocity vector - Vector3r velocity_vect; - if (cur_dest_norm < getDistanceAccuracy()) //our dest is approximately same as current - velocity_vect = Vector3r::Zero(); - else if (cur_dest_norm >= expected_dist) { - velocity_vect = (cur_dest / cur_dest_norm) * velocity; - //Utils::logMessage("velocity_vect=%s", VectorMath::toString(velocity_vect).c_str()); - } - else { //cur dest is too close than the distance we would travel - //generate velocity vector that is same size as cur_dest_norm / command period - //this velocity vect when executed for command period would yield cur_dest_norm - Utils::log(Utils::stringf("Too close dest: cur_dest_norm=%f, expected_dist=%f", cur_dest_norm, expected_dist)); - velocity_vect = (cur_dest / cur_dest_norm) * (cur_dest_norm / getCommandPeriod()); - } - - //send commands - //try to maintain altitude if path was in XY plan only, velocity based control is not as good - if (std::abs(cur.z() - dest.z()) <= getDistanceAccuracy()) //for paths in XY plan current code leaves z untouched, so we can compare with strict equality - moveByVelocityZInternal(velocity_vect.x(), velocity_vect.y(), dest.z(), yaw_mode); - else - moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode); -} + //send commands + //try to maintain altitude if path was in XY plan only, velocity based control is not as good + if (std::abs(cur.z() - dest.z()) <= getDistanceAccuracy()) //for paths in XY plan current code leaves z untouched, so we can compare with strict equality + moveByVelocityZInternal(velocity_vect.x(), velocity_vect.y(), dest.z(), yaw_mode); + else + moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode); + } -bool MultirotorApiBase::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, - float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z) -{ - if (safety_eval_ptr_ == nullptr) - throw std::invalid_argument("The setSafety call requires safety_eval_ptr_ to be set first"); + bool MultirotorApiBase::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z) + { + if (safety_eval_ptr_ == nullptr) + throw std::invalid_argument("The setSafety call requires safety_eval_ptr_ to be set first"); - //default strategy is for move. In hover mode we set new strategy temporarily - safety_eval_ptr_->setSafety(enable_reasons, obs_clearance, obs_startegy, origin, xy_length, max_z, min_z); + //default strategy is for move. In hover mode we set new strategy temporarily + safety_eval_ptr_->setSafety(enable_reasons, obs_clearance, obs_startegy, origin, xy_length, max_z, min_z); - obs_avoidance_vel_ = obs_avoidance_vel; - Utils::log(Utils::stringf("obs_avoidance_vel: %f", obs_avoidance_vel_)); + obs_avoidance_vel_ = obs_avoidance_vel; + Utils::log(Utils::stringf("obs_avoidance_vel: %f", obs_avoidance_vel_)); - return true; -} + return true; + } -bool MultirotorApiBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result) -{ - if (!result.is_safe) { - if (result.reason == SafetyEval::SafetyViolationType_::Obstacle) { - //are we supposed to do EM? - if (safety_eval_ptr_->getObsAvoidanceStrategy() != SafetyEval::ObsAvoidanceStrategy::RaiseException) { - //get suggested velocity vector - Vector3r avoidance_vel = getObsAvoidanceVelocity(result.cur_risk_dist, obs_avoidance_vel_) * result.suggested_vec; + bool MultirotorApiBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result) + { + if (!result.is_safe) { + if (result.reason == SafetyEval::SafetyViolationType_::Obstacle) { + //are we supposed to do EM? + if (safety_eval_ptr_->getObsAvoidanceStrategy() != SafetyEval::ObsAvoidanceStrategy::RaiseException) { + //get suggested velocity vector + Vector3r avoidance_vel = getObsAvoidanceVelocity(result.cur_risk_dist, obs_avoidance_vel_) * result.suggested_vec; - //use the unchecked command - commandVelocityZ(avoidance_vel.x(), avoidance_vel.y(), getPosition().z(), YawMode::Zero()); + //use the unchecked command + commandVelocityZ(avoidance_vel.x(), avoidance_vel.y(), getPosition().z(), YawMode::Zero()); - //tell caller not to execute planned command - return false; + //tell caller not to execute planned command + return false; + } + //other wise throw exception } - //other wise throw exception + //otherwise there is some other reason why we are in unsafe situation + //send last command to come to full stop + commandVelocity(0, 0, 0, YawMode::Zero()); + throw UnsafeMoveException(result); } - //otherwise there is some other reason why we are in unsafe situation - //send last command to come to full stop - commandVelocity(0, 0, 0, YawMode::Zero()); - throw UnsafeMoveException(result); + //else no unsafe situation + + return true; } - //else no unsafe situation - return true; -} + bool MultirotorApiBase::safetyCheckVelocity(const Vector3r& velocity) + { + if (safety_eval_ptr_ == nullptr) //safety checks disabled + return true; -bool MultirotorApiBase::safetyCheckVelocity(const Vector3r& velocity) -{ - if (safety_eval_ptr_ == nullptr) //safety checks disabled - return true; + const auto& result = safety_eval_ptr_->isSafeVelocity(getPosition(), velocity, getOrientation()); + return emergencyManeuverIfUnsafe(result); + } + bool MultirotorApiBase::safetyCheckVelocityZ(float vx, float vy, float z) + { + if (safety_eval_ptr_ == nullptr) //safety checks disabled + return true; - const auto& result = safety_eval_ptr_->isSafeVelocity(getPosition(), velocity, getOrientation()); - return emergencyManeuverIfUnsafe(result); -} -bool MultirotorApiBase::safetyCheckVelocityZ(float vx, float vy, float z) -{ - if (safety_eval_ptr_ == nullptr) //safety checks disabled - return true; + const auto& result = safety_eval_ptr_->isSafeVelocityZ(getPosition(), vx, vy, z, getOrientation()); + return emergencyManeuverIfUnsafe(result); + } + bool MultirotorApiBase::safetyCheckDestination(const Vector3r& dest_pos) + { + if (safety_eval_ptr_ == nullptr) //safety checks disabled + return true; - const auto& result = safety_eval_ptr_->isSafeVelocityZ(getPosition(), vx, vy, z, getOrientation()); - return emergencyManeuverIfUnsafe(result); -} -bool MultirotorApiBase::safetyCheckDestination(const Vector3r& dest_pos) -{ - if (safety_eval_ptr_ == nullptr) //safety checks disabled - return true; + const auto& result = safety_eval_ptr_->isSafeDestination(getPosition(), dest_pos, getOrientation()); + return emergencyManeuverIfUnsafe(result); + } - const auto& result = safety_eval_ptr_->isSafeDestination(getPosition(), dest_pos, getOrientation()); - return emergencyManeuverIfUnsafe(result); -} + float MultirotorApiBase::setNextPathPosition(const vector& path, const vector& path_segs, + const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc) + { + //note: cur_path_loc and next_path_loc may both point to same object + uint i = cur_path_loc.seg_index; + float offset = cur_path_loc.offset; + while (i < path.size() - 1) { + const PathSegment& seg = path_segs.at(i); + if (seg.seg_length > 0 && //protect against duplicate points in path, normalized seg will have NaN + seg.seg_length >= next_dist + offset) { + + next_path_loc.seg_index = i; + next_path_loc.offset = next_dist + offset; //how much total distance we will travel on this segment + next_path_loc.position = path.at(i) + seg.seg_normalized * next_path_loc.offset; + return 0; + } + //otherwise use up this segment, move on to next one + next_dist -= seg.seg_length - offset; + offset = 0; -float MultirotorApiBase::setNextPathPosition(const vector& path, const vector& path_segs, - const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc) -{ - //note: cur_path_loc and next_path_loc may both point to same object - uint i = cur_path_loc.seg_index; - float offset = cur_path_loc.offset; - while (i < path.size() - 1) { - const PathSegment& seg = path_segs.at(i); - if (seg.seg_length > 0 && //protect against duplicate points in path, normalized seg will have NaN - seg.seg_length >= next_dist + offset) { - - next_path_loc.seg_index = i; - next_path_loc.offset = next_dist + offset; //how much total distance we will travel on this segment - next_path_loc.position = path.at(i) + seg.seg_normalized * next_path_loc.offset; - return 0; - } - //otherwise use up this segment, move on to next one - next_dist -= seg.seg_length - offset; - offset = 0; + if (&cur_path_loc == &next_path_loc) + Utils::log(Utils::stringf("segment %d done: x=%f, y=%f, z=%f", i, path.at(i).x(), path.at(i).y(), path.at(i).z())); - if (&cur_path_loc == &next_path_loc) - Utils::log(Utils::stringf("segment %d done: x=%f, y=%f, z=%f", i, path.at(i).x(), path.at(i).y(), path.at(i).z())); + ++i; + } - ++i; + //if we are here then we ran out of segments + //consider last segment as zero length segment + next_path_loc.seg_index = i; + next_path_loc.offset = 0; + next_path_loc.position = path.at(i); + return next_dist; } - //if we are here then we ran out of segments - //consider last segment as zero length segment - next_path_loc.seg_index = i; - next_path_loc.offset = 0; - next_path_loc.position = path.at(i); - return next_dist; -} - -void MultirotorApiBase::adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode) -{ - //adjust yaw for the direction of travel in forward-only mode - if (drivetrain == DrivetrainType::ForwardOnly && !yaw_mode.is_rate) { - if (heading.norm() > getDistanceAccuracy()) { - yaw_mode.yaw_or_rate = yaw_mode.yaw_or_rate + (std::atan2(heading.y(), heading.x()) * 180 / M_PIf); - yaw_mode.yaw_or_rate = VectorMath::normalizeAngle(yaw_mode.yaw_or_rate); + void MultirotorApiBase::adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode) + { + //adjust yaw for the direction of travel in forward-only mode + if (drivetrain == DrivetrainType::ForwardOnly && !yaw_mode.is_rate) { + if (heading.norm() > getDistanceAccuracy()) { + yaw_mode.yaw_or_rate = yaw_mode.yaw_or_rate + (std::atan2(heading.y(), heading.x()) * 180 / M_PIf); + yaw_mode.yaw_or_rate = VectorMath::normalizeAngle(yaw_mode.yaw_or_rate); + } + else + yaw_mode.setZeroRate(); //don't change existing yaw if heading is too small because that can generate random result } - else - yaw_mode.setZeroRate(); //don't change existing yaw if heading is too small because that can generate random result + //else no adjustment needed } - //else no adjustment needed -} -void MultirotorApiBase::adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode) { - adjustYaw(Vector3r(x, y, 0), drivetrain, yaw_mode); -} + void MultirotorApiBase::adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode) + { + adjustYaw(Vector3r(x, y, 0), drivetrain, yaw_mode); + } -bool MultirotorApiBase::isYawWithinMargin(float yaw_target, float margin) const -{ - const float yaw_current = VectorMath::getYaw(getOrientation()) * 180 / M_PIf; - return std::abs(yaw_current - yaw_target) <= margin; -} + bool MultirotorApiBase::isYawWithinMargin(float yaw_target, float margin) const + { + const float yaw_current = VectorMath::getYaw(getOrientation()) * 180 / M_PIf; + return std::abs(yaw_current - yaw_target) <= margin; + } -float MultirotorApiBase::getAutoLookahead(float velocity, float adaptive_lookahead, - float max_factor, float min_factor) const -{ - //if auto mode requested for lookahead then calculate based on velocity - float command_period_dist = velocity * getCommandPeriod(); - float lookahead = command_period_dist * (adaptive_lookahead > 0 ? min_factor : max_factor); - lookahead = std::max(lookahead, getDistanceAccuracy()*1.5f); //50% more than distance accuracy - return lookahead; -} + float MultirotorApiBase::getAutoLookahead(float velocity, float adaptive_lookahead, + float max_factor, float min_factor) const + { + //if auto mode requested for lookahead then calculate based on velocity + float command_period_dist = velocity * getCommandPeriod(); + float lookahead = command_period_dist * (adaptive_lookahead > 0 ? min_factor : max_factor); + lookahead = std::max(lookahead, getDistanceAccuracy() * 1.5f); //50% more than distance accuracy + return lookahead; + } -float MultirotorApiBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const -{ - unused(risk_dist); - return max_obs_avoidance_vel; -} + float MultirotorApiBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const + { + unused(risk_dist); + return max_obs_avoidance_vel; + } -}} //namespace +} +} //namespace #endif diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 3f70987f53..0447a1afce 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -35,233 +35,225 @@ STRICT_MODE_OFF STRICT_MODE_ON #ifdef _MSC_VER -__pragma(warning( disable : 4239)) -#endif - - -namespace msr { namespace airlib { - - -typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators; - -struct MultirotorRpcLibClient::impl { -public: - std::future last_future; -}; - - -MultirotorRpcLibClient::MultirotorRpcLibClient(const string& ip_address, uint16_t port, float timeout_sec) - : RpcLibClientBase(ip_address, port, timeout_sec) -{ - pimpl_.reset(new impl()); -} - -MultirotorRpcLibClient::~MultirotorRpcLibClient() -{} - -MultirotorRpcLibClient* MultirotorRpcLibClient::takeoffAsync(float timeout_sec, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("takeoff", timeout_sec, vehicle_name); - return this; -} -MultirotorRpcLibClient* MultirotorRpcLibClient::landAsync(float timeout_sec, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("land", timeout_sec, vehicle_name); - return this; -} -MultirotorRpcLibClient* MultirotorRpcLibClient::goHomeAsync(float timeout_sec, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("goHome", timeout_sec, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, - DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityBodyFrame", vx, vy, vz, duration, - drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByMotorPWMs", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawZ", roll, pitch, yaw, z, duration, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawThrottle", roll, pitch, yaw, throttle, duration, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateThrottle", roll, pitch, yaw_rate, throttle, duration, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateZ", roll, pitch, yaw_rate, z, duration, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesZ", roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesThrottle", roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityAsync(float vx, float vy, float vz, float duration, - DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocity", vx, vy, vz, duration, - drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityZAsync(float vx, float vy, float z, float duration, - DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityZ", vx, vy, z, duration, - drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveOnPathAsync(const vector& path, float velocity, float duration, - DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -{ - vector conv_path; - MultirotorRpcLibAdapators::from(path, conv_path); - pimpl_->last_future = static_cast(getClient())->async_call("moveOnPath", conv_path, velocity, duration, - drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec, - DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("movetoGPS", latitude, longitude, altitude, velocity, timeout_sec, - drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec, - DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveToPosition", x, y, z, velocity, timeout_sec, - drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveToZAsync(float z, float velocity, float timeout_sec, const - YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveToZ", z, velocity, timeout_sec, - MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, - DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("moveByManual", vx_max, vy_max, z_min, duration, - drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::rotateToYawAsync(float yaw, float timeout_sec, float margin, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("rotateToYaw", yaw, timeout_sec, margin, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("rotateByYawRate", yaw_rate, duration, vehicle_name); - return this; -} - -MultirotorRpcLibClient* MultirotorRpcLibClient::hoverAsync(const std::string& vehicle_name) -{ - pimpl_->last_future = static_cast(getClient())->async_call("hover", vehicle_name); - return this; -} - -void MultirotorRpcLibClient::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -{ - static_cast(getClient())->call("setAngleLevelControllerGains", kp, ki, kd, vehicle_name); -} - -void MultirotorRpcLibClient::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -{ - static_cast(getClient())->call("setAngleRateControllerGains", kp, ki, kd, vehicle_name); -} - -void MultirotorRpcLibClient::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -{ - static_cast(getClient())->call("setVelocityControllerGains", kp, ki, kd, vehicle_name); -} - -void MultirotorRpcLibClient::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -{ - static_cast(getClient())->call("setPositionControllerGains", kp, ki, kd, vehicle_name); -} - -bool MultirotorRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, - float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) -{ - return static_cast(getClient())->call("setSafety", static_cast(enable_reasons), obs_clearance, obs_startegy, - obs_avoidance_vel, MultirotorRpcLibAdapators::Vector3r(origin), xy_length, max_z, min_z, vehicle_name).as(); -} - -//status getters -MultirotorState MultirotorRpcLibClient::getMultirotorState(const std::string& vehicle_name) -{ - return static_cast(getClient())->call("getMultirotorState", vehicle_name). - as().to(); -} +__pragma(warning(disable : 4239)) +#endif -void MultirotorRpcLibClient::moveByRC(const RCData& rc_data, const std::string& vehicle_name) + namespace msr { - static_cast(getClient())->call("moveByRC", MultirotorRpcLibAdapators::RCData(rc_data), vehicle_name); -} + namespace airlib + { + + typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators; + + struct MultirotorRpcLibClient::impl + { + public: + std::future last_future; + }; + + MultirotorRpcLibClient::MultirotorRpcLibClient(const string& ip_address, uint16_t port, float timeout_sec) + : RpcLibClientBase(ip_address, port, timeout_sec) + { + pimpl_.reset(new impl()); + } + + MultirotorRpcLibClient::~MultirotorRpcLibClient() + { + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::takeoffAsync(float timeout_sec, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("takeoff", timeout_sec, vehicle_name); + return this; + } + MultirotorRpcLibClient* MultirotorRpcLibClient::landAsync(float timeout_sec, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("land", timeout_sec, vehicle_name); + return this; + } + MultirotorRpcLibClient* MultirotorRpcLibClient::goHomeAsync(float timeout_sec, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("goHome", timeout_sec, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityBodyFrame", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByMotorPWMs", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawZ", roll, pitch, yaw, z, duration, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawThrottle", roll, pitch, yaw, throttle, duration, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateThrottle", roll, pitch, yaw_rate, throttle, duration, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateZ", roll, pitch, yaw_rate, z, duration, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesZ", roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesThrottle", roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocity", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityZAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityZ", vx, vy, z, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveOnPathAsync(const vector& path, float velocity, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + { + vector conv_path; + MultirotorRpcLibAdapators::from(path, conv_path); + pimpl_->last_future = static_cast(getClient())->async_call("moveOnPath", conv_path, velocity, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec, + DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("movetoGPS", latitude, longitude, altitude, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec, + DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveToPosition", x, y, z, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveToZAsync(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveToZ", z, velocity, timeout_sec, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByManual", vx_max, vy_max, z_min, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::rotateToYawAsync(float yaw, float timeout_sec, float margin, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("rotateToYaw", yaw, timeout_sec, margin, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("rotateByYawRate", yaw_rate, duration, vehicle_name); + return this; + } + + MultirotorRpcLibClient* MultirotorRpcLibClient::hoverAsync(const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("hover", vehicle_name); + return this; + } + + void MultirotorRpcLibClient::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + { + static_cast(getClient())->call("setAngleLevelControllerGains", kp, ki, kd, vehicle_name); + } + + void MultirotorRpcLibClient::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + { + static_cast(getClient())->call("setAngleRateControllerGains", kp, ki, kd, vehicle_name); + } + + void MultirotorRpcLibClient::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + { + static_cast(getClient())->call("setVelocityControllerGains", kp, ki, kd, vehicle_name); + } + + void MultirotorRpcLibClient::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + { + static_cast(getClient())->call("setPositionControllerGains", kp, ki, kd, vehicle_name); + } + + bool MultirotorRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) + { + return static_cast(getClient())->call("setSafety", static_cast(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, MultirotorRpcLibAdapators::Vector3r(origin), xy_length, max_z, min_z, vehicle_name).as(); + } + + //status getters + MultirotorState MultirotorRpcLibClient::getMultirotorState(const std::string& vehicle_name) + { + return static_cast(getClient())->call("getMultirotorState", vehicle_name).as().to(); + } + + void MultirotorRpcLibClient::moveByRC(const RCData& rc_data, const std::string& vehicle_name) + { + static_cast(getClient())->call("moveByRC", MultirotorRpcLibAdapators::RCData(rc_data), vehicle_name); + } + + //return value of last task. It should be true if task completed without + //cancellation or timeout + MultirotorRpcLibClient* MultirotorRpcLibClient::waitOnLastTask(bool* task_result, float timeout_sec) + { + bool result; + if (std::isnan(timeout_sec) || timeout_sec == Utils::max()) + result = pimpl_->last_future.get().as(); + else { + auto future_status = pimpl_->last_future.wait_for(std::chrono::duration(timeout_sec)); + if (future_status == std::future_status::ready) + result = pimpl_->last_future.get().as(); + else + result = false; + } + + if (task_result) + *task_result = result; + + return this; + } -//return value of last task. It should be true if task completed without -//cancellation or timeout -MultirotorRpcLibClient* MultirotorRpcLibClient::waitOnLastTask(bool* task_result, float timeout_sec) -{ - bool result; - if (std::isnan(timeout_sec) || timeout_sec == Utils::max()) - result = pimpl_->last_future.get().as(); - else { - auto future_status = pimpl_->last_future.wait_for(std::chrono::duration(timeout_sec)); - if (future_status == std::future_status::ready) - result = pimpl_->last_future.get().as(); - else - result = false; } - - if (task_result) - *task_result = result; - - return this; -} - -}} //namespace +} //namespace #endif #endif diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index e8b62cf78c..493762ccf5 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -9,7 +9,6 @@ #include "vehicles/multirotor/api/MultirotorRpcLibServer.hpp" - #include "common/Common.hpp" STRICT_MODE_OFF @@ -33,158 +32,115 @@ STRICT_MODE_OFF STRICT_MODE_ON +namespace msr +{ +namespace airlib +{ -namespace msr { namespace airlib { - -typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators; + typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators; -MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port) + MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port) : RpcLibServerBase(api_provider, server_address, port) -{ - (static_cast(getServer()))-> - bind("takeoff", [&](float timeout_sec, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->takeoff(timeout_sec); - }); - (static_cast(getServer()))-> - bind("land", [&](float timeout_sec, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->land(timeout_sec); - }); - (static_cast(getServer()))-> - bind("goHome", [&](float timeout_sec, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->goHome(timeout_sec); - }); - (static_cast(getServer()))-> - bind("moveByVelocityBodyFrame", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, - const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByVelocityBodyFrame(vx, vy, vz, duration, drivetrain, yaw_mode.to()); - }); - (static_cast(getServer()))-> - bind("moveByMotorPWMs", [&](float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) -> - bool { return getVehicleApi(vehicle_name)->moveByMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration); - }); - (static_cast(getServer()))-> - bind("moveByRollPitchYawZ", [&](float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) -> - bool { return getVehicleApi(vehicle_name)->moveByRollPitchYawZ(roll, pitch, yaw, z, duration); - }); - (static_cast(getServer()))-> - bind("moveByRollPitchYawThrottle", [&](float roll, float pitch, float yaw, float throttle, float duration, - const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByRollPitchYawThrottle(roll, pitch, yaw, throttle, duration); - }); - (static_cast(getServer()))-> - bind("moveByRollPitchYawrateThrottle", [&](float roll, float pitch, float yaw_rate, float throttle, float duration, - const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle, duration); - }); - (static_cast(getServer()))-> - bind("moveByRollPitchYawrateZ", [&](float roll, float pitch, float yaw_rate, float z, float duration, - const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByRollPitchYawrateZ(roll, pitch, yaw_rate, z, duration); - }); - (static_cast(getServer()))-> - bind("moveByAngleRatesZ", [&](float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, - const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z, duration); - }); - (static_cast(getServer()))-> - bind("moveByAngleRatesThrottle", [&](float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, - const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle, duration); - }); - (static_cast(getServer()))-> - bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, - const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByVelocity(vx, vy, vz, duration, drivetrain, yaw_mode.to()); - }); - (static_cast(getServer()))-> - bind("moveByVelocityZ", [&](float vx, float vy, float z, float duration, DrivetrainType drivetrain, - const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByVelocityZ(vx, vy, z, duration, drivetrain, yaw_mode.to()); - }); - (static_cast(getServer()))-> - bind("moveOnPath", [&](const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, - float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + { + (static_cast(getServer()))->bind("takeoff", [&](float timeout_sec, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->takeoff(timeout_sec); + }); + (static_cast(getServer()))->bind("land", [&](float timeout_sec, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->land(timeout_sec); + }); + (static_cast(getServer()))->bind("goHome", [&](float timeout_sec, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->goHome(timeout_sec); + }); + (static_cast(getServer()))->bind("moveByVelocityBodyFrame", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByVelocityBodyFrame(vx, vy, vz, duration, drivetrain, yaw_mode.to()); + }); + (static_cast(getServer()))->bind("moveByMotorPWMs", [&](float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration); + }); + (static_cast(getServer()))->bind("moveByRollPitchYawZ", [&](float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawZ(roll, pitch, yaw, z, duration); + }); + (static_cast(getServer()))->bind("moveByRollPitchYawThrottle", [&](float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawThrottle(roll, pitch, yaw, throttle, duration); + }); + (static_cast(getServer()))->bind("moveByRollPitchYawrateThrottle", [&](float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle, duration); + }); + (static_cast(getServer()))->bind("moveByRollPitchYawrateZ", [&](float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateZ(roll, pitch, yaw_rate, z, duration); + }); + (static_cast(getServer()))->bind("moveByAngleRatesZ", [&](float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z, duration); + }); + (static_cast(getServer()))->bind("moveByAngleRatesThrottle", [&](float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle, duration); + }); + (static_cast(getServer()))->bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByVelocity(vx, vy, vz, duration, drivetrain, yaw_mode.to()); + }); + (static_cast(getServer()))->bind("moveByVelocityZ", [&](float vx, float vy, float z, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByVelocityZ(vx, vy, z, duration, drivetrain, yaw_mode.to()); + }); + (static_cast(getServer()))->bind("moveOnPath", [&](const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { vector conv_path; MultirotorRpcLibAdapators::to(path, conv_path); return getVehicleApi(vehicle_name)->moveOnPath(conv_path, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); }); - (static_cast(getServer()))-> - bind("moveToGPS", [&](float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, - const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveToGPS(latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); - }); - (static_cast(getServer()))-> - bind("moveToPosition", [&](float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, - const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveToPosition(x, y, z, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); - }); - (static_cast(getServer()))-> - bind("moveToZ", [&](float z, float velocity, float timeout_sec, const MultirotorRpcLibAdapators::YawMode& yaw_mode, - float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveToZ(z, velocity, timeout_sec, yaw_mode.to(), lookahead, adaptive_lookahead); - }); - (static_cast(getServer()))-> - bind("moveByManual", [&](float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, - const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByManual(vx_max, vy_max, z_min, duration, drivetrain, yaw_mode.to()); - }); - - (static_cast(getServer()))-> - bind("rotateToYaw", [&](float yaw, float timeout_sec, float margin, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->rotateToYaw(yaw, timeout_sec, margin); - }); - (static_cast(getServer()))-> - bind("rotateByYawRate", [&](float yaw_rate, float duration, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->rotateByYawRate(yaw_rate, duration); - }); - (static_cast(getServer()))-> - bind("hover", [&](const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->hover(); - }); - (static_cast(getServer()))-> - bind("setAngleLevelControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + (static_cast(getServer()))->bind("moveToGPS", [&](float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveToGPS(latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); + }); + (static_cast(getServer()))->bind("moveToPosition", [&](float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveToPosition(x, y, z, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); + }); + (static_cast(getServer()))->bind("moveToZ", [&](float z, float velocity, float timeout_sec, const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveToZ(z, velocity, timeout_sec, yaw_mode.to(), lookahead, adaptive_lookahead); + }); + (static_cast(getServer()))->bind("moveByManual", [&](float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByManual(vx_max, vy_max, z_min, duration, drivetrain, yaw_mode.to()); + }); + + (static_cast(getServer()))->bind("rotateToYaw", [&](float yaw, float timeout_sec, float margin, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->rotateToYaw(yaw, timeout_sec, margin); + }); + (static_cast(getServer()))->bind("rotateByYawRate", [&](float yaw_rate, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->rotateByYawRate(yaw_rate, duration); + }); + (static_cast(getServer()))->bind("hover", [&](const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->hover(); + }); + (static_cast(getServer()))->bind("setAngleLevelControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->setAngleLevelControllerGains(kp, ki, kd); - }); - (static_cast(getServer()))-> - bind("setAngleRateControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + }); + (static_cast(getServer()))->bind("setAngleRateControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->setAngleRateControllerGains(kp, ki, kd); - }); - (static_cast(getServer()))-> - bind("setVelocityControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + }); + (static_cast(getServer()))->bind("setVelocityControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->setVelocityControllerGains(kp, ki, kd); - }); - (static_cast(getServer()))-> - bind("setPositionControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + }); + (static_cast(getServer()))->bind("setPositionControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->setPositionControllerGains(kp, ki, kd); - }); - (static_cast(getServer()))-> - bind("moveByRC", [&](const MultirotorRpcLibAdapators::RCData& data, const std::string& vehicle_name) -> void { - getVehicleApi(vehicle_name)->moveByRC(data.to()); - }); - - (static_cast(getServer()))-> - bind("setSafety", [&](uint enable_reasons, float obs_clearance, const SafetyEval::ObsAvoidanceStrategy& obs_startegy, - float obs_avoidance_vel, const MultirotorRpcLibAdapators::Vector3r& origin, float xy_length, - float max_z, float min_z, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->setSafety(SafetyEval::SafetyViolationType(enable_reasons), obs_clearance, obs_startegy, - obs_avoidance_vel, origin.to(), xy_length, max_z, min_z); - }); - - //getters - (static_cast(getServer()))-> - bind("getMultirotorState", [&](const std::string& vehicle_name) -> MultirotorRpcLibAdapators::MultirotorState { - return MultirotorRpcLibAdapators::MultirotorState(getVehicleApi(vehicle_name)->getMultirotorState()); - }); -} + }); + (static_cast(getServer()))->bind("moveByRC", [&](const MultirotorRpcLibAdapators::RCData& data, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->moveByRC(data.to()); + }); -//required for pimpl -MultirotorRpcLibServer::~MultirotorRpcLibServer() -{ -} + (static_cast(getServer()))->bind("setSafety", [&](uint enable_reasons, float obs_clearance, const SafetyEval::ObsAvoidanceStrategy& obs_startegy, float obs_avoidance_vel, const MultirotorRpcLibAdapators::Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->setSafety(SafetyEval::SafetyViolationType(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, origin.to(), xy_length, max_z, min_z); + }); + //getters + (static_cast(getServer()))->bind("getMultirotorState", [&](const std::string& vehicle_name) -> MultirotorRpcLibAdapators::MultirotorState { + return MultirotorRpcLibAdapators::MultirotorState(getVehicleApi(vehicle_name)->getMultirotorState()); + }); + } -}} //namespace + //required for pimpl + MultirotorRpcLibServer::~MultirotorRpcLibServer() + { + } +} +} //namespace #endif #endif From 0645a7752c8444a17c5dec81a9549f3af0bad1f9 Mon Sep 17 00:00:00 2001 From: zimmy87 Date: Thu, 2 Dec 2021 04:23:41 -0800 Subject: [PATCH 5/5] Update EarthUtils.hpp removing extra white space accidentally added during merge --- AirLib/include/common/EarthUtils.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/AirLib/include/common/EarthUtils.hpp b/AirLib/include/common/EarthUtils.hpp index 798efc36b3..45d595f846 100644 --- a/AirLib/include/common/EarthUtils.hpp +++ b/AirLib/include/common/EarthUtils.hpp @@ -391,7 +391,6 @@ namespace airlib return static_cast(DECLINATION_TABLE[lat_index][lon_index]); } }; - } } //namespace #endif