diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj
index 75b853bfb5..4dd04fc7ee 100644
--- a/AirLib/AirLib.vcxproj
+++ b/AirLib/AirLib.vcxproj
@@ -27,10 +27,12 @@
-
+
+
+
@@ -53,7 +55,7 @@
-
+
@@ -71,13 +73,14 @@
-
+
+
-
+
@@ -137,20 +140,17 @@
-
-
-
+
-
+
-
+
-
@@ -174,7 +174,6 @@
-
@@ -186,7 +185,7 @@
-
+
diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters
index c4231bfeaa..65aa0bb0d1 100644
--- a/AirLib/AirLib.vcxproj.filters
+++ b/AirLib/AirLib.vcxproj.filters
@@ -165,15 +165,6 @@
Header Files
-
- Header Files
-
-
- Header Files
-
-
- Header Files
-
Header Files
@@ -204,9 +195,6 @@
Header Files
-
- Header Files
-
Header Files
@@ -405,15 +393,6 @@
Header Files
-
- Header Files
-
-
- Header Files
-
-
- Header Files
-
Header Files
@@ -441,21 +420,9 @@
Header Files
-
- Header Files
-
Header Files
-
- Header Files
-
-
- Header Files
-
-
- Header Files
-
Header Files
@@ -489,10 +456,40 @@
Header Files
-
+
Header Files
-
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
+
Header Files
@@ -524,7 +521,7 @@
Source Files
-
+
Source Files
diff --git a/AirLib/include/api/ApiServerBase.hpp b/AirLib/include/api/ApiServerBase.hpp
index 5ddeb9cae1..0feec316f6 100644
--- a/AirLib/include/api/ApiServerBase.hpp
+++ b/AirLib/include/api/ApiServerBase.hpp
@@ -4,9 +4,10 @@
#ifndef air_ApiServerBase_hpp
#define air_ApiServerBase_hpp
-#include "common/Common.hpp"
#include
-
+#include "common/Common.hpp"
+#include "VehicleApiBase.hpp"
+#include "WorldSimApiBase.hpp"
namespace msr { namespace airlib {
@@ -14,6 +15,31 @@ class ApiServerBase {
public:
virtual void start(bool block = false) = 0;
virtual void stop() = 0;
+
+ virtual const VehicleApiBase* getVehicleApi(const std::string& vehicle_name = "") const = 0;
+ virtual VehicleApiBase* getVehicleApi(const std::string& vehicle_name = "")
+ {
+ return const_cast(getVehicleApi(vehicle_name));
+ }
+
+ virtual const WorldSimApiBase* getWorldSimApi() const = 0;
+ virtual WorldSimApiBase* getWorldSimApi()
+ {
+ return const_cast(getWorldSimApi());
+ }
+
+ virtual const VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "") const = 0;
+ virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "")
+ {
+ auto* world_sim_api = getWorldSimApi();
+ if (world_sim_api) {
+ auto* vehicle_sim_api = getWorldSimApi()->getVehicleSimApi(vehicle_name);
+ return const_cast(vehicle_sim_api);
+ }
+ return nullptr;
+ }
+
+
virtual ~ApiServerBase() = default;
};
diff --git a/AirLib/include/api/DebugApiServer.hpp b/AirLib/include/api/DebugApiServer.hpp
index 5c826383f0..edca45aa4d 100644
--- a/AirLib/include/api/DebugApiServer.hpp
+++ b/AirLib/include/api/DebugApiServer.hpp
@@ -19,6 +19,20 @@ namespace msr { namespace airlib {
{
common_utils::Utils::log("Debug server stopped");
}
+
+ virtual const VehicleApiBase* getVehicleApi() const
+ {
+ //not supported
+ return nullptr;
+ }
+
+ virtual const WorldSimApiBase* getWorldSimApi() const
+ {
+ //not supported
+ return nullptr;
+ }
+
+
virtual ~DebugApiServer() = default;
};
diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp
index e28c248220..1afb45a025 100644
--- a/AirLib/include/api/RpcLibClientBase.hpp
+++ b/AirLib/include/api/RpcLibClientBase.hpp
@@ -11,6 +11,7 @@
namespace msr { namespace airlib {
+//common methods for RCP clients of different vehicles
class RpcLibClientBase {
public:
enum class ConnectionState : uint {
@@ -18,6 +19,15 @@ class RpcLibClientBase {
};
public:
RpcLibClientBase(const string& ip_address = "localhost", uint16_t port = 41451, uint timeout_ms = 60000);
+ virtual ~RpcLibClientBase(); //required for pimpl
+
+ void confirmConnection();
+ bool isApiControlEnabled() const;
+ void enableApiControl(bool is_enabled);
+ void resetVehicle();
+ void simResetWorld();
+ bool armDisarm(bool arm);
+
ConnectionState getConnectionState();
bool ping();
int getClientVersion() const;
@@ -25,38 +35,35 @@ class RpcLibClientBase {
int getMinRequiredServerVersion() const;
int getMinRequiredClientVersion() const;
+ bool simIsPaused() const;
+ void simPause(bool is_paused);
+ void simContinueForTime(double seconds);
- vector simGetImages(vector request);
- vector simGetImage(int camera_id, ImageCaptureBase::ImageType type);
- msr::airlib::GeoPoint getHomeGeoPoint();
+ msr::airlib::GeoPoint getHomeGeoPoint() const;
- void simSetPose(const Pose& pose, bool ignore_collision);
- Pose simGetPose();
+ Pose simGetVehiclePose() const;
+ void simSetVehiclePose(const Pose& pose, bool ignore_collision);
+ Pose simGetObjectPose(const std::string& object_name) const;
- void confirmConnection();
- bool isApiControlEnabled();
- void enableApiControl(bool is_enabled);
- void reset();
- bool armDisarm(bool arm);
+ vector simGetImages(vector request);
+ vector simGetImage(int camera_id, ImageCaptureBase::ImageType type);
- CollisionInfo getCollisionInfo();
+ CollisionInfo simGetCollisionInfo() const;
bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false);
- int simGetSegmentationObjectID(const std::string& mesh_name);
+ int simGetSegmentationObjectID(const std::string& mesh_name) const;
void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
- Pose simGetObjectPose(const std::string& object_name);
- CameraInfo getCameraInfo(int camera_id);
+ CameraInfo getCameraInfo(int camera_id) const;
void setCameraOrientation(int camera_id, const Quaternionr& orientation);
- bool simIsPaused();
- void simPause(bool is_paused);
- void simContinueForTime(double seconds);
-
- virtual ~RpcLibClientBase(); //required for pimpl
protected:
- void* getClient();
+ const void* getClient() const;
+ void* getClient()
+ {
+ return const_cast(getClient());
+ }
private:
struct impl;
diff --git a/AirLib/include/api/RpcLibServerBase.hpp b/AirLib/include/api/RpcLibServerBase.hpp
index fcae3ed06c..548300805d 100644
--- a/AirLib/include/api/RpcLibServerBase.hpp
+++ b/AirLib/include/api/RpcLibServerBase.hpp
@@ -6,7 +6,7 @@
#include "common/Common.hpp"
#include "api/ApiServerBase.hpp"
-#include "api/SimModeApiBase.hpp"
+#include "api/WorldSimApiBase.hpp"
namespace msr { namespace airlib {
@@ -14,20 +14,16 @@ namespace msr { namespace airlib {
class RpcLibServerBase : public ApiServerBase {
public:
- RpcLibServerBase(SimModeApiBase* simmode_api, string server_address, uint16_t port);
+ RpcLibServerBase(const std::string& server_address, uint16_t port);
+ virtual ~RpcLibServerBase() override;
+
virtual void start(bool block = false) override;
virtual void stop() override;
- virtual ~RpcLibServerBase() override;
protected:
void* getServer() const;
- SimModeApiBase* getSimModeApi() const;
-
-private:
- VehicleApiBase* getVehicleApi() const;
private:
- SimModeApiBase* simmode_api_;
struct impl;
std::unique_ptr pimpl_;
};
diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp
deleted file mode 100644
index 7bfbb57452..0000000000
--- a/AirLib/include/api/SimModeApiBase.hpp
+++ /dev/null
@@ -1,27 +0,0 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
-
-#ifndef air_SimModeApiBase_hpp
-#define air_SimModeApiBase_hpp
-
-#include "common/CommonStructs.hpp"
-#include "VehicleApiBase.hpp"
-
-namespace msr { namespace airlib {
-
-
-class SimModeApiBase {
-public:
- virtual VehicleApiBase* getVehicleApi() = 0;
- virtual ~SimModeApiBase() = default;
-
- virtual bool isPaused() const = 0;
- virtual void reset() = 0;
- virtual void pause(bool is_paused) = 0;
- virtual void continueForTime(double seconds) = 0;
- virtual bool isSimulationMode() const = 0;
-};
-
-
-}} //namespace
-#endif
diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp
index 9062c40b0b..278422bb2e 100644
--- a/AirLib/include/api/VehicleApiBase.hpp
+++ b/AirLib/include/api/VehicleApiBase.hpp
@@ -4,37 +4,38 @@
#ifndef air_VehicleApiBase_hpp
#define air_VehicleApiBase_hpp
+#include "common/CommonStructs.hpp"
+#include "common/UpdatableObject.hpp"
+#include "common/Common.hpp"
+#include "common/Waiter.hpp"
+#include "safety/SafetyEval.hpp"
#include "common/CommonStructs.hpp"
#include "common/ImageCaptureBase.hpp"
+#include
+#include
namespace msr { namespace airlib {
-
-class VehicleApiBase {
+/*
+Vehicle controller allows to obtain state from vehicle and send control commands to the vehicle.
+State can include many things including sensor data, logs, estimated state from onboard computer etc.
+Control commands can be low level actuation commands or high level movement commands.
+The base class defines usually available methods that all vehicle controllers may implement.
+Some methods may not be applicable to specific vehicle in which case an exception may be raised or call may be ignored.
+*/
+class VehicleApiBase : public UpdatableObject {
public:
- virtual GeoPoint getHomeGeoPoint() const = 0;
virtual void enableApiControl(bool is_enabled) = 0;
- virtual bool armDisarm(bool arm) = 0;
virtual bool isApiControlEnabled() const = 0;
- virtual void reset() = 0;
+ virtual bool armDisarm(bool arm) = 0;
virtual void cancelPendingTasks() = 0;
+ virtual GeoPoint getHomeGeoPoint() const = 0;
-
- virtual vector simGetImages(const vector& request) const = 0;
- virtual vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const = 0;
-
- virtual void simSetPose(const Pose& pose, bool ignore_collision) = 0;
- virtual Pose simGetPose() const = 0;
-
- virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0;
- virtual int simGetSegmentationObjectID(const std::string& mesh_name) const = 0;
-
- virtual void simPrintLogMessage(const std::string& message,
- const std::string& message_param = "", unsigned char severity = 0) = 0;
-
- virtual CollisionInfo getCollisionInfo() const = 0;
-
- virtual Pose simGetObjectPose(const std::string& object_name) const = 0;
+ virtual void getStatusMessages(std::vector& messages)
+ {
+ unused(messages);
+ //default implementation
+ }
virtual CameraInfo getCameraInfo(int camera_id) const = 0;
virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;
@@ -42,6 +43,27 @@ class VehicleApiBase {
virtual ~VehicleApiBase() = default;
};
+class VehicleControllerException : public std::runtime_error {
+public:
+ VehicleControllerException(const std::string& message)
+ : runtime_error(message) {
+ }
+};
+
+class VehicleCommandNotImplementedException : public VehicleControllerException {
+public:
+ VehicleCommandNotImplementedException(const std::string& message)
+ : VehicleControllerException(message) {
+ }
+};
+
+class VehicleMoveException : public VehicleControllerException {
+public:
+ VehicleMoveException(const std::string& message)
+ : VehicleControllerException(message) {
+ }
+};
+
}} //namespace
#endif
diff --git a/AirLib/include/controllers/VehicleConnectorBase.hpp b/AirLib/include/api/VehicleConnectorBase.hpp
similarity index 93%
rename from AirLib/include/controllers/VehicleConnectorBase.hpp
rename to AirLib/include/api/VehicleConnectorBase.hpp
index 3b1fd07a8c..f2f1ab973d 100644
--- a/AirLib/include/controllers/VehicleConnectorBase.hpp
+++ b/AirLib/include/api/VehicleConnectorBase.hpp
@@ -4,7 +4,7 @@
#ifndef air_VehicleConnectorBase_hpp
#define air_VehicleConnectorBase_hpp
-#include "VehicleControllerBase.hpp"
+#include "VehicleApiBase.hpp"
#include "common/ImageCaptureBase.hpp"
#include "common/UpdatableObject.hpp"
@@ -20,7 +20,7 @@ class VehicleConnectorBase : public UpdatableObject
//called when render changes are required
virtual void updateRendering(float dt) = 0;
- virtual VehicleControllerBase* getController() = 0;
+ virtual VehicleApiBase* getController() = 0;
virtual ImageCaptureBase* getImageCapture() = 0;
virtual void setPose(const Pose& pose, bool ignore_collision) = 0;
virtual Pose getPose() = 0;
diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp
new file mode 100644
index 0000000000..b765bcc21d
--- /dev/null
+++ b/AirLib/include/api/VehicleSimApiBase.hpp
@@ -0,0 +1,38 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef air_VehicleSimApiBase_hpp
+#define air_VehicleSimApiBase_hpp
+
+#include "common/CommonStructs.hpp"
+#include "common/ImageCaptureBase.hpp"
+#include "physics/Kinematics.hpp"
+
+namespace msr { namespace airlib {
+
+class VehicleSimApiBase {
+public:
+ virtual ~VehicleSimApiBase() = default;
+
+ virtual void reset() = 0;
+
+ virtual vector getImages(const vector& request) const = 0;
+ virtual vector getImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const = 0;
+
+ virtual void setPose(const Pose& pose, bool ignore_collision) = 0;
+ virtual Pose getPose() const = 0;
+ virtual CollisionInfo getCollisionInfo() const = 0;
+ virtual void setCollisionInfo(const CollisionInfo& collision_info) = 0;
+ virtual Kinematics::State getTrueKinematics() const = 0;
+ virtual int getRemoteControlID() const { return -1; }
+ virtual void setRCData(const RCData& rcData) = 0;
+
+ //TODO: need better place for below two APIs?
+ //return 0 to 1 (corresponds to zero to full thrust)
+ virtual real_T getVertexControlSignal(unsigned int rotor_index) const = 0;
+ virtual size_t getVertexCount() const = 0;
+
+};
+
+} } //namespace
+#endif
diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp
new file mode 100644
index 0000000000..5b86a7151f
--- /dev/null
+++ b/AirLib/include/api/WorldSimApiBase.hpp
@@ -0,0 +1,38 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef air_WorldSimApiBase_hpp
+#define air_WorldSimApiBase_hpp
+
+#include "common/CommonStructs.hpp"
+#include "VehicleSimApiBase.hpp"
+
+namespace msr { namespace airlib {
+
+
+class WorldSimApiBase {
+public:
+ virtual ~WorldSimApiBase() = default;
+
+ virtual bool isPaused() const = 0;
+ virtual void reset() = 0;
+ virtual void pause(bool is_paused) = 0;
+ virtual void continueForTime(double seconds) = 0;
+
+ virtual const VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "") const = 0;
+ virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "")
+ {
+ return const_cast(getVehicleSimApi(vehicle_name));
+ }
+
+ virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0;
+ virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0;
+ virtual void printLogMessage(const std::string& message,
+ const std::string& message_param = "", unsigned char severity = 0) = 0;
+ virtual Pose getObjectPose(const std::string& object_name) const = 0;
+
+};
+
+
+}} //namespace
+#endif
diff --git a/AirLib/include/common/CancelToken.hpp b/AirLib/include/common/CancelToken.hpp
new file mode 100644
index 0000000000..47ecdaedfc
--- /dev/null
+++ b/AirLib/include/common/CancelToken.hpp
@@ -0,0 +1,91 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef air_CancelToken_hpp
+#define air_CancelToken_hpp
+
+#include
+#include
+#include "common/Common.hpp"
+#include "common/common_utils/Utils.hpp"
+
+namespace msr { namespace airlib {
+
+class CancelToken {
+public:
+ CancelToken()
+ : is_cancelled_(false), recursion_count_(0)
+ {
+ }
+
+ void reset()
+ {
+ is_cancelled_ = false;
+ recursion_count_ = 0;
+ }
+
+ bool isCancelled() const
+ {
+ return is_cancelled_;
+ }
+
+ void cancel()
+ {
+ is_cancelled_ = true;
+ }
+
+ bool sleep(double secs)
+ {
+ //We can pass duration directly to sleep_for however it is known that on
+ //some systems, sleep_for makes system call anyway even if passed duration
+ //is <= 0. This can cause 50us of delay due to context switch.
+ if (isCancelled()) {
+ return false;
+ }
+
+ TTimePoint start = ClockFactory::get()->nowNanos();
+ static constexpr std::chrono::duration MinSleepDuration(0);
+
+ while (secs > 0 && !isCancelled() &&
+ ClockFactory::get()->elapsedSince(start) < secs) {
+
+ std::this_thread::sleep_for(MinSleepDuration);
+ }
+
+ return !isCancelled();
+ }
+
+ int getRecursionCount()
+ {
+ return recursion_count_;
+ }
+
+ bool try_lock()
+ {
+ bool result = wait_mutex_.try_lock();
+ if (result)
+ ++recursion_count_;
+ return result;
+ }
+
+ void unlock()
+ {
+ wait_mutex_.unlock();
+ if (recursion_count_ > 0)
+ --recursion_count_;
+ }
+
+ void lock()
+ {
+ wait_mutex_.lock();
+ ++recursion_count_;
+ }
+
+private:
+ std::atomic is_cancelled_;
+ std::recursive_mutex wait_mutex_;
+ std::atomic recursion_count_;
+};
+
+}} //namespace
+#endif
diff --git a/AirLib/include/controllers/PidController.hpp b/AirLib/include/common/PidController.hpp
similarity index 96%
rename from AirLib/include/controllers/PidController.hpp
rename to AirLib/include/common/PidController.hpp
index c7d86c2587..f0a6ad6292 100644
--- a/AirLib/include/controllers/PidController.hpp
+++ b/AirLib/include/common/PidController.hpp
@@ -1,79 +1,79 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
-
-#ifndef msr_airlib__PidController_hpp
-#define msr_airlib__PidController_hpp
-
-#include
-
-namespace msr { namespace airlib {
-
-
-// This class implements a pid controller.
-// First call setPoint to set the desired point and the PID control variables
-// then call control(x) with new observed values and it will return the updated
-// control output needed to achieve the setPoint goal. Integration is done using
-// dt measured using system clock.
-class PidController
-{
-private:
- float set_point_;
- float kProportional_;
- float kIntegral_;
- float kDerivative_;
- float previous_error_;
- bool previous_set;
- float sum_;
- std::chrono::time_point prev_time_;
-public:
- //set desired point.
- void setPoint(float target, float kProportional, float kIntegral, float kDerivative) {
- set_point_ = target;
- kProportional_ = kProportional;
- kIntegral_ = kIntegral;
- kDerivative_ = kDerivative;
- prev_time_ = std::chrono::system_clock::now();
- previous_error_ = 0;
- sum_ = 0;
- previous_set = false;
- }
- float control(float processVariable) {
- auto t = std::chrono::system_clock::now();
- auto diff = std::chrono::system_clock::now() - prev_time_;
-
- float error = set_point_ - processVariable;
- if (!previous_set)
- {
- previous_set = true;
- previous_error_ = error;
- return 0;
- }
-
- float dt = static_cast(std::chrono::duration_cast(diff).count()) * 0.000001f;
- dt = fmax(dt, 0.01f);
- float proportionalGain = 0;
- float derivativeGain = 0;
- float integralGain = 0;
-
- if (kProportional_ != 0) {
- proportionalGain = error * kProportional_;
- }
- if (kDerivative_ != 0) {
- float derivative = (error - previous_error_) / dt;
- derivativeGain = derivative * kDerivative_;
- }
- if (kIntegral_ != 0) {
- sum_ += error * dt;
- integralGain = sum_ * kIntegral_;
- }
-
- previous_error_ = error;
- prev_time_ = t;
-
- return proportionalGain + derivativeGain + integralGain;
- }
-};
-
-
-}} //namespace
-#endif
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef msr_airlib__PidController_hpp
+#define msr_airlib__PidController_hpp
+
+#include
+
+namespace msr { namespace airlib {
+
+
+// This class implements a pid controller.
+// First call setPoint to set the desired point and the PID control variables
+// then call control(x) with new observed values and it will return the updated
+// control output needed to achieve the setPoint goal. Integration is done using
+// dt measured using system clock.
+class PidController
+{
+private:
+ float set_point_;
+ float kProportional_;
+ float kIntegral_;
+ float kDerivative_;
+ float previous_error_;
+ bool previous_set;
+ float sum_;
+ std::chrono::time_point prev_time_;
+public:
+ //set desired point.
+ void setPoint(float target, float kProportional, float kIntegral, float kDerivative) {
+ set_point_ = target;
+ kProportional_ = kProportional;
+ kIntegral_ = kIntegral;
+ kDerivative_ = kDerivative;
+ prev_time_ = std::chrono::system_clock::now();
+ previous_error_ = 0;
+ sum_ = 0;
+ previous_set = false;
+ }
+ float control(float processVariable) {
+ auto t = std::chrono::system_clock::now();
+ auto diff = std::chrono::system_clock::now() - prev_time_;
+
+ float error = set_point_ - processVariable;
+ if (!previous_set)
+ {
+ previous_set = true;
+ previous_error_ = error;
+ return 0;
+ }
+
+ float dt = static_cast(std::chrono::duration_cast(diff).count()) * 0.000001f;
+ dt = fmax(dt, 0.01f);
+ float proportionalGain = 0;
+ float derivativeGain = 0;
+ float integralGain = 0;
+
+ if (kProportional_ != 0) {
+ proportionalGain = error * kProportional_;
+ }
+ if (kDerivative_ != 0) {
+ float derivative = (error - previous_error_) / dt;
+ derivativeGain = derivative * kDerivative_;
+ }
+ if (kIntegral_ != 0) {
+ sum_ += error * dt;
+ integralGain = sum_ * kIntegral_;
+ }
+
+ previous_error_ = error;
+ prev_time_ = t;
+
+ return proportionalGain + derivativeGain + integralGain;
+ }
+};
+
+
+}} //namespace
+#endif
diff --git a/AirLib/include/common/Waiter.hpp b/AirLib/include/common/Waiter.hpp
index c6b8e97457..2c7ca79caa 100644
--- a/AirLib/include/common/Waiter.hpp
+++ b/AirLib/include/common/Waiter.hpp
@@ -9,17 +9,17 @@
#include "common/Common.hpp"
#include "common/common_utils/Utils.hpp"
#include "common/ClockFactory.hpp"
-#include "common/common_utils/WorkerThread.hpp"
+#include "common/CancelToken.hpp"
namespace msr { namespace airlib {
class Waiter {
private:
-
TTimePoint proc_start_;
TTimePoint loop_start_;
TTimeDelta sleep_duration_, timeout_duration_;
+
public:
Waiter(TTimeDelta sleep_duration_seconds, TTimeDelta timeout_duration = std::numeric_limits::max())
: sleep_duration_(sleep_duration_seconds), timeout_duration_(timeout_duration)
@@ -27,7 +27,7 @@ class Waiter {
proc_start_ = loop_start_ = clock()->nowNanos();
}
- virtual bool sleep(CancelableBase& cancelable_action)
+ virtual bool sleep(CancelToken& cancelable_action)
{
// Sleeps for the time needed to get current running time up to the requested sleep_duration_.
// So this can be used to "throttle" any loop to check something every sleep_duration_ seconds.
diff --git a/AirLib/include/common/common_utils/WorkerThread.hpp b/AirLib/include/common/WorkerThread.hpp
similarity index 77%
rename from AirLib/include/common/common_utils/WorkerThread.hpp
rename to AirLib/include/common/WorkerThread.hpp
index 964b0659e0..61147ade4f 100644
--- a/AirLib/include/common/common_utils/WorkerThread.hpp
+++ b/AirLib/include/common/WorkerThread.hpp
@@ -1,302 +1,284 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
-
-#ifndef commn_utils_WorkerThread_hpp
-#define commn_utils_WorkerThread_hpp
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include "Utils.hpp"
-#include "common/ClockFactory.hpp" //TODO: move this out of common_utils
-
-namespace msr { namespace airlib {
-
-class CancelableBase {
-protected:
- std::atomic is_cancelled_;
- std::atomic is_complete_;
-public:
- CancelableBase() : is_cancelled_(false), is_complete_(false) {
- }
-
- void reset()
- {
- is_cancelled_ = false;
- is_complete_ = false;
- }
-
- bool isCancelled() {
- return is_cancelled_;
- }
-
- void cancel() {
- is_cancelled_ = true;
- }
-
- virtual void execute() = 0;
-
- virtual bool sleep(double secs)
- {
- //We can pass duration directly to sleep_for however it is known that on
- //some systems, sleep_for makes system call anyway even if passed duration
- //is <= 0. This can cause 50us of delay due to context switch.
- if (isCancelled()) {
- return false;
- }
-
- TTimePoint start = ClockFactory::get()->nowNanos();
- static constexpr std::chrono::duration MinSleepDuration(0);
-
- while(secs > 0 && !isCancelled() && !is_complete_ &&
- ClockFactory::get()->elapsedSince(start) < secs) {
-
- std::this_thread::sleep_for(MinSleepDuration);
- }
-
- return !isCancelled();
- }
-
- void complete() {
- is_complete_ = true;
- }
-
- bool isComplete() {
- return is_complete_;
- }
-
-};
-
-// This wraps a condition_variable so we can handle the case where we may signal before wait
-// and implement the semantics that say wait should be a noop in that case.
-class WorkerThreadSignal
-{
- std::condition_variable cv_;
- std::mutex mutex_;
- std::atomic signaled_;
-public:
- WorkerThreadSignal() : signaled_(false)
- {
- }
-
- void signal()
- {
- {
- std::unique_lock lock(mutex_);
- signaled_ = true;
- }
- cv_.notify_one();
- }
-
- template
- void wait(_Predicate cancel)
- {
- // wait for signal or cancel predicate
- while (!signaled_) {
- std::unique_lock lock(mutex_);
- cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] {
- return cancel();
- });
- }
- signaled_ = false;
- }
-
- bool waitFor(double max_wait_seconds)
- {
- // wait for signal or timeout or cancel predicate
- while (!signaled_) {
- std::unique_lock lock(mutex_);
- cv_.wait_for(lock, std::chrono::milliseconds(
- static_cast(max_wait_seconds * 1000)));
- }
- signaled_ = false;
- return true;
- }
-
-};
-
-// This class provides a synchronized worker thread that guarantees to execute
-// cancelable tasks on a background thread one at a time.
-// It guarantees that previous task is cancelled before new task is started.
-// The queue size is 1, which means it does NOT guarantee all queued tasks are executed.
-// If enqueue is called very quickly the thread will not have a chance to execute each
-// task before they get cancelled, worst case in a tight loop all tasks are starved and
-// nothing executes.
-class WorkerThread {
-public:
- WorkerThread()
- : thread_running_(false), cancel_request_(false) {
- }
-
- ~WorkerThread() {
- cancel_request_ = true;
- cancel();
- }
- void enqueue(std::shared_ptr item) {
- //cancel previous item
- {
- std::unique_lock lock(mutex_);
- std::shared_ptr pending = pending_item_;
- if (pending != nullptr) {
- pending->cancel();
- }
- }
-
- bool running = false;
-
- {
- std::unique_lock lock(mutex_);
- pending_item_ = item;
- running = thread_running_;
- }
-
- if (running) {
- item_arrived_.signal();
- }
- else {
- start();
- }
- }
-
- bool enqueueAndWait(std::shared_ptr item, float max_wait_seconds)
- {
- //cancel previous item
- {
- std::unique_lock lock(mutex_);
- std::shared_ptr pending = pending_item_;
- if (pending != nullptr) {
- pending->cancel();
- }
- }
-
- bool running = false;
-
- //set new item to run
- {
- std::unique_lock lock(mutex_);
- pending_item_ = item;
- running = thread_running_;
- }
-
- if (running) {
- item_arrived_.signal();
- }
- else {
- start();
- }
-
- item->sleep(max_wait_seconds);
-
- //after the wait if item is still running then cancel it
- if (!item->isCancelled() && !item->isComplete())
- item->cancel();
-
- return !item->isCancelled();
- }
-
- void cancel()
- {
- std::unique_lock lock(mutex_);
- std::shared_ptr pending = pending_item_;
- pending_item_ = nullptr;
- if (pending != nullptr) {
- pending->cancel();
- }
- if (thread_.joinable()) {
- item_arrived_.signal();
- thread_.join();
- }
- }
-private:
- void start()
- {
- //if state == not running
- if (!thread_running_) {
-
- //make sure C++ previous thread is done
- {
- std::unique_lock lock(mutex_);
- if (thread_.joinable()) {
- thread_.join();
- }
- }
- Utils::cleanupThread(thread_);
-
- //start the thread
- cancel_request_ = false;
- thread_ = std::thread(&WorkerThread::run, this);
-
- //wait until thread tells us it has started
- thread_started_.wait([this] { return static_cast(cancel_request_); });
- }
- }
-
- void run()
- {
- thread_running_ = true;
-
- //tell the thread which started this thread that we are on now
- {
- std::unique_lock lock(mutex_);
- thread_started_.signal();
- }
-
- //until we don't get stopped and have work to do, keep running
- while (!cancel_request_ && pending_item_ != nullptr) {
- std::shared_ptr pending;
-
- //get the pending item
- {
- std::unique_lock lock(mutex_);
- pending = pending_item_;
- }
-
- //if pending item is not yet cancelled
- if (pending != nullptr && !pending->isCancelled()) {
-
- //execute pending item
- try {
- pending->execute();
- }
- catch (std::exception& e) {
- //Utils::DebugBreak();
- Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), Utils::kLogLevelError);
- }
-
- //we use cancel here to communicate to enqueueAndWait that the task is complete.
- pending->complete();
- }
-
- if (!cancel_request_) {
- //wait for next item to arrive or thread is stopped
- item_arrived_.wait([this] { return static_cast(cancel_request_); });
- }
- }
-
- thread_running_ = false;
- }
-
-private:
- //this is used to wait until our thread actually gets started
- WorkerThreadSignal thread_started_;
- //when new item arrived, we signal this so waiting thread can continue
- WorkerThreadSignal item_arrived_;
-
- // thread state
- std::shared_ptr pending_item_;
- std::mutex mutex_;
- std::thread thread_;
- //while run() is in progress this is true
- std::atomic thread_running_;
- //has request to stop this worker thread made?
- std::atomic cancel_request_;
-};
-
-
-}} //namespace
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef commn_utils_WorkerThread_hpp
+#define commn_utils_WorkerThread_hpp
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "Utils.hpp"
+#include "ClockFactory.hpp" //TODO: move this out of common_utils
+#include "CancelToken.hpp"
+
+namespace msr { namespace airlib {
+
+class CancelableAction : public CancelToken {
+protected:
+ virtual void executeAction() = 0;
+
+public:
+ CancelableAction()
+ : is_complete_(false)
+ {
+ }
+
+ void reset()
+ {
+ is_complete_ = false;
+ }
+
+ void execute()
+ {
+ is_complete_ = false;
+ try {
+ executeAction();
+ }
+ finally{
+ is_complete_ = true;
+ }
+ }
+
+ bool isComplete() const
+ {
+ return is_complete_;
+ }
+
+ private:
+ std::atomic is_complete_;
+
+};
+
+// This wraps a condition_variable so we can handle the case where we may signal before wait
+// and implement the semantics that say wait should be a noop in that case.
+class WorkerThreadSignal
+{
+ std::condition_variable cv_;
+ std::mutex mutex_;
+ std::atomic signaled_;
+public:
+ WorkerThreadSignal() : signaled_(false)
+ {
+ }
+
+ void signal()
+ {
+ {
+ std::unique_lock lock(mutex_);
+ signaled_ = true;
+ }
+ cv_.notify_one();
+ }
+
+ template
+ void wait(_Predicate cancel)
+ {
+ // wait for signal or cancel predicate
+ while (!signaled_) {
+ std::unique_lock lock(mutex_);
+ cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] {
+ return cancel();
+ });
+ }
+ signaled_ = false;
+ }
+
+ bool waitFor(double max_wait_seconds)
+ {
+ // wait for signal or timeout or cancel predicate
+ while (!signaled_) {
+ std::unique_lock lock(mutex_);
+ cv_.wait_for(lock, std::chrono::milliseconds(
+ static_cast(max_wait_seconds * 1000)));
+ }
+ signaled_ = false;
+ return true;
+ }
+
+};
+
+// This class provides a synchronized worker thread that guarantees to execute
+// cancelable tasks on a background thread one at a time.
+// It guarantees that previous task is cancelled before new task is started.
+// The queue size is 1, which means it does NOT guarantee all queued tasks are executed.
+// If enqueue is called very quickly the thread will not have a chance to execute each
+// task before they get cancelled, worst case in a tight loop all tasks are starved and
+// nothing executes.
+class WorkerThread {
+public:
+ WorkerThread()
+ : thread_running_(false), cancel_request_(false) {
+ }
+
+ ~WorkerThread() {
+ cancel_request_ = true;
+ cancel();
+ }
+ void enqueue(std::shared_ptr item) {
+ //cancel previous item
+ {
+ std::unique_lock lock(mutex_);
+ std::shared_ptr pending = pending_item_;
+ if (pending != nullptr) {
+ pending->cancel();
+ }
+ }
+
+ bool running = false;
+
+ {
+ std::unique_lock lock(mutex_);
+ pending_item_ = item;
+ running = thread_running_;
+ }
+
+ if (running) {
+ item_arrived_.signal();
+ }
+ else {
+ start();
+ }
+ }
+
+ bool enqueueAndWait(std::shared_ptr item, float max_wait_seconds)
+ {
+ //cancel previous item
+ {
+ std::unique_lock lock(mutex_);
+ std::shared_ptr pending = pending_item_;
+ if (pending != nullptr) {
+ pending->cancel();
+ }
+ }
+
+ bool running = false;
+
+ //set new item to run
+ {
+ std::unique_lock lock(mutex_);
+ pending_item_ = item;
+ running = thread_running_;
+ }
+
+ if (running) {
+ item_arrived_.signal();
+ }
+ else {
+ start();
+ }
+
+ item->sleep(max_wait_seconds);
+
+ //after the wait if item is still running then cancel it
+ if (!item->isCancelled() && !item->isComplete())
+ item->cancel();
+
+ return !item->isCancelled();
+ }
+
+ void cancel()
+ {
+ std::unique_lock lock(mutex_);
+ std::shared_ptr pending = pending_item_;
+ pending_item_ = nullptr;
+ if (pending != nullptr) {
+ pending->cancel();
+ }
+ if (thread_.joinable()) {
+ item_arrived_.signal();
+ thread_.join();
+ }
+ }
+private:
+ void start()
+ {
+ //if state == not running
+ if (!thread_running_) {
+
+ //make sure C++ previous thread is done
+ {
+ std::unique_lock lock(mutex_);
+ if (thread_.joinable()) {
+ thread_.join();
+ }
+ }
+ Utils::cleanupThread(thread_);
+
+ //start the thread
+ cancel_request_ = false;
+ thread_ = std::thread(&WorkerThread::run, this);
+
+ //wait until thread tells us it has started
+ thread_started_.wait([this] { return static_cast(cancel_request_); });
+ }
+ }
+
+ void run()
+ {
+ thread_running_ = true;
+
+ //tell the thread which started this thread that we are on now
+ {
+ std::unique_lock lock(mutex_);
+ thread_started_.signal();
+ }
+
+ //until we don't get stopped and have work to do, keep running
+ while (!cancel_request_ && pending_item_ != nullptr) {
+ std::shared_ptr pending;
+
+ //get the pending item
+ {
+ std::unique_lock lock(mutex_);
+ pending = pending_item_;
+ }
+
+ //if pending item is not yet cancelled
+ if (pending != nullptr && !pending->isCancelled()) {
+
+ //execute pending item
+ try {
+ pending->execute();
+ }
+ catch (std::exception& e) {
+ //Utils::DebugBreak();
+ Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), Utils::kLogLevelError);
+ }
+
+ //we use cancel here to communicate to enqueueAndWait that the task is complete.
+ pending->complete();
+ }
+
+ if (!cancel_request_) {
+ //wait for next item to arrive or thread is stopped
+ item_arrived_.wait([this] { return static_cast(cancel_request_); });
+ }
+ }
+
+ thread_running_ = false;
+ }
+
+private:
+ //this is used to wait until our thread actually gets started
+ WorkerThreadSignal thread_started_;
+ //when new item arrived, we signal this so waiting thread can continue
+ WorkerThreadSignal item_arrived_;
+
+ // thread state
+ std::shared_ptr pending_item_;
+ std::mutex mutex_;
+ std::thread thread_;
+ //while run() is in progress this is true
+ std::atomic thread_running_;
+ //has request to stop this worker thread made?
+ std::atomic cancel_request_;
+};
+
+
+}} //namespace
#endif
\ No newline at end of file
diff --git a/AirLib/include/controllers/ControllerBase.hpp b/AirLib/include/controllers/ControllerBase.hpp
deleted file mode 100644
index 0c4a934d3e..0000000000
--- a/AirLib/include/controllers/ControllerBase.hpp
+++ /dev/null
@@ -1,49 +0,0 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
-
-#ifndef msr_airlib_ControllerBase_hpp
-#define msr_airlib_ControllerBase_hpp
-
-#include "common/UpdatableObject.hpp"
-#include
-#include
-
-namespace msr { namespace airlib {
-
-/*
- This class defines the interface for vehicle controllers. A typical vehicle controller would consume
- sensor values as inputs and return control signals for various vertices on physics body as output. The update()
- method should be used to process the sensor inputs from vehicle as needed (the implementor should have vehicle object
- possibly through initialization). The getVertexControlSignal() is called to retrieve the value of control signal to be applied
- to physics body vertex.
- While reset() may be called at any time, all other methods should be expected be called after start() and before stop().
-*/
-class ControllerBase : public UpdatableObject {
-public:
- //return 0 to 1 (corresponds to zero to full thrust)
- virtual real_T getVertexControlSignal(unsigned int rotor_index) const = 0;
- virtual size_t getVertexCount() const = 0;
-
- virtual void getStatusMessages(std::vector& messages)
- {
- unused(messages);
- //default implementation
- }
-
- virtual void reportTelemetry(float renderTime)
- {
- unused(renderTime);
- //no default action
- }
-
-};
-
-class ControllerException : public std::runtime_error {
-public:
- ControllerException(const std::string& message)
- : runtime_error(message) {
- }
-};
-
-}} //namespace
-#endif
diff --git a/AirLib/include/controllers/VehicleControllerBase.hpp b/AirLib/include/controllers/VehicleControllerBase.hpp
deleted file mode 100644
index 5caead2419..0000000000
--- a/AirLib/include/controllers/VehicleControllerBase.hpp
+++ /dev/null
@@ -1,58 +0,0 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
-
-#ifndef msr_airlib_VehicleControllerBase_hpp
-#define msr_airlib_VehicleControllerBase_hpp
-
-#include "controllers/ControllerBase.hpp"
-#include "physics/PhysicsBody.hpp"
-#include
-#include
-
-namespace msr { namespace airlib {
-
-/*
- Defined additional interface for vehicles
-*/
-class VehicleControllerBase : public ControllerBase {
-public:
- //tells the controller to switch from human operated mode to computer operated mode
- virtual void enableApiControl(bool is_enabled) = 0;
- virtual void setSimulationMode(bool is_set) = 0;
- virtual bool isApiControlEnabled() const = 0;
-
- //if controller connects via USB/UDP and connection fails then this
- //should return false
- virtual bool isAvailable(std::string& message) const = 0;
-
- //TODO: below method is needed to support firmwares without state estimation. In future, we should probably remove this support.
- virtual void setGroundTruth(PhysicsBody* physics_body)
- {
- unused(physics_body);
- //by default don't use it. If derived class needs this, it should override.
- }
-};
-
-class VehicleControllerException : public ControllerException {
-public:
- VehicleControllerException(const std::string& message)
- : ControllerException(message) {
- }
-};
-
-class VehicleCommandNotImplementedException : public VehicleControllerException {
-public:
- VehicleCommandNotImplementedException(const std::string& message)
- : VehicleControllerException(message) {
- }
-};
-
-class VehicleMoveException : public VehicleControllerException {
-public:
- VehicleMoveException(const std::string& message)
- : VehicleControllerException(message) {
- }
-};
-
-}} //namespace
-#endif
diff --git a/AirLib/include/safety/SafetyEval.hpp b/AirLib/include/safety/SafetyEval.hpp
index 2647bd5c7e..d0355834fc 100644
--- a/AirLib/include/safety/SafetyEval.hpp
+++ b/AirLib/include/safety/SafetyEval.hpp
@@ -11,7 +11,7 @@
#include "common/common_utils/Utils.hpp"
#include "IGeoFence.hpp"
#include "common/Common.hpp"
-#include "vehicles/multirotor/controllers/DroneCommon.hpp"
+#include "vehicles/multirotor/controllers/MultirotorCommon.hpp"
#include "common/common_utils/EnumFlags.hpp"
namespace msr { namespace airlib {
@@ -72,7 +72,7 @@ class SafetyEval {
};
private:
- VehicleParams vehicle_params_;
+ MultirotorApiParams vehicle_params_;
shared_ptr fence_ptr_;
shared_ptr obs_xy_ptr_;
SafetyViolationType enable_reasons_ = SafetyEval::SafetyViolationType_::GeoFence;
@@ -86,7 +86,7 @@ class SafetyEval {
void setSuggestedVelocity(SafetyEval::EvalResult& result, const Quaternionr& quaternion);
float adjustClearanceForPrStl(float base_clearance, float obs_confidence);
public:
- SafetyEval(VehicleParams vehicle_params, shared_ptr fence_ptr, shared_ptr obs_xy);
+ SafetyEval(MultirotorApiParams vehicle_params, shared_ptr fence_ptr, shared_ptr obs_xy);
EvalResult isSafeVelocity(const Vector3r& cur_pos, const Vector3r& velocity, const Quaternionr& quaternion);
EvalResult isSafeVelocityZ(const Vector3r& cur_pos, float vx, float vy, float z, const Quaternionr& quaternion);
EvalResult isSafeDestination(const Vector3r& dest,const Vector3r& cur_pos, const Quaternionr& quaternion);
diff --git a/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp b/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp
index 8f9cded19b..b4d727aa18 100644
--- a/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp
+++ b/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp
@@ -13,7 +13,7 @@ namespace msr { namespace airlib {
class CarRpcLibServer : public RpcLibServerBase {
public:
- CarRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port = 41451);
+ CarRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port = 41451);
virtual ~CarRpcLibServer();
private:
diff --git a/AirLib/include/vehicles/multirotor/MultiRotor.hpp b/AirLib/include/vehicles/multirotor/MultiRotor.hpp
index 284bc4c11b..6ff7d646fe 100644
--- a/AirLib/include/vehicles/multirotor/MultiRotor.hpp
+++ b/AirLib/include/vehicles/multirotor/MultiRotor.hpp
@@ -7,7 +7,7 @@
#include "common/Common.hpp"
#include "common/CommonStructs.hpp"
#include "Rotor.hpp"
-#include "controllers/ControllerBase.hpp"
+#include "api/VehicleApiBase.hpp"
#include "MultiRotorParams.hpp"
#include
#include "physics/PhysicsBody.hpp"
@@ -52,7 +52,7 @@ class MultiRotor : public PhysicsBody {
getController()->setGroundTruth(this);
}
- DroneControllerBase* getController()
+ MultirotorApiBase* getController()
{
return params_->getController();
}
diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
index 212c01de41..e4c3da0c5c 100644
--- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
+++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
@@ -7,7 +7,7 @@
#include "common/Common.hpp"
#include "RotorParams.hpp"
#include "sensors/SensorCollection.hpp"
-#include "controllers/DroneControllerBase.hpp"
+#include "controllers/MultirotorApiBase.h"
@@ -85,11 +85,11 @@ class MultiRotorParams {
{
return sensors_;
}
- DroneControllerBase* getController()
+ MultirotorApiBase* getController()
{
return controller_.get();
}
- const DroneControllerBase* getController() const
+ const MultirotorApiBase* getController() const
{
return controller_.get();
}
@@ -126,7 +126,7 @@ class MultiRotorParams {
protected: //must override by derived class
virtual void setupParams() = 0;
virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) = 0;
- virtual std::unique_ptr createController() = 0;
+ virtual std::unique_ptr createController() = 0;
protected: //static utility functions for derived classes to use
@@ -249,7 +249,7 @@ class MultiRotorParams {
Params params_;
SensorCollection sensors_; //maintains sensor type indexed collection of sensors
vector> sensor_storage_; //RAII for created sensors
- std::unique_ptr controller_;
+ std::unique_ptr controller_;
};
}} //namespace
diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp
index 4410eb8847..87b396330f 100644
--- a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp
+++ b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp
@@ -5,7 +5,7 @@
#define msr_airlib_vehicles_MultiRotorParamsFactory_hpp
#include "vehicles/multirotor/configs/Px4MultiRotor.hpp"
-#include "vehicles/multirotor/controllers/MavLinkDroneController.hpp"
+#include "vehicles/multirotor/controllers/MavLinkMultirotorApi.hpp"
#include "vehicles/multirotor/configs/RosFlightQuadX.hpp"
#include "vehicles/multirotor/configs/SimpleFlightQuadX.hpp"
diff --git a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp b/AirLib/include/vehicles/multirotor/api/MavLinkMultirotorApi.hpp
similarity index 86%
rename from AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp
rename to AirLib/include/vehicles/multirotor/api/MavLinkMultirotorApi.hpp
index 1b64edc298..0f4cf8f340 100644
--- a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp
+++ b/AirLib/include/vehicles/multirotor/api/MavLinkMultirotorApi.hpp
@@ -22,9 +22,9 @@
#include "common/CommonStructs.hpp"
#include "common/VectorMath.hpp"
#include "common/AirSimSettings.hpp"
-#include "vehicles/multirotor//MultiRotor.hpp"
-#include "vehicles/multirotor/controllers/DroneControllerBase.hpp"
-#include "controllers/PidController.hpp"
+#include "vehicles/multirotor/api/MultirotorApiBase.h"
+#include "common/PidController.hpp"
+#include "sensors/SensorCollection.hpp"
//sensors
#include "sensors/barometer/BarometerBase.hpp"
@@ -36,7 +36,7 @@
namespace msr { namespace airlib {
-class MavLinkDroneController : public DroneControllerBase
+class MavLinkMultirotorApi : public MultirotorApiBase
{
public:
typedef msr::airlib::GeoPoint GeoPoint;
@@ -45,13 +45,12 @@ class MavLinkDroneController : public DroneControllerBase
typedef msr::airlib::Quaternionr Quaternionr;
typedef common_utils::Utils Utils;
typedef msr::airlib::real_T real_T;
- typedef msr::airlib::MultiRotor MultiRotor;
typedef msr::airlib::AirSimSettings::MavLinkConnectionInfo MavLinkConnectionInfo;
public:
//required for pimpl
- MavLinkDroneController();
- virtual ~MavLinkDroneController();
+ MavLinkMultirotorApi();
+ virtual ~MavLinkMultirotorApi();
//non-base interface specific to MavLinKDroneController
void initialize(const MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation);
@@ -62,21 +61,16 @@ class MavLinkDroneController : public DroneControllerBase
void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height);
bool hasVideoRequest();
- //*** Start: VehicleControllerBase implementation ***//
+ //*** Start: VehicleApiBase implementation ***//
virtual void reset() override;
virtual void update() override;
- virtual size_t getVertexCount() const override;
- virtual real_T getVertexControlSignal(unsigned int rotor_index) const override;
virtual void getStatusMessages(std::vector& messages) override;
- virtual bool isAvailable(std::string& message) const override;
virtual bool isApiControlEnabled() const override;
virtual void enableApiControl(bool is_enabled) override;
- virtual void setSimulationMode(bool is_set) override;
- virtual Pose getDebugPose() const override;
- //*** End: VehicleControllerBase implementation ***//
+ //*** End: VehicleApiBase implementation ***//
- //*** Start: DroneControllerBase implementation ***//
+ //*** Start: MultirotorApiBase implementation ***//
public:
virtual Kinematics::State getKinematicsEstimated() const override;
virtual Vector3r getPosition() const override;
@@ -84,31 +78,28 @@ class MavLinkDroneController : public DroneControllerBase
virtual Quaternionr getOrientation() const override;
virtual LandedState getLandedState() const override;
virtual RCData getRCData() const override;
- virtual void setRCData(const RCData& rcData) override;
- virtual bool armDisarm(bool arm, CancelableBase& cancelable_action) override;
- virtual bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) override;
- virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action) override;
- virtual bool goHome(CancelableBase& cancelable_action) override;
- virtual bool hover(CancelableBase& cancelable_action) override;
+ virtual bool armDisarm(bool arm) override;
+ virtual bool takeoff(float max_wait_seconds) override;
+ virtual bool land(float max_wait_seconds) override;
+ virtual bool goHome() override;
+ virtual bool hover() override;
virtual GeoPoint getHomeGeoPoint() const override;
virtual GeoPoint getGpsLocation() const override;
- virtual void reportTelemetry(float renderTime) override;
+ virtual void reportTelemetry(float renderTime);
virtual float getCommandPeriod() const override;
virtual float getTakeoffZ() const override;
virtual float getDistanceAccuracy() const override;
- virtual bool loopCommandPre() override;
- virtual void loopCommandPost() override;
protected:
virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override;
virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override;
virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override;
virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override;
virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override;
- const VehicleParams& getVehicleParams() const override;
- //*** End: DroneControllerBase implementation ***//
+ const MultirotorApiParams& getVehicleParams() const override;
+ //*** End: MultirotorApiBase implementation ***//
private: //pimpl
struct impl;
@@ -133,1389 +124,1332 @@ class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog
std::shared_ptr proxy_;
};
-struct MavLinkDroneController::impl {
+struct MavLinkMultirotorApi::impl {
public:
- static const int pixhawkVendorId = 9900; ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow
- static const int pixhawkFMUV4ProductId = 18; ///< Product ID for Pixhawk V2 board
- static const int pixhawkFMUV2ProductId = 17; ///< Product ID for Pixhawk V2 board
- static const int pixhawkFMUV2OldBootloaderProductId = 22; ///< Product ID for Bootloader on older Pixhawk V2 boards
- static const int pixhawkFMUV1ProductId = 16; ///< Product ID for PX4 FMU V1 board
- static const int RotorControlsCount = 8;
- static const int messageReceivedTimeout = 10; ///< Seconds
-
- std::shared_ptr logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_;
-
- size_t status_messages_MaxSize = 5000;
-
- std::shared_ptr hil_node_;
- std::shared_ptr connection_;
- std::shared_ptr video_server_;
- std::shared_ptr mav_vehicle_control_;
-
- mavlinkcom::MavLinkAttPosMocap MocapPoseMessage;
- mavlinkcom::MavLinkHeartbeat HeartbeatMessage;
- mavlinkcom::MavLinkSetMode SetModeMessage;
- mavlinkcom::MavLinkStatustext StatusTextMessage;
- mavlinkcom::MavLinkHilControls HilControlsMessage;
- mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage;
- mavlinkcom::MavLinkCommandLong CommandLongMessage;
-
- mavlinkcom::MavLinkHilSensor last_sensor_message_;
- mavlinkcom::MavLinkDistanceSensor last_distance_message_;
- mavlinkcom::MavLinkHilGps last_gps_message_;
-
- std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, hil_controls_mutex_, last_message_mutex_;
- MavLinkDroneController* parent_;
-
- impl(MavLinkDroneController* parent)
+ impl(MavLinkMultirotorApi* parent)
: parent_(parent)
{
}
- //variables required for VehicleControllerBase implementation
- MavLinkConnectionInfo connection_info_;
- bool is_any_heartbeat_, is_hil_mode_set_, is_armed_;
- bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1?
- float rotor_controls_[RotorControlsCount];
- std::queue status_messages_;
- int hil_state_freq_;
- bool actuators_message_supported_;
- const SensorCollection* sensors_; //this is optional
- uint64_t last_gps_time_;
- bool was_reset_;
- Pose debug_pose_;
- std::string is_available_message_;
- bool is_available_;
-
- //additional variables required for DroneControllerBase implementation
- //this is optional for methods that might not use vehicle commands
- std::shared_ptr mav_vehicle_;
- int state_version_;
- mavlinkcom::VehicleState current_state;
- float target_height_;
- bool is_api_control_enabled_;
- bool is_simulation_mode_;
- PidController thrust_controller_;
- common_utils::Timer hil_message_timer_;
- common_utils::Timer sitl_message_timer_;
-
void initialize(const MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation)
{
connection_info_ = connection_info;
sensors_ = sensors;
is_simulation_mode_ = is_simulation;
- try {
- openAllConnections();
- is_available_ = true;
- }
- catch (std::exception& ex) {
- is_available_ = false;
- is_available_message_ = Utils::stringf("Failed to connect: %s", ex.what());
- }
+ openAllConnections();
}
- bool isAvailable(std::string& message) const
+ MavLinkConnectionInfo getMavConnectionInfo() const
{
- if (!is_available_)
- message = is_available_message_;
- return is_available_;
+ return connection_info_;
}
- MavLinkConnectionInfo getMavConnectionInfo() const
+ //*** Start: VehicleApiBase implementation ***//
+ void reset()
{
- return connection_info_;
+ resetState();
+ was_reset_ = true;
+ setNormalMode();
}
- void normalizeRotorControls()
+ const ImuBase* getImu() const
{
- //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case
- //we normalize them to 0 to 1 for PX4
- if (!is_controls_0_1_) {
- // change -1 to 1 to 0 to 1.
- for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
- rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f;
- }
- }
- else {
- //this applies to PX4 and may work differently on other firmwares.
- //We use 0.2 as idle rotors which leaves out range of 0.8
- for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
- rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f);
- }
- }
+ return static_cast(sensors_->getByType(SensorBase::SensorType::Imu));
+ }
+ const MagnetometerBase* getMagnetometer() const
+ {
+ return static_cast(sensors_->getByType(SensorBase::SensorType::Magnetometer));
+ }
+ const BarometerBase* getBarometer() const
+ {
+ return static_cast(sensors_->getByType(SensorBase::SensorType::Barometer));
+ }
+ const DistanceBase* getDistance() const
+ {
+ return static_cast(sensors_->getByType(SensorBase::SensorType::Distance));
+ }
+ const GpsBase* getGps() const
+ {
+ return static_cast(sensors_->getByType(SensorBase::SensorType::Gps));
}
- void initializeMavSubscriptions()
+ void update()
{
- if (connection_ != nullptr && mav_vehicle_ != nullptr) {
- is_any_heartbeat_ = false;
- is_hil_mode_set_ = false;
- is_armed_ = false;
- is_controls_0_1_ = true;
- Utils::setValue(rotor_controls_, 0.0f);
- //TODO: main_node_->setMessageInterval(...);
- connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) {
- unused(connection);
- processMavMessages(msg);
- });
+ if (sensors_ == nullptr || connection_ == nullptr || !connection_->isOpen())
+ return;
- // listen to the other mavlink connection also
- auto mavcon = mav_vehicle_->getConnection();
- if (mavcon != connection_) {
- mavcon->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) {
- unused(connection);
- processMavMessages(msg);
- });
- }
- }
- }
+ //send sensor updates
+ const auto& imu_output = getImu()->getOutput();
+ const auto& mag_output = getMagnetometer()->getOutput();
+ const auto& baro_output = getBarometer()->getOutput();
- bool sendTestMessage(std::shared_ptr node) {
- try {
- // try and send a test message.
- mavlinkcom::MavLinkHeartbeat test;
- test.autopilot = static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4);
- test.type = static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS);
- test.base_mode = 0;
- test.custom_mode = 0;
- test.mavlink_version = 3;
- node->sendMessage(test);
- test.system_status = 0;
- return true;
- }
- catch (std::exception) {
- return false;
+ sendHILSensor(imu_output.linear_acceleration,
+ imu_output.angular_velocity,
+ mag_output.magnetic_field_body,
+ baro_output.pressure * 0.01f /*Pa to Milibar */, baro_output.altitude);
+
+
+ const auto * distance = getDistance();
+ if (distance) {
+ const auto& distance_output = distance->getOutput();
+ float pitch, roll, yaw;
+ VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw);
+
+ sendDistanceSensor(distance_output.min_distance / 100, //m -> cm
+ distance_output.max_distance / 100, //m -> cm
+ distance_output.distance,
+ 0, //sensor type: //TODO: allow changing in settings?
+ 77, //sensor id, //TODO: should this be something real?
+ pitch); //TODO: convert from radians to degrees?
}
- }
- bool connectToLogViewer()
- {
- //set up logviewer proxy
- if (connection_info_.logviewer_ip_address.size() > 0) {
- std::shared_ptr connection;
- createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip,
- logviewer_proxy_, connection);
- if (!sendTestMessage(logviewer_proxy_)) {
- // error talking to log viewer, so don't keep trying, and close the connection also.
- logviewer_proxy_->getConnection()->close();
- logviewer_proxy_ = nullptr;
- }
+ const auto gps = getGps();
+ if (gps != nullptr) {
+ const auto& gps_output = gps->getOutput();
- std::shared_ptr out_connection;
- createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip,
- logviewer_out_proxy_, out_connection);
- if (!sendTestMessage(logviewer_out_proxy_)) {
- // error talking to log viewer, so don't keep trying, and close the connection also.
- logviewer_out_proxy_->getConnection()->close();
- logviewer_out_proxy_ = nullptr;
- }
- else if (mav_vehicle_ != nullptr) {
- mav_vehicle_->getConnection()->startLoggingSendMessage(std::make_shared(logviewer_out_proxy_));
+ //send GPS
+ if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
+ last_gps_time_ = gps_output.gnss.time_utc;
+ Vector3r gps_velocity = gps_output.gnss.velocity;
+ Vector3r gps_velocity_xy = gps_velocity;
+ gps_velocity_xy.z() = 0;
+ float gps_cog;
+ if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f))
+ gps_cog = 0;
+ else
+ gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x()));
+ if (gps_cog < 0)
+ gps_cog += 360;
+
+ sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog,
+ gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10);
}
}
- return logviewer_proxy_ != nullptr;
+
+ //must be done at the end
+ if (was_reset_)
+ was_reset_ = false;
}
- bool connectToQGC()
+ void getStatusMessages(std::vector& messages)
{
- if (connection_info_.qgc_ip_address.size() > 0) {
- std::shared_ptr connection;
- createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection);
- if (!sendTestMessage(qgc_proxy_)) {
- // error talking to QGC, so don't keep trying, and close the connection also.
- qgc_proxy_->getConnection()->close();
- qgc_proxy_ = nullptr;
- }
- else {
- connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) {
- unused(connection_val);
- processQgcMessages(msg);
- });
- }
+ messages.clear();
+ std::lock_guard guard(status_text_mutex_);
+
+ while (!status_messages_.empty()) {
+ messages.push_back(status_messages_.front());
+ status_messages_.pop();
}
- return qgc_proxy_ != nullptr;
}
-
- void createProxy(std::string name, std::string ip, int port, string local_host_ip,
- std::shared_ptr& node, std::shared_ptr& connection)
+ void openAllConnections()
{
- if (connection_ == nullptr)
- throw std::domain_error("MavLinkDroneController requires connection object to be set before createProxy call");
-
- connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port);
-
- // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint
- // and all other messages are funnelled through from PX4 via the Join method below.
- node = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid);
- node->connect(connection);
+ close(); //just in case if connections were open
+ resetState(); //reset all variables we might have changed during last session
- // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be
- // send directly to the PX4 (using whatever sysid/compid comes from that remote node).
- connection_->join(connection);
+ connect();
+ connectToLogViewer();
+ connectToQGC();
- auto mavcon = mav_vehicle_->getConnection();
- if (mavcon != connection_) {
- mavcon->join(connection);
- }
}
-
- static std::string findPX4()
+ void closeAllConnection()
{
- auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0);
- for (auto iter = result.begin(); iter != result.end(); iter++)
- {
- mavlinkcom::SerialPortInfo info = *iter;
- if (
- (
- (info.vid == pixhawkVendorId) &&
- (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId)
- ) ||
- (
- (info.displayName.find(L"PX4_") != std::string::npos)
- )
- )
- {
- // printf("Auto Selecting COM port: %S\n", info.displayName.c_str());
- return std::string(info.portName.begin(), info.portName.end());
- }
- }
- return "";
+ close();
}
+ //*** End: VehicleApiBase implementation ***//
- void connect()
+ void getMocapPose(Vector3r& position, Quaternionr& orientation)
{
- createMavConnection(connection_info_);
- initializeMavSubscriptions();
+ position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z;
+ orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1];
+ orientation.y() = MocapPoseMessage.q[2]; orientation.z() = MocapPoseMessage.q[3];
}
- void createMavConnection(const MavLinkConnectionInfo& connection_info)
+ void sendCollision(float normalX, float normalY, float normalZ)
{
- if (connection_info.use_serial) {
- createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate);
- }
- else {
- createMavUdpConnection(connection_info.ip_address, connection_info.ip_port);
- }
- //Uncomment below for sending images over MavLink
- //connectToVideoServer();
+ checkVehicle();
+
+ mavlinkcom::MavLinkCollision collision{};
+ collision.src = 1; //provider of data is MavLink system in id field
+ collision.id = mav_vehicle_->getLocalSystemId();
+ collision.action = static_cast(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT);
+ collision.threat_level = static_cast(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE);
+ // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off.
+ collision.time_to_minimum_delta = normalX;
+ collision.altitude_minimum_delta = normalY;
+ collision.horizontal_minimum_delta = normalZ;
+ mav_vehicle_->sendMessage(collision);
}
- void createMavUdpConnection(const std::string& ip, int port)
+ bool hasVideoRequest()
{
- close();
+ mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req;
+ return video_server_->hasVideoRequest(image_req);
+ }
- if (ip == "") {
- throw std::invalid_argument("UdpIp setting is invalid.");
- }
+ void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height)
+ {
+ const int MAVLINK_DATA_STREAM_IMG_PNG = 6;
+ video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0);
+ }
- if (port == 0) {
- throw std::invalid_argument("UdpPort setting has an invalid value.");
+ void setNormalMode()
+ {
+ if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) {
+
+ // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode
+ std::lock_guard guard(set_mode_mutex_);
+ int mode = mav_vehicle_->getVehicleState().mode;
+ mode &= ~static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
+
+ mavlinkcom::MavCmdDoSetMode cmd;
+ cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
+ cmd.Mode = static_cast(mode);
+ mav_vehicle_->sendCommand(cmd);
+
+ is_hil_mode_set_ = false;
}
+ }
- addStatusMessage(Utils::stringf("Connecting to UDP port %d, local IP %s, remote IP...", port, connection_info_.local_host_ip.c_str(), ip.c_str()));
- connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, ip, port);
- hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid);
- hil_node_->connect(connection_);
- addStatusMessage(std::string("Connected over UDP."));
+ void setHILMode()
+ {
+ if (!is_simulation_mode_)
+ throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode");
- mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
- if (connection_info_.sitl_ip_address != "" && connection_info_.sitl_ip_port != 0 && connection_info_.sitl_ip_port != port) {
- // bugbug: the PX4 SITL mode app cannot receive commands to control the drone over the same mavlink connection
- // as the HIL_SENSOR messages, we must establish a separate mavlink channel for that so that DroneShell works.
- addStatusMessage(Utils::stringf("Connecting to PX4 SITL UDP port %d, local IP %s, remote IP...",
- connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), connection_info_.sitl_ip_address.c_str()));
+ checkVehicle();
- auto sitlconnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("sitl",
- connection_info_.local_host_ip, connection_info_.sitl_ip_address, connection_info_.sitl_ip_port);
- mav_vehicle_->connect(sitlconnection);
+ // add MAV_MODE_FLAG_HIL_ENABLED flag to current mode
+ std::lock_guard guard(set_mode_mutex_);
+ int mode = mav_vehicle_->getVehicleState().mode;
+ mode |= static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
- addStatusMessage(std::string("Connected to SITL over UDP."));
- }
- else {
- mav_vehicle_->connect(connection_);
- }
+ mavlinkcom::MavCmdDoSetMode cmd;
+ cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
+ cmd.Mode = static_cast(mode);
+ mav_vehicle_->sendCommand(cmd);
- mav_vehicle_->startHeartbeat();
+ is_hil_mode_set_ = true;
}
- void createMavSerialConnection(const std::string& port_name, int baud_rate)
+ void close()
{
- close();
-
- std::string port_name_auto = port_name;
- if (port_name_auto == "" || port_name_auto == "*") {
- port_name_auto = findPX4();
- if (port_name_auto == "") {
- throw std::domain_error("Could not detect a connected PX4 flight controller on any USB ports. You can specify USB port in settings.json.");
+ if (connection_ != nullptr) {
+ if (is_hil_mode_set_ && mav_vehicle_ != nullptr) {
+ setNormalMode();
}
+
+ connection_->close();
}
- if (port_name_auto == "") {
- throw std::invalid_argument("USB port for PX4 flight controller is empty. Please set it in settings.json.");
+ if (hil_node_ != nullptr)
+ hil_node_->close();
+
+ if (video_server_ != nullptr)
+ video_server_->close();
+
+ if (logviewer_proxy_ != nullptr) {
+ logviewer_proxy_->close();
+ logviewer_proxy_ = nullptr;
}
- if (baud_rate == 0) {
- throw std::invalid_argument("Baud rate specified in settings.json is 0 which is invalid");
+ if (logviewer_out_proxy_ != nullptr) {
+ if (mav_vehicle_ != nullptr) {
+ mav_vehicle_->getConnection()->stopLoggingSendMessage();
+ }
+ logviewer_out_proxy_->close();
+ logviewer_out_proxy_ = nullptr;
}
- addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate));
- connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate);
- connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mocap messages
- hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid);
- hil_node_->connect(connection_);
- addStatusMessage("Connected to PX4 over serial port.");
+ if (qgc_proxy_ != nullptr) {
+ qgc_proxy_->close();
+ qgc_proxy_ = nullptr;
+ }
+ if (mav_vehicle_ != nullptr) {
+ mav_vehicle_->close();
+ mav_vehicle_ = nullptr;
+ }
+ }
- mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
- mav_vehicle_->connect(connection_); // in this case we can use the same connection.
- mav_vehicle_->startHeartbeat();
+ //additional methods for MultirotorApiBase
+ void updateState()
+ {
+ StatusLock lock(parent_);
+ if (mav_vehicle_ != nullptr) {
+ int version = mav_vehicle_->getVehicleStateVersion();
+ if (version != state_version_)
+ {
+ current_state = mav_vehicle_->getVehicleState();
+ state_version_ = version;
+ }
+ }
+ }
+
+ Kinematics::State getKinematicsEstimated()
+ {
+ updateState();
+ Kinematics::State state;
+ //TODO: reduce code duplication below?
+ state.pose.position = Vector3r(current_state.local_est.pos.x, current_state.local_est.pos.y, current_state.local_est.pos.z);
+ state.pose.orientation = VectorMath::toQuaternion(current_state.attitude.pitch, current_state.attitude.roll, current_state.attitude.yaw);
+ state.twist.linear = Vector3r(current_state.local_est.lin_vel.x, current_state.local_est.lin_vel.y, current_state.local_est.lin_vel.z);
+ state.twist.angular = Vector3r(current_state.attitude.roll_rate, current_state.attitude.pitch_rate, current_state.attitude.yaw_rate);
+ state.pose.position = Vector3r(current_state.local_est.acc.x, current_state.local_est.acc.y, current_state.local_est.acc.z);
+ //TODO: how do we get angular acceleration?
+ return state;
}
- mavlinkcom::MavLinkHilSensor getLastSensorMessage()
+ Vector3r getPosition()
{
- std::lock_guard guard(last_message_mutex_);
- return last_sensor_message_;
+ updateState();
+ return Vector3r(current_state.local_est.pos.x, current_state.local_est.pos.y, current_state.local_est.pos.z);
}
- mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage()
+ Vector3r getVelocity()
{
- std::lock_guard guard(last_message_mutex_);
- return last_distance_message_;
+ updateState();
+ return Vector3r(current_state.local_est.lin_vel.x, current_state.local_est.lin_vel.y, current_state.local_est.lin_vel.z);
}
- mavlinkcom::MavLinkHilGps getLastGpsMessage()
+ Quaternionr getOrientation()
{
- std::lock_guard guard(last_message_mutex_);
- return last_gps_message_;
+ updateState();
+ return VectorMath::toQuaternion(current_state.attitude.pitch, current_state.attitude.roll, current_state.attitude.yaw);
}
- void setArmed(bool armed)
+ GeoPoint getHomeGeoPoint()
{
- is_armed_ = armed;
- if (!armed) {
- //reset motor controls
- for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
- rotor_controls_[i] = 0;
- }
- }
+ updateState();
+ if (current_state.home.is_set)
+ return GeoPoint(current_state.home.global_pos.lat, current_state.home.global_pos.lon, current_state.home.global_pos.alt);
+ else
+ return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan());
}
- void processQgcMessages(const mavlinkcom::MavLinkMessage& msg)
+ GeoPoint getGpsLocation()
{
- if (msg.msgid == MocapPoseMessage.msgid) {
- std::lock_guard guard(mocap_pose_mutex_);
- MocapPoseMessage.decode(msg);
- getMocapPose(debug_pose_.position, debug_pose_.orientation);
- }
- //else ignore message
+ updateState();
+ return GeoPoint(current_state.global_est.pos.lat, current_state.global_est.pos.lon, current_state.global_est.pos.alt);
}
- void addStatusMessage(const std::string& message)
+
+ LandedState getLandedState()
{
- std::lock_guard guard_status(status_text_mutex_);
- //if queue became too large, clear it first
- if (status_messages_.size() > status_messages_MaxSize)
- Utils::clear(status_messages_, status_messages_MaxSize - status_messages_.size());
- status_messages_.push(message);
+ updateState();
+ return current_state.controls.landed ? LandedState::Landed : LandedState::Flying;
}
- void processMavMessages(const mavlinkcom::MavLinkMessage& msg)
+ //administrative
+
+ bool armDisarm(bool arm)
{
- if (msg.msgid == HeartbeatMessage.msgid) {
- std::lock_guard guard_heartbeat(heartbeat_mutex_);
+ unused(arm);
+ checkVehicle();
+ bool rc = false;
+ mav_vehicle_->armDisarm(arm).wait(10000, &rc);
+ return rc;
+ }
- //TODO: have MavLinkNode track armed state so we don't have to re-decode message here again
- HeartbeatMessage.decode(msg);
- bool armed = (HeartbeatMessage.base_mode & static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0;
- setArmed(armed);
- if (!is_any_heartbeat_) {
- is_any_heartbeat_ = true;
- if (HeartbeatMessage.autopilot == static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4) &&
- HeartbeatMessage.type == static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_FIXED_WING)) {
- // PX4 will scale fixed wing servo outputs to -1 to 1
- // and it scales multi rotor servo outpus to 0 to 1.
- is_controls_0_1_ = false;
- }
- } else if (is_simulation_mode_ && !is_hil_mode_set_) {
- setHILMode();
- }
- } else if (msg.msgid == StatusTextMessage.msgid) {
- StatusTextMessage.decode(msg);
- //lock is established by below method
- addStatusMessage(std::string(StatusTextMessage.text));
- } else if (msg.msgid == CommandLongMessage.msgid) {
- CommandLongMessage.decode(msg);
- if (CommandLongMessage.command == static_cast(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) {
- int msg_id = static_cast(CommandLongMessage.param1 + 0.5);
- if (msg_id == 115) { //HIL_STATE_QUATERNION
- hil_state_freq_ = static_cast(CommandLongMessage.param2 + 0.5);
- }
- }
- } else if (msg.msgid == HilControlsMessage.msgid) {
- if (!actuators_message_supported_) {
- std::lock_guard guard_controls(hil_controls_mutex_);
-
- HilControlsMessage.decode(msg);
- //is_arned_ = (HilControlsMessage.mode & 128) > 0; //TODO: is this needed?
- rotor_controls_[0] = HilControlsMessage.roll_ailerons;
- rotor_controls_[1] = HilControlsMessage.pitch_elevator;
- rotor_controls_[2] = HilControlsMessage.yaw_rudder;
- rotor_controls_[3] = HilControlsMessage.throttle;
- rotor_controls_[4] = HilControlsMessage.aux1;
- rotor_controls_[5] = HilControlsMessage.aux2;
- rotor_controls_[6] = HilControlsMessage.aux3;
- rotor_controls_[7] = HilControlsMessage.aux4;
+ bool isApiControlEnabled()
+ {
+ return is_api_control_enabled_;
+ }
- normalizeRotorControls();
- }
+ void enableApiControl(bool is_enabled)
+ {
+ checkVehicle();
+ if (is_enabled) {
+ mav_vehicle_->requestControl();
+ is_api_control_enabled_ = true;
}
- else if (msg.msgid == HilActuatorControlsMessage.msgid) {
- actuators_message_supported_ = true;
-
- std::lock_guard guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl
-
- HilActuatorControlsMessage.decode(msg);
- //is_arned_ = (HilControlsMessage.mode & 128) > 0; //TODO: is this needed?
- for (auto i = 0; i < 8; ++i) {
- rotor_controls_[i] = HilActuatorControlsMessage.controls[i];
- }
- normalizeRotorControls();
+ else {
+ mav_vehicle_->releaseControl();
+ is_api_control_enabled_ = false;
}
- //else ignore message
}
- void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt)
+ void setSimulationMode(bool is_set)
{
- if (!is_simulation_mode_)
- throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
-
- mavlinkcom::MavLinkHilSensor hil_sensor;
- hil_sensor.time_usec = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0);
-
- hil_sensor.xacc = acceleration.x();
- hil_sensor.yacc = acceleration.y();
- hil_sensor.zacc = acceleration.z();
- hil_sensor.xgyro = gyro.x();
- hil_sensor.ygyro = gyro.y();
- hil_sensor.zgyro = gyro.z();
-
- hil_sensor.xmag = mag.x();
- hil_sensor.ymag = mag.y();
- hil_sensor.zmag = mag.z();
-
- hil_sensor.abs_pressure = abs_pressure;
- hil_sensor.pressure_alt = pressure_alt;
- //TODO: enable temperature? diff_pressure
- hil_sensor.fields_updated = was_reset_ ? (1 << 31) : 0;
-
- if (hil_node_ != nullptr) {
- hil_node_->sendMessage(hil_sensor);
+ if (connection_ != nullptr) {
+ //TODO: can we do this?
+ throw std::domain_error("Cannot call setSimulationMode after connection was already established for MavLink");
}
- std::lock_guard guard(last_message_mutex_);
- last_sensor_message_ = hil_sensor;
+ is_simulation_mode_ = is_set;
}
- void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, float orientation)
+ bool takeoff(float max_wait_seconds)
{
- if (!is_simulation_mode_)
- throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode");
-
- mavlinkcom::MavLinkDistanceSensor distance_sensor;
- distance_sensor.time_boot_ms = static_cast(Utils::getTimeSinceEpochNanos() / 1000000.0);
-
- distance_sensor.min_distance = static_cast(min_distance);
- distance_sensor.max_distance = static_cast(max_distance);
- distance_sensor.current_distance = static_cast(current_distance);
- distance_sensor.type = static_cast(sensor_type);
- distance_sensor.id = static_cast(sensor_id);
- distance_sensor.orientation = static_cast(orientation);
- //TODO: use covariance parameter?
+ checkVehicle();
- if (hil_node_ != nullptr) {
- hil_node_->sendMessage(distance_sensor);
+ bool rc = false;
+ auto vec = getPosition();
+ float z = vec.z() + getTakeoffZ();
+ if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, 0.0f /* yaw */).wait(static_cast(max_wait_seconds * 1000), &rc))
+ {
+ throw VehicleMoveException("TakeOff command - timeout waiting for response");
}
+ if (!rc) {
+ throw VehicleMoveException("TakeOff command rejected by drone");
+ }
+ if (max_wait_seconds <= 0)
+ return true; // client doesn't want to wait.
- std::lock_guard guard(last_message_mutex_);
- last_distance_message_ = distance_sensor;
+ return parent_->waitForZ(max_wait_seconds, z, getDistanceAccuracy(), );
}
- void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog,
- float eph, float epv, int fix_type, unsigned int satellites_visible)
+ bool hover()
{
- if (!is_simulation_mode_)
- throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode");
-
- mavlinkcom::MavLinkHilGps hil_gps;
- hil_gps.time_usec = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0);
- hil_gps.lat = static_cast(geo_point.latitude * 1E7);
- hil_gps.lon = static_cast(geo_point.longitude* 1E7);
- hil_gps.alt = static_cast(geo_point.altitude * 1000);
- hil_gps.vn = static_cast(velocity.x() * 100);
- hil_gps.ve = static_cast(velocity.y() * 100);
- hil_gps.vd = static_cast(velocity.z() * 100);
- hil_gps.eph = static_cast(eph * 100);
- hil_gps.epv = static_cast(epv * 100);
- hil_gps.fix_type = static_cast(fix_type);
- hil_gps.vel = static_cast(velocity_xy * 100);
- hil_gps.cog = static_cast(cog * 100);
- hil_gps.satellites_visible = static_cast(satellites_visible);
+ bool rc = false;
+ checkVehicle();
+ mavlinkcom::AsyncResult result = mav_vehicle_->loiter();
+ //auto start_time = std::chrono::system_clock::now();
+ while (!.isCancelled())
+ {
+ if (result.wait(100, &rc))
+ {
+ break;
+ }
+ }
+ return rc;
+ }
- if (hil_node_ != nullptr) {
- hil_node_->sendMessage(hil_gps);
+ bool land(float max_wait_seconds)
+ {
+ // bugbug: really need a downward pointing distance to ground sensor to do this properly, for now
+ // we assume the ground is relatively flat an we are landing roughly at the home altitude.
+ updateState();
+ checkVehicle();
+ if (current_state.home.is_set)
+ {
+ bool rc = false;
+ if (!mav_vehicle_->land(current_state.global_est.pos.lat, current_state.global_est.pos.lon, current_state.home.global_pos.alt).wait(10000, &rc))
+ {
+ throw VehicleMoveException("Landing command - timeout waiting for response from drone");
+ }
+ else if (!rc) {
+ throw VehicleMoveException("Landing command rejected by drone");
+ }
+ }
+ else
+ {
+ throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude. Could fix this if we hook up a distance to ground sensor...");
}
- if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) {
- //Utils::DebugBreak();
- Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError);
+ // Wait for landed state (or user cancelation)
+ if (!parent_->waitForFunction([&]() {
+ updateState();
+ return current_state.controls.landed;
+ }, max_wait_seconds, ))
+ {
+ throw VehicleMoveException("Drone hasn't reported a landing state");
}
+ return true;
+ }
- std::lock_guard guard(last_message_mutex_);
- last_gps_message_ = hil_gps;
+ bool goHome()
+ {
+ checkVehicle();
+ bool rc = false;
+ if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(10000, &rc)) {
+ throw VehicleMoveException("goHome - timeout waiting for response from drone");
+ }
+ return rc;
}
- real_T getVertexControlSignal(unsigned int rotor_index)
+ void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate)
{
- if (!is_simulation_mode_)
- throw std::logic_error("Attempt to read simulated motor controls while not in simulation mode");
+ checkVehicle();
+ mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle);
+ }
+ void commandRollPitchZ(float pitch, float roll, float z, float yaw)
+ {
+ if (target_height_ != -z) {
+ // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best
+ // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not
+ // been tested on a real drone outside jMavSim, so it may need recalibrating...
+ thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
+ target_height_ = -z;
+ }
+ checkVehicle();
+ auto state = mav_vehicle_->getVehicleState();
+ float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
+ mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust);
+ }
+ void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode)
+ {
+ checkVehicle();
+ float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
+ mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw);
+ }
+ void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode)
+ {
+ checkVehicle();
+ float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
+ mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw);
+ }
+ void commandPosition(float x, float y, float z, const YawMode& yaw_mode)
+ {
+ checkVehicle();
+ float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
+ mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw);
+ }
- std::lock_guard guard(hil_controls_mutex_);
- return rotor_controls_[rotor_index];
+ RCData getRCData()
+ {
+ throw VehicleCommandNotImplementedException("getRCData() function is not yet implemented");
}
- void resetState()
+ void setRCData(const RCData& rcData)
{
- //reset state
- is_any_heartbeat_ = is_hil_mode_set_ = is_armed_ = false;
- is_controls_0_1_ = true;
- hil_state_freq_ = -1;
- actuators_message_supported_ = false;
- last_gps_time_ = 0;
- state_version_ = 0;
- current_state = mavlinkcom::VehicleState();
- target_height_ = 0;
- is_api_control_enabled_ = false;
- thrust_controller_ = PidController();
- Utils::setValue(rotor_controls_, 0.0f);
- was_reset_ = false;
- debug_pose_ = Pose::nanPose();
+ unused(rcData);
+ //TODO: use RC data to control MavLink drone
}
- //*** Start: VehicleControllerBase implementation ***//
- void reset()
+ bool validateRCData(const RCData& rc_data)
{
- resetState();
- was_reset_ = true;
- setNormalMode();
+ unused(rc_data);
+ return true;
}
- const ImuBase* getImu() const
+ //drone parameters
+ float getCommandPeriod()
{
- return static_cast(sensors_->getByType(SensorBase::SensorType::Imu));
+ return 1.0f / 50; //1 period of 50hz
}
- const MagnetometerBase* getMagnetometer() const
+ float getTakeoffZ()
{
- return static_cast(sensors_->getByType(SensorBase::SensorType::Magnetometer));
+ // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe
+ // enough to get out of the backwash turbulance. Negative due to NED coordinate system.
+ return -3.0f;
}
- const BarometerBase* getBarometer() const
+ float getDistanceAccuracy()
{
- return static_cast(sensors_->getByType(SensorBase::SensorType::Barometer));
+ return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance travelled
}
- const DistanceBase* getDistance() const
+ const MultirotorApiParams& getVehicleParams()
{
- return static_cast(sensors_->getByType(SensorBase::SensorType::Distance));
+ return getInternalVehicleParams(); //defaults are good for PX4 generic quadrocopter.
}
- const GpsBase* getGps() const
+ //TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval
+ const MultirotorApiParams& getInternalVehicleParams()
{
- return static_cast(sensors_->getByType(SensorBase::SensorType::Gps));
+ static const MultirotorApiParams vehicle_params_;
+ return vehicle_params_; //defaults are good for DJI Matrice 100
}
- void update()
+ void reportTelemetry(float renderTime)
{
- if (sensors_ == nullptr || connection_ == nullptr || !connection_->isOpen())
+ if (logviewer_proxy_ == nullptr || connection_ == nullptr || mav_vehicle_ == nullptr) {
return;
-
- //send sensor updates
- const auto& imu_output = getImu()->getOutput();
- const auto& mag_output = getMagnetometer()->getOutput();
- const auto& baro_output = getBarometer()->getOutput();
-
- sendHILSensor(imu_output.linear_acceleration,
- imu_output.angular_velocity,
- mag_output.magnetic_field_body,
- baro_output.pressure * 0.01f /*Pa to Milibar */, baro_output.altitude);
-
-
- const auto * distance = getDistance();
- if (distance) {
- const auto& distance_output = distance->getOutput();
- float pitch, roll, yaw;
- VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw);
-
- sendDistanceSensor(distance_output.min_distance / 100, //m -> cm
- distance_output.max_distance / 100, //m -> cm
- distance_output.distance,
- 0, //sensor type: //TODO: allow changing in settings?
- 77, //sensor id, //TODO: should this be something real?
- pitch); //TODO: convert from radians to degrees?
}
-
- const auto gps = getGps();
- if (gps != nullptr) {
- const auto& gps_output = gps->getOutput();
-
- //send GPS
- if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
- last_gps_time_ = gps_output.gnss.time_utc;
- Vector3r gps_velocity = gps_output.gnss.velocity;
- Vector3r gps_velocity_xy = gps_velocity;
- gps_velocity_xy.z() = 0;
- float gps_cog;
- if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f))
- gps_cog = 0;
- else
- gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x()));
- if (gps_cog < 0)
- gps_cog += 360;
-
- sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog,
- gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10);
+ mavlinkcom::MavLinkTelemetry data;
+ connection_->getTelemetry(data);
+ if (data.messagesReceived == 0) {
+ if (!hil_message_timer_.started()) {
+ hil_message_timer_.start();
+ } else if (hil_message_timer_.seconds() > messageReceivedTimeout) {
+ addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again");
}
+ } else {
+ hil_message_timer_.stop();
}
- //must be done at the end
- if (was_reset_)
- was_reset_ = false;
- }
+ // listen to the other mavlink connection also
+ auto mavcon = mav_vehicle_->getConnection();
+ if (mavcon != connection_) {
+ mavlinkcom::MavLinkTelemetry sitl;
+ mavcon->getTelemetry(sitl);
- void getStatusMessages(std::vector& messages)
- {
- messages.clear();
- std::lock_guard guard(status_text_mutex_);
+ data.handlerMicroseconds += sitl.handlerMicroseconds;
+ data.messagesHandled += sitl.messagesHandled;
+ data.messagesReceived += sitl.messagesReceived;
+ data.messagesSent += sitl.messagesSent;
- while (!status_messages_.empty()) {
- messages.push_back(status_messages_.front());
- status_messages_.pop();
+ if (sitl.messagesReceived == 0)
+ {
+ if (!sitl_message_timer_.started()) {
+ sitl_message_timer_.start();
+ }
+ else if (sitl_message_timer_.seconds() > messageReceivedTimeout) {
+ addStatusMessage("not receiving any messages from SITL, please restart your SITL node and try again");
+ }
+ }
+ else {
+ sitl_message_timer_.stop();
+ }
}
- }
-
- void openAllConnections()
- {
- close(); //just in case if connections were open
- resetState(); //reset all variables we might have changed during last session
- connect();
- connectToLogViewer();
- connectToQGC();
-
- }
- void closeAllConnection()
- {
- close();
- }
- //*** End: VehicleControllerBase implementation ***//
-
- void getMocapPose(Vector3r& position, Quaternionr& orientation)
- {
- position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z;
- orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1];
- orientation.y() = MocapPoseMessage.q[2]; orientation.z() = MocapPoseMessage.q[3];
+ data.renderTime = static_cast(renderTime * 1000000);// microseconds
+ logviewer_proxy_->sendMessage(data);
}
- void sendCollision(float normalX, float normalY, float normalZ)
+ bool startOffboardMode()
{
checkVehicle();
-
- mavlinkcom::MavLinkCollision collision{};
- collision.src = 1; //provider of data is MavLink system in id field
- collision.id = mav_vehicle_->getLocalSystemId();
- collision.action = static_cast(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT);
- collision.threat_level = static_cast(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE);
- // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off.
- collision.time_to_minimum_delta = normalX;
- collision.altitude_minimum_delta = normalY;
- collision.horizontal_minimum_delta = normalZ;
- mav_vehicle_->sendMessage(collision);
- }
-
- bool hasVideoRequest()
- {
- mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req;
- return video_server_->hasVideoRequest(image_req);
+ try {
+ mav_vehicle_->requestControl();
+ }
+ catch (std::exception& ex) {
+ ensureSafeMode();
+ addStatusMessage(std::string("Request control failed: ") + ex.what());
+ return false;
+ }
+ return true;
}
- void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height)
+ void endOffboardMode()
{
- const int MAVLINK_DATA_STREAM_IMG_PNG = 6;
- video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0);
+ // bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth.
+ // The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout
+ // when you stop sending move commands and the behavior on timeout is then determined by the drone itself.
+ // mav_vehicle_->releaseControl();
+ ensureSafeMode();
}
- void setNormalMode()
+ void ensureSafeMode()
{
- if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) {
-
- // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode
- std::lock_guard guard(set_mode_mutex_);
- int mode = mav_vehicle_->getVehicleState().mode;
- mode &= ~static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
-
- mavlinkcom::MavCmdDoSetMode cmd;
- cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
- cmd.Mode = static_cast(mode);
- mav_vehicle_->sendCommand(cmd);
-
- is_hil_mode_set_ = false;
+ if (mav_vehicle_ != nullptr) {
+ const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState();
+ if (state.controls.landed || !state.controls.armed) {
+ return;
+ }
}
}
- void setHILMode()
+ bool loopCommandPre()
{
- if (!is_simulation_mode_)
- throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode");
-
-
- checkVehicle();
-
- // add MAV_MODE_FLAG_HIL_ENABLED flag to current mode
- std::lock_guard guard(set_mode_mutex_);
- int mode = mav_vehicle_->getVehicleState().mode;
- mode |= static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
+ return startOffboardMode();
+ }
- mavlinkcom::MavCmdDoSetMode cmd;
- cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
- cmd.Mode = static_cast(mode);
- mav_vehicle_->sendCommand(cmd);
+ void loopCommandPost()
+ {
+ endOffboardMode();
+ }
- is_hil_mode_set_ = true;
+ void checkVehicle() {
+ if (mav_vehicle_ == nullptr) {
+ throw std::logic_error("Cannot perform operation when no vehicle is connected");
+ }
}
- void close()
+private: //methods
+ void normalizeRotorControls()
{
- if (connection_ != nullptr) {
- if (is_hil_mode_set_ && mav_vehicle_ != nullptr) {
- setNormalMode();
+ //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case
+ //we normalize them to 0 to 1 for PX4
+ if (!is_controls_0_1_) {
+ // change -1 to 1 to 0 to 1.
+ for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
+ rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f;
}
-
- connection_->close();
}
-
- if (hil_node_ != nullptr)
- hil_node_->close();
-
- if (video_server_ != nullptr)
- video_server_->close();
-
- if (logviewer_proxy_ != nullptr) {
- logviewer_proxy_->close();
- logviewer_proxy_ = nullptr;
+ else {
+ //this applies to PX4 and may work differently on other firmwares.
+ //We use 0.2 as idle rotors which leaves out range of 0.8
+ for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
+ rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f);
+ }
}
+ }
- if (logviewer_out_proxy_ != nullptr) {
- if (mav_vehicle_ != nullptr) {
- mav_vehicle_->getConnection()->stopLoggingSendMessage();
+ void initializeMavSubscriptions()
+ {
+ if (connection_ != nullptr && mav_vehicle_ != nullptr) {
+ is_any_heartbeat_ = false;
+ is_hil_mode_set_ = false;
+ is_armed_ = false;
+ is_controls_0_1_ = true;
+ Utils::setValue(rotor_controls_, 0.0f);
+ //TODO: main_node_->setMessageInterval(...);
+ connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) {
+ unused(connection);
+ processMavMessages(msg);
+ });
+
+ // listen to the other mavlink connection also
+ auto mavcon = mav_vehicle_->getConnection();
+ if (mavcon != connection_) {
+ mavcon->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) {
+ unused(connection);
+ processMavMessages(msg);
+ });
}
- logviewer_out_proxy_->close();
- logviewer_out_proxy_ = nullptr;
}
+ }
- if (qgc_proxy_ != nullptr) {
- qgc_proxy_->close();
- qgc_proxy_ = nullptr;
+ bool sendTestMessage(std::shared_ptr node) {
+ try {
+ // try and send a test message.
+ mavlinkcom::MavLinkHeartbeat test;
+ test.autopilot = static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4);
+ test.type = static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS);
+ test.base_mode = 0;
+ test.custom_mode = 0;
+ test.mavlink_version = 3;
+ node->sendMessage(test);
+ test.system_status = 0;
+ return true;
}
- if (mav_vehicle_ != nullptr) {
- mav_vehicle_->close();
- mav_vehicle_ = nullptr;
+ catch (std::exception) {
+ return false;
}
}
- //additional methods for DroneControllerBase
- void updateState()
+ bool connectToLogViewer()
{
- StatusLock lock(parent_);
- if (mav_vehicle_ != nullptr) {
- int version = mav_vehicle_->getVehicleStateVersion();
- if (version != state_version_)
- {
- current_state = mav_vehicle_->getVehicleState();
- state_version_ = version;
+ //set up logviewer proxy
+ if (connection_info_.logviewer_ip_address.size() > 0) {
+ std::shared_ptr connection;
+ createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip,
+ logviewer_proxy_, connection);
+ if (!sendTestMessage(logviewer_proxy_)) {
+ // error talking to log viewer, so don't keep trying, and close the connection also.
+ logviewer_proxy_->getConnection()->close();
+ logviewer_proxy_ = nullptr;
+ }
+
+ std::shared_ptr out_connection;
+ createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip,
+ logviewer_out_proxy_, out_connection);
+ if (!sendTestMessage(logviewer_out_proxy_)) {
+ // error talking to log viewer, so don't keep trying, and close the connection also.
+ logviewer_out_proxy_->getConnection()->close();
+ logviewer_out_proxy_ = nullptr;
+ }
+ else if (mav_vehicle_ != nullptr) {
+ mav_vehicle_->getConnection()->startLoggingSendMessage(std::make_shared(logviewer_out_proxy_));
}
}
- }
-
- Kinematics::State getKinematicsEstimated()
- {
- updateState();
- Kinematics::State state;
- //TODO: reduce code duplication below?
- state.pose.position = Vector3r(current_state.local_est.pos.x, current_state.local_est.pos.y, current_state.local_est.pos.z);
- state.pose.orientation = VectorMath::toQuaternion(current_state.attitude.pitch, current_state.attitude.roll, current_state.attitude.yaw);
- state.twist.linear = Vector3r(current_state.local_est.lin_vel.x, current_state.local_est.lin_vel.y, current_state.local_est.lin_vel.z);
- state.twist.angular = Vector3r(current_state.attitude.roll_rate, current_state.attitude.pitch_rate, current_state.attitude.yaw_rate);
- state.pose.position = Vector3r(current_state.local_est.acc.x, current_state.local_est.acc.y, current_state.local_est.acc.z);
- //TODO: how do we get angular acceleration?
- return state;
+ return logviewer_proxy_ != nullptr;
}
- Vector3r getPosition()
+ bool connectToQGC()
{
- updateState();
- return Vector3r(current_state.local_est.pos.x, current_state.local_est.pos.y, current_state.local_est.pos.z);
+ if (connection_info_.qgc_ip_address.size() > 0) {
+ std::shared_ptr connection;
+ createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection);
+ if (!sendTestMessage(qgc_proxy_)) {
+ // error talking to QGC, so don't keep trying, and close the connection also.
+ qgc_proxy_->getConnection()->close();
+ qgc_proxy_ = nullptr;
+ }
+ else {
+ connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) {
+ unused(connection_val);
+ processQgcMessages(msg);
+ });
+ }
+ }
+ return qgc_proxy_ != nullptr;
}
- Vector3r getVelocity()
- {
- updateState();
- return Vector3r(current_state.local_est.lin_vel.x, current_state.local_est.lin_vel.y, current_state.local_est.lin_vel.z);
- }
- Quaternionr getOrientation()
+ void createProxy(std::string name, std::string ip, int port, string local_host_ip,
+ std::shared_ptr& node, std::shared_ptr& connection)
{
- updateState();
- return VectorMath::toQuaternion(current_state.attitude.pitch, current_state.attitude.roll, current_state.attitude.yaw);
- }
+ if (connection_ == nullptr)
+ throw std::domain_error("MavLinkMultirotorApi requires connection object to be set before createProxy call");
- GeoPoint getHomeGeoPoint()
- {
- updateState();
- if (current_state.home.is_set)
- return GeoPoint(current_state.home.global_pos.lat, current_state.home.global_pos.lon, current_state.home.global_pos.alt);
- else
- return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan());
- }
+ connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port);
- GeoPoint getGpsLocation()
- {
- updateState();
- return GeoPoint(current_state.global_est.pos.lat, current_state.global_est.pos.lon, current_state.global_est.pos.alt);
- }
+ // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint
+ // and all other messages are funnelled through from PX4 via the Join method below.
+ node = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid);
+ node->connect(connection);
+ // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be
+ // send directly to the PX4 (using whatever sysid/compid comes from that remote node).
+ connection_->join(connection);
- LandedState getLandedState()
- {
- updateState();
- return current_state.controls.landed ? LandedState::Landed : LandedState::Flying;
+ auto mavcon = mav_vehicle_->getConnection();
+ if (mavcon != connection_) {
+ mavcon->join(connection);
+ }
}
- //administrative
-
- bool armDisarm(bool arm, CancelableBase& cancelable_action)
+ static std::string findPX4()
{
- unused(arm);
- unused(cancelable_action);
- checkVehicle();
- bool rc = false;
- mav_vehicle_->armDisarm(arm).wait(10000, &rc);
- return rc;
+ auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0);
+ for (auto iter = result.begin(); iter != result.end(); iter++)
+ {
+ mavlinkcom::SerialPortInfo info = *iter;
+ if (
+ (
+ (info.vid == pixhawkVendorId) &&
+ (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId)
+ ) ||
+ (
+ (info.displayName.find(L"PX4_") != std::string::npos)
+ )
+ )
+ {
+ // printf("Auto Selecting COM port: %S\n", info.displayName.c_str());
+ return std::string(info.portName.begin(), info.portName.end());
+ }
+ }
+ return "";
}
- bool isApiControlEnabled()
+ void connect()
{
- return is_api_control_enabled_;
+ createMavConnection(connection_info_);
+ initializeMavSubscriptions();
}
- void enableApiControl(bool is_enabled)
+ void createMavConnection(const MavLinkConnectionInfo& connection_info)
{
- checkVehicle();
- if (is_enabled) {
- mav_vehicle_->requestControl();
- is_api_control_enabled_ = true;
+ if (connection_info.use_serial) {
+ createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate);
}
else {
- mav_vehicle_->releaseControl();
- is_api_control_enabled_ = false;
+ createMavUdpConnection(connection_info.ip_address, connection_info.ip_port);
}
+ //Uncomment below for sending images over MavLink
+ //connectToVideoServer();
}
- void setSimulationMode(bool is_set)
+ void createMavUdpConnection(const std::string& ip, int port)
{
- if (connection_ != nullptr) {
- //TODO: can we do this?
- throw std::domain_error("Cannot call setSimulationMode after connection was already established for MavLink");
+ close();
+
+ if (ip == "") {
+ throw std::invalid_argument("UdpIp setting is invalid.");
}
- is_simulation_mode_ = is_set;
- }
+ if (port == 0) {
+ throw std::invalid_argument("UdpPort setting has an invalid value.");
+ }
- bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action)
- {
- unused(cancelable_action);
- checkVehicle();
+ addStatusMessage(Utils::stringf("Connecting to UDP port %d, local IP %s, remote IP...", port, connection_info_.local_host_ip.c_str(), ip.c_str()));
+ connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, ip, port);
+ hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid);
+ hil_node_->connect(connection_);
+ addStatusMessage(std::string("Connected over UDP."));
- bool rc = false;
- auto vec = getPosition();
- float z = vec.z() + getTakeoffZ();
- if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, 0.0f /* yaw */).wait(static_cast(max_wait_seconds * 1000), &rc))
- {
- throw VehicleMoveException("TakeOff command - timeout waiting for response");
+ mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
+
+ if (connection_info_.sitl_ip_address != "" && connection_info_.sitl_ip_port != 0 && connection_info_.sitl_ip_port != port) {
+ // bugbug: the PX4 SITL mode app cannot receive commands to control the drone over the same mavlink connection
+ // as the HIL_SENSOR messages, we must establish a separate mavlink channel for that so that DroneShell works.
+ addStatusMessage(Utils::stringf("Connecting to PX4 SITL UDP port %d, local IP %s, remote IP...",
+ connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), connection_info_.sitl_ip_address.c_str()));
+
+ auto sitlconnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("sitl",
+ connection_info_.local_host_ip, connection_info_.sitl_ip_address, connection_info_.sitl_ip_port);
+ mav_vehicle_->connect(sitlconnection);
+
+ addStatusMessage(std::string("Connected to SITL over UDP."));
}
- if (!rc) {
- throw VehicleMoveException("TakeOff command rejected by drone");
+ else {
+ mav_vehicle_->connect(connection_);
}
- if (max_wait_seconds <= 0)
- return true; // client doesn't want to wait.
- return parent_->waitForZ(max_wait_seconds, z, getDistanceAccuracy(), cancelable_action);
+ mav_vehicle_->startHeartbeat();
}
- bool hover(CancelableBase& cancelable_action)
+ void createMavSerialConnection(const std::string& port_name, int baud_rate)
{
- bool rc = false;
- checkVehicle();
- mavlinkcom::AsyncResult result = mav_vehicle_->loiter();
- //auto start_time = std::chrono::system_clock::now();
- while (!cancelable_action.isCancelled())
- {
- if (result.wait(100, &rc))
- {
- break;
- }
- }
- return rc;
- }
+ close();
- bool land(float max_wait_seconds, CancelableBase& cancelable_action)
- {
- unused(cancelable_action);
- // bugbug: really need a downward pointing distance to ground sensor to do this properly, for now
- // we assume the ground is relatively flat an we are landing roughly at the home altitude.
- updateState();
- checkVehicle();
- if (current_state.home.is_set)
- {
- bool rc = false;
- if (!mav_vehicle_->land(current_state.global_est.pos.lat, current_state.global_est.pos.lon, current_state.home.global_pos.alt).wait(10000, &rc))
- {
- throw VehicleMoveException("Landing command - timeout waiting for response from drone");
- }
- else if (!rc) {
- throw VehicleMoveException("Landing command rejected by drone");
+ std::string port_name_auto = port_name;
+ if (port_name_auto == "" || port_name_auto == "*") {
+ port_name_auto = findPX4();
+ if (port_name_auto == "") {
+ throw std::domain_error("Could not detect a connected PX4 flight controller on any USB ports. You can specify USB port in settings.json.");
}
}
- else
- {
- throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude. Could fix this if we hook up a distance to ground sensor...");
+
+ if (port_name_auto == "") {
+ throw std::invalid_argument("USB port for PX4 flight controller is empty. Please set it in settings.json.");
}
- // Wait for landed state (or user cancelation)
- if (!parent_->waitForFunction([&]() {
- updateState();
- return current_state.controls.landed;
- }, max_wait_seconds, cancelable_action))
- {
- throw VehicleMoveException("Drone hasn't reported a landing state");
+ if (baud_rate == 0) {
+ throw std::invalid_argument("Baud rate specified in settings.json is 0 which is invalid");
}
- return true;
+
+ addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate));
+ connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate);
+ connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mocap messages
+ hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid);
+ hil_node_->connect(connection_);
+ addStatusMessage("Connected to PX4 over serial port.");
+
+ mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
+ mav_vehicle_->connect(connection_); // in this case we can use the same connection.
+ mav_vehicle_->startHeartbeat();
}
- bool goHome(CancelableBase& cancelable_action)
+ mavlinkcom::MavLinkHilSensor getLastSensorMessage()
{
- unused(cancelable_action);
- checkVehicle();
- bool rc = false;
- if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(10000, &rc)) {
- throw VehicleMoveException("goHome - timeout waiting for response from drone");
- }
- return rc;
+ std::lock_guard guard(last_message_mutex_);
+ return last_sensor_message_;
}
- void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate)
+ mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage()
{
- checkVehicle();
- mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle);
+ std::lock_guard guard(last_message_mutex_);
+ return last_distance_message_;
}
- void commandRollPitchZ(float pitch, float roll, float z, float yaw)
+
+ mavlinkcom::MavLinkHilGps getLastGpsMessage()
{
- if (target_height_ != -z) {
- // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best
- // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not
- // been tested on a real drone outside jMavSim, so it may need recalibrating...
- thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
- target_height_ = -z;
- }
- checkVehicle();
- auto state = mav_vehicle_->getVehicleState();
- float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
- mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust);
+ std::lock_guard