diff --git a/.gitignore b/.gitignore index 785b5da60..f1a03ac77 100644 --- a/.gitignore +++ b/.gitignore @@ -358,11 +358,18 @@ xcuserdata/ !*.xcworkspace/contents.xcworkspacedata /*.gcno -/Unity/UnityDemo/Library +# Unity /Unity/UnityDemo/Assembly-CSharp.csproj /Unity/UnityDemo/UnityDemo.sln -/Unity/UnityDemo/Logs/ -/Unity/UnityDemo/Assets/Plugins/AirsimWrapper.dll +/Unity/linux-build +/Unity/UnityDemo/Assets/Plugins/ +/Unity/UnityDemo/[Ll]ibrary/ +/Unity/UnityDemo/[Tt]emp/ +/Unity/UnityDemo/[Oo]bj/ +/Unity/UnityDemo/[Bb]uild/ +/Unity/UnityDemo/[Bb]uilds/ +/Unity/UnityDemo/[Ll]ogs/ +/Unity/UnityDemo/[Uu]ser[Ss]ettings/ # ROS ros/.catkin_tools/ @@ -370,3 +377,10 @@ ros/devel/ ros/logs/ ros/.catkin_workspace ros/src/CMakeLists.txt + +# docs +docs/README.md +build_docs/ + +# api docs +PythonClient/docs/_build diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index e69de29bb..000000000 diff --git a/.travis.yml b/.travis.yml index ba4a4a03f..048098288 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,73 +1,58 @@ language: cpp -#git: -# shorten this if we can nail down submodule depth -# depth: 3 -fast_finish: false +jobs: + include: + - name: Ubuntu 18.04 + os: linux + dist: bionic -matrix: - allow_failures: - - env: ALLOW_FAILURES=true + - name: Ubuntu 16.04 + os: linux + dist: xenial - include: - # linux version have unique dependencies, so we set them up individually - - os: linux - dist: trusty - sudo: required - services: - - docker - #- os: windows - # dotnet: 2.1.5 - # services: - # - docker - + - name: Windows 10 + os: windows + env: + - MSBUILD_PATH="c:\Program Files (x86)\Microsoft Visual Studio\2019\BuildTools\MSBuild\16.0\Bin" + - PATH_FIX="C:\Windows\System32" - # OS X CMake - #- os: osx - # sudo: required - # osx_image: xcode8.2 - # compiler: - # - clang - # env: - # - TOOL="cmake" - # - DESCRIPTION="OS X build/test via CMake" + - name: MacOS + os: osx + osx_image: xcode11.4 -# docker exec xenial apt-get -y install clang libclang-3.8-dev; -install: - - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then - docker pull ubuntu:xenial; - docker run -d --name xenial -dti ubuntu:xenial bash; - docker ps -a; - docker exec xenial mkdir /build; - docker cp . xenial:/build; - docker exec xenial apt-get update; - docker exec xenial apt-get -y upgrade; - docker exec xenial apt-get -y install git wget unzip sudo; - docker exec xenial apt-get -y install build-essential software-properties-common cmake; - docker exec xenial apt-get update; - elif [[ "$TRAVIS_OS_NAME" == "windows" ]]; then - echo "No install osx actions--using repo"; - elif [[ "$TRAVIS_OS_NAME" == "osx" ]] && [[ "$TOOL" == "cmake" ]]; then - echo "No install Windows actions--using repo"; - fi before_install: - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then - echo "No pre-install linux actions--using docker"; - elif [[ "$TRAVIS_OS_NAME" == "osx" ]] && [[ "$TOOL" == "cmake" ]]; then - echo "No pre-install osx actions"; + sudo apt-get update; + sudo apt-get -y install git wget unzip; + sudo apt-get -y install build-essential software-properties-common cmake rsync libboost-all-dev; + elif [[ "$TRAVIS_OS_NAME" == "osx" ]]; then + echo "No before_install actions for OSX"; + elif [[ "$TRAVIS_OS_NAME" == "windows" ]]; then + choco install visualstudio2019buildtools --package-parameters "--includeRecommended --includeOptional"; + choco install visualstudio2019-workload-vctools; fi -#docker exec xenial /bin/sh -c "export EIGEN3_INCLUDE_DIR=/build/eigen/eigen3 && cd /build/cmake && sh ./build.sh && cmake . && make"; script: - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then - docker exec -t xenial /build/setup.sh; - docker exec -t xenial /build/build.sh; + ./setup.sh; + ./build.sh || travis_terminate 1; + echo "Starting Unity Build!"; + (cd Unity && ./build.sh || travis_terminate 1); elif [[ "$TRAVIS_OS_NAME" == "windows" ]]; then - .\\build.cmd; - .\\Unity\\build.cmd; + export PATH=$MSBUILD_PATH:$PATH_FIX:$PATH; + cmd.exe //C 'C:\Program Files (x86)\Microsoft Visual Studio\2019\BuildTools\VC\Auxiliary\Build\vcvarsall.bat' amd64 '&&' + build.cmd '&&' + cd Unity '&&' build.cmd; elif [[ "$TRAVIS_OS_NAME" == "osx" ]]; then - git submodule update --init --recursive; ./setup.sh; - ./build.sh; + ./build.sh || travis_terminate 1; + echo "Starting Unity Build!"; + (cd Unity && ./build.sh || travis_terminate 1); + fi + + # Test ROS wrapper compilation + - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then + ./tools/install_ros_deps.sh; + (cd ros && source ~/.bashrc && catkin build -DCMAKE_C_COMPILER=gcc-8 -DCMAKE_CXX_COMPILER=g++-8); fi diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj index fc94131a9..e0069b3fc 100644 --- a/AirLib/AirLib.vcxproj +++ b/AirLib/AirLib.vcxproj @@ -1,5 +1,5 @@  - + true @@ -45,6 +45,7 @@ + @@ -164,10 +165,10 @@ - + - + @@ -196,26 +197,26 @@ StaticLibrary true - v141 + v142 Unicode StaticLibrary false - v141 + v142 true Unicode StaticLibrary true - v141 + v142 Unicode StaticLibrary false - v141 + v142 true Unicode @@ -267,6 +268,7 @@ include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -292,6 +294,7 @@ true /w34263 /w34266 %(AdditionalOptions) 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings) + stdcpp17 Windows @@ -318,6 +321,7 @@ include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Windows @@ -346,6 +350,7 @@ include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Windows diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters index f2d972b15..02a017018 100644 --- a/AirLib/AirLib.vcxproj.filters +++ b/AirLib/AirLib.vcxproj.filters @@ -213,7 +213,7 @@ Header Files - + Header Files @@ -222,7 +222,7 @@ Header Files - + Header Files @@ -453,6 +453,9 @@ Header Files + + Header Files + diff --git a/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirLib/include/api/RpcLibAdapatorsBase.hpp index ca860d121..13304cc8f 100644 --- a/AirLib/include/api/RpcLibAdapatorsBase.hpp +++ b/AirLib/include/api/RpcLibAdapatorsBase.hpp @@ -672,7 +672,7 @@ class RpcLibAdapatorsBase { DistanceSensorData() {} - DistanceSensorData(const msr::airlib::DistanceBase::Output& s) + DistanceSensorData(const msr::airlib::DistanceSensorData& s) { time_stamp = s.time_stamp; distance = s.distance; @@ -681,9 +681,9 @@ class RpcLibAdapatorsBase { relative_pose = s.relative_pose; } - msr::airlib::DistanceBase::Output to() const + msr::airlib::DistanceSensorData to() const { - msr::airlib::DistanceBase::Output d; + msr::airlib::DistanceSensorData d; d.time_stamp = time_stamp; d.distance = distance; @@ -694,6 +694,68 @@ class RpcLibAdapatorsBase { return d; } }; + + struct MeshPositionVertexBuffersResponse { + Vector3r position; + Quaternionr orientation; + + std::vector vertices; + std::vector indices; + std::string name; + + MSGPACK_DEFINE_MAP(position, orientation, vertices, indices, name); + + MeshPositionVertexBuffersResponse() + {} + + MeshPositionVertexBuffersResponse(const msr::airlib::MeshPositionVertexBuffersResponse& s) + { + position = Vector3r(s.position); + orientation = Quaternionr(s.orientation); + + vertices = s.vertices; + indices = s.indices; + + if (vertices.size() == 0) + vertices.push_back(0); + if (indices.size() == 0) + indices.push_back(0); + + name = s.name; + } + + msr::airlib::MeshPositionVertexBuffersResponse to() const + { + msr::airlib::MeshPositionVertexBuffersResponse d; + d.position = position.to(); + d.orientation = orientation.to(); + d.vertices = vertices; + d.indices = indices; + d.name = name; + + return d; + } + + static std::vector to( + const std::vector& response_adapter) { + std::vector response; + for (const auto& item : response_adapter) + response.push_back(item.to()); + + return response; + } + + static std::vector from( + const std::vector& response) { + std::vector response_adapter; + for (const auto& item : response) + response_adapter.push_back(MeshPositionVertexBuffersResponse(item)); + + return response_adapter; + } + }; + + }; }} //namespace diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index f0acd387c..7f9807274 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -50,8 +50,11 @@ class RpcLibClientBase { vector simListSceneObjects(const string& name_regex = string(".*")) const; Pose simGetObjectPose(const std::string& object_name) const; + bool simLoadLevel(const string& level_name); + Vector3r simGetObjectScale(const std::string& object_name) const; bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true); - + bool simSetObjectScale(const std::string& object_name, const Vector3r& scale); + //task management APIs void cancelLastTask(const std::string& vehicle_name = ""); virtual RpcLibClientBase* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan()); @@ -60,6 +63,14 @@ class RpcLibClientBase { int simGetSegmentationObjectID(const std::string& mesh_name) const; void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0); + void simFlushPersistentMarkers(); + void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent); + void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); + void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); + void simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent); + void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration); + void simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent); + void simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration); bool armDisarm(bool arm, const std::string& vehicle_name = ""); bool isApiControlEnabled(const std::string& vehicle_name = "") const; @@ -73,42 +84,35 @@ class RpcLibClientBase { msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name = "", const std::string& vehicle_name = "") const; msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = "") const; msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const; - msr::airlib::DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const; + msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const; // sensor omniscient APIs vector simGetLidarSegmentation(const std::string& lidar_name = "", const std::string& vehicle_name = "") const; Pose simGetVehiclePose(const std::string& vehicle_name = "") const; void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = ""); + void simSetTraceLine(const std::vector& color_rgba, float thickness=3.0f, const std::string& vehicle_name = ""); vector simGetImages(vector request, const std::string& vehicle_name = ""); vector simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = ""); + vector simGetMeshPositionVertexBuffers(); + CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const; CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const; - void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = ""); + void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = ""); + void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = ""); msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const; msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const; - //----------- APIs to control ACharacter in scene ----------/ - void simCharSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name = ""); - float simCharGetFaceExpression(const std::string& expression_name, const std::string& character_name = "") const; - std::vector simCharGetAvailableFaceExpressions(); - void simCharSetSkinDarkness(float value, const std::string& character_name = ""); - float simCharGetSkinDarkness(const std::string& character_name = "") const; - void simCharSetSkinAgeing(float value, const std::string& character_name = ""); - float simCharGetSkinAgeing(const std::string& character_name = "") const; - void simCharSetHeadRotation(const msr::airlib::Quaternionr& q, const std::string& character_name = ""); - msr::airlib::Quaternionr simCharGetHeadRotation(const std::string& character_name = "") const; - void simCharSetBonePose(const std::string& bone_name, const msr::airlib::Pose& pose, const std::string& character_name = ""); - msr::airlib::Pose simCharGetBonePose(const std::string& bone_name, const std::string& character_name = "") const; - void simCharResetBonePose(const std::string& bone_name, const std::string& character_name = ""); - void simCharSetFacePreset(const std::string& preset_name, float value, const std::string& character_name = ""); - void simSetFacePresets(const std::unordered_map& presets, const std::string& character_name = ""); - void simSetBonePoses(const std::unordered_map& poses, const std::string& character_name = ""); - std::unordered_map simGetBonePoses(const std::vector& bone_names, const std::string& character_name = "") const; + std::vector simSwapTextures(const std::string& tags, int tex_id = 0, int component_id = 0, int material_id = 0); + + // Recording APIs + void startRecording(); + void stopRecording(); + bool isRecording(); protected: void* getClient(); diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index ddb6679f3..fa90fcaf2 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -46,12 +46,18 @@ class VehicleApiBase : public UpdatableObject { { //if derived class supports async task then override this method } + virtual bool isReady(std::string& message) const { unused(message); return true; } + virtual bool canArm() const + { + return true; + } + //if vehicle supports it, call this method to send //kinematics and other info to somewhere (ex. log viewer, file, cloud etc) virtual void sendTelemetry(float last_interval = -1) @@ -89,6 +95,7 @@ class VehicleApiBase : public UpdatableObject { static const RCData invalid_rc_data {}; return invalid_rc_data; } + //set external RC data to vehicle (if unsupported then returns false) virtual bool setRCData(const RCData& rc_data) { @@ -208,7 +215,7 @@ class VehicleApiBase : public UpdatableObject { } // Distance Sensor API - virtual DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name) const + virtual DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name) const { const DistanceBase* distance_sensor = nullptr; diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index c913c2eac..471a2cfd8 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -54,7 +54,8 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject { virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0; virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 0; - virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) = 0; + virtual void setCameraPose(const std::string& camera_name, const Pose& pose) = 0; + virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0; virtual CollisionInfo getCollisionInfo() const = 0; virtual int getRemoteControlID() const = 0; //which RC to use, 0 is first one, -1 means disable RC (use keyborad) @@ -62,6 +63,7 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject { virtual std::string getVehicleName() const = 0; virtual std::string getRecordFileLine(bool is_header_line) const = 0; virtual void toggleTrace() = 0; + virtual void setTraceLine(const std::vector& color_rgba, float thickness) = 0; //use pointer here because of derived classes for VehicleSetting const AirSimSettings::VehicleSetting* getVehicleSetting() const diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 949c5efd4..e07cc9636 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -25,6 +25,11 @@ class WorldSimApiBase { virtual ~WorldSimApiBase() = default; + // ------ Level setting apis ----- // + virtual bool loadLevel(const std::string& level_name) = 0; + virtual string spawnObject(string& object_name, const string& load_component, const Pose& pose, const Vector3r& scale) = 0; + virtual bool destroyObject(const string& object_name) = 0; + virtual bool isPaused() const = 0; virtual void reset() = 0; virtual void pause(bool is_paused) = 0; @@ -42,28 +47,29 @@ class WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; + //----------- Plotting APIs ----------/ + virtual void simFlushPersistentMarkers() = 0; + virtual void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) = 0; + virtual void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; + virtual void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) = 0; + virtual void simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) = 0; + virtual void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration) = 0; + virtual void simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent) = 0; + virtual void simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) = 0; + virtual std::vector listSceneObjects(const std::string& name_regex) const = 0; virtual Pose getObjectPose(const std::string& object_name) const = 0; + virtual Vector3r getObjectScale(const std::string& object_name) const = 0; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0; + virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) = 0; - //----------- APIs to control ACharacter in scene ----------/ - virtual void charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) = 0; - virtual float charGetFaceExpression(const std::string& expression_name, const std::string& character_name) const = 0; - virtual std::vector charGetAvailableFaceExpressions() = 0; - virtual void charSetSkinDarkness(float value, const std::string& character_name) = 0; - virtual float charGetSkinDarkness(const std::string& character_name) const = 0; - virtual void charSetSkinAgeing(float value, const std::string& character_name) = 0; - virtual float charGetSkinAgeing(const std::string& character_name) const = 0; - virtual void charSetHeadRotation(const msr::airlib::Quaternionr& q, const std::string& character_name) = 0; - virtual msr::airlib::Quaternionr charGetHeadRotation(const std::string& character_name) const = 0; - virtual void charSetBonePose(const std::string& bone_name, const msr::airlib::Pose& pose, const std::string& character_name) = 0; - virtual msr::airlib::Pose charGetBonePose(const std::string& bone_name, const std::string& character_name) const = 0; - virtual void charResetBonePose(const std::string& bone_name, const std::string& character_name) = 0; - virtual void charSetFacePreset(const std::string& preset_name, float value, const std::string& character_name) = 0; - virtual void charSetFacePresets(const std::unordered_map& presets, const std::string& character_name) = 0; - virtual void charSetBonePoses(const std::unordered_map& poses, const std::string& character_name) = 0; - virtual std::unordered_map charGetBonePoses(const std::vector& bone_names, const std::string& character_name) const = 0; + virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) = 0; + virtual vector getMeshPositionVertexBuffers() const = 0; + // Recording APIs + virtual void startRecording() = 0; + virtual void stopRecording() = 0; + virtual bool isRecording() const = 0; }; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 2e2dcbf1d..348f235c8 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "Settings.hpp" #include "CommonStructs.hpp" #include "common_utils/Utils.hpp" @@ -24,10 +25,11 @@ struct AirSimSettings { public: //types static constexpr int kSubwindowCount = 3; //must be >= 3 for now static constexpr char const * kVehicleTypePX4 = "px4multirotor"; - static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo"; - static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight"; + static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo"; + static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight"; static constexpr char const * kVehicleTypeArduCopter = "arducopter"; static constexpr char const * kVehicleTypePhysXCar = "physxcar"; + static constexpr char const * kVehicleTypeArduRover = "ardurover"; static constexpr char const * kVehicleTypeComputerVision = "computervision"; static constexpr char const * kVehicleInertialFrame = "VehicleInertialFrame"; @@ -64,7 +66,7 @@ struct AirSimSettings { PawnPath(const std::string& pawn_bp_val = "", const std::string& slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery", - const std::string& non_slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery") + const std::string& non_slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery") : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val) { } @@ -194,6 +196,12 @@ struct AirSimSettings { }; struct DistanceSetting : SensorSetting { + // shared defaults + real_T min_distance = 20.0f / 100; //m + real_T max_distance = 4000.0f / 100; //m + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + bool draw_debug_points = false; }; struct LidarSetting : SensorSetting { @@ -209,7 +217,7 @@ struct AirSimSettings { // defaults specific to a mode float vertical_FOV_upper = Utils::nan(); // drones -15, car +10 float vertical_FOV_lower = Utils::nan(); // drones -45, car -10 - Vector3r position = VectorMath::nanVector(); + Vector3r position = VectorMath::nanVector(); Rotation rotation = Rotation::nanRotation(); bool draw_debug_points = false; @@ -230,7 +238,7 @@ struct AirSimSettings { bool enable_trace = false; bool enable_collisions = true; bool is_fpv_vehicle = false; - + //nan means use player start Vector3r position = VectorMath::nanVector(); //in global NED Rotation rotation = Rotation::nanRotation(); @@ -244,28 +252,35 @@ struct AirSimSettings { struct MavLinkConnectionInfo { /* Default values are requires so uninitialized instance doesn't have random values */ - bool use_serial = true; // false means use UDP instead - //Used to connect via HITL: needed only if use_serial = true + bool use_serial = true; // false means use UDP or TCP instead + + //Used to connect via HITL: needed only if use_serial = true std::string serial_port = "*"; int baud_rate = 115200; - //Used to connect to drone over UDP: needed only if use_serial = false - std::string ip_address = "127.0.0.1"; - int ip_port = 14560; + // Used to connect to drone over UDP: needed only if use_serial = false and use_tcp == false + std::string udp_address = "127.0.0.1"; + int udp_port = 14560; + + // Used to accept connections from drone over TCP: needed only if use_tcp = true + bool use_tcp = false; + int tcp_port = 4560; - // The PX4 SITL app requires receiving drone commands over a different mavlink channel. + // The PX4 SITL app requires receiving drone commands over a different mavlink channel called + // the "ground control station" channel. // So set this to empty string to disable this separate command channel. - std::string sitl_ip_address = "127.0.0.1"; - int sitl_ip_port = 14556; + std::string control_ip_address = "127.0.0.1"; + int control_port = 14580; // The log viewer can be on a different machine, so you can configure it's ip address and port here. int logviewer_ip_port = 14388; int logviewer_ip_sport = 14389; // for logging all messages we send to the vehicle. - std::string logviewer_ip_address = "127.0.0.1"; + std::string logviewer_ip_address = ""; - // The QGroundControl app can be on a different machine, so you can configure it's ip address and port here. - int qgc_ip_port = 14550; - std::string qgc_ip_address = "127.0.0.1"; + // The QGroundControl app can be on a different machine, and AirSim can act as a proxy to forward + // the mavlink stream over to that machine if you configure it's ip address and port here. + int qgc_ip_port = 0; + std::string qgc_ip_address = ""; // mavlink vehicle identifiers uint8_t sim_sysid = 142; @@ -275,17 +290,19 @@ struct AirSimSettings { uint8_t vehicle_sysid = 135; int vehicle_compid = 1; - // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi versus ethernet) + // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi versus ethernet) // then you will want to change the LocalHostIp accordingly. This default only works when log viewer and QGC are also on the // same machine. Whatever network you choose it has to be the same one for external std::string local_host_ip = "127.0.0.1"; std::string model = "Generic"; + + std::map params; }; - struct MavLinkVehicleSetting : public VehicleSetting { - MavLinkConnectionInfo connection_info; - }; + struct MavLinkVehicleSetting : public VehicleSetting { + MavLinkConnectionInfo connection_info; + }; struct SegmentationSetting { enum class InitMethodType { @@ -316,6 +333,7 @@ struct AirSimSettings { public: //fields std::string simmode_name = ""; + std::string level_name = ""; std::vector subwindow_settings; RecordingSetting recording_setting; @@ -324,12 +342,12 @@ struct AirSimSettings { std::vector warning_messages; std::vector error_messages; - + bool is_record_ui_visible = false; - int initial_view_mode = 3; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME + int initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME bool enable_rpc = true; std::string api_server_address = ""; - int api_port = RpcLibPort; + int api_port = RpcLibPort; std::string physics_engine_name = ""; std::string clock_type = ""; @@ -341,12 +359,12 @@ struct AirSimSettings { std::map> vehicles; CameraSetting camera_defaults; CameraDirectorSetting camera_director; - float speed_unit_factor = 1.0f; - std::string speed_unit_label = "m\\s"; + float speed_unit_factor = 1.0f; + std::string speed_unit_label = "m\\s"; std::map> sensor_defaults; public: //methods - static AirSimSettings& singleton() + static AirSimSettings& singleton() { static AirSimSettings instance; return instance; @@ -368,6 +386,7 @@ struct AirSimSettings { checkSettingsVersion(settings_json); loadCoreSimModeSettings(settings_json, simmode_getter); + loadLevelSettings(settings_json); loadDefaultCameraSetting(settings_json, camera_defaults); loadCameraDirectorSetting(settings_json, camera_director, simmode_name); loadSubWindowsSettings(settings_json, subwindow_settings); @@ -421,7 +440,7 @@ struct AirSimSettings { if (upgrade_required) { bool auto_upgrade = false; - //if we have default setting file not modified by user then we will + //if we have default setting file not modified by user then we will //just auto-upgrade it if (has_default_settings) { auto_upgrade = true; @@ -503,6 +522,11 @@ struct AirSimSettings { physics_engine_name = "PhysX"; //this value is only informational for now } } + + void loadLevelSettings(const Settings& settings_json) + { + level_name = settings_json.getString("Default Environment", ""); + } void loadViewModeSettings(const Settings& settings_json) { @@ -518,21 +542,21 @@ struct AirSimSettings { } if (view_mode_string == "Fpv") - initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV; + initial_view_mode = 0; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV; else if (view_mode_string == "GroundObserver") - initial_view_mode = 2; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER; + initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER; else if (view_mode_string == "FlyWithMe") - initial_view_mode = 3; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME; + initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME; else if (view_mode_string == "Manual") - initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL; + initial_view_mode = 3; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL; else if (view_mode_string == "SpringArmChase") - initial_view_mode = 5; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE; + initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE; else if (view_mode_string == "Backup") - initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP; + initial_view_mode = 5; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP; else if (view_mode_string == "NoDisplay") - initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; + initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; else if (view_mode_string == "Front") - initial_view_mode = 8; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; + initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; else error_messages.push_back("ViewMode setting is not recognized: " + view_mode_string); } @@ -541,7 +565,7 @@ struct AirSimSettings { { Settings rc_json; if (settings_json.getChild("RC", rc_json)) { - rc_setting.remote_control_id = rc_json.getInt("RemoteControlID", + rc_setting.remote_control_id = rc_json.getInt("RemoteControlID", simmode_name == "Multirotor" ? 0 : -1); rc_setting.allow_api_when_disconnected = rc_json.getBool("AllowAPIWhenDisconnected", rc_setting.allow_api_when_disconnected); @@ -550,7 +574,7 @@ struct AirSimSettings { static std::string getCameraName(const Settings& settings_json) { - return settings_json.getString("CameraName", + return settings_json.getString("CameraName", //TODO: below exist only due to legacy reason and can be replaced by "" in future std::to_string(settings_json.getInt("CameraID", 0))); } @@ -601,7 +625,7 @@ struct AirSimSettings { Settings json_parent; if (settings_json.getChild("CaptureSettings", json_parent)) { for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { - Settings json_settings_child; + Settings json_settings_child; if (json_parent.getChild(child_index, json_settings_child)) { CaptureSetting capture_setting; createCaptureSettings(json_settings_child, capture_setting); @@ -611,11 +635,11 @@ struct AirSimSettings { } } - static std::unique_ptr createMavLinkVehicleSetting(const Settings& settings_json) + static std::unique_ptr createMavLinkVehicleSetting(const Settings& settings_json) { //these settings_json are expected in same section, not in another child std::unique_ptr vehicle_setting_p = std::unique_ptr(new MavLinkVehicleSetting()); - MavLinkVehicleSetting* vehicle_setting = static_cast(vehicle_setting_p.get()); + MavLinkVehicleSetting* vehicle_setting = static_cast(vehicle_setting_p.get()); //TODO: we should be selecting remote if available else keyboard //currently keyboard is not supported so use rc as default @@ -638,26 +662,49 @@ struct AirSimSettings { connection_info.qgc_ip_address = settings_json.getString("QgcHostIp", connection_info.qgc_ip_address); connection_info.qgc_ip_port = settings_json.getInt("QgcPort", connection_info.qgc_ip_port); - connection_info.sitl_ip_address = settings_json.getString("SitlIp", connection_info.sitl_ip_address); - connection_info.sitl_ip_port = settings_json.getInt("SitlPort", connection_info.sitl_ip_port); + connection_info.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address); + connection_info.control_port = settings_json.getInt("ControlPort", connection_info.control_port); - connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip); + std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address); + if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0) + { + // backwards compat + connection_info.control_ip_address = sitlip; + } + if (settings_json.hasKey("SitlPort")) + { + // backwards compat + connection_info.control_port = settings_json.getInt("SitlPort", connection_info.control_port); + } + connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip); connection_info.use_serial = settings_json.getBool("UseSerial", connection_info.use_serial); - connection_info.ip_address = settings_json.getString("UdpIp", connection_info.ip_address); - connection_info.ip_port = settings_json.getInt("UdpPort", connection_info.ip_port); + connection_info.udp_address = settings_json.getString("UdpIp", connection_info.udp_address); + connection_info.udp_port = settings_json.getInt("UdpPort", connection_info.udp_port); + connection_info.use_tcp = settings_json.getBool("UseTcp", connection_info.use_tcp); + connection_info.tcp_port = settings_json.getInt("TcpPort", connection_info.tcp_port); connection_info.serial_port = settings_json.getString("SerialPort", connection_info.serial_port); connection_info.baud_rate = settings_json.getInt("SerialBaudRate", connection_info.baud_rate); connection_info.model = settings_json.getString("Model", connection_info.model); + Settings params; + if (settings_json.getChild("Parameters", params)) { + std::vector keys; + params.getChildNames(keys); + for (auto key: keys) + { + connection_info.params[key] = params.getFloat(key, 0); + } + } + return vehicle_setting_p; } static Vector3r createVectorSetting(const Settings& settings_json, const Vector3r& default_vec) { - return Vector3r(settings_json.getFloat("X", default_vec.x()), - settings_json.getFloat("Y", default_vec.y()), + return Vector3r(settings_json.getFloat("X", default_vec.x()), + settings_json.getFloat("Y", default_vec.y()), settings_json.getFloat("Z", default_vec.z())); } static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot) @@ -673,7 +720,8 @@ struct AirSimSettings { auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", "")); std::unique_ptr vehicle_setting; - if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo || vehicle_type == kVehicleTypeArduCopter) + if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo + || vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover) vehicle_setting = createMavLinkVehicleSetting(settings_json); //for everything else we don't need derived class yet else { @@ -692,11 +740,11 @@ struct AirSimSettings { //optional settings_json vehicle_setting->pawn_path = settings_json.getString("PawnPath", ""); vehicle_setting->default_vehicle_state = settings_json.getString("DefaultVehicleState", ""); - vehicle_setting->allow_api_always = settings_json.getBool("AllowAPIAlways", + vehicle_setting->allow_api_always = settings_json.getBool("AllowAPIAlways", vehicle_setting->allow_api_always); vehicle_setting->auto_create = settings_json.getBool("AutoCreate", vehicle_setting->auto_create); - vehicle_setting->enable_collision_passthrough = settings_json.getBool("EnableCollisionPassthrogh", + vehicle_setting->enable_collision_passthrough = settings_json.getBool("EnableCollisionPassthrogh", vehicle_setting->enable_collision_passthrough); vehicle_setting->enable_trace = settings_json.getBool("EnableTrace", vehicle_setting->enable_trace); @@ -712,7 +760,7 @@ struct AirSimSettings { loadCameraSettings(settings_json, vehicle_setting->cameras); loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors); - + return vehicle_setting; } @@ -778,7 +826,7 @@ struct AirSimSettings { PawnPath("Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'")); pawn_paths.emplace("DefaultComputerVision", PawnPath("Class'/AirSim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'")); - + } static void loadPawnPaths(const Settings& settings_json, std::map& pawn_paths) @@ -853,7 +901,7 @@ struct AirSimSettings { Settings json_parent; if (settings_json.getChild("NoiseSettings", json_parent)) { for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { - Settings json_settings_child; + Settings json_settings_child; if (json_parent.getChild(child_index, json_settings_child)) { NoiseSetting noise_setting; loadNoiseSetting(json_settings_child, noise_setting); @@ -936,7 +984,7 @@ struct AirSimSettings { capture_setting.auto_exposure_min_brightness = settings_json.getFloat("AutoExposureMinBrightness", capture_setting.auto_exposure_min_brightness); capture_setting.motion_blur_amount = settings_json.getFloat("MotionBlurAmount", capture_setting.motion_blur_amount); capture_setting.image_type = settings_json.getInt("ImageType", 0); - capture_setting.target_gamma = settings_json.getFloat("TargetGamma", + capture_setting.target_gamma = settings_json.getFloat("TargetGamma", capture_setting.image_type == 0 ? CaptureSetting::kSceneTargetGamma : Utils::nan()); std::string projection_mode = Utils::toLower(settings_json.getString("ProjectionMode", "")); @@ -986,7 +1034,7 @@ struct AirSimSettings { //because for docker container default is 0.0.0.0 and people get really confused why things //don't work api_server_address = settings_json.getString("LocalHostIp", ""); - api_port = settings_json.getInt("ApiServerPort", RpcLibPort); + api_port = settings_json.getInt("ApiServerPort", RpcLibPort); is_record_ui_visible = settings_json.getBool("RecordUIVisible", true); engine_sound = settings_json.getBool("EngineSound", false); enable_rpc = settings_json.getBool("EnableRpc", enable_rpc); @@ -1026,7 +1074,7 @@ struct AirSimSettings { } } - static void loadCameraDirectorSetting(const Settings& settings_json, + static void loadCameraDirectorSetting(const Settings& settings_json, CameraDirectorSetting& camera_director, const std::string& simmode_name) { camera_director = CameraDirectorSetting(); @@ -1073,7 +1121,7 @@ struct AirSimSettings { clock_type = "SteppableClock"; for (auto const& vehicle : vehicles) { - if (vehicle.second->auto_create && + if (vehicle.second->auto_create && vehicle.second->vehicle_type == kVehicleTypePX4) { clock_type = "ScalableClock"; break; @@ -1119,10 +1167,12 @@ struct AirSimSettings { static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& settings_json) { - unused(distance_setting); - unused(settings_json); + distance_setting.min_distance = settings_json.getFloat("MinDistance", distance_setting.min_distance); + distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance); + distance_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", distance_setting.draw_debug_points); - //TODO: set from json as needed + distance_setting.position = createVectorSetting(settings_json, distance_setting.position); + distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation); } static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json) @@ -1222,7 +1272,7 @@ struct AirSimSettings { auto sensor_type = Utils::toEnum(child.getInt("SensorType", 0)); auto enabled = child.getBool("Enabled", false); - + sensors[key] = createSensorSetting(sensor_type, key, enabled); initializeSensorSetting(sensors[key].get(), child); } @@ -1248,7 +1298,7 @@ struct AirSimSettings { } // loads or creates default sensor list - static void loadDefaultSensorSettings(const std::string& simmode_name, + static void loadDefaultSensorSettings(const std::string& simmode_name, const Settings& settings_json, std::map>& sensors) { diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp index 51da55e18..405848e3b 100644 --- a/AirLib/include/common/CommonStructs.hpp +++ b/AirLib/include/common/CommonStructs.hpp @@ -296,7 +296,6 @@ struct RCData { }; struct LidarData { - TTimePoint time_stamp = 0; vector point_cloud; Pose pose; @@ -305,5 +304,25 @@ struct LidarData { {} }; +struct DistanceSensorData { + TTimePoint time_stamp; + real_T distance; //meters + real_T min_distance; //m + real_T max_distance; //m + Pose relative_pose; + + DistanceSensorData() + {} +}; + +struct MeshPositionVertexBuffersResponse { + Vector3r position; + Quaternionr orientation; + + std::vector vertices; + std::vector indices; + std::string name; +}; + }} //namespace #endif diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 114a67341..732994eb8 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -324,6 +324,9 @@ class VectorMathT { static Vector3T toAngularVelocity(const QuaternionT& start, const QuaternionT& end, RealT dt) { + if (dt == 0) + return Vector3T(0, 0, 0); + RealT p_s, r_s, y_s; toEulerianAngle(start, p_s, r_s, y_s); diff --git a/AirLib/include/common/common_utils/SmoothingFilter.hpp b/AirLib/include/common/common_utils/SmoothingFilter.hpp new file mode 100644 index 000000000..1b450be27 --- /dev/null +++ b/AirLib/include/common/common_utils/SmoothingFilter.hpp @@ -0,0 +1,105 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef common_utils_SmoothingFilter_hpp +#define common_utils_SmoothingFilter_hpp + +#include +#include +#include + +namespace common_utils { + +// SmoothingFilter is a filter that removes the outliers that fall outside a given percentage from the min/max +// range of numbers over a given window size. +template +class SmoothingFilter { + private: + std::vector buffer_; + int window_size_; + int window_size_2x_; + float outlier_factor_; + int buffer_index_; + public: + SmoothingFilter(); + SmoothingFilter(int window_size, float outlier_factor); + void initialize(int window_size, float outlier_factor); + + // Each call to the filter method adds to the current window, and returns + // the mean and variance over that window, after removing any outliers. + // It returns an error variance = -1 until the window is full. So you must + // ignore the first window_size values returned by this filter. + std::tuple filter(T value); +}; + +template +void SmoothingFilter::initialize(int window_size, float outlier_factor) { + buffer_.resize(window_size); + window_size_ = window_size; + window_size_2x_ = window_size * 2; + outlier_factor_ = outlier_factor; + buffer_index_ = 0; +} + +template +SmoothingFilter::SmoothingFilter() { + initialize(static_cast(buffer_.capacity()), std::numeric_limits::infinity()); +} + +template +SmoothingFilter::SmoothingFilter(int window_size, float outlier_factor) { + initialize(window_size, std::numeric_limits::infinity()); +} + +template +std::tuple SmoothingFilter::filter(T value){ + buffer_[buffer_index_++ % window_size_] = value; + if (buffer_index_ == window_size_2x_) + buffer_index_ = window_size_; + + if (buffer_index_ >= window_size_) { + // find min/max and range of the current buffer. + double min = *std::min_element(buffer_.begin(), buffer_.end()); + double max = *std::max_element(buffer_.begin(), buffer_.end()); + auto range = (max - min); + + // outliers fall outside this lower and upper bound. + auto lower_bound = min + (range * outlier_factor_); + auto upper_bound = max - (range * outlier_factor_); + + double sum = 0; int count = 0; + for(auto i = 0; i < window_size_; ++i) { + if (buffer_[i] >= lower_bound && buffer_[i] <= upper_bound) { + sum += buffer_[i]; + ++count; + } + } + + if (count == 0) + { + // this can happen if the numbers are oddly clustered around the min/max values. + return std::make_tuple(double(min + range / 2), -1); + } + double mean = sum / count; + + double std_dev_sum = 0; + for(auto i = 0; i < window_size_; ++i) { + if (buffer_[i] >= lower_bound && buffer_[i] <= upper_bound) { + double diff = buffer_[i] - mean; + std_dev_sum += diff*diff; + } + } + double variance = std_dev_sum / count; + + return std::make_tuple(mean, variance); + } + else { + // window is not yet full, so return an error code informing the caller that the filter + // is not yet ready. + return std::make_tuple(0, -1); + } +} + + +} //namespace +#endif diff --git a/AirLib/include/common/common_utils/Utils.hpp b/AirLib/include/common/common_utils/Utils.hpp index f93d19384..25149893c 100644 --- a/AirLib/include/common/common_utils/Utils.hpp +++ b/AirLib/include/common/common_utils/Utils.hpp @@ -56,6 +56,7 @@ */ #ifndef _MSC_VER +__attribute__((__format__ (__printf__, 1, 0))) static int _vscprintf(const char * format, va_list pargs) { int retval; @@ -235,20 +236,9 @@ class Utils { return str.substr(i, len - i); } - static string formatNumber(double number, int digits_after_decimal = -1, int digits_before_decimal = -1, bool sign_always = false) - { - std::string format_string = "%"; - if (sign_always) - format_string += "+"; - if (digits_before_decimal >= 0) - format_string += "0" + std::to_string(digits_before_decimal + std::max(digits_after_decimal, 0) + 1); - if (digits_after_decimal >= 0) - format_string += "." + std::to_string(digits_after_decimal); - format_string += "f"; - - return stringf(format_string.c_str(), number); - } - + #ifndef _MSC_VER + __attribute__((__format__ (__printf__, 1, 0))) + #endif static string stringf(const char* format, ...) { va_list args; diff --git a/AirLib/include/common/common_utils/WindowsApisCommonPost.hpp b/AirLib/include/common/common_utils/WindowsApisCommonPost.hpp index 4016a974a..ee7a44aeb 100644 --- a/AirLib/include/common/common_utils/WindowsApisCommonPost.hpp +++ b/AirLib/include/common/common_utils/WindowsApisCommonPost.hpp @@ -6,7 +6,7 @@ //following is required to support Unreal Build System #if (defined _WIN32 || defined _WIN64) && (defined UE_GAME || defined UE_EDITOR) #pragma warning(pop) -#include "HideWindowsPlatformAtomics.h" -#include "HideWindowsPlatformTypes.h" -#include "PostWindowsApi.h" +#include "Windows/HideWindowsPlatformAtomics.h" +#include "Windows/HideWindowsPlatformTypes.h" +#include "Windows/PostWindowsApi.h" #endif diff --git a/AirLib/include/common/common_utils/WindowsApisCommonPre.hpp b/AirLib/include/common/common_utils/WindowsApisCommonPre.hpp index 0772672ce..a0b0866a5 100644 --- a/AirLib/include/common/common_utils/WindowsApisCommonPre.hpp +++ b/AirLib/include/common/common_utils/WindowsApisCommonPre.hpp @@ -6,9 +6,9 @@ //following is required to support Unreal Build System #if (defined _WIN32 || defined _WIN64) && (defined UE_GAME || defined UE_EDITOR) #include "CoreTypes.h" -#include "PreWindowsApi.h" -#include "AllowWindowsPlatformTypes.h" -#include "AllowWindowsPlatformAtomics.h" +#include "Windows/PreWindowsApi.h" +#include "Windows/AllowWindowsPlatformTypes.h" +#include "Windows/AllowWindowsPlatformAtomics.h" //remove warnings for VC++ #pragma warning(push) #pragma warning(disable:4191 6000 28251) diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 61dc19b90..52e6bea05 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -50,7 +50,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { { for (PhysicsBody* body_ptr : *this) { reporter.writeValue("Phys", debug_string_.str()); - reporter.writeValue("Is Gounded", body_ptr->isGrounded()); + reporter.writeValue("Is Grounded", body_ptr->isGrounded()); reporter.writeValue("Force (world)", body_ptr->getWrench().force); reporter.writeValue("Torque (body)", body_ptr->getWrench().torque); } @@ -324,10 +324,11 @@ class FastPhysicsEngine : public PhysicsEngineBase { const Wrench body_wrench = getBodyWrench(body, current.pose.orientation); if (body.isGrounded()) { - // make it stick to the ground until we see body wrench force greater than gravity. - float normalizedForce = body_wrench.force.squaredNorm(); - float normalizedGravity = body.getEnvironment().getState().gravity.squaredNorm(); - if (normalizedForce >= normalizedGravity) + // make it stick to the ground until the magnitude of net external force on body exceeds its weight. + float external_force_magnitude = body_wrench.force.squaredNorm(); + Vector3r weight = body.getMass() * body.getEnvironment().getState().gravity; + float weight_magnitude = weight.squaredNorm(); + if (external_force_magnitude >= weight_magnitude) { //throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1); body.setGrounded(false); diff --git a/AirLib/include/sensors/SensorFactory.hpp b/AirLib/include/sensors/SensorFactory.hpp index 1fffe8641..ed69b4abb 100644 --- a/AirLib/include/sensors/SensorFactory.hpp +++ b/AirLib/include/sensors/SensorFactory.hpp @@ -57,6 +57,8 @@ class SensorFactory { } } } + + virtual ~SensorFactory() = default; }; diff --git a/AirLib/include/sensors/distance/DistanceBase.hpp b/AirLib/include/sensors/distance/DistanceBase.hpp index a0365b88a..0200873a7 100644 --- a/AirLib/include/sensors/distance/DistanceBase.hpp +++ b/AirLib/include/sensors/distance/DistanceBase.hpp @@ -16,16 +16,6 @@ class DistanceBase : public SensorBase { : SensorBase(sensor_name) {} -public: //types - struct Output { //same fields as ROS message - TTimePoint time_stamp; - real_T distance; //meters - real_T min_distance;//m - real_T max_distance;//m - Pose relative_pose; - }; - - public: virtual void reportState(StateReporter& reporter) override { @@ -35,20 +25,20 @@ class DistanceBase : public SensorBase { reporter.writeValue("Dist-Curr", output_.distance); } - const Output& getOutput() const + const DistanceSensorData& getOutput() const { return output_; } protected: - void setOutput(const Output& output) + void setOutput(const DistanceSensorData& output) { output_ = output; } private: - Output output_; + DistanceSensorData output_; }; diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp index 74bb8069d..45adcdddc 100644 --- a/AirLib/include/sensors/distance/DistanceSimple.hpp +++ b/AirLib/include/sensors/distance/DistanceSimple.hpp @@ -14,7 +14,7 @@ namespace msr { namespace airlib { -class DistanceSimple : public DistanceBase { +class DistanceSimple : public DistanceBase { public: DistanceSimple(const AirSimSettings::DistanceSetting& setting = AirSimSettings::DistanceSetting()) : DistanceBase(setting.sensor_name) @@ -62,17 +62,18 @@ class DistanceSimple : public DistanceBase { virtual ~DistanceSimple() = default; -protected: - virtual real_T getRayLength(const Pose& pose) = 0; - const DistanceSimpleParams& getParams() + const DistanceSimpleParams& getParams() const { return params_; } +protected: + virtual real_T getRayLength(const Pose& pose) = 0; + private: //methods - Output getOutputInternal() + DistanceSensorData getOutputInternal() { - Output output; + DistanceSensorData output; const GroundTruth& ground_truth = getGroundTruth(); //order of Pose addition is important here because it also adds quaternions which is not commutative! @@ -97,7 +98,7 @@ class DistanceSimple : public DistanceBase { RandomGeneratorGausianR uncorrelated_noise_; FrequencyLimiter freq_limiter_; - DelayLine delay_line_; + DelayLine delay_line_; //start time }; diff --git a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp index a028365f1..fa1c95b5d 100644 --- a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp +++ b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp @@ -14,7 +14,13 @@ namespace msr { namespace airlib { struct DistanceSimpleParams { real_T min_distance = 20.0f / 100; //m real_T max_distance = 4000.0f / 100; //m - Pose relative_pose; + + Pose relative_pose { + Vector3r(0,0,-1), // position - a little above vehicle (especially for cars) or Vector3r::Zero() + Quaternionr::Identity() // orientation - by default Quaternionr(1, 0, 0, 0) + }; + + bool draw_debug_points = false; /* Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters @@ -40,7 +46,33 @@ struct DistanceSimpleParams { void initializeFromSettings(const AirSimSettings::DistanceSetting& settings) { - unused(settings); + std::string simmode_name = AirSimSettings::singleton().simmode_name; + + min_distance = settings.min_distance; + max_distance = settings.max_distance; + + draw_debug_points = settings.draw_debug_points; + + relative_pose.position = settings.position; + if (std::isnan(relative_pose.position.x())) + relative_pose.position.x() = 0; + if (std::isnan(relative_pose.position.y())) + relative_pose.position.y() = 0; + if (std::isnan(relative_pose.position.z())) { + if (simmode_name == "Multirotor") + relative_pose.position.z() = 0; + else + relative_pose.position.z() = -1; // a little bit above for cars + } + + float pitch, roll, yaw; + pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0; + roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0; + yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0; + relative_pose.orientation = VectorMath::toQuaternion( + Utils::degreesToRadians(pitch), //pitch - rotation around Y axis + Utils::degreesToRadians(roll), //roll - rotation around X axis + Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis } }; diff --git a/AirLib/include/vehicles/car/CarApiFactory.hpp b/AirLib/include/vehicles/car/CarApiFactory.hpp new file mode 100644 index 000000000..fcf361a82 --- /dev/null +++ b/AirLib/include/vehicles/car/CarApiFactory.hpp @@ -0,0 +1,37 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_vehicles_CarApiFactory_hpp +#define msr_airlib_vehicles_CarApiFactory_hpp + +#include "vehicles/car/firmwares/physxcar/PhysXCarApi.hpp" +#include "vehicles/car/firmwares/ardurover/ArduRoverApi.hpp" + +namespace msr { namespace airlib { + +class CarApiFactory { +public: + static std::unique_ptr createApi(const AirSimSettings::VehicleSetting* vehicle_setting, + std::shared_ptr sensor_factory, + const Kinematics::State& state, const Environment& environment, + const msr::airlib::GeoPoint& home_geopoint) + { + if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduRover) { + return std::unique_ptr(new ArduRoverApi(vehicle_setting, sensor_factory, + state, environment, home_geopoint)); + } + else if (vehicle_setting->vehicle_type == "" || //default config + vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePhysXCar) { + return std::unique_ptr(new PhysXCarApi(vehicle_setting, sensor_factory, + state, environment, home_geopoint)); + } + else + throw std::runtime_error(Utils::stringf( + "Cannot create vehicle config because vehicle name '%s' is not recognized", + vehicle_setting->vehicle_name.c_str())); + } +}; + +}} // namespace + +#endif diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 2177ee4b7..3f012b51e 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -58,12 +58,26 @@ class CarApiBase : public VehicleApiBase { Kinematics::State kinematics_estimated; uint64_t timestamp; + CarState() + { + } + CarState(float speed_val, int gear_val, float rpm_val, float maxrpm_val, bool handbrake_val, const Kinematics::State& kinematics_estimated_val, uint64_t timestamp_val) : speed(speed_val), gear(gear_val), rpm(rpm_val), maxrpm(maxrpm_val), handbrake(handbrake_val), kinematics_estimated(kinematics_estimated_val), timestamp(timestamp_val) { } + + //shortcuts + const Vector3r& getPosition() const + { + return kinematics_estimated.pose.position; + } + const Quaternionr& getOrientation() const + { + return kinematics_estimated.pose.orientation; + } }; public: @@ -127,8 +141,9 @@ class CarApiBase : public VehicleApiBase { } virtual void setCarControls(const CarControls& controls) = 0; - virtual CarState getCarState() const = 0; - virtual const CarApiBase::CarControls& getCarControls() const = 0; + virtual void updateCarState(const CarState& state) = 0; + virtual const CarState& getCarState() const = 0; + virtual const CarControls& getCarControls() const = 0; virtual ~CarApiBase() = default; @@ -146,4 +161,4 @@ class CarApiBase : public VehicleApiBase { }} //namespace -#endif \ No newline at end of file +#endif diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp new file mode 100644 index 000000000..37ce927c3 --- /dev/null +++ b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp @@ -0,0 +1,286 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_ArduRoverCarController_hpp +#define msr_airlib_ArduRoverCarController_hpp + +#include "vehicles/car/api/CarApiBase.hpp" +#include "sensors/SensorCollection.hpp" + +#include "common/Common.hpp" +#include "common/AirSimSettings.hpp" + +// Sensors +#include "sensors/imu/ImuBase.hpp" +#include "sensors/gps/GpsBase.hpp" +#include "sensors/magnetometer/MagnetometerBase.hpp" +#include "sensors/barometer/BarometerBase.hpp" +#include "sensors/lidar/LidarBase.hpp" + +#include "UdpSocket.hpp" + +#include +#include + +#define __STDC_FORMAT_MACROS +#include + +namespace msr { namespace airlib { + +class ArduRoverApi : public CarApiBase { + +public: + ArduRoverApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, + const Kinematics::State& state, const Environment& environment, const msr::airlib::GeoPoint& home_geopoint) + : CarApiBase(vehicle_setting, sensor_factory, state, environment), + home_geopoint_(home_geopoint) + { + connection_info_ = static_cast(vehicle_setting)->connection_info; + sensors_ = &getSensors(); + + connect(); + } + + ~ArduRoverApi() + { + closeConnections(); + } + +protected: + virtual void resetImplementation() override + { + CarApiBase::resetImplementation(); + } + +public: + // Update sensor data & send to Ardupilot + virtual void update() override + { + CarApiBase::update(); + + sendSensors(); + recvRoverControl(); + } + + virtual const SensorCollection& getSensors() const override + { + return CarApiBase::getSensors(); + } + + // TODO: VehicleApiBase implementation + virtual bool isApiControlEnabled() const override + { + // Return true so that CarPawnSim gets control message from external firmware and not keyboard + return true; + } + + virtual void enableApiControl(bool is_enabled) override + { + Utils::log("enableApiControl() - Not Implemented", Utils::kLogLevelInfo); + unused(is_enabled); + } + + virtual bool armDisarm(bool arm) override + { + Utils::log("armDisarm() - Not Implemented", Utils::kLogLevelInfo); + unused(arm); + return false; + } + + virtual GeoPoint getHomeGeoPoint() const override + { + return home_geopoint_; + } + + virtual void getStatusMessages(std::vector& messages) override + { + unused(messages); + } + +public: + virtual void setCarControls(const CarControls& controls) override + { + // Currently ArduPilot vehicles don't implement AirSim movement APIs + unused(controls); + } + + virtual void updateCarState(const CarState& car_state) override + { + last_car_state_ = car_state; + } + + virtual const CarState& getCarState() const override + { + return last_car_state_; + } + + virtual const CarControls& getCarControls() const override + { + return last_controls_; + } + +protected: + void closeConnections() + { + if (udpSocket_ != nullptr) + udpSocket_->close(); + } + + void connect() + { + port_ = static_cast(connection_info_.udp_port); + ip_ = connection_info_.udp_address; + + closeConnections(); + + if (ip_ == "") { + throw std::invalid_argument("UdpIp setting is invalid."); + } + + if (port_ == 0) { + throw std::invalid_argument("UdpPort setting has an invalid value."); + } + + Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); + + udpSocket_ = std::make_shared(); + udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port); + } + +private: + void recvRoverControl() + { + // Receive motor data + RoverControlMessage pkt; + int recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100); + while (recv_ret != sizeof(pkt)) { + if (recv_ret <= 0) { + Utils::log(Utils::stringf("Error while receiving rotor control data - ErrorNo: %d", recv_ret), Utils::kLogLevelInfo); + } else { + Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes", recv_ret, sizeof(pkt)), Utils::kLogLevelInfo); + } + + recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100); + } + + last_controls_.throttle = pkt.throttle; + last_controls_.steering = pkt.steering; + + if (pkt.throttle > 0) { + last_controls_.is_manual_gear = false; + last_controls_.manual_gear = 0; + } else { + last_controls_.is_manual_gear = true; + last_controls_.manual_gear = -1; + } + } + + void sendSensors() + { + if (sensors_ == nullptr) + return; + + const auto& gps_output = getGpsData(""); + const auto& imu_output = getImuData(""); + + std::ostringstream oss; + + const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar); + // TODO: Add bool value in settings to check whether to send lidar data or not + // Since it's possible that we don't want to send the lidar data to Ardupilot but still have the lidar (maybe as a ROS topic) + if (count_lidars != 0) { + const auto& lidar_output = getLidarData(""); + oss << "," + "\"lidar\": {" + "\"point_cloud\": ["; + + std::copy(lidar_output.point_cloud.begin(), lidar_output.point_cloud.end(), std::ostream_iterator(oss, ",")); + oss << "]}"; + } + + float yaw; + float pitch; + float roll; + VectorMath::toEulerianAngle(imu_output.orientation, pitch, roll, yaw); + + char buf[65000]; + + // TODO: Split the following sensor packet formation into different parts for individual sensors + + // UDP packets have a maximum size limit of 65kB + int ret = snprintf(buf, sizeof(buf), + "{" + "\"timestamp\": %" PRIu64 "," + "\"imu\": {" + "\"angular_velocity\": [%.12f, %.12f, %.12f]," + "\"linear_acceleration\": [%.12f, %.12f, %.12f]" + "}," + "\"gps\": {" + "\"lat\": %.7f," + "\"lon\": %.7f," + "\"alt\": %.3f" + "}," + "\"velocity\": {" + "\"world_linear_velocity\": [%.12f, %.12f, %.12f]" + "}," + "\"pose\": {" + "\"roll\": %.12f," + "\"pitch\": %.12f," + "\"yaw\": %.12f" + "}" + "%s" + "}\n", + static_cast(msr::airlib::ClockFactory::get()->nowNanos() / 1.0E3), + imu_output.angular_velocity[0], + imu_output.angular_velocity[1], + imu_output.angular_velocity[2], + imu_output.linear_acceleration[0], + imu_output.linear_acceleration[1], + imu_output.linear_acceleration[2], + gps_output.gnss.geo_point.latitude, + gps_output.gnss.geo_point.longitude, + gps_output.gnss.geo_point.altitude, + gps_output.gnss.velocity[0], + gps_output.gnss.velocity[1], + gps_output.gnss.velocity[2], + roll, pitch, yaw, + oss.str().c_str()); + + if (ret == -1) { + Utils::log("Error while allocating memory for sensor message", Utils::kLogLevelInfo); + return; + } + else if (static_cast(ret) >= sizeof(buf)) { + Utils::log(Utils::stringf("Sensor message truncated, lost %d bytes", ret - sizeof(buf)), Utils::kLogLevelInfo); + } + + // Send data + if (udpSocket_ != nullptr) { + udpSocket_->sendto(buf, strlen(buf), ip_, port_); + } + } + +private: + struct RoverControlMessage { + float throttle; + float steering; + }; + + AirSimSettings::MavLinkConnectionInfo connection_info_; + + std::shared_ptr udpSocket_; + + uint16_t port_; + std::string ip_; + + const SensorCollection* sensors_; + + CarControls last_controls_; + GeoPoint home_geopoint_; + CarState last_car_state_; +}; + +}} // namespace + +#endif diff --git a/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp b/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp new file mode 100644 index 000000000..8e2cbfe47 --- /dev/null +++ b/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp @@ -0,0 +1,97 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_PhysXCarController_hpp +#define msr_airlib_PhysXCarController_hpp + +#include "vehicles/car/api/CarApiBase.hpp" + +namespace msr { namespace airlib { + +class PhysXCarApi : public CarApiBase { +public: + PhysXCarApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, + const Kinematics::State& state, const Environment& environment, const msr::airlib::GeoPoint& home_geopoint) + : CarApiBase(vehicle_setting, sensor_factory, state, environment), + home_geopoint_(home_geopoint) + {} + + ~PhysXCarApi() + {} + +protected: + virtual void resetImplementation() override + { + CarApiBase::resetImplementation(); + } + +public: + virtual void update() override + { + CarApiBase::update(); + } + + virtual const SensorCollection& getSensors() const override + { + return CarApiBase::getSensors(); + } + + // VehicleApiBase Implementation + virtual void enableApiControl(bool is_enabled) override + { + if (api_control_enabled_ != is_enabled) { + last_controls_ = CarControls(); + api_control_enabled_ = is_enabled; + } + } + + virtual bool isApiControlEnabled() const override + { + return api_control_enabled_; + } + + virtual GeoPoint getHomeGeoPoint() const override + { + return home_geopoint_; + } + + virtual bool armDisarm(bool arm) override + { + //TODO: implement arming for car + unused(arm); + return true; + } + + +public: + virtual void setCarControls(const CarControls& controls) override + { + last_controls_ = controls; + } + + virtual void updateCarState(const CarState& car_state) override + { + last_car_state_ = car_state; + } + + virtual const CarState& getCarState() const override + { + return last_car_state_; + } + + virtual const CarControls& getCarControls() const override + { + return last_controls_; + } + +private: + bool api_control_enabled_ = false; + GeoPoint home_geopoint_; + CarControls last_controls_; + CarState last_car_state_; + +}; + +}} + +#endif diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index d7135d52d..29566c915 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -36,12 +36,12 @@ class MultiRotorParams { Vector3r body_box; /*********** optional parameters with defaults ***********/ - real_T linear_drag_coefficient = 1.3f / 4.0f; + real_T linear_drag_coefficient = 1.3f / 4.0f; //sample value 1.3 from http://klsin.bpmsg.com/how-fast-can-a-quadcopter-fly/, but divided by 4 to account // for nice streamlined frame design and allow higher top speed which is more fun. //angular coefficient is usually 10X smaller than linear, however we should replace this with exact number //http://physics.stackexchange.com/q/304742/14061 - real_T angular_drag_coefficient = linear_drag_coefficient; + real_T angular_drag_coefficient = linear_drag_coefficient; real_T restitution = 0.55f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic. real_T friction = 0.5f; RotorParams rotor_params; @@ -100,27 +100,27 @@ class MultiRotorParams { /// which shows that given an array of 4 motors, the first is placed top right and flies counter clockwise (CCW) and /// the second is placed bottom left, and also flies CCW. The third in the array is placed top left and flies clockwise (CW) /// while the last is placed bottom right and also flies clockwise. This is how the 4 items in the arm_lengths and - /// arm_angles arrays will be used. So arm_lengths is 4 numbers (in meters) where four arm lengths, 0 is top right, - /// 1 is bottom left, 2 is top left and 3 is bottom right. arm_angles is 4 numbers (in degrees) relative to forward vector (0,1), - /// provided in the same order where 0 is top right, 1 is bottom left, 2 is top left and 3 is bottom right, so for example, + /// arm_angles arrays will be used. So arm_lengths is 4 numbers (in meters) where four arm lengths, 0 is top right, + /// 1 is bottom left, 2 is top left and 3 is bottom right. arm_angles is 4 numbers (in degrees) relative to forward vector (0,1), + /// provided in the same order where 0 is top right, 1 is bottom left, 2 is top left and 3 is bottom right, so for example, /// the angles for a regular symmetric X pattern would be 45, 225, 315, 135. The rotor_z is the offset of each motor upwards - /// relative to the center of mass (in meters). + /// relative to the center of mass (in meters). static void initializeRotorQuadX(vector& rotor_poses /* the result we are building */, - uint rotor_count /* must be 4 */, - real_T arm_lengths[], + uint rotor_count /* must be 4 */, + real_T arm_lengths[], real_T rotor_z /* z relative to center of gravity */) { Vector3r unit_z(0, 0, -1); //NED frame if (rotor_count == 4) { rotor_poses.clear(); - /* Note: rotor_poses are built in this order: + /* Note: rotor_poses are built in this order: x-axis (2) | (0) | -------------- y-axis | - (1) | (3) + (1) | (3) */ // vectors below are rotated according to NED left hand rule (so the vectors are rotated counter clockwise). Quaternionr quadx_rot(AngleAxisr(M_PIf / 4, unit_z)); @@ -128,9 +128,9 @@ class MultiRotorParams { unit_z, RotorTurningDirection::RotorTurningDirectionCCW); rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(0, -arm_lengths[1], rotor_z), quadx_rot, true), unit_z, RotorTurningDirection::RotorTurningDirectionCCW); - rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[2], 0, rotor_z), quadx_rot, true), + rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[2], 0, rotor_z), quadx_rot, true), unit_z, RotorTurningDirection::RotorTurningDirectionCW); - rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(-arm_lengths[3], 0, rotor_z), quadx_rot, true), + rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(-arm_lengths[3], 0, rotor_z), quadx_rot, true), unit_z, RotorTurningDirection::RotorTurningDirectionCW); } else @@ -153,8 +153,8 @@ class MultiRotorParams { \ / \/ (1)-------(0) y-axis - /\ - / \ + /\ + / \ (5) (3) */ @@ -200,9 +200,9 @@ class MultiRotorParams { inertia = Matrix3x3r::Zero(); //http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node64.html - inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z()); - inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z()); - inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y()); + inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z()); + inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z()); + inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y()); for (size_t i = 0; i < rotor_poses.size(); ++i) { const auto& pos = rotor_poses.at(i).position; @@ -212,6 +212,194 @@ class MultiRotorParams { } } + // Some Frame types which can be used by different firmwares + // Specific frame configurations, modifications can be done in the Firmware Params + + void setupFrameGenericQuad(Params& params) + { + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 4; + std::vector arm_lengths(params.rotor_count, 0.2275f); + + //set up mass + params.mass = 1.0f; //can be varied from 0.800 to 1.600 + real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust + // given new thrust coefficients, motor max_rpm and propeller diameter. + params.rotor_params.calculateMaxThrust(); + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; + real_T rotor_z = 2.5f / 100; + + //computer rotor poses + initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + + void setupFrameGenericHex(Params& params) + { + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 6; + std::vector arm_lengths(params.rotor_count, 0.2275f); + + //set up mass + params.mass = 1.0f; //can be varied from 0.800 to 1.600 + real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust + // given new thrust coefficients, motor max_rpm and propeller diameter. + params.rotor_params.calculateMaxThrust(); + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; + real_T rotor_z = 2.5f / 100; + + //computer rotor poses + initializeRotorHexX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + + void setupFrameFlamewheel(Params& params) + { + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 4; + std::vector arm_lengths(params.rotor_count, 0.225f); + + //set up mass + params.mass = 1.635f; + real_T motor_assembly_weight = 0.052f; + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + params.rotor_params.C_T = 0.11f; + params.rotor_params.C_P = 0.047f; + params.rotor_params.max_rpm = 9500; + params.rotor_params.calculateMaxThrust(); + params.linear_drag_coefficient *= 4; // make top speed more real. + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f; + real_T rotor_z = 0.15f; + + //computer rotor poses + initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + + void setupFrameFlamewheelFLA(Params& params) + { + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 4; + std::vector arm_lengths(params.rotor_count, 0.225f); + + //set up mass + params.mass = 2.25f; + real_T motor_assembly_weight = 0.1f; + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + params.rotor_params.C_T = 0.2f; + params.rotor_params.C_P = 0.1f; + params.rotor_params.max_rpm = 9324; + params.rotor_params.calculateMaxThrust(); + params.linear_drag_coefficient *= 4; // make top speed more real. + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f; + real_T rotor_z = 0.15f; + + //computer rotor poses + initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + + void setupFrameBlacksheep(Params& params) + { + /* + Motor placement: + x + (2) | (0) + | + ------------ y + | + (1) | (3) + | + + */ + //set up arm lengths + //dimensions are for Team Blacksheep Discovery (http://team-blacksheep.com/products/product:98) + params.rotor_count = 4; + std::vector arm_lengths; + + Vector3r unit_z(0, 0, -1); //NED frame + + // relative to Forward vector in the order (0,3,1,2) required by quad X pattern + // http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg + arm_lengths.push_back(0.22f); + arm_lengths.push_back(0.255f); + arm_lengths.push_back(0.22f); + arm_lengths.push_back(0.255f); + + // note: the Forward vector is actually the "x" axis, and the AngleAxisr rotation is pointing down and is left handed, so this means the rotation + // is counter clockwise, so the vector (arm_lengths[i], 0) is the X-axis, so the CCW rotations to position each arm correctly are listed below: + // See measurements here: http://diydrones.com/profiles/blogs/arducopter-tbs-discovery-style (angles reversed because we are doing CCW rotation) + std::vector arm_angles; + arm_angles.push_back(-55.0f); + arm_angles.push_back(125.0f); + arm_angles.push_back(55.0f); + arm_angles.push_back(-125.0f); + + // quad X pattern + std::vector rotor_directions; + rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); + rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); + rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); + rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); + + // data from + // http://dronesvision.net/team-blacksheep-750kv-motor-esc-set-for-tbs-discovery-fpv-quadcopter/ + //set up mass + params.mass = 2.0f; //can be varied from 0.800 to 1.600 + real_T motor_assembly_weight = 0.052f; // weight for TBS motors + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + // the props we are using a E-Prop, which I didn't find in UIUC database, but this one is close: + // http://m-selig.ae.illinois.edu/props/volume-2/plots/ef_130x70_static_ctcp.png + params.rotor_params.C_T = 0.11f; + params.rotor_params.C_P = 0.047f; + params.rotor_params.max_rpm = 9500; + params.rotor_params.calculateMaxThrust(); + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.20f; params.body_box.y() = 0.12f; params.body_box.z() = 0.04f; + real_T rotor_z = 2.5f / 100; + + //computer rotor poses + params.rotor_poses.clear(); + for (uint i = 0; i < 4; i++) + { + Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z)); + params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]); + }; + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + private: Params params_; SensorCollection sensors_; //maintains sensor type indexed collection of sensors diff --git a/AirLib/include/vehicles/multirotor/MultiRotor.hpp b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp similarity index 82% rename from AirLib/include/vehicles/multirotor/MultiRotor.hpp rename to AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp index 454e6559a..81902d6cf 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotor.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp @@ -1,12 +1,12 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. -#ifndef msr_airlib_multirotor_hpp -#define msr_airlib_multirotor_hpp +#ifndef msr_airlib_multirotorphysicsbody_hpp +#define msr_airlib_multirotorphysicsbody_hpp #include "common/Common.hpp" #include "common/CommonStructs.hpp" -#include "Rotor.hpp" +#include "RotorActuator.hpp" #include "api/VehicleApiBase.hpp" #include "api/VehicleSimApiBase.hpp" #include "MultiRotorParams.hpp" @@ -16,9 +16,9 @@ namespace msr { namespace airlib { -class MultiRotor : public PhysicsBody { +class MultiRotorPhysicsBody : public PhysicsBody { public: - MultiRotor(MultiRotorParams* params, VehicleApiBase* vehicle_api, + MultiRotorPhysicsBody(MultiRotorParams* params, VehicleApiBase* vehicle_api, Kinematics* kinematics, Environment* environment) : params_(params), vehicle_api_(vehicle_api) { @@ -101,15 +101,15 @@ class MultiRotor : public PhysicsBody { virtual uint dragVertexCount() const override { - return static_cast(drag_vertices_.size()); + return static_cast(drag_faces_.size()); } virtual PhysicsBodyVertex& getDragVertex(uint index) override { - return drag_vertices_.at(index); + return drag_faces_.at(index); } virtual const PhysicsBodyVertex& getDragVertex(uint index) const override { - return drag_vertices_.at(index); + return drag_faces_.at(index); } virtual real_T getRestitution() const override @@ -121,12 +121,12 @@ class MultiRotor : public PhysicsBody { return params_->getParams().friction; } - Rotor::Output getRotorOutput(uint rotor_index) const + RotorActuator::Output getRotorOutput(uint rotor_index) const { return rotors_.at(rotor_index).getOutput(); } - virtual ~MultiRotor() = default; + virtual ~MultiRotorPhysicsBody() = default; private: //methods void initialize(Kinematics* kinematics, Environment* environment) @@ -139,7 +139,7 @@ class MultiRotor : public PhysicsBody { initSensors(*params_, getKinematics(), getEnvironment()); } - static void createRotors(const MultiRotorParams& params, vector& rotors, const Environment* environment) + static void createRotors(const MultiRotorParams& params, vector& rotors, const Environment* environment) { rotors.clear(); //for each rotor pose @@ -191,13 +191,13 @@ class MultiRotor : public PhysicsBody { * params.linear_drag_coefficient / 2; //add six drag vertices representing 6 sides - drag_vertices_.clear(); - drag_vertices_.emplace_back(Vector3r(0, 0, -params.body_box.z()), Vector3r(0, 0, -1), drag_factor_unit.z()); - drag_vertices_.emplace_back(Vector3r(0, 0, params.body_box.z()), Vector3r(0, 0, 1), drag_factor_unit.z()); - drag_vertices_.emplace_back(Vector3r(0, -params.body_box.y(), 0), Vector3r(0, -1, 0), drag_factor_unit.y()); - drag_vertices_.emplace_back(Vector3r(0, params.body_box.y(), 0), Vector3r(0, 1, 0), drag_factor_unit.y()); - drag_vertices_.emplace_back(Vector3r(-params.body_box.x(), 0, 0), Vector3r(-1, 0, 0), drag_factor_unit.x()); - drag_vertices_.emplace_back(Vector3r( params.body_box.x(), 0, 0), Vector3r( 1, 0, 0), drag_factor_unit.x()); + drag_faces_.clear(); + drag_faces_.emplace_back(Vector3r(0, 0, -params.body_box.z() / 2.0f), Vector3r(0, 0, -1), drag_factor_unit.z()); + drag_faces_.emplace_back(Vector3r(0, 0, params.body_box.z() / 2.0f), Vector3r(0, 0, 1), drag_factor_unit.z()); + drag_faces_.emplace_back(Vector3r(0, -params.body_box.y() / 2.0f, 0), Vector3r(0, -1, 0), drag_factor_unit.y()); + drag_faces_.emplace_back(Vector3r(0, params.body_box.y() / 2.0f, 0), Vector3r(0, 1, 0), drag_factor_unit.y()); + drag_faces_.emplace_back(Vector3r(-params.body_box.x() / 2.0f, 0, 0), Vector3r(-1, 0, 0), drag_factor_unit.x()); + drag_faces_.emplace_back(Vector3r( params.body_box.x() / 2.0f, 0, 0), Vector3r( 1, 0, 0), drag_factor_unit.x()); } @@ -205,8 +205,8 @@ class MultiRotor : public PhysicsBody { MultiRotorParams* params_; //let us be the owner of rotors object - vector rotors_; - vector drag_vertices_; + vector rotors_; + vector drag_faces_; std::unique_ptr environment_; VehicleApiBase* vehicle_api_; diff --git a/AirLib/include/vehicles/multirotor/Rotor.hpp b/AirLib/include/vehicles/multirotor/RotorActuator.hpp similarity index 93% rename from AirLib/include/vehicles/multirotor/Rotor.hpp rename to AirLib/include/vehicles/multirotor/RotorActuator.hpp index bc749cba8..48f7a9690 100644 --- a/AirLib/include/vehicles/multirotor/Rotor.hpp +++ b/AirLib/include/vehicles/multirotor/RotorActuator.hpp @@ -3,8 +3,8 @@ -#ifndef airsimcore_motor_hpp -#define airsimcore_motor_hpp +#ifndef rotor_actuator_hpp +#define rotor_actuator_hpp #include #include "common/Common.hpp" @@ -18,7 +18,7 @@ namespace msr { namespace airlib { //Rotor gets control signal as input (PWM or voltage represented from 0 to 1) which causes //change in rotation speed and turning direction and ultimately produces force and thrust as //output -class Rotor : public PhysicsBodyVertex { +class RotorActuator : public PhysicsBodyVertex { public: //types struct Output { real_T thrust; @@ -30,11 +30,11 @@ class Rotor : public PhysicsBodyVertex { }; public: //methods - Rotor() + RotorActuator() { //allow default constructor with later call for initialize } - Rotor(const Vector3r& position, const Vector3r& normal, RotorTurningDirection turning_direction, + RotorActuator(const Vector3r& position, const Vector3r& normal, RotorTurningDirection turning_direction, const RotorParams& params, const Environment* environment, uint id = -1) { initialize(position, normal, turning_direction, params, environment, id); @@ -122,7 +122,7 @@ class Rotor : public PhysicsBodyVertex { //see relationship of rotation speed with thrust: http://physics.stackexchange.com/a/32013/14061 output.speed = sqrt(output.control_signal_filtered * params.max_speed_square); output.thrust = output.control_signal_filtered * params.max_thrust; - output.torque_scaler = output.control_signal_input * params.max_torque * static_cast(turning_direction); + output.torque_scaler = output.control_signal_filtered * params.max_torque * static_cast(turning_direction); output.turning_direction = turning_direction; } diff --git a/AirLib/include/vehicles/multirotor/RotorParams.hpp b/AirLib/include/vehicles/multirotor/RotorParams.hpp index d2bf8bafd..11180de81 100644 --- a/AirLib/include/vehicles/multirotor/RotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/RotorParams.hpp @@ -4,7 +4,6 @@ #ifndef msr_airlib_RotorParams_hpp #define msr_airlib_RotorParams_hpp - #include "common/Common.hpp" namespace msr { @@ -24,7 +23,7 @@ namespace msr { torque in N.m = C_P * \rho * n^2 * D^5 / (2*pi) where, \rho = air density (1.225 kg/m^3) - n = radians per sec + n = revolutions per sec D = propeller diameter in meters C_T, C_P = dimensionless constants available at propeller performance database http://m-selig.ae.illinois.edu/props/propDB.html diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index 8ec6e581f..b95c22211 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -24,12 +24,20 @@ class MultirotorApiBase : public VehicleApiBase { protected: //must be implemented /************************* low level move APIs *********************************/ - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) = 0; - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) = 0; + virtual void commandMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm) = 0; + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0; + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0; + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0; + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0; + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0; + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 0; virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; + /************************* set Controller Gains APIs *********************************/ + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) = 0; + /************************* State APIs *********************************/ virtual Kinematics::State getKinematicsEstimated() const = 0; virtual LandedState getLandedState() const = 0; @@ -81,8 +89,13 @@ class MultirotorApiBase : public VehicleApiBase { virtual bool land(float timeout_sec); virtual bool goHome(float timeout_sec); - virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration); - virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration); + virtual bool moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration); + virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); + virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); + virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); + virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration); virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, @@ -97,6 +110,12 @@ class MultirotorApiBase : public VehicleApiBase { virtual bool hover(); virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); + /************************* set angle gain APIs *********************************/ + virtual void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd); + /************************* Safety APIs *********************************/ virtual void setSafetyEval(const shared_ptr safety_eval_ptr); virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, @@ -112,7 +131,8 @@ class MultirotorApiBase : public VehicleApiBase { state.timestamp = clock()->nowNanos(); state.landed_state = getLandedState(); state.rc_data = getRCData(); - + state.ready = isReady(state.ready_message); + state.can_arm = canArm(); return state; } @@ -126,11 +146,15 @@ class MultirotorApiBase : public VehicleApiBase { typedef std::function WaitFunction; //*********************************safe wrapper around low level commands*************************************************** + virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z); + virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle); + virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle); + virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z); + virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z); + virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle); virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); - virtual void moveByRollPitchZInternal(float pitch, float roll, float z, float yaw); - virtual void moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate); /************* safety checks & emergency maneuvers ************/ virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); @@ -315,6 +339,7 @@ class MultirotorApiBase : public VehicleApiBase { //TODO: make this configurable? float landing_vel_ = 0.2f; //velocity to use for landing float approx_zero_vel_ = 0.05f; + float approx_zero_angular_vel_ = 0.01f; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp index b78fcca6c..4c813b9fa 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp @@ -74,15 +74,18 @@ struct MultirotorState { uint64_t timestamp; LandedState landed_state; RCData rc_data; + bool ready; // indicates drone is ready for commands + std::string ready_message; // can show error message if drone is not reachable over the network or is not responding + bool can_arm; // indicates drone is ready to be armed MultirotorState() {} MultirotorState(const CollisionInfo& collision_val, const Kinematics::State& kinematics_estimated_val, const GeoPoint& gps_location_val, uint64_t timestamp_val, - LandedState landed_state_val, const RCData& rc_data_val) + LandedState landed_state_val, const RCData& rc_data_val, bool ready_val, const std::string& message, bool can_arm_val) : collision(collision_val), kinematics_estimated(kinematics_estimated_val), gps_location(gps_location_val), timestamp(timestamp_val), - landed_state(landed_state_val), rc_data(rc_data_val) + landed_state(landed_state_val), rc_data(rc_data_val), ready(ready_val), ready_message(message), can_arm(can_arm_val) { } diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp index c4cfd7a95..2faf19e43 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp @@ -47,8 +47,10 @@ class MultirotorRpcLibAdapators : public RpcLibAdapatorsBase { uint64_t timestamp; LandedState landed_state; RCData rc_data; + bool ready; + std::string ready_message; std::vector controller_messages; - + bool can_arm; MSGPACK_DEFINE_MAP(collision, kinematics_estimated, gps_location, timestamp, landed_state, rc_data); @@ -63,12 +65,15 @@ class MultirotorRpcLibAdapators : public RpcLibAdapatorsBase { timestamp = s.timestamp; landed_state = s.landed_state; rc_data = RCData(s.rc_data); + ready = s.ready; + ready_message = s.ready_message; + can_arm = s.can_arm; } msr::airlib::MultirotorState to() const { return msr::airlib::MultirotorState(collision.to(), kinematics_estimated.to(), - gps_location.to(), timestamp, landed_state, rc_data.to()); + gps_location.to(), timestamp, landed_state, rc_data.to(), ready, ready_message, can_arm); } }; }; diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index b90177e2f..05e63cbc9 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -23,8 +23,13 @@ class MultirotorRpcLibClient : public RpcLibClientBase { MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByAngleZAsync(float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByAngleThrottleAsync(float pitch, float roll, float throttle, float yaw_rate, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration, DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration, @@ -43,9 +48,12 @@ class MultirotorRpcLibClient : public RpcLibClientBase { MultirotorRpcLibClient* rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name = ""); MultirotorRpcLibClient* hoverAsync(const std::string& vehicle_name = ""); + void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); + void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); + void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); + void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); void moveByRC(const RCData& rc_data, const std::string& vehicle_name = ""); - MultirotorState getMultirotorState(const std::string& vehicle_name = ""); bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp index c4ff3ff86..400ac408e 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -34,7 +34,7 @@ class ArduCopterApi : public MultirotorApiBase { public: ArduCopterApi(const MultiRotorParams* vehicle_params, const AirSimSettings::MavLinkConnectionInfo& connection_info) - : ip(connection_info.ip_address), vehicle_params_(vehicle_params) + : ip_(connection_info.udp_address), vehicle_params_(vehicle_params) { connection_info_ = connection_info; sensors_ = &getSensors(); @@ -48,7 +48,7 @@ class ArduCopterApi : public MultirotorApiBase { } -public: +public: virtual void resetImplementation() override { MultirotorApiBase::resetImplementation(); @@ -68,26 +68,28 @@ class ArduCopterApi : public MultirotorApiBase { // TODO:VehicleApiBase implementation virtual bool isApiControlEnabled() const override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: isApiControlEnabled", Utils::kLogLevelInfo); return false; } - virtual void enableApiControl(bool) override + virtual void enableApiControl(bool is_enabled) override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: enableApiControl", Utils::kLogLevelInfo); + unused(is_enabled); } - virtual bool armDisarm(bool) override + virtual bool armDisarm(bool arm) override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: armDisarm", Utils::kLogLevelInfo); + unused(arm); return false; } virtual GeoPoint getHomeGeoPoint() const override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: getHomeGeoPoint", Utils::kLogLevelInfo); return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); } - virtual void getStatusMessages(std::vector&) override + virtual void getStatusMessages(std::vector& messages) override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + unused(messages); } virtual const SensorCollection& getSensors() const override @@ -100,54 +102,61 @@ class ArduCopterApi : public MultirotorApiBase { { return rotor_controls_[rotor_index]; } + virtual size_t getActuatorCount() const override { return vehicle_params_->getParams().rotor_count; } + virtual void moveByRC(const RCData& rc_data) override { setRCData(rc_data); } - virtual void setSimulatedGroundTruth(const Kinematics::State*, const Environment*) override + + virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) override { + Utils::log("Not Implemented: setSimulatedGroundTruth", Utils::kLogLevelInfo); + unused(kinematics); + unused(environment); } + virtual bool setRCData(const RCData& rc_data) override { last_rcData_ = rc_data; - is_rc_connected = true; + is_rc_connected_ = true; return true; } -protected: +protected: virtual Kinematics::State getKinematicsEstimated() const override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: getKinematicsEstimated", Utils::kLogLevelInfo); Kinematics::State state; return state; } virtual Vector3r getPosition() const override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: getPosition", Utils::kLogLevelInfo); return Vector3r(Utils::nan(), Utils::nan(), Utils::nan()); } virtual Vector3r getVelocity() const override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: getVelocity", Utils::kLogLevelInfo); return Vector3r(Utils::nan(), Utils::nan(), Utils::nan()); } virtual Quaternionr getOrientation() const override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: getOrientation", Utils::kLogLevelInfo); return Quaternionr(Utils::nan(), Utils::nan(), Utils::nan(), Utils::nan()); } virtual LandedState getLandedState() const override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: getLandedState", Utils::kLogLevelInfo); return LandedState::Landed; } @@ -159,7 +168,7 @@ class ArduCopterApi : public MultirotorApiBase { virtual GeoPoint getGpsLocation() const override { - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: getGpsLocation", Utils::kLogLevelInfo); return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); } @@ -170,7 +179,7 @@ class ArduCopterApi : public MultirotorApiBase { virtual float getTakeoffZ() const override { - // pick a number, 3 meters is probably safe + // pick a number, 3 meters is probably safe // enough to get out of the backwash turbulence. Negative due to NED coordinate system. // return params_.takeoff.takeoff_z; return 3.0; @@ -181,22 +190,76 @@ class ArduCopterApi : public MultirotorApiBase { return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled } - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) override + { + unused(controllerType); + unused(kp); + unused(ki); + unused(kd); + Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo); + } + + virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override + { + unused(front_right_pwm); + unused(front_left_pwm); + unused(rear_right_pwm); + unused(rear_left_pwm); + Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override { - unused(pitch); unused(roll); - unused(throttle); + unused(pitch); unused(yaw_rate); - Utils::log("Not Implemented", Utils::kLogLevelInfo); + unused(throttle); + Utils::log("Not Implemented: commandRollPitchYawrateThrottle", Utils::kLogLevelInfo); } - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override { - unused(pitch); unused(roll); + unused(pitch); + unused(yaw); unused(z); + Utils::log("Not Implemented: commandRollPitchYawZ", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + unused(roll); + unused(pitch); unused(yaw); - Utils::log("Not Implemented", Utils::kLogLevelInfo); + unused(throttle); + Utils::log("Not Implemented: commandRollPitchYawThrottle", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override + { + unused(roll); + unused(pitch); + unused(yaw_rate); + unused(z); + Utils::log("Not Implemented: commandRollPitchYawrateZ", Utils::kLogLevelInfo); + } + + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override + { + unused(roll_rate); + unused(pitch_rate); + unused(yaw_rate); + unused(z); + Utils::log("Not Implemented: commandAngleRatesZ", Utils::kLogLevelInfo); + } + + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + unused(roll_rate); + unused(pitch_rate); + unused(yaw_rate); + unused(throttle); + Utils::log("Not Implemented: commandAngleRatesZ", Utils::kLogLevelInfo); } virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override @@ -205,7 +268,7 @@ class ArduCopterApi : public MultirotorApiBase { unused(vy); unused(vz); unused(yaw_mode); - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: commandVelocity", Utils::kLogLevelInfo); } virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override @@ -214,7 +277,7 @@ class ArduCopterApi : public MultirotorApiBase { unused(vy); unused(z); unused(yaw_mode); - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: commandVelocityZ", Utils::kLogLevelInfo); } virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override @@ -223,7 +286,7 @@ class ArduCopterApi : public MultirotorApiBase { unused(y); unused(z); unused(yaw_mode); - Utils::log("Not Implemented", Utils::kLogLevelInfo); + Utils::log("Not Implemented: commandPosition", Utils::kLogLevelInfo); } virtual const MultirotorApiParams& getMultirotorApiParams() const override @@ -242,44 +305,23 @@ class ArduCopterApi : public MultirotorApiBase { void connect() { - port = static_cast(connection_info_.ip_port); + port_ = static_cast(connection_info_.udp_port); closeConnections(); - if (ip == "") { + if (ip_ == "") { throw std::invalid_argument("UdpIp setting is invalid."); } - if (port == 0) { + if (port_ == 0) { throw std::invalid_argument("UdpPort setting has an invalid value."); } - Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); - Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); udpSocket_ = std::make_shared(); - udpSocket_->bind(connection_info_.local_host_ip, connection_info_.sitl_ip_port); - } - - const GpsBase* getGps() const - { - return static_cast(sensors_->getByType(SensorBase::SensorType::Gps)); - } - const ImuBase* getImu() const - { - 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 LidarBase* getLidar() const - { - return static_cast(sensors_->getByType(SensorBase::SensorType::Lidar)); + udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port); } private: @@ -296,15 +338,13 @@ class ArduCopterApi : public MultirotorApiBase { if (sensors_ == nullptr) return; - const auto& gps_output = getGps()->getOutput(); - const auto& imu_output = getImu()->getOutput(); - // const auto& baro_output = getBarometer()->getOutput(); - // const auto& mag_output = getMagnetometer()->getOutput(); + const auto& gps_output = getGpsData(""); + const auto& imu_output = getImuData(""); std::ostringstream oss; // Send RC channels to Ardupilot if present - if (is_rc_connected && last_rcData_.is_valid) { + if (is_rc_connected_ && last_rcData_.is_valid) { oss << "," "\"rc\": {" "\"channels\": [" @@ -319,11 +359,11 @@ class ArduCopterApi : public MultirotorApiBase { << "]}"; } - const auto lidar = getLidar(); + const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar); // TODO: Add bool value in settings to check whether to send lidar data or not // Since it's possible that we don't want to send the lidar data to Ardupilot but still have the lidar (maybe as a ROS topic) - if (lidar != nullptr) { - const auto& lidar_output = lidar->getOutput(); + if (count_lidars != 0) { + const auto& lidar_output = getLidarData(""); oss << "," "\"lidar\": {" "\"point_cloud\": ["; @@ -345,22 +385,22 @@ class ArduCopterApi : public MultirotorApiBase { int ret = snprintf(buf, sizeof(buf), "{" "\"timestamp\": %" PRIu64 "," - "\"imu\": {" - "\"angular_velocity\": [%lf, %lf, %lf]," - "\"linear_acceleration\": [%lf, %lf, %lf]" + "\"imu\": {" + "\"angular_velocity\": [%.12f, %.12f, %.12f]," + "\"linear_acceleration\": [%.12f, %.12f, %.12f]" "}," "\"gps\": {" - "\"lat\": %lf," - "\"lon\": %lf," - "\"alt\": %lf" + "\"lat\": %.7f," + "\"lon\": %.7f," + "\"alt\": %.3f" "}," "\"velocity\": {" - "\"world_linear_velocity\": [%lf, %lf, %lf]" + "\"world_linear_velocity\": [%.12f, %.12f, %.12f]" "}," "\"pose\": {" - "\"roll\": %lf," - "\"pitch\": %lf," - "\"yaw\": %lf" + "\"roll\": %.12f," + "\"pitch\": %.12f," + "\"yaw\": %.12f" "}" "%s" "}\n", @@ -390,7 +430,7 @@ class ArduCopterApi : public MultirotorApiBase { // Send data if (udpSocket_ != nullptr) { - udpSocket_->sendto(buf, strlen(buf), ip, port); + udpSocket_->sendto(buf, strlen(buf), ip_, port_); } } @@ -405,11 +445,11 @@ class ArduCopterApi : public MultirotorApiBase { } else { Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes", recv_ret, sizeof(pkt)), Utils::kLogLevelInfo); } - + recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100); } - for (auto i = 0; i < RotorControlCount && i < kArduCopterRotorControlCount; ++i) { + for (auto i = 0; i < kArduCopterRotorControlCount; ++i) { rotor_controls_[i] = pkt.pwm[i]; } @@ -426,20 +466,17 @@ class ArduCopterApi : public MultirotorApiBase { std::shared_ptr udpSocket_; AirSimSettings::MavLinkConnectionInfo connection_info_; - uint16_t port; - const std::string& ip; + uint16_t port_; + const std::string& ip_; const SensorCollection* sensors_; const MultiRotorParams* vehicle_params_; MultirotorApiParams safety_params_; RCData last_rcData_; - bool is_rc_connected; - - // TODO: Increase to 6 or 8 for hexa or larger frames, 11 was used in SoloAPI - static const int RotorControlCount = 4; + bool is_rc_connected_; - float rotor_controls_[RotorControlCount]; + float rotor_controls_[kArduCopterRotorControlCount]; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp index c6117a64a..38237602d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp @@ -28,39 +28,14 @@ class ArduCopterParams : public MultiRotorParams { } protected: - // TODO: Change the below function to setupGenericQuadX() which can be called by setupParams() based on connection_info.model - // This will be needed for different frames virtual void setupParams() override { auto& params = getParams(); - /******* Below is same config as PX4 generic model ********/ + // Use connection_info_.model for the model name, see Px4MultiRotorParams for example - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.2275f); - - //set up mass - params.mass = 1.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust - // given new thrust coefficients, motor max_rpm and propeller diameter. - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - - //leave everything else to defaults + // Only Generic for now + setupFrameGenericQuad(params); } virtual const SensorFactory* getSensorFactory() const override @@ -75,8 +50,6 @@ class ArduCopterParams : public MultiRotorParams { private: AirSimSettings::MavLinkConnectionInfo connection_info_; - vector> sensor_storage_; - // const AirSimSettings::MavlinkVehicleSetting* vehicle_setting_; //store as pointer because of derived classes std::shared_ptr sensor_factory_; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp index 3c2990d52..5bf951040 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp @@ -2,7 +2,7 @@ #define msr_airlib_ArduCopterSoloApi_h #include "AdHocConnection.hpp" -#include "vehicles/multirotor/MultiRotor.hpp" +#include "vehicles/multirotor/MultiRotorPhysicsBody.hpp" #include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp" namespace msr { namespace airlib { @@ -11,207 +11,205 @@ namespace msr { namespace airlib { class ArduCopterSoloApi : public MavLinkMultirotorApi { public: - virtual ~ArduCopterSoloApi() - { - closeAllConnection(); - } - - virtual void update() - { - if (sensors_ == nullptr) - return; - - // send GPS and other sensor updates - const auto gps = getGps(); - if (gps != nullptr) { - const auto& gps_output = gps->getOutput(); - const auto& imu_output = getImu()->getOutput(); - // const auto& mag_output = getMagnetometer()->getOutput(); - // const auto& baro_output = getBarometer()->getOutput(); - - SensorMessage packet; - packet.timestamp = Utils::getTimeSinceEpochNanos() / 1000; - packet.latitude = gps_output.gnss.geo_point.latitude; - packet.longitude = gps_output.gnss.geo_point.longitude; - packet.altitude = gps_output.gnss.geo_point.altitude; - - common_utils::Utils::log("Current LLA: " + gps_output.gnss.geo_point.to_string(), common_utils::Utils::kLogLevelInfo); - - packet.speedN = gps_output.gnss.velocity[0]; - packet.speedE = gps_output.gnss.velocity[1]; - packet.speedD = gps_output.gnss.velocity[2]; - - packet.xAccel = imu_output.linear_acceleration[0]; - packet.yAccel = imu_output.linear_acceleration[1]; - packet.zAccel = imu_output.linear_acceleration[2]; - - float yaw; - float pitch; - float roll; - VectorMath::toEulerianAngle(imu_output.orientation, pitch, roll, yaw); - packet.yawDeg = yaw * 180.0 / M_PI; - packet.pitchDeg = pitch * 180.0 / M_PI; - packet.rollDeg = roll * 180.0 / M_PI; - - Vector3r bodyRPY(roll, pitch, yaw); - - // In the Unreal world, yaw is rotation around Z, so this seems to be RPY, like PySim - Vector3r bodyVelocityRPY(imu_output.angular_velocity[0], imu_output.angular_velocity[1], imu_output.angular_velocity[2]); - Vector3r earthRPY = bodyAnglesToEarthAngles(bodyRPY, bodyVelocityRPY); - - packet.rollRate = earthRPY[0] * 180.0 / M_PI; - packet.pitchRate = earthRPY[1] * 180.0 / M_PI; - packet.yawRate = earthRPY[2] * 180.0 / M_PI; - - // Heading appears to be unused by AruPilot. But use yaw for now - packet.heading = yaw; - - packet.airspeed = std::sqrt( - packet.speedN * packet.speedN - + packet.speedE * packet.speedE - + packet.speedD * packet.speedD); - - packet.magic = 0x4c56414f; - - if (udpSocket_ != nullptr) - { - std::vector msg(sizeof(packet)); - memcpy(msg.data(), &packet, sizeof(packet)); - udpSocket_->sendMessage(msg); - } - } - } - - virtual void close() - { - MavLinkMultirotorApi::close(); - - if (udpSocket_ != nullptr) { - udpSocket_->close(); - udpSocket_->unsubscribe(rotorSubscriptionId_); - udpSocket_ = nullptr; - } - } + virtual ~ArduCopterSoloApi() + { + closeAllConnection(); + } + + virtual void update() + { + if (sensors_ == nullptr) + return; + + // send GPS and other sensor updates + const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps); + if (count_gps_sensors != 0) { + const auto& gps_output = getGpsData(""); + const auto& imu_output = getImuData(""); + + SensorMessage packet; + packet.timestamp = Utils::getTimeSinceEpochNanos() / 1000; + packet.latitude = gps_output.gnss.geo_point.latitude; + packet.longitude = gps_output.gnss.geo_point.longitude; + packet.altitude = gps_output.gnss.geo_point.altitude; + + common_utils::Utils::log("Current LLA: " + gps_output.gnss.geo_point.to_string(), common_utils::Utils::kLogLevelInfo); + + packet.speedN = gps_output.gnss.velocity[0]; + packet.speedE = gps_output.gnss.velocity[1]; + packet.speedD = gps_output.gnss.velocity[2]; + + packet.xAccel = imu_output.linear_acceleration[0]; + packet.yAccel = imu_output.linear_acceleration[1]; + packet.zAccel = imu_output.linear_acceleration[2]; + + float yaw; + float pitch; + float roll; + VectorMath::toEulerianAngle(imu_output.orientation, pitch, roll, yaw); + packet.yawDeg = yaw * 180.0 / M_PI; + packet.pitchDeg = pitch * 180.0 / M_PI; + packet.rollDeg = roll * 180.0 / M_PI; + + Vector3r bodyRPY(roll, pitch, yaw); + + // In the Unreal world, yaw is rotation around Z, so this seems to be RPY, like PySim + Vector3r bodyVelocityRPY(imu_output.angular_velocity[0], imu_output.angular_velocity[1], imu_output.angular_velocity[2]); + Vector3r earthRPY = bodyAnglesToEarthAngles(bodyRPY, bodyVelocityRPY); + + packet.rollRate = earthRPY[0] * 180.0 / M_PI; + packet.pitchRate = earthRPY[1] * 180.0 / M_PI; + packet.yawRate = earthRPY[2] * 180.0 / M_PI; + + // Heading appears to be unused by AruPilot. But use yaw for now + packet.heading = yaw; + + packet.airspeed = std::sqrt( + packet.speedN * packet.speedN + + packet.speedE * packet.speedE + + packet.speedD * packet.speedD); + + packet.magic = 0x4c56414f; + + if (udpSocket_ != nullptr) + { + std::vector msg(sizeof(packet)); + memcpy(msg.data(), &packet, sizeof(packet)); + udpSocket_->sendMessage(msg); + } + } + } + + virtual void close() + { + MavLinkMultirotorApi::close(); + + if (udpSocket_ != nullptr) { + udpSocket_->close(); + udpSocket_->unsubscribe(rotorSubscriptionId_); + udpSocket_ = nullptr; + } + } protected: - virtual void connect() - { - if (!is_simulation_mode_) { + virtual void connect() + { + if (!is_simulation_mode_) { - MavLinkMultirotorApi::connect(); - } - else { - const std::string& ip = connection_info_.ip_address; - int port = connection_info_.ip_port; + MavLinkMultirotorApi::connect(); + } + else { + const std::string& ip = connection_info_.udp_address; + int port = connection_info_.udp_port; - close(); + close(); - if (ip == "") { - throw std::invalid_argument("UdpIp setting is invalid."); - } + if (ip == "") { + throw std::invalid_argument("UdpIp setting is invalid."); + } - if (port == 0) { - throw std::invalid_argument("UdpPort setting has an invalid value."); - } + if (port == 0) { + throw std::invalid_argument("UdpPort setting has an invalid value."); + } - Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); - Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); - udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.sitl_ip_port); - mavlinkcom::AdHocMessageHandler handler = [this](std::shared_ptr connection, const std::vector &msg) { - this->rotorPowerMessageHandler(connection, msg); - }; + udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port); + mavlinkcom::AdHocMessageHandler handler = [this](std::shared_ptr connection, const std::vector &msg) { + this->rotorPowerMessageHandler(connection, msg); + }; - rotorSubscriptionId_ = udpSocket_->subscribe(handler); - } + rotorSubscriptionId_ = udpSocket_->subscribe(handler); + } } private: #ifdef __linux__ - struct __attribute__((__packed__)) SensorMessage { + struct __attribute__((__packed__)) SensorMessage { #else #pragma pack(push,1) - struct SensorMessage { + struct SensorMessage { #endif - // this is the packet sent by the simulator - // to the APM executable to update the simulator state - // All values are little-endian - uint64_t timestamp; - double latitude, longitude; // degrees - double altitude; // MSL - double heading; // degrees - double speedN, speedE, speedD; // m/s - double xAccel, yAccel, zAccel; // m/s/s in body frame - double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame - double rollDeg, pitchDeg, yawDeg; // euler angles, degrees - double airspeed; // m/s - uint32_t magic; // 0x4c56414f - }; + // this is the packet sent by the simulator + // to the APM executable to update the simulator state + // All values are little-endian + uint64_t timestamp; + double latitude, longitude; // degrees + double altitude; // MSL + double heading; // degrees + double speedN, speedE, speedD; // m/s + double xAccel, yAccel, zAccel; // m/s/s in body frame + double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame + double rollDeg, pitchDeg, yawDeg; // euler angles, degrees + double airspeed; // m/s + uint32_t magic; // 0x4c56414f + }; #ifndef __linux__ #pragma pack(pop) #endif - static const int kArduCopterRotorControlCount = 11; + static const int kArduCopterRotorControlCount = 11; - struct RotorControlMessage { - // ArduPilot Solo rotor control datagram format - uint16_t pwm[kArduCopterRotorControlCount]; - uint16_t speed, direction, turbulance; - }; + struct RotorControlMessage { + // ArduPilot Solo rotor control datagram format + uint16_t pwm[kArduCopterRotorControlCount]; + uint16_t speed, direction, turbulance; + }; - std::shared_ptr udpSocket_; - int rotorSubscriptionId_; + std::shared_ptr udpSocket_; + int rotorSubscriptionId_; - virtual void normalizeRotorControls() - { - // change 1000-2000 to 0-1. - for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { - rotor_controls_[i] = (rotor_controls_[i] - 1000.0f) / 1000.0f; - } - } + virtual void normalizeRotorControls() + { + // change 1000-2000 to 0-1. + for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { + rotor_controls_[i] = (rotor_controls_[i] - 1000.0f) / 1000.0f; + } + } - void rotorPowerMessageHandler(std::shared_ptr connection, const std::vector &msg) - { - if (msg.size() != sizeof(RotorControlMessage)) - { - Utils::log("Got rotor control message of size " + std::to_string(msg.size()) + " when we were expecting size " + std::to_string(sizeof(RotorControlMessage)), Utils::kLogLevelError); - return; - } + void rotorPowerMessageHandler(std::shared_ptr connection, const std::vector &msg) + { + if (msg.size() != sizeof(RotorControlMessage)) + { + Utils::log("Got rotor control message of size " + std::to_string(msg.size()) + " when we were expecting size " + std::to_string(sizeof(RotorControlMessage)), Utils::kLogLevelError); + return; + } - RotorControlMessage rotorControlMessage; - memcpy(&rotorControlMessage, msg.data(), sizeof(RotorControlMessage)); + RotorControlMessage rotorControlMessage; + memcpy(&rotorControlMessage, msg.data(), sizeof(RotorControlMessage)); - std::lock_guard guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl + std::lock_guard guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl - for (auto i = 0; i < RotorControlsCount && i < kArduCopterRotorControlCount; ++i) { - rotor_controls_[i] = rotorControlMessage.pwm[i]; - } + for (auto i = 0; i < RotorControlsCount && i < kArduCopterRotorControlCount; ++i) { + rotor_controls_[i] = rotorControlMessage.pwm[i]; + } - normalizeRotorControls(); - } + normalizeRotorControls(); + } - Vector3r bodyAnglesToEarthAngles(Vector3r bodyRPY, Vector3r bodyVelocityRPY) - { - float p = bodyVelocityRPY[0]; - float q = bodyVelocityRPY[1]; - float r = bodyVelocityRPY[2]; + Vector3r bodyAnglesToEarthAngles(Vector3r bodyRPY, Vector3r bodyVelocityRPY) + { + float p = bodyVelocityRPY[0]; + float q = bodyVelocityRPY[1]; + float r = bodyVelocityRPY[2]; - // Roll, pitch, yaw - float phi = bodyRPY[0]; - float theta = bodyRPY[1]; + // Roll, pitch, yaw + float phi = bodyRPY[0]; + float theta = bodyRPY[1]; - float phiDot = p + tan(theta)*(q*sin(phi) + r * cos(phi)); + float phiDot = p + tan(theta)*(q*sin(phi) + r * cos(phi)); - float thetaDot = q * cos(phi) - r * sin(phi); - if (fabs(cos(theta)) < 1.0e-20) - { - theta += 1.0e-10f; - } + float thetaDot = q * cos(phi) - r * sin(phi); + if (fabs(cos(theta)) < 1.0e-20) + { + theta += 1.0e-10f; + } - float psiDot = (q*sin(phi) + r * cos(phi)) / cos(theta); + float psiDot = (q*sin(phi) + r * cos(phi)) / cos(theta); - return Vector3r(phiDot, thetaDot, psiDot); - } + return Vector3r(phiDot, thetaDot, psiDot); + } }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index 73273359f..d13185d94 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -16,8 +16,11 @@ #include #include #include +#include +#include #include "common/Common.hpp" +#include "common/common_utils/SmoothingFilter.hpp" #include "common/common_utils/Timer.hpp" #include "common/CommonStructs.hpp" #include "common/VectorMath.hpp" @@ -35,13 +38,16 @@ namespace msr { namespace airlib { - class MavLinkMultirotorApi : public MultirotorApiBase { public: //methods virtual ~MavLinkMultirotorApi() { closeAllConnection(); + if (this->connect_thread_.joinable()) + { + this->connect_thread_.join(); + } } //non-base interface specific to MavLinKDroneController @@ -85,57 +91,68 @@ class MavLinkMultirotorApi : public MultirotorApiBase //update sensors in PX4 stack virtual void update() override { - MultirotorApiBase::update(); + try { + MultirotorApiBase::update(); + + if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_) + return; + + if (send_params_) { + send_params_ = false; + sendParams(); + } + + //send sensor updates + const auto& imu_output = getImuData(""); + const auto& mag_output = getMagnetometerData(""); + const auto& baro_output = getBarometerData(""); + + sendHILSensor(imu_output.linear_acceleration, + imu_output.angular_velocity, + mag_output.magnetic_field_body, + baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude); - if (sensors_ == nullptr || connection_ == nullptr || !connection_->isOpen()) - 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 Millibar */, 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); + const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); + if (count_distance_sensors != 0) { + const auto& distance_output = getDistanceSensorData(""); + + 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? + distance_output.relative_pose.orientation); //TODO: convert from radians to degrees? } + + const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps); + if (count_gps_sensors != 0) { + const auto& gps_output = getGpsData(""); + + //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); + } + } + } + catch (std::exception& e) { + addStatusMessage("Exception sending messages to vehicle"); + addStatusMessage(e.what()); + disconnect(); + connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on. } //must be done at the end @@ -145,11 +162,17 @@ class MavLinkMultirotorApi : public MultirotorApiBase virtual bool isReady(std::string& message) const override { - if (!is_ready_) + if (!is_ready_ && is_ready_message_.size() > 0) { message = is_ready_message_; + } return is_ready_; } + virtual bool canArm() const override + { + return is_ready_ && has_gps_lock_; + } + //TODO: this method can't be const yet because it clears previous messages virtual void getStatusMessages(std::vector& messages) override { @@ -175,7 +198,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase 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); + state.accelerations.linear = 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; } @@ -202,7 +225,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase { updateState(); return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z); - } + } virtual Vector3r getVelocity() const override { updateState(); @@ -240,20 +263,57 @@ class MavLinkMultirotorApi : public MultirotorApiBase checkValidVehicle(); bool rc = false; + if (arm) { + float timeout_sec = 10; + waitForHomeLocation(timeout_sec); + waitForStableGroundPosition(timeout_sec); + } + mav_vehicle_->armDisarm(arm).wait(10000, &rc); return rc; } + void waitForHomeLocation(float timeout_sec) + { + if (!current_state_.home.is_set) { + addStatusMessage("Waiting for valid GPS home location..."); + if (!waitForFunction([&]() { + return current_state_.home.is_set; + }, timeout_sec).isComplete()) { + throw VehicleMoveException("Vehicle does not have a valid GPS home location"); + } + } + } + + void waitForStableGroundPosition(float timeout_sec) + { + // wait for ground stabilization + if (ground_variance_ > GroundTolerance) { + addStatusMessage("Waiting for z-position to stabilize..."); + if (!waitForFunction([&]() { + return ground_variance_ <= GroundTolerance; + }, timeout_sec).isComplete()) + { + auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_); + throw VehicleMoveException(msg); + } + } + } + virtual bool takeoff(float timeout_sec) override { SingleCall lock(this); checkValidVehicle(); + + waitForHomeLocation(timeout_sec); + waitForStableGroundPosition(timeout_sec); + bool rc = false; auto vec = getPosition(); + auto yaw = current_state_.attitude.yaw; float z = vec.z() + getTakeoffZ(); - if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, 0.0f /* yaw */).wait(static_cast(timeout_sec * 1000), &rc)) - { + if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast(timeout_sec * 1000), &rc)) { throw VehicleMoveException("TakeOff command - timeout waiting for response"); } if (!rc) { @@ -273,8 +333,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase //we assume the ground is relatively flat an we are landing roughly at the home altitude. updateState(); checkValidVehicle(); - if (current_state_.home.is_set) - { + 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)) { @@ -284,15 +343,14 @@ class MavLinkMultirotorApi : public MultirotorApiBase throw VehicleMoveException("Landing command rejected by drone"); } } - else - { + 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..."); } const auto& waiter = waitForFunction([&]() { updateState(); return current_state_.controls.landed; - }, timeout_sec); + }, timeout_sec); // Wait for landed state (or user cancellation) if (!waiter.isComplete()) @@ -323,7 +381,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase checkValidVehicle(); mavlinkcom::AsyncResult result = mav_vehicle_->loiter(); //auto start_time = std::chrono::system_clock::now(); - while (! getCancelToken().isCancelled()) + while (!getCancelToken().isCancelled()) { if (result.wait(100, &rc)) { @@ -370,25 +428,25 @@ class MavLinkMultirotorApi : public MultirotorApiBase // listen to the other mavlink connection also auto mavcon = mav_vehicle_->getConnection(); if (mavcon != connection_) { - mavlinkcom::MavLinkTelemetry sitl; - mavcon->getTelemetry(sitl); + mavlinkcom::MavLinkTelemetry gcs; + mavcon->getTelemetry(gcs); - data.handlerMicroseconds += sitl.handlerMicroseconds; - data.messagesHandled += sitl.messagesHandled; - data.messagesReceived += sitl.messagesReceived; - data.messagesSent += sitl.messagesSent; + data.handlerMicroseconds += gcs.handlerMicroseconds; + data.messagesHandled += gcs.messagesHandled; + data.messagesReceived += gcs.messagesReceived; + data.messagesSent += gcs.messagesSent; - if (sitl.messagesReceived == 0) + if (gcs.messagesReceived == 0) { - if (!sitl_message_timer_.started()) { - sitl_message_timer_.start(); + if (!gcs_message_timer_.started()) { + gcs_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 if (gcs_message_timer_.seconds() > messageReceivedTimeout) { + addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again"); } } else { - sitl_message_timer_.stop(); + gcs_message_timer_.stop(); } } @@ -402,7 +460,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase } virtual float getTakeoffZ() const override { - // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe + // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe // enough to get out of the backwash turbulence. Negative due to NED coordinate system. return -3.0f; } @@ -412,7 +470,25 @@ class MavLinkMultirotorApi : public MultirotorApiBase } protected: //methods - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) override + { + unused(controllerType); + unused(kp); + unused(ki); + unused(kd); + Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo); + } + + virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override + { + unused(front_right_pwm); + unused(front_left_pwm); + unused(rear_right_pwm); + unused(rear_left_pwm); + Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override { if (target_height_ != -z) { // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best @@ -426,11 +502,44 @@ class MavLinkMultirotorApi : public MultirotorApiBase float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust); } - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override { + if (target_height_ != -z) { + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } checkValidVehicle(); - mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust); } + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + checkValidVehicle(); + mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle); + } + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override + { + checkValidVehicle(); + mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle); + } + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override + { + if (target_height_ != -z) { + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkValidVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust); + } + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + checkValidVehicle(); + mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle); + } + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override { checkValidVehicle(); @@ -467,7 +576,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase endOffboardMode(); } -public: +public: class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog { @@ -486,92 +595,91 @@ class MavLinkMultirotorApi : public MultirotorApiBase std::shared_ptr proxy_; }; - + protected: //methods - - virtual void connect() - { - createMavConnection(connection_info_); - initializeMavSubscriptions(); - } + virtual void connect() + { + if (!connecting_) { + connecting_ = true; + if (this->connect_thread_.joinable()) + { + this->connect_thread_.join(); + } + this->connect_thread_ = std::thread(&MavLinkMultirotorApi::connect_thread, this); + } + } - virtual void close() - { - if (connection_ != nullptr) { - if (is_hil_mode_set_ && mav_vehicle_ != nullptr) { - setNormalMode(); - } + virtual void disconnect() { + addStatusMessage("Disconnecting mavlink vehicle"); + connected_ = false; + if (connection_ != nullptr) { + if (is_hil_mode_set_ && mav_vehicle_ != nullptr) { + setNormalMode(); + } - connection_->close(); - } + connection_->close(); + } - if (hil_node_ != nullptr) - hil_node_->close(); + if (hil_node_ != nullptr) { + hil_node_->close(); + } - if (video_server_ != nullptr) - video_server_->close(); + if (mav_vehicle_ != nullptr) { + mav_vehicle_->getConnection()->stopLoggingSendMessage(); + mav_vehicle_->close(); + mav_vehicle_ = nullptr; + } - if (logviewer_proxy_ != nullptr) { - logviewer_proxy_->close(); - logviewer_proxy_ = nullptr; - } + if (video_server_ != nullptr) + video_server_->close(); - if (logviewer_out_proxy_ != nullptr) { - if (mav_vehicle_ != nullptr) { - mav_vehicle_->getConnection()->stopLoggingSendMessage(); - } - logviewer_out_proxy_->close(); - logviewer_out_proxy_ = nullptr; - } + if (logviewer_proxy_ != nullptr) { + logviewer_proxy_->close(); + logviewer_proxy_ = nullptr; + } - if (qgc_proxy_ != nullptr) { - qgc_proxy_->close(); - qgc_proxy_ = nullptr; - } - if (mav_vehicle_ != nullptr) { - mav_vehicle_->close(); - mav_vehicle_ = nullptr; - } - } + if (logviewer_out_proxy_ != nullptr) { + logviewer_out_proxy_->close(); + logviewer_out_proxy_ = nullptr; + } - const ImuBase* getImu() const - { - return static_cast(sensors_->getByType(SensorBase::SensorType::Imu)); - } - const MagnetometerBase* getMagnetometer() const - { - return static_cast(sensors_->getByType(SensorBase::SensorType::Magnetometer)); + if (qgc_proxy_ != nullptr) { + qgc_proxy_->close(); + qgc_proxy_ = nullptr; + } } - const BarometerBase* getBarometer() const + + void connect_thread() { - return static_cast(sensors_->getByType(SensorBase::SensorType::Barometer)); + addStatusMessage("Waiting for mavlink vehicle..."); + createMavConnection(connection_info_); + if (mav_vehicle_ != nullptr) { + connectToLogViewer(); + connectToQGC(); + } + connecting_ = false; + connected_ = true; } - const DistanceBase* getDistance() const + + virtual void close() { - return static_cast(sensors_->getByType(SensorBase::SensorType::Distance)); + disconnect(); } - const GpsBase* getGps() const + + void closeAllConnection() { - return static_cast(sensors_->getByType(SensorBase::SensorType::Gps)); + close(); } - void closeAllConnection() - { - close(); - } - private: //methods - void openAllConnections() + 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 getMocapPose(Vector3r& position, Quaternionr& orientation) const @@ -617,7 +725,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase { if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) { - // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode + // 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); @@ -640,7 +748,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase checkValidVehicle(); - // add MAV_MODE_FLAG_HIL_ENABLED flag to current mode + // 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); @@ -687,8 +795,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase } void checkValidVehicle() { - if (mav_vehicle_ == nullptr) { - throw std::logic_error("Cannot perform operation when no vehicle is connected"); + if (mav_vehicle_ == nullptr || connection_ == nullptr || !connection_->isOpen() || !connected_) { + throw std::logic_error("Cannot perform operation when no vehicle is connected or vehicle is not responding"); } } @@ -717,7 +825,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase } } else { - //this applies to PX4 and may work differently on other firmwares. + //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); @@ -725,31 +833,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase } } - 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); - }); - } - } - } - bool sendTestMessage(std::shared_ptr node) { try { // try and send a test message. @@ -810,7 +893,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) { unused(connection_val); processQgcMessages(msg); - }); + }); } } return qgc_proxy_ != nullptr; @@ -843,21 +926,24 @@ class MavLinkMultirotorApi : public MultirotorApiBase static std::string findPX4() { auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0); - for (auto iter = result.begin(); iter != result.end(); iter++) - { + for (auto iter = result.begin(); iter != result.end(); iter++) { mavlinkcom::SerialPortInfo info = *iter; - if ( - ( + if (( (info.vid == pixhawkVendorId) && - (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId) - ) || - ( - (info.displayName.find(L"PX4_") != std::string::npos) - ) - ) - { + (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()); + + std::string portName_str; + + for (wchar_t ch : info.portName) + { + portName_str.push_back(static_cast(ch)); + } + return portName_str; } } return ""; @@ -869,81 +955,176 @@ class MavLinkMultirotorApi : public MultirotorApiBase createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate); } else { - createMavUdpConnection(connection_info.ip_address, connection_info.ip_port); + createMavEthernetConnection(connection_info); } + //Uncomment below for sending images over MavLink //connectToVideoServer(); } - void createMavUdpConnection(const std::string& ip, int port) + void createMavEthernetConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info) { close(); - if (ip == "") { - throw std::invalid_argument("UdpIp setting is invalid."); + got_first_heartbeat_ = false; + is_hil_mode_set_ = false; + is_armed_ = false; + has_home_ = false; + is_controls_0_1_ = true; + Utils::setValue(rotor_controls_, 0.0f); + + if (connection_info.use_tcp) { + if (connection_info.tcp_port == 0) { + throw std::invalid_argument("TcpPort setting has an invalid value."); + } + + auto msg = Utils::stringf("Waiting for TCP connection on port %d, local IP %s", connection_info.tcp_port, connection_info_.local_host_ip.c_str()); + addStatusMessage(msg); + try { + connection_ = std::make_shared(); + connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port); + } + catch (std::exception& e) { + addStatusMessage("Accepting TCP socket failed, is another instance running?"); + addStatusMessage(e.what()); + return; + } } + else if (connection_info.udp_address.size() > 0) { + if (connection_info.udp_port == 0) { + throw std::invalid_argument("UdpPort setting has an invalid value."); + } - if (port == 0) { - throw std::invalid_argument("UdpPort setting has an invalid value."); + connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, connection_info.udp_address, connection_info.udp_port); + } + else { + throw std::invalid_argument("Please provide valid connection info for your drone."); } - 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); + // start listening to the SITL connection. + connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processMavMessages(msg); + }); + hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); hil_node_->connect(connection_); - addStatusMessage(std::string("Connected over UDP.")); + + if (connection_info.use_tcp) { + addStatusMessage(std::string("Connected to SITL over TCP.")); + } + else { + addStatusMessage(std::string("Connected to SITL over UDP.")); + } 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())); + if (connection_info_.control_ip_address != "") { + if (connection_info_.control_port == 0) { + throw std::invalid_argument("ControlPort setting has an invalid value."); + } - 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); + // The PX4 SITL mode app cannot receive commands to control the drone over the same HIL mavlink connection. + // The HIL mavlink connection can only handle HIL_SENSOR messages. This separate channel is needed for + // everything else. + addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP...", + connection_info_.control_port, connection_info_.local_host_ip.c_str(), connection_info_.control_ip_address.c_str())); - addStatusMessage(std::string("Connected to SITL over UDP.")); + // if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ? + std::this_thread::sleep_for(std::chrono::seconds(2)); + + auto gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs", + connection_info_.local_host_ip, connection_info_.control_ip_address, connection_info_.control_port); + mav_vehicle_->connect(gcsConnection); + + addStatusMessage(std::string("Ground control connected over UDP.")); + } + + connectVehicle(); + } + + void connectVehicle() + { + // listen to this UDP 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); + }); } else { mav_vehicle_->connect(connection_); } + connected_ = true; + // now we can start our heartbeats. mav_vehicle_->startHeartbeat(); + + // Also request home position messages + mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1); } void createMavSerialConnection(const std::string& port_name, int baud_rate) { close(); + bool reported = false; std::string port_name_auto = port_name; - if (port_name_auto == "" || port_name_auto == "*") { + while (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 (!reported) { + reported = true; + addStatusMessage("Could not detect a connected PX4 flight controller on any USB ports."); + addStatusMessage("You can specify USB port in settings.json."); + } + std::this_thread::sleep_for(std::chrono::seconds(1)); } } if (port_name_auto == "") { - throw std::invalid_argument("USB port for PX4 flight controller is empty. Please set it in settings.json."); + addStatusMessage("USB port for PX4 flight controller is empty. Please set it in settings.json."); + return; } if (baud_rate == 0) { - throw std::invalid_argument("Baud rate specified in settings.json is 0 which is invalid"); + addStatusMessage("Baud rate specified in settings.json is 0 which is invalid"); + return; } 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 Mo-cap 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."); + reported = false; + + while (true) { + try { + 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 Mo-cap messages + hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + hil_node_->connect(connection_); + addStatusMessage(Utils::stringf("Connected to PX4 over serial port: %s", port_name_auto.c_str())); + + 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(); + // start listening to the HITL connection. + connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processMavMessages(msg); + }); - 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(); + return; + } + catch (std::exception& e) { + if (!reported) { + reported = true; + addStatusMessage("Error connecting to mavlink vehicle."); + addStatusMessage(e.what()); + addStatusMessage("Please check your USB port in settings.json."); + } + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } } mavlinkcom::MavLinkHilSensor getLastSensorMessage() @@ -964,6 +1145,25 @@ class MavLinkMultirotorApi : public MultirotorApiBase return last_gps_message_; } + void sendParams() + { + // send any mavlink parameters from settings.json through to the connected vehicle. + if (connection_info_.params.size() > 0) { + for (auto iter : connection_info_.params) { + auto key = iter.first; + auto value = iter.second; + mavlinkcom::MavLinkParameter p; + p.name = key; + p.value = value; + bool result = false; + mav_vehicle_->setParameter(p).wait(1000, &result); + if (!result) { + Utils::log(Utils::stringf("Failed to set mavlink parameter '%s'", key.c_str())); + } + } + } + } + void setArmed(bool armed) { is_armed_ = armed; @@ -987,11 +1187,14 @@ class MavLinkMultirotorApi : public MultirotorApiBase void addStatusMessage(const std::string& message) { - 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); + if (message.size() != 0) { + Utils::log(message); + 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); + } } void processMavMessages(const mavlinkcom::MavLinkMessage& msg) @@ -999,18 +1202,22 @@ class MavLinkMultirotorApi : public MultirotorApiBase if (msg.msgid == HeartbeatMessage.msgid) { std::lock_guard guard_heartbeat(heartbeat_mutex_); - //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 (!got_first_heartbeat_) { + Utils::log("received first heartbeat"); + + got_first_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 output to 0 to 1. is_controls_0_1_ = false; } + + send_params_ = true; } else if (is_simulation_mode_ && !is_hil_mode_set_) { setHILMode(); @@ -1035,7 +1242,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase 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; @@ -1046,6 +1252,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase rotor_controls_[7] = HilControlsMessage.aux4; normalizeRotorControls(); + received_actuator_controls_ = true; } } else if (msg.msgid == HilActuatorControlsMessage.msgid) { @@ -1054,11 +1261,57 @@ class MavLinkMultirotorApi : public MultirotorApiBase 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? + bool isarmed = (HilActuatorControlsMessage.mode & 128) != 0; for (auto i = 0; i < 8; ++i) { - rotor_controls_[i] = HilActuatorControlsMessage.controls[i]; + if (isarmed) { + rotor_controls_[i] = HilActuatorControlsMessage.controls[i]; + } + else { + rotor_controls_[i] = 0; + } + } + if (isarmed) { + normalizeRotorControls(); } - normalizeRotorControls(); + received_actuator_controls_ = true; + // if the timestamps match then it means we are in lockstep mode. + if (!lock_step_enabled_) { + // && (HilActuatorControlsMessage.flags & 0x1)) // todo: enable this check when this flag is widely available... + if (hil_sensor_clock_ == HilActuatorControlsMessage.time_usec) { + addStatusMessage("Enabling lockstep mode"); + lock_step_enabled_ = true; + } + } + } + else if (msg.msgid == MavLinkGpsRawInt.msgid) { + MavLinkGpsRawInt.decode(msg); + auto fix_type = static_cast(MavLinkGpsRawInt.fix_type); + auto locked = (fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_GPS && + fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_FIX); + if (locked && !has_gps_lock_) { + addStatusMessage("Got GPS lock"); + has_gps_lock_ = true; + } + if (!has_home_ && current_state_.home.is_set) { + addStatusMessage("Got GPS Home Location"); + has_home_ = true; + } + + } + else if (msg.msgid == mavlinkcom::MavLinkLocalPositionNed::kMessageId) { + // we are getting position information... so we can use this to check the stability of the z coordinate before takeoff. + if (current_state_.controls.landed) + { + monitorGroundAltitude(); + } + } + else if (msg.msgid == mavlinkcom::MavLinkExtendedSysState::kMessageId) { + // check landed state. + getLandedState(); + } + else if (msg.msgid == mavlinkcom::MavLinkHomePosition::kMessageId) { + mavlinkcom::MavLinkHomePosition home; + home.decode(msg); } //else ignore message } @@ -1068,34 +1321,63 @@ class MavLinkMultirotorApi : public MultirotorApiBase if (!is_simulation_mode_) throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode"); + auto now = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0); + if (lock_step_enabled_) { + if (last_hil_sensor_time_ + 100000 < now) { + // if 100 ms passes then something is terribly wrong, reset lockstep mode + lock_step_enabled_ = false; + addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode"); + } + + if (!received_actuator_controls_) { + // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage. + return; + } + } + + hil_sensor_clock_ = now; + mavlinkcom::MavLinkHilSensor hil_sensor; - hil_sensor.time_usec = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0); + last_hil_sensor_time_ = now; + hil_sensor.time_usec = hil_sensor_clock_; hil_sensor.xacc = acceleration.x(); hil_sensor.yacc = acceleration.y(); hil_sensor.zacc = acceleration.z(); + hil_sensor.fields_updated = 0b111; // Set accel bit fields + hil_sensor.xgyro = gyro.x(); hil_sensor.ygyro = gyro.y(); hil_sensor.zgyro = gyro.z(); + hil_sensor.fields_updated |= 0b111000; // Set gyro bit fields + hil_sensor.xmag = mag.x(); hil_sensor.ymag = mag.y(); hil_sensor.zmag = mag.z(); + hil_sensor.fields_updated |= 0b111000000; // Set mag bit fields + hil_sensor.abs_pressure = abs_pressure; hil_sensor.pressure_alt = pressure_alt; + + hil_sensor.fields_updated |= 0b1101000000000; // Set baro bit fields + //TODO: enable temperature? diff_pressure - hil_sensor.fields_updated = was_reset_ ? (1 << 31) : 0; + if (was_reset_) { + hil_sensor.fields_updated = static_cast(1 << 31); + } if (hil_node_ != nullptr) { hil_node_->sendMessage(hil_sensor); + received_actuator_controls_ = false; } std::lock_guard guard(last_message_mutex_); last_sensor_message_ = hil_sensor; } - void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, float orientation) + void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, Quaternionr orientation) { if (!is_simulation_mode_) throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); @@ -1108,7 +1390,14 @@ class MavLinkMultirotorApi : public MultirotorApiBase 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); + + // Use custom orientation + distance_sensor.orientation = 100; // MAV_SENSOR_ROTATION_CUSTOM + distance_sensor.quaternion[0] = orientation.w(); + distance_sensor.quaternion[1] = orientation.x(); + distance_sensor.quaternion[2] = orientation.y(); + distance_sensor.quaternion[3] = orientation.z(); + //TODO: use covariance parameter? if (hil_node_ != nullptr) { @@ -1126,9 +1415,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase 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.time_usec = hil_sensor_clock_; hil_gps.lat = static_cast(geo_point.latitude * 1E7); - hil_gps.lon = static_cast(geo_point.longitude* 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); @@ -1138,7 +1427,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase 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); + hil_gps.satellites_visible = static_cast(15); if (hil_node_ != nullptr) { hil_node_->sendMessage(hil_gps); @@ -1156,7 +1445,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase void resetState() { //reset state - is_any_heartbeat_ = is_hil_mode_set_ = is_armed_ = false; + is_hil_mode_set_ = false; is_controls_0_1_ = true; hil_state_freq_ = -1; actuators_message_supported_ = false; @@ -1168,7 +1457,25 @@ class MavLinkMultirotorApi : public MultirotorApiBase thrust_controller_ = PidController(); Utils::setValue(rotor_controls_, 0.0f); was_reset_ = false; + received_actuator_controls_ = false; + lock_step_enabled_ = false; + has_gps_lock_ = false; + send_params_ = false; mocap_pose_ = Pose::nanPose(); + ground_variance_ = 1; + ground_filter_.initialize(25, 0.1f); + cancelLastTask(); + } + + void monitorGroundAltitude() + { + // used to ensure stable altitude before takeoff. + auto position = getPosition(); + auto result = ground_filter_.filter(position.z()); + auto variance = std::get<1>(result); + if (variance >= 0) { // filter returns -1 if we don't have enough data yet. + ground_variance_ = variance; + } } @@ -1176,13 +1483,13 @@ class MavLinkMultirotorApi : public MultirotorApiBase //TODO: below was made protected from private to support Ardupilot //implementation but we need to review this and avoid having protected variables - static const int RotorControlsCount = 8; + static const int RotorControlsCount = 8; - const SensorCollection* sensors_; - mutable std::mutex hil_controls_mutex_; - AirSimSettings::MavLinkConnectionInfo connection_info_; - float rotor_controls_[RotorControlsCount]; - bool is_simulation_mode_; + const SensorCollection* sensors_; + mutable std::mutex hil_controls_mutex_; + AirSimSettings::MavLinkConnectionInfo connection_info_; + float rotor_controls_[RotorControlsCount]; + bool is_simulation_mode_; private: //variables @@ -1191,7 +1498,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase 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 messageReceivedTimeout = 10; ///< Seconds + static const int messageReceivedTimeout = 10; ///< Seconds std::shared_ptr logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_; @@ -1208,7 +1515,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase mavlinkcom::MavLinkStatustext StatusTextMessage; mavlinkcom::MavLinkHilControls HilControlsMessage; mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage; + mavlinkcom::MavLinkGpsRawInt MavLinkGpsRawInt; mavlinkcom::MavLinkCommandLong CommandLongMessage; + mavlinkcom::MavLinkLocalPositionNed MavLinkLocalPositionNed; mavlinkcom::MavLinkHilSensor last_sensor_message_; mavlinkcom::MavLinkDistanceSensor last_distance_message_; @@ -1217,16 +1526,29 @@ class MavLinkMultirotorApi : public MultirotorApiBase std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_; //variables required for VehicleApiBase implementation - bool is_any_heartbeat_, is_hil_mode_set_, is_armed_; + bool got_first_heartbeat_, is_hil_mode_set_, is_armed_; bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1? + bool send_params_ = false; std::queue status_messages_; int hil_state_freq_; - bool actuators_message_supported_; - uint64_t last_gps_time_; - bool was_reset_; - bool is_ready_; + bool actuators_message_supported_ = false; + uint64_t last_gps_time_ = 0; + uint64_t last_hil_sensor_time_ = 0; + uint64_t hil_sensor_clock_ = 0; + bool was_reset_ = false; + bool has_home_ = false; + bool is_ready_ = false; + bool has_gps_lock_ = false; + bool lock_step_enabled_ = false; + bool received_actuator_controls_ = false; std::string is_ready_message_; Pose mocap_pose_; + std::thread connect_thread_; + bool connecting_ = false; + bool connected_ = false; + common_utils::SmoothingFilter ground_filter_; + double ground_variance_ = 1; + const double GroundTolerance = 0.1; //additional variables required for MultirotorApiBase implementation //this is optional for methods that might not use vehicle commands @@ -1235,14 +1557,13 @@ class MavLinkMultirotorApi : public MultirotorApiBase bool is_api_control_enabled_; PidController thrust_controller_; common_utils::Timer hil_message_timer_; - common_utils::Timer sitl_message_timer_; + common_utils::Timer gcs_message_timer_; //every time we return status update, we need to check if we have new data //this is why below two variables are marked as mutable mutable int state_version_; mutable mavlinkcom::VehicleState current_state_; }; - - -}} //namespace +} +} //namespace #endif diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp index 836f8943b..6044e5e18 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp @@ -57,192 +57,6 @@ class Px4MultiRotorParams : public MultiRotorParams { } private: - void setupFrameGenericQuad(Params& params) - { - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.2275f); - - //set up mass - params.mass = 1.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust - // given new thrust coefficients, motor max_rpm and propeller diameter. - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - void setupFrameGenericHex(Params& params) - { - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 6; - std::vector arm_lengths(params.rotor_count, 0.2275f); - - //set up mass - params.mass = 1.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust - // given new thrust coefficients, motor max_rpm and propeller diameter. - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - initializeRotorHexX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - void setupFrameFlamewheel(Params& params) - { - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.225f); - - //set up mass - params.mass = 1.635f; - real_T motor_assembly_weight = 0.052f; - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - params.rotor_params.C_T = 0.11f; - params.rotor_params.C_P = 0.047f; - params.rotor_params.max_rpm = 9500; - params.rotor_params.calculateMaxThrust(); - params.linear_drag_coefficient *= 4; // make top speed more real. - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f; - real_T rotor_z = 0.15f; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - void setupFrameFlamewheelFLA(Params& params) - { - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.225f); - - //set up mass - params.mass = 2.25f; - real_T motor_assembly_weight = 0.1f; - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - params.rotor_params.C_T = 0.2f; - params.rotor_params.C_P = 0.1f; - params.rotor_params.max_rpm = 9324; - params.rotor_params.calculateMaxThrust(); - params.linear_drag_coefficient *= 4; // make top speed more real. - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f; - real_T rotor_z = 0.15f; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - void setupFrameBlacksheep(Params& params) - { - /* - Motor placement: - x - (2) | (0) - | - ------------ y - | - (1) | (3) - | - - */ - //set up arm lengths - //dimensions are for Team Blacksheep Discovery (http://team-blacksheep.com/products/product:98) - params.rotor_count = 4; - std::vector arm_lengths; - - Vector3r unit_z(0, 0, -1); //NED frame - - // relative to Forward vector in the order (0,3,1,2) required by quad X pattern - // http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg - arm_lengths.push_back(0.22f); - arm_lengths.push_back(0.255f); - arm_lengths.push_back(0.22f); - arm_lengths.push_back(0.255f); - - // note: the Forward vector is actually the "x" axis, and the AngleAxisr rotation is pointing down and is left handed, so this means the rotation - // is counter clockwise, so the vector (arm_lengths[i], 0) is the X-axis, so the CCW rotations to position each arm correctly are listed below: - // See measurements here: http://diydrones.com/profiles/blogs/arducopter-tbs-discovery-style (angles reversed because we are doing CCW rotation) - std::vector arm_angles; - arm_angles.push_back(-55.0f); - arm_angles.push_back(125.0f); - arm_angles.push_back(55.0f); - arm_angles.push_back(-125.0f); - - // quad X pattern - std::vector rotor_directions; - rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); - rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); - rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); - rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); - - // data from - // http://dronesvision.net/team-blacksheep-750kv-motor-esc-set-for-tbs-discovery-fpv-quadcopter/ - //set up mass - params.mass = 2.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.052f; // weight for TBS motors - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // the props we are using a E-Prop, which I didn't find in UIUC database, but this one is close: - // http://m-selig.ae.illinois.edu/props/volume-2/plots/ef_130x70_static_ctcp.png - params.rotor_params.C_T = 0.11f; - params.rotor_params.C_P = 0.047f; - params.rotor_params.max_rpm = 9500; - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.20f; params.body_box.y() = 0.12f; params.body_box.z() = 0.04f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - params.rotor_poses.clear(); - for (uint i = 0; i < 4; i++) - { - Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z)); - params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]); - }; - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - static const AirSimSettings::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting) { return vehicle_setting.connection_info; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 71dfa9910..a6a67dd52 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -137,7 +137,7 @@ class SimpleFlightApi : public MultirotorApiBase { return true; } -protected: +protected: virtual Kinematics::State getKinematicsEstimated() const override { return AirSimSimpleFlightCommon::toKinematicsState3r(firmware_->offboardApi(). @@ -185,7 +185,7 @@ class SimpleFlightApi : public MultirotorApiBase { virtual float getTakeoffZ() const override { - // pick a number, 3 meters is probably safe + // pick a number, 3 meters is probably safe // enough to get out of the backwash turbulence. Negative due to NED coordinate system. return params_.takeoff.takeoff_z; } @@ -195,22 +195,22 @@ class SimpleFlightApi : public MultirotorApiBase { return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled } - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + virtual void commandMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm) override { - //Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate)); + //Utils::log(Utils::stringf("commandMotorPWMs %f, %f, %f, %f", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm)); typedef simple_flight::GoalModeType GoalModeType; - simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); + simple_flight::GoalMode mode(GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough); - simple_flight::Axis4r goal(roll, pitch, yaw_rate, throttle); + simple_flight::Axis4r goal(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); std::string message; firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override { - //Utils::log(Utils::stringf("commandRollPitchZ %f, %f, %f, %f", pitch, roll, z, yaw)); + //Utils::log(Utils::stringf("commandRollPitchYawZ %f, %f, %f, %f", pitch, roll, z, yaw)); typedef simple_flight::GoalModeType GoalModeType; simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld); @@ -221,13 +221,78 @@ class SimpleFlightApi : public MultirotorApiBase { firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll, pitch, yaw, throttle); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll, pitch, yaw_rate, throttle); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::PositionWorld); + + simple_flight::Axis4r goal(roll, pitch, yaw_rate, z); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::PositionWorld); + + simple_flight::Axis4r goal(roll_rate, pitch_rate, yaw_rate, z); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll_rate, pitch_rate, yaw_rate, throttle); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override { //Utils::log(Utils::stringf("commandVelocity %f, %f, %f, %f", vx, vy, vz, yaw_mode.yaw_or_rate)); typedef simple_flight::GoalModeType GoalModeType; - simple_flight::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, - yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, + simple_flight::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, + yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, GoalModeType::VelocityWorld); simple_flight::Axis4r goal(vy, vx, Utils::degreesToRadians(yaw_mode.yaw_or_rate), vz); @@ -241,8 +306,8 @@ class SimpleFlightApi : public MultirotorApiBase { //Utils::log(Utils::stringf("commandVelocityZ %f, %f, %f, %f", vx, vy, z, yaw_mode.yaw_or_rate)); typedef simple_flight::GoalModeType GoalModeType; - simple_flight::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, - yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, + simple_flight::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, + yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, GoalModeType::PositionWorld); simple_flight::Axis4r goal(vy, vx, Utils::degreesToRadians(yaw_mode.yaw_or_rate), z); @@ -251,13 +316,65 @@ class SimpleFlightApi : public MultirotorApiBase { firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } + virtual void setControllerGains(uint8_t controller_type, const vector& kp, const vector& ki, const vector& kd) override + { + simple_flight::GoalModeType controller_type_enum = static_cast(controller_type); + + vector kp_axis4(4); + vector ki_axis4(4); + vector kd_axis4(4); + + switch(controller_type_enum) { + // roll gain, pitch gain, yaw gain, and no gains in throttle / z axis + case simple_flight::GoalModeType::AngleRate: + kp_axis4 = {kp[0], kp[1], kp[2], 1.0}; + ki_axis4 ={ki[0], ki[1], ki[2], 0.0}; + kd_axis4 = {kd[0], kd[1], kd[2], 0.0}; + params_.angle_rate_pid.p.setValues(kp_axis4); + params_.angle_rate_pid.i.setValues(ki_axis4); + params_.angle_rate_pid.d.setValues(kd_axis4); + params_.gains_changed = true; + break; + case simple_flight::GoalModeType::AngleLevel: + kp_axis4 = {kp[0], kp[1], kp[2], 1.0}; + ki_axis4 = {ki[0], ki[1], ki[2], 0.0}; + kd_axis4 = {kd[0], kd[1], kd[2], 0.0}; + params_.angle_level_pid.p.setValues(kp_axis4); + params_.angle_level_pid.i.setValues(ki_axis4); + params_.angle_level_pid.d.setValues(kd_axis4); + params_.gains_changed = true; + break; + case simple_flight::GoalModeType::VelocityWorld: + kp_axis4 = {kp[1], kp[0], 0.0, kp[2]}; + ki_axis4 = {ki[1], ki[0], 0.0, ki[2]}; + kd_axis4 = {kd[1], kd[0], 0.0, kd[2]}; + params_.velocity_pid.p.setValues(kp_axis4); + params_.velocity_pid.i.setValues(ki_axis4); + params_.velocity_pid.d.setValues(kd_axis4); + params_.gains_changed = true; + break; + case simple_flight::GoalModeType::PositionWorld: + kp_axis4 = {kp[1], kp[0], 0.0, kp[2]}; + ki_axis4 = {ki[1], ki[0], 0.0, ki[2]}; + kd_axis4 = {kd[1], kd[0], 0.0, kd[2]}; + params_.position_pid.p.setValues(kp_axis4); + params_.position_pid.i.setValues(ki_axis4); + params_.position_pid.d.setValues(kd_axis4); + params_.gains_changed = true; + break; + default: + Utils::log("Unimplemented controller type"); + break; + } + } + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override { //Utils::log(Utils::stringf("commandPosition %f, %f, %f, %f", x, y, z, yaw_mode.yaw_or_rate)); typedef simple_flight::GoalModeType GoalModeType; - simple_flight::GoalMode mode(GoalModeType::PositionWorld, GoalModeType::PositionWorld, - yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, + simple_flight::GoalMode mode(GoalModeType::PositionWorld, GoalModeType::PositionWorld, + yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, GoalModeType::PositionWorld); simple_flight::Axis4r goal(y, x, Utils::degreesToRadians(yaw_mode.yaw_or_rate), z); @@ -315,4 +432,4 @@ class SimpleFlightApi : public MultirotorApiBase { }; }} //namespace -#endif \ No newline at end of file +#endif diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp index 29c0b2493..1fae80143 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp @@ -31,33 +31,10 @@ class SimpleFlightQuadXParams : public MultiRotorParams { { auto& params = getParams(); - /******* Below is same config as PX4 generic model ********/ + // Use connection_info_.model for the model name, see Px4MultiRotorParams for example - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.2275f); - - //set up mass - params.mass = 1.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust - // given new thrust coefficients, motor max_rpm and propeller diameter. - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - - //leave everything else to defaults + // Only Generic for now + setupFrameGenericQuad(params); } virtual const SensorFactory* getSensorFactory() const override @@ -66,7 +43,6 @@ class SimpleFlightQuadXParams : public MultiRotorParams { } private: - vector> sensor_storage_; const AirSimSettings::VehicleSetting* vehicle_setting_; //store as pointer because of derived classes std::shared_ptr sensor_factory_; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index 42156bb4e..7de5f9a12 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -58,6 +58,19 @@ class AdaptiveController : public IController { return output_controls_; } + virtual bool isLastGoalModeAllPassthrough() override + { + is_last_goal_mode_all_passthrough_ = true; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + if (last_mode_[axis] != GoalModeType::Passthrough) { + is_last_goal_mode_all_passthrough_ = false; + } + } + + return is_last_goal_mode_all_passthrough_; + } + private: const IBoardClock* clock_; const IGoal* goal_; @@ -101,6 +114,7 @@ class AdaptiveController : public IController { bool reset = true; double x_0[12]; GoalMode last_mode_; + bool is_last_goal_mode_all_passthrough_; //double error[3] = { 0 }; double ref_vec[10][3] = {{ 0 }}; double ref_sum[3] = { 0 }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp index 42d6d34c8..95324951d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp @@ -21,7 +21,7 @@ class AngleLevelController : public IGoal //for internal rate controller { public: - AngleLevelController(const Params* params, const IBoardClock* clock = nullptr) + AngleLevelController(Params* params, const IBoardClock* clock = nullptr) : params_(params), clock_(clock) { } @@ -37,7 +37,7 @@ class AngleLevelController : //initialize level PID pid_.reset(new PidController(clock_, - PidConfig(params_->angle_level_pid.p[axis], 0, 0))); + PidConfig(params_->angle_level_pid.p[axis], params_->angle_level_pid.i[axis], params_->angle_level_pid.d[axis]))); //initialize rate controller rate_controller_.reset(new AngleRateController(params_, clock_)); @@ -135,7 +135,7 @@ class AngleLevelController : TReal output_; - const Params* params_; + Params* params_; const IBoardClock* clock_; std::unique_ptr> pid_; std::unique_ptr rate_controller_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp index e961c963f..19e6e12d3 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp @@ -16,7 +16,7 @@ namespace simple_flight { class AngleRateController : public IAxisController { public: - AngleRateController(const Params* params, const IBoardClock* clock) + AngleRateController(Params* params, const IBoardClock* clock) : params_(params), clock_(clock) { } @@ -31,7 +31,7 @@ class AngleRateController : public IAxisController { state_estimator_ = state_estimator; pid_.reset(new PidController(clock_, - PidConfig(params_->angle_rate_pid.p[axis], 0, 0))); + PidConfig(params_->angle_rate_pid.p[axis], params_->angle_rate_pid.i[axis], params_->angle_rate_pid.d[axis]))); } virtual void reset() override @@ -65,7 +65,7 @@ class AngleRateController : public IAxisController { TReal output_; - const Params* params_; + Params* params_; const IBoardClock* clock_; std::unique_ptr> pid_; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/CascadeController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/CascadeController.hpp index 73b06f088..c480541ad 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/CascadeController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/CascadeController.hpp @@ -19,7 +19,7 @@ namespace simple_flight { class CascadeController : public IController { public: - CascadeController(const Params* params, const IBoardClock* clock, ICommLink* comm_link) + CascadeController(Params* params, const IBoardClock* clock, ICommLink* comm_link) : params_(params), clock_(clock), comm_link_(comm_link) { } @@ -62,8 +62,8 @@ class CascadeController : public IController { } for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { - //re-create axis controllers if goal mode was changed since last time - if (goal_mode[axis] != last_goal_mode_[axis]) { + //re-create axis controllers if goal mode was changed since last time, or if gains have been updated + if (goal_mode[axis] != last_goal_mode_[axis] || params_->gains_changed == true) { switch (goal_mode[axis]) { case GoalModeType::AngleRate: axis_controllers_[axis].reset(new AngleRateController(params_, clock_)); @@ -107,6 +107,7 @@ class CascadeController : public IController { else comm_link_->log(std::string("Axis controller type is not set for axis ").append(std::to_string(axis)), ICommLink::kLogLevelInfo); } + params_->gains_changed = false; } virtual const Axis4r& getOutput() override @@ -114,9 +115,21 @@ class CascadeController : public IController { return output_; } + virtual bool isLastGoalModeAllPassthrough() override + { + is_last_goal_mode_all_passthrough_ = true; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + if (last_goal_mode_[axis] != GoalModeType::Passthrough) { + is_last_goal_mode_all_passthrough_ = false; + } + } + + return is_last_goal_mode_all_passthrough_; + } private: - const Params* params_; + Params* params_; const IBoardClock* clock_; const IGoal* goal_; @@ -127,6 +140,7 @@ class CascadeController : public IController { GoalMode last_goal_mode_; Axis4r last_goal_val_; + bool is_last_goal_mode_all_passthrough_; std::unique_ptr axis_controllers_[Axis4r::AxisCount()]; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp index 506df25d5..e830b4387 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp @@ -18,7 +18,7 @@ namespace simple_flight { class Firmware : public IFirmware { public: - Firmware(const Params* params, IBoard* board, ICommLink* comm_link, IStateEstimator* state_estimator) + Firmware(Params* params, IBoard* board, ICommLink* comm_link, IStateEstimator* state_estimator) : params_(params), board_(board), comm_link_(comm_link), state_estimator_(state_estimator), offboard_api_(params, board, board, state_estimator, comm_link), mixer_(params) { @@ -59,8 +59,17 @@ class Firmware : public IFirmware { const Axis4r& output_controls = controller_->getOutput(); - //convert controller output in to motor outputs - mixer_.getMotorOutput(output_controls, motor_outputs_); + // if last goal mode is passthrough for all axes (which means moveByMotorPWMs was called), + // we directly set the motor outputs to controller outputs + // note that the order of motors is as explained MultiRotorParams::initializeRotorQuadX() + if (controller_->isLastGoalModeAllPassthrough()) { + for (uint16_t motor_index = 0; motor_index < params_->motor.motor_count; ++motor_index) + motor_outputs_[motor_index] = output_controls[motor_index]; + } + else { + // apply motor mixing matrix to convert from controller output to motor outputs + mixer_.getMotorOutput(output_controls, motor_outputs_); + } //finally write the motor outputs for (uint16_t motor_index = 0; motor_index < params_->motor.motor_count; ++motor_index) @@ -77,7 +86,7 @@ class Firmware : public IFirmware { private: //objects we use - const Params* params_; + Params* params_; IBoard* board_; ICommLink* comm_link_; IStateEstimator* state_estimator_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp index 7af8486b5..cba144d63 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp @@ -51,44 +51,64 @@ struct Params { struct AngleRatePid { //max_xxx_rate > 5 would introduce wobble/oscillations const float kMaxLimit = 2.5f; + const float kP = 0.25f; + const float kI = 0.0f; + const float kD = 0.0f; + Axis3r max_limit = Axis3r(kMaxLimit, kMaxLimit, kMaxLimit); //roll, pitch, yaw - in radians/sec //p_xxx_rate params are sensitive to gyro noise. Values higher than 0.5 would require //noise filtration - const float kP = 0.25f; Axis4r p = Axis4r(kP, kP, kP, 1.0f); + Axis4r i = Axis4r(kI, kI, kI, 0.0f); + Axis4r d = Axis4r(kD, kD, kD, 0.0f); } angle_rate_pid; struct AngleLevelPid { const float pi = 3.14159265359f; //180-degrees + const float kP = 2.5f; + const float kI = 0.0f; + const float kD = 0.0f; - //max_pitch/roll_angle > 5.5 would produce versicle thrust that is not enough to keep vehicle in air at extremities of controls + //max_pitch/roll_angle > 5.5 would produce verticle thrust that is not enough to keep vehicle in air at extremities of controls Axis4r max_limit = Axis4r(pi / 5.5f, pi / 5.5f, pi, 1.0f); //roll, pitch, yaw - in radians/sec - const float kP = 2.5f; Axis4r p = Axis4r(kP, kP, kP, 1.0f); + Axis4r i = Axis4r(kI, kI, kI, 0.0f); + Axis4r d = Axis4r(kD, kD, kD, 0.0f); } angle_level_pid; struct PositionPid { const float kMaxLimit = 8.8E26f; //some big number like size of known universe + const float kP = 0.25f; + const float kI = 0.0f; + const float kD = 0.0f; + Axis4r max_limit = Axis4r(kMaxLimit, kMaxLimit, kMaxLimit, 1.0f); //x, y, z in meters - Axis4r p = Axis4r( 0.25f, 0.25f, 0, 0.25f); + Axis4r p = Axis4r(kP, kP, 0, kP); + Axis4r i = Axis4r(kI, kI, kI, kI); + Axis4r d = Axis4r(kD, kD, kD, kD); } position_pid; struct VelocityPid { const float kMinThrottle = std::min(1.0f, Params::min_armed_throttle() * 3.0f); const float kMaxLimit = 6.0f; // m/s + const float kP = 0.2f; + const float kI = 2.0f; + const float kD = 0.0f; + Axis4r max_limit = Axis4r(kMaxLimit, kMaxLimit, 0, kMaxLimit); //x, y, yaw, z in meters - Axis4r p = Axis4r(0.2f, 0.2f, 0, 2.0f); + Axis4r p = Axis4r(kP, kP, 0.0f, 2.0f); // todo why 2.0f hardcoded + Axis4r i = Axis4r(0.0f, 0.0f, 0.0f, kI); + Axis4r d = Axis4r(kD, kD, kD, kD); - Axis4r i = Axis4r(0, 0, 0, 2.0f); Axis4r iterm_discount = Axis4r(1, 1, 1, 0.9999f); Axis4r output_bias = Axis4r(0, 0, 0, 0); //we keep min throttle higher so that if we are angling a lot, its still supported - float min_throttle =kMinThrottle ; + float min_throttle = kMinThrottle ; } velocity_pid; struct Takeoff { @@ -105,6 +125,7 @@ struct Params { VehicleStateType default_vehicle_state = VehicleStateType::Inactive; uint64_t api_goal_timeout = 60; //milliseconds ControllerType controller_type = ControllerType::Cascade; + bool gains_changed; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp index af9d2161c..fdc53880a 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp @@ -59,7 +59,7 @@ class PidController : public IUpdatable { config_ = config; if (renabled) { - last_goal_ = goal_; + last_error_ = goal_ - measured_; integrator->set(output_); } } @@ -77,7 +77,7 @@ class PidController : public IUpdatable { measured_ = T(); last_time_ = clock_ == nullptr ? 0 : clock_->millis(); integrator->reset(); - last_goal_ = goal_; + last_error_ = goal_ - measured_; min_dt_ = config_.time_scale * config_.time_scale; } @@ -99,11 +99,9 @@ class PidController : public IUpdatable { if (dt > min_dt_) { integrator->update(dt, error, last_time_); - //To eliminate "derivative kick", we assume goal was approximately - //constant between successive calls. dE = dGoal - dInput = -dInput - float error_der = - (goal_ - last_goal_) / dt; + float error_der = dt > 0 ? (error - last_error_) / dt : 0; dterm = error_der * config_.kd; - last_goal_ = goal_; + last_error_ = error; } output_ = config_.output_bias + pterm + integrator->getOutput() + dterm; @@ -127,7 +125,7 @@ class PidController : public IUpdatable { uint64_t last_time_; const IBoardClock* clock_; - float last_goal_; + float last_error_; float min_dt_; const PidConfig config_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PositionController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PositionController.hpp index 30b8e0a0d..1db9340dd 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PositionController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PositionController.hpp @@ -18,14 +18,14 @@ class PositionController : public IGoal //for internal child controller { public: - PositionController(const Params* params, const IBoardClock* clock = nullptr) + PositionController(Params* params, const IBoardClock* clock = nullptr) : params_(params), clock_(clock) { } virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override { - if (axis_ == 2) + if (axis == 2) throw std::invalid_argument("PositionController does not support yaw axis i.e. " + std::to_string(axis)); axis_ = axis; @@ -34,7 +34,7 @@ class PositionController : //initialize parent PID pid_.reset(new PidController(clock_, - PidConfig(params_->position_pid.p[axis], 0, 0))); + PidConfig(params_->position_pid.p[axis], params_->position_pid.i[axis], params_->position_pid.d[axis]))); //initialize child controller velocity_controller_.reset(new VelocityController(params_, clock_)); @@ -100,7 +100,7 @@ class PositionController : TReal output_; - const Params* params_; + Params* params_; const IBoardClock* clock_; std::unique_ptr> pid_; std::unique_ptr velocity_controller_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp index 34ab4bed0..3918e170b 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp @@ -18,7 +18,7 @@ class VelocityController : public IGoal //for internal child controller { public: - VelocityController(const Params* params, const IBoardClock* clock = nullptr) + VelocityController(Params* params, const IBoardClock* clock = nullptr) : params_(params), clock_(clock) { } @@ -30,7 +30,7 @@ class VelocityController : state_estimator_ = state_estimator; PidConfig pid_config(params_->velocity_pid.p[axis], - params_->velocity_pid.i[axis], 0); + params_->velocity_pid.i[axis], params_->velocity_pid.d[axis]); pid_config.iterm_discount = params_->velocity_pid.iterm_discount[axis]; pid_config.output_bias = params_->velocity_pid.output_bias[axis]; @@ -146,7 +146,7 @@ class VelocityController : TReal output_; - const Params* params_; + Params* params_; const IBoardClock* clock_; std::unique_ptr> pid_; std::unique_ptr child_controller_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp index 3a0b794c7..2ffcea4c3 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp @@ -20,10 +20,12 @@ class Axis3 { { return vals_[index]; } + virtual const T& operator[] (unsigned int index) const { return vals_[index]; } + virtual std::string toString() const { return std::to_string(static_cast(vals_[0])) @@ -95,6 +97,14 @@ class Axis4 : public Axis3 { (*this)[axis] = axis3[axis]; } + void setValues(const vector& vals) + { + (*this)[0] = vals[0]; + (*this)[1] = vals[1]; + (*this)[2] = vals[2]; + val4_ = vals[3]; + } + T& val4() { return val4_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IController.hpp index 28944282c..4a7e51bac 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IController.hpp @@ -12,6 +12,7 @@ class IController : public IUpdatable { public: virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) = 0; virtual const Axis4r& getOutput() = 0; + virtual bool isLastGoalModeAllPassthrough() = 0; }; } //namespace \ No newline at end of file diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 82ba021c9..be9c01fe8 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -184,7 +184,7 @@ msr::airlib::GpsBase::Output RpcLibClientBase::getGpsData(const std::string& gps return pimpl_->client.call("getGpsData", gps_name, vehicle_name).as().to(); } -msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const std::string& distance_sensor_name, const std::string& vehicle_name) const +msr::airlib::DistanceSensorData RpcLibClientBase::getDistanceSensorData(const std::string& distance_sensor_name, const std::string& vehicle_name) const { return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as().to(); } @@ -219,6 +219,11 @@ void RpcLibClientBase::simSetVehiclePose(const Pose& pose, bool ignore_collision pimpl_->client.call("simSetVehiclePose", RpcLibAdapatorsBase::Pose(pose), ignore_collision, vehicle_name); } +void RpcLibClientBase::simSetTraceLine(const std::vector& color_rgba, float thickness, const std::string & vehicle_name) +{ + pimpl_->client.call("simSetTraceLine", color_rgba, thickness, vehicle_name); +} + vector RpcLibClientBase::simGetImages(vector request, const std::string& vehicle_name) { const auto& response_adaptor = pimpl_->client.call("simGetImages", @@ -237,11 +242,74 @@ vector RpcLibClientBase::simGetImage(const std::string& camera_name, Im return result; } +vector RpcLibClientBase::simGetMeshPositionVertexBuffers() +{ + const auto& response_adaptor = pimpl_->client.call("simGetMeshPositionVertexBuffers").as>(); + return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::to(response_adaptor); +} + void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity) { pimpl_->client.call("simPrintLogMessage", message, message_param, severity); } +void RpcLibClientBase::simFlushPersistentMarkers() +{ + pimpl_->client.call("simFlushPersistentMarkers"); +} + +void RpcLibClientBase::simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotPoints", conv_points, color_rgba, size, duration, is_persistent); +} + +void RpcLibClientBase::simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotLineStrip", conv_points, color_rgba, thickness, duration, is_persistent); +} + +void RpcLibClientBase::simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) +{ + vector conv_points; + RpcLibAdapatorsBase::from(points, conv_points); + pimpl_->client.call("simPlotLineList", conv_points, color_rgba, thickness, duration, is_persistent); +} + +void RpcLibClientBase::simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) +{ + vector conv_points_start; + RpcLibAdapatorsBase::from(points_start, conv_points_start); + vector conv_points_end; + RpcLibAdapatorsBase::from(points_end, conv_points_end); + pimpl_->client.call("simPlotArrows", conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); +} + +void RpcLibClientBase::simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration) +{ + vector conv_positions; + RpcLibAdapatorsBase::from(positions, conv_positions); + pimpl_->client.call("simPlotStrings", strings, conv_positions, scale, color_rgba, duration); +} + +void RpcLibClientBase::simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent) +{ + vector conv_poses; + RpcLibAdapatorsBase::from(poses, conv_poses); + pimpl_->client.call("simPlotTransforms", conv_poses, scale, thickness, duration, is_persistent); +} + +void RpcLibClientBase::simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) +{ + vector conv_poses; + RpcLibAdapatorsBase::from(poses, conv_poses); + pimpl_->client.call("simPlotTransformsWithNames", conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration); + +} + bool RpcLibClientBase::simIsPaused() const { return pimpl_->client.call("simIsPaused").as(); @@ -278,22 +346,49 @@ vector RpcLibClientBase::simListSceneObjects(const string& name_regex) c return pimpl_->client.call("simListSceneObjects", name_regex).as>(); } +std::vector RpcLibClientBase::simSwapTextures(const std::string& tags, int tex_id, int component_id, int material_id) +{ + return pimpl_->client.call("simSwapTextures", tags, tex_id, component_id, material_id).as>(); +} + +bool RpcLibClientBase::simLoadLevel(const string& level_name) +{ + return pimpl_->client.call("simLoadLevel", level_name).as(); +} + +msr::airlib::Vector3r RpcLibClientBase::simGetObjectScale(const std::string& object_name) const +{ + return pimpl_->client.call("simGetObjectScale", object_name).as().to(); +} + msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) const { return pimpl_->client.call("simGetObjectPose", object_name).as().to(); } + bool RpcLibClientBase::simSetObjectPose(const std::string& object_name, const msr::airlib::Pose& pose, bool teleport) { return pimpl_->client.call("simSetObjectPose", object_name, RpcLibAdapatorsBase::Pose(pose), teleport).as(); } +bool RpcLibClientBase::simSetObjectScale(const std::string& object_name, const msr::airlib::Vector3r& scale) +{ + return pimpl_->client.call("simSetObjectScale", object_name, RpcLibAdapatorsBase::Vector3r(scale)).as(); +} + CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name) const { return pimpl_->client.call("simGetCameraInfo", camera_name, vehicle_name).as().to(); } -void RpcLibClientBase::simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name) + +void RpcLibClientBase::simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name) +{ + pimpl_->client.call("simSetCameraPose", camera_name, RpcLibAdapatorsBase::Pose(pose), vehicle_name); +} + +void RpcLibClientBase::simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name) { - pimpl_->client.call("simSetCameraOrientation", camera_name, RpcLibAdapatorsBase::Quaternionr(orientation), vehicle_name); + pimpl_->client.call("simSetCameraFov", camera_name, fov_degrees, vehicle_name); } msr::airlib::Kinematics::State RpcLibClientBase::simGetGroundTruthKinematics(const std::string& vehicle_name) const @@ -322,108 +417,30 @@ RpcLibClientBase* RpcLibClientBase::waitOnLastTask(bool* task_result, float time return this; } -void* RpcLibClientBase::getClient() -{ - return &pimpl_->client; -} -const void* RpcLibClientBase::getClient() const -{ - return &pimpl_->client; -} - -//----------- APIs to control ACharacter in scene ----------/ -void RpcLibClientBase::simCharSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) -{ - pimpl_->client.call("simCharSetFaceExpression", expression_name, value, character_name); -} - -float RpcLibClientBase::simCharGetFaceExpression(const std::string& expression_name, const std::string& character_name) const -{ - return pimpl_->client.call("simCharGetFaceExpression", expression_name, character_name).as(); -} - -std::vector RpcLibClientBase::simCharGetAvailableFaceExpressions() -{ - return pimpl_->client.call("simCharGetAvailableFaceExpressions").as>(); -} - -void RpcLibClientBase::simCharSetSkinDarkness(float value, const std::string& character_name) -{ - pimpl_->client.call("simCharSetSkinDarkness", character_name, value); -} - -float RpcLibClientBase::simCharGetSkinDarkness(const std::string& character_name) const -{ - return pimpl_->client.call("simCharGetSkinDarkness", character_name).as(); -} - -void RpcLibClientBase::simCharSetSkinAgeing(float value, const std::string& character_name) +void RpcLibClientBase::startRecording() { - pimpl_->client.call("simCharSetSkinAgeing", character_name, value); + pimpl_->client.call("startRecording"); } -float RpcLibClientBase::simCharGetSkinAgeing(const std::string& character_name) const +void RpcLibClientBase::stopRecording() { - return pimpl_->client.call("simCharGetSkinAgeing", character_name).as(); + pimpl_->client.call("stopRecording"); } -void RpcLibClientBase::simCharSetHeadRotation(const msr::airlib::Quaternionr& q, const std::string& character_name) +bool RpcLibClientBase::isRecording() { - pimpl_->client.call("simCharSetHeadRotation", RpcLibAdapatorsBase::Quaternionr(q), character_name); + return pimpl_->client.call("isRecording").as(); } -msr::airlib::Quaternionr RpcLibClientBase::simCharGetHeadRotation(const std::string& character_name) const -{ - return pimpl_->client.call("simCharGetHeadRotation", character_name) - .as().to(); -} - -void RpcLibClientBase::simCharSetBonePose(const std::string& bone_name, const msr::airlib::Pose& pose, const std::string& character_name) -{ - pimpl_->client.call("simCharSetBonePose", bone_name, RpcLibAdapatorsBase::Pose(pose), character_name); -} - -msr::airlib::Pose RpcLibClientBase::simCharGetBonePose(const std::string& bone_name, const std::string& character_name) const -{ - return pimpl_->client.call("simCharGetBonePose", bone_name, character_name) - .as().to(); -} - -void RpcLibClientBase::simCharResetBonePose(const std::string& bone_name, const std::string& character_name) -{ - pimpl_->client.call("simCharResetBonePose", bone_name, character_name); -} - -void RpcLibClientBase::simCharSetFacePreset(const std::string& preset_name, float value, const std::string& character_name) -{ - pimpl_->client.call("simCharSetFacePreset", preset_name, value, character_name); -} -void RpcLibClientBase::simSetFacePresets(const std::unordered_map& presets, const std::string& character_name) -{ - pimpl_->client.call("simSetFacePresets", presets, character_name); -} -void RpcLibClientBase::simSetBonePoses(const std::unordered_map& poses, const std::string& character_name) +void* RpcLibClientBase::getClient() { - std::unordered_map r; - for (const auto& p : poses) - r[p.first] = RpcLibAdapatorsBase::Pose(p.second); - - pimpl_->client.call("simSetBonePoses", r, character_name); + return &pimpl_->client; } -std::unordered_map RpcLibClientBase::simGetBonePoses(const std::vector& bone_names, const std::string& character_name) const +const void* RpcLibClientBase::getClient() const { - std::unordered_map t = - pimpl_->client.call("simGetBonePoses", bone_names, character_name) - .as>(); - - std::unordered_map r; - for (const auto& p : t) - r[p.first] = p.second.to(); - - return r; + return &pimpl_->client; } - }} //namespace #endif diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 3146d46ac..43e62e534 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -30,6 +30,7 @@ STRICT_MODE_OFF #include "api/RpcLibAdapatorsBase.hpp" #include +#include STRICT_MODE_ON @@ -48,7 +49,26 @@ struct RpcLibServerBase::impl { ~impl() { } + void stop() { + server.close_sessions(); + if (!is_async_) { + // this deadlocks UI thread if async_run was called while there are pending rpc calls. + server.stop(); + } + } + + void run(bool block, std::size_t thread_count) + { + if (block) { + server.run(); + } else { + is_async_ = true; + server.async_run(thread_count); //4 threads + } + } + rpc::server server; + bool is_async_ = false; }; typedef msr::airlib_rpclib::RpcLibAdapatorsBase RpcLibAdapatorsBase; @@ -63,9 +83,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_.reset(new impl(server_address, port)); pimpl_->server.bind("ping", [&]() -> bool { return true; }); + pimpl_->server.bind("getServerVersion", []() -> int { return 1; }); + pimpl_->server.bind("getMinRequiredClientVersion", []() -> int { return 1; }); @@ -73,9 +95,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simPause", [&](bool is_paused) -> void { getWorldSimApi()->pause(is_paused); }); + pimpl_->server.bind("simIsPaused", [&]() -> bool { return getWorldSimApi()->isPaused(); }); + pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void { getWorldSimApi()->continueForTime(seconds); }); @@ -89,6 +113,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simEnableWeather", [&](bool enable) -> void { getWorldSimApi()->enableWeather(enable); }); + pimpl_->server.bind("simSetWeatherParameter", [&](WorldSimApiBase::WeatherParameter param, float val) -> void { getWorldSimApi()->setWeatherParameter(param, val); }); @@ -96,9 +121,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->enableApiControl(is_enabled); }); + pimpl_->server.bind("isApiControlEnabled", [&](const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->isApiControlEnabled(); }); + pimpl_->server.bind("armDisarm", [&](bool arm, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->armDisarm(arm); }); @@ -108,6 +135,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& const auto& response = getVehicleSimApi(vehicle_name)->getImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter)); return RpcLibAdapatorsBase::ImageResponse::from(response); }); + pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) -> vector { auto result = getVehicleSimApi(vehicle_name)->getImage(camera_name, type); if (result.size() == 0) { @@ -117,15 +145,25 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return result; }); + pimpl_->server.bind("simGetMeshPositionVertexBuffers", [&]() ->vector { + const auto& response = getWorldSimApi()->getMeshPositionVertexBuffers(); + return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::from(response); + }); + pimpl_->server. bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { getVehicleSimApi(vehicle_name)->setPose(pose.to(), ignore_collision); }); + pimpl_->server.bind("simGetVehiclePose", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::Pose { const auto& pose = getVehicleSimApi(vehicle_name)->getPose(); return RpcLibAdapatorsBase::Pose(pose); }); + pimpl_->server.bind("simSetTraceLine", [&](const std::vector& color_rgba, float thickness, const std::string& vehicle_name) -> void { + getVehicleSimApi(vehicle_name)->setTraceLine(color_rgba, thickness); + }); + pimpl_->server. bind("simGetLidarSegmentation", [&](const std::string& lidar_name, const std::string& vehicle_name) -> std::vector { return getVehicleApi(vehicle_name)->getLidarSegmentation(lidar_name); @@ -153,7 +191,8 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& sim_world_api->reset(); else getVehicleApi("")->reset(); - resetInProgress = false; + + resetInProgress = false; }); pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void { @@ -200,9 +239,14 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::CameraInfo(camera_info); }); - pimpl_->server.bind("simSetCameraOrientation", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Quaternionr& orientation, + pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Pose& pose, const std::string& vehicle_name) -> void { - getVehicleSimApi(vehicle_name)->setCameraOrientation(camera_name, orientation.to()); + getVehicleSimApi(vehicle_name)->setCameraPose(camera_name, pose.to()); + }); + + pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees, + const std::string& vehicle_name) -> void { + getVehicleSimApi(vehicle_name)->setCameraFoV(camera_name, fov_degrees); }); pimpl_->server.bind("simGetCollisionInfo", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::CollisionInfo { @@ -214,89 +258,112 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return getWorldSimApi()->listSceneObjects(name_regex); }); + pimpl_->server.bind("simLoadLevel", [&](const std::string& level_name) -> bool { + return getWorldSimApi()->loadLevel(level_name); + }); + + pimpl_->server.bind("simSpawnObject", [&](string& object_name, const string& load_component, const RpcLibAdapatorsBase::Pose& pose, const RpcLibAdapatorsBase::Vector3r& scale) -> string { + return getWorldSimApi()->spawnObject(object_name, load_component, pose.to(), scale.to()); + }); + + pimpl_->server.bind("simDestroyObject", [&](const string& object_name) -> bool { + return getWorldSimApi()->destroyObject(object_name); + }); + pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { const auto& pose = getWorldSimApi()->getObjectPose(object_name); return RpcLibAdapatorsBase::Pose(pose); }); + + pimpl_->server.bind("simGetObjectScale", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Vector3r { + const auto& scale = getWorldSimApi()->getObjectScale(object_name); + return RpcLibAdapatorsBase::Vector3r(scale); + }); + pimpl_->server.bind("simSetObjectPose", [&](const std::string& object_name, const RpcLibAdapatorsBase::Pose& pose, bool teleport) -> bool { return getWorldSimApi()->setObjectPose(object_name, pose.to(), teleport); }); - pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState { - const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics(); - return RpcLibAdapatorsBase::KinematicsState(result); + pimpl_->server.bind("simSetObjectScale", [&](const std::string& object_name, const RpcLibAdapatorsBase::Vector3r& scale) -> bool { + return getWorldSimApi()->setObjectScale(object_name, scale.to()); }); - pimpl_->server.bind("simGetGroundTruthEnvironment", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::EnvironmentState { - const Environment::State& result = (*getVehicleSimApi(vehicle_name)->getGroundTruthEnvironment()).getState(); - return RpcLibAdapatorsBase::EnvironmentState(result); + pimpl_->server.bind("simFlushPersistentMarkers", [&]() -> void { + getWorldSimApi()->simFlushPersistentMarkers(); }); - pimpl_->server.bind("cancelLastTask", [&](const std::string& vehicle_name) -> void { - getVehicleApi(vehicle_name)->cancelLastTask(); + pimpl_->server.bind("simPlotPoints", [&](const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotPoints(conv_points, color_rgba, size, duration, is_persistent); }); - //----------- APIs to control ACharacter in scene ----------/ - pimpl_->server.bind("simCharSetFaceExpression", [&](const std::string& expression_name, float value, const std::string& character_name) -> void { - getWorldSimApi()->charSetFaceExpression(expression_name, value, character_name); + pimpl_->server.bind("simPlotLineStrip", [&](const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotLineStrip(conv_points, color_rgba, thickness, duration, is_persistent); }); - pimpl_->server.bind("simCharGetFaceExpression", [&](const std::string& expression_name, const std::string& character_name) -> float { - return getWorldSimApi()->charGetFaceExpression(expression_name, character_name); - }); - pimpl_->server.bind("simCharGetAvailableFaceExpressions", [&]() -> std::vector { - return getWorldSimApi()->charGetAvailableFaceExpressions(); - }); - pimpl_->server.bind("simCharSetSkinDarkness", [&](float value, const std::string& character_name) -> void { - getWorldSimApi()->charSetSkinDarkness(value, character_name); - }); - pimpl_->server.bind("simCharGetSkinDarkness", [&](const std::string& character_name) -> float { - return getWorldSimApi()->charGetSkinDarkness(character_name); - }); - pimpl_->server.bind("simCharSetSkinAgeing", [&](float value, const std::string& character_name) -> void { - getWorldSimApi()->charSetSkinAgeing(value, character_name); + + pimpl_->server.bind("simPlotLineList", [&](const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) -> void { + vector conv_points; + RpcLibAdapatorsBase::to(points, conv_points); + getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, duration, is_persistent); }); - pimpl_->server.bind("simCharGetSkinAgeing", [&](const std::string& character_name) -> float { - return getWorldSimApi()->charGetSkinAgeing(character_name); + + pimpl_->server.bind("simPlotArrows", [&](const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) -> void { + vector conv_points_start; + RpcLibAdapatorsBase::to(points_start, conv_points_start); + vector conv_points_end; + RpcLibAdapatorsBase::to(points_end, conv_points_end); + getWorldSimApi()->simPlotArrows(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); }); - pimpl_->server.bind("simCharSetHeadRotation", [&](const RpcLibAdapatorsBase::Quaternionr& q, const std::string& character_name) -> void { - getWorldSimApi()->charSetHeadRotation(q.to(), character_name); + + pimpl_->server.bind("simPlotStrings", [&](const std::vector strings, const std::vector& positions, float scale, const vector& color_rgba, float duration) -> void { + vector conv_positions; + RpcLibAdapatorsBase::to(positions, conv_positions); + getWorldSimApi()->simPlotStrings(strings, conv_positions, scale, color_rgba, duration); }); - pimpl_->server.bind("simCharGetHeadRotation", [&](const std::string& character_name) -> RpcLibAdapatorsBase::Quaternionr { - msr::airlib::Quaternionr q = getWorldSimApi()->charGetHeadRotation(character_name); - return RpcLibAdapatorsBase::Quaternionr(q); + + pimpl_->server.bind("simPlotTransforms", [&](const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) -> void { + vector conv_poses; + RpcLibAdapatorsBase::to(poses, conv_poses); + getWorldSimApi()->simPlotTransforms(conv_poses, scale, thickness, duration, is_persistent); }); - pimpl_->server.bind("simCharSetBonePose", [&](const std::string& bone_name, const RpcLibAdapatorsBase::Pose& pose, const std::string& character_name) -> void { - getWorldSimApi()->charSetBonePose(bone_name, pose.to(), character_name); + + pimpl_->server.bind("simPlotTransformsWithNames", [&](const std::vector& poses, const std::vector names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) -> void { + vector conv_poses; + RpcLibAdapatorsBase::to(poses, conv_poses); + getWorldSimApi()->simPlotTransformsWithNames(conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration); }); - pimpl_->server.bind("simCharGetBonePose", [&](const std::string& bone_name, const std::string& character_name) -> RpcLibAdapatorsBase::Pose { - msr::airlib::Pose pose = getWorldSimApi()->charGetBonePose(bone_name, character_name); - return RpcLibAdapatorsBase::Pose(pose); + + pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState { + const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics(); + return RpcLibAdapatorsBase::KinematicsState(result); }); - pimpl_->server.bind("simCharResetBonePose", [&](const std::string& bone_name, const std::string& character_name) -> void { - getWorldSimApi()->charResetBonePose(bone_name, character_name); + + pimpl_->server.bind("simGetGroundTruthEnvironment", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::EnvironmentState { + const Environment::State& result = (*getVehicleSimApi(vehicle_name)->getGroundTruthEnvironment()).getState(); + return RpcLibAdapatorsBase::EnvironmentState(result); }); - pimpl_->server.bind("simCharSetFacePreset", [&](const std::string& preset_name, float value, const std::string& character_name) -> void { - getWorldSimApi()->charSetFacePreset(preset_name, value, character_name); + + pimpl_->server.bind("cancelLastTask", [&](const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->cancelLastTask(); }); - pimpl_->server.bind("simSetFacePresets", [&](const std::unordered_map& presets, const std::string& character_name) -> void { - getWorldSimApi()->charSetFacePresets(presets, character_name); + + pimpl_->server.bind("simSwapTextures", [&](const std::string tag, int tex_id, int component_id, int material_id) -> std::vector { + return *getWorldSimApi()->swapTextures(tag, tex_id, component_id, material_id); }); - pimpl_->server.bind("simSetBonePoses", [&](const std::unordered_map& poses, const std::string& character_name) -> void { - std::unordered_map r; - for (const auto& p : poses) - r[p.first] = p.second.to(); - getWorldSimApi()->charSetBonePoses(r, character_name); + pimpl_->server.bind("startRecording", [&]() -> void { + getWorldSimApi()->startRecording(); }); - pimpl_->server.bind("simGetBonePoses", [&](const std::vector& bone_names, const std::string& character_name) - -> std::unordered_map { - std::unordered_map poses = getWorldSimApi()->charGetBonePoses(bone_names, character_name); - std::unordered_map r; - for (const auto& p : poses) - r[p.first] = RpcLibAdapatorsBase::Pose(p.second); + pimpl_->server.bind("stopRecording", [&]() -> void { + getWorldSimApi()->stopRecording(); + }); - return r; + pimpl_->server.bind("isRecording", [&]() -> bool { + return getWorldSimApi()->isRecording(); }); //if we don't suppress then server will bomb out for exceptions raised by any method @@ -311,15 +378,12 @@ RpcLibServerBase::~RpcLibServerBase() void RpcLibServerBase::start(bool block, std::size_t thread_count) { - if (block) - pimpl_->server.run(); - else - pimpl_->server.async_run(thread_count); //4 threads + pimpl_->run(block, thread_count); } void RpcLibServerBase::stop() { - pimpl_->server.stop(); + pimpl_->stop(); } void* RpcLibServerBase::getServer() const diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index be32d215f..6bd1d4c19 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -72,7 +72,7 @@ bool MultirotorApiBase::goHome(float timeout_sec) return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); } -bool MultirotorApiBase::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration) +bool MultirotorApiBase::moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration) { SingleTaskCall lock(this); @@ -80,12 +80,12 @@ bool MultirotorApiBase::moveByAngleZ(float pitch, float roll, float z, float yaw return true; return waitForFunction([&]() { - moveByRollPitchZInternal(pitch, roll, z, yaw); + commandMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); return false; //keep moving until timeout }, duration).isTimeout(); } -bool MultirotorApiBase::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration) +bool MultirotorApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration) { SingleTaskCall lock(this); @@ -93,7 +93,72 @@ bool MultirotorApiBase::moveByAngleThrottle(float pitch, float roll, float throt return true; return waitForFunction([&]() { - moveByRollPitchThrottleInternal(pitch, roll, throttle, yaw_rate); + moveByRollPitchYawZInternal(roll, pitch, yaw, z); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawThrottleInternal(roll, pitch, yaw, throttle); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawrateThrottleInternal(roll, pitch, yaw_rate, throttle); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawrateZInternal(roll, pitch, yaw_rate, z); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByAngleRatesZInternal(roll_rate, pitch_rate, yaw_rate, z); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByAngleRatesThrottleInternal(roll_rate, pitch_rate, yaw_rate, throttle); return false; //keep moving until timeout }, duration).isTimeout(); } @@ -381,25 +446,29 @@ bool MultirotorApiBase::rotateToYaw(float yaw, float timeout_sec, float margin) { SingleTaskCall lock(this); - const YawMode yaw_mode(false, VectorMath::normalizeAngle(yaw)); - Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); - - float estimated_pitch, estimated_roll, estimated_yaw; + if (timeout_sec <= 0) + return true; auto start_pos = getPosition(); - do { - auto kinematics = getKinematicsEstimated(); - VectorMath::toEulerianAngle(kinematics.pose.orientation, - estimated_pitch, estimated_roll, estimated_yaw); - - if (isYawWithinMargin(estimated_yaw, margin)) - return true; + float yaw_target = VectorMath::normalizeAngle(yaw); + YawMode move_yaw_mode(false, yaw_target); + YawMode stop_yaw_mode(true, 0); - //change yaw by moving to same position but constant yaw mode - moveToPositionInternal(start_pos, yaw_mode); - } while (waiter.sleep()); + return waitForFunction([&]() { + if (isYawWithinMargin(yaw_target, margin)) { // yaw is within margin, then trying to stop rotation + moveToPositionInternal(start_pos, stop_yaw_mode); // let yaw rate be zero + auto yaw_rate = getKinematicsEstimated().twist.angular.z(); + if (abs(yaw_rate) <= approx_zero_angular_vel_) { // already sopped + return true; //stop all for stably achieving the goal + } + } + else { // yaw is not within margin, go on rotation + moveToPositionInternal(start_pos, move_yaw_mode); + } - return false; //we are not exiting because we reached yaw + // yaw is not within margin + return false; //keep moving until timeout + }, timeout_sec).isComplete(); } bool MultirotorApiBase::rotateByYawRate(float yaw_rate, float duration) @@ -411,12 +480,35 @@ bool MultirotorApiBase::rotateByYawRate(float yaw_rate, float duration) auto start_pos = getPosition(); YawMode yaw_mode(true, yaw_rate); - Waiter waiter(getCommandPeriod(), duration, getCancelToken()); - do { + + return waitForFunction([&]() { moveToPositionInternal(start_pos, yaw_mode); - } while (waiter.sleep()); + return false; //keep moving until timeout + }, duration).isTimeout(); +} - return waiter.isTimeout(); +void MultirotorApiBase::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd) +{ + uint8_t controller_type = 2; + setControllerGains(controller_type, kp, ki, kd); +} + +void MultirotorApiBase::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd) +{ + uint8_t controller_type = 3; + setControllerGains(controller_type, kp, ki, kd); +} + +void MultirotorApiBase::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd) +{ + uint8_t controller_type = 4; + setControllerGains(controller_type, kp, ki, kd); +} + +void MultirotorApiBase::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd) +{ + uint8_t controller_type = 5; + setControllerGains(controller_type, kp, ki, kd); } bool MultirotorApiBase::hover() @@ -451,16 +543,40 @@ void MultirotorApiBase::moveToPositionInternal(const Vector3r& dest, const YawMo commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode); } -void MultirotorApiBase::moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate) +void MultirotorApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawZ(roll, pitch, yaw, z); +} + +void MultirotorApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawThrottle(roll, pitch, yaw, throttle); +} + +void MultirotorApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle); +} + +void MultirotorApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateZ(roll, pitch, yaw_rate, z); +} + +void MultirotorApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z) { if (safetyCheckVelocity(getVelocity())) - commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); + commandAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z); } -void MultirotorApiBase::moveByRollPitchZInternal(float pitch, float roll, float z, float yaw) +void MultirotorApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle) { if (safetyCheckVelocity(getVelocity())) - commandRollPitchZ(pitch, roll, z, yaw); + commandAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle); } //executes a given function until it returns true. Each execution is spaced apart at command period. diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 01f17fcd5..769fad684 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -75,15 +75,45 @@ MultirotorRpcLibClient* MultirotorRpcLibClient::goHomeAsync(float timeout_sec, c return this; } -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleZAsync(float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name) +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) { - pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleZ", pitch, roll, z, yaw, duration, vehicle_name); + pimpl_->last_future = static_cast(getClient())->async_call("moveByMotorPWMs", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name); return this; } -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleThrottleAsync(float pitch, float roll, float throttle, float yaw_rate, float duration, const std::string& vehicle_name) +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) { - pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleThrottle", pitch, roll, throttle, yaw_rate, duration, vehicle_name); + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawZ", roll, pitch, yaw, z, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawThrottle", roll, pitch, yaw, throttle, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateThrottle", roll, pitch, yaw_rate, throttle, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateZ", roll, pitch, yaw_rate, z, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesZ", roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesThrottle", roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name); return this; } @@ -155,6 +185,26 @@ MultirotorRpcLibClient* MultirotorRpcLibClient::hoverAsync(const std::string& ve return this; } +void MultirotorRpcLibClient::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) +{ + static_cast(getClient())->call("setAngleLevelControllerGains", kp, ki, kd, vehicle_name); +} + +void MultirotorRpcLibClient::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) +{ + static_cast(getClient())->call("setAngleRateControllerGains", kp, ki, kd, vehicle_name); +} + +void MultirotorRpcLibClient::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) +{ + static_cast(getClient())->call("setVelocityControllerGains", kp, ki, kd, vehicle_name); +} + +void MultirotorRpcLibClient::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) +{ + static_cast(getClient())->call("setPositionControllerGains", kp, ki, kd, vehicle_name); +} + bool MultirotorRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) { diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 7051bc58d..9035b304f 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -53,14 +53,38 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string bind("goHome", [&](float timeout_sec, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->goHome(timeout_sec); }); - (static_cast(getServer()))-> - bind("moveByAngleZ", [&](float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name) -> - bool { return getVehicleApi(vehicle_name)->moveByAngleZ(pitch, roll, z, yaw, duration); }); + bind("moveByMotorPWMs", [&](float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) -> + bool { return getVehicleApi(vehicle_name)->moveByMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawZ", [&](float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) -> + bool { return getVehicleApi(vehicle_name)->moveByRollPitchYawZ(roll, pitch, yaw, z, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawThrottle", [&](float roll, float pitch, float yaw, float throttle, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawThrottle(roll, pitch, yaw, throttle, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawrateThrottle", [&](float roll, float pitch, float yaw_rate, float throttle, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawrateZ", [&](float roll, float pitch, float yaw_rate, float z, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateZ(roll, pitch, yaw_rate, z, duration); + }); + (static_cast(getServer()))-> + bind("moveByAngleRatesZ", [&](float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z, duration); + }); (static_cast(getServer()))-> - bind("moveByAngleThrottle", [&](float pitch, float roll, float throttle, float yaw_rate, float duration, + bind("moveByAngleRatesThrottle", [&](float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); + return getVehicleApi(vehicle_name)->moveByAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle, duration); }); (static_cast(getServer()))-> bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, @@ -107,6 +131,22 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string bind("hover", [&](const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->hover(); }); + (static_cast(getServer()))-> + bind("setAngleLevelControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setAngleLevelControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))-> + bind("setAngleRateControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setAngleRateControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))-> + bind("setVelocityControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setVelocityControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))-> + bind("setPositionControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setPositionControllerGains(kp, ki, kd); + }); (static_cast(getServer()))-> bind("moveByRC", [&](const MultirotorRpcLibAdapators::RCData& data, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->moveByRC(data.to()); diff --git a/AirLibUnitTests/AirLibUnitTests.vcxproj b/AirLibUnitTests/AirLibUnitTests.vcxproj index 8ee90fce9..bcb91d2fc 100644 --- a/AirLibUnitTests/AirLibUnitTests.vcxproj +++ b/AirLibUnitTests/AirLibUnitTests.vcxproj @@ -1,5 +1,5 @@  - + true @@ -31,26 +31,26 @@ Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode @@ -101,6 +101,8 @@ WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include true + 5205%(DisableSpecificWarnings) + stdcpp17 Console @@ -119,7 +121,8 @@ $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include true /w34263 /w34266 %(AdditionalOptions) - 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings) + 4100;4505;4820;4464;4514;4710;4571;5205%(DisableSpecificWarnings) + stdcpp17 Console @@ -138,8 +141,10 @@ true WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include - true + false /w34263 /w34266 %(AdditionalOptions) + 5205%(DisableSpecificWarnings) + stdcpp17 Console @@ -160,8 +165,10 @@ true NDEBUG;_CONSOLE;%(PreprocessorDefinitions) $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include - true + false /w34263 /w34266 %(AdditionalOptions) + 5205%(DisableSpecificWarnings) + stdcpp17 Console diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 949c046f8..963b526da 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -8,7 +8,7 @@ #include "physics/FastPhysicsEngine.hpp" #include "vehicles/multirotor/api/MultirotorApiBase.hpp" #include "common/SteppableClock.hpp" -#include "vehicles/multirotor/MultiRotor.hpp" +#include "vehicles/multirotor/MultiRotorPhysicsBody.hpp" namespace msr { namespace airlib { @@ -39,7 +39,7 @@ class SimpleFlightTest : public TestBase initial_environment.geo_point = GeoPoint(); environment.reset(new Environment(initial_environment)); - MultiRotor vehicle(params.get(), api.get(), kinematics.get(), environment.get()); + MultiRotorPhysicsBody vehicle(params.get(), api.get(), kinematics.get(), environment.get()); std::vector vehicles = { &vehicle }; std::unique_ptr physics_engine(new FastPhysicsEngine()); diff --git a/AirLibUnitTests/TestBase.hpp b/AirLibUnitTests/TestBase.hpp index 13342dd2c..d1916fb96 100644 --- a/AirLibUnitTests/TestBase.hpp +++ b/AirLibUnitTests/TestBase.hpp @@ -9,6 +9,7 @@ namespace msr { namespace airlib { class TestBase { public: + virtual ~TestBase() = default; virtual void run() = 0; void testAssert(double lhs, double rhs, const std::string& message) { diff --git a/AirSim.sln b/AirSim.sln index 9a7501985..5d9bf8920 100644 --- a/AirSim.sln +++ b/AirSim.sln @@ -1,7 +1,7 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 15 -VisualStudioVersion = 15.0.27428.2043 +# Visual Studio 16 +VisualStudioVersion = 16.0.27428.2043 MinimumVisualStudioVersion = 10.0.40219.1 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DroneShell", "DroneShell\DroneShell.vcxproj", "{9FE9234B-373A-4D5A-AD6B-FB0B593312DD}" EndProject @@ -39,9 +39,9 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "UnrealPluginFiles", "Unreal EndProject Project("{888888A0-9F3D-457C-B088-3A5042F75D52}") = "PythonClient", "PythonClient\PythonClient.pyproj", "{E2049E20-B6DD-474E-8BCA-1C8DC54725AA}" EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "sgmstereo", "SGM\src\sgmstereo\sgmstereo_vc15.vcxproj", "{A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}" +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "sgmstereo", "SGM\src\sgmstereo\sgmstereo.vcxproj", "{A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}" EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "stereoPipeline", "SGM\src\stereoPipeline\stereoPipeline_vc15.vcxproj", "{E512EB59-4EAB-49D1-9174-0CAF1B40CED0}" +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "stereoPipeline", "SGM\src\stereoPipeline\stereoPipeline.vcxproj", "{E512EB59-4EAB-49D1-9174-0CAF1B40CED0}" EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution diff --git a/CHANGELOG.md b/CHANGELOG.md index 49208967e..6545cf2c0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,113 +1,3 @@ -# What's new +# Changelog -Below is summarized list of important changes. This does not include minor/less important changes or bug fixes or documentation update. This list updated every few months. For complete detailed changes, please review [commit history](https://github.com/Microsoft/AirSim/commits/master). - -### November, 2018 -* Added Weather Effects and [APIs](docs/apis.md#weather-apis) -* Added [Time of Day API](docs/apis.md#time-of-day-api) -* An experimental integration of [AirSim on Unity](https://github.com/Microsoft/AirSim/tree/master/Unity) is now available. Learn more in [Unity blog post](https://blogs.unity3d.com/2018/11/14/airsim-on-unity-experiment-with-autonomous-vehicle-simulation). -* [New environments](https://github.com/Microsoft/AirSim/releases/tag/v1.2.1): Forest, Plains (windmill farm), TalkingHeads (human head simulation), TrapCam (animal detection via camera) -* Highly efficient [NoDisplay view mode](https://github.com/Microsoft/AirSim/blob/master/docs/settings.md#viewmode) to turn off main screen rendering so you can capture images at high rate -* [Enable/disable sensors](https://github.com/Microsoft/AirSim/pull/1479) via settings -* [Lidar Sensor](docs/lidar.md) -* [Support for Flysky FS-SM100 RC](https://github.com/Microsoft/AirSim/commit/474214364676b6631c01b3ed79d00c83ba5bccf5) USB adapter -* Case Study: [Formula Student Technion Driverless](https://github.com/Microsoft/AirSim/wiki/technion) -* [Multi-Vehicle Capability](docs/multi_vehicle.md) -* [Custom speed units](https://github.com/Microsoft/AirSim/pull/1181) -* [ROS publisher](https://github.com/Microsoft/AirSim/pull/1135) -* [simSetObjectPose API](https://github.com/Microsoft/AirSim/pull/1161) -* [Character Control APIs](https://github.com/Microsoft/AirSim/blob/master/PythonClient/airsim/client.py#L137) (works on TalkingHeads binaries in release) -* [Arducopter Solo Support](https://github.com/Microsoft/AirSim/pull/1387) -* [Linux install without sudo access](https://github.com/Microsoft/AirSim/pull/1434) -* [Kinect like ROS publisher](https://github.com/Microsoft/AirSim/pull/1298) - - -### June, 2018 -* Development workflow doc -* Better Python 2 compatibility -* OSX setup fixes -* Almost complete rewrite of our APIs with new threading model, merging old APIs and creating few newer ones - -### April, 2018 -* Upgraded to Unreal Engine 4.18 and Visual Studio 2017 -* API framework refactoring to support world-level APIs -* Latest PX4 firmware now supported -* CarState with more information -* ThrustMaster wheel support -* pause and continueForTime APIs for drone as well as car -* Allow drone simulation run at higher clock rate without any degradation -* Forward-only mode fully functional for drone (do orbits while looking at center) -* Better PID tuning to reduce wobble for drones -* Ability to set arbitrary vehicle blueprint for drone as well as car -* gimbal stabilization via settings -* Ability to segment skinned and skeletal meshes by their name -* moveByAngleThrottle API -* Car physics tuning for better maneuverability -* Configure additional cameras via settings -* Time of day with geographically computed sun position -* Better car steering via keyboard -* Added MeshNamingMethod in segmentation setting -* gimbal API -* getCameraParameters API -* Ability turn off main rendering to save GPU resources -* Projection mode for capture settings -* getRCData, setRCData APIs -* Ability to turn off segmentation using negative IDs -* OSX build improvements -* Segmentation working for very large environments with initial IDs -* Better and extensible hash calculation for segmentation IDs -* Extensible PID controller for custom integration methods -* Sensor architecture now enables renderer specific features like ray casting -* Laser altimeter sensor - - -### Jan 2018 -* Config system rewrite, enable flexible config we are targeting in future -* Multi-Vehicle support Phase 1, core infrastructure changes -* MacOS support -* Infrared view -* 5 types of noise and interference for cameras -* WYSIWIG capture settings for cameras, preview recording settings in main view -* Azure support Phase 1, enable configurability of instances for headless mode -* Full kinematics APIs, ability to get pose, linear and angular velocities + accelerations via APIs -* Record multiple images from multiple cameras -* New segmentation APIs, ability to set configure object IDs, search via regex -* New object pose APIs, ability to get pose of objects (like animals) in environment -* Camera infrastructure enhancements, ability to add new image types like IR with just few lines -* Clock speed APIs for drone as well as car, simulation can be run with speed factor of 0 < x < infinity -* Support for Logitech G920 wheel -* Physics tuning of the car, Car doesn’t roll over, responds to steering with better curve, releasing gas paddle behavior more realistic -* Debugging APIs -* Stress tested to 24+ hours of continuous runs -* Support for Landscape and sky segmentation -* Manual navigation with accelerated controls in CV mode, user can explore environment much more easily -* Collison APIs -* Recording enhancements, log several new data points including ground truth, multiple images, controls state -* Planner and Perspective Depth views -* Disparity view -* New Image APIs supports float, png or numpy formats -* 6 config settings for image capture, ability to set auto-exposure, motion blur, gamma etc -* Full multi-camera support through out including sub-windows, recording, APIs etc -* Command line script to build all environments in one shot -* Remove submodules, use rpclib as download - -### Nov 2017 -* We now have the [car model](docs/using_car.md). -* No need to build the code. Just download [binaries](https://github.com/Microsoft/AirSim/releases) and you are good to go! -* The [reinforcement learning example](docs/reinforcement_learning.md) with AirSim -* New built-in flight controller called [simple_flight](docs/simple_flight.md) that "just works" without any additional setup. It is also now *default*. -* AirSim now also generates [depth as well as disparity images](docs/image_apis.md) that is in camera plan. -* We also have official Linux build now! - -## Sep 2017 -- We have added [car model](docs/using_car.md)! - -## Aug 2017 -- [simple_flight](docs/simple_flight.md) is now default flight controller for drones. If you want to use PX4, you will need to modify settings.json as per [PX4 setup doc](docs/px4_setup.md). -- Linux build is official and currently uses Unreal 4.17 due to various bug fixes required -- ImageType enum has breaking changes with several new additions and clarifying existing ones -- SubWindows are now configurable from settings.json -- PythonClient is now complete and has parity with C++ APIs. Some of these would have breaking changes. - -## Feb 2017 -- First release! \ No newline at end of file +This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/CHANGELOG.md). \ No newline at end of file diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 866634956..99687b285 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,17 +1,3 @@ # Contributing -## Quick Start -- Please read our [short and sweet coding guidelines](docs/coding_guidelines.md). -- For big changes such as adding new feature or refactoring, [file an issue first](https://github.com/Microsoft/AirSim/issues). -- Use our [recommended development workflow](docs/dev_workflow.md) to make changes and test it. -- Use [usual steps](https://akrabat.com/the-beginners-guide-to-contributing-to-a-github-project/) to make contributions just like other GitHub projects. If you are not familiar with Git Branch-Rebase-Merge workflow, please [read this first](http://shitalshah.com/p/git-workflow-branch-rebase-squash-merge/). - -## Checklist -- Use same style and formatting as rest of code even if it's not your preferred one. -- Change any documentation that goes with code changes. -- Do not include OS specific header files or new 3rd party dependencies. -- Keep your pull request small, ideally under 10 files. -- Make sure you don't include large binary files. -- When adding new includes, make dependency is absolutely necessary. -- Rebase your branch frequently with master (once every 2-3 days is ideal). -- Make sure your code would compile on Windows, Linux and OSX. +This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/CONTRIBUTING.md). \ No newline at end of file diff --git a/DroneServer/DroneServer.vcxproj b/DroneServer/DroneServer.vcxproj index 8254af722..f298c171a 100644 --- a/DroneServer/DroneServer.vcxproj +++ b/DroneServer/DroneServer.vcxproj @@ -1,5 +1,5 @@  - + true @@ -39,26 +39,26 @@ Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode @@ -110,6 +110,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\MavLinkCom\include;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -129,6 +130,7 @@ true /w34263 /w34266 %(AdditionalOptions) 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings) + stdcpp17 Console @@ -149,6 +151,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\MavLinkCom\include;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -171,6 +174,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\MavLinkCom\include;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index a7f8c946a..b64000cc4 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -11,7 +11,7 @@ using namespace std; using namespace msr::airlib; /* - This is a sample code demonstrating how to deploy rpc server on-board + This is a sample code demonstrating how to deploy rpc server on-board real drone so we can use same APIs on real vehicle that we used in simulation. This demonstration is designed for PX4 powered drone. */ @@ -32,7 +32,7 @@ int main(int argc, const char* argv[]) std::cout << "WARNING: This is not simulation!" << std::endl; AirSimSettings::MavLinkConnectionInfo connection_info; - + // read settings and override defaults auto settings_full_filepath = Settings::getUserDirectoryFullPath("settings.json"); Settings& settings = Settings::singleton().loadJSonFile(settings_full_filepath); @@ -57,14 +57,16 @@ int main(int argc, const char* argv[]) connection_info.qgc_ip_address = child.getString("QgcHostIp", connection_info.qgc_ip_address); connection_info.qgc_ip_port = child.getInt("QgcPort", connection_info.qgc_ip_port); - connection_info.sitl_ip_address = child.getString("SitlIp", connection_info.sitl_ip_address); - connection_info.sitl_ip_port = child.getInt("SitlPort", connection_info.sitl_ip_port); + connection_info.control_ip_address = child.getString("ControlIp", connection_info.control_ip_address); + connection_info.control_port = child.getInt("ControlPort", connection_info.control_port); connection_info.local_host_ip = child.getString("LocalHostIp", connection_info.local_host_ip); connection_info.use_serial = child.getBool("UseSerial", connection_info.use_serial); - connection_info.ip_address = child.getString("UdpIp", connection_info.ip_address); - connection_info.ip_port = child.getInt("UdpPort", connection_info.ip_port); + connection_info.udp_address = child.getString("UdpIp", connection_info.udp_address); + connection_info.udp_port = child.getInt("UdpPort", connection_info.udp_port); + connection_info.use_tcp = child.getBool("UseTcp", connection_info.use_tcp); + connection_info.tcp_port = child.getInt("TcpPort", connection_info.tcp_port); connection_info.serial_port = child.getString("SerialPort", connection_info.serial_port); connection_info.baud_rate = child.getInt("SerialBaudRate", connection_info.baud_rate); @@ -83,11 +85,16 @@ int main(int argc, const char* argv[]) ApiProvider api_provider(nullptr); api_provider.insert_or_assign("", &api, nullptr); msr::airlib::MultirotorRpcLibServer server(&api_provider, connection_info.local_host_ip); - + //start server in async mode server.start(false, 4); - std::cout << "Server connected to MavLink endpoint at " << connection_info.local_host_ip << ":" << connection_info.ip_port << std::endl; + if (connection_info.use_tcp) { + std::cout << "Server connected to MavLink TCP endpoint at " << connection_info.local_host_ip << ":" << connection_info.tcp_port << std::endl; + } + else { + std::cout << "Server connected to MavLink UDP endpoint at " << connection_info.local_host_ip << ":" << connection_info.udp_port << std::endl; + } std::cout << "Hit Ctrl+C to terminate." << std::endl; std::vector messages; @@ -98,7 +105,7 @@ int main(int argc, const char* argv[]) for (const auto& message : messages) { std::cout << message << std::endl; } - } + } constexpr static std::chrono::milliseconds MessageCheckDurationMillis(100); std::this_thread::sleep_for(MessageCheckDurationMillis); diff --git a/DroneShell/DroneShell.vcxproj b/DroneShell/DroneShell.vcxproj index b15f08669..3d7e8c50d 100644 --- a/DroneShell/DroneShell.vcxproj +++ b/DroneShell/DroneShell.vcxproj @@ -1,6 +1,6 @@  - - + + true @@ -31,26 +31,26 @@ Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode @@ -102,6 +102,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -121,6 +122,7 @@ true /w34263 /w34266 %(AdditionalOptions) 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings) + stdcpp17 Console @@ -141,6 +143,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -163,6 +166,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp index 791ae73bf..3ff83e962 100644 --- a/DroneShell/src/main.cpp +++ b/DroneShell/src/main.cpp @@ -505,7 +505,7 @@ class MoveByAngleZCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleZAsync(pitch, roll, z, yaw, duration); + context->client.moveByRollPitchYawZAsync(roll, pitch, yaw, z, duration); }); return false; @@ -534,7 +534,7 @@ class MoveByAngleThrottleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleThrottleAsync(pitch, roll, throttle, yaw_rate, duration); + context->client.moveByRollPitchYawrateThrottleAsync(roll, pitch, yaw_rate, throttle, duration); }); return false; @@ -693,13 +693,13 @@ class BackForthByAngleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleZAsync(pitch, roll, z, yaw, duration); + context->client.moveByRollPitchYawZAsync(roll, pitch, yaw, z, duration); if (!context->client.waitOnLastTask()) { throw std::runtime_error("BackForthByAngleCommand canceled"); } context->client.hoverAsync(); context->sleep_for(pause_time); - context->client.moveByAngleZAsync(-pitch, -roll, z, yaw, duration); + context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, duration); if (!context->client.waitOnLastTask()){ throw std::runtime_error("BackForthByAngleCommand canceled"); } @@ -782,28 +782,28 @@ class SquareByAngleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleZAsync(pitch, -roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(-roll, pitch, yaw, z, 0); if (!context->client.waitOnLastTask()) { throw std::runtime_error("SquareByAngleCommand canceled"); } context->client.hoverAsync(); context->sleep_for(pause_time); - context->client.moveByAngleZAsync(-pitch, -roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, 0); if (!context->client.waitOnLastTask()) { throw std::runtime_error("SquareByAngleCommand canceled"); } context->client.hoverAsync(); context->sleep_for(pause_time); - context->client.moveByAngleZAsync(-pitch, roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(roll, -pitch, yaw, z, 0); if (!context->client.waitOnLastTask()) { throw std::runtime_error("SquareByAngleCommand canceled"); } context->client.hoverAsync(); context->sleep_for(pause_time); - context->client.moveByAngleZAsync(-pitch, -roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, 0); if (!context->client.waitOnLastTask()){ throw std::runtime_error("SquareByAngleCommand canceled"); } diff --git a/Examples/Examples.vcxproj b/Examples/Examples.vcxproj index 5a609d933..a0f145a77 100644 --- a/Examples/Examples.vcxproj +++ b/Examples/Examples.vcxproj @@ -1,5 +1,5 @@  - + true @@ -29,10 +29,10 @@ {8510c7a4-bf63-41d2-94f6-d8731d137a5a} - + {a01e543f-ef34-46bb-8f3f-29ab84e7a5d4} - + {e512eb59-4eab-49d1-9174-0caf1b40ced0} @@ -45,26 +45,26 @@ Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode @@ -116,6 +116,7 @@ include;$(ProjectDir)..\AirLib\deps\rpclib\include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -135,6 +136,7 @@ true /w34263 /w34266 %(AdditionalOptions) 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings) + stdcpp17 Console @@ -155,6 +157,7 @@ include;$(ProjectDir)..\AirLib\deps\rpclib\include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -177,6 +180,7 @@ include;$(ProjectDir)..\AirLib\deps\rpclib\include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console diff --git a/HelloCar/HelloCar.vcxproj b/HelloCar/HelloCar.vcxproj index c24e862b8..b76d29095 100644 --- a/HelloCar/HelloCar.vcxproj +++ b/HelloCar/HelloCar.vcxproj @@ -1,5 +1,5 @@  - + true @@ -42,26 +42,26 @@ Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode @@ -113,6 +113,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -132,6 +133,7 @@ true /w34263 /w34266 %(AdditionalOptions) 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings) + stdcpp17 Console @@ -152,6 +154,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -174,6 +177,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console diff --git a/HelloDrone/HelloDrone.vcxproj b/HelloDrone/HelloDrone.vcxproj index cd59ed34c..790db10ad 100644 --- a/HelloDrone/HelloDrone.vcxproj +++ b/HelloDrone/HelloDrone.vcxproj @@ -1,5 +1,5 @@  - + true @@ -42,26 +42,26 @@ Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode @@ -113,6 +113,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -132,6 +133,7 @@ true /w34263 /w34266 %(AdditionalOptions) 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings) + stdcpp17 Console @@ -152,6 +154,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console @@ -174,6 +177,7 @@ $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include true /w34263 /w34266 %(AdditionalOptions) + stdcpp17 Console diff --git a/MavLinkCom/MavLinkCom.vcxproj b/MavLinkCom/MavLinkCom.vcxproj index 7838ec785..319db2dad 100644 --- a/MavLinkCom/MavLinkCom.vcxproj +++ b/MavLinkCom/MavLinkCom.vcxproj @@ -1,9 +1,9 @@  - + true - v141 + v142 @@ -136,6 +136,7 @@ common_utils;include true true + stdcpp17 Console @@ -205,6 +206,7 @@ common_utils;include true true + stdcpp17 Console @@ -242,6 +244,7 @@ MultiThreadedDLL common_utils;include true + stdcpp17 Console @@ -312,6 +315,7 @@ common_utils;include true true + stdcpp17 Console @@ -381,7 +385,6 @@ - diff --git a/MavLinkCom/MavLinkCom.vcxproj.filters b/MavLinkCom/MavLinkCom.vcxproj.filters index 4d1686b17..0cc6ffdba 100644 --- a/MavLinkCom/MavLinkCom.vcxproj.filters +++ b/MavLinkCom/MavLinkCom.vcxproj.filters @@ -124,9 +124,6 @@ Mavlink - - common_utils - common_utils diff --git a/MavLinkCom/MavLinkComGenerator/MavLinkComGenerator.sln b/MavLinkCom/MavLinkComGenerator/MavLinkComGenerator.sln new file mode 100644 index 000000000..59459efc8 --- /dev/null +++ b/MavLinkCom/MavLinkComGenerator/MavLinkComGenerator.sln @@ -0,0 +1,25 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.29430.225 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MavLinkComGenerator", "MavLinkComGenerator.csproj", "{7F5D2A5F-60CC-4C88-8D43-B8B1573D6398}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {7F5D2A5F-60CC-4C88-8D43-B8B1573D6398}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {7F5D2A5F-60CC-4C88-8D43-B8B1573D6398}.Debug|Any CPU.Build.0 = Debug|Any CPU + {7F5D2A5F-60CC-4C88-8D43-B8B1573D6398}.Release|Any CPU.ActiveCfg = Release|Any CPU + {7F5D2A5F-60CC-4C88-8D43-B8B1573D6398}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {E3E033CC-900D-4073-AA80-DA3E2F4B3F28} + EndGlobalSection +EndGlobal diff --git a/MavLinkCom/MavLinkComGenerator/MavLinkGenerator.cs b/MavLinkCom/MavLinkComGenerator/MavLinkGenerator.cs index 8b525aca5..05073e125 100644 --- a/MavLinkCom/MavLinkComGenerator/MavLinkGenerator.cs +++ b/MavLinkCom/MavLinkComGenerator/MavLinkGenerator.cs @@ -5,6 +5,7 @@ using System.Collections.Generic; using System.IO; using System.Linq; +using System.Security.Cryptography; using System.Text; using System.Threading.Tasks; @@ -80,7 +81,15 @@ private void GenerateCommands() UniqueList unique = new UniqueList(); foreach (var p in cmd.parameters) { - string fieldName = NameFromDescription(p.description); + string fieldName = p.label; + if (string.IsNullOrWhiteSpace(fieldName)) + { + fieldName = NameFromDescription(p.description); + } + else + { + fieldName = LegalizeIdentifier(fieldName); + } if (fieldName != "Empty" && fieldName != "Reserved") { if (!string.IsNullOrWhiteSpace(p.description)) @@ -100,7 +109,15 @@ private void GenerateCommands() foreach (var p in cmd.parameters) { i++; - string fieldName = NameFromDescription(p.description); + string fieldName = p.label; + if (string.IsNullOrWhiteSpace(fieldName)) + { + fieldName = NameFromDescription(p.description); + } + else + { + fieldName = LegalizeIdentifier(fieldName); + } if (fieldName != "Empty" && fieldName != "Reserved") { impl.WriteLine(" param{0} = {1};", i, unique.Add(fieldName)); @@ -116,7 +133,15 @@ private void GenerateCommands() foreach (var p in cmd.parameters) { i++; - string fieldName = NameFromDescription(p.description); + string fieldName = p.label; + if (string.IsNullOrWhiteSpace(fieldName)) + { + fieldName = NameFromDescription(p.description); + } + else + { + fieldName = LegalizeIdentifier(fieldName); + } if (fieldName != "Empty" && fieldName != "Reserved") { impl.WriteLine(" {1} = param{0};", i, unique.Add(fieldName)); @@ -152,7 +177,6 @@ private string CamelCase(string name) private string NameFromDescription(string description) { - StringBuilder sb = new StringBuilder(); int i = description.IndexOf("this sets"); if (i > 0) { @@ -163,23 +187,24 @@ private string NameFromDescription(string description) { description = description.Substring(0, i); } - string[] words = description.Split(whitespace, StringSplitOptions.RemoveEmptyEntries); + return LegalizeIdentifier(description); + } + + private string LegalizeIdentifier(string identifier) + { + StringBuilder sb = new StringBuilder(); + + string[] words = identifier.Split(whitespace, StringSplitOptions.RemoveEmptyEntries); int count = 0; for (int j = 0; j < words.Length; j++) { string w = words[j]; - w = w.Replace("'", ""); - w = w.Replace("/", ""); - w = w.Replace("?", ""); + w = RemoveIllegalNameChars(w); w = CamelCase(w); if (j > 0 && (w == "In" || w == "From" || w == "And" || w == "As" || w == "The" || w == "And" || w == "It" || w == "On")) { continue; // skip filler words } - if (j == 0 && Char.IsDigit(w[0])) - { - w = "p" + w; // make it a legal C++ identifier. - } sb.Append(w); count++; if (count == 3) @@ -187,6 +212,32 @@ private string NameFromDescription(string description) break; } } + if (sb.Length == 0) + { + sb.Append("p"); + } + return sb.ToString(); + } + + private string RemoveIllegalNameChars(string w) + { + StringBuilder sb = new StringBuilder(); + for (int i = 0, n = w.Length; i < n; i++) + { + char ch = w[i]; + if (!char.IsLetter(ch) && ch != '_') + { + sb.Append("p"); + } + if (char.IsLetterOrDigit(ch) || ch == '_') + { + sb.Append(ch); + } + } + if (sb.Length == 0) + { + sb.Append("p"); + } return sb.ToString(); } @@ -226,9 +277,6 @@ private void GenerateMessages() } } - // mavlink packs the fields by descending size for some odd reason. - m.fields = m.fields.OrderByDescending(x => typeSize[x.type]).ToList(); - int length = m.fields.Count; for (int i = 0; i < length; i++) { @@ -252,6 +300,19 @@ private void GenerateMessages() } } + // mavlink packs the fields in order of descending size (but not including the extension fields. + var sortedFields = m.fields; + int extensionPos = m.ExtensionPos; + if (extensionPos == 0) + { + extensionPos = m.fields.Count; + } + sortedFields = new List(m.fields.Take(extensionPos)); + sortedFields = sortedFields.OrderByDescending(x => typeSize[x.type]).ToList(); + sortedFields.AddRange(m.fields.Skip(extensionPos)); + + m.fields = sortedFields; + header.WriteLine(" virtual std::string toJSon();"); header.WriteLine("protected:"); diff --git a/MavLinkCom/MavLinkComGenerator/MavLinkMessage.cs b/MavLinkCom/MavLinkComGenerator/MavLinkMessage.cs index 2d8fa9cd1..d0f2cc3dd 100644 --- a/MavLinkCom/MavLinkComGenerator/MavLinkMessage.cs +++ b/MavLinkCom/MavLinkComGenerator/MavLinkMessage.cs @@ -46,6 +46,10 @@ public class MavParam [XmlText] public string description { get; set; } + + [XmlAttribute] + public string label { get; set; } + } public class MavEnumEntry @@ -97,6 +101,8 @@ public class MavMessage [XmlElement(ElementName ="field")] public List fields { get; set; } + public int ExtensionPos { get; set; } + public MavMessage() { } } } diff --git a/MavLinkCom/MavLinkComGenerator/MavlinkParser.cs b/MavLinkCom/MavLinkComGenerator/MavlinkParser.cs index 5277adbc7..676e6ee25 100644 --- a/MavLinkCom/MavLinkComGenerator/MavlinkParser.cs +++ b/MavLinkCom/MavLinkComGenerator/MavlinkParser.cs @@ -7,6 +7,7 @@ using System.Linq; using System.Text; using System.Threading.Tasks; +using System.Xml.Linq; using System.Xml.Serialization; namespace MavLinkComGenerator @@ -19,6 +20,32 @@ public static MavLink Parse(string xmlFile) { XmlSerializer s = new XmlSerializer(typeof(MavLink)); MavLink mavlink = (MavLink)s.Deserialize(fs); + + // unfortunately the sections in each is not something the XmlSerializer can handle, + // so we have to post-process the MavLink tree with this additional information. + fs.Seek(0, SeekOrigin.Begin); + XDocument doc = XDocument.Load(fs); + foreach (XElement message in doc.Root.Element("messages").Elements("message")) + { + string id = (string)message.Attribute("id"); + MavMessage msg = (from m in mavlink.messages where m.id == id select m).FirstOrDefault(); + int count = 0; + foreach (XElement child in message.Elements()) + { + string childName = child.Name.LocalName; + if (childName == "field") + { + count++; + } + else if (childName == "extensions") + { + msg.ExtensionPos = count; + break; + } + } + } + + return mavlink; } } diff --git a/MavLinkCom/MavLinkComGenerator/regen.cmd b/MavLinkCom/MavLinkComGenerator/regen.cmd new file mode 100644 index 000000000..1e6dd5bbf --- /dev/null +++ b/MavLinkCom/MavLinkComGenerator/regen.cmd @@ -0,0 +1,10 @@ +@echo off +set SCRIPT_PATH=%~dp0 +cd %~dp0 +msbuild MavLinkComGenerator.csproj + +bin\Debug\MavLinkComGenerator.exe -xml:%SCRIPT_PATH%\..\mavlink\message_definitions\common.xml -out:%SCRIPT_PATH%\..\include +copy ..\include\MavLinkMessages.cpp ..\src +del ..\include\MavLinkMessages.cpp + +goto :eof diff --git a/MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj b/MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj index e5dd5fafe..ac2742a22 100644 --- a/MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj +++ b/MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj @@ -1,5 +1,5 @@  - + diff --git a/MavLinkCom/MavLinkMoCap/Readme.md b/MavLinkCom/MavLinkMoCap/Readme.md index cb5924778..073408f79 100644 --- a/MavLinkCom/MavLinkMoCap/Readme.md +++ b/MavLinkCom/MavLinkMoCap/Readme.md @@ -1,30 +1,3 @@ # Welcome to MavLinkMoCap -This folder contains the MavLinkMoCap library which connects to a OptiTrack camera system -for accurate indoor location. - -## Dependencies: -* [OptiTrack Motive](http://www.optitrack.com/products/motive/). -* [MavLinkCom](../MavLinkCom). - -### Setup RigidBody - -First you need to define a RigidBody named 'Quadrocopter' using Motive. -See [Rigid_Body_Tracking](http://wiki.optitrack.com/index.php?title=Rigid_Body_Tracking). - -### MavLinkTest - -Use MavLinkTest to talk to your PX4 drone, with "-server:addr:port", for example, when connected -to drone wifi use: - - MavLinkMoCap -server:10.42.0.228:14590 "-project:D:\OptiTrack\Motive Project 2016-12-19 04.09.42 PM.ttp" - -This publishes the ATT_POS_MOCAP messages and you can proxy those through to the PX4 by running -MavLinkTest on the dronebrain using: - - MavLinkTest -serial:/dev/ttyACM0,115200 -proxy:10.42.0.228:14590 - -Now the drone will get the ATT_POS_MOCAP and you should see the light turn green meaning it is -now has a home position and is ready to fly. - - +This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/mavlinkcom_mocap.md). \ No newline at end of file diff --git a/MavLinkCom/MavLinkTest/Commands.cpp b/MavLinkCom/MavLinkTest/Commands.cpp index 90a630480..7ccc29153 100644 --- a/MavLinkCom/MavLinkTest/Commands.cpp +++ b/MavLinkCom/MavLinkTest/Commands.cpp @@ -11,19 +11,12 @@ #include "FileSystem.hpp" #include "Utils.hpp" -#if defined(_WIN32) || ((defined __cplusplus) && (__cplusplus >= 201700L)) #include -#define USE_CPP_FILESYSTEM -#else -#undef USE_CPP_FILESYSTEM -#endif - +using namespace std::filesystem; using namespace mavlink_utils; using namespace mavlinkcom; -// from main.cpp. - static std::vector const * all_commands_; std::vector const * Command::getAllCommand() { return all_commands_; } void Command::setAllCommand(std::vector const * all_commands) { all_commands_ = all_commands; } @@ -466,15 +459,6 @@ void DumpLogCommandsCommand::processLogCommands(MavLinkFileLog& log, const std:: void DumpLogCommandsCommand::Execute(std::shared_ptr com) { unused(com); -//TODO: make below future proof (i.e. usable by C++17 compiler) - also change same in main.cpp -#if defined(USE_CPP_FILESYSTEM) -//can't use experimental stuff on Linux because of potential ABI issues -#if defined(_WIN32) || ((defined __cplusplus) && (__cplusplus < 201700L)) - using namespace std::experimental::filesystem; -#else - using namespace std::filesystem; -#endif - path dirPath(log_folder_); for (directory_iterator next(dirPath), end; next != end; ++next) { @@ -491,9 +475,6 @@ void DumpLogCommandsCommand::Execute(std::shared_ptr com) } printf("dumplogcommands is done\n"); -#else - printf("dumplogcommands is available only with C++17 features\n"); -#endif } diff --git a/MavLinkCom/MavLinkTest/Commands.h b/MavLinkCom/MavLinkTest/Commands.h index 3113f60ed..c05f51a6e 100644 --- a/MavLinkCom/MavLinkTest/Commands.h +++ b/MavLinkCom/MavLinkTest/Commands.h @@ -691,7 +691,7 @@ class PidController proportionalGain = error * kProportional_; } if (kDerivative_ != 0) { - float derivative = (error - previous_error_) / dt; + float derivative = dt > 0 ? (error - previous_error_) / dt : 0; derivativeGain = derivative * kDerivative_; } if (kIntegral_ != 0) { diff --git a/MavLinkCom/MavLinkTest/MavLinkTest.vcxproj b/MavLinkCom/MavLinkTest/MavLinkTest.vcxproj index f36a5cae3..65fd58440 100644 --- a/MavLinkCom/MavLinkTest/MavLinkTest.vcxproj +++ b/MavLinkCom/MavLinkTest/MavLinkTest.vcxproj @@ -1,5 +1,5 @@  - + @@ -28,7 +28,7 @@ - v141 + v142 {25EB67BE-468A-4AA5-910F-07EFD58C5516} @@ -134,6 +134,7 @@ MultiThreadedDebugDLL true true + stdcpp17 Console @@ -183,6 +184,7 @@ MultiThreadedDebugDLL true true + stdcpp17 Console @@ -208,6 +210,7 @@ ../include;../common_utils; MultiThreadedDLL true + stdcpp17 Console @@ -256,6 +259,7 @@ MultiThreadedDLL true true + stdcpp17 Console diff --git a/MavLinkCom/MavLinkTest/UnitTests.cpp b/MavLinkCom/MavLinkTest/UnitTests.cpp index eabf9d48c..a5e0deb8e 100644 --- a/MavLinkCom/MavLinkTest/UnitTests.cpp +++ b/MavLinkCom/MavLinkTest/UnitTests.cpp @@ -287,7 +287,7 @@ void UnitTests::FtpTest() { const char* TestPattern = "This is line %d\n"; for (int i = 0; i < 100; i++) { - std::string line = Utils::stringf(TestPattern, i); + std::string line = Utils::stringf("%s %i", TestPattern, i); stream << line; } @@ -331,7 +331,7 @@ void UnitTests::FtpTest() { std::getline(istream, line); while (line.size() > 0) { line += '\n'; - std::string expected = Utils::stringf(TestPattern, count); + std::string expected = Utils::stringf("%s %i", TestPattern, count); if (line != expected) { throw std::runtime_error(Utils::stringf("ftp local file contains unexpected contents '%s' on line %d\n", line.c_str(), count)); diff --git a/MavLinkCom/MavLinkTest/main.cpp b/MavLinkCom/MavLinkTest/main.cpp index 23b5dd0ef..bec3091b3 100644 --- a/MavLinkCom/MavLinkTest/main.cpp +++ b/MavLinkCom/MavLinkTest/main.cpp @@ -22,12 +22,8 @@ STRICT_MODE_OFF STRICT_MODE_ON #include "UnitTests.h" -#if defined(_WIN32) || ((defined __cplusplus) && (__cplusplus >= 201700L)) #include -#define USE_CPP_FILESYSTEM -#else -#undef USE_CPP_FILESYSTEM -#endif +using namespace std::filesystem; /* enable math defines on Windows */ @@ -79,6 +75,7 @@ void DebugOutput(const char* message, ...) { } #else // how do you write to the debug output windows on Unix ? + __attribute__((__format__ (__printf__, 1, 0))) void DebugOutput(const char* message, ...) { va_list args; va_start(args, message); @@ -121,7 +118,7 @@ PortAddress serverEndPoint; bool connectLogViewer = false; PortAddress logViewerEndPoint; -#define DEFAULT_LOGVIEWER_PORT 14570 +#define DEFAULT_LOGVIEWER_PORT 14388 // These are used to echo the mavlink messages to other 3rd party apps like QGC or LogViewer. std::vector proxyEndPoints; @@ -153,15 +150,6 @@ std::shared_ptr logConnection; std::shared_ptr mavLinkVehicle; -#if defined(USE_CPP_FILESYSTEM) - -//can't use experimental stuff on Linux because of potential ABI issues -#if defined(_WIN32) || ((defined __cplusplus) && (__cplusplus < 201700L)) -using namespace std::experimental::filesystem; -#else -using namespace std::filesystem; -#endif - void ConvertLogFileToJson(std::string logFile) { std::string fullPath = FileSystem::getFullPath(logFile); @@ -393,9 +381,6 @@ void ConvertLogFilesToCsv(std::string directory) } } - -#endif - void OpenLogFiles() { if (logDirectory.size() > 0) { @@ -1033,7 +1018,10 @@ std::string findPixhawk() { if (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId) { printf("Auto Selecting COM port: %S\n", info.displayName.c_str()); - return std::string(info.portName.begin(), info.portName.end()); + + std::wstring_convert, wchar_t> converter; + std::string portName_str = converter.to_bytes(info.portName); + return portName_str; } } } @@ -1068,7 +1056,7 @@ std::shared_ptr connectServer(const PortAddress& endPoint, st void runTelemetry() { while (telemetry) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::seconds(1)); if (droneConnection != nullptr) { MavLinkTelemetry tel; tel.wifiInterfaceName = ifaceName.c_str(); @@ -1122,13 +1110,6 @@ bool connect() usedPorts.push_back(offboardEndPoint); } - if (verbose) { - droneConnection->subscribe([=](std::shared_ptr con, const MavLinkMessage& msg) { - printf("Received msg %d from drone\n", static_cast(msg.msgid)); - }); - } - - if (server) { if (serverEndPoint.addr == "") { @@ -1154,6 +1135,12 @@ bool connect() return false; } + if (verbose) { + droneConnection->subscribe([=](std::shared_ptr con, const MavLinkMessage& msg) { + printf("Received msg %d from drone\n", static_cast(msg.msgid)); + }); + } + if (outLogFile != nullptr) { droneConnection->startLoggingSendMessage(outLogFile); } diff --git a/MavLinkCom/README.md b/MavLinkCom/README.md index bcd0d466d..6bd67cea3 100644 --- a/MavLinkCom/README.md +++ b/MavLinkCom/README.md @@ -1,220 +1,3 @@ # Welcome to MavLinkCom -MavLinkCom is a cross-platform C++ library that helps connect to and communicate with [MavLink](https://github.com/mavlink/mavlink) based vehicles. -Specifically this library is designed to work well with [PX4](https://github.com/PX4/Firmware) based drones. - -## Design - -You can view and edit the [Design.dgml](Design/Design.dgml) diagram in Visual Studio. -![Overview](Design/images/overview.png) - -The following are the most important classes in this library. - -### MavLinkNode - -This is the base class for all MavLinkNodes (subclasses include MavLinkVehicle, MavLinkVideoClient and MavLinkVideoServer). -The node connects to your mavlink enabled vehicle via a MavLinkConnection and provides methods for sending MavLinkMessages and MavLinkCommands -and for subscribing to receive messages. This base class also stores the local system id and component id your app wants to use to identify -itself to your remove vehicle. You can also call startHeartbeat to send regular heartbeat messages to keep the connection alive. - -### MavLinkMessage - -This is the encoded MavLinkMessage. For those who have used the mavlink.h C API, this is similar to mavlink_message_t. You do -not create these manually, they are encoded from a strongly typed MavLinkMessageBase subclass. - -### Strongly typed message and command classes - -The MavLinkComGenerator parses the mavlink common.xml message definitions and generates all the MavLink* MavLinkMessageBase subclasses -as well as a bunch of handy mavlink enums and a bunch of strongly typed MavLinkCommand subclasses. - -### MavLinkMessageBase - -This is the base class for a set of strongly typed message classes that are code generated by the MavLinkComGenerator project. -This replaces the C messages defined in the mavlink C API and provides a slightly more object oriented way to send and receive messages -via sendMessage on MavLinkNode. These classes have encode/decode methods that convert to and from the MavLinkMessage class. - -### MavLinkCommand - -This is the base class for a set of strongly typed command classes that are code generated by the MavLinkComGenerator project. -This replaces the C definitions defined in the mavlink C API and provides a more object oriented way to send commands via the sendCommand -method on MavLinkNode. The MavLinkNode takes care of turning these into the underlying mavlink COMMAND_LONG message. - -### MavLinkConnection - -This class provides static helper methods for creating connections to remote MavLink nodes, over serial ports, as well as UDP, or TCP sockets. -This class provides a way to subscribe to receive messages from that node in a pub/sub way so you can have multiple subscribers on the -same connection. MavLinkVehicle uses this to track various messages that define the overall vehicle state. - -### MavLinkVehicle - -MavLinkVehicle is a MavLinkNode that tracks various messages that define the overall vehicle state and provides a VehicleState struct -containing a snapshot of that state, including home position, current orientation, local position, global position, and so on. -This class also provides a bunch of helper methods that wrap commonly used commands providing simple method calls to do things like -arm, disarm, takeoff, land, go to a local coordinate, and fly under offbaord control either by position or velocity control. - -### MavLinkTcpServer - -This helper class provides a way to setup a "server" that accepts MavLinkConnections from remote nodes. You can use this class -to get a connection that you can then give to MavLinkVideoServer to serve images over MavLink. - -### MavLinkFtpClient - -This helper class takes a given MavLinkConnection and provides FTP client support for the MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL -for vehicles that support the FTP capability. This class provides simple methods to list directory contents, and the get and put -files. - -### MavLinkVideoClient - -This helper class takes a given MavLinkConnection and provides helper methods for requesting video from remote node and -packaging up the MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE and MAVLINK_MSG_ID_ENCAPSULATED_DATA messages into simple to use -MavLinkVideoFrames. - -### MavLinkVideoServer - -This helper class takes a given MavLinkConnection and provides the server side of the MavLinkVideoClient protocol, including helper methods -for notifying when there is a video request to process (hasVideoRequest) and a method to send video frames (sendFrame) which -will generate the right MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE and MAVLINK_MSG_ID_ENCAPSULATED_DATA sequence. - -## Examples - -The following code from the UnitTest project shows how to connect to a [Pixhawk](http://www.pixhawk.org/) flight controller over USB serial port, -then wait for the first heartbeat message to be received: - - auto connection = MavLinkConnection::connectSerial("drone", "/dev/ttyACM0", 115200, "sh /etc/init.d/rc.usb\n"); - MavLinkHeartbeat heartbeat; - if (!waitForHeartbeat(10000, heartbeat)) { - throw std::runtime_error("Received no heartbeat from PX4 after 10 seconds"); - } - - -The following code connects to serial port, and then forwards all messages to and from QGroundControl to that drone using another connection -that is joined to the drone stream. - - auto droneConnection = MavLinkConnection::connectSerial("drone", "/dev/ttyACM0", 115200, "sh /etc/init.d/rc.usb\n"); - auto proxyConnection = MavLinkConnection::connectRemoteUdp("qgc", "127.0.0.1", "127.0.0.1", 14550); - droneConnection->join(proxyConnection); - -The following code then takes that connection and turns on heartBeats and starts tracking vehicle information using local -system id 166 and component id 1. - - auto vehicle = std::make_shared(166, 1); - vehicle->connect(connection); - vehicle->startHeartbeat(); - - std::this_thread::sleep_for(std::chrono::seconds(5)); - - VehicleState state = vehicle->getVehicleState(); - printf("Home position is %s, %f,%f,%f\n", state.home.is_set ? "set" : "not set", - state.home.global_pos.lat, state.home.global_pos.lon, state.home.global_pos.alt); - -The following code uses the vehicle object to arm the drone and take off and wait for the takeoff altitude to be reached: - - bool rc = false; - if (!vehicle->armDisarm(true).wait(3000, &rc) || !rc) { - printf("arm command failed\n"); - return; - } - if (!vehicle->takeoff(targetAlt).wait(3000, &rc) || !rc) { - printf("takeoff command failed\n"); - return; - } - int version = vehicle->getVehicleStateVersion(); - while (true) { - int newVersion = vehicle->getVehicleStateVersion(); - if (version != newVersion) { - VehicleState state = vehicle->getVehicleState(); - float alt = state.local_est.pos.z; - if (alt >= targetAlt - delta && alt <= targetAlt + delta) - { - reached = true; - printf("Target altitude reached\n"); - break; - } - } else { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } - -The following code uses offboard control to make the drone fly in a circle with camera pointed at the center. -Here we use the subscribe method to check each new local position message to indicate so we can compute the new -velocity vector as soon as that new position is received. We request a high rate for those messages using -setMessageInterval to ensure smooth circular orbit. - - vehicle->setMessageInterval((int)MavLinkMessageIds::MAVLINK_MSG_ID_LOCAL_POSITION_NED, 30); - vehicle->requestControl(); - int subscription = vehicle->getConnection()->subscribe( - [&](std::shared_ptr connection, const MavLinkMessage& m) { - if (m.msgid == (int)MavLinkMessageIds::MAVLINK_MSG_ID_LOCAL_POSITION_NED) - { - float x = localPos.x; - float y = localPos.y; - float dx = x - cx; - float dy = y - cy; - float angle = atan2(dy, dx); - if (angle < 0) angle += M_PI * 2; - float tangent = angle + M_PI_2; - double newvx = orbitSpeed * cos(tangent); - double newvy = orbitSpeed * sin(tangent); - float heading = angle + M_PI; - vehicle->moveByLocalVelocityWithAltHold(newvx, newvy, altitude, true, heading); - } - }); - -The following code stops flying the drone in offboard mode and tells the drone to loiter at its current location. -This version of the code shows how to use the AsyncResult without blocking on a wait call. - - vehicle->releaseControl(); - if (vehicle->loiter().then([=](bool rc) { - printf("loiter command %s\n", rc ? "succeeded" : "failed"); - } - -The following code gets all configurable parameters from the drone and prints them: - - auto list = vehicle->getParamList(); - auto end = list.end(); - int count = 0; - for (auto iter = list.begin(); iter < end; iter++) - { - count++; - MavLinkParameter p = *iter; - if (p.type == MAV_PARAM_TYPE_REAL32 || p.type == MAV_PARAM_TYPE_REAL64) { - printf("%s=%f\n", p.name.c_str(), p.value); - } - else { - printf("%s=%d\n", p.name.c_str(), static_cast(p.value)); - } - } - -The following code sets a parameter on the Pixhawk to disable the USB safety check (this is handy if you are controlling -the Pixhawk over USB using another onboard computer that is part of the drone itself). You should NOT do this if you -are connecting your PC or laptop to the drone over USB. - - MavLinkParameter p; - p.name = "CBRK_USB_CHK"; - p.value = 197848; - if (!vehicle->setParameter(p).wait(3000,&rc) || !rc) { - printf("Setting the CBRK_USB_CHK failed"); - } - -MavLinkVehicle actually has a helper method for this called allowFlightControlOverUsb, so now you know how it is implemented :-) - -## Advanced Connections - -You can wire up different configurations of mavlink pipelines using the MavLinkConnection class "join" method as shown below. - -Example 1, we connect to PX4 over serial, and proxy those messages through to QGroundControl and the LogViewer who are listening on remote ports. - -![Serial to QGC](Design/images/example1.png) - -Example 2: simulation can talk to jMavSim and jMavSim connects to PX4. jMavSim can also manage multiple connections, so it can talk to unreal simulator. -Another MavLinkConnection can be joined to proxy connections that jMavSim doesn't support, like the LogViewer or a remote camera node. - -![Serial to QGC](Design/images/example2.png) - -Example 3: we use MavLinkConnection to connect to PX4 over serial, then join additional connections for all our remote nodes including jMavSim. - -![Serial to QGC](Design/images/example3.png) - -Example 4: We can also do distributed systems to control the drone remotely: - -![Serial to QGC](Design/images/example4.png) +This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/mavlinkcom.md). \ No newline at end of file diff --git a/MavLinkCom/common_utils/MedianFilter.hpp b/MavLinkCom/common_utils/MedianFilter.hpp deleted file mode 100644 index d9625745c..000000000 --- a/MavLinkCom/common_utils/MedianFilter.hpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef common_utils_MedianFilter_hpp -#define common_utils_MedianFilter_hpp - -#include -#include -#include - -namespace mavlink_utils { - -template -class MedianFilter { - private: - std::vector buffer_, buffer_copy_; - int window_size_, window_size_2x_, window_size_half_; - float outlier_factor_; - int buffer_index_; - public: - MedianFilter(); - MedianFilter(int window_size, float outlier_factor); - void initialize(int window_size, float outlier_factor); - std::tuple filter(T value); -}; - -template -void MedianFilter::initialize(int window_size, float outlier_factor) { - buffer_.resize(window_size); - buffer_copy_.resize(window_size); - window_size_ = window_size; - window_size_2x_ = window_size_ * 2; - window_size_half_ = window_size_ / 2; - outlier_factor_ = outlier_factor; - buffer_index_ = 0; -} - -template -MedianFilter::MedianFilter() { - initialize(buffer_.capacity(), std::numeric_limits::infinity()); -} - -template -MedianFilter::MedianFilter(int window_size, float outlier_factor) { - initialize(window_size, std::numeric_limits::infinity()); -} - -template -std::tuple MedianFilter::filter(T value){ - buffer_[buffer_index_++ % window_size_] = value; - if (buffer_index_ == window_size_2x_) - buffer_index_ = window_size_; - - if (buffer_index_ >= window_size_) { - //find median - for(auto i = 0; i < window_size_; ++i) - buffer_copy_[i] = buffer_[i]; - std::sort(buffer_copy_.begin(), buffer_copy_.end()); - double median = buffer_copy_[window_size_half_]; - - //average values that fall between upper and lower bound of median - auto lower_bound = median - median * outlier_factor_, upper_bound = median + median * outlier_factor_; - double sum = 0; int count = 0; - for(auto i = 0; i < window_size_; ++i) { - if (buffer_copy_[i] >= lower_bound && buffer_copy_[i] <= upper_bound) { - sum += buffer_copy_[i]; - ++count; - } - } - double mean = sum / count; - - double std_dev_sum = 0; - for(auto i = 0; i < window_size_; ++i) { - if (buffer_copy_[i] >= lower_bound && buffer_copy_[i] <= upper_bound) { - double diff = buffer_copy_[i] - mean; - sum += diff*diff; - } - } - double variance = std_dev_sum / count; - - return std::make_tuple(mean, variance); - } - else { - //window is not full, return the input as-is - //TODO: use growing window here - return std::make_tuple(double(value), 0); - } -} - - -} //namespace -#endif diff --git a/MavLinkCom/common_utils/ThreadUtils.cpp b/MavLinkCom/common_utils/ThreadUtils.cpp index ecac8b380..ea4eb3570 100644 --- a/MavLinkCom/common_utils/ThreadUtils.cpp +++ b/MavLinkCom/common_utils/ThreadUtils.cpp @@ -1,5 +1,6 @@ #include "ThreadUtils.hpp" - +#include "StrictMode.hpp" +#include #ifdef _WIN32 #include // SetThreadPriority and GetCurrentThread @@ -34,3 +35,38 @@ bool CurrentThread::setMaximumPriority() return err == 0; #endif } + +#ifdef _WIN32 +typedef HRESULT (WINAPI *SetThreadDescriptionFunction)( _In_ HANDLE hThread, _In_ PCWSTR lpThreadDescription); +static SetThreadDescriptionFunction setThreadDescriptionFunction = nullptr; +#endif + +// setThreadName is a helper function that is useful when debugging because your threads +// show up in the debugger with the name you set which makes it easier to find the threads +// that you are interested in. +bool CurrentThread::setThreadName(const std::string& name) +{ +#ifdef _WIN32 + // unfortunately this is only available on Windows 10, and AirSim is not limited to that. + if (setThreadDescriptionFunction == nullptr) { + HINSTANCE hGetProcIDDLL = LoadLibrary(L"Kernel32"); + FARPROC func = GetProcAddress(hGetProcIDDLL, "SetThreadDescription"); + if (func != nullptr) + { + setThreadDescriptionFunction = (SetThreadDescriptionFunction)func; + } + } + if (setThreadDescriptionFunction != nullptr) { + std::wstring_convert, wchar_t> converter; + std::wstring wide_path = converter.from_bytes(name); + auto rc = (*setThreadDescriptionFunction)(GetCurrentThread(), wide_path.c_str()); + return S_OK == rc; + } + return false; +#elif defined(__APPLE__) + return 0 == pthread_setname_np(name.c_str()); +#else + return 0 == pthread_setname_np(pthread_self(), name.c_str()); +#endif + +} diff --git a/MavLinkCom/common_utils/ThreadUtils.hpp b/MavLinkCom/common_utils/ThreadUtils.hpp index 9e43e545b..97cfedbac 100644 --- a/MavLinkCom/common_utils/ThreadUtils.hpp +++ b/MavLinkCom/common_utils/ThreadUtils.hpp @@ -2,6 +2,7 @@ #define MavLinkCom_HighPriorityThread_hpp #include +#include namespace mavlink_utils { @@ -11,6 +12,9 @@ namespace mavlink_utils { // make the current thread run with maximum priority. static bool setMaximumPriority(); + // set a nice name on the current thread which aids in debugging. + static bool setThreadName(const std::string& name); + }; } diff --git a/MavLinkCom/common_utils/Utils.hpp b/MavLinkCom/common_utils/Utils.hpp index 6367b28e1..66f27b6d3 100644 --- a/MavLinkCom/common_utils/Utils.hpp +++ b/MavLinkCom/common_utils/Utils.hpp @@ -65,6 +65,7 @@ using std::experimental::optional; */ #ifndef _MSC_VER +__attribute__((__format__ (__printf__, 1, 0))) static int _vscprintf(const char * format, va_list pargs) { int retval; @@ -214,20 +215,9 @@ class Utils { return str.substr(i, len - i); } - static string formatNumber(double number, int digits_after_decimal = -1, int digits_before_decimal = -1, bool sign_always = false) - { - std::string format_string = "%"; - if (sign_always) - format_string += "+"; - if (digits_before_decimal >= 0) - format_string += "0" + std::to_string(digits_before_decimal + std::max(digits_after_decimal, 0) + 1); - if (digits_after_decimal >= 0) - format_string += "." + std::to_string(digits_after_decimal); - format_string += "f"; - - return stringf(format_string.c_str(), number); - } - + #ifndef _MSC_VER + __attribute__((__format__ (__printf__, 1, 0))) + #endif static string stringf(const char* format, ...) { va_list args; diff --git a/MavLinkCom/include/MavLinkConnection.hpp b/MavLinkCom/include/MavLinkConnection.hpp index c1b270bd1..6dc7cd3e0 100644 --- a/MavLinkCom/include/MavLinkConnection.hpp +++ b/MavLinkCom/include/MavLinkConnection.hpp @@ -85,6 +85,12 @@ namespace mavlinkcom { // NIC to use, for example, wifi versus hard wired ethernet adapter. For localhost pass 127.0.0.1. static std::shared_ptr connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort); + // This method accepts one tcp connection from a remote host on a given port. + // You may need to open this port in your firewall. + // The localAddr can also a specific local ip address if you need to specify which + // NIC to use, for example, wifi versus hard wired ethernet adapter. For localhost pass 127.0.0.1. + void acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort); + // instance methods std::string getName(); int getTargetComponentId(); @@ -127,6 +133,10 @@ namespace mavlinkcom { // message signing according to the target node we are communicating with, and return the message length. int prepareForSending(MavLinkMessage& msg); + // Returns true if we are on the publishing thread. Certain blocing operations that wait for messages from mavlin vehicle are not + // allowed on this thread. + bool isPublishThread() const; + protected: void startListening(const std::string& nodeName, std::shared_ptr connectedPort); diff --git a/MavLinkCom/include/MavLinkMessages.hpp b/MavLinkCom/include/MavLinkMessages.hpp index d00f16d03..2287754e8 100644 --- a/MavLinkCom/include/MavLinkMessages.hpp +++ b/MavLinkCom/include/MavLinkMessages.hpp @@ -18,6 +18,7 @@ enum class MavLinkMessageIds { MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL = 5, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK = 6, MAVLINK_MSG_ID_AUTH_KEY = 7, + MAVLINK_MSG_ID_LINK_NODE_STATUS = 8, MAVLINK_MSG_ID_SET_MODE = 11, MAVLINK_MSG_ID_PARAM_REQUEST_READ = 20, MAVLINK_MSG_ID_PARAM_REQUEST_LIST = 21, @@ -51,6 +52,7 @@ enum class MavLinkMessageIds { MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN = 49, MAVLINK_MSG_ID_PARAM_MAP_RC = 50, MAVLINK_MSG_ID_MISSION_REQUEST_INT = 51, + MAVLINK_MSG_ID_MISSION_CHANGED = 52, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA = 54, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA = 55, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV = 61, @@ -128,11 +130,13 @@ enum class MavLinkMessageIds { MAVLINK_MSG_ID_BATTERY_STATUS = 147, MAVLINK_MSG_ID_AUTOPILOT_VERSION = 148, MAVLINK_MSG_ID_LANDING_TARGET = 149, + MAVLINK_MSG_ID_FENCE_STATUS = 162, MAVLINK_MSG_ID_ESTIMATOR_STATUS = 230, MAVLINK_MSG_ID_WIND_COV = 231, MAVLINK_MSG_ID_GPS_INPUT = 232, MAVLINK_MSG_ID_GPS_RTCM_DATA = 233, MAVLINK_MSG_ID_HIGH_LATENCY = 234, + MAVLINK_MSG_ID_HIGH_LATENCY2 = 235, MAVLINK_MSG_ID_VIBRATION = 241, MAVLINK_MSG_ID_HOME_POSITION = 242, MAVLINK_MSG_ID_SET_HOME_POSITION = 243, @@ -146,8 +150,7 @@ enum class MavLinkMessageIds { MAVLINK_MSG_ID_NAMED_VALUE_FLOAT = 251, MAVLINK_MSG_ID_NAMED_VALUE_INT = 252, MAVLINK_MSG_ID_STATUSTEXT = 253, - MAVLINK_MSG_ID_DEBUG = 254, - MAVLINK_MSG_ID_PROTOCOL_VERSION = 300 + MAVLINK_MSG_ID_DEBUG = 254 }; // Micro air vehicle / autopilot classes. This identifies the individual model. enum class MAV_AUTOPILOT { @@ -157,7 +160,7 @@ enum class MAV_AUTOPILOT { MAV_AUTOPILOT_RESERVED = 1, // SLUGS autopilot, http://slugsuav.soe.ucsc.edu MAV_AUTOPILOT_SLUGS = 2, - // ArduPilotMega / ArduCopter, http://diydrones.com + // ArduPilot - Plane/Copter/Rover/Sub/Tracker, http://ardupilot.org MAV_AUTOPILOT_ARDUPILOTMEGA = 3, // OpenPilot, http://openpilot.org MAV_AUTOPILOT_OPENPILOT = 4, @@ -175,7 +178,7 @@ enum class MAV_AUTOPILOT { MAV_AUTOPILOT_UDB = 10, // FlexiPilot MAV_AUTOPILOT_FP = 11, - // PX4 Autopilot - http://pixhawk.ethz.ch/px4/ + // PX4 Autopilot - http://px4.io/ MAV_AUTOPILOT_PX4 = 12, // SMACCMPilot - http://smaccmpilot.org MAV_AUTOPILOT_SMACCMPILOT = 13, @@ -186,11 +189,19 @@ enum class MAV_AUTOPILOT { // Aerob -- http://aerob.ru MAV_AUTOPILOT_AEROB = 16, // ASLUAV autopilot -- http://www.asl.ethz.ch - MAV_AUTOPILOT_ASLUAV = 17 + MAV_AUTOPILOT_ASLUAV = 17, + // SmartAP Autopilot - http://sky-drones.com + MAV_AUTOPILOT_SMARTAP = 18, + // AirRails - http://uaventure.com + MAV_AUTOPILOT_AIRRAILS = 19 }; +// MAVLINK component type reported in HEARTBEAT message. Flight controllers must report +// the type of the vehicle on which they are mounted (e.g. MAV_TYPE_OCTOROTOR). All +// other components must report a value appropriate for their type (e.g. a camera +// must use MAV_TYPE_CAMERA). enum class MAV_TYPE { - // Generic micro air vehicle. + // Generic micro air vehicle MAV_TYPE_GENERIC = 0, // Fixed wing aircraft. MAV_TYPE_FIXED_WING = 1, @@ -242,10 +253,22 @@ enum class MAV_TYPE { MAV_TYPE_VTOL_RESERVED4 = 24, // VTOL reserved 5 MAV_TYPE_VTOL_RESERVED5 = 25, - // Onboard gimbal + // Gimbal MAV_TYPE_GIMBAL = 26, - // Onboard ADSB peripheral - MAV_TYPE_ADSB = 27 + // ADSB system + MAV_TYPE_ADSB = 27, + // Steerable, nonrigid airfoil + MAV_TYPE_PARAFOIL = 28, + // Dodecarotor + MAV_TYPE_DODECAROTOR = 29, + // Camera + MAV_TYPE_CAMERA = 30, + // Charging station + MAV_TYPE_CHARGING_STATION = 31, + // FLARM collision avoidance system + MAV_TYPE_FLARM = 32, + // Servo + MAV_TYPE_SERVO = 33 }; // These values define the type of firmware release. These values indicate the first @@ -264,6 +287,38 @@ enum class FIRMWARE_VERSION_TYPE { FIRMWARE_VERSION_TYPE_OFFICIAL = 255 }; +// Flags to report failure cases over the high latency telemtry. +enum class HL_FAILURE_FLAG { + // GPS failure. + HL_FAILURE_FLAG_GPS = 1, + // Differential pressure sensor failure. + HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE = 2, + // Absolute pressure sensor failure. + HL_FAILURE_FLAG_ABSOLUTE_PRESSURE = 4, + // Accelerometer sensor failure. + HL_FAILURE_FLAG_3D_ACCEL = 8, + // Gyroscope sensor failure. + HL_FAILURE_FLAG_3D_GYRO = 16, + // Magnetometer sensor failure. + HL_FAILURE_FLAG_3D_MAG = 32, + // Terrain subsystem failure. + HL_FAILURE_FLAG_TERRAIN = 64, + // Battery failure/critical low battery. + HL_FAILURE_FLAG_BATTERY = 128, + // RC receiver failure/no rc connection. + HL_FAILURE_FLAG_RC_RECEIVER = 256, + // Offboard link failure. + HL_FAILURE_FLAG_OFFBOARD_LINK = 512, + // Engine failure. + HL_FAILURE_FLAG_ENGINE = 1024, + // Geofence violation. + HL_FAILURE_FLAG_GEOFENCE = 2048, + // Estimator failure, for example measurement rejection or large variances. + HL_FAILURE_FLAG_ESTIMATOR = 4096, + // Mission failure. + HL_FAILURE_FLAG_MISSION = 8192 +}; + // These flags encode the MAV mode. enum class MAV_MODE_FLAG { // 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. @@ -279,7 +334,7 @@ enum class MAV_MODE_FLAG { // 0b00010000 system stabilizes electronically its attitude (and optionally position). // It needs however further control inputs to move around. MAV_MODE_FLAG_STABILIZE_ENABLED = 16, - // 0b00001000 guided mode enabled, system flies MISSIONs / mission items. + // 0b00001000 guided mode enabled, system flies waypoints / mission items. MAV_MODE_FLAG_GUIDED_ENABLED = 8, // 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided // flag can be set or not, depends on the actual implementation. @@ -306,7 +361,7 @@ enum class MAV_MODE_FLAG_DECODE_POSITION { MAV_MODE_FLAG_DECODE_POSITION_STABILIZE = 16, // Fifth bit: 00001000 MAV_MODE_FLAG_DECODE_POSITION_GUIDED = 8, - // Sixt bit: 00000100 + // Sixth bit: 00000100 MAV_MODE_FLAG_DECODE_POSITION_AUTO = 4, // Seventh bit: 00000010 MAV_MODE_FLAG_DECODE_POSITION_TEST = 2, @@ -314,7 +369,7 @@ enum class MAV_MODE_FLAG_DECODE_POSITION { MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE = 1 }; -// Override command, pauses current mission execution and moves immediately to a position +// Actions that may be specified in MAV_CMD_OVERRIDE_GOTO to override mission execution. enum class MAV_GOTO { // Hold at the current position. MAV_GOTO_DO_HOLD = 0, @@ -345,10 +400,10 @@ enum class MAV_MODE { // System is allowed to be active, under autonomous control, manual setpoint MAV_MODE_GUIDED_ARMED = 216, // System is allowed to be active, under autonomous control and navigation (the - // trajectory is decided onboard and not pre-programmed by MISSIONs) + // trajectory is decided onboard and not pre-programmed by waypoints) MAV_MODE_AUTO_DISARMED = 92, // System is allowed to be active, under autonomous control and navigation (the - // trajectory is decided onboard and not pre-programmed by MISSIONs) + // trajectory is decided onboard and not pre-programmed by waypoints) MAV_MODE_AUTO_ARMED = 220, // UNDEFINED mode. This solely depends on the autopilot - use with caution, intended // for developers only. @@ -375,45 +430,347 @@ enum class MAV_STATE { // whole airframe. It is in mayday and going down. MAV_STATE_EMERGENCY, // System just initialized its power-down sequence, will shut down now. - MAV_STATE_POWEROFF -}; - + MAV_STATE_POWEROFF, + // System is terminating itself. + MAV_STATE_FLIGHT_TERMINATION +}; + +// Component ids (values) for the different types and instances of onboard hardware/software +// that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance +// systems etc.). Components must use the appropriate ID in their source address when +// sending messages. Components can also use IDs to determine if they are the intended +// recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate +// messages that must be processed by all components. When creating new entries, components +// that can have multiple instances (e.g. cameras, servos etc.) should be allocated +// sequential values. An appropriate number of values should be left free after these +// components to allow the number of instances to be expanded. enum class MAV_COMPONENT { + // Target id (target_component) used to broadcast messages to all components of + // the receiving system. Components should attempt to process messages with this + // component ID and forward to components on any other interfaces. Note: This + // is not a valid *source* component id for a message. MAV_COMP_ID_ALL = 0, - MAV_COMP_ID_GPS = 220, - MAV_COMP_ID_MISSIONPLANNER = 190, - MAV_COMP_ID_PATHPLANNER = 195, - MAV_COMP_ID_MAPPER = 180, + // System flight controller component ("autopilot"). Only one autopilot is expected + // in a particular system. + MAV_COMP_ID_AUTOPILOT1 = 1, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER1 = 25, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER2 = 26, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER3 = 27, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER4 = 28, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER5 = 29, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER6 = 30, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER7 = 31, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER8 = 32, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER9 = 33, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER10 = 34, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER11 = 35, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER12 = 36, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER13 = 37, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER14 = 38, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER15 = 39, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USE16 = 40, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER17 = 41, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER18 = 42, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER19 = 43, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER20 = 44, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER21 = 45, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER22 = 46, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER23 = 47, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER24 = 48, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER25 = 49, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER26 = 50, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER27 = 51, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER28 = 52, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER29 = 53, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER30 = 54, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER31 = 55, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER32 = 56, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER33 = 57, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER34 = 58, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER35 = 59, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER36 = 60, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER37 = 61, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER38 = 62, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER39 = 63, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER40 = 64, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER41 = 65, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER42 = 66, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER43 = 67, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER44 = 68, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER45 = 69, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER46 = 70, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER47 = 71, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER48 = 72, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER49 = 73, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER50 = 74, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER51 = 75, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER52 = 76, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER53 = 77, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER54 = 78, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER55 = 79, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER56 = 80, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER57 = 81, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER58 = 82, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER59 = 83, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER60 = 84, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER61 = 85, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER62 = 86, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER63 = 87, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER64 = 88, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER65 = 89, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER66 = 90, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER67 = 91, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER68 = 92, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER69 = 93, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER70 = 94, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER71 = 95, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER72 = 96, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER73 = 97, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER74 = 98, + // Id for a component on privately managed MAVLink network. Can be used for any + // purpose but may not be published by components outside of the private network. + MAV_COMP_ID_USER75 = 99, + // Camera #1. MAV_COMP_ID_CAMERA = 100, - MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_IMU_2 = 201, - MAV_COMP_ID_IMU_3 = 202, - MAV_COMP_ID_UDP_BRIDGE = 240, - MAV_COMP_ID_UART_BRIDGE = 241, - MAV_COMP_ID_SYSTEM_CONTROL = 250, + // Camera #2. + MAV_COMP_ID_CAMERA2 = 101, + // Camera #3. + MAV_COMP_ID_CAMERA3 = 102, + // Camera #4. + MAV_COMP_ID_CAMERA4 = 103, + // Camera #5. + MAV_COMP_ID_CAMERA5 = 104, + // Camera #6. + MAV_COMP_ID_CAMERA6 = 105, + // Servo #1. MAV_COMP_ID_SERVO1 = 140, + // Servo #2. MAV_COMP_ID_SERVO2 = 141, + // Servo #3. MAV_COMP_ID_SERVO3 = 142, + // Servo #4. MAV_COMP_ID_SERVO4 = 143, + // Servo #5. MAV_COMP_ID_SERVO5 = 144, + // Servo #6. MAV_COMP_ID_SERVO6 = 145, + // Servo #7. MAV_COMP_ID_SERVO7 = 146, + // Servo #8. MAV_COMP_ID_SERVO8 = 147, + // Servo #9. MAV_COMP_ID_SERVO9 = 148, + // Servo #10. MAV_COMP_ID_SERVO10 = 149, + // Servo #11. MAV_COMP_ID_SERVO11 = 150, + // Servo #12. MAV_COMP_ID_SERVO12 = 151, + // Servo #13. MAV_COMP_ID_SERVO13 = 152, + // Servo #14. MAV_COMP_ID_SERVO14 = 153, + // Gimbal #1. MAV_COMP_ID_GIMBAL = 154, + // Logging component. MAV_COMP_ID_LOG = 155, + // Automatic Dependent Surveillance-Broadcast (ADS-B) component. MAV_COMP_ID_ADSB = 156, - // On Screen Display (OSD) devices for video links + // On Screen Display (OSD) devices for video links. MAV_COMP_ID_OSD = 157, // Generic autopilot peripheral component ID. Meant for devices that do not implement - // the parameter sub-protocol + // the parameter microservice. MAV_COMP_ID_PERIPHERAL = 158, - MAV_COMP_ID_QX1_GIMBAL = 159 + // Gimbal ID for QX1. + MAV_COMP_ID_QX1_GIMBAL = 159, + // FLARM collision alert component. + MAV_COMP_ID_FLARM = 160, + // Gimbal #2. + MAV_COMP_ID_GIMBAL2 = 171, + // Gimbal #3. + MAV_COMP_ID_GIMBAL3 = 172, + // Gimbal #4 + MAV_COMP_ID_GIMBAL4 = 173, + // Gimbal #5. + MAV_COMP_ID_GIMBAL5 = 174, + // Gimbal #6. + MAV_COMP_ID_GIMBAL6 = 175, + // Component that can generate/supply a mission flight plan (e.g. GCS or developer + // API). + MAV_COMP_ID_MISSIONPLANNER = 190, + // Component that finds an optimal path between points based on a certain constraint + // (e.g. minimum snap, shortest path, cost, etc.). + MAV_COMP_ID_PATHPLANNER = 195, + // Component that plans a collision free path between two points. + MAV_COMP_ID_OBSTACLE_AVOIDANCE = 196, + // Component that provides position estimates using VIO techniques. + MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197, + // Inertial Measurement Unit (IMU) #1. + MAV_COMP_ID_IMU = 200, + // Inertial Measurement Unit (IMU) #2. + MAV_COMP_ID_IMU_2 = 201, + // Inertial Measurement Unit (IMU) #3. + MAV_COMP_ID_IMU_3 = 202, + // GPS #1. + MAV_COMP_ID_GPS = 220, + // GPS #2. + MAV_COMP_ID_GPS2 = 221, + // Component to bridge MAVLink to UDP (i.e. from a UART). + MAV_COMP_ID_UDP_BRIDGE = 240, + // Component to bridge to UART (i.e. from UDP). + MAV_COMP_ID_UART_BRIDGE = 241, + // Component handling TUNNEL messages (e.g. vendor specific GUI of a component). + MAV_COMP_ID_TUNNEL_NODE = 242, + // Component for handling system messages (e.g. to ARM, takeoff, etc.). + MAV_COMP_ID_SYSTEM_CONTROL = 250 }; // These encode the sensors whose status is sent as part of the SYS_STATUS message. @@ -467,33 +824,38 @@ enum class MAV_SYS_STATUS_SENSOR { // 0x800000 Motors are reversed MAV_SYS_STATUS_REVERSE_MOTOR = 8388608, // 0x1000000 Logging - MAV_SYS_STATUS_LOGGING = 16777216 + MAV_SYS_STATUS_LOGGING = 16777216, + // 0x2000000 Battery + MAV_SYS_STATUS_SENSOR_BATTERY = 33554432, + // 0x4000000 Proximity + MAV_SYS_STATUS_SENSOR_PROXIMITY = 67108864, + // 0x8000000 Satellite Communication + MAV_SYS_STATUS_SENSOR_SATCOM = 134217728 }; enum class MAV_FRAME { - // Global coordinate frame, WGS84 coordinate system. First value / x: latitude, + // Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, // second value / y: longitude, third value / z: positive altitude over mean sea - // level (MSL) + // level (MSL). MAV_FRAME_GLOBAL = 0, - // Local coordinate frame, Z-up (x: north, y: east, z: down). + // Local coordinate frame, Z-down (x: north, y: east, z: down). MAV_FRAME_LOCAL_NED = 1, // NOT a coordinate frame, indicates a mission command. MAV_FRAME_MISSION = 2, - // Global coordinate frame, WGS84 coordinate system, relative altitude over ground - // with respect to the home position. First value / x: latitude, second value - // / y: longitude, third value / z: positive altitude with 0 being at the altitude - // of the home location. + // Global (WGS84) coordinate frame + altitude relative to the home position. First + // value / x: latitude, second value / y: longitude, third value / z: positive + // altitude with 0 being at the altitude of the home location. MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, - // Local coordinate frame, Z-down (x: east, y: north, z: up) + // Local coordinate frame, Z-up (x: east, y: north, z: up). MAV_FRAME_LOCAL_ENU = 4, - // Global coordinate frame, WGS84 coordinate system. First value / x: latitude + // Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude // in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value - // / z: positive altitude over mean sea level (MSL) + // / z: positive altitude over mean sea level (MSL). MAV_FRAME_GLOBAL_INT = 5, - // Global coordinate frame, WGS84 coordinate system, relative altitude over ground - // with respect to the home position. First value / x: latitude in degrees*10e-7, - // second value / y: longitude in degrees*10e-7, third value / z: positive altitude - // with 0 being at the altitude of the home location. + // Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. + // First value / x: latitude in degrees*10e-7, second value / y: longitude in + // degrees*10e-7, third value / z: positive altitude with 0 being at the altitude + // of the home location. MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, // Offset to the current local frame. Anything expressed in this frame should // be added to the current local frame position. @@ -505,18 +867,44 @@ enum class MAV_FRAME { // flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration // to the east. MAV_FRAME_BODY_OFFSET_NED = 9, - // Global coordinate frame with above terrain level altitude. WGS84 coordinate - // system, relative altitude over terrain with respect to the waypoint coordinate. + // Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). // First value / x: latitude in degrees, second value / y: longitude in degrees, // third value / z: positive altitude in meters with 0 being at ground level in // terrain model. MAV_FRAME_GLOBAL_TERRAIN_ALT = 10, - // Global coordinate frame with above terrain level altitude. WGS84 coordinate - // system, relative altitude over terrain with respect to the waypoint coordinate. - // First value / x: latitude in degrees*10e-7, second value / y: longitude in - // degrees*10e-7, third value / z: positive altitude in meters with 0 being at - // ground level in terrain model. - MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + // Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint + // coordinate). First value / x: latitude in degrees*10e-7, second value / y: + // longitude in degrees*10e-7, third value / z: positive altitude in meters with + // 0 being at ground level in terrain model. + MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11, + // Body fixed frame of reference, Z-down (x: forward, y: right, z: down). + MAV_FRAME_BODY_FRD = 12, + // Body fixed frame of reference, Z-up (x: forward, y: left, z: up). + MAV_FRAME_BODY_FLU = 13, + // Odometry local coordinate frame of data given by a motion capture system, Z-down + // (x: north, y: east, z: down). + MAV_FRAME_MOCAP_NED = 14, + // Odometry local coordinate frame of data given by a motion capture system, Z-up + // (x: east, y: north, z: up). + MAV_FRAME_MOCAP_ENU = 15, + // Odometry local coordinate frame of data given by a vision estimation system, + // Z-down (x: north, y: east, z: down). + MAV_FRAME_VISION_NED = 16, + // Odometry local coordinate frame of data given by a vision estimation system, + // Z-up (x: east, y: north, z: up). + MAV_FRAME_VISION_ENU = 17, + // Odometry local coordinate frame of data given by an estimator running onboard + // the vehicle, Z-down (x: north, y: east, z: down). + MAV_FRAME_ESTIM_NED = 18, + // Odometry local coordinate frame of data given by an estimator running onboard + // the vehicle, Z-up (x: east, y: noth, z: up). + MAV_FRAME_ESTIM_ENU = 19, + // Forward, Right, Down coordinate frame. This is a local frame with Z-down and + // arbitrary F/R alignment (i.e. not aligned with NED/earth frame). + MAV_FRAME_LOCAL_FRD = 20, + // Forward, Left, Up coordinate frame. This is a local frame with Z-up and arbitrary + // F/L alignment (i.e. not aligned with ENU/earth frame). + MAV_FRAME_LOCAL_FLU = 21 }; enum class MAVLINK_DATA_STREAM_TYPE { @@ -553,6 +941,16 @@ enum class FENCE_BREACH { FENCE_BREACH_BOUNDARY = 3 }; +// Actions being taken to mitigate/prevent fence breach +enum class FENCE_MITIGATE { + // Unknown + FENCE_MITIGATE_UNKNOWN = 0, + // No actions being taken + FENCE_MITIGATE_NONE = 1, + // Velocity limiting active to prevent breach + FENCE_MITIGATE_VEL_LIMIT = 2 +}; + // Enumeration of possible mount operation modes enum class MAV_MOUNT_MODE { // Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization @@ -567,23 +965,79 @@ enum class MAV_MOUNT_MODE { MAV_MOUNT_MODE_GPS_POINT = 4 }; +// Generalized UAVCAN node health +enum class UAVCAN_NODE_HEALTH { + // The node is functioning properly. + UAVCAN_NODE_HEALTH_OK = 0, + // A critical parameter went out of range or the node has encountered a minor + // failure. + UAVCAN_NODE_HEALTH_WARNING = 1, + // The node has encountered a major failure. + UAVCAN_NODE_HEALTH_ERROR = 2, + // The node has suffered a fatal malfunction. + UAVCAN_NODE_HEALTH_CRITICAL = 3 +}; + +// Generalized UAVCAN node mode +enum class UAVCAN_NODE_MODE { + // The node is performing its primary functions. + UAVCAN_NODE_MODE_OPERATIONAL = 0, + // The node is initializing; this mode is entered immediately after startup. + UAVCAN_NODE_MODE_INITIALIZATION = 1, + // The node is under maintenance. + UAVCAN_NODE_MODE_MAINTENANCE = 2, + // The node is in the process of updating its software. + UAVCAN_NODE_MODE_SOFTWARE_UPDATE = 3, + // The node is no longer available online. + UAVCAN_NODE_MODE_OFFLINE = 7 +}; + +// Flags to indicate the status of camera storage. +enum class STORAGE_STATUS { + // Storage is missing (no microSD card loaded for example.) + STORAGE_STATUS_EMPTY = 0, + // Storage present but unformatted. + STORAGE_STATUS_UNFORMATTED = 1, + // Storage present and ready. + STORAGE_STATUS_READY = 2, + // Camera does not supply storage status information. Capacity information in + // STORAGE_INFORMATION fields will be ignored. + STORAGE_STATUS_NOT_SUPPORTED = 3 +}; + +// Yaw behaviour during orbit flight. +enum class ORBIT_YAW_BEHAVIOUR { + // Vehicle front points to the center (default). + ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER = 0, + // Vehicle front holds heading when message received. + ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1, + // Yaw uncontrolled. + ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2, + // Vehicle front follows flight path (tangential to circle). + ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3, + // Yaw controlled by RC input. + ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 +}; + // Commands to be executed by the MAV. They can be executed on user request, or as // part of a mission script. If the action is used in a mission, the parameter mapping // to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param // 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 // is for commercial aircraft: A data format how to interpret waypoint/mission data. +// See https://mavlink.io/en/guide/xml_schema.html#MAV_CMD for information about the +// structure of the MAV_CMD entries enum class MAV_CMD { - // Navigate to MISSION. + // Navigate to waypoint. MAV_CMD_NAV_WAYPOINT = 16, - // Loiter around this MISSION an unlimited amount of time + // Loiter around this waypoint an unlimited amount of time MAV_CMD_NAV_LOITER_UNLIM = 17, - // Loiter around this MISSION for X turns + // Loiter around this waypoint for X turns MAV_CMD_NAV_LOITER_TURNS = 18, - // Loiter around this MISSION for X seconds + // Loiter around this waypoint for X seconds MAV_CMD_NAV_LOITER_TIME = 19, // Return to launch location MAV_CMD_NAV_RETURN_TO_LAUNCH = 20, - // Land at location + // Land at location. MAV_CMD_NAV_LAND = 21, // Takeoff from ground / hand MAV_CMD_NAV_TAKEOFF = 22, @@ -603,19 +1057,23 @@ enum class MAV_CMD { // Required parameter is non-zero the aircraft will not leave the loiter until // heading toward the next waypoint. MAV_CMD_NAV_LOITER_TO_ALT = 31, - // Being following a target + // Begin following a target MAV_CMD_DO_FOLLOW = 32, // Reposition the MAV after a follow target command has been sent MAV_CMD_DO_FOLLOW_REPOSITION = 33, + // Start orbiting on the circumference of a circle defined by the parameters. + // Setting any value NaN results in using defaults. + MAV_CMD_DO_ORBIT = 34, // Sets the region of interest (ROI) for a sensor set or the vehicle itself. This // can then be used by the vehicles control system to control the vehicle attitude // and the attitude of various sensors such as cameras. MAV_CMD_NAV_ROI = 80, // Control autonomous path planning on the MAV. MAV_CMD_NAV_PATHPLANNING = 81, - // Navigate to MISSION using a spline path. + // Navigate to waypoint using a spline path. MAV_CMD_NAV_SPLINE_WAYPOINT = 82, - // Takeoff from ground using VTOL mode + // Takeoff from ground using VTOL mode, and transition to forward flight with + // specified heading. MAV_CMD_NAV_VTOL_TAKEOFF = 84, // Land using VTOL mode MAV_CMD_NAV_VTOL_LAND = 85, @@ -624,6 +1082,11 @@ enum class MAV_CMD { // Delay the next navigation command a number of seconds or until a specified // time MAV_CMD_NAV_DELAY = 93, + // Descend and place payload. Vehicle moves to specified location, descends until + // it detects a hanging payload has reached the ground, and then releases the + // payload. If ground is not detected before the reaching the maximum descent + // value (param1), the command will complete without releasing the payload. + MAV_CMD_NAV_PAYLOAD_PLACE = 94, // NOP - This command is only used to mark the upper limit of the NAV/ACTION commands // in the enumeration MAV_CMD_NAV_LAST = 95, @@ -653,7 +1116,7 @@ enum class MAV_CMD { MAV_CMD_DO_SET_PARAMETER = 180, // Set a relay to a condition. MAV_CMD_DO_SET_RELAY = 181, - // Cycle a relay on and off for a desired number of cyles with a desired period. + // Cycle a relay on and off for a desired number of cycles with a desired period. MAV_CMD_DO_REPEAT_RELAY = 182, // Set a servo to a desired PWM value. MAV_CMD_DO_SET_SERVO = 183, @@ -668,12 +1131,12 @@ enum class MAV_CMD { // to tell the autopilot where a sequence of mission items that represents a landing // starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which // case the nearest (geographically) landing sequence in the mission will be used. - // The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If - // specified then it will be used to help find the closest landing sequence. + // The Latitude/Longitude is optional, and may be set to 0 if not needed. If specified + // then it will be used to help find the closest landing sequence. MAV_CMD_DO_LAND_START = 189, // Mission command to perform a landing from a rally point. MAV_CMD_DO_RALLY_LAND = 190, - // Mission command to safely abort an autonmous landing. + // Mission command to safely abort an autonomous landing. MAV_CMD_DO_GO_AROUND = 191, // Reposition the vehicle to a specific WGS84 global position. MAV_CMD_DO_REPOSITION = 192, @@ -681,47 +1144,76 @@ enum class MAV_CMD { MAV_CMD_DO_PAUSE_CONTINUE = 193, // Set moving direction to forward or reverse. MAV_CMD_DO_SET_REVERSE = 194, + // Sets the region of interest (ROI) to a location. This can then be used by the + // vehicles control system to control the vehicle attitude and the attitude of + // various sensors such as cameras. + MAV_CMD_DO_SET_ROI_LOCATION = 195, + // Sets the region of interest (ROI) to be toward next waypoint, with optional + // pitch/roll/yaw offset. This can then be used by the vehicles control system + // to control the vehicle attitude and the attitude of various sensors such as + // cameras. + MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196, + // Cancels any previous ROI command returning the vehicle/sensors to default flight + // characteristics. This can then be used by the vehicles control system to control + // the vehicle attitude and the attitude of various sensors such as cameras. + MAV_CMD_DO_SET_ROI_NONE = 197, // Control onboard camera system. MAV_CMD_DO_CONTROL_VIDEO = 200, // Sets the region of interest (ROI) for a sensor set or the vehicle itself. This // can then be used by the vehicles control system to control the vehicle attitude // and the attitude of various sensors such as cameras. MAV_CMD_DO_SET_ROI = 201, - // Mission command to configure an on-board camera controller system. + // Configure digital camera. This is a fallback message for systems that have + // not yet implemented PARAM_EXT_XXX messages and camera definition files (see + // https://mavlink.io/en/services/camera_def.html ). MAV_CMD_DO_DIGICAM_CONFIGURE = 202, - // Mission command to control an on-board camera controller system. + // Control digital camera. This is a fallback message for systems that have not + // yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html + // ). MAV_CMD_DO_DIGICAM_CONTROL = 203, // Mission command to configure a camera or antenna mount MAV_CMD_DO_MOUNT_CONFIGURE = 204, // Mission command to control a camera or antenna mount MAV_CMD_DO_MOUNT_CONTROL = 205, - // Mission command to set CAM_TRIGG_DIST for this flight + // Mission command to set camera trigger distance for this flight. The camera + // is triggered each time this distance is exceeded. This command can also be + // used to set the shutter integration time for the camera. MAV_CMD_DO_SET_CAM_TRIGG_DIST = 206, // Mission command to enable the geofence MAV_CMD_DO_FENCE_ENABLE = 207, // Mission command to trigger a parachute MAV_CMD_DO_PARACHUTE = 208, - // Mission command to perform motor test + // Mission command to perform motor test. MAV_CMD_DO_MOTOR_TEST = 209, - // Change to/from inverted flight + // Change to/from inverted flight. MAV_CMD_DO_INVERTED_FLIGHT = 210, - // Sets a desired vehicle turn angle and thrust change - MAV_CMD_DO_SET_POSITION_YAW_THRUST = 213, + // Sets a desired vehicle turn angle and speed change. + MAV_CMD_NAV_SET_YAW_SPEED = 213, + // Mission command to set camera trigger interval for this flight. If triggering + // is enabled, the camera is triggered each time this interval expires. This command + // can also be used to set the shutter integration time for the camera. + MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214, // Mission command to control a camera or antenna mount, using a quaternion as // reference. MAV_CMD_DO_MOUNT_CONTROL_QUAT = 220, // set id of master controller MAV_CMD_DO_GUIDED_MASTER = 221, - // set limits for external control + // Set limits for external control MAV_CMD_DO_GUIDED_LIMITS = 222, // Control vehicle engine. This is interpreted by the vehicles engine controller // to change the target engine state. It is intended for vehicles with internal // combustion engines MAV_CMD_DO_ENGINE_CONTROL = 223, + // Set the mission item with sequence number seq as current item. This means that + // the MAV will continue to this mission item on the shortest path (not following + // the mission items in-between). + MAV_CMD_DO_SET_MISSION_CURRENT = 224, // NOP - This command is only used to mark the upper limit of the DO commands // in the enumeration MAV_CMD_DO_LAST = 240, // Trigger calibration. This command will be only accepted if in pre-flight mode. + // Except for Temperature Calibration, only one sensor should be set in a single + // message and all others should be zero. MAV_CMD_PREFLIGHT_CALIBRATION = 241, // Set sensor offsets. This command will be only accepted if in pre-flight mode. MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, @@ -733,70 +1225,144 @@ enum class MAV_CMD { MAV_CMD_PREFLIGHT_STORAGE = 245, // Request the reboot or shutdown of system components. MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246, - // Hold / continue the current action + // Override current mission with command to pause mission, pause mission and move + // to position, continue/resume mission. When param 1 indicates that the mission + // is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or + // moves to another position. MAV_CMD_OVERRIDE_GOTO = 252, // start running a mission MAV_CMD_MISSION_START = 300, // Arms / Disarms a component MAV_CMD_COMPONENT_ARM_DISARM = 400, + // Turns illuminators ON/OFF. An illuminator is a light source that is used for + // lighting up dark areas external to the sytstem: e.g. a torch or searchlight + // (as opposed to a light source for illuminating the system itself, e.g. an indicator + // light). + MAV_CMD_ILLUMINATOR_ON_OFF = 405, // Request the home position from the vehicle. MAV_CMD_GET_HOME_POSITION = 410, - // Starts receiver pairing + // Starts receiver pairing. MAV_CMD_START_RX_PAIR = 500, - // Request the interval between messages for a particular MAVLink message ID - MAV_CMD_GET_MESSAGE_INTERVAL = 510, // Request the interval between messages for a particular MAVLink message ID. - // This interface replaces REQUEST_DATA_STREAM + // The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL + // message. + MAV_CMD_GET_MESSAGE_INTERVAL = 510, + // Set the interval between messages for a particular MAVLink message ID. This + // interface replaces REQUEST_DATA_STREAM. MAV_CMD_SET_MESSAGE_INTERVAL = 511, + // Request the target system(s) emit a single instance of a specified message + // (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). + MAV_CMD_REQUEST_MESSAGE = 512, // Request MAVLink protocol version compatibility MAV_CMD_REQUEST_PROTOCOL_VERSION = 519, - // Request autopilot capabilities + // Request autopilot capabilities. The receiver should ACK the command and then + // emit its capabilities in an AUTOPILOT_VERSION message MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES = 520, - // WIP: Request camera information (CAMERA_INFORMATION) + // Request camera information (CAMERA_INFORMATION). MAV_CMD_REQUEST_CAMERA_INFORMATION = 521, - // WIP: Request camera settings (CAMERA_SETTINGS) + // Request camera settings (CAMERA_SETTINGS). MAV_CMD_REQUEST_CAMERA_SETTINGS = 522, - // WIP: Set the camera settings part 1 (CAMERA_SETTINGS) - MAV_CMD_SET_CAMERA_SETTINGS_1 = 523, - // WIP: Set the camera settings part 2 (CAMERA_SETTINGS) - MAV_CMD_SET_CAMERA_SETTINGS_2 = 524, - // WIP: Request storage information (STORAGE_INFORMATION) + // Request storage information (STORAGE_INFORMATION). Use the command's target_component + // to target a specific component's storage. MAV_CMD_REQUEST_STORAGE_INFORMATION = 525, - // WIP: Format a storage medium + // Format a storage medium. Once format is complete, a STORAGE_INFORMATION message + // is sent. Use the command's target_component to target a specific component's + // storage. MAV_CMD_STORAGE_FORMAT = 526, - // WIP: Request camera capture status (CAMERA_CAPTURE_STATUS) + // Request camera capture status (CAMERA_CAPTURE_STATUS) MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS = 527, - // WIP: Request flight information (FLIGHT_INFORMATION) + // Request flight information (FLIGHT_INFORMATION) MAV_CMD_REQUEST_FLIGHT_INFORMATION = 528, - // Start image capture sequence + // Reset all camera settings to Factory Default + MAV_CMD_RESET_CAMERA_SETTINGS = 529, + // Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS + // command after a mode change if the camera supports video streaming. + MAV_CMD_SET_CAMERA_MODE = 530, + // Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). + // Use NaN for reserved values. + MAV_CMD_SET_CAMERA_ZOOM = 531, + // Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). + // Use NaN for reserved values. + MAV_CMD_SET_CAMERA_FOCUS = 532, + // Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. + MAV_CMD_JUMP_TAG = 600, + // Jump to the matching tag in the mission list. Repeat this action for the specified + // number of times. A mission should contain a single matching tag for each jump. + // If this is not the case then a jump to a missing tag should complete the mission, + // and a jump where there are multiple matching tags should always select the + // one with the lowest mission sequence number. + MAV_CMD_DO_JUMP_TAG = 601, + // Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. + // Use NaN for reserved values. MAV_CMD_IMAGE_START_CAPTURE = 2000, - // Stop image capture sequence + // Stop image capture sequence Use NaN for reserved values. MAV_CMD_IMAGE_STOP_CAPTURE = 2001, + // Re-request a CAMERA_IMAGE_CAPTURE message. Use NaN for reserved values. + MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE = 2002, // Enable or disable on-board camera triggering system. MAV_CMD_DO_TRIGGER_CONTROL = 2003, - // Starts video capture + // Starts video capture (recording). Use NaN for reserved values. MAV_CMD_VIDEO_START_CAPTURE = 2500, - // Stop the current video capture + // Stop the current video capture (recording). Use NaN for reserved values. MAV_CMD_VIDEO_STOP_CAPTURE = 2501, + // Start video streaming + MAV_CMD_VIDEO_START_STREAMING = 2502, + // Stop the given video stream + MAV_CMD_VIDEO_STOP_STREAMING = 2503, + // Request video stream information (VIDEO_STREAM_INFORMATION) + MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION = 2504, + // Request video stream status (VIDEO_STREAM_STATUS) + MAV_CMD_REQUEST_VIDEO_STREAM_STATUS = 2505, // Request to start streaming logging data over MAVLink (see also LOGGING_DATA // message) MAV_CMD_LOGGING_START = 2510, // Request to stop streaming log data over MAVLink MAV_CMD_LOGGING_STOP = 2511, MAV_CMD_AIRFRAME_CONFIGURATION = 2520, + // Request to start/stop transmitting over the high latency telemetry + MAV_CMD_CONTROL_HIGH_LATENCY = 2600, // Create a panorama at the current position MAV_CMD_PANORAMA_CREATE = 2800, // Request VTOL transition MAV_CMD_DO_VTOL_TRANSITION = 3000, + // Request authorization to arm the vehicle to a external entity, the arm authorizer + // is responsible to request all data that is needs from the vehicle before authorize + // or deny the request. If approved the progress of command_ack message should + // be set with period of time that this authorization is valid in seconds or in + // case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. + MAV_CMD_ARM_AUTHORIZATION_REQUEST = 3001, // This command sets the submode to standard guided when vehicle is in guided // mode. The vehicle holds position and altitude and the user can input the desired - // velocites along all three axes. + // velocities along all three axes. MAV_CMD_SET_GUIDED_SUBMODE_STANDARD = 4000, // This command sets submode circle when vehicle is in guided mode. Vehicle flies // along a circle facing the center of the circle. The user can input the velocity // along the circle and change the radius. If no input is given the vehicle will // hold position. MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE = 4001, + // Delay mission state machine until gate has been reached. + MAV_CMD_CONDITION_GATE = 4501, + // Fence return point. There can only be one fence return point. + MAV_CMD_NAV_FENCE_RETURN_POINT = 5000, + // Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). + // The vehicle must stay within this area. Minimum of 3 vertices required. + MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION = 5001, + // Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). + // The vehicle must stay outside this area. Minimum of 3 vertices required. + MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION = 5002, + // Circular fence area. The vehicle must stay inside this area. + MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION = 5003, + // Circular fence area. The vehicle must stay outside this area. + MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION = 5004, + // Rally point. You can have multiple rally points defined. + MAV_CMD_NAV_RALLY_POINT = 5100, + // Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, + // one message per every UAVCAN node that is online. Note that some of the response + // messages can be lost, which the receiver can detect easily by checking whether + // every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received + // earlier; if not, this command should be sent again in order to request re-transmission + // of the node information messages. + MAV_CMD_UAVCAN_GET_NODE_INFO = 5200, // Deploy payload on a Lat / Lon / Alt position. This includes the navigation // to reach the required release position and velocity. MAV_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, @@ -849,7 +1415,6 @@ enum class MAV_CMD { MAV_CMD_USER_5 = 31014 }; -// THIS INTERFACE IS DEPRECATED AS OF JULY 2015. Please use MESSAGE_INTERVAL instead. // A data stream is not a fixed set of messages, but rather a recommendation to the // autopilot software. Individual autopilots may or may not obey the recommended messages. enum class MAV_DATA_STREAM { @@ -878,9 +1443,9 @@ enum class MAV_DATA_STREAM { enum class MAV_ROI { // No region of interest. MAV_ROI_NONE = 0, - // Point toward next MISSION. + // Point toward next waypoint, with optional pitch/roll/yaw offset. MAV_ROI_WPNEXT = 1, - // Point toward given MISSION. + // Point toward given waypoint. MAV_ROI_WPINDEX = 2, // Point toward fixed location. MAV_ROI_LOCATION = 3, @@ -938,52 +1503,95 @@ enum class MAV_PARAM_TYPE { MAV_PARAM_TYPE_REAL64 = 10 }; -// result from a mavlink command +// Specifies the datatype of a MAVLink extended parameter. +enum class MAV_PARAM_EXT_TYPE { + // 8-bit unsigned integer + MAV_PARAM_EXT_TYPE_UINT8 = 1, + // 8-bit signed integer + MAV_PARAM_EXT_TYPE_INT8 = 2, + // 16-bit unsigned integer + MAV_PARAM_EXT_TYPE_UINT16 = 3, + // 16-bit signed integer + MAV_PARAM_EXT_TYPE_INT16 = 4, + // 32-bit unsigned integer + MAV_PARAM_EXT_TYPE_UINT32 = 5, + // 32-bit signed integer + MAV_PARAM_EXT_TYPE_INT32 = 6, + // 64-bit unsigned integer + MAV_PARAM_EXT_TYPE_UINT64 = 7, + // 64-bit signed integer + MAV_PARAM_EXT_TYPE_INT64 = 8, + // 32-bit floating-point + MAV_PARAM_EXT_TYPE_REAL32 = 9, + // 64-bit floating-point + MAV_PARAM_EXT_TYPE_REAL64 = 10, + // Custom Type + MAV_PARAM_EXT_TYPE_CUSTOM = 11 +}; + +// Result from a MAVLink command (MAV_CMD) enum class MAV_RESULT { - // Command ACCEPTED and EXECUTED + // Command is valid (is supported and has valid parameters), and was executed. MAV_RESULT_ACCEPTED = 0, - // Command TEMPORARY REJECTED/DENIED + // Command is valid, but cannot be executed at this time. This is used to indicate + // a problem that should be fixed just by waiting (e.g. a state machine is busy, + // can't arm because have not got GPS lock, etc.). Retrying later should work. MAV_RESULT_TEMPORARILY_REJECTED = 1, - // Command PERMANENTLY DENIED + // Command is invalid (is supported but has invalid parameters). Retrying same + // command and parameters will not work. MAV_RESULT_DENIED = 2, - // Command UNKNOWN/UNSUPPORTED + // Command is not supported (unknown). MAV_RESULT_UNSUPPORTED = 3, - // Command executed, but failed - MAV_RESULT_FAILED = 4 -}; - -// result in a mavlink mission ack + // Command is valid, but execution has failed. This is used to indicate any non-temporary + // or unexpected problem, i.e. any problem that must be fixed before the command + // can succeed/be retried. For example, attempting to write a file when out of + // memory, attempting to arm when sensors are not calibrated, etc. + MAV_RESULT_FAILED = 4, + // Command is valid and is being executed. This will be followed by further progress + // updates, i.e. the component may send further COMMAND_ACK messages with result + // MAV_RESULT_IN_PROGRESS (at a rate decided by the implementation), and must + // terminate by sending a COMMAND_ACK message with final result of the operation. + // The COMMAND_ACK.progress field can be used to indicate the progress of the + // operation. There is no need for the sender to retry the command, but if done + // during execution, the component will return MAV_RESULT_IN_PROGRESS with an + // updated progress. + MAV_RESULT_IN_PROGRESS = 5 +}; + +// Result of mission operation (in a MISSION_ACK message). enum class MAV_MISSION_RESULT { // mission accepted OK MAV_MISSION_ACCEPTED = 0, - // generic error / not accepting mission commands at all right now + // Generic error / not accepting mission commands at all right now. MAV_MISSION_ERROR = 1, - // coordinate frame is not supported + // Coordinate frame is not supported. MAV_MISSION_UNSUPPORTED_FRAME = 2, - // command is not supported + // Command is not supported. MAV_MISSION_UNSUPPORTED = 3, - // mission item exceeds storage space + // Mission item exceeds storage space. MAV_MISSION_NO_SPACE = 4, - // one of the parameters has an invalid value + // One of the parameters has an invalid value. MAV_MISSION_INVALID = 5, - // param1 has an invalid value + // param1 has an invalid value. MAV_MISSION_INVALID_PARAM1 = 6, - // param2 has an invalid value + // param2 has an invalid value. MAV_MISSION_INVALID_PARAM2 = 7, - // param3 has an invalid value + // param3 has an invalid value. MAV_MISSION_INVALID_PARAM3 = 8, - // param4 has an invalid value + // param4 has an invalid value. MAV_MISSION_INVALID_PARAM4 = 9, - // x/param5 has an invalid value + // x / param5 has an invalid value. MAV_MISSION_INVALID_PARAM5_X = 10, - // y/param6 has an invalid value + // y / param6 has an invalid value. MAV_MISSION_INVALID_PARAM6_Y = 11, - // param7 has an invalid value + // z / param7 has an invalid value. MAV_MISSION_INVALID_PARAM7 = 12, - // received waypoint out of sequence + // Mission item received out of sequence MAV_MISSION_INVALID_SEQUENCE = 13, - // not accepting any mission commands from this communication partner - MAV_MISSION_DENIED = 14 + // Not accepting any mission commands from this communication partner. + MAV_MISSION_DENIED = 14, + // Current mission operation cancelled (e.g. mission upload, mission download). + MAV_MISSION_OPERATION_CANCELLED = 15 }; // Indicates the severity level, generally used for status messages to indicate their @@ -1000,7 +1608,7 @@ enum class MAV_SEVERITY { // Indicates about a possible future error if this is not resolved within a given // timeframe. Example would be a low battery warning. MAV_SEVERITY_WARNING = 4, - // An unusual event has occured, though not an error condition. This should be + // An unusual event has occurred, though not an error condition. This should be // investigated for the root cause. MAV_SEVERITY_NOTICE = 5, // Normal operational messages. Useful for logging. No action is required for @@ -1065,7 +1673,11 @@ enum class MAV_DISTANCE_SENSOR { // Ultrasound rangefinder, e.g. MaxBotix units MAV_DISTANCE_SENSOR_ULTRASOUND = 1, // Infrared rangefinder, e.g. Sharp units - MAV_DISTANCE_SENSOR_INFRARED = 2 + MAV_DISTANCE_SENSOR_INFRARED = 2, + // Radar type, e.g. uLanding units + MAV_DISTANCE_SENSOR_RADAR = 3, + // Broken or unknown type, e.g. analog units + MAV_DISTANCE_SENSOR_UNKNOWN = 4 }; // Enumeration of sensor orientation, according to its rotations @@ -1146,8 +1758,14 @@ enum class MAV_SENSOR_ORIENTATION { MAV_SENSOR_ROTATION_ROLL_90_PITCH_180_YAW_90 = 36, // Roll: 90, Pitch: 0, Yaw: 270 MAV_SENSOR_ROTATION_ROLL_90_YAW_270 = 37, - // Roll: 315, Pitch: 315, Yaw: 315 - MAV_SENSOR_ROTATION_ROLL_315_PITCH_315_YAW_315 = 38 + // Roll: 90, Pitch: 68, Yaw: 293 + MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, + // Pitch: 315 + MAV_SENSOR_ROTATION_PITCH_315 = 39, + // Roll: 90, Pitch: 315 + MAV_SENSOR_ROTATION_ROLL_90_PITCH_315 = 40, + // Custom orientation + MAV_SENSOR_ROTATION_CUSTOM = 100 }; // Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot @@ -1180,12 +1798,33 @@ enum class MAV_PROTOCOL_CAPABILITY { MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION = 2048, // Autopilot supports onboard compass calibration. MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION = 4096, - // Autopilot supports mavlink version 2. - MAV_PROTOCOL_CAPABILITY_MAVLINK2 = 8192 + // Autopilot supports MAVLink version 2. + MAV_PROTOCOL_CAPABILITY_MAVLINK2 = 8192, + // Autopilot supports mission fence protocol. + MAV_PROTOCOL_CAPABILITY_MISSION_FENCE = 16384, + // Autopilot supports mission rally point protocol. + MAV_PROTOCOL_CAPABILITY_MISSION_RALLY = 32768, + // Autopilot supports the flight information protocol. + MAV_PROTOCOL_CAPABILITY_FLIGHT_INFORMATION = 65536 +}; + +// Type of mission items being requested/sent in mission protocol. +enum class MAV_MISSION_TYPE { + // Items are mission commands for main mission. + MAV_MISSION_TYPE_MISSION = 0, + // Specifies GeoFence area(s). Items are MAV_CMD_NAV_FENCE_ GeoFence items. + MAV_MISSION_TYPE_FENCE = 1, + // Specifies the rally points for the vehicle. Rally points are alternative RTL + // points. Items are MAV_CMD_NAV_RALLY_POINT rally point items. + MAV_MISSION_TYPE_RALLY = 2, + // Only used in MISSION_CLEAR_ALL to clear all mission types. + MAV_MISSION_TYPE_ALL = 255 }; // Enumeration of estimator types enum class MAV_ESTIMATOR_TYPE { + // Unknown type of the estimator. + MAV_ESTIMATOR_TYPE_UNKNOWN = 0, // This is a naive estimator without any real covariance feedback. MAV_ESTIMATOR_TYPE_NAIVE = 1, // Computer vision based estimate. Might be up to scale. @@ -1195,7 +1834,13 @@ enum class MAV_ESTIMATOR_TYPE { // Plain GPS estimate. MAV_ESTIMATOR_TYPE_GPS = 4, // Estimator integrating GPS and inertial sensing. - MAV_ESTIMATOR_TYPE_GPS_INS = 5 + MAV_ESTIMATOR_TYPE_GPS_INS = 5, + // Estimate from external motion capturing system. + MAV_ESTIMATOR_TYPE_MOCAP = 6, + // Estimator based on lidar sensor input. + MAV_ESTIMATOR_TYPE_LIDAR = 7, + // Estimator on autopilot. + MAV_ESTIMATOR_TYPE_AUTOPILOT = 8 }; // Enumeration of battery types @@ -1226,6 +1871,44 @@ enum class MAV_BATTERY_FUNCTION { MAV_BATTERY_TYPE_PAYLOAD = 4 }; +// Enumeration for battery charge states. +enum class MAV_BATTERY_CHARGE_STATE { + // Low battery state is not provided + MAV_BATTERY_CHARGE_STATE_UNDEFINED = 0, + // Battery is not in low state. Normal operation. + MAV_BATTERY_CHARGE_STATE_OK = 1, + // Battery state is low, warn and monitor close. + MAV_BATTERY_CHARGE_STATE_LOW = 2, + // Battery state is critical, return or abort immediately. + MAV_BATTERY_CHARGE_STATE_CRITICAL = 3, + // Battery state is too low for ordinary abort sequence. Perform fastest possible + // emergency stop to prevent damage. + MAV_BATTERY_CHARGE_STATE_EMERGENCY = 4, + // Battery failed, damage unavoidable. + MAV_BATTERY_CHARGE_STATE_FAILED = 5, + // Battery is diagnosed to be defective or an error occurred, usage is discouraged + // / prohibited. + MAV_BATTERY_CHARGE_STATE_UNHEALTHY = 6, + // Battery is charging. + MAV_BATTERY_CHARGE_STATE_CHARGING = 7 +}; + +// Smart battery supply status/fault flags (bitmask) for health indication. +enum class MAV_SMART_BATTERY_FAULT { + // Battery has deep discharged. + MAV_SMART_BATTERY_FAULT_DEEP_DISCHARGE = 1, + // Voltage spikes. + MAV_SMART_BATTERY_FAULT_SPIKES = 2, + // Single cell has failed. + MAV_SMART_BATTERY_FAULT_SINGLE_CELL_FAIL = 4, + // Over-current fault. + MAV_SMART_BATTERY_FAULT_OVER_CURRENT = 8, + // Over-temperature fault. + MAV_SMART_BATTERY_FAULT_OVER_TEMPERATURE = 16, + // Under-temperature fault. + MAV_SMART_BATTERY_FAULT_UNDER_TEMPERATURE = 32 +}; + // Enumeration of VTOL states enum class MAV_VTOL_STATE { // MAV is not configured as VTOL @@ -1247,7 +1930,11 @@ enum class MAV_LANDED_STATE { // MAV is landed (on ground) MAV_LANDED_STATE_ON_GROUND = 1, // MAV is in air - MAV_LANDED_STATE_IN_AIR = 2 + MAV_LANDED_STATE_IN_AIR = 2, + // MAV currently taking off + MAV_LANDED_STATE_TAKEOFF = 3, + // MAV currently landing + MAV_LANDED_STATE_LANDING = 4 }; // Enumeration of the ADSB altimeter types @@ -1291,10 +1978,13 @@ enum class ADSB_FLAGS { ADSB_FLAGS_VALID_VELOCITY = 8, ADSB_FLAGS_VALID_CALLSIGN = 16, ADSB_FLAGS_VALID_SQUAWK = 32, - ADSB_FLAGS_SIMULATED = 64 + ADSB_FLAGS_SIMULATED = 64, + ADSB_FLAGS_VERTICAL_VELOCITY_VALID = 128, + ADSB_FLAGS_BARO_VALID = 256, + ADSB_FLAGS_SOURCE_UAT = 32768 }; -// Bitmask of options for the MAV_CMD_DO_REPOSITION +// Bitmap of options for the MAV_CMD_DO_REPOSITION enum class MAV_DO_REPOSITION_FLAGS { // The aircraft should immediately transition into guided. This should not be // set for follow me applications @@ -1327,7 +2017,19 @@ enum class ESTIMATOR_STATUS_FLAGS { // position estimate ESTIMATOR_PRED_POS_HORIZ_ABS = 512, // True if the EKF has detected a GPS glitch - ESTIMATOR_GPS_GLITCH = 1024 + ESTIMATOR_GPS_GLITCH = 1024, + // True if the EKF has detected bad accelerometer data + ESTIMATOR_ACCEL_ERROR = 2048 +}; + +enum class MOTOR_TEST_ORDER { + // default autopilot motor test method + MOTOR_TEST_ORDER_DEFAULT = 0, + // motor numbers are specified as their index in a predefined vehicle-specific + // sequence + MOTOR_TEST_ORDER_SEQUENCE = 1, + // motor numbers are specified as the output as labeled on the board + MOTOR_TEST_ORDER_BOARD = 2 }; enum class MOTOR_TEST_THROTTLE_TYPE { @@ -1336,7 +2038,9 @@ enum class MOTOR_TEST_THROTTLE_TYPE { // throttle as an absolute PWM value (normally in range of 1000~2000) MOTOR_TEST_THROTTLE_PWM = 1, // throttle pass-through from pilot's transmitter - MOTOR_TEST_THROTTLE_PILOT = 2 + MOTOR_TEST_THROTTLE_PILOT = 2, + // per-motor compass calibration test + MOTOR_TEST_COMPASS_CAL = 3 }; enum class GPS_INPUT_IGNORE_FLAGS { @@ -1364,9 +2068,9 @@ enum class MAV_COLLISION_ACTION { MAV_COLLISION_ACTION_NONE = 0, // Report potential collision MAV_COLLISION_ACTION_REPORT = 1, - // Ascend or Descend to avoid thread + // Ascend or Descend to avoid threat MAV_COLLISION_ACTION_ASCEND_OR_DESCEND = 2, - // Ascend or Descend to avoid thread + // Move horizontally to avoid threat MAV_COLLISION_ACTION_MOVE_HORIZONTALLY = 3, // Aircraft to move perpendicular to the collision's velocity vector MAV_COLLISION_ACTION_MOVE_PERPENDICULAR = 4, @@ -1382,7 +2086,7 @@ enum class MAV_COLLISION_THREAT_LEVEL { MAV_COLLISION_THREAT_LEVEL_NONE = 0, // Craft is mildly concerned about this threat MAV_COLLISION_THREAT_LEVEL_LOW = 1, - // Craft is panicing, and may take actions to avoid threat + // Craft is panicking, and may take actions to avoid threat MAV_COLLISION_THREAT_LEVEL_HIGH = 2 }; @@ -1411,27 +2115,689 @@ enum class GPS_FIX_TYPE { // RTK Fixed, 3D position GPS_FIX_TYPE_RTK_FIXED = 6, // Static fixed, typically used for base stations - GPS_FIX_TYPE_STATIC = 7 -}; - -// The heartbeat message shows that a system is present and responding. The type of -// the MAV and Autopilot hardware allow the receiving system to treat further messages -// from this system appropriate (e.g. by laying out the user interface based on the -// autopilot). + GPS_FIX_TYPE_STATIC = 7, + // PPP, 3D position. + GPS_FIX_TYPE_PPP = 8 +}; + +// RTK GPS baseline coordinate system, used for RTK corrections +enum class RTK_BASELINE_COORDINATE_SYSTEM { + // Earth-centered, Earth-fixed + RTK_BASELINE_COORDINATE_SYSTEM_ECEF = 0, + // RTK basestation centered, north, east, down + RTK_BASELINE_COORDINATE_SYSTEM_NED = 1 +}; + +// Type of landing target +enum class LANDING_TARGET_TYPE { + // Landing target signaled by light beacon (ex: IR-LOCK) + LANDING_TARGET_TYPE_LIGHT_BEACON = 0, + // Landing target signaled by radio beacon (ex: ILS, NDB) + LANDING_TARGET_TYPE_RADIO_BEACON = 1, + // Landing target represented by a fiducial marker (ex: ARTag) + LANDING_TARGET_TYPE_VISION_FIDUCIAL = 2, + // Landing target represented by a pre-defined visual shape/feature (ex: X-marker, + // H-marker, square) + LANDING_TARGET_TYPE_VISION_OTHER = 3 +}; + +// Direction of VTOL transition +enum class VTOL_TRANSITION_HEADING { + // Respect the heading configuration of the vehicle. + VTOL_TRANSITION_HEADING_VEHICLE_DEFAULT = 0, + // Use the heading pointing towards the next waypoint. + VTOL_TRANSITION_HEADING_NEXT_WAYPOINT = 1, + // Use the heading on takeoff (while sitting on the ground). + VTOL_TRANSITION_HEADING_TAKEOFF = 2, + // Use the specified heading in parameter 4. + VTOL_TRANSITION_HEADING_SPECIFIED = 3, + // Use the current heading when reaching takeoff altitude (potentially facing + // the wind when weather-vaning is active). + VTOL_TRANSITION_HEADING_ANY = 4 +}; + +// Camera capability flags (Bitmap) +enum class CAMERA_CAP_FLAGS { + // Camera is able to record video + CAMERA_CAP_FLAGS_CAPTURE_VIDEO = 1, + // Camera is able to capture images + CAMERA_CAP_FLAGS_CAPTURE_IMAGE = 2, + // Camera has separate Video and Image/Photo modes (MAV_CMD_SET_CAMERA_MODE) + CAMERA_CAP_FLAGS_HAS_MODES = 4, + // Camera can capture images while in video mode + CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE = 8, + // Camera can capture videos while in Photo/Image mode + CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE = 16, + // Camera has image survey mode (MAV_CMD_SET_CAMERA_MODE) + CAMERA_CAP_FLAGS_HAS_IMAGE_SURVEY_MODE = 32, + // Camera has basic zoom control (MAV_CMD_SET_CAMERA_ZOOM) + CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM = 64, + // Camera has basic focus control (MAV_CMD_SET_CAMERA_FOCUS) + CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS = 128, + // Camera has video streaming capabilities (use MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION + // for video streaming info) + CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM = 256 +}; + +// Stream status flags (Bitmap) +enum class VIDEO_STREAM_STATUS_FLAGS { + // Stream is active (running) + VIDEO_STREAM_STATUS_FLAGS_RUNNING = 1, + // Stream is thermal imaging + VIDEO_STREAM_STATUS_FLAGS_THERMAL = 2 +}; + +// Video stream types +enum class VIDEO_STREAM_TYPE { + // Stream is RTSP + VIDEO_STREAM_TYPE_RTSP = 0, + // Stream is RTP UDP (URI gives the port number) + VIDEO_STREAM_TYPE_RTPUDP = 1, + // Stream is MPEG on TCP + VIDEO_STREAM_TYPE_TCP_MPEG = 2, + // Stream is h.264 on MPEG TS (URI gives the port number) + VIDEO_STREAM_TYPE_MPEG_TS_H264 = 3 +}; + +// Zoom types for MAV_CMD_SET_CAMERA_ZOOM +enum class CAMERA_ZOOM_TYPE { + // Zoom one step increment (-1 for wide, 1 for tele) + ZOOM_TYPE_STEP = 0, + // Continuous zoom up/down until stopped (-1 for wide, 1 for tele, 0 to stop zooming) + ZOOM_TYPE_CONTINUOUS = 1, + // Zoom value as proportion of full camera range (a value between 0.0 and 100.0) + ZOOM_TYPE_RANGE = 2, + // Zoom value/variable focal length in milimetres. Note that there is no message + // to get the valid zoom range of the camera, so this can type can only be used + // for cameras where the zoom range is known (implying that this cannot reliably + // be used in a GCS for an arbitrary camera) + ZOOM_TYPE_FOCAL_LENGTH = 3 +}; + +// Focus types for MAV_CMD_SET_CAMERA_FOCUS +enum class SET_FOCUS_TYPE { + // Focus one step increment (-1 for focusing in, 1 for focusing out towards infinity). + FOCUS_TYPE_STEP = 0, + // Continuous focus up/down until stopped (-1 for focusing in, 1 for focusing + // out towards infinity, 0 to stop focusing) + FOCUS_TYPE_CONTINUOUS = 1, + // Focus value as proportion of full camera focus range (a value between 0.0 and + // 100.0) + FOCUS_TYPE_RANGE = 2, + // Focus value in metres. Note that there is no message to get the valid focus + // range of the camera, so this can type can only be used for cameras where the + // range is known (implying that this cannot reliably be used in a GCS for an + // arbitrary camera). + FOCUS_TYPE_METERS = 3 +}; + +// Result from a PARAM_EXT_SET message. +enum class PARAM_ACK { + // Parameter value ACCEPTED and SET + PARAM_ACK_ACCEPTED = 0, + // Parameter value UNKNOWN/UNSUPPORTED + PARAM_ACK_VALUE_UNSUPPORTED = 1, + // Parameter failed to set + PARAM_ACK_FAILED = 2, + // Parameter value received but not yet validated or set. A subsequent PARAM_EXT_ACK + // will follow once operation is completed with the actual result. These are for + // parameters that may take longer to set. Instead of waiting for an ACK and potentially + // timing out, you will immediately receive this response to let you know it was + // received. + PARAM_ACK_IN_PROGRESS = 3 +}; + +// Camera Modes. +enum class CAMERA_MODE { + // Camera is in image/photo capture mode. + CAMERA_MODE_IMAGE = 0, + // Camera is in video capture mode. + CAMERA_MODE_VIDEO = 1, + // Camera is in image survey capture mode. It allows for camera controller to + // do specific settings for surveys. + CAMERA_MODE_IMAGE_SURVEY = 2 +}; + +enum class MAV_ARM_AUTH_DENIED_REASON { + // Not a specific reason + MAV_ARM_AUTH_DENIED_REASON_GENERIC = 0, + // Authorizer will send the error as string to GCS + MAV_ARM_AUTH_DENIED_REASON_NONE = 1, + // At least one waypoint have a invalid value + MAV_ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT = 2, + // Timeout in the authorizer process(in case it depends on network) + MAV_ARM_AUTH_DENIED_REASON_TIMEOUT = 3, + // Airspace of the mission in use by another vehicle, second result parameter + // can have the waypoint id that caused it to be denied. + MAV_ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4, + // Weather is not good to fly + MAV_ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 +}; + +// RC type +enum class RC_TYPE { + // Spektrum DSM2 + RC_TYPE_SPEKTRUM_DSM2 = 0, + // Spektrum DSMX + RC_TYPE_SPEKTRUM_DSMX = 1 +}; + +// Bitmap to indicate which dimensions should be ignored by the vehicle: a value of +// 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions +// should be ignored. If bit 9 is set the floats afx afy afz should be interpreted +// as force instead of acceleration. +enum class POSITION_TARGET_TYPEMASK { + // Ignore position x + POSITION_TARGET_TYPEMASK_X_IGNORE = 1, + // Ignore position y + POSITION_TARGET_TYPEMASK_Y_IGNORE = 2, + // Ignore position z + POSITION_TARGET_TYPEMASK_Z_IGNORE = 4, + // Ignore velocity x + POSITION_TARGET_TYPEMASK_VX_IGNORE = 8, + // Ignore velocity y + POSITION_TARGET_TYPEMASK_VY_IGNORE = 16, + // Ignore velocity z + POSITION_TARGET_TYPEMASK_VZ_IGNORE = 32, + // Ignore acceleration x + POSITION_TARGET_TYPEMASK_AX_IGNORE = 64, + // Ignore acceleration y + POSITION_TARGET_TYPEMASK_AY_IGNORE = 128, + // Ignore acceleration z + POSITION_TARGET_TYPEMASK_AZ_IGNORE = 256, + // Use force instead of acceleration + POSITION_TARGET_TYPEMASK_FORCE_SET = 512, + // Ignore yaw + POSITION_TARGET_TYPEMASK_YAW_IGNORE = 1024, + // Ignore yaw rate + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE = 2048 +}; + +// Airborne status of UAS. +enum class UTM_FLIGHT_STATE { + // The flight state can't be determined. + UTM_FLIGHT_STATE_UNKNOWN = 1, + // UAS on ground. + UTM_FLIGHT_STATE_GROUND = 2, + // UAS airborne. + UTM_FLIGHT_STATE_AIRBORNE = 3, + // UAS is in an emergency flight state. + UTM_FLIGHT_STATE_EMERGENCY = 16, + // UAS has no active controls. + UTM_FLIGHT_STATE_NOCTRL = 32 +}; + +// Flags for the global position report. +enum class UTM_DATA_AVAIL_FLAGS { + // The field time contains valid data. + UTM_DATA_AVAIL_FLAGS_TIME_VALID = 1, + // The field uas_id contains valid data. + UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE = 2, + // The fields lat, lon and h_acc contain valid data. + UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE = 4, + // The fields alt and v_acc contain valid data. + UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE = 8, + // The field relative_alt contains valid data. + UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE = 16, + // The fields vx and vy contain valid data. + UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE = 32, + // The field vz contains valid data. + UTM_DATA_AVAIL_FLAGS_VERTICAL_VELO_AVAILABLE = 64, + // The fields next_lat, next_lon and next_alt contain valid data. + UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE = 128 +}; + +// Cellular network radio type +enum class CELLULAR_NETWORK_RADIO_TYPE { + CELLULAR_NETWORK_RADIO_TYPE_NONE = 0, + CELLULAR_NETWORK_RADIO_TYPE_GSM = 1, + CELLULAR_NETWORK_RADIO_TYPE_CDMA = 2, + CELLULAR_NETWORK_RADIO_TYPE_WCDMA = 3, + CELLULAR_NETWORK_RADIO_TYPE_LTE = 4 +}; + +// These flags encode the cellular network status +enum class CELLULAR_NETWORK_STATUS_FLAG { + // Roaming is active + CELLULAR_NETWORK_STATUS_FLAG_ROAMING = 1 +}; + +// Precision land modes (used in MAV_CMD_NAV_LAND). +enum class PRECISION_LAND_MODE { + // Normal (non-precision) landing. + PRECISION_LAND_MODE_DISABLED = 0, + // Use precision landing if beacon detected when land command accepted, otherwise + // land normally. + PRECISION_LAND_MODE_OPPORTUNISTIC = 1, + // Use precision landing, searching for beacon if not found when land command + // accepted (land normally if beacon cannot be found). + PRECISION_LAND_MODE_REQUIRED = 2 +}; + +enum class PARACHUTE_ACTION { + // Disable parachute release. + PARACHUTE_DISABLE = 0, + // Enable parachute release. + PARACHUTE_ENABLE = 1, + // Release parachute. + PARACHUTE_RELEASE = 2 +}; + +enum class MAV_TUNNEL_PAYLOAD_TYPE { + // Encoding of payload unknown. + MAV_TUNNEL_PAYLOAD_TYPE_UNKNOWN = 0, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED0 = 200, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED1 = 201, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED2 = 202, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED3 = 203, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED4 = 204, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED5 = 205, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED6 = 206, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED7 = 207, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED8 = 208, + // Registered for STorM32 gimbal controller. + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED9 = 209 +}; + +enum class MAV_ODID_ID_TYPE { + // No type defined. + MAV_ODID_ID_TYPE_NONE = 0, + // Manufacturer Serial Number (ANSI/CTA-2063 format). + MAV_ODID_ID_TYPE_SERIAL_NUMBER = 1, + // CAA (Civil Aviation Authority) registered ID. Format: [ICAO Country Code].[CAA + // Assigned ID]. + MAV_ODID_ID_TYPE_CAA_REGISTRATION_ID = 2, + // UTM (Unmanned Traffic Management) assigned UUID (RFC4122). + MAV_ODID_ID_TYPE_UTM_ASSIGNED_UUID = 3 +}; + +enum class MAV_ODID_UA_TYPE { + // No UA (Unmanned Aircraft) type defined. + MAV_ODID_UA_TYPE_NONE = 0, + // Aeroplane/Airplane. Fixed wing. + MAV_ODID_UA_TYPE_AEROPLANE = 1, + // Rotorcraft (including Multirotor). + MAV_ODID_UA_TYPE_ROTORCRAFT = 2, + // Gyroplane. + MAV_ODID_UA_TYPE_GYROPLANE = 3, + // VTOL (Vertical Take-Off and Landing). Fixed wing aircraft that can take off + // vertically. + MAV_ODID_UA_TYPE_VTOL = 4, + // Ornithopter. + MAV_ODID_UA_TYPE_ORNITHOPTER = 5, + // Glider. + MAV_ODID_UA_TYPE_GLIDER = 6, + // Kite. + MAV_ODID_UA_TYPE_KITE = 7, + // Free Balloon. + MAV_ODID_UA_TYPE_FREE_BALLOON = 8, + // Captive Balloon. + MAV_ODID_UA_TYPE_CAPTIVE_BALLOON = 9, + // Airship. E.g. a blimp. + MAV_ODID_UA_TYPE_AIRSHIP = 10, + // Free Fall/Parachute. + MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE = 11, + // Rocket. + MAV_ODID_UA_TYPE_ROCKET = 12, + // Tethered powered aircraft. + MAV_ODID_UA_TYPE_TETHERED_POWERED_AIRCRAFT = 13, + // Ground Obstacle. + MAV_ODID_UA_TYPE_GROUND_OBSTACLE = 14, + // Other type of aircraft not listed earlier. + MAV_ODID_UA_TYPE_OTHER = 15 +}; + +enum class MAV_ODID_STATUS { + // The status of the (UA) Unmanned Aircraft is undefined. + MAV_ODID_STATUS_UNDECLARED = 0, + // The UA is on the ground. + MAV_ODID_STATUS_GROUND = 1, + // The UA is in the air. + MAV_ODID_STATUS_AIRBORNE = 2 +}; + +enum class MAV_ODID_HEIGHT_REF { + // The height field is relative to the take-off location. + MAV_ODID_HEIGHT_REF_OVER_TAKEOFF = 0, + // The height field is relative to ground. + MAV_ODID_HEIGHT_REF_OVER_GROUND = 1 +}; + +enum class MAV_ODID_HOR_ACC { + // The horizontal accuracy is unknown. + MAV_ODID_HOR_ACC_UNKNOWN = 0, + // The horizontal accuracy is smaller than 10 Nautical Miles. 18.52 km. + MAV_ODID_HOR_ACC_10NM = 1, + // The horizontal accuracy is smaller than 4 Nautical Miles. 7.408 km. + MAV_ODID_HOR_ACC_4NM = 2, + // The horizontal accuracy is smaller than 2 Nautical Miles. 3.704 km. + MAV_ODID_HOR_ACC_2NM = 3, + // The horizontal accuracy is smaller than 1 Nautical Miles. 1.852 km. + MAV_ODID_HOR_ACC_1NM = 4, + // The horizontal accuracy is smaller than 0.5 Nautical Miles. 926 m. + MAV_ODID_HOR_ACC_0_5NM = 5, + // The horizontal accuracy is smaller than 0.3 Nautical Miles. 555.6 m. + MAV_ODID_HOR_ACC_0_3NM = 6, + // The horizontal accuracy is smaller than 0.1 Nautical Miles. 185.2 m. + MAV_ODID_HOR_ACC_0_1NM = 7, + // The horizontal accuracy is smaller than 0.05 Nautical Miles. 92.6 m. + MAV_ODID_HOR_ACC_0_05NM = 8, + // The horizontal accuracy is smaller than 30 meter. + MAV_ODID_HOR_ACC_30_METER = 9, + // The horizontal accuracy is smaller than 10 meter. + MAV_ODID_HOR_ACC_10_METER = 10, + // The horizontal accuracy is smaller than 3 meter. + MAV_ODID_HOR_ACC_3_METER = 11, + // The horizontal accuracy is smaller than 1 meter. + MAV_ODID_HOR_ACC_1_METER = 12 +}; + +enum class MAV_ODID_VER_ACC { + // The vertical accuracy is unknown. + MAV_ODID_VER_ACC_UNKNOWN = 0, + // The vertical accuracy is smaller than 150 meter. + MAV_ODID_VER_ACC_150_METER = 1, + // The vertical accuracy is smaller than 45 meter. + MAV_ODID_VER_ACC_45_METER = 2, + // The vertical accuracy is smaller than 25 meter. + MAV_ODID_VER_ACC_25_METER = 3, + // The vertical accuracy is smaller than 10 meter. + MAV_ODID_VER_ACC_10_METER = 4, + // The vertical accuracy is smaller than 3 meter. + MAV_ODID_VER_ACC_3_METER = 5, + // The vertical accuracy is smaller than 1 meter. + MAV_ODID_VER_ACC_1_METER = 6 +}; + +enum class MAV_ODID_SPEED_ACC { + // The speed accuracy is unknown. + MAV_ODID_SPEED_ACC_UNKNOWN = 0, + // The speed accuracy is smaller than 10 meters per second. + MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND = 1, + // The speed accuracy is smaller than 3 meters per second. + MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND = 2, + // The speed accuracy is smaller than 1 meters per second. + MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND = 3, + // The speed accuracy is smaller than 0.3 meters per second. + MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND = 4 +}; + +enum class MAV_ODID_TIME_ACC { + // The timestamp accuracy is unknown. + MAV_ODID_TIME_ACC_UNKNOWN = 0, + // The timestamp accuracy is smaller than 0.1 second. + MAV_ODID_TIME_ACC_0_1_SECOND = 1, + // The timestamp accuracy is smaller than 0.2 second. + MAV_ODID_TIME_ACC_0_2_SECOND = 2, + // The timestamp accuracy is smaller than 0.3 second. + MAV_ODID_TIME_ACC_0_3_SECOND = 3, + // The timestamp accuracy is smaller than 0.4 second. + MAV_ODID_TIME_ACC_0_4_SECOND = 4, + // The timestamp accuracy is smaller than 0.5 second. + MAV_ODID_TIME_ACC_0_5_SECOND = 5, + // The timestamp accuracy is smaller than 0.6 second. + MAV_ODID_TIME_ACC_0_6_SECOND = 6, + // The timestamp accuracy is smaller than 0.7 second. + MAV_ODID_TIME_ACC_0_7_SECOND = 7, + // The timestamp accuracy is smaller than 0.8 second. + MAV_ODID_TIME_ACC_0_8_SECOND = 8, + // The timestamp accuracy is smaller than 0.9 second. + MAV_ODID_TIME_ACC_0_9_SECOND = 9, + // The timestamp accuracy is smaller than 1.0 second. + MAV_ODID_TIME_ACC_1_0_SECOND = 10, + // The timestamp accuracy is smaller than 1.1 second. + MAV_ODID_TIME_ACC_1_1_SECOND = 11, + // The timestamp accuracy is smaller than 1.2 second. + MAV_ODID_TIME_ACC_1_2_SECOND = 12, + // The timestamp accuracy is smaller than 1.3 second. + MAV_ODID_TIME_ACC_1_3_SECOND = 13, + // The timestamp accuracy is smaller than 1.4 second. + MAV_ODID_TIME_ACC_1_4_SECOND = 14, + // The timestamp accuracy is smaller than 1.5 second. + MAV_ODID_TIME_ACC_1_5_SECOND = 15 +}; + +enum class MAV_ODID_AUTH_TYPE { + // No authentication type is specified. + MAV_ODID_AUTH_TYPE_NONE = 0, + // Signature for the UAS (Unmanned Aircraft System) ID. + MAV_ODID_AUTH_TYPE_UAS_ID_SIGNATURE = 1, + // Signature for the Operator ID. + MAV_ODID_AUTH_TYPE_OPERATOR_ID_SIGNATURE = 2, + // Signature for the entire message set. + MAV_ODID_AUTH_TYPE_MESSAGE_SET_SIGNATURE = 3, + // Authentication is provided by Network Remote ID. + MAV_ODID_AUTH_TYPE_NETWORK_REMOTE_ID = 4 +}; + +enum class MAV_ODID_DESC_TYPE { + // Free-form text description of the purpose of the flight. + MAV_ODID_DESC_TYPE_TEXT = 0 +}; + +enum class MAV_ODID_LOCATION_SRC { + // The location of the operator is the same as the take-off location. + MAV_ODID_LOCATION_SRC_TAKEOFF = 0, + // The location of the operator is based on live GNSS data. + MAV_ODID_LOCATION_SRC_LIVE_GNSS = 1, + // The location of the operator is a fixed location. + MAV_ODID_LOCATION_SRC_FIXED = 2 +}; + +enum class MAV_ODID_OPERATOR_ID_TYPE { + // CAA (Civil Aviation Authority) registered operator ID. + MAV_ODID_OPERATOR_ID_TYPE_CAA = 0 +}; + +// Tune formats (used for vehicle buzzer/tone generation). +enum class TUNE_FORMAT { + // Format is QBasic 1.1 Play: https://www.qbasic.net/en/reference/qb11/Statement/PLAY-006.htm. + TUNE_FORMAT_QBASIC1_1 = 1, + // Format is Modern Music Markup Language (MML): https://en.wikipedia.org/wiki/Music_Macro_Language#Modern_MML. + TUNE_FORMAT_MML_MODERN = 2 +}; + +// Component capability flags (Bitmap) +enum class COMPONENT_CAP_FLAGS { + // Component has parameters, and supports the parameter protocol (PARAM messages). + COMPONENT_CAP_FLAGS_PARAM = 1, + // Component has parameters, and supports the extended parameter protocol (PARAM_EXT + // messages). + COMPONENT_CAP_FLAGS_PARAM_EXT = 2 +}; + +// Type of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html +enum class AIS_TYPE { + // Not available (default). + AIS_TYPE_UNKNOWN = 0, + AIS_TYPE_RESERVED_1 = 1, + AIS_TYPE_RESERVED_2 = 2, + AIS_TYPE_RESERVED_3 = 3, + AIS_TYPE_RESERVED_4 = 4, + AIS_TYPE_RESERVED_5 = 5, + AIS_TYPE_RESERVED_6 = 6, + AIS_TYPE_RESERVED_7 = 7, + AIS_TYPE_RESERVED_8 = 8, + AIS_TYPE_RESERVED_9 = 9, + AIS_TYPE_RESERVED_10 = 10, + AIS_TYPE_RESERVED_11 = 11, + AIS_TYPE_RESERVED_12 = 12, + AIS_TYPE_RESERVED_13 = 13, + AIS_TYPE_RESERVED_14 = 14, + AIS_TYPE_RESERVED_15 = 15, + AIS_TYPE_RESERVED_16 = 16, + AIS_TYPE_RESERVED_17 = 17, + AIS_TYPE_RESERVED_18 = 18, + AIS_TYPE_RESERVED_19 = 19, + // Wing In Ground effect. + AIS_TYPE_WIG = 20, + AIS_TYPE_WIG_HAZARDOUS_A = 21, + AIS_TYPE_WIG_HAZARDOUS_B = 22, + AIS_TYPE_WIG_HAZARDOUS_C = 23, + AIS_TYPE_WIG_HAZARDOUS_D = 24, + AIS_TYPE_WIG_RESERVED_1 = 25, + AIS_TYPE_WIG_RESERVED_2 = 26, + AIS_TYPE_WIG_RESERVED_3 = 27, + AIS_TYPE_WIG_RESERVED_4 = 28, + AIS_TYPE_WIG_RESERVED_5 = 29, + AIS_TYPE_FISHING = 30, + AIS_TYPE_TOWING = 31, + // Towing: length exceeds 200m or breadth exceeds 25m. + AIS_TYPE_TOWING_LARGE = 32, + // Dredging or other underwater ops. + AIS_TYPE_DREDGING = 33, + AIS_TYPE_DIVING = 34, + AIS_TYPE_MILITARY = 35, + AIS_TYPE_SAILING = 36, + AIS_TYPE_PLEASURE = 37, + AIS_TYPE_RESERVED_20 = 38, + AIS_TYPE_RESERVED_21 = 39, + // High Speed Craft. + AIS_TYPE_HSC = 40, + AIS_TYPE_HSC_HAZARDOUS_A = 41, + AIS_TYPE_HSC_HAZARDOUS_B = 42, + AIS_TYPE_HSC_HAZARDOUS_C = 43, + AIS_TYPE_HSC_HAZARDOUS_D = 44, + AIS_TYPE_HSC_RESERVED_1 = 45, + AIS_TYPE_HSC_RESERVED_2 = 46, + AIS_TYPE_HSC_RESERVED_3 = 47, + AIS_TYPE_HSC_RESERVED_4 = 48, + AIS_TYPE_HSC_UNKNOWN = 49, + AIS_TYPE_PILOT = 50, + // Search And Rescue vessel. + AIS_TYPE_SAR = 51, + AIS_TYPE_TUG = 52, + AIS_TYPE_PORT_TENDER = 53, + // Anti-pollution equipment. + AIS_TYPE_ANTI_POLLUTION = 54, + AIS_TYPE_LAW_ENFORCEMENT = 55, + AIS_TYPE_SPARE_LOCAL_1 = 56, + AIS_TYPE_SPARE_LOCAL_2 = 57, + AIS_TYPE_MEDICAL_TRANSPORT = 58, + // Noncombatant ship according to RR Resolution No. 18. + AIS_TYPE_NONECOMBATANT = 59, + AIS_TYPE_PASSENGER = 60, + AIS_TYPE_PASSENGER_HAZARDOUS_A = 61, + AIS_TYPE_PASSENGER_HAZARDOUS_B = 62, + AIS_TYPE_AIS_TYPE_PASSENGER_HAZARDOUS_C = 63, + AIS_TYPE_PASSENGER_HAZARDOUS_D = 64, + AIS_TYPE_PASSENGER_RESERVED_1 = 65, + AIS_TYPE_PASSENGER_RESERVED_2 = 66, + AIS_TYPE_PASSENGER_RESERVED_3 = 67, + AIS_TYPE_AIS_TYPE_PASSENGER_RESERVED_4 = 68, + AIS_TYPE_PASSENGER_UNKNOWN = 69, + AIS_TYPE_CARGO = 70, + AIS_TYPE_CARGO_HAZARDOUS_A = 71, + AIS_TYPE_CARGO_HAZARDOUS_B = 72, + AIS_TYPE_CARGO_HAZARDOUS_C = 73, + AIS_TYPE_CARGO_HAZARDOUS_D = 74, + AIS_TYPE_CARGO_RESERVED_1 = 75, + AIS_TYPE_CARGO_RESERVED_2 = 76, + AIS_TYPE_CARGO_RESERVED_3 = 77, + AIS_TYPE_CARGO_RESERVED_4 = 78, + AIS_TYPE_CARGO_UNKNOWN = 79, + AIS_TYPE_TANKER = 80, + AIS_TYPE_TANKER_HAZARDOUS_A = 81, + AIS_TYPE_TANKER_HAZARDOUS_B = 82, + AIS_TYPE_TANKER_HAZARDOUS_C = 83, + AIS_TYPE_TANKER_HAZARDOUS_D = 84, + AIS_TYPE_TANKER_RESERVED_1 = 85, + AIS_TYPE_TANKER_RESERVED_2 = 86, + AIS_TYPE_TANKER_RESERVED_3 = 87, + AIS_TYPE_TANKER_RESERVED_4 = 88, + AIS_TYPE_TANKER_UNKNOWN = 89, + AIS_TYPE_OTHER = 90, + AIS_TYPE_OTHER_HAZARDOUS_A = 91, + AIS_TYPE_OTHER_HAZARDOUS_B = 92, + AIS_TYPE_OTHER_HAZARDOUS_C = 93, + AIS_TYPE_OTHER_HAZARDOUS_D = 94, + AIS_TYPE_OTHER_RESERVED_1 = 95, + AIS_TYPE_OTHER_RESERVED_2 = 96, + AIS_TYPE_OTHER_RESERVED_3 = 97, + AIS_TYPE_OTHER_RESERVED_4 = 98, + AIS_TYPE_OTHER_UNKNOWN = 99 +}; + +// Navigational status of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html +enum class AIS_NAV_STATUS { + // Under way using engine. + UNDER_WAY = 0, + AIS_NAV_ANCHORED = 1, + AIS_NAV_UN_COMMANDED = 2, + AIS_NAV_RESTRICTED_MANOEUVERABILITY = 3, + AIS_NAV_DRAUGHT_CONSTRAINED = 4, + AIS_NAV_MOORED = 5, + AIS_NAV_AGROUND = 6, + AIS_NAV_FISHING = 7, + AIS_NAV_SAILING = 8, + AIS_NAV_RESERVED_HSC = 9, + AIS_NAV_RESERVED_WIG = 10, + AIS_NAV_RESERVED_1 = 11, + AIS_NAV_RESERVED_2 = 12, + AIS_NAV_RESERVED_3 = 13, + // Search And Rescue Transponder. + AIS_NAV_AIS_SART = 14, + // Not available (default). + AIS_NAV_UNKNOWN = 15 +}; + +// These flags are used in the AIS_VESSEL.fields bitmask to indicate validity of data +// in the other message fields. When set, the data is valid. +enum class AIS_FLAGS { + // 1 = Position accuracy less than 10m, 0 = position accuracy greater than 10m. + AIS_FLAGS_POSITION_ACCURACY = 1, + AIS_FLAGS_VALID_COG = 2, + AIS_FLAGS_VALID_VELOCITY = 4, + // 1 = Velocity over 52.5765m/s (102.2 knots) + AIS_FLAGS_HIGH_VELOCITY = 8, + AIS_FLAGS_VALID_TURN_RATE = 16, + // Only the sign of the returned turn rate value is valid, either greater than + // 5deg/30s or less than -5deg/30s + AIS_FLAGS_TURN_RATE_SIGN_ONLY = 32, + AIS_FLAGS_VALID_DIMENSIONS = 64, + // Distance to bow is larger than 511m + AIS_FLAGS_LARGE_BOW_DIMENSION = 128, + // Distance to stern is larger than 511m + AIS_FLAGS_LARGE_STERN_DIMENSION = 256, + // Distance to port side is larger than 63m + AIS_FLAGS_LARGE_PORT_DIMENSION = 512, + // Distance to starboard side is larger than 63m + AIS_FLAGS_LARGE_STARBOARD_DIMENSION = 1024, + AIS_FLAGS_VALID_CALLSIGN = 2048, + AIS_FLAGS_VALID_NAME = 4096 +}; + +// The heartbeat message shows that a system or component is present and responding. +// The type and autopilot fields (along with the message component id), allow the +// receiving system to treat further messages from this system appropriately (e.g. +// by laying out the user interface based on the autopilot). This microservice is +// documented at https://mavlink.io/en/services/heartbeat.html class MavLinkHeartbeat : public MavLinkMessageBase { public: const static uint8_t kMessageId = 0; MavLinkHeartbeat() { msgid = kMessageId; } - // A bitfield for use for autopilot-specific flags. - uint32_t custom_mode = 0; - // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE - // ENUM) + // Vehicle or component type. For a flight controller component the vehicle type + // (quadrotor, helicopter, etc.). For other components the component type (e.g. + // camera, gimbal, etc.). This should be used in preference to component id for + // identifying the component type. uint8_t type = 0; - // Autopilot type / class. defined in MAV_AUTOPILOT ENUM + // Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not + // flight controllers. uint8_t autopilot = 0; - // System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + // System mode bitmap. uint8_t base_mode = 0; - // System status flag, see MAV_STATE ENUM + // A bitfield for use for autopilot-specific flags + uint32_t custom_mode = 0; + // System status flag. uint8_t system_status = 0; // MAVLink version, not writable by user, gets added by protocol because of magic // data type: uint8_t_mavlink_version @@ -1449,36 +2815,35 @@ class MavLinkHeartbeat : public MavLinkMessageBase { // or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current // flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. // This represents the internal navigation state machine. The system status shows -// wether the system is currently active or not and if an emergency occured. During +// whether the system is currently active or not and if an emergency occurred. During // the CRITICAL and EMERGENCY states the MAV is still considered to be active, but -// should start emergency procedures autonomously. After a failure occured it should +// should start emergency procedures autonomously. After a failure occurred it should // first move from active to critical to allow manual intervention and then move to // emergency after a certain timeout. class MavLinkSysStatus : public MavLinkMessageBase { public: const static uint8_t kMessageId = 1; MavLinkSysStatus() { msgid = kMessageId; } - // Bitmask showing which onboard controllers and sensors are present. Value of - // 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + // Bitmap showing which onboard controllers and sensors are present. Value of + // 0: not present. Value of 1: present. uint32_t onboard_control_sensors_present = 0; - // Bitmask showing which onboard controllers and sensors are enabled: Value of - // 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + // Bitmap showing which onboard controllers and sensors are enabled: Value of + // 0: not enabled. Value of 1: enabled. uint32_t onboard_control_sensors_enabled = 0; - // Bitmask showing which onboard controllers and sensors are operational or have - // an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by - // ENUM MAV_SYS_STATUS_SENSOR + // Bitmap showing which onboard controllers and sensors have an error (or are + // operational). Value of 0: error. Value of 1: healthy. uint32_t onboard_control_sensors_health = 0; - // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be - // always below 1000 + // Maximum usage in percent of the mainloop time. Values: [0-1000] - should always + // be below 1000 uint16_t load = 0; - // Battery voltage, in millivolts (1 = 1 millivolt) + // Battery voltage, UINT16_MAX: Voltage not sent by autopilot uint16_t voltage_battery = 0; - // Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does - // not measure the current + // Battery current, -1: Current not sent by autopilot int16_t current_battery = 0; - // Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), - // dropped packets on all links (packets that were corrupted on reception on the - // MAV) + // Battery energy remaining, -1: Battery remaining energy not sent by autopilot + int8_t battery_remaining = 0; + // Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links + // (packets that were corrupted on reception on the MAV) uint16_t drop_rate_comm = 0; // Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets // that were corrupted on reception on the MAV) @@ -1491,9 +2856,6 @@ class MavLinkSysStatus : public MavLinkMessageBase { uint16_t errors_count3 = 0; // Autopilot-specific errors uint16_t errors_count4 = 0; - // Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining - // battery - int8_t battery_remaining = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -1506,9 +2868,9 @@ class MavLinkSystemTime : public MavLinkMessageBase { public: const static uint8_t kMessageId = 2; MavLinkSystemTime() { msgid = kMessageId; } - // Timestamp of the master clock in microseconds since UNIX epoch. + // Timestamp (UNIX epoch time). uint64_t time_unix_usec = 0; - // Timestamp of the component clock since boot time in milliseconds. + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; virtual std::string toJSon(); protected: @@ -1517,21 +2879,23 @@ class MavLinkSystemTime : public MavLinkMessageBase { }; // A ping message either requesting or responding to a ping. This allows to measure -// the system latencies, including serial port, radio modem and UDP connections. +// the system latencies, including serial port, radio modem and UDP connections. The +// ping microservice is documented at https://mavlink.io/en/services/ping.html class MavLinkPing : public MavLinkMessageBase { public: const static uint8_t kMessageId = 4; MavLinkPing() { msgid = kMessageId; } - // Unix timestamp in microseconds or since system boot if smaller than MAVLink - // epoch (1.1.2009) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // PING sequence uint32_t seq = 0; - // 0: request ping from all receiving systems, if greater than 0: message is a + // 0: request ping from all receiving systems. If greater than 0: message is a // ping response and number is the system id of the requesting system uint8_t target_system = 0; - // 0: request ping from all receiving components, if greater than 0: message is - // a ping response and number is the system id of the requesting system + // 0: request ping from all receiving components. If greater than 0: message is + // a ping response and number is the component id of the requesting component. uint8_t target_component = 0; virtual std::string toJSon(); protected: @@ -1595,19 +2959,52 @@ class MavLinkAuthKey : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// THIS INTERFACE IS DEPRECATED. USE COMMAND_LONG with MAV_CMD_DO_SET_MODE INSTEAD. +// Status generated in each node in the communication chain and injected into MAVLink +// stream. +class MavLinkLinkNodeStatus : public MavLinkMessageBase { +public: + const static uint8_t kMessageId = 8; + MavLinkLinkNodeStatus() { msgid = kMessageId; } + // Timestamp (time since system boot). + uint64_t timestamp = 0; + // Remaining free transmit buffer space + uint8_t tx_buf = 0; + // Remaining free receive buffer space + uint8_t rx_buf = 0; + // Transmit rate + uint32_t tx_rate = 0; + // Receive rate + uint32_t rx_rate = 0; + // Number of bytes that could not be parsed correctly. + uint16_t rx_parse_err = 0; + // Transmit buffer overflows. This number wraps around as it reaches UINT16_MAX + uint16_t tx_overflows = 0; + // Receive buffer overflows. This number wraps around as it reaches UINT16_MAX + uint16_t rx_overflows = 0; + // Messages sent + uint32_t messages_sent = 0; + // Messages received (estimated from counting seq) + uint32_t messages_received = 0; + // Messages lost (estimated from counting seq) + uint32_t messages_lost = 0; + virtual std::string toJSon(); +protected: + virtual int pack(char* buffer) const; + virtual int unpack(const char* buffer); +}; + // Set the system mode, as defined by enum MAV_MODE. There is no target component // id as the mode is by definition for the overall aircraft, not only for one component. class MavLinkSetMode : public MavLinkMessageBase { public: const static uint8_t kMessageId = 11; MavLinkSetMode() { msgid = kMessageId; } - // The new autopilot-specific mode. This field can be ignored by an autopilot. - uint32_t custom_mode = 0; // The system setting the mode uint8_t target_system = 0; - // The new base mode + // The new base mode. uint8_t base_mode = 0; + // The new autopilot-specific mode. This field can be ignored by an autopilot. + uint32_t custom_mode = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -1618,15 +3015,12 @@ class MavLinkSetMode : public MavLinkMessageBase { // are stored as key[const char*] -> value[float]. This allows to send a parameter // to any other component (such as the GCS) without the need of previous knowledge // of possible parameter names. Thus the same GCS can store different parameters for -// different autopilots. See also http://qgroundcontrol.org/parameter_interface for +// different autopilots. See also https://mavlink.io/en/services/parameter.html for // a full documentation of QGroundControl and IMU code. class MavLinkParamRequestRead : public MavLinkMessageBase { public: const static uint8_t kMessageId = 20; MavLinkParamRequestRead() { msgid = kMessageId; } - // Parameter index. Send -1 to use the param ID field as identifier (else the - // param id will be ignored) - int16_t param_index = 0; // System ID uint8_t target_system = 0; // Component ID @@ -1636,6 +3030,9 @@ class MavLinkParamRequestRead : public MavLinkMessageBase { // chars - applications have to provide 16+1 bytes storage if the ID is stored // as string char param_id[16] = { 0 }; + // Parameter index. Send -1 to use the param ID field as identifier (else the + // param id will be ignored) + int16_t param_index = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -1643,7 +3040,7 @@ class MavLinkParamRequestRead : public MavLinkMessageBase { }; // Request all parameters of this component. After this request, all parameters are -// emitted. +// emitted. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html class MavLinkParamRequestList : public MavLinkMessageBase { public: const static uint8_t kMessageId = 21; @@ -1660,43 +3057,41 @@ class MavLinkParamRequestList : public MavLinkMessageBase { // Emit the value of a onboard parameter. The inclusion of param_count and param_index // in the message allows the recipient to keep track of received parameters and allows -// him to re-request missing parameters after a loss or timeout. +// him to re-request missing parameters after a loss or timeout. The parameter microservice +// is documented at https://mavlink.io/en/services/parameter.html class MavLinkParamValue : public MavLinkMessageBase { public: const static uint8_t kMessageId = 22; MavLinkParamValue() { msgid = kMessageId; } - // Onboard parameter value - float param_value = 0; - // Total number of onboard parameters - uint16_t param_count = 0; - // Index of this onboard parameter - uint16_t param_index = 0; // Onboard parameter id, terminated by NULL if the length is less than 16 human-readable // chars and WITHOUT null termination (NULL) byte if the length is exactly 16 // chars - applications have to provide 16+1 bytes storage if the ID is stored // as string char param_id[16] = { 0 }; - // Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + // Onboard parameter value + float param_value = 0; + // Onboard parameter type. uint8_t param_type = 0; + // Total number of onboard parameters + uint16_t param_count = 0; + // Index of this onboard parameter + uint16_t param_index = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Set a parameter value TEMPORARILY to RAM. It will be reset to default on system -// reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents -// to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter -// value by sending a param_value message to all communication partners. This will -// also ensure that multiple GCS all have an up-to-date list of all parameters. If -// the sending GCS did not receive a PARAM_VALUE message within its timeout time, -// it should re-send the PARAM_SET message. +// Set a parameter value (write new value to permanent storage). IMPORTANT: The receiving +// component should acknowledge the new parameter value by sending a PARAM_VALUE message +// to all communication partners. This will also ensure that multiple GCS all have +// an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE +// message within its timeout time, it should re-send the PARAM_SET message. The parameter +// microservice is documented at https://mavlink.io/en/services/parameter.html class MavLinkParamSet : public MavLinkMessageBase { public: const static uint8_t kMessageId = 23; MavLinkParamSet() { msgid = kMessageId; } - // Onboard parameter value - float param_value = 0; // System ID uint8_t target_system = 0; // Component ID @@ -1706,7 +3101,9 @@ class MavLinkParamSet : public MavLinkMessageBase { // chars - applications have to provide 16+1 bytes storage if the ID is stored // as string char param_id[16] = { 0 }; - // Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + // Onboard parameter value + float param_value = 0; + // Onboard parameter type. uint8_t param_type = 0; virtual std::string toJSon(); protected: @@ -1716,34 +3113,45 @@ class MavLinkParamSet : public MavLinkMessageBase { // The global position, as returned by the Global Positioning System (GPS). This is // NOT the global position estimate of the system, but rather a RAW sensor value. -// See message GLOBAL_POSITION for the global position estimate. Coordinate frame -// is right-handed, Z-axis up (GPS frame). +// See message GLOBAL_POSITION for the global position estimate. class MavLinkGpsRawInt : public MavLinkMessageBase { public: const static uint8_t kMessageId = 24; MavLinkGpsRawInt() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Latitude (WGS84), in degrees * 1E7 + // GPS fix type. + uint8_t fix_type = 0; + // Latitude (WGS84, EGM96 ellipsoid) int32_t lat = 0; - // Longitude (WGS84), in degrees * 1E7 + // Longitude (WGS84, EGM96 ellipsoid) int32_t lon = 0; - // Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually - // all GPS modules provide the AMSL altitude in addition to the WGS84 altitude. + // Altitude (MSL). Positive for up. Note that virtually all GPS modules provide + // the MSL altitude in addition to the WGS84 altitude. int32_t alt = 0; // GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX uint16_t eph = 0; // GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX uint16_t epv = 0; - // GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + // GPS ground speed. If unknown, set to: UINT16_MAX uint16_t vel = 0; // Course over ground (NOT heading, but direction of movement) in degrees * 100, // 0.0..359.99 degrees. If unknown, set to: UINT16_MAX uint16_t cog = 0; - // See the GPS_FIX_TYPE enum. - uint8_t fix_type = 0; // Number of satellites visible. If unknown, set to 255 uint8_t satellites_visible = 0; + // Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + int32_t alt_ellipsoid = 0; + // Position uncertainty. Positive for up. + uint32_t h_acc = 0; + // Altitude uncertainty. Positive for up. + uint32_t v_acc = 0; + // Speed uncertainty. Positive for up. + uint32_t vel_acc = 0; + // Heading / track uncertainty + uint32_t hdg_acc = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -1782,40 +3190,45 @@ class MavLinkScaledImu : public MavLinkMessageBase { public: const static uint8_t kMessageId = 26; MavLinkScaledImu() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // X acceleration (mg) + // X acceleration int16_t xacc = 0; - // Y acceleration (mg) + // Y acceleration int16_t yacc = 0; - // Z acceleration (mg) + // Z acceleration int16_t zacc = 0; - // Angular speed around X axis (millirad /sec) + // Angular speed around X axis int16_t xgyro = 0; - // Angular speed around Y axis (millirad /sec) + // Angular speed around Y axis int16_t ygyro = 0; - // Angular speed around Z axis (millirad /sec) + // Angular speed around Z axis int16_t zgyro = 0; - // X Magnetic field (milli tesla) + // X Magnetic field int16_t xmag = 0; - // Y Magnetic field (milli tesla) + // Y Magnetic field int16_t ymag = 0; - // Z Magnetic field (milli tesla) + // Z Magnetic field int16_t zmag = 0; + // Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C + // it must send 1 (0.01C). + int16_t temperature = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// The RAW IMU readings for the usual 9DOF sensor setup. This message should always -// contain the true raw values without any scaling to allow data capture and system -// debugging. +// The RAW IMU readings for a 9DOF sensor, which is identified by the id (default +// IMU1). This message should always contain the true raw values without any scaling +// to allow data capture and system debugging. class MavLinkRawImu : public MavLinkMessageBase { public: const static uint8_t kMessageId = 27; MavLinkRawImu() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // X acceleration (raw) int16_t xacc = 0; @@ -1835,6 +3248,12 @@ class MavLinkRawImu : public MavLinkMessageBase { int16_t ymag = 0; // Z Magnetic field (raw) int16_t zmag = 0; + // Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will + // have a message with id=0) + uint8_t id = 0; + // Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C + // it must send 1 (0.01C). + int16_t temperature = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -1848,13 +3267,15 @@ class MavLinkRawPressure : public MavLinkMessageBase { public: const static uint8_t kMessageId = 28; MavLinkRawPressure() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // Absolute pressure (raw) int16_t press_abs = 0; - // Differential pressure 1 (raw, 0 if nonexistant) + // Differential pressure 1 (raw, 0 if nonexistent) int16_t press_diff1 = 0; - // Differential pressure 2 (raw, 0 if nonexistant) + // Differential pressure 2 (raw, 0 if nonexistent) int16_t press_diff2 = 0; // Raw Temperature measurement (raw) int16_t temperature = 0; @@ -1870,13 +3291,13 @@ class MavLinkScaledPressure : public MavLinkMessageBase { public: const static uint8_t kMessageId = 29; MavLinkScaledPressure() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Absolute pressure (hectopascal) + // Absolute pressure float press_abs = 0; - // Differential pressure 1 (hectopascal) + // Differential pressure 1 float press_diff = 0; - // Temperature measurement (0.01 degrees celsius) + // Temperature int16_t temperature = 0; virtual std::string toJSon(); protected: @@ -1889,19 +3310,19 @@ class MavLinkAttitude : public MavLinkMessageBase { public: const static uint8_t kMessageId = 30; MavLinkAttitude() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Roll angle (rad, -pi..+pi) + // Roll angle (-pi..+pi) float roll = 0; - // Pitch angle (rad, -pi..+pi) + // Pitch angle (-pi..+pi) float pitch = 0; - // Yaw angle (rad, -pi..+pi) + // Yaw angle (-pi..+pi) float yaw = 0; - // Roll angular speed (rad/s) + // Roll angular speed float rollspeed = 0; - // Pitch angular speed (rad/s) + // Pitch angular speed float pitchspeed = 0; - // Yaw angular speed (rad/s) + // Yaw angular speed float yawspeed = 0; virtual std::string toJSon(); protected: @@ -1916,7 +3337,7 @@ class MavLinkAttitudeQuaternion : public MavLinkMessageBase { public: const static uint8_t kMessageId = 31; MavLinkAttitudeQuaternion() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; // Quaternion component 1, w (1 in null-rotation) float q1 = 0; @@ -1926,12 +3347,20 @@ class MavLinkAttitudeQuaternion : public MavLinkMessageBase { float q3 = 0; // Quaternion component 4, z (0 in null-rotation) float q4 = 0; - // Roll angular speed (rad/s) + // Roll angular speed float rollspeed = 0; - // Pitch angular speed (rad/s) + // Pitch angular speed float pitchspeed = 0; - // Yaw angular speed (rad/s) + // Yaw angular speed float yawspeed = 0; + // Rotation offset by which the attitude quaternion and angular speed vector should + // be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation + // is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended + // for systems in which the reference attitude may change during flight. For example, + // tailsitters VTOLs rotate their reference attitude by 90 degrees between hover + // mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover + // mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. + float repr_offset_q[4] = { 0 }; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -1944,7 +3373,7 @@ class MavLinkLocalPositionNed : public MavLinkMessageBase { public: const static uint8_t kMessageId = 32; MavLinkLocalPositionNed() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; // X Position float x = 0; @@ -1971,25 +3400,24 @@ class MavLinkGlobalPositionInt : public MavLinkMessageBase { public: const static uint8_t kMessageId = 33; MavLinkGlobalPositionInt() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Latitude, expressed as degrees * 1E7 + // Latitude, expressed int32_t lat = 0; - // Longitude, expressed as degrees * 1E7 + // Longitude, expressed int32_t lon = 0; - // Altitude in meters, expressed as * 1000 (millimeters), AMSL (not WGS84 - note - // that virtually all GPS modules provide the AMSL as well) + // Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and + // MSL. int32_t alt = 0; - // Altitude above ground in meters, expressed as * 1000 (millimeters) + // Altitude above ground int32_t relative_alt = 0; - // Ground X Speed (Latitude, positive north), expressed as m/s * 100 + // Ground X Speed (Latitude, positive north) int16_t vx = 0; - // Ground Y Speed (Longitude, positive east), expressed as m/s * 100 + // Ground Y Speed (Longitude, positive east) int16_t vy = 0; - // Ground Z Speed (Altitude, positive down), expressed as m/s * 100 + // Ground Z Speed (Altitude, positive down) int16_t vz = 0; - // Vehicle heading (yaw angle) in degrees * 100, 0.0..359.99 degrees. If unknown, - // set to: UINT16_MAX + // Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX uint16_t hdg = 0; virtual std::string toJSon(); protected: @@ -1997,42 +3425,35 @@ class MavLinkGlobalPositionInt : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000. +// The scaled values of the RC channels received: (-100%) -10000, (0%) 0, (100%) 10000. // Channels that are inactive should be set to UINT16_MAX. class MavLinkRcChannelsScaled : public MavLinkMessageBase { public: const static uint8_t kMessageId = 34; MavLinkRcChannelsScaled() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) - // INT16_MAX. + // Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk + // should use: 0 = MAIN, 1 = AUX. + uint8_t port = 0; + // RC channel 1 value scaled. int16_t chan1_scaled = 0; - // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) - // INT16_MAX. + // RC channel 2 value scaled. int16_t chan2_scaled = 0; - // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) - // INT16_MAX. + // RC channel 3 value scaled. int16_t chan3_scaled = 0; - // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) - // INT16_MAX. + // RC channel 4 value scaled. int16_t chan4_scaled = 0; - // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) - // INT16_MAX. + // RC channel 5 value scaled. int16_t chan5_scaled = 0; - // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) - // INT16_MAX. + // RC channel 6 value scaled. int16_t chan6_scaled = 0; - // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) - // INT16_MAX. + // RC channel 7 value scaled. int16_t chan7_scaled = 0; - // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) - // INT16_MAX. + // RC channel 8 value scaled. int16_t chan8_scaled = 0; - // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, - // but this allows for more than 8 servos. - uint8_t port = 0; - // Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + // Receive signal strength indicator in device-dependent units/scale. Values: + // [0-254], 255: invalid/unknown. uint8_t rssi = 0; virtual std::string toJSon(); protected: @@ -2041,42 +3462,35 @@ class MavLinkRcChannelsScaled : public MavLinkMessageBase { }; // The RAW values of the RC channels received. The standard PPM modulation is as follows: -// 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters -// might violate this specification. +// 1000 microseconds: 0%, 2000 microseconds: 100%. A value of UINT16_MAX implies the +// channel is unused. Individual receivers/transmitters might violate this specification. class MavLinkRcChannelsRaw : public MavLinkMessageBase { public: const static uint8_t kMessageId = 35; MavLinkRcChannelsRaw() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk + // should use: 0 = MAIN, 1 = AUX. + uint8_t port = 0; + // RC channel 1 value. uint16_t chan1_raw = 0; - // RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 2 value. uint16_t chan2_raw = 0; - // RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 3 value. uint16_t chan3_raw = 0; - // RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 4 value. uint16_t chan4_raw = 0; - // RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 5 value. uint16_t chan5_raw = 0; - // RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 6 value. uint16_t chan6_raw = 0; - // RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 7 value. uint16_t chan7_raw = 0; - // RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 8 value. uint16_t chan8_raw = 0; - // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, - // but this allows for more than 8 servos. - uint8_t port = 0; - // Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + // Receive signal strength indicator in device-dependent units/scale. Values: + // [0-254], 255: invalid/unknown. uint8_t rssi = 0; virtual std::string toJSon(); protected: @@ -2084,71 +3498,75 @@ class MavLinkRcChannelsRaw : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS -// messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 -// microseconds: 100%. +// Superseded by ACTUATOR_OUTPUT_STATUS. The RAW values of the servo outputs (for +// RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation +// is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. class MavLinkServoOutputRaw : public MavLinkMessageBase { public: const static uint8_t kMessageId = 36; MavLinkServoOutputRaw() { msgid = kMessageId; } - // Timestamp (microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint32_t time_usec = 0; - // Servo output 1 value, in microseconds + // Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk + // should use: 0 = MAIN, 1 = AUX. + uint8_t port = 0; + // Servo output 1 value uint16_t servo1_raw = 0; - // Servo output 2 value, in microseconds + // Servo output 2 value uint16_t servo2_raw = 0; - // Servo output 3 value, in microseconds + // Servo output 3 value uint16_t servo3_raw = 0; - // Servo output 4 value, in microseconds + // Servo output 4 value uint16_t servo4_raw = 0; - // Servo output 5 value, in microseconds + // Servo output 5 value uint16_t servo5_raw = 0; - // Servo output 6 value, in microseconds + // Servo output 6 value uint16_t servo6_raw = 0; - // Servo output 7 value, in microseconds + // Servo output 7 value uint16_t servo7_raw = 0; - // Servo output 8 value, in microseconds + // Servo output 8 value uint16_t servo8_raw = 0; - // Servo output 9 value, in microseconds + // Servo output 9 value uint16_t servo9_raw = 0; - // Servo output 10 value, in microseconds + // Servo output 10 value uint16_t servo10_raw = 0; - // Servo output 11 value, in microseconds + // Servo output 11 value uint16_t servo11_raw = 0; - // Servo output 12 value, in microseconds + // Servo output 12 value uint16_t servo12_raw = 0; - // Servo output 13 value, in microseconds + // Servo output 13 value uint16_t servo13_raw = 0; - // Servo output 14 value, in microseconds + // Servo output 14 value uint16_t servo14_raw = 0; - // Servo output 15 value, in microseconds + // Servo output 15 value uint16_t servo15_raw = 0; - // Servo output 16 value, in microseconds + // Servo output 16 value uint16_t servo16_raw = 0; - // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, - // but this allows to encode more than 8 servos. - uint8_t port = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Request a partial list of mission items from the system/component. http://qgroundcontrol.org/mavlink/waypoint_protocol. +// Request a partial list of mission items from the system/component. https://mavlink.io/en/services/mission.html. // If start and end index are the same, just send one waypoint. class MavLinkMissionRequestPartialList : public MavLinkMessageBase { public: const static uint8_t kMessageId = 37; MavLinkMissionRequestPartialList() { msgid = kMessageId; } - // Start index, 0 by default - int16_t start_index = 0; - // End index, -1 by default (-1: send list to end). Else a valid index of the - // list - int16_t end_index = 0; // System ID uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Start index + int16_t start_index = 0; + // End index, -1 by default (-1: send list to end). Else a valid index of the + // list + int16_t end_index = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2162,15 +3580,17 @@ class MavLinkMissionWritePartialList : public MavLinkMessageBase { public: const static uint8_t kMessageId = 38; MavLinkMissionWritePartialList() { msgid = kMessageId; } - // Start index, 0 by default and smaller / equal to the largest index of the current - // onboard list. - int16_t start_index = 0; - // End index, equal or greater than start index. - int16_t end_index = 0; // System ID uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Start index. Must be smaller / equal to the largest index of the current onboard + // list. + int16_t start_index = 0; + // End index, equal or greater than start index. + int16_t end_index = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2181,11 +3601,25 @@ class MavLinkMissionWritePartialList : public MavLinkMessageBase { // of a mission item and to set a mission item on the system. The mission item can // be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame // is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). See also -// http://qgroundcontrol.org/mavlink/waypoint_protocol. +// https://mavlink.io/en/services/mission.html. class MavLinkMissionItem : public MavLinkMessageBase { public: const static uint8_t kMessageId = 39; MavLinkMissionItem() { msgid = kMessageId; } + // System ID + uint8_t target_system = 0; + // Component ID + uint8_t target_component = 0; + // Sequence + uint16_t seq = 0; + // The coordinate system of the waypoint. + uint8_t frame = 0; + // The scheduled action for the waypoint. + uint16_t command = 0; + // false:0, true:1 + uint8_t current = 0; + // Autocontinue to next waypoint + uint8_t autocontinue = 0; // PARAM1, see MAV_CMD enum float param1 = 0; // PARAM2, see MAV_CMD enum @@ -2194,26 +3628,15 @@ class MavLinkMissionItem : public MavLinkMessageBase { float param3 = 0; // PARAM4, see MAV_CMD enum float param4 = 0; - // PARAM5 / local: x position, global: latitude + // PARAM5 / local: X coordinate, global: latitude float x = 0; - // PARAM6 / y position: global: longitude + // PARAM6 / local: Y coordinate, global: longitude float y = 0; - // PARAM7 / z position: global: altitude (relative or absolute, depending on frame. + // PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending + // on frame). float z = 0; - // Sequence - uint16_t seq = 0; - // The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs - uint16_t command = 0; - // System ID - uint8_t target_system = 0; - // Component ID - uint8_t target_component = 0; - // The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h - uint8_t frame = 0; - // false:0, true:1 - uint8_t current = 0; - // autocontinue to next wp - uint8_t autocontinue = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2221,17 +3644,19 @@ class MavLinkMissionItem : public MavLinkMessageBase { }; // Request the information of the mission item with the sequence number seq. The response -// of the system to this message should be a MISSION_ITEM message. http://qgroundcontrol.org/mavlink/waypoint_protocol +// of the system to this message should be a MISSION_ITEM message. https://mavlink.io/en/services/mission.html class MavLinkMissionRequest : public MavLinkMessageBase { public: const static uint8_t kMessageId = 40; MavLinkMissionRequest() { msgid = kMessageId; } - // Sequence - uint16_t seq = 0; // System ID uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Sequence + uint16_t seq = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2245,12 +3670,12 @@ class MavLinkMissionSetCurrent : public MavLinkMessageBase { public: const static uint8_t kMessageId = 41; MavLinkMissionSetCurrent() { msgid = kMessageId; } - // Sequence - uint16_t seq = 0; // System ID uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Sequence + uint16_t seq = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2280,6 +3705,8 @@ class MavLinkMissionRequestList : public MavLinkMessageBase { uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2288,17 +3715,19 @@ class MavLinkMissionRequestList : public MavLinkMessageBase { // This message is emitted as response to MISSION_REQUEST_LIST by the MAV and to initiate // a write transaction. The GCS can then request the individual mission item based -// on the knowledge of the total number of MISSIONs. +// on the knowledge of the total number of waypoints. class MavLinkMissionCount : public MavLinkMessageBase { public: const static uint8_t kMessageId = 44; MavLinkMissionCount() { msgid = kMessageId; } - // Number of mission items in the sequence - uint16_t count = 0; // System ID uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Number of mission items in the sequence + uint16_t count = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2314,6 +3743,8 @@ class MavLinkMissionClearAll : public MavLinkMessageBase { uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2322,7 +3753,7 @@ class MavLinkMissionClearAll : public MavLinkMessageBase { // A certain mission item has been reached. The system will either hold this position // (or circle on the orbit) or (if the autocontinue on the WP was set) continue to -// the next MISSION. +// the next waypoint. class MavLinkMissionItemReached : public MavLinkMessageBase { public: const static uint8_t kMessageId = 46; @@ -2335,8 +3766,8 @@ class MavLinkMissionItemReached : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// Ack message during MISSION handling. The type field states if this message is a -// positive ack (type=0) or if an error happened (type=non-zero). +// Acknowledgment message during waypoint handling. The type field states if this +// message is a positive ack (type=0) or if an error happened (type=non-zero). class MavLinkMissionAck : public MavLinkMessageBase { public: const static uint8_t kMessageId = 47; @@ -2345,73 +3776,72 @@ class MavLinkMissionAck : public MavLinkMessageBase { uint8_t target_system = 0; // Component ID uint8_t target_component = 0; - // See MAV_MISSION_RESULT enum + // Mission result. uint8_t type = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// As local waypoints exist, the global MISSION reference allows to transform between -// the local coordinate frame and the global (GPS) coordinate frame. This can be necessary -// when e.g. in- and outdoor settings are connected and the MAV should move from in- -// to outdoor. +// Sets the GPS co-ordinates of the vehicle local origin (0,0,0) position. Vehicle +// should emit GPS_GLOBAL_ORIGIN irrespective of whether the origin is changed. This +// enables transform between the local coordinate frame and the global (GPS) coordinate +// frame, which may be necessary when (for example) indoor and outdoor settings are +// connected and the MAV should move from in- to outdoor. class MavLinkSetGpsGlobalOrigin : public MavLinkMessageBase { public: const static uint8_t kMessageId = 48; MavLinkSetGpsGlobalOrigin() { msgid = kMessageId; } - // Latitude (WGS84), in degrees * 1E7 + // System ID + uint8_t target_system = 0; + // Latitude (WGS84) int32_t latitude = 0; - // Longitude (WGS84, in degrees * 1E7 + // Longitude (WGS84) int32_t longitude = 0; - // Altitude (AMSL), in meters * 1000 (positive for up) + // Altitude (MSL). Positive for up. int32_t altitude = 0; - // System ID - uint8_t target_system = 0; + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. + uint64_t time_usec = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Once the MAV sets a new GPS-Local correspondence, this message announces the origin -// (0,0,0) position +// Publishes the GPS co-ordinates of the vehicle local origin (0,0,0) position. Emitted +// whenever a new GPS-Local position mapping is requested or set - e.g. following +// SET_GPS_GLOBAL_ORIGIN message. class MavLinkGpsGlobalOrigin : public MavLinkMessageBase { public: const static uint8_t kMessageId = 49; MavLinkGpsGlobalOrigin() { msgid = kMessageId; } - // Latitude (WGS84), in degrees * 1E7 + // Latitude (WGS84) int32_t latitude = 0; - // Longitude (WGS84), in degrees * 1E7 + // Longitude (WGS84) int32_t longitude = 0; - // Altitude (AMSL), in meters * 1000 (positive for up) + // Altitude (MSL). Positive for up. int32_t altitude = 0; + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. + uint64_t time_usec = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Bind a RC channel to a parameter. The parameter should change accoding to the RC -// channel value. +// Bind a RC channel to a parameter. The parameter should change according to the +// RC channel value. class MavLinkParamMapRc : public MavLinkMessageBase { public: const static uint8_t kMessageId = 50; MavLinkParamMapRc() { msgid = kMessageId; } - // Initial parameter value - float param_value0 = 0; - // Scale, maps the RC range [-1, 1] to a parameter value - float scale = 0; - // Minimum param value. The protocol does not define if this overwrites an onboard - // minimum value. (Depends on implementation) - float param_value_min = 0; - // Maximum param value. The protocol does not define if this overwrites an onboard - // maximum value. (Depends on implementation) - float param_value_max = 0; - // Parameter index. Send -1 to use the param ID field as identifier (else the - // param id will be ignored), send -2 to disable any existing map for this rc_channel_index. - int16_t param_index = 0; // System ID uint8_t target_system = 0; // Component ID @@ -2421,9 +3851,22 @@ class MavLinkParamMapRc : public MavLinkMessageBase { // chars - applications have to provide 16+1 bytes storage if the ID is stored // as string char param_id[16] = { 0 }; - // Index of parameter RC channel. Not equal to the RC channel id. Typically correpsonds + // Parameter index. Send -1 to use the param ID field as identifier (else the + // param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + int16_t param_index = 0; + // Index of parameter RC channel. Not equal to the RC channel id. Typically corresponds // to a potentiometer-knob on the RC. uint8_t parameter_rc_channel_index = 0; + // Initial parameter value + float param_value0 = 0; + // Scale, maps the RC range [-1, 1] to a parameter value + float scale = 0; + // Minimum param value. The protocol does not define if this overwrites an onboard + // minimum value. (Depends on implementation) + float param_value_min = 0; + // Maximum param value. The protocol does not define if this overwrites an onboard + // maximum value. (Depends on implementation) + float param_value_max = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2431,17 +3874,42 @@ class MavLinkParamMapRc : public MavLinkMessageBase { }; // Request the information of the mission item with the sequence number seq. The response -// of the system to this message should be a MISSION_ITEM_INT message. http://qgroundcontrol.org/mavlink/waypoint_protocol +// of the system to this message should be a MISSION_ITEM_INT message. https://mavlink.io/en/services/mission.html class MavLinkMissionRequestInt : public MavLinkMessageBase { public: const static uint8_t kMessageId = 51; MavLinkMissionRequestInt() { msgid = kMessageId; } - // Sequence - uint16_t seq = 0; // System ID uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Sequence + uint16_t seq = 0; + // Mission type. + uint8_t mission_type = 0; + virtual std::string toJSon(); +protected: + virtual int pack(char* buffer) const; + virtual int unpack(const char* buffer); +}; + +// A broadcast message to notify any ground station or SDK if a mission, geofence +// or safe points have changed on the vehicle. +class MavLinkMissionChanged : public MavLinkMessageBase { +public: + const static uint8_t kMessageId = 52; + MavLinkMissionChanged() { msgid = kMessageId; } + // Start index for partial mission change (-1 for all items). + int16_t start_index = 0; + // End index of a partial mission change. -1 is a synonym for the last mission + // item (i.e. selects all items from start_index). Ignore field if start_index=-1. + int16_t end_index = 0; + // System ID of the author of the new mission. + uint8_t origin_sysid = 0; + // Compnent ID of the author of the new mission. + uint8_t origin_compid = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2449,12 +3917,19 @@ class MavLinkMissionRequestInt : public MavLinkMessageBase { }; // Set a safety zone (volume), which is defined by two corners of a cube. This message -// can be used to tell the MAV which setpoints/MISSIONs to accept and which to reject. +// can be used to tell the MAV which setpoints/waypoints to accept and which to reject. // Safety areas are often enforced by national or competition regulations. class MavLinkSafetySetAllowedArea : public MavLinkMessageBase { public: const static uint8_t kMessageId = 54; MavLinkSafetySetAllowedArea() { msgid = kMessageId; } + // System ID + uint8_t target_system = 0; + // Component ID + uint8_t target_component = 0; + // Coordinate frame. Can be either global, GPS, right-handed with Z axis up or + // local, right handed, Z axis down. + uint8_t frame = 0; // x position 1 / Latitude 1 float p1x = 0; // y position 1 / Longitude 1 @@ -2467,13 +3942,6 @@ class MavLinkSafetySetAllowedArea : public MavLinkMessageBase { float p2y = 0; // z position 2 / Altitude 2 float p2z = 0; - // System ID - uint8_t target_system = 0; - // Component ID - uint8_t target_component = 0; - // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either - // global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - uint8_t frame = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2485,6 +3953,9 @@ class MavLinkSafetyAllowedArea : public MavLinkMessageBase { public: const static uint8_t kMessageId = 55; MavLinkSafetyAllowedArea() { msgid = kMessageId; } + // Coordinate frame. Can be either global, GPS, right-handed with Z axis up or + // local, right handed, Z axis down. + uint8_t frame = 0; // x position 1 / Latitude 1 float p1x = 0; // y position 1 / Longitude 1 @@ -2497,9 +3968,6 @@ class MavLinkSafetyAllowedArea : public MavLinkMessageBase { float p2y = 0; // z position 2 / Altitude 2 float p2z = 0; - // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either - // global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - uint8_t frame = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2513,17 +3981,21 @@ class MavLinkAttitudeQuaternionCov : public MavLinkMessageBase { public: const static uint8_t kMessageId = 61; MavLinkAttitudeQuaternionCov() { msgid = kMessageId; } - // Timestamp (microseconds since system boot or since UNIX epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) float q[4] = { 0 }; - // Roll angular speed (rad/s) + // Roll angular speed float rollspeed = 0; - // Pitch angular speed (rad/s) + // Pitch angular speed float pitchspeed = 0; - // Yaw angular speed (rad/s) + // Yaw angular speed float yawspeed = 0; - // Attitude covariance + // Row-major representation of a 3x3 attitude covariance matrix (states: roll, + // pitch, yaw; first three entries are the first ROW, next three entries are the + // second row, etc.). If unknown, assign NaN value to first element in the array. float covariance[9] = { 0 }; virtual std::string toJSon(); protected: @@ -2536,22 +4008,22 @@ class MavLinkNavControllerOutput : public MavLinkMessageBase { public: const static uint8_t kMessageId = 62; MavLinkNavControllerOutput() { msgid = kMessageId; } - // Current desired roll in degrees + // Current desired roll float nav_roll = 0; - // Current desired pitch in degrees + // Current desired pitch float nav_pitch = 0; - // Current altitude error in meters - float alt_error = 0; - // Current airspeed error in meters/second - float aspd_error = 0; - // Current crosstrack error on x-y plane in meters - float xtrack_error = 0; - // Current desired heading in degrees + // Current desired heading int16_t nav_bearing = 0; - // Bearing to current MISSION/target in degrees + // Bearing to current waypoint/target int16_t target_bearing = 0; - // Distance to active MISSION in meters + // Distance to active waypoint uint16_t wp_dist = 0; + // Current altitude error + float alt_error = 0; + // Current airspeed error + float aspd_error = 0; + // Current crosstrack error on x-y plane + float xtrack_error = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2568,27 +4040,31 @@ class MavLinkGlobalPositionIntCov : public MavLinkMessageBase { public: const static uint8_t kMessageId = 63; MavLinkGlobalPositionIntCov() { msgid = kMessageId; } - // Timestamp (microseconds since system boot or since UNIX epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Latitude, expressed as degrees * 1E7 + // Class id of the estimator this estimate originated from. + uint8_t estimator_type = 0; + // Latitude int32_t lat = 0; - // Longitude, expressed as degrees * 1E7 + // Longitude int32_t lon = 0; - // Altitude in meters, expressed as * 1000 (millimeters), above MSL + // Altitude in meters above MSL int32_t alt = 0; - // Altitude above ground in meters, expressed as * 1000 (millimeters) + // Altitude above ground int32_t relative_alt = 0; - // Ground X Speed (Latitude), expressed as m/s + // Ground X Speed (Latitude) float vx = 0; - // Ground Y Speed (Longitude), expressed as m/s + // Ground Y Speed (Longitude) float vy = 0; - // Ground Z Speed (Altitude), expressed as m/s + // Ground Z Speed (Altitude) float vz = 0; - // Covariance matrix (first six entries are the first ROW, next six entries are - // the second row, etc.) + // Row-major representation of a 6x6 position and velocity 6x6 cross-covariance + // matrix (states: lat, lon, alt, vx, vy, vz; first six entries are the first + // ROW, next six entries are the second row, etc.). If unknown, assign NaN value + // to first element in the array. float covariance[36] = { 0 }; - // Class id of the estimator this estimate originated from. - uint8_t estimator_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2601,31 +4077,35 @@ class MavLinkLocalPositionNedCov : public MavLinkMessageBase { public: const static uint8_t kMessageId = 64; MavLinkLocalPositionNedCov() { msgid = kMessageId; } - // Timestamp (microseconds since system boot or since UNIX epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; + // Class id of the estimator this estimate originated from. + uint8_t estimator_type = 0; // X Position float x = 0; // Y Position float y = 0; // Z Position float z = 0; - // X Speed (m/s) + // X Speed float vx = 0; - // Y Speed (m/s) + // Y Speed float vy = 0; - // Z Speed (m/s) + // Z Speed float vz = 0; - // X Acceleration (m/s^2) + // X Acceleration float ax = 0; - // Y Acceleration (m/s^2) + // Y Acceleration float ay = 0; - // Z Acceleration (m/s^2) + // Z Acceleration float az = 0; - // Covariance matrix upper right triangular (first nine entries are the first - // ROW, next eight entries are the second row, etc.) + // Row-major representation of position, velocity and acceleration 9x9 cross-covariance + // matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first + // nine entries are the first ROW, next eight entries are the second row, etc.). + // If unknown, assign NaN value to first element in the array. float covariance[45] = { 0 }; - // Class id of the estimator this estimate originated from. - uint8_t estimator_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2633,73 +4113,56 @@ class MavLinkLocalPositionNedCov : public MavLinkMessageBase { }; // The PPM values of the RC channels received. The standard PPM modulation is as follows: -// 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters -// might violate this specification. +// 1000 microseconds: 0%, 2000 microseconds: 100%. A value of UINT16_MAX implies the +// channel is unused. Individual receivers/transmitters might violate this specification. class MavLinkRcChannels : public MavLinkMessageBase { public: const static uint8_t kMessageId = 65; MavLinkRcChannels() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // Total number of RC channels being received. This can be larger than 18, indicating + // that more channels are available but not given in this message. This value + // should be 0 when no RC channels are available. + uint8_t chancount = 0; + // RC channel 1 value. uint16_t chan1_raw = 0; - // RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 2 value. uint16_t chan2_raw = 0; - // RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 3 value. uint16_t chan3_raw = 0; - // RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 4 value. uint16_t chan4_raw = 0; - // RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 5 value. uint16_t chan5_raw = 0; - // RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 6 value. uint16_t chan6_raw = 0; - // RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 7 value. uint16_t chan7_raw = 0; - // RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 8 value. uint16_t chan8_raw = 0; - // RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 9 value. uint16_t chan9_raw = 0; - // RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 10 value. uint16_t chan10_raw = 0; - // RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 11 value. uint16_t chan11_raw = 0; - // RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 12 value. uint16_t chan12_raw = 0; - // RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 13 value. uint16_t chan13_raw = 0; - // RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 14 value. uint16_t chan14_raw = 0; - // RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 15 value. uint16_t chan15_raw = 0; - // RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 16 value. uint16_t chan16_raw = 0; - // RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 17 value. uint16_t chan17_raw = 0; - // RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel - // is unused. + // RC channel 18 value. uint16_t chan18_raw = 0; - // Total number of RC channels being received. This can be larger than 18, indicating - // that more channels are available but not given in this message. This value - // should be 0 when no RC channels are available. - uint8_t chancount = 0; - // Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + // Receive signal strength indicator in device-dependent units/scale. Values: + // [0-254], 255: invalid/unknown. uint8_t rssi = 0; virtual std::string toJSon(); protected: @@ -2707,19 +4170,19 @@ class MavLinkRcChannels : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// THIS INTERFACE IS DEPRECATED. USE SET_MESSAGE_INTERVAL INSTEAD. +// Request a data stream. class MavLinkRequestDataStream : public MavLinkMessageBase { public: const static uint8_t kMessageId = 66; MavLinkRequestDataStream() { msgid = kMessageId; } - // The requested message rate - uint16_t req_message_rate = 0; // The target requested to send the message stream. uint8_t target_system = 0; // The target requested to send the message stream. uint8_t target_component = 0; // The ID of the requested data stream uint8_t req_stream_id = 0; + // The requested message rate + uint16_t req_message_rate = 0; // 1 to start sending, 0 to stop sending. uint8_t start_stop = 0; virtual std::string toJSon(); @@ -2728,15 +4191,15 @@ class MavLinkRequestDataStream : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// THIS INTERFACE IS DEPRECATED. USE MESSAGE_INTERVAL INSTEAD. +// Data stream status information. class MavLinkDataStream : public MavLinkMessageBase { public: const static uint8_t kMessageId = 67; MavLinkDataStream() { msgid = kMessageId; } - // The message rate - uint16_t message_rate = 0; // The ID of the requested data stream uint8_t stream_id = 0; + // The message rate + uint16_t message_rate = 0; // 1 stream is enabled, 0 stream is stopped. uint8_t on_off = 0; virtual std::string toJSon(); @@ -2752,6 +4215,8 @@ class MavLinkManualControl : public MavLinkMessageBase { public: const static uint8_t kMessageId = 69; MavLinkManualControl() { msgid = kMessageId; } + // The system to be controlled. + uint8_t target = 0; // X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates // that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) // movement on a joystick and the pitch of a vehicle. @@ -2774,8 +4239,6 @@ class MavLinkManualControl : public MavLinkMessageBase { // A bitfield corresponding to the joystick buttons' current state, 1 for pressed, // 0 for released. The lowest bit corresponds to Button 1. uint16_t buttons = 0; - // The system to be controlled. - uint8_t target = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2791,48 +4254,76 @@ class MavLinkRcChannelsOverride : public MavLinkMessageBase { public: const static uint8_t kMessageId = 70; MavLinkRcChannelsOverride() { msgid = kMessageId; } - // RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore - // this field. + // System ID + uint8_t target_system = 0; + // Component ID + uint8_t target_component = 0; + // RC channel 1 value. A value of UINT16_MAX means to ignore this field. uint16_t chan1_raw = 0; - // RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore - // this field. + // RC channel 2 value. A value of UINT16_MAX means to ignore this field. uint16_t chan2_raw = 0; - // RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore - // this field. + // RC channel 3 value. A value of UINT16_MAX means to ignore this field. uint16_t chan3_raw = 0; - // RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore - // this field. + // RC channel 4 value. A value of UINT16_MAX means to ignore this field. uint16_t chan4_raw = 0; - // RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore - // this field. + // RC channel 5 value. A value of UINT16_MAX means to ignore this field. uint16_t chan5_raw = 0; - // RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore - // this field. + // RC channel 6 value. A value of UINT16_MAX means to ignore this field. uint16_t chan6_raw = 0; - // RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore - // this field. + // RC channel 7 value. A value of UINT16_MAX means to ignore this field. uint16_t chan7_raw = 0; - // RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore - // this field. + // RC channel 8 value. A value of UINT16_MAX means to ignore this field. uint16_t chan8_raw = 0; - // System ID - uint8_t target_system = 0; - // Component ID - uint8_t target_component = 0; - virtual std::string toJSon(); -protected: - virtual int pack(char* buffer) const; + // RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan9_raw = 0; + // RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan10_raw = 0; + // RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan11_raw = 0; + // RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan12_raw = 0; + // RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan13_raw = 0; + // RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan14_raw = 0; + // RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan15_raw = 0; + // RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan16_raw = 0; + // RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan17_raw = 0; + // RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. + uint16_t chan18_raw = 0; + virtual std::string toJSon(); +protected: + virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; // Message encoding a mission item. This message is emitted to announce the presence // of a mission item and to set a mission item on the system. The mission item can // be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame -// is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). See alsohttp://qgroundcontrol.org/mavlink/waypoint_protocol. +// is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). See also +// https://mavlink.io/en/services/mission.html. class MavLinkMissionItemInt : public MavLinkMessageBase { public: const static uint8_t kMessageId = 73; MavLinkMissionItemInt() { msgid = kMessageId; } + // System ID + uint8_t target_system = 0; + // Component ID + uint8_t target_component = 0; + // Waypoint ID (sequence number). Starts at zero. Increases monotonically for + // each waypoint, no gaps in the sequence (0,1,2,3,4). + uint16_t seq = 0; + // The coordinate system of the waypoint. + uint8_t frame = 0; + // The scheduled action for the waypoint. + uint16_t command = 0; + // false:0, true:1 + uint8_t current = 0; + // Autocontinue to next waypoint + uint8_t autocontinue = 0; // PARAM1, see MAV_CMD enum float param1 = 0; // PARAM2, see MAV_CMD enum @@ -2849,44 +4340,31 @@ class MavLinkMissionItemInt : public MavLinkMessageBase { // PARAM7 / z position: global: altitude in meters (relative or absolute, depending // on frame. float z = 0; - // Waypoint ID (sequence number). Starts at zero. Increases monotonically for - // each waypoint, no gaps in the sequence (0,1,2,3,4). - uint16_t seq = 0; - // The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs - uint16_t command = 0; - // System ID - uint8_t target_system = 0; - // Component ID - uint8_t target_component = 0; - // The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h - uint8_t frame = 0; - // false:0, true:1 - uint8_t current = 0; - // autocontinue to next wp - uint8_t autocontinue = 0; + // Mission type. + uint8_t mission_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Metrics typically displayed on a HUD for fixed wing aircraft +// Metrics typically displayed on a HUD for fixed wing aircraft. class MavLinkVfrHud : public MavLinkMessageBase { public: const static uint8_t kMessageId = 74; MavLinkVfrHud() { msgid = kMessageId; } - // Current airspeed in m/s + // Current indicated airspeed (IAS). float airspeed = 0; - // Current ground speed in m/s + // Current ground speed. float groundspeed = 0; - // Current altitude (MSL), in meters - float alt = 0; - // Current climb rate in meters/second - float climb = 0; - // Current heading in degrees, in compass units (0..360, 0=north) + // Current heading in compass units (0-360, 0=north). int16_t heading = 0; - // Current throttle setting in integer percent, 0 to 100 + // Current throttle setting (0 to 100). uint16_t throttle = 0; + // Current altitude (MSL). + float alt = 0; + // Current climb rate. + float climb = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2894,11 +4372,23 @@ class MavLinkVfrHud : public MavLinkMessageBase { }; // Message encoding a command with parameters as scaled integers. Scaling depends -// on the actual command value. +// on the actual command value. The command microservice is documented at https://mavlink.io/en/services/command.html class MavLinkCommandInt : public MavLinkMessageBase { public: const static uint8_t kMessageId = 75; MavLinkCommandInt() { msgid = kMessageId; } + // System ID + uint8_t target_system = 0; + // Component ID + uint8_t target_component = 0; + // The coordinate system of the COMMAND. + uint8_t frame = 0; + // The scheduled action for the mission item. + uint16_t command = 0; + // false:0, true:1 + uint8_t current = 0; + // autocontinue to next wp + uint8_t autocontinue = 0; // PARAM1, see MAV_CMD enum float param1 = 0; // PARAM2, see MAV_CMD enum @@ -2913,70 +4403,70 @@ class MavLinkCommandInt : public MavLinkMessageBase { // 10^7 int32_t y = 0; // PARAM7 / z position: global: altitude in meters (relative or absolute, depending - // on frame. + // on frame). float z = 0; - // The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink - // specs - uint16_t command = 0; - // System ID - uint8_t target_system = 0; - // Component ID - uint8_t target_component = 0; - // The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h - uint8_t frame = 0; - // false:0, true:1 - uint8_t current = 0; - // autocontinue to next wp - uint8_t autocontinue = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Send a command with up to seven parameters to the MAV +// Send a command with up to seven parameters to the MAV. The command microservice +// is documented at https://mavlink.io/en/services/command.html class MavLinkCommandLong : public MavLinkMessageBase { public: const static uint8_t kMessageId = 76; MavLinkCommandLong() { msgid = kMessageId; } - // Parameter 1, as defined by MAV_CMD enum. - float param1 = 0; - // Parameter 2, as defined by MAV_CMD enum. - float param2 = 0; - // Parameter 3, as defined by MAV_CMD enum. - float param3 = 0; - // Parameter 4, as defined by MAV_CMD enum. - float param4 = 0; - // Parameter 5, as defined by MAV_CMD enum. - float param5 = 0; - // Parameter 6, as defined by MAV_CMD enum. - float param6 = 0; - // Parameter 7, as defined by MAV_CMD enum. - float param7 = 0; - // Command ID, as defined by MAV_CMD enum. - uint16_t command = 0; // System which should execute the command uint8_t target_system = 0; // Component which should execute the command, 0 for all components uint8_t target_component = 0; + // Command ID (of command to send). + uint16_t command = 0; // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. // for kill command) uint8_t confirmation = 0; + // Parameter 1 (for the specific command). + float param1 = 0; + // Parameter 2 (for the specific command). + float param2 = 0; + // Parameter 3 (for the specific command). + float param3 = 0; + // Parameter 4 (for the specific command). + float param4 = 0; + // Parameter 5 (for the specific command). + float param5 = 0; + // Parameter 6 (for the specific command). + float param6 = 0; + // Parameter 7 (for the specific command). + float param7 = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Report status of a command. Includes feedback wether the command was executed. +// Report status of a command. Includes feedback whether the command was executed. +// The command microservice is documented at https://mavlink.io/en/services/command.html class MavLinkCommandAck : public MavLinkMessageBase { public: const static uint8_t kMessageId = 77; MavLinkCommandAck() { msgid = kMessageId; } - // Command ID, as defined by MAV_CMD enum. + // Command ID (of acknowledged command). uint16_t command = 0; - // See MAV_RESULT enum + // Result of command. uint8_t result = 0; + // WIP: Also used as result_param1, it can be set with a enum containing the errors + // reasons of why the command was denied or the progress percentage or 255 if + // unknown the progress when result is MAV_RESULT_IN_PROGRESS. + uint8_t progress = 0; + // WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT + // caused it to be denied. + int32_t result_param2 = 0; + // WIP: System which requested the command to be executed + uint8_t target_system = 0; + // WIP: Component which requested the command to be executed + uint8_t target_component = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -2988,13 +4478,13 @@ class MavLinkManualSetpoint : public MavLinkMessageBase { public: const static uint8_t kMessageId = 81; MavLinkManualSetpoint() { msgid = kMessageId; } - // Timestamp in milliseconds since system boot + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Desired roll rate in radians per second + // Desired roll rate float roll = 0; - // Desired pitch rate in radians per second + // Desired pitch rate float pitch = 0; - // Desired yaw rate in radians per second + // Desired yaw rate float yaw = 0; // Collective thrust, normalized to 0 .. 1 float thrust = 0; @@ -3014,19 +4504,8 @@ class MavLinkSetAttitudeTarget : public MavLinkMessageBase { public: const static uint8_t kMessageId = 82; MavLinkSetAttitudeTarget() { msgid = kMessageId; } - // Timestamp in milliseconds since system boot + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) - float q[4] = { 0 }; - // Body roll rate in radians per second - float body_roll_rate = 0; - // Body roll rate in radians per second - float body_pitch_rate = 0; - // Body roll rate in radians per second - float body_yaw_rate = 0; - // Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse - // trust) - float thrust = 0; // System ID uint8_t target_system = 0; // Component ID @@ -3035,6 +4514,17 @@ class MavLinkSetAttitudeTarget : public MavLinkMessageBase { // bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit // 6: reserved, bit 7: throttle, bit 8: attitude uint8_t type_mask = 0; + // Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + float q[4] = { 0 }; + // Body roll rate + float body_roll_rate = 0; + // Body pitch rate + float body_pitch_rate = 0; + // Body yaw rate + float body_yaw_rate = 0; + // Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse + // trust) + float thrust = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3048,23 +4538,23 @@ class MavLinkAttitudeTarget : public MavLinkMessageBase { public: const static uint8_t kMessageId = 83; MavLinkAttitudeTarget() { msgid = kMessageId; } - // Timestamp in milliseconds since system boot + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; + // Mappings: If any of these bits are set, the corresponding input should be ignored: + // bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit + // 7: reserved, bit 8: attitude + uint8_t type_mask = 0; // Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) float q[4] = { 0 }; - // Body roll rate in radians per second + // Body roll rate float body_roll_rate = 0; - // Body roll rate in radians per second + // Body pitch rate float body_pitch_rate = 0; - // Body roll rate in radians per second + // Body yaw rate float body_yaw_rate = 0; // Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse // trust) float thrust = 0; - // Mappings: If any of these bits are set, the corresponding input should be ignored: - // bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit - // 7: reserved, bit 8: attitude - uint8_t type_mask = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3077,19 +4567,28 @@ class MavLinkSetPositionTargetLocalNed : public MavLinkMessageBase { public: const static uint8_t kMessageId = 84; MavLinkSetPositionTargetLocalNed() { msgid = kMessageId; } - // Timestamp in milliseconds since system boot + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // X Position in NED frame in meters + // System ID + uint8_t target_system = 0; + // Component ID + uint8_t target_component = 0; + // Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, + // MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + uint8_t coordinate_frame = 0; + // Bitmap to indicate which dimensions should be ignored by the vehicle. + uint16_t type_mask = 0; + // X Position in NED frame float x = 0; - // Y Position in NED frame in meters + // Y Position in NED frame float y = 0; - // Z Position in NED frame in meters (note, altitude is negative in NED) + // Z Position in NED frame (note, altitude is negative in NED) float z = 0; - // X velocity in NED frame in meter / s + // X velocity in NED frame float vx = 0; - // Y velocity in NED frame in meter / s + // Y velocity in NED frame float vy = 0; - // Z velocity in NED frame in meter / s + // Z velocity in NED frame float vz = 0; // X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter // / s^2 or N @@ -3100,24 +4599,10 @@ class MavLinkSetPositionTargetLocalNed : public MavLinkMessageBase { // Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter // / s^2 or N float afz = 0; - // yaw setpoint in rad + // yaw setpoint float yaw = 0; - // yaw rate setpoint in rad/s + // yaw rate setpoint float yaw_rate = 0; - // Bitmask to indicate which dimensions should be ignored by the vehicle: a value - // of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint - // dimensions should be ignored. If bit 10 is set the floats afx afy afz should - // be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: - // y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: - // az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate - uint16_t type_mask = 0; - // System ID - uint8_t target_system = 0; - // Component ID - uint8_t target_component = 0; - // Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, - // MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 - uint8_t coordinate_frame = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3131,19 +4616,24 @@ class MavLinkPositionTargetLocalNed : public MavLinkMessageBase { public: const static uint8_t kMessageId = 85; MavLinkPositionTargetLocalNed() { msgid = kMessageId; } - // Timestamp in milliseconds since system boot + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // X Position in NED frame in meters + // Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, + // MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + uint8_t coordinate_frame = 0; + // Bitmap to indicate which dimensions should be ignored by the vehicle. + uint16_t type_mask = 0; + // X Position in NED frame float x = 0; - // Y Position in NED frame in meters + // Y Position in NED frame float y = 0; - // Z Position in NED frame in meters (note, altitude is negative in NED) + // Z Position in NED frame (note, altitude is negative in NED) float z = 0; - // X velocity in NED frame in meter / s + // X velocity in NED frame float vx = 0; - // Y velocity in NED frame in meter / s + // Y velocity in NED frame float vy = 0; - // Z velocity in NED frame in meter / s + // Z velocity in NED frame float vz = 0; // X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter // / s^2 or N @@ -3154,20 +4644,10 @@ class MavLinkPositionTargetLocalNed : public MavLinkMessageBase { // Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter // / s^2 or N float afz = 0; - // yaw setpoint in rad + // yaw setpoint float yaw = 0; - // yaw rate setpoint in rad/s + // yaw rate setpoint float yaw_rate = 0; - // Bitmask to indicate which dimensions should be ignored by the vehicle: a value - // of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint - // dimensions should be ignored. If bit 10 is set the floats afx afy afz should - // be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: - // y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: - // az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate - uint16_t type_mask = 0; - // Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, - // MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 - uint8_t coordinate_frame = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3181,22 +4661,30 @@ class MavLinkSetPositionTargetGlobalInt : public MavLinkMessageBase { public: const static uint8_t kMessageId = 86; MavLinkSetPositionTargetGlobalInt() { msgid = kMessageId; } - // Timestamp in milliseconds since system boot. The rationale for the timestamp - // in the setpoint is to allow the system to compensate for the transport delay - // of the setpoint. This allows the system to compensate processing latency. + // Timestamp (time since system boot). The rationale for the timestamp in the + // setpoint is to allow the system to compensate for the transport delay of the + // setpoint. This allows the system to compensate processing latency. uint32_t time_boot_ms = 0; - // X Position in WGS84 frame in 1e7 * meters + // System ID + uint8_t target_system = 0; + // Component ID + uint8_t target_component = 0; + // Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT + // = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + uint8_t coordinate_frame = 0; + // Bitmap to indicate which dimensions should be ignored by the vehicle. + uint16_t type_mask = 0; + // X Position in WGS84 frame int32_t lat_int = 0; - // Y Position in WGS84 frame in 1e7 * meters + // Y Position in WGS84 frame int32_t lon_int = 0; - // Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above - // terrain if GLOBAL_TERRAIN_ALT_INT + // Altitude (MSL, Relative to home, or AGL - depending on frame) float alt = 0; - // X velocity in NED frame in meter / s + // X velocity in NED frame float vx = 0; - // Y velocity in NED frame in meter / s + // Y velocity in NED frame float vy = 0; - // Z velocity in NED frame in meter / s + // Z velocity in NED frame float vz = 0; // X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter // / s^2 or N @@ -3207,24 +4695,10 @@ class MavLinkSetPositionTargetGlobalInt : public MavLinkMessageBase { // Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter // / s^2 or N float afz = 0; - // yaw setpoint in rad + // yaw setpoint float yaw = 0; - // yaw rate setpoint in rad/s + // yaw rate setpoint float yaw_rate = 0; - // Bitmask to indicate which dimensions should be ignored by the vehicle: a value - // of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint - // dimensions should be ignored. If bit 10 is set the floats afx afy afz should - // be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: - // y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: - // az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate - uint16_t type_mask = 0; - // System ID - uint8_t target_system = 0; - // Component ID - uint8_t target_component = 0; - // Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT - // = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 - uint8_t coordinate_frame = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3238,22 +4712,26 @@ class MavLinkPositionTargetGlobalInt : public MavLinkMessageBase { public: const static uint8_t kMessageId = 87; MavLinkPositionTargetGlobalInt() { msgid = kMessageId; } - // Timestamp in milliseconds since system boot. The rationale for the timestamp - // in the setpoint is to allow the system to compensate for the transport delay - // of the setpoint. This allows the system to compensate processing latency. + // Timestamp (time since system boot). The rationale for the timestamp in the + // setpoint is to allow the system to compensate for the transport delay of the + // setpoint. This allows the system to compensate processing latency. uint32_t time_boot_ms = 0; - // X Position in WGS84 frame in 1e7 * meters + // Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT + // = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + uint8_t coordinate_frame = 0; + // Bitmap to indicate which dimensions should be ignored by the vehicle. + uint16_t type_mask = 0; + // X Position in WGS84 frame int32_t lat_int = 0; - // Y Position in WGS84 frame in 1e7 * meters + // Y Position in WGS84 frame int32_t lon_int = 0; - // Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above - // terrain if GLOBAL_TERRAIN_ALT_INT + // Altitude (MSL, AGL or relative to home altitude, depending on frame) float alt = 0; - // X velocity in NED frame in meter / s + // X velocity in NED frame float vx = 0; - // Y velocity in NED frame in meter / s + // Y velocity in NED frame float vy = 0; - // Z velocity in NED frame in meter / s + // Z velocity in NED frame float vz = 0; // X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter // / s^2 or N @@ -3264,20 +4742,10 @@ class MavLinkPositionTargetGlobalInt : public MavLinkMessageBase { // Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter // / s^2 or N float afz = 0; - // yaw setpoint in rad + // yaw setpoint float yaw = 0; - // yaw rate setpoint in rad/s + // yaw rate setpoint float yaw_rate = 0; - // Bitmask to indicate which dimensions should be ignored by the vehicle: a value - // of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint - // dimensions should be ignored. If bit 10 is set the floats afx afy afz should - // be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: - // y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: - // az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate - uint16_t type_mask = 0; - // Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT - // = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 - uint8_t coordinate_frame = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3291,7 +4759,7 @@ class MavLinkLocalPositionNedSystemGlobalOffset : public MavLinkMessageBase { public: const static uint8_t kMessageId = 89; MavLinkLocalPositionNedSystemGlobalOffset() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; // X Position float x = 0; @@ -3311,45 +4779,45 @@ class MavLinkLocalPositionNedSystemGlobalOffset : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// DEPRECATED PACKET! Suffers from missing airspeed fields and singularities due to -// Euler angles. Please use HIL_STATE_QUATERNION instead. Sent from simulation to -// autopilot. This packet is useful for high throughput applications such as hardware -// in the loop simulations. +// Sent from simulation to autopilot. This packet is useful for high throughput applications +// such as hardware in the loop simulations. class MavLinkHilState : public MavLinkMessageBase { public: const static uint8_t kMessageId = 90; MavLinkHilState() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Roll angle (rad) + // Roll angle float roll = 0; - // Pitch angle (rad) + // Pitch angle float pitch = 0; - // Yaw angle (rad) + // Yaw angle float yaw = 0; - // Body frame roll / phi angular speed (rad/s) + // Body frame roll / phi angular speed float rollspeed = 0; - // Body frame pitch / theta angular speed (rad/s) + // Body frame pitch / theta angular speed float pitchspeed = 0; - // Body frame yaw / psi angular speed (rad/s) + // Body frame yaw / psi angular speed float yawspeed = 0; - // Latitude, expressed as * 1E7 + // Latitude int32_t lat = 0; - // Longitude, expressed as * 1E7 + // Longitude int32_t lon = 0; - // Altitude in meters, expressed as * 1000 (millimeters) + // Altitude int32_t alt = 0; - // Ground X Speed (Latitude), expressed as m/s * 100 + // Ground X Speed (Latitude) int16_t vx = 0; - // Ground Y Speed (Longitude), expressed as m/s * 100 + // Ground Y Speed (Longitude) int16_t vy = 0; - // Ground Z Speed (Altitude), expressed as m/s * 100 + // Ground Z Speed (Altitude) int16_t vz = 0; - // X acceleration (mg) + // X acceleration int16_t xacc = 0; - // Y acceleration (mg) + // Y acceleration int16_t yacc = 0; - // Z acceleration (mg) + // Z acceleration int16_t zacc = 0; virtual std::string toJSon(); protected: @@ -3362,7 +4830,9 @@ class MavLinkHilControls : public MavLinkMessageBase { public: const static uint8_t kMessageId = 91; MavLinkHilControls() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // Control output -1 .. 1 float roll_ailerons = 0; @@ -3380,7 +4850,7 @@ class MavLinkHilControls : public MavLinkMessageBase { float aux3 = 0; // Aux 4, -1 .. 1 float aux4 = 0; - // System mode (MAV_MODE) + // System mode. uint8_t mode = 0; // Navigation mode (MAV_NAV_MODE) uint8_t nav_mode = 0; @@ -3397,33 +4867,36 @@ class MavLinkHilRcInputsRaw : public MavLinkMessageBase { public: const static uint8_t kMessageId = 92; MavLinkHilRcInputsRaw() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // RC channel 1 value, in microseconds + // RC channel 1 value uint16_t chan1_raw = 0; - // RC channel 2 value, in microseconds + // RC channel 2 value uint16_t chan2_raw = 0; - // RC channel 3 value, in microseconds + // RC channel 3 value uint16_t chan3_raw = 0; - // RC channel 4 value, in microseconds + // RC channel 4 value uint16_t chan4_raw = 0; - // RC channel 5 value, in microseconds + // RC channel 5 value uint16_t chan5_raw = 0; - // RC channel 6 value, in microseconds + // RC channel 6 value uint16_t chan6_raw = 0; - // RC channel 7 value, in microseconds + // RC channel 7 value uint16_t chan7_raw = 0; - // RC channel 8 value, in microseconds + // RC channel 8 value uint16_t chan8_raw = 0; - // RC channel 9 value, in microseconds + // RC channel 9 value uint16_t chan9_raw = 0; - // RC channel 10 value, in microseconds + // RC channel 10 value uint16_t chan10_raw = 0; - // RC channel 11 value, in microseconds + // RC channel 11 value uint16_t chan11_raw = 0; - // RC channel 12 value, in microseconds + // RC channel 12 value uint16_t chan12_raw = 0; - // Receive signal strength indicator, 0: 0%, 255: 100% + // Receive signal strength indicator in device-dependent units/scale. Values: + // [0-254], 255: invalid/unknown. uint8_t rssi = 0; virtual std::string toJSon(); protected: @@ -3437,14 +4910,16 @@ class MavLinkHilActuatorControls : public MavLinkMessageBase { public: const static uint8_t kMessageId = 93; MavLinkHilActuatorControls() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Flags as bitfield, reserved for future use. - uint64_t flags = 0; // Control outputs -1 .. 1. Channel assignment depends on the simulated hardware. float controls[16] = { 0 }; - // System mode (MAV_MODE), includes arming state. + // System mode. Includes arming state. uint8_t mode = 0; + // Flags as bitfield, reserved for future use. + uint64_t flags = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3456,34 +4931,40 @@ class MavLinkOpticalFlow : public MavLinkMessageBase { public: const static uint8_t kMessageId = 100; MavLinkOpticalFlow() { msgid = kMessageId; } - // Timestamp (UNIX) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Flow in meters in x-sensor direction, angular-speed compensated - float flow_comp_m_x = 0; - // Flow in meters in y-sensor direction, angular-speed compensated - float flow_comp_m_y = 0; - // Ground distance in meters. Positive value: distance known. Negative value: - // Unknown distance - float ground_distance = 0; - // Flow in pixels * 10 in x-sensor direction (dezi-pixels) - int16_t flow_x = 0; - // Flow in pixels * 10 in y-sensor direction (dezi-pixels) - int16_t flow_y = 0; // Sensor ID uint8_t sensor_id = 0; + // Flow in x-sensor direction + int16_t flow_x = 0; + // Flow in y-sensor direction + int16_t flow_y = 0; + // Flow in x-sensor direction, angular-speed compensated + float flow_comp_m_x = 0; + // Flow in y-sensor direction, angular-speed compensated + float flow_comp_m_y = 0; // Optical flow quality / confidence. 0: bad, 255: maximum quality uint8_t quality = 0; + // Ground distance. Positive value: distance known. Negative value: Unknown distance + float ground_distance = 0; + // Flow rate about X axis + float flow_rate_x = 0; + // Flow rate about Y axis + float flow_rate_y = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; +// Global position/attitude estimate from a vision source. class MavLinkGlobalVisionPositionEstimate : public MavLinkMessageBase { public: const static uint8_t kMessageId = 101; MavLinkGlobalVisionPositionEstimate() { msgid = kMessageId; } - // Timestamp (microseconds, synced to UNIX time or since system boot) + // Timestamp (UNIX time or since system boot) uint64_t usec = 0; // Global X position float x = 0; @@ -3491,47 +4972,69 @@ class MavLinkGlobalVisionPositionEstimate : public MavLinkMessageBase { float y = 0; // Global Z position float z = 0; - // Roll angle in rad + // Roll angle float roll = 0; - // Pitch angle in rad + // Pitch angle float pitch = 0; - // Yaw angle in rad + // Yaw angle float yaw = 0; + // Row-major representation of pose 6x6 cross-covariance matrix upper right triangle + // (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries + // are the first ROW, next five entries are the second ROW, etc.). If unknown, + // assign NaN value to first element in the array. + float covariance[21] = { 0 }; + // Estimate reset counter. This should be incremented when the estimate resets + // in any of the dimensions (position, velocity, attitude, angular speed). This + // is designed to be used when e.g an external SLAM system detects a loop-closure + // and the estimate jumps. + uint8_t reset_counter = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; +// Local position/attitude estimate from a vision source. class MavLinkVisionPositionEstimate : public MavLinkMessageBase { public: const static uint8_t kMessageId = 102; MavLinkVisionPositionEstimate() { msgid = kMessageId; } - // Timestamp (microseconds, synced to UNIX time or since system boot) + // Timestamp (UNIX time or time since system boot) uint64_t usec = 0; - // Global X position + // Local X position float x = 0; - // Global Y position + // Local Y position float y = 0; - // Global Z position + // Local Z position float z = 0; - // Roll angle in rad + // Roll angle float roll = 0; - // Pitch angle in rad + // Pitch angle float pitch = 0; - // Yaw angle in rad + // Yaw angle float yaw = 0; + // Row-major representation of pose 6x6 cross-covariance matrix upper right triangle + // (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next + // five entries are the second ROW, etc.). If unknown, assign NaN value to first + // element in the array. + float covariance[21] = { 0 }; + // Estimate reset counter. This should be incremented when the estimate resets + // in any of the dimensions (position, velocity, attitude, angular speed). This + // is designed to be used when e.g an external SLAM system detects a loop-closure + // and the estimate jumps. + uint8_t reset_counter = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; +// Speed estimate from a vision source. class MavLinkVisionSpeedEstimate : public MavLinkMessageBase { public: const static uint8_t kMessageId = 103; MavLinkVisionSpeedEstimate() { msgid = kMessageId; } - // Timestamp (microseconds, synced to UNIX time or since system boot) + // Timestamp (UNIX time or time since system boot) uint64_t usec = 0; // Global X speed float x = 0; @@ -3539,17 +5042,27 @@ class MavLinkVisionSpeedEstimate : public MavLinkMessageBase { float y = 0; // Global Z speed float z = 0; + // Row-major representation of 3x3 linear velocity covariance matrix (states: + // vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value + // to first element in the array. + float covariance[9] = { 0 }; + // Estimate reset counter. This should be incremented when the estimate resets + // in any of the dimensions (position, velocity, attitude, angular speed). This + // is designed to be used when e.g an external SLAM system detects a loop-closure + // and the estimate jumps. + uint8_t reset_counter = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; +// Global position estimate from a Vicon motion system source. class MavLinkViconPositionEstimate : public MavLinkMessageBase { public: const static uint8_t kMessageId = 104; MavLinkViconPositionEstimate() { msgid = kMessageId; } - // Timestamp (microseconds, synced to UNIX time or since system boot) + // Timestamp (UNIX time or time since system boot) uint64_t usec = 0; // Global X position float x = 0; @@ -3557,12 +5070,17 @@ class MavLinkViconPositionEstimate : public MavLinkMessageBase { float y = 0; // Global Z position float z = 0; - // Roll angle in rad + // Roll angle float roll = 0; - // Pitch angle in rad + // Pitch angle float pitch = 0; - // Yaw angle in rad + // Yaw angle float yaw = 0; + // Row-major representation of 6x6 pose cross-covariance matrix upper right triangle + // (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next + // five entries are the second ROW, etc.). If unknown, assign NaN value to first + // element in the array. + float covariance[21] = { 0 }; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3574,37 +5092,42 @@ class MavLinkHighresImu : public MavLinkMessageBase { public: const static uint8_t kMessageId = 105; MavLinkHighresImu() { msgid = kMessageId; } - // Timestamp (microseconds, synced to UNIX time or since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // X acceleration (m/s^2) + // X acceleration float xacc = 0; - // Y acceleration (m/s^2) + // Y acceleration float yacc = 0; - // Z acceleration (m/s^2) + // Z acceleration float zacc = 0; - // Angular speed around X axis (rad / sec) + // Angular speed around X axis float xgyro = 0; - // Angular speed around Y axis (rad / sec) + // Angular speed around Y axis float ygyro = 0; - // Angular speed around Z axis (rad / sec) + // Angular speed around Z axis float zgyro = 0; - // X Magnetic field (Gauss) + // X Magnetic field float xmag = 0; - // Y Magnetic field (Gauss) + // Y Magnetic field float ymag = 0; - // Z Magnetic field (Gauss) + // Z Magnetic field float zmag = 0; - // Absolute pressure in millibar + // Absolute pressure float abs_pressure = 0; - // Differential pressure in millibar + // Differential pressure float diff_pressure = 0; // Altitude calculated from pressure float pressure_alt = 0; - // Temperature in degrees celsius + // Temperature float temperature = 0; - // Bitmask for fields that have updated since last message, bit 0 = xacc, bit - // 12: temperature + // Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: + // temperature uint16_t fields_updated = 0; + // Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will + // have a message with id=0) + uint8_t id = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3616,37 +5139,36 @@ class MavLinkOpticalFlowRad : public MavLinkMessageBase { public: const static uint8_t kMessageId = 106; MavLinkOpticalFlowRad() { msgid = kMessageId; } - // Timestamp (microseconds, synced to UNIX time or since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Integration time in microseconds. Divide integrated_x and integrated_y by the - // integration time to obtain average flow. The integration time also indicates - // the. + // Sensor ID + uint8_t sensor_id = 0; + // Integration time. Divide integrated_x and integrated_y by the integration time + // to obtain average flow. The integration time also indicates the. uint32_t integration_time_us = 0; - // Flow in radians around X axis (Sensor RH rotation about the X axis induces - // a positive flow. Sensor linear motion along the positive Y axis induces a negative - // flow.) + // Flow around X axis (Sensor RH rotation about the X axis induces a positive + // flow. Sensor linear motion along the positive Y axis induces a negative flow.) float integrated_x = 0; - // Flow in radians around Y axis (Sensor RH rotation about the Y axis induces - // a positive flow. Sensor linear motion along the positive X axis induces a positive - // flow.) + // Flow around Y axis (Sensor RH rotation about the Y axis induces a positive + // flow. Sensor linear motion along the positive X axis induces a positive flow.) float integrated_y = 0; - // RH rotation around X axis (rad) + // RH rotation around X axis float integrated_xgyro = 0; - // RH rotation around Y axis (rad) + // RH rotation around Y axis float integrated_ygyro = 0; - // RH rotation around Z axis (rad) + // RH rotation around Z axis float integrated_zgyro = 0; - // Time in microseconds since the distance was sampled. - uint32_t time_delta_distance_us = 0; - // Distance to the center of the flow field in meters. Positive value (including - // zero): distance known. Negative value: Unknown distance. - float distance = 0; - // Temperature * 100 in centi-degrees Celsius + // Temperature int16_t temperature = 0; - // Sensor ID - uint8_t sensor_id = 0; // Optical flow quality / confidence. 0: no valid flow, 255: maximum quality uint8_t quality = 0; + // Time since the distance was sampled. + uint32_t time_delta_distance_us = 0; + // Distance to the center of the flow field. Positive value (including zero): + // distance known. Negative value: Unknown distance. + float distance = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3658,37 +5180,39 @@ class MavLinkHilSensor : public MavLinkMessageBase { public: const static uint8_t kMessageId = 107; MavLinkHilSensor() { msgid = kMessageId; } - // Timestamp (microseconds, synced to UNIX time or since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // X acceleration (m/s^2) + // X acceleration float xacc = 0; - // Y acceleration (m/s^2) + // Y acceleration float yacc = 0; - // Z acceleration (m/s^2) + // Z acceleration float zacc = 0; - // Angular speed around X axis in body frame (rad / sec) + // Angular speed around X axis in body frame float xgyro = 0; - // Angular speed around Y axis in body frame (rad / sec) + // Angular speed around Y axis in body frame float ygyro = 0; - // Angular speed around Z axis in body frame (rad / sec) + // Angular speed around Z axis in body frame float zgyro = 0; - // X Magnetic field (Gauss) + // X Magnetic field float xmag = 0; - // Y Magnetic field (Gauss) + // Y Magnetic field float ymag = 0; - // Z Magnetic field (Gauss) + // Z Magnetic field float zmag = 0; - // Absolute pressure in millibar + // Absolute pressure float abs_pressure = 0; - // Differential pressure (airspeed) in millibar + // Differential pressure (airspeed) float diff_pressure = 0; // Altitude calculated from pressure float pressure_alt = 0; - // Temperature in degrees celsius + // Temperature float temperature = 0; - // Bitmask for fields that have updated since last message, bit 0 = xacc, bit - // 12: temperature, bit 31: full reset of attitude/position/velocities/etc was - // performed in sim. + // Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: + // temperature, bit 31: full reset of attitude/position/velocities/etc was performed + // in sim. uint32_t fields_updated = 0; virtual std::string toJSon(); protected: @@ -3718,33 +5242,33 @@ class MavLinkSimState : public MavLinkMessageBase { // Attitude yaw expressed as Euler angles, not recommended except for human-readable // outputs float yaw = 0; - // X acceleration m/s/s + // X acceleration float xacc = 0; - // Y acceleration m/s/s + // Y acceleration float yacc = 0; - // Z acceleration m/s/s + // Z acceleration float zacc = 0; - // Angular speed around X axis rad/s + // Angular speed around X axis float xgyro = 0; - // Angular speed around Y axis rad/s + // Angular speed around Y axis float ygyro = 0; - // Angular speed around Z axis rad/s + // Angular speed around Z axis float zgyro = 0; - // Latitude in degrees + // Latitude float lat = 0; - // Longitude in degrees + // Longitude float lon = 0; - // Altitude in meters + // Altitude float alt = 0; // Horizontal position standard deviation float std_dev_horz = 0; // Vertical position standard deviation float std_dev_vert = 0; - // True velocity in m/s in NORTH direction in earth-fixed NED frame + // True velocity in north direction in earth-fixed NED frame float vn = 0; - // True velocity in m/s in EAST direction in earth-fixed NED frame + // True velocity in east direction in earth-fixed NED frame float ve = 0; - // True velocity in m/s in DOWN direction in earth-fixed NED frame + // True velocity in down direction in earth-fixed NED frame float vd = 0; virtual std::string toJSon(); protected: @@ -3757,20 +5281,24 @@ class MavLinkRadioStatus : public MavLinkMessageBase { public: const static uint8_t kMessageId = 109; MavLinkRadioStatus() { msgid = kMessageId; } - // Receive errors - uint16_t rxerrors = 0; - // Count of error corrected packets - uint16_t fixed = 0; - // Local signal strength + // Local (message sender) recieved signal strength indication in device-dependent + // units/scale. Values: [0-254], 255: invalid/unknown. uint8_t rssi = 0; - // Remote signal strength + // Remote (message receiver) signal strength indication in device-dependent units/scale. + // Values: [0-254], 255: invalid/unknown. uint8_t remrssi = 0; - // Remaining free buffer space in percent. + // Remaining free transmitter buffer space. uint8_t txbuf = 0; - // Background noise level + // Local background noise level. These are device dependent RSSI values (scale + // as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. uint8_t noise = 0; - // Remote background noise level + // Remote background noise level. These are device dependent RSSI values (scale + // as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. uint8_t remnoise = 0; + // Count of radio packet receive errors (since boot). + uint16_t rxerrors = 0; + // Count of error corrected radio packets (since boot). + uint16_t fixed = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3820,7 +5348,9 @@ class MavLinkCameraTrigger : public MavLinkMessageBase { public: const static uint8_t kMessageId = 112; MavLinkCameraTrigger() { msgid = kMessageId; } - // Timestamp for the image frame in microseconds + // Timestamp for image frame (UNIX Epoch time or time since system boot). The + // receiving end can infer timestamp format (since 1.1.1970 or since system boot) + // by checking for the magnitude the number. uint64_t time_usec = 0; // Image frame sequence uint32_t seq = 0; @@ -3832,39 +5362,39 @@ class MavLinkCameraTrigger : public MavLinkMessageBase { // The global position, as returned by the Global Positioning System (GPS). This is // NOT the global position estimate of the sytem, but rather a RAW sensor value. See -// message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, -// Z-axis up (GPS frame). +// message GLOBAL_POSITION for the global position estimate. class MavLinkHilGps : public MavLinkMessageBase { public: const static uint8_t kMessageId = 113; MavLinkHilGps() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Latitude (WGS84), in degrees * 1E7 + // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value + // of this field unless it is at least two, so always correctly fill in the fix. + uint8_t fix_type = 0; + // Latitude (WGS84) int32_t lat = 0; - // Longitude (WGS84), in degrees * 1E7 + // Longitude (WGS84) int32_t lon = 0; - // Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + // Altitude (MSL). Positive for up. int32_t alt = 0; - // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: - // 65535 + // GPS HDOP horizontal dilution of position. If unknown, set to: 65535 uint16_t eph = 0; - // GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 + // GPS VDOP vertical dilution of position. If unknown, set to: 65535 uint16_t epv = 0; - // GPS ground speed (m/s * 100). If unknown, set to: 65535 + // GPS ground speed. If unknown, set to: 65535 uint16_t vel = 0; - // GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + // GPS velocity in north direction in earth-fixed NED frame int16_t vn = 0; - // GPS velocity in cm/s in EAST direction in earth-fixed NED frame + // GPS velocity in east direction in earth-fixed NED frame int16_t ve = 0; - // GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + // GPS velocity in down direction in earth-fixed NED frame int16_t vd = 0; - // Course over ground (NOT heading, but direction of movement) in degrees * 100, - // 0.0..359.99 degrees. If unknown, set to: 65535 + // Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. + // If unknown, set to: 65535 uint16_t cog = 0; - // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value - // of this field unless it is at least two, so always correctly fill in the fix. - uint8_t fix_type = 0; // Number of satellites visible. If unknown, set to 255 uint8_t satellites_visible = 0; virtual std::string toJSon(); @@ -3878,11 +5408,14 @@ class MavLinkHilOpticalFlow : public MavLinkMessageBase { public: const static uint8_t kMessageId = 114; MavLinkHilOpticalFlow() { msgid = kMessageId; } - // Timestamp (microseconds, synced to UNIX time or since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Integration time in microseconds. Divide integrated_x and integrated_y by the - // integration time to obtain average flow. The integration time also indicates - // the. + // Sensor ID + uint8_t sensor_id = 0; + // Integration time. Divide integrated_x and integrated_y by the integration time + // to obtain average flow. The integration time also indicates the. uint32_t integration_time_us = 0; // Flow in radians around X axis (Sensor RH rotation about the X axis induces // a positive flow. Sensor linear motion along the positive Y axis induces a negative @@ -3892,23 +5425,21 @@ class MavLinkHilOpticalFlow : public MavLinkMessageBase { // a positive flow. Sensor linear motion along the positive X axis induces a positive // flow.) float integrated_y = 0; - // RH rotation around X axis (rad) + // RH rotation around X axis float integrated_xgyro = 0; - // RH rotation around Y axis (rad) + // RH rotation around Y axis float integrated_ygyro = 0; - // RH rotation around Z axis (rad) + // RH rotation around Z axis float integrated_zgyro = 0; - // Time in microseconds since the distance was sampled. - uint32_t time_delta_distance_us = 0; - // Distance to the center of the flow field in meters. Positive value (including - // zero): distance known. Negative value: Unknown distance. - float distance = 0; - // Temperature * 100 in centi-degrees Celsius + // Temperature int16_t temperature = 0; - // Sensor ID - uint8_t sensor_id = 0; // Optical flow quality / confidence. 0: no valid flow, 255: maximum quality uint8_t quality = 0; + // Time since the distance was sampled. + uint32_t time_delta_distance_us = 0; + // Distance to the center of the flow field. Positive value (including zero): + // distance known. Negative value: Unknown distance. + float distance = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3922,38 +5453,40 @@ class MavLinkHilStateQuaternion : public MavLinkMessageBase { public: const static uint8_t kMessageId = 115; MavLinkHilStateQuaternion() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with // 1 0 0 0 being the null-rotation) float attitude_quaternion[4] = { 0 }; - // Body frame roll / phi angular speed (rad/s) + // Body frame roll / phi angular speed float rollspeed = 0; - // Body frame pitch / theta angular speed (rad/s) + // Body frame pitch / theta angular speed float pitchspeed = 0; - // Body frame yaw / psi angular speed (rad/s) + // Body frame yaw / psi angular speed float yawspeed = 0; - // Latitude, expressed as * 1E7 + // Latitude int32_t lat = 0; - // Longitude, expressed as * 1E7 + // Longitude int32_t lon = 0; - // Altitude in meters, expressed as * 1000 (millimeters) + // Altitude int32_t alt = 0; - // Ground X Speed (Latitude), expressed as m/s * 100 + // Ground X Speed (Latitude) int16_t vx = 0; - // Ground Y Speed (Longitude), expressed as m/s * 100 + // Ground Y Speed (Longitude) int16_t vy = 0; - // Ground Z Speed (Altitude), expressed as m/s * 100 + // Ground Z Speed (Altitude) int16_t vz = 0; - // Indicated airspeed, expressed as m/s * 100 + // Indicated airspeed uint16_t ind_airspeed = 0; - // True airspeed, expressed as m/s * 100 + // True airspeed uint16_t true_airspeed = 0; - // X acceleration (mg) + // X acceleration int16_t xacc = 0; - // Y acceleration (mg) + // Y acceleration int16_t yacc = 0; - // Z acceleration (mg) + // Z acceleration int16_t zacc = 0; virtual std::string toJSon(); protected: @@ -3967,26 +5500,29 @@ class MavLinkScaledImu2 : public MavLinkMessageBase { public: const static uint8_t kMessageId = 116; MavLinkScaledImu2() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // X acceleration (mg) + // X acceleration int16_t xacc = 0; - // Y acceleration (mg) + // Y acceleration int16_t yacc = 0; - // Z acceleration (mg) + // Z acceleration int16_t zacc = 0; - // Angular speed around X axis (millirad /sec) + // Angular speed around X axis int16_t xgyro = 0; - // Angular speed around Y axis (millirad /sec) + // Angular speed around Y axis int16_t ygyro = 0; - // Angular speed around Z axis (millirad /sec) + // Angular speed around Z axis int16_t zgyro = 0; - // X Magnetic field (milli tesla) + // X Magnetic field int16_t xmag = 0; - // Y Magnetic field (milli tesla) + // Y Magnetic field int16_t ymag = 0; - // Z Magnetic field (milli tesla) + // Z Magnetic field int16_t zmag = 0; + // Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C + // it must send 1 (0.01C). + int16_t temperature = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -3999,14 +5535,14 @@ class MavLinkLogRequestList : public MavLinkMessageBase { public: const static uint8_t kMessageId = 117; MavLinkLogRequestList() { msgid = kMessageId; } - // First log id (0 for first available) - uint16_t start = 0; - // Last log id (0xffff for last available) - uint16_t end = 0; // System ID uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // First log id (0 for first available) + uint16_t start = 0; + // Last log id (0xffff for last available) + uint16_t end = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4018,16 +5554,16 @@ class MavLinkLogEntry : public MavLinkMessageBase { public: const static uint8_t kMessageId = 118; MavLinkLogEntry() { msgid = kMessageId; } - // UTC timestamp of log in seconds since 1970, or 0 if not available - uint32_t time_utc = 0; - // Size of the log (may be approximate) in bytes - uint32_t size = 0; // Log id uint16_t id = 0; // Total number of logs uint16_t num_logs = 0; // High log number uint16_t last_log_num = 0; + // UTC timestamp of log since 1970, or 0 if not available + uint32_t time_utc = 0; + // Size of the log (may be approximate) + uint32_t size = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4039,16 +5575,16 @@ class MavLinkLogRequestData : public MavLinkMessageBase { public: const static uint8_t kMessageId = 119; MavLinkLogRequestData() { msgid = kMessageId; } - // Offset into the log - uint32_t ofs = 0; - // Number of bytes - uint32_t count = 0; - // Log id (from LOG_ENTRY reply) - uint16_t id = 0; // System ID uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Log id (from LOG_ENTRY reply) + uint16_t id = 0; + // Offset into the log + uint32_t ofs = 0; + // Number of bytes + uint32_t count = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4060,10 +5596,10 @@ class MavLinkLogData : public MavLinkMessageBase { public: const static uint8_t kMessageId = 120; MavLinkLogData() { msgid = kMessageId; } - // Offset into the log - uint32_t ofs = 0; // Log id (from LOG_ENTRY reply) uint16_t id = 0; + // Offset into the log + uint32_t ofs = 0; // Number of bytes (zero for end of log) uint8_t count = 0; // log data @@ -4104,7 +5640,7 @@ class MavLinkLogRequestEnd : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// data for injecting into the onboard GPS (used for DGPS) +// Data for injecting into the onboard GPS (used for DGPS) class MavLinkGpsInjectData : public MavLinkMessageBase { public: const static uint8_t kMessageId = 123; @@ -4113,9 +5649,9 @@ class MavLinkGpsInjectData : public MavLinkMessageBase { uint8_t target_system = 0; // Component ID uint8_t target_component = 0; - // data length + // Data length uint8_t len = 0; - // raw data (110 is enough for 12 satellites of RTCMv2) + // Raw data (110 is enough for 12 satellites of RTCMv2) uint8_t data[110] = { 0 }; virtual std::string toJSon(); protected: @@ -4123,37 +5659,38 @@ class MavLinkGpsInjectData : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// Second GPS data. Coordinate frame is right-handed, Z-axis up (GPS frame). +// Second GPS data. class MavLinkGps2Raw : public MavLinkMessageBase { public: const static uint8_t kMessageId = 124; MavLinkGps2Raw() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Latitude (WGS84), in degrees * 1E7 + // GPS fix type. + uint8_t fix_type = 0; + // Latitude (WGS84) int32_t lat = 0; - // Longitude (WGS84), in degrees * 1E7 + // Longitude (WGS84) int32_t lon = 0; - // Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + // Altitude (MSL). Positive for up. int32_t alt = 0; - // Age of DGPS info - uint32_t dgps_age = 0; - // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: - // UINT16_MAX + // GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX uint16_t eph = 0; - // GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + // GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX uint16_t epv = 0; - // GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + // GPS ground speed. If unknown, set to: UINT16_MAX uint16_t vel = 0; - // Course over ground (NOT heading, but direction of movement) in degrees * 100, - // 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + // Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. + // If unknown, set to: UINT16_MAX uint16_t cog = 0; - // See the GPS_FIX_TYPE enum. - uint8_t fix_type = 0; // Number of satellites visible. If unknown, set to 255 uint8_t satellites_visible = 0; // Number of DGPS satellites uint8_t dgps_numch = 0; + // Age of DGPS info + uint32_t dgps_age = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4165,11 +5702,11 @@ class MavLinkPowerStatus : public MavLinkMessageBase { public: const static uint8_t kMessageId = 125; MavLinkPowerStatus() { msgid = kMessageId; } - // 5V rail voltage in millivolts + // 5V rail voltage. uint16_t Vcc = 0; - // servo rail voltage in millivolts + // Servo rail voltage. uint16_t Vservo = 0; - // power supply status flags (see MAV_POWER_STATUS enum) + // Bitmap of power supply status flags. uint16_t flags = 0; virtual std::string toJSon(); protected: @@ -4185,14 +5722,14 @@ class MavLinkSerialControl : public MavLinkMessageBase { public: const static uint8_t kMessageId = 126; MavLinkSerialControl() { msgid = kMessageId; } - // Baudrate of transfer. Zero means no change. - uint32_t baudrate = 0; - // Timeout for reply data in milliseconds - uint16_t timeout = 0; - // See SERIAL_CONTROL_DEV enum + // Serial control device type. uint8_t device = 0; - // See SERIAL_CONTROL_FLAG enum + // Bitmap of serial control flags. uint8_t flags = 0; + // Timeout for reply data + uint16_t timeout = 0; + // Baudrate of transfer. Zero means no change. + uint32_t baudrate = 0; // how many bytes in this transfer uint8_t count = 0; // serial data @@ -4209,32 +5746,32 @@ class MavLinkGpsRtk : public MavLinkMessageBase { public: const static uint8_t kMessageId = 127; MavLinkGpsRtk() { msgid = kMessageId; } - // Time since boot of last baseline message received in ms. + // Time since boot of last baseline message received. uint32_t time_last_baseline_ms = 0; + // Identification of connected RTK receiver. + uint8_t rtk_receiver_id = 0; + // GPS Week Number of last baseline + uint16_t wn = 0; // GPS Time of Week of last baseline uint32_t tow = 0; - // Current baseline in ECEF x or NED north component in mm. + // GPS-specific health report for RTK data. + uint8_t rtk_health = 0; + // Rate of baseline messages being received by GPS + uint8_t rtk_rate = 0; + // Current number of sats used for RTK calculation. + uint8_t nsats = 0; + // Coordinate system of baseline + uint8_t baseline_coords_type = 0; + // Current baseline in ECEF x or NED north component. int32_t baseline_a_mm = 0; - // Current baseline in ECEF y or NED east component in mm. + // Current baseline in ECEF y or NED east component. int32_t baseline_b_mm = 0; - // Current baseline in ECEF z or NED down component in mm. + // Current baseline in ECEF z or NED down component. int32_t baseline_c_mm = 0; // Current estimate of baseline accuracy. uint32_t accuracy = 0; // Current number of integer ambiguity hypotheses. int32_t iar_num_hypotheses = 0; - // GPS Week Number of last baseline - uint16_t wn = 0; - // Identification of connected RTK receiver. - uint8_t rtk_receiver_id = 0; - // GPS-specific health report for RTK data. - uint8_t rtk_health = 0; - // Rate of baseline messages being received by GPS, in HZ - uint8_t rtk_rate = 0; - // Current number of sats used for RTK calculation. - uint8_t nsats = 0; - // Coordinate system of baseline. 0 == ECEF, 1 == NED - uint8_t baseline_coords_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4247,32 +5784,32 @@ class MavLinkGps2Rtk : public MavLinkMessageBase { public: const static uint8_t kMessageId = 128; MavLinkGps2Rtk() { msgid = kMessageId; } - // Time since boot of last baseline message received in ms. + // Time since boot of last baseline message received. uint32_t time_last_baseline_ms = 0; + // Identification of connected RTK receiver. + uint8_t rtk_receiver_id = 0; + // GPS Week Number of last baseline + uint16_t wn = 0; // GPS Time of Week of last baseline uint32_t tow = 0; - // Current baseline in ECEF x or NED north component in mm. + // GPS-specific health report for RTK data. + uint8_t rtk_health = 0; + // Rate of baseline messages being received by GPS + uint8_t rtk_rate = 0; + // Current number of sats used for RTK calculation. + uint8_t nsats = 0; + // Coordinate system of baseline + uint8_t baseline_coords_type = 0; + // Current baseline in ECEF x or NED north component. int32_t baseline_a_mm = 0; - // Current baseline in ECEF y or NED east component in mm. + // Current baseline in ECEF y or NED east component. int32_t baseline_b_mm = 0; - // Current baseline in ECEF z or NED down component in mm. + // Current baseline in ECEF z or NED down component. int32_t baseline_c_mm = 0; // Current estimate of baseline accuracy. uint32_t accuracy = 0; // Current number of integer ambiguity hypotheses. int32_t iar_num_hypotheses = 0; - // GPS Week Number of last baseline - uint16_t wn = 0; - // Identification of connected RTK receiver. - uint8_t rtk_receiver_id = 0; - // GPS-specific health report for RTK data. - uint8_t rtk_health = 0; - // Rate of baseline messages being received by GPS, in HZ - uint8_t rtk_rate = 0; - // Current number of sats used for RTK calculation. - uint8_t nsats = 0; - // Coordinate system of baseline. 0 == ECEF, 1 == NED - uint8_t baseline_coords_type = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4285,50 +5822,55 @@ class MavLinkScaledImu3 : public MavLinkMessageBase { public: const static uint8_t kMessageId = 129; MavLinkScaledImu3() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // X acceleration (mg) + // X acceleration int16_t xacc = 0; - // Y acceleration (mg) + // Y acceleration int16_t yacc = 0; - // Z acceleration (mg) + // Z acceleration int16_t zacc = 0; - // Angular speed around X axis (millirad /sec) + // Angular speed around X axis int16_t xgyro = 0; - // Angular speed around Y axis (millirad /sec) + // Angular speed around Y axis int16_t ygyro = 0; - // Angular speed around Z axis (millirad /sec) + // Angular speed around Z axis int16_t zgyro = 0; - // X Magnetic field (milli tesla) + // X Magnetic field int16_t xmag = 0; - // Y Magnetic field (milli tesla) + // Y Magnetic field int16_t ymag = 0; - // Z Magnetic field (milli tesla) + // Z Magnetic field int16_t zmag = 0; + // Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C + // it must send 1 (0.01C). + int16_t temperature = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; +// Handshake message to initiate, control and stop image streaming when using the +// Image Transmission Protocol: https://mavlink.io/en/services/image_transmission.html. class MavLinkDataTransmissionHandshake : public MavLinkMessageBase { public: const static uint8_t kMessageId = 130; MavLinkDataTransmissionHandshake() { msgid = kMessageId; } - // total data size in bytes (set on ACK only) + // Type of requested/acknowledged data. + uint8_t type = 0; + // total data size (set on ACK only). uint32_t size = 0; - // Width of a matrix or image + // Width of a matrix or image. uint16_t width = 0; - // Height of a matrix or image + // Height of a matrix or image. uint16_t height = 0; - // number of packets beeing sent (set on ACK only) + // Number of packets being sent (set on ACK only). uint16_t packets = 0; - // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - uint8_t type = 0; - // payload size per packet (normally 253 byte, see DATA field size in message - // ENCAPSULATED_DATA) (set on ACK only) + // Payload size per packet (normally 253 byte, see DATA field size in message + // ENCAPSULATED_DATA) (set on ACK only). uint8_t payload = 0; - // JPEG quality out of [1,100] + // JPEG quality. Values: [1-100]. uint8_t jpg_quality = 0; virtual std::string toJSon(); protected: @@ -4336,6 +5878,7 @@ class MavLinkDataTransmissionHandshake : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; +// Data packet for images sent using the Image Transmission Protocol: https://mavlink.io/en/services/image_transmission.html. class MavLinkEncapsulatedData : public MavLinkMessageBase { public: const static uint8_t kMessageId = 131; @@ -4350,26 +5893,40 @@ class MavLinkEncapsulatedData : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; +// Distance sensor information for an onboard rangefinder. class MavLinkDistanceSensor : public MavLinkMessageBase { public: const static uint8_t kMessageId = 132; MavLinkDistanceSensor() { msgid = kMessageId; } - // Time since system boot + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Minimum distance the sensor can measure in centimeters + // Minimum distance the sensor can measure uint16_t min_distance = 0; - // Maximum distance the sensor can measure in centimeters + // Maximum distance the sensor can measure uint16_t max_distance = 0; // Current distance reading uint16_t current_distance = 0; - // Type from MAV_DISTANCE_SENSOR enum. + // Type of distance sensor. uint8_t type = 0; // Onboard ID of the sensor uint8_t id = 0; - // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum. + // Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: + // ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, + // left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 uint8_t orientation = 0; - // Measurement covariance in centimeters, 0 for unknown / invalid readings + // Measurement variance. Max standard deviation is 6cm. 255 if unknown. uint8_t covariance = 0; + // Horizontal Field of View (angle) where the distance measurement is valid and + // the field of view is known. Otherwise this is set to 0. + float horizontal_fov = 0; + // Vertical Field of View (angle) where the distance measurement is valid and + // the field of view is known. Otherwise this is set to 0. + float vertical_fov = 0; + // Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, + // zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. + // This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. + // Set it to 0 if invalid." + float quaternion[4] = { 0 }; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4381,14 +5938,14 @@ class MavLinkTerrainRequest : public MavLinkMessageBase { public: const static uint8_t kMessageId = 133; MavLinkTerrainRequest() { msgid = kMessageId; } - // Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) - uint64_t mask = 0; - // Latitude of SW corner of first grid (degrees *10^7) + // Latitude of SW corner of first grid int32_t lat = 0; - // Longitude of SW corner of first grid (in degrees *10^7) + // Longitude of SW corner of first grid int32_t lon = 0; - // Grid spacing in meters + // Grid spacing uint16_t grid_spacing = 0; + // Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + uint64_t mask = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4401,16 +5958,16 @@ class MavLinkTerrainData : public MavLinkMessageBase { public: const static uint8_t kMessageId = 134; MavLinkTerrainData() { msgid = kMessageId; } - // Latitude of SW corner of first grid (degrees *10^7) + // Latitude of SW corner of first grid int32_t lat = 0; - // Longitude of SW corner of first grid (in degrees *10^7) + // Longitude of SW corner of first grid int32_t lon = 0; - // Grid spacing in meters + // Grid spacing uint16_t grid_spacing = 0; - // Terrain data in meters AMSL - int16_t data[16] = { 0 }; // bit within the terrain request mask uint8_t gridbit = 0; + // Terrain data MSL + int16_t data[16] = { 0 }; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4423,9 +5980,9 @@ class MavLinkTerrainCheck : public MavLinkMessageBase { public: const static uint8_t kMessageId = 135; MavLinkTerrainCheck() { msgid = kMessageId; } - // Latitude (degrees *10^7) + // Latitude int32_t lat = 0; - // Longitude (degrees *10^7) + // Longitude int32_t lon = 0; virtual std::string toJSon(); protected: @@ -4438,16 +5995,16 @@ class MavLinkTerrainReport : public MavLinkMessageBase { public: const static uint8_t kMessageId = 136; MavLinkTerrainReport() { msgid = kMessageId; } - // Latitude (degrees *10^7) + // Latitude int32_t lat = 0; - // Longitude (degrees *10^7) + // Longitude int32_t lon = 0; - // Terrain height in meters AMSL - float terrain_height = 0; - // Current vehicle height above lat/lon terrain height (meters) - float current_height = 0; // grid spacing (zero if terrain at this location unavailable) uint16_t spacing = 0; + // Terrain height MSL + float terrain_height = 0; + // Current vehicle height above lat/lon terrain height + float current_height = 0; // Number of 4x4 terrain blocks waiting to be received or read from disk uint16_t pending = 0; // Number of 4x4 terrain blocks in memory @@ -4463,13 +6020,13 @@ class MavLinkScaledPressure2 : public MavLinkMessageBase { public: const static uint8_t kMessageId = 137; MavLinkScaledPressure2() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Absolute pressure (hectopascal) + // Absolute pressure float press_abs = 0; - // Differential pressure 1 (hectopascal) + // Differential pressure float press_diff = 0; - // Temperature measurement (0.01 degrees celsius) + // Temperature measurement int16_t temperature = 0; virtual std::string toJSon(); protected: @@ -4482,16 +6039,23 @@ class MavLinkAttPosMocap : public MavLinkMessageBase { public: const static uint8_t kMessageId = 138; MavLinkAttPosMocap() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) float q[4] = { 0 }; - // X position in meters (NED) + // X position (NED) float x = 0; - // Y position in meters (NED) + // Y position (NED) float y = 0; - // Z position in meters (NED) + // Z position (NED) float z = 0; + // Row-major representation of a pose 6x6 cross-covariance matrix upper right + // triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first + // ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value + // to first element in the array. + float covariance[21] = { 0 }; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4503,14 +6067,10 @@ class MavLinkSetActuatorControlTarget : public MavLinkMessageBase { public: const static uint8_t kMessageId = 139; MavLinkSetActuatorControlTarget() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for - // single rotation direction motors is 0..1, negative range for reverse direction. - // Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, - // yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through - // mixer to repurpose them as generic outputs. - float controls[8] = { 0 }; // Actuator group. The "_mlx" indicates this is a multi-instance message and a // MAVLink parser should use this field to difference between instances. uint8_t group_mlx = 0; @@ -4518,6 +6078,12 @@ class MavLinkSetActuatorControlTarget : public MavLinkMessageBase { uint8_t target_system = 0; // Component ID uint8_t target_component = 0; + // Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for + // single rotation direction motors is 0..1, negative range for reverse direction. + // Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, + // yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through + // mixer to repurpose them as generic outputs. + float controls[8] = { 0 }; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4529,17 +6095,19 @@ class MavLinkActuatorControlTarget : public MavLinkMessageBase { public: const static uint8_t kMessageId = 140; MavLinkActuatorControlTarget() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; + // Actuator group. The "_mlx" indicates this is a multi-instance message and a + // MAVLink parser should use this field to difference between instances. + uint8_t group_mlx = 0; // Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for // single rotation direction motors is 0..1, negative range for reverse direction. // Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, // yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through // mixer to repurpose them as generic outputs. float controls[8] = { 0 }; - // Actuator group. The "_mlx" indicates this is a multi-instance message and a - // MAVLink parser should use this field to difference between instances. - uint8_t group_mlx = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4551,7 +6119,9 @@ class MavLinkAltitude : public MavLinkMessageBase { public: const static uint8_t kMessageId = 141; MavLinkAltitude() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // This altitude measure is initialized on system boot and monotonic (it is never // reset, but represents the local altitude change). The only guarantee on this @@ -4563,7 +6133,7 @@ class MavLinkAltitude : public MavLinkMessageBase { // (it might reset on events like GPS lock or when a new QNH value is set). It // should be the altitude to which global altitude waypoints are compared to. // Note that it is *not* the GPS altitude, however, most GPS modules already output - // AMSL by default and not the WGS84 altitude. + // MSL by default and not the WGS84 altitude. float altitude_amsl = 0; // This is the local altitude in the local coordinate frame. It is not the altitude // above home, but in reference to the coordinate origin (0, 0, 0). It is up-positive. @@ -4614,13 +6184,13 @@ class MavLinkScaledPressure3 : public MavLinkMessageBase { public: const static uint8_t kMessageId = 143; MavLinkScaledPressure3() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Absolute pressure (hectopascal) + // Absolute pressure float press_abs = 0; - // Differential pressure 1 (hectopascal) + // Differential pressure float press_diff = 0; - // Temperature measurement (0.01 degrees celsius) + // Temperature measurement int16_t temperature = 0; virtual std::string toJSon(); protected: @@ -4628,20 +6198,21 @@ class MavLinkScaledPressure3 : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// current motion information from a designated system +// Current motion information from a designated system class MavLinkFollowTarget : public MavLinkMessageBase { public: const static uint8_t kMessageId = 144; MavLinkFollowTarget() { msgid = kMessageId; } - // Timestamp in milliseconds since system boot + // Timestamp (time since system boot). uint64_t timestamp = 0; - // button states or switches of a tracker device - uint64_t custom_state = 0; - // Latitude (WGS84), in degrees * 1E7 + // bit positions for tracker reporting capabilities (POS = 0, VEL = 1, ACCEL = + // 2, ATT + RATES = 3) + uint8_t est_capabilities = 0; + // Latitude (WGS84) int32_t lat = 0; - // Longitude (WGS84), in degrees * 1E7 + // Longitude (WGS84) int32_t lon = 0; - // AMSL, in meters + // Altitude (MSL) float alt = 0; // target velocity (0,0,0) for unknown float vel[3] = { 0 }; @@ -4653,9 +6224,8 @@ class MavLinkFollowTarget : public MavLinkMessageBase { float rates[3] = { 0 }; // eph epv float position_cov[3] = { 0 }; - // bit positions for tracker reporting capabilities (POS = 0, VEL = 1, ACCEL = - // 2, ATT + RATES = 3) - uint8_t est_capabilities = 0; + // button states or switches of a tracker device + uint64_t custom_state = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4667,7 +6237,9 @@ class MavLinkControlSystemState : public MavLinkMessageBase { public: const static uint8_t kMessageId = 146; MavLinkControlSystemState() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // X acceleration in body frame float x_acc = 0; @@ -4707,50 +6279,52 @@ class MavLinkControlSystemState : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; -// Battery information +// Battery information. Updates GCS with flight controller battery status. Use SMART_BATTERY_* +// messages instead for smart batteries. class MavLinkBatteryStatus : public MavLinkMessageBase { public: const static uint8_t kMessageId = 147; MavLinkBatteryStatus() { msgid = kMessageId; } - // Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide - // mAh consumption estimate - int32_t current_consumed = 0; - // Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot - // does not provide energy consumption estimate - int32_t energy_consumed = 0; - // Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown - // temperature. - int16_t temperature = 0; - // Battery voltage of cells, in millivolts (1 = 1 millivolt). Cells above the - // valid cell count for this battery should have the UINT16_MAX value. - uint16_t voltages[10] = { 0 }; - // Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does - // not measure the current - int16_t current_battery = 0; // Battery ID uint8_t id = 0; // Function of the battery uint8_t battery_function = 0; // Type (chemistry) of the battery uint8_t type = 0; - // Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate - // the remaining battery + // Temperature of the battery. INT16_MAX for unknown temperature. + int16_t temperature = 0; + // Battery voltage of cells. Cells above the valid cell count for this battery + // should have the UINT16_MAX value. + uint16_t voltages[10] = { 0 }; + // Battery current, -1: autopilot does not measure the current + int16_t current_battery = 0; + // Consumed charge, -1: autopilot does not provide consumption estimate + int32_t current_consumed = 0; + // Consumed energy, -1: autopilot does not provide energy consumption estimate + int32_t energy_consumed = 0; + // Remaining battery energy. Values: [0-100], -1: autopilot does not estimate + // the remaining battery. int8_t battery_remaining = 0; + // Remaining battery time, 0: autopilot does not provide remaining battery time + // estimate + int32_t time_remaining = 0; + // State for extent of discharge, provided by autopilot for warning or external + // reactions + uint8_t charge_state = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Version and capability of autopilot software +// Version and capability of autopilot software. This should be emitted in response +// to a MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES command. class MavLinkAutopilotVersion : public MavLinkMessageBase { public: const static uint8_t kMessageId = 148; MavLinkAutopilotVersion() { msgid = kMessageId; } - // bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + // Bitmap of capabilities uint64_t capabilities = 0; - // UID if provided by hardware - uint64_t uid = 0; // Firmware version number uint32_t flight_sw_version = 0; // Middleware version number @@ -4759,10 +6333,6 @@ class MavLinkAutopilotVersion : public MavLinkMessageBase { uint32_t os_sw_version = 0; // HW / board version (last 8 bytes should be silicon ID, if any) uint32_t board_version = 0; - // ID of the board vendor - uint16_t vendor_id = 0; - // ID of the product - uint16_t product_id = 0; // Custom version field, commonly the first 8 bytes of the git hash. This is not // an unique identifier, but should allow to identify the commit using the main // version number even for very large code bases. @@ -4775,34 +6345,79 @@ class MavLinkAutopilotVersion : public MavLinkMessageBase { // an unique identifier, but should allow to identify the commit using the main // version number even for very large code bases. uint8_t os_custom_version[8] = { 0 }; + // ID of the board vendor + uint16_t vendor_id = 0; + // ID of the product + uint16_t product_id = 0; + // UID if provided by hardware (see uid2) + uint64_t uid = 0; + // UID if provided by hardware (supersedes the uid field. If this is non-zero, + // use this field, otherwise use uid) + uint8_t uid2[18] = { 0 }; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// The location of a landing area captured from a downward facing camera +// The location of a landing target. See: https://mavlink.io/en/services/landing_target.html class MavLinkLandingTarget : public MavLinkMessageBase { public: const static uint8_t kMessageId = 149; MavLinkLandingTarget() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // X-axis angular offset (in radians) of the target from the center of the image + // The ID of the target if multiple targets are present + uint8_t target_num = 0; + // Coordinate frame used for following fields. + uint8_t frame = 0; + // X-axis angular offset of the target from the center of the image float angle_x = 0; - // Y-axis angular offset (in radians) of the target from the center of the image + // Y-axis angular offset of the target from the center of the image float angle_y = 0; - // Distance to the target from the vehicle in meters + // Distance to the target from the vehicle float distance = 0; - // Size in radians of target along x-axis + // Size of target along x-axis float size_x = 0; - // Size in radians of target along y-axis + // Size of target along y-axis float size_y = 0; - // The ID of the target if multiple targets are present - uint8_t target_num = 0; - // MAV_FRAME enum specifying the whether the following feilds are earth-frame, - // body-frame, etc. - uint8_t frame = 0; + // X Position of the landing target in MAV_FRAME + float x = 0; + // Y Position of the landing target in MAV_FRAME + float y = 0; + // Z Position of the landing target in MAV_FRAME + float z = 0; + // Quaternion of landing target orientation (w, x, y, z order, zero-rotation is + // 1, 0, 0, 0) + float q[4] = { 0 }; + // Type of landing target + uint8_t type = 0; + // Boolean indicating whether the position fields (x, y, z, q, type) contain valid + // target position information (valid: 1, invalid: 0). Default is 0 (invalid). + uint8_t position_valid = 0; + virtual std::string toJSon(); +protected: + virtual int pack(char* buffer) const; + virtual int unpack(const char* buffer); +}; + +// Status of geo-fencing. Sent in extended status stream when fencing enabled. +class MavLinkFenceStatus : public MavLinkMessageBase { +public: + const static uint8_t kMessageId = 162; + MavLinkFenceStatus() { msgid = kMessageId; } + // Breach status (0 if currently inside fence, 1 if outside). + uint8_t breach_status = 0; + // Number of fence breaches. + uint16_t breach_count = 0; + // Last breach type. + uint8_t breach_type = 0; + // Time (since boot) of last breach. + uint32_t breach_time = 0; + // Active action to prevent fence breach + uint8_t breach_mitigation = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -4812,8 +6427,8 @@ class MavLinkLandingTarget : public MavLinkMessageBase { // Estimator status message including flags, innovation test ratios and estimated // accuracies. The flags message is an integer bitmask containing information on which // EKF outputs are valid. See the ESTIMATOR_STATUS_FLAGS enum definition for further -// information. The innovaton test ratios show the magnitude of the sensor innovation -// divided by the innovation check threshold. Under normal operation the innovaton +// information. The innovation test ratios show the magnitude of the sensor innovation +// divided by the innovation check threshold. Under normal operation the innovation // test ratios should be below 0.5 with occasional values up to 1.0. Values greater // than 1.0 should be rare under normal operation and indicate that a measurement // has been rejected by the filter. The user should be notified if an innovation test @@ -4823,8 +6438,12 @@ class MavLinkEstimatorStatus : public MavLinkMessageBase { public: const static uint8_t kMessageId = 230; MavLinkEstimatorStatus() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; + // Bitmap indicating which EKF outputs are valid. + uint16_t flags = 0; // Velocity innovation test ratio float vel_ratio = 0; // Horizontal position innovation test ratio @@ -4837,36 +6456,36 @@ class MavLinkEstimatorStatus : public MavLinkMessageBase { float hagl_ratio = 0; // True airspeed innovation test ratio float tas_ratio = 0; - // Horizontal position 1-STD accuracy relative to the EKF local origin (m) + // Horizontal position 1-STD accuracy relative to the EKF local origin float pos_horiz_accuracy = 0; - // Vertical position 1-STD accuracy relative to the EKF local origin (m) + // Vertical position 1-STD accuracy relative to the EKF local origin float pos_vert_accuracy = 0; - // Integer bitmask indicating which EKF outputs are valid. See definition for - // ESTIMATOR_STATUS_FLAGS. - uint16_t flags = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; +// Wind covariance estimate from vehicle. class MavLinkWindCov : public MavLinkMessageBase { public: const static uint8_t kMessageId = 231; MavLinkWindCov() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // Wind in X (NED) direction in m/s + // Wind in X (NED) direction float wind_x = 0; - // Wind in Y (NED) direction in m/s + // Wind in Y (NED) direction float wind_y = 0; - // Wind in Z (NED) direction in m/s + // Wind in Z (NED) direction float wind_z = 0; // Variability of the wind in XY. RMS of a 1 Hz lowpassed wind estimate. float var_horiz = 0; // Variability of the wind in Z. RMS of a 1 Hz lowpassed wind estimate. float var_vert = 0; - // AMSL altitude (m) this measurement was taken at + // Altitude (MSL) that this measurement was taken at float wind_alt = 0; // Horizontal speed 1-STD accuracy float horiz_accuracy = 0; @@ -4879,60 +6498,73 @@ class MavLinkWindCov : public MavLinkMessageBase { }; // GPS sensor input message. This is a raw sensor value sent by the GPS. This is NOT -// the global position estimate of the sytem. +// the global position estimate of the system. class MavLinkGpsInput : public MavLinkMessageBase { public: const static uint8_t kMessageId = 232; MavLinkGpsInput() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; - // GPS time (milliseconds from start of GPS week) + // ID of the GPS for multiple GPS inputs + uint8_t gps_id = 0; + // Bitmap indicating which GPS input flags fields to ignore. All other fields + // must be provided. + uint16_t ignore_flags = 0; + // GPS time (from start of GPS week) uint32_t time_week_ms = 0; - // Latitude (WGS84), in degrees * 1E7 + // GPS week number + uint16_t time_week = 0; + // 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK + uint8_t fix_type = 0; + // Latitude (WGS84) int32_t lat = 0; - // Longitude (WGS84), in degrees * 1E7 + // Longitude (WGS84) int32_t lon = 0; - // Altitude (AMSL, not WGS84), in m (positive for up) + // Altitude (MSL). Positive for up. float alt = 0; - // GPS HDOP horizontal dilution of position in m + // GPS HDOP horizontal dilution of position float hdop = 0; - // GPS VDOP vertical dilution of position in m + // GPS VDOP vertical dilution of position float vdop = 0; - // GPS velocity in m/s in NORTH direction in earth-fixed NED frame + // GPS velocity in north direction in earth-fixed NED frame float vn = 0; - // GPS velocity in m/s in EAST direction in earth-fixed NED frame + // GPS velocity in east direction in earth-fixed NED frame float ve = 0; - // GPS velocity in m/s in DOWN direction in earth-fixed NED frame + // GPS velocity in down direction in earth-fixed NED frame float vd = 0; - // GPS speed accuracy in m/s + // GPS speed accuracy float speed_accuracy = 0; - // GPS horizontal accuracy in m + // GPS horizontal accuracy float horiz_accuracy = 0; - // GPS vertical accuracy in m + // GPS vertical accuracy float vert_accuracy = 0; - // Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum). - // All other fields must be provided. - uint16_t ignore_flags = 0; - // GPS week number - uint16_t time_week = 0; - // ID of the GPS for multiple GPS inputs - uint8_t gps_id = 0; - // 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK - uint8_t fix_type = 0; // Number of satellites visible. uint8_t satellites_visible = 0; + // Yaw of vehicle relative to Earth's North, zero means not available, use 36000 + // for north + uint16_t yaw = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// WORK IN PROGRESS! RTCM message for injecting into the onboard GPS (used for DGPS) +// RTCM message for injecting into the onboard GPS (used for DGPS) class MavLinkGpsRtcmData : public MavLinkMessageBase { public: const static uint8_t kMessageId = 233; MavLinkGpsRtcmData() { msgid = kMessageId; } - // LSB: 1 means message is fragmented + // LSB: 1 means message is fragmented, next 2 bits are the fragment ID, the remaining + // 5 bits are used for the sequence ID. Messages are only to be flushed to the + // GPS when the entire message has been reconstructed on the autopilot. The fragment + // ID specifies which order the fragments should be assembled into a buffer, while + // the sequence ID is used to detect a mismatch between different buffers. The + // buffer is considered fully reconstructed when either all 4 fragments are present, + // or all the fragments before the first fragment with a non full payload is received. + // This management is used to ensure that normal GPS operation doesn't corrupt + // RTCM data, and to recover from a unreliable transport delivery order. uint8_t flags = 0; // data length uint8_t len = 0; @@ -4949,51 +6581,41 @@ class MavLinkHighLatency : public MavLinkMessageBase { public: const static uint8_t kMessageId = 234; MavLinkHighLatency() { msgid = kMessageId; } - // Timestamp (microseconds since UNIX epoch) - uint64_t time_usec = 0; + // Bitmap of enabled system modes. + uint8_t base_mode = 0; // A bitfield for use for autopilot-specific flags. uint32_t custom_mode = 0; - // Latitude, expressed as degrees * 1E7 - int32_t latitude = 0; - // Longitude, expressed as degrees * 1E7 - int32_t longitude = 0; - // roll (centidegrees) + // The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + uint8_t landed_state = 0; + // roll int16_t roll = 0; - // pitch (centidegrees) + // pitch int16_t pitch = 0; - // heading (centidegrees) + // heading uint16_t heading = 0; - // roll setpoint (centidegrees) - int16_t roll_sp = 0; - // pitch setpoint (centidegrees) - int16_t pitch_sp = 0; - // heading setpoint (centidegrees) + // throttle (percentage) + int8_t throttle = 0; + // heading setpoint int16_t heading_sp = 0; - // Altitude above the home position (meters) - int16_t altitude_home = 0; - // Altitude above mean sea level (meters) + // Latitude + int32_t latitude = 0; + // Longitude + int32_t longitude = 0; + // Altitude above mean sea level int16_t altitude_amsl = 0; - // Altitude setpoint relative to the home position (meters) + // Altitude setpoint relative to the home position int16_t altitude_sp = 0; - // distance to target (meters) - uint16_t wp_distance = 0; - // System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h - uint8_t base_mode = 0; - // The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. - uint8_t landed_state = 0; - // throttle (percentage) - int8_t throttle = 0; - // airspeed (m/s) + // airspeed uint8_t airspeed = 0; - // airspeed setpoint (m/s) + // airspeed setpoint uint8_t airspeed_sp = 0; - // groundspeed (m/s) + // groundspeed uint8_t groundspeed = 0; - // climb rate (m/s) + // climb rate int8_t climb_rate = 0; // Number of satellites visible. If unknown, set to 255 uint8_t gps_nsat = 0; - // See the GPS_FIX_TYPE enum. + // GPS Fix type. uint8_t gps_fix_type = 0; // Remaining battery (percentage) uint8_t battery_remaining = 0; @@ -5006,6 +6628,74 @@ class MavLinkHighLatency : public MavLinkMessageBase { uint8_t failsafe = 0; // current waypoint number uint8_t wp_num = 0; + // distance to target + uint16_t wp_distance = 0; + virtual std::string toJSon(); +protected: + virtual int pack(char* buffer) const; + virtual int unpack(const char* buffer); +}; + +// Message appropriate for high latency connections like Iridium (version 2) +class MavLinkHighLatency2 : public MavLinkMessageBase { +public: + const static uint8_t kMessageId = 235; + MavLinkHighLatency2() { msgid = kMessageId; } + // Timestamp (milliseconds since boot or Unix epoch) + uint32_t timestamp = 0; + // Type of the MAV (quadrotor, helicopter, etc.) + uint8_t type = 0; + // Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not + // flight controllers. + uint8_t autopilot = 0; + // A bitfield for use for autopilot-specific flags (2 byte version). + uint16_t custom_mode = 0; + // Latitude + int32_t latitude = 0; + // Longitude + int32_t longitude = 0; + // Altitude above mean sea level + int16_t altitude = 0; + // Altitude setpoint + int16_t target_altitude = 0; + // Heading + uint8_t heading = 0; + // Heading setpoint + uint8_t target_heading = 0; + // Distance to target waypoint or position + uint16_t target_distance = 0; + // Throttle + uint8_t throttle = 0; + // Airspeed + uint8_t airspeed = 0; + // Airspeed setpoint + uint8_t airspeed_sp = 0; + // Groundspeed + uint8_t groundspeed = 0; + // Windspeed + uint8_t windspeed = 0; + // Wind heading + uint8_t wind_heading = 0; + // Maximum error horizontal position since last message + uint8_t eph = 0; + // Maximum error vertical position since last message + uint8_t epv = 0; + // Air temperature from airspeed sensor + int8_t temperature_air = 0; + // Maximum climb rate magnitude since last message + int8_t climb_rate = 0; + // Battery level (-1 if field not provided). + int8_t battery = 0; + // Current waypoint number + uint16_t wp_num = 0; + // Bitmap of failure flags. + uint16_t failure_flags = 0; + // Field for custom payload. + int8_t custom0 = 0; + // Field for custom payload. + int8_t custom1 = 0; + // Field for custom payload. + int8_t custom2 = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -5017,7 +6707,9 @@ class MavLinkVibration : public MavLinkMessageBase { public: const static uint8_t kMessageId = 241; MavLinkVibration() { msgid = kMessageId; } - // Timestamp (micros since boot or Unix epoch) + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // Vibration levels on X-axis float vibration_x = 0; @@ -5039,7 +6731,7 @@ class MavLinkVibration : public MavLinkMessageBase { // This message can be requested by sending the MAV_CMD_GET_HOME_POSITION command. // The position the system will return to and land on. The position is set automatically -// by the system during the takeoff in case it was not explicitely set by the operator +// by the system during the takeoff in case it was not explicitly set by the operator // before or after. The position the system will return to and land on. The global // and local positions encode the position in the respective coordinate frames, while // the q parameter encodes the orientation of the surface. Under normal conditions @@ -5051,11 +6743,11 @@ class MavLinkHomePosition : public MavLinkMessageBase { public: const static uint8_t kMessageId = 242; MavLinkHomePosition() { msgid = kMessageId; } - // Latitude (WGS84), in degrees * 1E7 + // Latitude (WGS84) int32_t latitude = 0; - // Longitude (WGS84, in degrees * 1E7 + // Longitude (WGS84) int32_t longitude = 0; - // Altitude (AMSL), in meters * 1000 (positive for up) + // Altitude (MSL). Positive for up. int32_t altitude = 0; // Local X position of this position in the local coordinate frame float x = 0; @@ -5084,6 +6776,10 @@ class MavLinkHomePosition : public MavLinkMessageBase { // should set it to the opposite direction of the takeoff, assuming the takeoff // happened from the threshold / touchdown zone. float approach_z = 0; + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. + uint64_t time_usec = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -5091,7 +6787,7 @@ class MavLinkHomePosition : public MavLinkMessageBase { }; // The position the system will return to and land on. The position is set automatically -// by the system during the takeoff in case it was not explicitely set by the operator +// by the system during the takeoff in case it was not explicitly set by the operator // before or after. The global and local positions encode the position in the respective // coordinate frames, while the q parameter encodes the orientation of the surface. // Under normal conditions it describes the heading and terrain slope, which can be @@ -5102,11 +6798,13 @@ class MavLinkSetHomePosition : public MavLinkMessageBase { public: const static uint8_t kMessageId = 243; MavLinkSetHomePosition() { msgid = kMessageId; } - // Latitude (WGS84), in degrees * 1E7 + // System ID. + uint8_t target_system = 0; + // Latitude (WGS84) int32_t latitude = 0; - // Longitude (WGS84, in degrees * 1E7 + // Longitude (WGS84) int32_t longitude = 0; - // Altitude (AMSL), in meters * 1000 (positive for up) + // Altitude (MSL). Positive for up. int32_t altitude = 0; // Local X position of this position in the local coordinate frame float x = 0; @@ -5135,25 +6833,29 @@ class MavLinkSetHomePosition : public MavLinkMessageBase { // should set it to the opposite direction of the takeoff, assuming the takeoff // happened from the threshold / touchdown zone. float approach_z = 0; - // System ID. - uint8_t target_system = 0; + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. + uint64_t time_usec = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// This interface replaces DATA_STREAM +// The interval between messages for a particular MAVLink message ID. This message +// is the response to the MAV_CMD_GET_MESSAGE_INTERVAL command. This interface replaces +// DATA_STREAM. class MavLinkMessageInterval : public MavLinkMessageBase { public: const static uint8_t kMessageId = 244; MavLinkMessageInterval() { msgid = kMessageId; } - // The interval between two messages, in microseconds. A value of -1 indicates - // this stream is disabled, 0 indicates it is not available, > 0 indicates the - // interval at which it is sent. - int32_t interval_us = 0; // The ID of the requested MAVLink message. v1.0 is limited to 254 messages. uint16_t message_id = 0; + // The interval between two messages. A value of -1 indicates this stream is disabled, + // 0 indicates it is not available, > 0 indicates the interval at which it is + // sent. + int32_t interval_us = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -5183,30 +6885,30 @@ class MavLinkAdsbVehicle : public MavLinkMessageBase { MavLinkAdsbVehicle() { msgid = kMessageId; } // ICAO address uint32_t ICAO_address = 0; - // Latitude, expressed as degrees * 1E7 + // Latitude int32_t lat = 0; - // Longitude, expressed as degrees * 1E7 + // Longitude int32_t lon = 0; - // Altitude(ASL) in millimeters + // ADSB altitude type. + uint8_t altitude_type = 0; + // Altitude(ASL) int32_t altitude = 0; - // Course over ground in centidegrees + // Course over ground uint16_t heading = 0; - // The horizontal velocity in centimeters/second + // The horizontal velocity uint16_t hor_velocity = 0; - // The vertical velocity in centimeters/second, positive is up + // The vertical velocity. Positive is up int16_t ver_velocity = 0; - // Flags to indicate various statuses including valid data fields - uint16_t flags = 0; - // Squawk code - uint16_t squawk = 0; - // Type from ADSB_ALTITUDE_TYPE enum - uint8_t altitude_type = 0; // The callsign, 8+null char callsign[9] = { 0 }; - // Type from ADSB_EMITTER_TYPE enum + // ADSB emitter type. uint8_t emitter_type = 0; // Time since last communication in seconds uint8_t tslc = 0; + // Bitmap to indicate various statuses including valid data fields + uint16_t flags = 0; + // Squawk code + uint16_t squawk = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -5218,20 +6920,20 @@ class MavLinkCollision : public MavLinkMessageBase { public: const static uint8_t kMessageId = 247; MavLinkCollision() { msgid = kMessageId; } - // Unique identifier, domain based on src field - uint32_t id = 0; - // Estimated time until collision occurs (seconds) - float time_to_minimum_delta = 0; - // Closest vertical distance in meters between vehicle and object - float altitude_minimum_delta = 0; - // Closest horizontal distance in meteres between vehicle and object - float horizontal_minimum_delta = 0; // Collision data source uint8_t src = 0; + // Unique identifier, domain based on src field + uint32_t id = 0; // Action that is being taken to avoid this collision uint8_t action = 0; // How concerned the aircraft is about this collision uint8_t threat_level = 0; + // Estimated time until collision occurs + float time_to_minimum_delta = 0; + // Closest vertical distance between vehicle and object + float altitude_minimum_delta = 0; + // Closest horizontal distance between vehicle and object + float horizontal_minimum_delta = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -5244,25 +6946,28 @@ class MavLinkV2Extension : public MavLinkMessageBase { public: const static uint8_t kMessageId = 248; MavLinkV2Extension() { msgid = kMessageId; } - // A code that identifies the software component that understands this message - // (analogous to usb device classes or mime type strings). If this code is less - // than 32768, it is considered a 'registered' protocol extension and the corresponding - // entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. - // Software creators can register blocks of message IDs as needed (useful for - // GCS specific metadata, etc...). Message_types greater than 32767 are considered - // local experiments and should not be checked in to any widely distributed codebase. - uint16_t message_type = 0; // Network ID (0 for broadcast) uint8_t target_network = 0; // System ID (0 for broadcast) uint8_t target_system = 0; // Component ID (0 for broadcast) uint8_t target_component = 0; - // Variable length payload. The length is defined by the remaining message length - // when subtracting the header and other fields. The entire content of this block - // is opaque unless you understand any the encoding message_type. The particular + // A code that identifies the software component that understands this message + // (analogous to USB device classes or mime type strings). If this code is less + // than 32768, it is considered a 'registered' protocol extension and the corresponding + // entry should be added to https://github.com/mavlink/mavlink/definition_files/extension_message_ids.xml. + // Software creators can register blocks of message IDs as needed (useful for + // GCS specific metadata, etc...). Message_types greater than 32767 are considered + // local experiments and should not be checked in to any widely distributed codebase. + uint16_t message_type = 0; + // Variable length payload. The length must be encoded in the payload as part + // of the message_type protocol, e.g. by including the length as payload data, + // or by terminating the payload data with a non-zero marker. This is required + // in order to reconstruct zero-terminated payloads that are (or otherwise would + // be) trimmed by MAVLink 2 empty-byte truncation. The entire content of the payload + // block is opaque unless you understand the encoding message_type. The particular // encoding used can be extension specific and might not always be documented - // as part of the mavlink specification. + // as part of the MAVLink specification. uint8_t payload[249] = { 0 }; virtual std::string toJSon(); protected: @@ -5293,11 +6998,16 @@ class MavLinkMemoryVect : public MavLinkMessageBase { virtual int unpack(const char* buffer); }; +// To debug something using a named 3D vector. class MavLinkDebugVect : public MavLinkMessageBase { public: const static uint8_t kMessageId = 250; MavLinkDebugVect() { msgid = kMessageId; } - // Timestamp + // Name + char name[10] = { 0 }; + // Timestamp (UNIX Epoch time or time since system boot). The receiving end can + // infer timestamp format (since 1.1.1970 or since system boot) by checking for + // the magnitude the number. uint64_t time_usec = 0; // x float x = 0; @@ -5305,8 +7015,6 @@ class MavLinkDebugVect : public MavLinkMessageBase { float y = 0; // z float z = 0; - // Name - char name[10] = { 0 }; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -5320,12 +7028,12 @@ class MavLinkNamedValueFloat : public MavLinkMessageBase { public: const static uint8_t kMessageId = 251; MavLinkNamedValueFloat() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Floating point value - float value = 0; // Name of the debug variable char name[10] = { 0 }; + // Floating point value + float value = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -5339,12 +7047,12 @@ class MavLinkNamedValueInt : public MavLinkMessageBase { public: const static uint8_t kMessageId = 252; MavLinkNamedValueInt() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // Signed integer value - int32_t value = 0; // Name of the debug variable char name[10] = { 0 }; + // Signed integer value + int32_t value = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; @@ -5359,7 +7067,7 @@ class MavLinkStatustext : public MavLinkMessageBase { public: const static uint8_t kMessageId = 253; MavLinkStatustext() { msgid = kMessageId; } - // Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + // Severity of status. Relies on the definitions within RFC-5424. uint8_t severity = 0; // Status text message, without null termination character char text[50] = { 0 }; @@ -5375,55 +7083,33 @@ class MavLinkDebug : public MavLinkMessageBase { public: const static uint8_t kMessageId = 254; MavLinkDebug() { msgid = kMessageId; } - // Timestamp (milliseconds since system boot) + // Timestamp (time since system boot). uint32_t time_boot_ms = 0; - // DEBUG value - float value = 0; // index of debug variable uint8_t ind = 0; + // DEBUG value + float value = 0; virtual std::string toJSon(); protected: virtual int pack(char* buffer) const; virtual int unpack(const char* buffer); }; -// Version and capability of protocol version. This message is the response to REQUEST_PROTOCOL_VERSION -// and is used as part of the handshaking to establish which MAVLink version should be used on the network. -// Every node should respond to REQUEST_PROTOCOL_VERSION to enable the handshaking. Library implementers -// should consider adding this into the default decoding state machine to allow the protocol core to respond directly. -class MavLinkProtocolVersion : public MavLinkMessageBase { -public: - const static uint32_t kMessageId = 300; - MavLinkProtocolVersion() { msgid = kMessageId; } - // Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. - uint16_t version = 0; - uint16_t min_version = 0; - uint16_t max_version = 0; - uint8_t spec_version_hash[8] = { 0 }; - uint8_t library_version_hash[8] = { 0 }; - virtual std::string toJSon(); -protected: - virtual int pack(char* buffer) const; - virtual int unpack(const char* buffer); -}; - -// Navigate to MISSION. +// Navigate to waypoint. class MavCmdNavWaypoint : public MavLinkCommand { public: const static uint16_t kCommandId = 16; MavCmdNavWaypoint() { command = kCommandId; } - // Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION - // for rotary wing) - float HoldTimeDecimal = 0; - // Acceptance radius in meters (if the sphere with this radius is hit, the MISSION - // counts as reached) - float AcceptanceRadiusMeters = 0; - // 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value - // for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory - // control. - float p0ToPass = 0; - // Desired yaw angle at MISSION (rotary wing) - float DesiredYawAngle = 0; + // Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) + float Hold = 0; + // Acceptance radius (if the sphere with this radius is hit, the waypoint counts + // as reached) + float AcceptRadius = 0; + // 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise + // orbit, negative value for counter-clockwise orbit. Allows trajectory control. + float PassRadius = 0; + // Desired yaw angle at waypoint (rotary wing). NaN for unchanged. + float Yaw = 0; // Latitude float Latitude = 0; // Longitude @@ -5434,15 +7120,15 @@ class MavCmdNavWaypoint : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; -// Loiter around this MISSION an unlimited amount of time +// Loiter around this waypoint an unlimited amount of time class MavCmdNavLoiterUnlim : public MavLinkCommand { public: const static uint16_t kCommandId = 17; MavCmdNavLoiterUnlim() { command = kCommandId; } - // Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise - float RadiusAroundMission = 0; - // Desired yaw angle. - float DesiredYawAngle = 0; + // Radius around waypoint. If positive loiter clockwise, else counter-clockwise + float Radius = 0; + // Desired yaw angle. NaN for unchanged. + float Yaw = 0; // Latitude float Latitude = 0; // Longitude @@ -5453,17 +7139,17 @@ class MavCmdNavLoiterUnlim : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; -// Loiter around this MISSION for X turns +// Loiter around this waypoint for X turns class MavCmdNavLoiterTurns : public MavLinkCommand { public: const static uint16_t kCommandId = 18; MavCmdNavLoiterTurns() { command = kCommandId; } - // Turns + // Number of turns. float Turns = 0; - // Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise - float RadiusAroundMission = 0; + // Radius around waypoint. If positive loiter clockwise, else counter-clockwise + float Radius = 0; // Forward moving aircraft this sets exit xtrack location: 0 for center of loiter - // wp, 1 for exit location. Else, this is desired yaw angle + // wp, 1 for exit location. Else, this is desired yaw angle. NaN for unchanged. float ExitXtrackLocation = 0; // Latitude float Latitude = 0; @@ -5475,17 +7161,17 @@ class MavCmdNavLoiterTurns : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; -// Loiter around this MISSION for X seconds +// Loiter around this waypoint for X seconds class MavCmdNavLoiterTime : public MavLinkCommand { public: const static uint16_t kCommandId = 19; MavCmdNavLoiterTime() { command = kCommandId; } - // Seconds (decimal) - float Seconds = 0; - // Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise - float RadiusAroundMission = 0; + // Loiter time. + float Time = 0; + // Radius around waypoint. If positive loiter clockwise, else counter-clockwise. + float Radius = 0; // Forward moving aircraft this sets exit xtrack location: 0 for center of loiter - // wp, 1 for exit location. Else, this is desired yaw angle + // wp, 1 for exit location. Else, this is desired yaw angle. NaN for unchanged. float ExitXtrackLocation = 0; // Latitude float Latitude = 0; @@ -5506,21 +7192,23 @@ class MavCmdNavReturnToLaunch : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; -// Land at location +// Land at location. class MavCmdNavLand : public MavLinkCommand { public: const static uint16_t kCommandId = 21; MavCmdNavLand() { command = kCommandId; } - // Abort Alt + // Minimum target altitude if landing is aborted (0 = undefined/use system default). float AbortAlt = 0; - // Desired yaw angle - float DesiredYawAngle = 0; - // Latitude + // Precision land mode. + float LandMode = 0; + // Desired yaw angle. NaN for unchanged. + float YawAngle = 0; + // Latitude. float Latitude = 0; - // Longitude + // Longitude. float Longitude = 0; - // Altitude - float Altitude = 0; + // Landing altitude (ground level in current frame). + float LandingAltitude = 0; protected: virtual void pack(); virtual void unpack(); @@ -5531,9 +7219,10 @@ class MavCmdNavTakeoff : public MavLinkCommand { const static uint16_t kCommandId = 22; MavCmdNavTakeoff() { command = kCommandId; } // Minimum pitch (if airspeed sensor present), desired pitch without sensor - float MinimumPitch = 0; - // Yaw angle (if magnetometer present), ignored without magnetometer - float YawAngle = 0; + float Pitch = 0; + // Yaw angle (if magnetometer present), ignored without magnetometer. NaN for + // unchanged. + float Yaw = 0; // Latitude float Latitude = 0; // Longitude @@ -5550,22 +7239,22 @@ class MavCmdNavLandLocal : public MavLinkCommand { const static uint16_t kCommandId = 23; MavCmdNavLandLocal() { command = kCommandId; } // Landing target number (if available) - float LandingTargetNumber = 0; - // Maximum accepted offset from desired landing position [m] - computed magnitude + float Target = 0; + // Maximum accepted offset from desired landing position - computed magnitude // from spherical coordinates: d = sqrt(x^2 + y^2 + z^2), which gives the maximum // accepted distance between the desired landing position and the position where // the vehicle is about to land - float MaximumAcceptedOffset = 0; - // Landing descend rate [ms^-1] - float LandingDescendRate = 0; - // Desired yaw angle [rad] - float DesiredYawAngle = 0; - // Y-axis position [m] - float Y = 0; - // X-axis position [m] - float X = 0; - // Z-axis / ground level position [m] - float Z = 0; + float Offset = 0; + // Landing descend rate + float DescendRate = 0; + // Desired yaw angle + float Yaw = 0; + // Y-axis position + float YPosition = 0; + // X-axis position + float XPosition = 0; + // Z-axis / ground level position + float ZPosition = 0; protected: virtual void pack(); virtual void unpack(); @@ -5575,19 +7264,19 @@ class MavCmdNavTakeoffLocal : public MavLinkCommand { public: const static uint16_t kCommandId = 24; MavCmdNavTakeoffLocal() { command = kCommandId; } - // Minimum pitch (if airspeed sensor present), desired pitch without sensor [rad] - float MinimumPitch = 0; - // Takeoff ascend rate [ms^-1] - float TakeoffAscendRate = 0; - // Yaw angle [rad] (if magnetometer or another yaw estimation source present), - // ignored without one of these - float YawAngle = 0; - // Y-axis position [m] - float Y = 0; - // X-axis position [m] - float X = 0; - // Z-axis position [m] - float Z = 0; + // Minimum pitch (if airspeed sensor present), desired pitch without sensor + float Pitch = 0; + // Takeoff ascend rate + float AscendRate = 0; + // Yaw angle (if magnetometer or another yaw estimation source present), ignored + // without one of these + float Yaw = 0; + // Y-axis position + float YPosition = 0; + // X-axis position + float XPosition = 0; + // Z-axis position + float ZPosition = 0; protected: virtual void pack(); virtual void unpack(); @@ -5599,13 +7288,13 @@ class MavCmdNavFollow : public MavLinkCommand { MavCmdNavFollow() { command = kCommandId; } // Following logic to use (e.g. loitering or sinusoidal following) - depends on // specific autopilot implementation - float FollowingLogicTo = 0; + float Following = 0; // Ground speed of vehicle to be followed - float GroundSpeedOf = 0; - // Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise - float RadiusAroundMission = 0; + float GroundSpeed = 0; + // Radius around waypoint. If positive loiter clockwise, else counter-clockwise + float Radius = 0; // Desired yaw angle. - float DesiredYawAngle = 0; + float Yaw = 0; // Latitude float Latitude = 0; // Longitude @@ -5626,9 +7315,9 @@ class MavCmdNavContinueAndChangeAlt : public MavLinkCommand { // Climb or Descend (0 = Neutral, command completes when within 5m of this command's // altitude, 1 = Climbing, command completes when at or above this command's altitude, // 2 = Descending, command completes when at or below this command's altitude. - float ClimbOrDescend = 0; - // Desired altitude in meters - float DesiredAltitudeMeters = 0; + float Action = 0; + // Desired altitude + float Altitude = 0; protected: virtual void pack(); virtual void unpack(); @@ -5644,12 +7333,12 @@ class MavCmdNavLoiterToAlt : public MavLinkCommand { MavCmdNavLoiterToAlt() { command = kCommandId; } // Heading Required (0 = False) float HeadingRequired = 0; - // Radius in meters. If positive loiter clockwise, negative counter-clockwise, - // 0 means no change to standard loiter. - float RadiusMeters = 0; + // Radius. If positive loiter clockwise, negative counter-clockwise, 0 means no + // change to standard loiter. + float Radius = 0; // Forward moving aircraft this sets exit xtrack location: 0 for center of loiter // wp, 1 for exit location - float ExitXtrackLocation = 0; + float XtrackLocation = 0; // Latitude float Latitude = 0; // Longitude @@ -5660,22 +7349,22 @@ class MavCmdNavLoiterToAlt : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; -// Being following a target +// Begin following a target class MavCmdDoFollow : public MavLinkCommand { public: const static uint16_t kCommandId = 32; MavCmdDoFollow() { command = kCommandId; } - // System ID (the system ID of the FOLLOW_TARGET beacon). Send 0 to disable follow-me - // and return to the default position hold mode + // System ID (of the FOLLOW_TARGET beacon). Send 0 to disable follow-me and return + // to the default position hold mode. float SystemId = 0; - // altitude flag: 0: Keep current altitude, 1: keep altitude difference to target, - // 2: go to a fixed altitude above home - float AltitudeFlag = 0; - // altitude + // Altitude mode: 0: Keep current altitude, 1: keep altitude difference to target, + // 2: go to a fixed altitude above home. + float AltitudeMode = 0; + // Altitude above home. (used if mode=2) float Altitude = 0; - // TTL in seconds in which the MAV should go to the default position hold mode - // after a message rx timeout - float TtlSecondsWhich = 0; + // Time to land in which the MAV should go to the default position hold mode after + // a message RX timeout. + float TimeToLand = 0; protected: virtual void pack(); virtual void unpack(); @@ -5686,19 +7375,47 @@ class MavCmdDoFollowReposition : public MavLinkCommand { const static uint16_t kCommandId = 33; MavCmdDoFollowReposition() { command = kCommandId; } // Camera q1 (where 0 is on the ray from the camera to the tracking device) - float CameraQ1 = 0; + float CameraQp1 = 0; // Camera q2 - float CameraQ2 = 0; + float CameraQp2 = 0; // Camera q3 - float CameraQ3 = 0; + float CameraQp3 = 0; // Camera q4 - float CameraQ4 = 0; - // altitude offset from target (m) - float AltitudeOffsetTarget = 0; - // X offset from target (m) - float XOffsetTarget = 0; - // Y offset from target (m) - float YOffsetTarget = 0; + float CameraQp4 = 0; + // altitude offset from target + float AltitudeOffset = 0; + // X offset from target + float XOffset = 0; + // Y offset from target + float YOffset = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Start orbiting on the circumference of a circle defined by the parameters. Setting +// any value NaN results in using defaults. +class MavCmdDoOrbit : public MavLinkCommand { +public: + const static uint16_t kCommandId = 34; + MavCmdDoOrbit() { command = kCommandId; } + // Radius of the circle. positive: Orbit clockwise. negative: Orbit counter-clockwise. + float Radius = 0; + // Tangential Velocity. NaN: Vehicle configuration default. + float Velocity = 0; + // Yaw behavior of the vehicle. + float YawBehavior = 0; + // Center point latitude (if no MAV_FRAME specified) / X coordinate according + // to MAV_FRAME. NaN: Use current vehicle position or current center if already + // orbiting. + float CenterPointLatitude = 0; + // Center point longitude (if no MAV_FRAME specified) / Y coordinate according + // to MAV_FRAME. NaN: Use current vehicle position or current center if already + // orbiting. + float CenterPointLongitude = 0; + // Center point altitude (MSL) (if no MAV_FRAME specified) / Z coordinate according + // to MAV_FRAME. NaN: Use current vehicle position or current center if already + // orbiting. + float CenterPointAltitude = 0; protected: virtual void pack(); virtual void unpack(); @@ -5710,14 +7427,14 @@ class MavCmdNavRoi : public MavLinkCommand { public: const static uint16_t kCommandId = 80; MavCmdNavRoi() { command = kCommandId; } - // Region of intereset mode. (see MAV_ROI enum) - float RegionOfIntereset = 0; - // MISSION index/ target ID. (see MAV_ROI enum) - float MissionIndexTarget = 0; + // Region of interest mode. + float RoiMode = 0; + // Waypoint index/ target ID. (see MAV_ROI enum) + float WpIndex = 0; // ROI index (allows a vehicle to manage multiple ROI's) float RoiIndex = 0; // x the location of the fixed ROI (see MAV_FRAME) - float XLocationOf = 0; + float X = 0; // y float Y = 0; // z @@ -5733,48 +7450,50 @@ class MavCmdNavPathplanning : public MavLinkCommand { MavCmdNavPathplanning() { command = kCommandId; } // 0: Disable local obstacle avoidance / local path planning (without resetting // map), 1: Enable local path planning, 2: Enable and reset local path planning - float p0 = 0; + float LocalCtrl = 0; // 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable // and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy // grid - float p02 = 0; - // Yaw angle at goal, in compass degrees, [0..360] - float YawAngleAt = 0; + float GlobalCtrl = 0; + // Yaw angle at goal + float Yaw = 0; // Latitude/X of goal - float LatitudexOfGoal = 0; + float LatitudepxOfGoal = 0; // Longitude/Y of goal - float LongitudeyOfGoal = 0; + float LongitudepyOfGoal = 0; // Altitude/Z of goal - float AltitudezOfGoal = 0; + float AltitudepzOfGoal = 0; protected: virtual void pack(); virtual void unpack(); }; -// Navigate to MISSION using a spline path. +// Navigate to waypoint using a spline path. class MavCmdNavSplineWaypoint : public MavLinkCommand { public: const static uint16_t kCommandId = 82; MavCmdNavSplineWaypoint() { command = kCommandId; } - // Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION - // for rotary wing) - float HoldTimeDecimal = 0; + // Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) + float Hold = 0; // Latitude/X of goal - float LatitudexOfGoal = 0; + float LatitudepxOfGoal = 0; // Longitude/Y of goal - float LongitudeyOfGoal = 0; + float LongitudepyOfGoal = 0; // Altitude/Z of goal - float AltitudezOfGoal = 0; + float AltitudepzOfGoal = 0; protected: virtual void pack(); virtual void unpack(); }; -// Takeoff from ground using VTOL mode +// Takeoff from ground using VTOL mode, and transition to forward flight with specified +// heading. class MavCmdNavVtolTakeoff : public MavLinkCommand { public: const static uint16_t kCommandId = 84; MavCmdNavVtolTakeoff() { command = kCommandId; } - // Yaw angle in degrees - float YawAngleDegrees = 0; + // Front transition heading. + float TransitionHeading = 0; + // Yaw angle. NaN for unchanged. + float YawAngle = 0; // Latitude float Latitude = 0; // Longitude @@ -5790,14 +7509,16 @@ class MavCmdNavVtolLand : public MavLinkCommand { public: const static uint16_t kCommandId = 85; MavCmdNavVtolLand() { command = kCommandId; } - // Yaw angle in degrees - float YawAngleDegrees = 0; + // Approach altitude (with the same reference as the Altitude field). NaN if unspecified. + float Altitude = 0; + // Yaw angle. NaN for unchanged. + float Yaw = 0; // Latitude float Latitude = 0; // Longitude float Longitude = 0; - // Altitude - float Altitude = 0; + // Altitude (ground level) + float Altitude2 = 0; protected: virtual void pack(); virtual void unpack(); @@ -5808,7 +7529,7 @@ class MavCmdNavGuidedEnable : public MavLinkCommand { const static uint16_t kCommandId = 92; MavCmdNavGuidedEnable() { command = kCommandId; } // On / Off (> 0.5f on) - float OnOff = 0; + float Enable = 0; protected: virtual void pack(); virtual void unpack(); @@ -5818,8 +7539,8 @@ class MavCmdNavDelay : public MavLinkCommand { public: const static uint16_t kCommandId = 93; MavCmdNavDelay() { command = kCommandId; } - // Delay in seconds (decimal, -1 to enable time-of-day fields) - float DelaySeconds = 0; + // Delay (-1 to enable time-of-day fields) + float Delay = 0; // hour (24h format, UTC, -1 to ignore) float Hour = 0; // minute (24h format, UTC, -1 to ignore) @@ -5830,6 +7551,26 @@ class MavCmdNavDelay : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; +// Descend and place payload. Vehicle moves to specified location, descends until +// it detects a hanging payload has reached the ground, and then releases the payload. +// If ground is not detected before the reaching the maximum descent value (param1), +// the command will complete without releasing the payload. +class MavCmdNavPayloadPlace : public MavLinkCommand { +public: + const static uint16_t kCommandId = 94; + MavCmdNavPayloadPlace() { command = kCommandId; } + // Maximum distance to descend. + float MaxDescent = 0; + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; + // Altitude + float Altitude = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; // NOP - This command is only used to mark the upper limit of the NAV/ACTION commands // in the enumeration class MavCmdNavLast : public MavLinkCommand { @@ -5845,8 +7586,8 @@ class MavCmdConditionDelay : public MavLinkCommand { public: const static uint16_t kCommandId = 112; MavCmdConditionDelay() { command = kCommandId; } - // Delay in seconds (decimal) - float DelaySeconds = 0; + // Delay + float Delay = 0; protected: virtual void pack(); virtual void unpack(); @@ -5856,8 +7597,8 @@ class MavCmdConditionChangeAlt : public MavLinkCommand { public: const static uint16_t kCommandId = 113; MavCmdConditionChangeAlt() { command = kCommandId; } - // Descent / Ascend rate (m/s) - float DescentAscend = 0; + // Descent / Ascend rate. + float Rate = 0; // Finish Altitude float FinishAltitude = 0; protected: @@ -5869,7 +7610,7 @@ class MavCmdConditionDistance : public MavLinkCommand { public: const static uint16_t kCommandId = 114; MavCmdConditionDistance() { command = kCommandId; } - // Distance (meters) + // Distance. float Distance = 0; protected: virtual void pack(); @@ -5880,14 +7621,14 @@ class MavCmdConditionYaw : public MavLinkCommand { public: const static uint16_t kCommandId = 115; MavCmdConditionYaw() { command = kCommandId; } - // target angle: [0-360], 0 is north - float TargetAngle = 0; - // speed during yaw change:[deg per second] - float SpeedDuringYaw = 0; - // direction: negative: counter clockwise, positive: clockwise [-1,1] + // target angle, 0 is north + float Angle = 0; + // angular speed + float AngularSpeed = 0; + // direction: -1: counter clockwise, 1: clockwise float Direction = 0; - // relative offset or absolute angle: [ 1,0] - float RelativeOffsetOr = 0; + // 0: absolute angle, 1: relative offset + float Relative = 0; protected: virtual void pack(); virtual void unpack(); @@ -5907,14 +7648,14 @@ class MavCmdDoSetMode : public MavLinkCommand { public: const static uint16_t kCommandId = 176; MavCmdDoSetMode() { command = kCommandId; } - // Mode, as defined by ENUM MAV_MODE + // Mode float Mode = 0; // Custom mode - this is system specific, please refer to the individual autopilot // specifications for details. float CustomMode = 0; // Custom sub mode - this is system specific, please refer to the individual autopilot // specifications for details. - float CustomSubMode = 0; + float CustomSubmode = 0; protected: virtual void pack(); virtual void unpack(); @@ -5926,9 +7667,9 @@ class MavCmdDoJump : public MavLinkCommand { const static uint16_t kCommandId = 177; MavCmdDoJump() { command = kCommandId; } // Sequence number - float SequenceNumber = 0; + float Number = 0; // Repeat count - float RepeatCount = 0; + float Repeat = 0; protected: virtual void pack(); virtual void unpack(); @@ -5938,14 +7679,14 @@ class MavCmdDoChangeSpeed : public MavLinkCommand { public: const static uint16_t kCommandId = 178; MavCmdDoChangeSpeed() { command = kCommandId; } - // Speed type (0=Airspeed, 1=Ground Speed) + // Speed type (0=Airspeed, 1=Ground Speed, 2=Climb Speed, 3=Descent Speed) float SpeedType = 0; - // Speed (m/s, -1 indicates no change) + // Speed (-1 indicates no change) float Speed = 0; - // Throttle ( Percent, -1 indicates no change) + // Throttle (-1 indicates no change) float Throttle = 0; - // absolute or relative [0,1] - float AbsoluteOrRelative = 0; + // 0: absolute, 1: relative + float Relative = 0; protected: virtual void pack(); virtual void unpack(); @@ -5974,9 +7715,9 @@ class MavCmdDoSetParameter : public MavLinkCommand { const static uint16_t kCommandId = 180; MavCmdDoSetParameter() { command = kCommandId; } // Parameter number - float ParameterNumber = 0; + float Number = 0; // Parameter value - float ParameterValue = 0; + float Value = 0; protected: virtual void pack(); virtual void unpack(); @@ -5986,25 +7727,25 @@ class MavCmdDoSetRelay : public MavLinkCommand { public: const static uint16_t kCommandId = 181; MavCmdDoSetRelay() { command = kCommandId; } - // Relay number - float RelayNumber = 0; - // Setting (1=on, 0=off, others possible depending on system hardware) + // Relay instance number. + float Instance = 0; + // Setting. (1=on, 0=off, others possible depending on system hardware) float Setting = 0; protected: virtual void pack(); virtual void unpack(); }; -// Cycle a relay on and off for a desired number of cyles with a desired period. +// Cycle a relay on and off for a desired number of cycles with a desired period. class MavCmdDoRepeatRelay : public MavLinkCommand { public: const static uint16_t kCommandId = 182; MavCmdDoRepeatRelay() { command = kCommandId; } - // Relay number - float RelayNumber = 0; - // Cycle count - float CycleCount = 0; - // Cycle time (seconds, decimal) - float CycleTime = 0; + // Relay instance number. + float Instance = 0; + // Cycle count. + float Count = 0; + // Cycle time. + float Time = 0; protected: virtual void pack(); virtual void unpack(); @@ -6014,9 +7755,9 @@ class MavCmdDoSetServo : public MavLinkCommand { public: const static uint16_t kCommandId = 183; MavCmdDoSetServo() { command = kCommandId; } - // Servo number - float ServoNumber = 0; - // PWM (microseconds, 1000 to 2000 typical) + // Servo instance number. + float Instance = 0; + // Pulse Width Modulation. float Pwm = 0; protected: virtual void pack(); @@ -6028,14 +7769,14 @@ class MavCmdDoRepeatServo : public MavLinkCommand { public: const static uint16_t kCommandId = 184; MavCmdDoRepeatServo() { command = kCommandId; } - // Servo number - float ServoNumber = 0; - // PWM (microseconds, 1000 to 2000 typical) + // Servo instance number. + float Instance = 0; + // Pulse Width Modulation. float Pwm = 0; - // Cycle count - float CycleCount = 0; - // Cycle time (seconds) - float CycleTime = 0; + // Cycle count. + float Count = 0; + // Cycle time. + float Time = 0; protected: virtual void pack(); virtual void unpack(); @@ -6046,7 +7787,7 @@ class MavCmdDoFlighttermination : public MavLinkCommand { const static uint16_t kCommandId = 185; MavCmdDoFlighttermination() { command = kCommandId; } // Flight termination activated if > 0.5 - float FlightTerminationActivated = 0; + float Terminate = 0; protected: virtual void pack(); virtual void unpack(); @@ -6056,10 +7797,10 @@ class MavCmdDoChangeAltitude : public MavLinkCommand { public: const static uint16_t kCommandId = 186; MavCmdDoChangeAltitude() { command = kCommandId; } - // Altitude in meters - float AltitudeMeters = 0; - // Mav frame of new altitude (see MAV_FRAME) - float MavFrameOf = 0; + // Altitude. + float Altitude = 0; + // Frame of new altitude. + float Frame = 0; protected: virtual void pack(); virtual void unpack(); @@ -6068,7 +7809,7 @@ class MavCmdDoChangeAltitude : public MavLinkCommand { // tell the autopilot where a sequence of mission items that represents a landing // starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case // the nearest (geographically) landing sequence in the mission will be used. The -// Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified +// Latitude/Longitude is optional, and may be set to 0 if not needed. If specified // then it will be used to help find the closest landing sequence. class MavCmdDoLandStart : public MavLinkCommand { public: @@ -6087,20 +7828,20 @@ class MavCmdDoRallyLand : public MavLinkCommand { public: const static uint16_t kCommandId = 190; MavCmdDoRallyLand() { command = kCommandId; } - // Break altitude (meters) - float BreakAltitude = 0; - // Landing speed (m/s) - float LandingSpeed = 0; + // Break altitude + float Altitude = 0; + // Landing speed + float Speed = 0; protected: virtual void pack(); virtual void unpack(); }; -// Mission command to safely abort an autonmous landing. +// Mission command to safely abort an autonomous landing. class MavCmdDoGoAround : public MavLinkCommand { public: const static uint16_t kCommandId = 191; MavCmdDoGoAround() { command = kCommandId; } - // Altitude (meters) + // Altitude float Altitude = 0; protected: virtual void pack(); @@ -6112,15 +7853,15 @@ class MavCmdDoReposition : public MavLinkCommand { const static uint16_t kCommandId = 192; MavCmdDoReposition() { command = kCommandId; } // Ground speed, less than 0 (-1) for default - float GroundSpeed = 0; - // Bitmask of option flags, see the MAV_DO_REPOSITION_FLAGS enum. - float BitmaskOfOption = 0; + float Speed = 0; + // Bitmask of option flags. + float Bitmask = 0; // Yaw heading, NaN for unchanged. For planes indicates loiter direction (0: clockwise, // 1: counter clockwise) - float YawHeading = 0; - // Latitude (deg * 1E7) + float Yaw = 0; + // Latitude float Latitude = 0; - // Longitude (deg * 1E7) + // Longitude float Longitude = 0; // Altitude (meters) float Altitude = 0; @@ -6136,7 +7877,7 @@ class MavCmdDoPauseContinue : public MavLinkCommand { // 0: Pause current mission or reposition command, hold current position. 1: Continue // mission. A VTOL capable vehicle should enter hover mode (multicopter and VTOL // planes). A plane should loiter with the default loiter radius. - float p0 = 0; + float Continue = 0; protected: virtual void pack(); virtual void unpack(); @@ -6147,7 +7888,52 @@ class MavCmdDoSetReverse : public MavLinkCommand { const static uint16_t kCommandId = 194; MavCmdDoSetReverse() { command = kCommandId; } // Direction (0=Forward, 1=Reverse) - float Direction = 0; + float Reverse = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Sets the region of interest (ROI) to a location. This can then be used by the vehicles +// control system to control the vehicle attitude and the attitude of various sensors +// such as cameras. +class MavCmdDoSetRoiLocation : public MavLinkCommand { +public: + const static uint16_t kCommandId = 195; + MavCmdDoSetRoiLocation() { command = kCommandId; } + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; + // Altitude + float Altitude = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw +// offset. This can then be used by the vehicles control system to control the vehicle +// attitude and the attitude of various sensors such as cameras. +class MavCmdDoSetRoiWpnextOffset : public MavLinkCommand { +public: + const static uint16_t kCommandId = 196; + MavCmdDoSetRoiWpnextOffset() { command = kCommandId; } + // pitch offset from next waypoint + float PitchOffset = 0; + // roll offset from next waypoint + float RollOffset = 0; + // yaw offset from next waypoint + float YawOffset = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Cancels any previous ROI command returning the vehicle/sensors to default flight +// characteristics. This can then be used by the vehicles control system to control +// the vehicle attitude and the attitude of various sensors such as cameras. +class MavCmdDoSetRoiNone : public MavLinkCommand { +public: + const static uint16_t kCommandId = 197; + MavCmdDoSetRoiNone() { command = kCommandId; } protected: virtual void pack(); virtual void unpack(); @@ -6158,11 +7944,11 @@ class MavCmdDoControlVideo : public MavLinkCommand { const static uint16_t kCommandId = 200; MavCmdDoControlVideo() { command = kCommandId; } // Camera ID (-1 for all) - float CameraId = 0; + float Id = 0; // Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw float Transmission = 0; - // Transmission mode: 0: video stream, >0: single images every n seconds (decimal) - float TransmissionMode = 0; + // Transmission mode: 0: video stream, >0: single images every n seconds + float Interval = 0; // Recording: 0: disabled, 1: enabled compressed, 2: enabled raw float Recording = 0; protected: @@ -6176,62 +7962,69 @@ class MavCmdDoSetRoi : public MavLinkCommand { public: const static uint16_t kCommandId = 201; MavCmdDoSetRoi() { command = kCommandId; } - // Region of intereset mode. (see MAV_ROI enum) - float RegionOfIntereset = 0; - // MISSION index/ target ID. (see MAV_ROI enum) - float MissionIndexTarget = 0; - // ROI index (allows a vehicle to manage multiple ROI's) + // Region of interest mode. + float RoiMode = 0; + // Waypoint index/ target ID (depends on param 1). + float WpIndex = 0; + // Region of interest index. (allows a vehicle to manage multiple ROI's) float RoiIndex = 0; - // x the location of the fixed ROI (see MAV_FRAME) - float XLocationOf = 0; - // y - float Y = 0; - // z - float Z = 0; + // MAV_ROI_WPNEXT: pitch offset from next waypoint, MAV_ROI_LOCATION: latitude + float MavRoiWpnext = 0; + // MAV_ROI_WPNEXT: roll offset from next waypoint, MAV_ROI_LOCATION: longitude + float MavRoiWpnext2 = 0; + // MAV_ROI_WPNEXT: yaw offset from next waypoint, MAV_ROI_LOCATION: altitude + float MavRoiWpnext3 = 0; protected: virtual void pack(); virtual void unpack(); }; -// Mission command to configure an on-board camera controller system. +// Configure digital camera. This is a fallback message for systems that have not +// yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html +// ). class MavCmdDoDigicamConfigure : public MavLinkCommand { public: const static uint16_t kCommandId = 202; MavCmdDoDigicamConfigure() { command = kCommandId; } - // Modes: P, TV, AV, M, Etc - float Modes = 0; - // Shutter speed: Divisor number for one second + // Modes: P, TV, AV, M, Etc. + float Mode = 0; + // Shutter speed: Divisor number for one second. float ShutterSpeed = 0; - // Aperture: F stop number + // Aperture: F stop number. float Aperture = 0; - // ISO number e.g. 80, 100, 200, Etc - float IsoNumberE = 0; - // Exposure type enumerator - float ExposureTypeEnumerator = 0; - // Command Identity + // ISO number e.g. 80, 100, 200, Etc. + float Iso = 0; + // Exposure type enumerator. + float Exposure = 0; + // Command Identity. float CommandIdentity = 0; - // Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - float MainEngineCut = 0; + // Main engine cut-off time before camera trigger. (0 means no cut-off) + float EngineCutpoff = 0; protected: virtual void pack(); virtual void unpack(); }; -// Mission command to control an on-board camera controller system. +// Control digital camera. This is a fallback message for systems that have not yet +// implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html +// ). class MavCmdDoDigicamControl : public MavLinkCommand { public: const static uint16_t kCommandId = 203; MavCmdDoDigicamControl() { command = kCommandId; } // Session control e.g. show/hide lens - float SessionControlE = 0; + float SessionControl = 0; // Zoom's absolute position - float ZoomsAbsolutePosition = 0; + float ZoomAbsolute = 0; // Zooming step value to offset zoom from the current position - float ZoomingStepValue = 0; + float ZoomRelative = 0; // Focus Locking, Unlocking or Re-locking - float FocusLocking = 0; + float Focus = 0; // Shooting Command - float ShootingCommand = 0; + float ShootCommand = 0; // Command Identity float CommandIdentity = 0; + // Test shot identifier. If set to 1, image will only be captured, but not counted + // towards internal frame count. + float ShotId = 0; protected: virtual void pack(); virtual void unpack(); @@ -6241,14 +8034,20 @@ class MavCmdDoMountConfigure : public MavLinkCommand { public: const static uint16_t kCommandId = 204; MavCmdDoMountConfigure() { command = kCommandId; } - // Mount operation mode (see MAV_MOUNT_MODE enum) - float MountOperationMode = 0; + // Mount operation mode + float Mode = 0; // stabilize roll? (1 = yes, 0 = no) float StabilizeRoll = 0; // stabilize pitch? (1 = yes, 0 = no) float StabilizePitch = 0; // stabilize yaw? (1 = yes, 0 = no) float StabilizeYaw = 0; + // roll input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame) + float RollInput = 0; + // pitch input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame) + float PitchInput = 0; + // yaw input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame) + float YawInput = 0; protected: virtual void pack(); virtual void unpack(); @@ -6258,31 +8057,38 @@ class MavCmdDoMountControl : public MavLinkCommand { public: const static uint16_t kCommandId = 205; MavCmdDoMountControl() { command = kCommandId; } - // pitch (WIP: DEPRECATED: or lat in degrees) depending on mount mode. - float Pitch = 0; - // roll (WIP: DEPRECATED: or lon in degrees) depending on mount mode. - float Roll = 0; - // yaw (WIP: DEPRECATED: or alt in meters) depending on mount mode. - float Yaw = 0; - // WIP: alt in meters depending on mount mode. - float Wip = 0; - // WIP: latitude in degrees * 1E7, set if appropriate mount mode. - float Wip2 = 0; - // WIP: longitude in degrees * 1E7, set if appropriate mount mode. - float Wip3 = 0; - // MAV_MOUNT_MODE enum value - float MavMountModeEnumValue = 0; + // pitch depending on mount mode (degrees or degrees/second depending on pitch + // input). + float PitchDependingMount = 0; + // roll depending on mount mode (degrees or degrees/second depending on roll input). + float RollDependingMount = 0; + // yaw depending on mount mode (degrees or degrees/second depending on yaw input). + float YawDependingMount = 0; + // altitude depending on mount mode. + float Altitude = 0; + // latitude, set if appropriate mount mode. + float Latitude = 0; + // longitude, set if appropriate mount mode. + float Longitude = 0; + // Mount mode. + float Mode = 0; protected: virtual void pack(); virtual void unpack(); }; -// Mission command to set CAM_TRIGG_DIST for this flight +// Mission command to set camera trigger distance for this flight. The camera is triggered +// each time this distance is exceeded. This command can also be used to set the shutter +// integration time for the camera. class MavCmdDoSetCamTriggDist : public MavLinkCommand { public: const static uint16_t kCommandId = 206; MavCmdDoSetCamTriggDist() { command = kCommandId; } - // Camera trigger distance (meters) - float CameraTriggerDistance = 0; + // Camera trigger distance. 0 to stop triggering. + float Distance = 0; + // Camera shutter integration time. -1 or 0 to ignore + float Shutter = 0; + // Trigger camera once immediately. (0 = no trigger, 1 = trigger) + float Trigger = 0; protected: virtual void pack(); virtual void unpack(); @@ -6303,51 +8109,72 @@ class MavCmdDoParachute : public MavLinkCommand { public: const static uint16_t kCommandId = 208; MavCmdDoParachute() { command = kCommandId; } - // action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION - // enum, not in general message set.) + // action float Action = 0; protected: virtual void pack(); virtual void unpack(); }; -// Mission command to perform motor test +// Mission command to perform motor test. class MavCmdDoMotorTest : public MavLinkCommand { public: const static uint16_t kCommandId = 209; MavCmdDoMotorTest() { command = kCommandId; } - // motor sequence number (a number from 1 to max number of motors on the vehicle) - float MotorSequenceNumber = 0; - // throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. - // See MOTOR_TEST_THROTTLE_TYPE enum) + // Motor instance number. (from 1 to max number of motors on the vehicle) + float Instance = 0; + // Throttle type. float ThrottleType = 0; - // throttle + // Throttle. float Throttle = 0; - // timeout (in seconds) + // Timeout. float Timeout = 0; + // Motor count. (number of motors to test to test in sequence, waiting for the + // timeout above between them; 0=1 motor, 1=1 motor, 2=2 motors...) + float MotorCount = 0; + // Motor test order. + float TestOrder = 0; protected: virtual void pack(); virtual void unpack(); }; -// Change to/from inverted flight +// Change to/from inverted flight. class MavCmdDoInvertedFlight : public MavLinkCommand { public: const static uint16_t kCommandId = 210; MavCmdDoInvertedFlight() { command = kCommandId; } - // inverted (0=normal, 1=inverted) + // Inverted flight. (0=normal, 1=inverted) float Inverted = 0; protected: virtual void pack(); virtual void unpack(); }; -// Sets a desired vehicle turn angle and thrust change -class MavCmdDoSetPositionYawThrust : public MavLinkCommand { +// Sets a desired vehicle turn angle and speed change. +class MavCmdNavSetYawSpeed : public MavLinkCommand { public: const static uint16_t kCommandId = 213; - MavCmdDoSetPositionYawThrust() { command = kCommandId; } - // yaw angle to adjust steering by in centidegress - float YawAngleTo = 0; - // Thrust - normalized to -2 .. 2 - float Thrust = 0; + MavCmdNavSetYawSpeed() { command = kCommandId; } + // Yaw angle to adjust steering by. + float Yaw = 0; + // Speed. + float Speed = 0; + // Final angle. (0=absolute, 1=relative) + float Angle = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Mission command to set camera trigger interval for this flight. If triggering is +// enabled, the camera is triggered each time this interval expires. This command +// can also be used to set the shutter integration time for the camera. +class MavCmdDoSetCamTriggInterval : public MavLinkCommand { +public: + const static uint16_t kCommandId = 214; + MavCmdDoSetCamTriggInterval() { command = kCommandId; } + // Camera trigger cycle time. -1 or 0 to ignore. + float TriggerCycle = 0; + // Camera shutter integration time. Should be less than trigger cycle time. -1 + // or 0 to ignore. + float ShutterIntegration = 0; protected: virtual void pack(); virtual void unpack(); @@ -6357,14 +8184,14 @@ class MavCmdDoMountControlQuat : public MavLinkCommand { public: const static uint16_t kCommandId = 220; MavCmdDoMountControlQuat() { command = kCommandId; } - // q1 - quaternion param #1, w (1 in null-rotation) - float Q1 = 0; - // q2 - quaternion param #2, x (0 in null-rotation) - float Q2 = 0; - // q3 - quaternion param #3, y (0 in null-rotation) - float Q3 = 0; - // q4 - quaternion param #4, z (0 in null-rotation) - float Q4 = 0; + // quaternion param q1, w (1 in null-rotation) + float Qp1 = 0; + // quaternion param q2, x (0 in null-rotation) + float Qp2 = 0; + // quaternion param q3, y (0 in null-rotation) + float Qp3 = 0; + // quaternion param q4, z (0 in null-rotation) + float Qp4 = 0; protected: virtual void pack(); virtual void unpack(); @@ -6382,25 +8209,24 @@ class MavCmdDoGuidedMaster : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; -// set limits for external control +// Set limits for external control class MavCmdDoGuidedLimits : public MavLinkCommand { public: const static uint16_t kCommandId = 222; MavCmdDoGuidedLimits() { command = kCommandId; } - // timeout - maximum time (in seconds) that external controller will be allowed - // to control vehicle. 0 means no timeout + // Timeout - maximum time that external controller will be allowed to control + // vehicle. 0 means no timeout. float Timeout = 0; - // absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, - // the command will be aborted and the mission will continue. 0 means no lower - // altitude limit - float AbsoluteAltitudeMin = 0; - // absolute altitude max (in meters)- if vehicle moves above this alt, the command - // will be aborted and the mission will continue. 0 means no upper altitude limit - float AbsoluteAltitudeMax = 0; - // horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance - // from it's location at the moment the command was executed, the command will - // be aborted and the mission will continue. 0 means no horizontal altitude limit - float HorizontalMoveLimit = 0; + // Altitude (MSL) min - if vehicle moves below this alt, the command will be aborted + // and the mission will continue. 0 means no lower altitude limit. + float MinAltitude = 0; + // Altitude (MSL) max - if vehicle moves above this alt, the command will be aborted + // and the mission will continue. 0 means no upper altitude limit. + float MaxAltitude = 0; + // Horizontal move limit - if vehicle moves more than this distance from its location + // at the moment the command was executed, the command will be aborted and the + // mission will continue. 0 means no horizontal move limit. + float HorizpMoveLimit = 0; protected: virtual void pack(); virtual void unpack(); @@ -6413,17 +8239,30 @@ class MavCmdDoEngineControl : public MavLinkCommand { const static uint16_t kCommandId = 223; MavCmdDoEngineControl() { command = kCommandId; } // 0: Stop engine, 1:Start Engine - float p0 = 0; + float StartEngine = 0; // 0: Warm start, 1:Cold start. Controls use of choke where applicable - float p02 = 0; - // Height delay (meters). This is for commanding engine start only after the vehicle - // has gained the specified height. Used in VTOL vehicles during takeoff to start + float ColdStart = 0; + // Height delay. This is for commanding engine start only after the vehicle has + // gained the specified height. Used in VTOL vehicles during takeoff to start // engine after the aircraft is off the ground. Zero for no delay. float HeightDelay = 0; protected: virtual void pack(); virtual void unpack(); }; +// Set the mission item with sequence number seq as current item. This means that +// the MAV will continue to this mission item on the shortest path (not following +// the mission items in-between). +class MavCmdDoSetMissionCurrent : public MavLinkCommand { +public: + const static uint16_t kCommandId = 224; + MavCmdDoSetMissionCurrent() { command = kCommandId; } + // Mission sequence value to set + float Number = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; // NOP - This command is only used to mark the upper limit of the DO commands in the // enumeration class MavCmdDoLast : public MavLinkCommand { @@ -6435,22 +8274,28 @@ class MavCmdDoLast : public MavLinkCommand { virtual void unpack(); }; // Trigger calibration. This command will be only accepted if in pre-flight mode. +// Except for Temperature Calibration, only one sensor should be set in a single message +// and all others should be zero. class MavCmdPreflightCalibration : public MavLinkCommand { public: const static uint16_t kCommandId = 241; MavCmdPreflightCalibration() { command = kCommandId; } - // Gyro calibration: 0: no, 1: yes - float GyroCalibration = 0; - // Magnetometer calibration: 0: no, 1: yes - float MagnetometerCalibration = 0; - // Ground pressure: 0: no, 1: yes + // 1: gyro calibration, 3: gyro temperature calibration + float GyroTemperature = 0; + // 1: magnetometer calibration + float Magnetometer = 0; + // 1: ground pressure calibration float GroundPressure = 0; - // Radio calibration: 0: no, 1: yes - float RadioCalibration = 0; - // Accelerometer calibration: 0: no, 1: yes - float AccelerometerCalibration = 0; - // Compass/Motor interference calibration: 0: no, 1: yes - float CompassmotorInterferenceCalibration = 0; + // 1: radio RC calibration, 2: RC trim calibration + float RemoteControl = 0; + // 1: accelerometer calibration, 2: board level calibration, 3: accelerometer + // temperature calibration, 4: simple accelerometer calibration + float Accelerometer = 0; + // 1: APM: compass/motor interference calibration (PX4: airspeed calibration, + // deprecated), 2: airspeed calibration + float CompmotOrAirspeed = 0; + // 1: ESC calibration, 3: barometer temperature calibration + float EscOrBaro = 0; protected: virtual void pack(); virtual void unpack(); @@ -6462,19 +8307,19 @@ class MavCmdPreflightSetSensorOffsets : public MavLinkCommand { MavCmdPreflightSetSensorOffsets() { command = kCommandId; } // Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, // 3: barometer, 4: optical flow, 5: second magnetometer, 6: third magnetometer - float SensorToAdjust = 0; + float SensorType = 0; // X axis offset (or generic dimension 1), in the sensor's raw units - float XAxisOffset = 0; + float XOffset = 0; // Y axis offset (or generic dimension 2), in the sensor's raw units - float YAxisOffset = 0; + float YOffset = 0; // Z axis offset (or generic dimension 3), in the sensor's raw units - float ZAxisOffset = 0; + float ZOffset = 0; // Generic dimension 4, in the sensor's raw units - float GenericDimension4 = 0; + float P4thDimension = 0; // Generic dimension 5, in the sensor's raw units - float GenericDimension5 = 0; + float P5thDimension = 0; // Generic dimension 6, in the sensor's raw units - float GenericDimension6 = 0; + float P6thDimension = 0; protected: virtual void pack(); virtual void unpack(); @@ -6485,7 +8330,7 @@ class MavCmdPreflightUavcan : public MavLinkCommand { const static uint16_t kCommandId = 243; MavCmdPreflightUavcan() { command = kCommandId; } // 1: Trigger actuator ID assignment and direction mapping. - float p1 = 0; + float ActuatorId = 0; protected: virtual void pack(); virtual void unpack(); @@ -6503,9 +8348,8 @@ class MavCmdPreflightStorage : public MavLinkCommand { // 2: Reset to defaults float MissionStorage = 0; // Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, - // > 1: start logging with rate of param 3 in Hz (e.g. set to 1000 for 1000 Hz - // logging) - float OnboardLogging = 0; + // > 1: logging rate (e.g. set to 1000 for 1000 Hz logging) + float LoggingRate = 0; protected: virtual void pack(); virtual void unpack(); @@ -6517,11 +8361,11 @@ class MavCmdPreflightRebootShutdown : public MavLinkCommand { MavCmdPreflightRebootShutdown() { command = kCommandId; } // 0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot, 3: // Reboot autopilot and keep it in the bootloader until upgraded. - float p0 = 0; + float Autopilot = 0; // 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown // onboard computer, 3: Reboot onboard computer and keep it in the bootloader // until upgraded. - float p02 = 0; + float Companion = 0; // WIP: 0: Do nothing for camera, 1: Reboot onboard camera, 2: Shutdown onboard // camera, 3: Reboot onboard camera and keep it in the bootloader until upgraded float Wip = 0; @@ -6534,27 +8378,30 @@ class MavCmdPreflightRebootShutdown : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; -// Hold / continue the current action +// Override current mission with command to pause mission, pause mission and move +// to position, continue/resume mission. When param 1 indicates that the mission is +// paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to +// another position. class MavCmdOverrideGoto : public MavLinkCommand { public: const static uint16_t kCommandId = 252; MavCmdOverrideGoto() { command = kCommandId; } - // MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission - // plan - float MavGotoDoHold = 0; - // MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: - // hold at specified position - float MavGotoHoldAtCurrentPosition = 0; - // MAV_FRAME coordinate frame of hold point - float MavFrameCoordinateFrame = 0; - // Desired yaw angle in degrees - float DesiredYawAngle = 0; - // Latitude / X position - float LatitudeX = 0; - // Longitude / Y position - float LongitudeY = 0; - // Altitude / Z position - float AltitudeZ = 0; + // MAV_GOTO_DO_HOLD: pause mission and either hold or move to specified position + // (depending on param2), MAV_GOTO_DO_CONTINUE: resume mission. + float Continue = 0; + // MAV_GOTO_HOLD_AT_CURRENT_POSITION: hold at current position, MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: + // hold at specified position. + float Position = 0; + // Coordinate frame of hold point. + float Frame = 0; + // Desired yaw angle. + float Yaw = 0; + // Latitude / X position. + float LatitudePX = 0; + // Longitude / Y position. + float LongitudePY = 0; + // Altitude / Z position. + float AltitudePZ = 0; protected: virtual void pack(); virtual void unpack(); @@ -6578,8 +8425,25 @@ class MavCmdComponentArmDisarm : public MavLinkCommand { public: const static uint16_t kCommandId = 400; MavCmdComponentArmDisarm() { command = kCommandId; } - // 1 to arm, 0 to disarm - float p1ToArm = 0; + // 0: disarm, 1: arm + float Arm = 0; + // 0: arm-disarm unless prevented by safety checks (i.e. when landed), 21196: + // force arming/disarming (e.g. allow arming to override preflight checks and + // disarming in flight) + float Force = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Turns illuminators ON/OFF. An illuminator is a light source that is used for lighting +// up dark areas external to the sytstem: e.g. a torch or searchlight (as opposed +// to a light source for illuminating the system itself, e.g. an indicator light). +class MavCmdIlluminatorOnOff : public MavLinkCommand { +public: + const static uint16_t kCommandId = 405; + MavCmdIlluminatorOnOff() { command = kCommandId; } + // 0: Illuminators OFF, 1: Illuminators ON + float Enable = 0; protected: virtual void pack(); virtual void unpack(); @@ -6593,205 +8457,289 @@ class MavCmdGetHomePosition : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; -// Starts receiver pairing +// Starts receiver pairing. class MavCmdStartRxPair : public MavLinkCommand { public: const static uint16_t kCommandId = 500; MavCmdStartRxPair() { command = kCommandId; } - // 0:Spektrum - float p0 = 0; - // 0:Spektrum DSM2, 1:Spektrum DSMX - float p02 = 0; + // 0:Spektrum. + float Spektrum = 0; + // RC type. + float RcType = 0; protected: virtual void pack(); virtual void unpack(); }; -// Request the interval between messages for a particular MAVLink message ID +// Request the interval between messages for a particular MAVLink message ID. The +// receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL +// message. class MavCmdGetMessageInterval : public MavLinkCommand { public: const static uint16_t kCommandId = 510; MavCmdGetMessageInterval() { command = kCommandId; } // The MAVLink message ID - float TheMavlinkMessage = 0; + float MessageId = 0; protected: virtual void pack(); virtual void unpack(); }; -// Request the interval between messages for a particular MAVLink message ID. This -// interface replaces REQUEST_DATA_STREAM +// Set the interval between messages for a particular MAVLink message ID. This interface +// replaces REQUEST_DATA_STREAM. class MavCmdSetMessageInterval : public MavLinkCommand { public: const static uint16_t kCommandId = 511; MavCmdSetMessageInterval() { command = kCommandId; } // The MAVLink message ID - float TheMavlinkMessage = 0; - // The interval between two messages, in microseconds. Set to -1 to disable and - // 0 to request default rate. - float TheIntervalBetween = 0; + float MessageId = 0; + // The interval between two messages. Set to -1 to disable and 0 to request default + // rate. + float Interval = 0; + // Target address of message stream (if message has target address fields). 0: + // Flight-stack default (recommended), 1: address of requestor, 2: broadcast. + float ResponseTarget = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Request the target system(s) emit a single instance of a specified message (i.e. +// a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). +class MavCmdRequestMessage : public MavLinkCommand { +public: + const static uint16_t kCommandId = 512; + MavCmdRequestMessage() { command = kCommandId; } + // The MAVLink message ID of the requested message. + float MessageId = 0; + // Index id (if appropriate). The use of this parameter (if any), must be defined + // in the requested message. + float IndexId = 0; + // The use of this parameter (if any), must be defined in the requested message. + // By default assumed not used (0). + float TheUseOf = 0; + // The use of this parameter (if any), must be defined in the requested message. + // By default assumed not used (0). + float TheUseOf2 = 0; + // The use of this parameter (if any), must be defined in the requested message. + // By default assumed not used (0). + float TheUseOf3 = 0; + // The use of this parameter (if any), must be defined in the requested message. + // By default assumed not used (0). + float TheUseOf4 = 0; + // Target address for requested message (if message has target address fields). + // 0: Flight-stack default, 1: address of requestor, 2: broadcast. + float ResponseTarget = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Request MAVLink protocol version compatibility +class MavCmdRequestProtocolVersion : public MavLinkCommand { +public: + const static uint16_t kCommandId = 519; + MavCmdRequestProtocolVersion() { command = kCommandId; } + // 1: Request supported protocol versions by all nodes on the network + float Protocol = 0; protected: virtual void pack(); virtual void unpack(); }; -// Request autopilot capabilities +// Request autopilot capabilities. The receiver should ACK the command and then emit +// its capabilities in an AUTOPILOT_VERSION message class MavCmdRequestAutopilotCapabilities : public MavLinkCommand { public: const static uint16_t kCommandId = 520; MavCmdRequestAutopilotCapabilities() { command = kCommandId; } // 1: Request autopilot version - float p1 = 0; + float Version = 0; protected: virtual void pack(); virtual void unpack(); }; -// WIP: Request camera information (CAMERA_INFORMATION) +// Request camera information (CAMERA_INFORMATION). class MavCmdRequestCameraInformation : public MavLinkCommand { public: const static uint16_t kCommandId = 521; MavCmdRequestCameraInformation() { command = kCommandId; } - // 1: Request camera capabilities - float p1 = 0; - // Camera ID - float CameraId = 0; + // 0: No action 1: Request camera capabilities + float Capabilities = 0; protected: virtual void pack(); virtual void unpack(); }; -// WIP: Request camera settings (CAMERA_SETTINGS) +// Request camera settings (CAMERA_SETTINGS). class MavCmdRequestCameraSettings : public MavLinkCommand { public: const static uint16_t kCommandId = 522; MavCmdRequestCameraSettings() { command = kCommandId; } - // 1: Request camera settings - float p1 = 0; - // Camera ID - float CameraId = 0; + // 0: No Action 1: Request camera settings + float Settings = 0; protected: virtual void pack(); virtual void unpack(); }; -// WIP: Set the camera settings part 1 (CAMERA_SETTINGS) -class MavCmdSetCameraSettings1 : public MavLinkCommand { -public: - const static uint16_t kCommandId = 523; - MavCmdSetCameraSettings1() { command = kCommandId; } - // Camera ID - float CameraId = 0; - // Aperture (1/value) - float Aperture = 0; - // Aperture locked (0: auto, 1: locked) - float ApertureLocked = 0; - // Shutter speed in s - float ShutterSpeedS = 0; - // Shutter speed locked (0: auto, 1: locked) - float ShutterSpeedLocked = 0; - // ISO sensitivity - float IsoSensitivity = 0; - // ISO sensitivity locked (0: auto, 1: locked) - float IsoSensitivityLocked = 0; -protected: - virtual void pack(); - virtual void unpack(); -}; -// WIP: Set the camera settings part 2 (CAMERA_SETTINGS) -class MavCmdSetCameraSettings2 : public MavLinkCommand { -public: - const static uint16_t kCommandId = 524; - MavCmdSetCameraSettings2() { command = kCommandId; } - // Camera ID - float CameraId = 0; - // White balance locked (0: auto, 1: locked) - float WhiteBalanceLocked = 0; - // White balance (color temperature in K) - float WhiteBalance = 0; - // Reserved for camera mode ID - float ReservedForCamera = 0; - // Reserved for color mode ID - float ReservedForColor = 0; - // Reserved for image format ID - float ReservedForImage = 0; -protected: - virtual void pack(); - virtual void unpack(); -}; -// WIP: Request storage information (STORAGE_INFORMATION) +// Request storage information (STORAGE_INFORMATION). Use the command's target_component +// to target a specific component's storage. class MavCmdRequestStorageInformation : public MavLinkCommand { public: const static uint16_t kCommandId = 525; MavCmdRequestStorageInformation() { command = kCommandId; } - // 1: Request storage information - float p1 = 0; - // Storage ID + // Storage ID (0 for all, 1 for first, 2 for second, etc.) float StorageId = 0; + // 0: No Action 1: Request storage information + float Information = 0; protected: virtual void pack(); virtual void unpack(); }; -// WIP: Format a storage medium +// Format a storage medium. Once format is complete, a STORAGE_INFORMATION message +// is sent. Use the command's target_component to target a specific component's storage. class MavCmdStorageFormat : public MavLinkCommand { public: const static uint16_t kCommandId = 526; MavCmdStorageFormat() { command = kCommandId; } - // 1: Format storage - float p1 = 0; - // Storage ID + // Storage ID (1 for first, 2 for second, etc.) float StorageId = 0; + // 0: No action 1: Format storage + float Format = 0; protected: virtual void pack(); virtual void unpack(); }; -// WIP: Request camera capture status (CAMERA_CAPTURE_STATUS) +// Request camera capture status (CAMERA_CAPTURE_STATUS) class MavCmdRequestCameraCaptureStatus : public MavLinkCommand { public: const static uint16_t kCommandId = 527; MavCmdRequestCameraCaptureStatus() { command = kCommandId; } - // 1: Request camera capture status - float p1 = 0; - // Camera ID - float CameraId = 0; + // 0: No Action 1: Request camera capture status + float CaptureStatus = 0; protected: virtual void pack(); virtual void unpack(); }; -// WIP: Request flight information (FLIGHT_INFORMATION) +// Request flight information (FLIGHT_INFORMATION) class MavCmdRequestFlightInformation : public MavLinkCommand { public: const static uint16_t kCommandId = 528; MavCmdRequestFlightInformation() { command = kCommandId; } // 1: Request flight information - float p1 = 0; + float FlightInformation = 0; protected: virtual void pack(); virtual void unpack(); }; -// Start image capture sequence +// Reset all camera settings to Factory Default +class MavCmdResetCameraSettings : public MavLinkCommand { +public: + const static uint16_t kCommandId = 529; + MavCmdResetCameraSettings() { command = kCommandId; } + // 0: No Action 1: Reset all settings + float Reset = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS +// command after a mode change if the camera supports video streaming. +class MavCmdSetCameraMode : public MavLinkCommand { +public: + const static uint16_t kCommandId = 530; + MavCmdSetCameraMode() { command = kCommandId; } + // Camera mode + float CameraMode = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). +// Use NaN for reserved values. +class MavCmdSetCameraZoom : public MavLinkCommand { +public: + const static uint16_t kCommandId = 531; + MavCmdSetCameraZoom() { command = kCommandId; } + // Zoom type + float ZoomType = 0; + // Zoom value. The range of valid values depend on the zoom type. + float ZoomValue = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). +// Use NaN for reserved values. +class MavCmdSetCameraFocus : public MavLinkCommand { +public: + const static uint16_t kCommandId = 532; + MavCmdSetCameraFocus() { command = kCommandId; } + // Focus type + float FocusType = 0; + // Focus value + float FocusValue = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. +class MavCmdJumpTag : public MavLinkCommand { +public: + const static uint16_t kCommandId = 600; + MavCmdJumpTag() { command = kCommandId; } + // Tag. + float Tag = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Jump to the matching tag in the mission list. Repeat this action for the specified +// number of times. A mission should contain a single matching tag for each jump. +// If this is not the case then a jump to a missing tag should complete the mission, +// and a jump where there are multiple matching tags should always select the one +// with the lowest mission sequence number. +class MavCmdDoJumpTag : public MavLinkCommand { +public: + const static uint16_t kCommandId = 601; + MavCmdDoJumpTag() { command = kCommandId; } + // Target tag to jump to. + float Tag = 0; + // Repeat count. + float Repeat = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use +// NaN for reserved values. class MavCmdImageStartCapture : public MavLinkCommand { public: const static uint16_t kCommandId = 2000; MavCmdImageStartCapture() { command = kCommandId; } - // Duration between two consecutive pictures (in seconds) - float DurationBetweenTwo = 0; - // Number of images to capture total - 0 for unlimited capture - float NumberOfImages = 0; - // Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc), set to 0 - // if param 4/5 are used - float ResolutionMegapixels = 0; - // WIP: Resolution horizontal in pixels - float Wip = 0; - // WIP: Resolution horizontal in pixels - float Wip2 = 0; - // WIP: Camera ID - float Wip3 = 0; + // Desired elapsed time between two consecutive pictures (in seconds). Minimum + // values depend on hardware (typically greater than 2 seconds). + float Interval = 0; + // Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE. + float CaptureCount = 0; + // Capture sequence number starting from 1. This is only valid for single-capture + // (param3 == 1). Increment the capture ID for each capture command to prevent + // double captures when a command is re-transmitted. Use 0 to ignore it. + float SequenceNumber = 0; protected: virtual void pack(); virtual void unpack(); }; -// Stop image capture sequence +// Stop image capture sequence Use NaN for reserved values. class MavCmdImageStopCapture : public MavLinkCommand { public: const static uint16_t kCommandId = 2001; MavCmdImageStopCapture() { command = kCommandId; } - // Camera ID - float CameraId = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Re-request a CAMERA_IMAGE_CAPTURE message. Use NaN for reserved values. +class MavCmdRequestCameraImageCapture : public MavLinkCommand { +public: + const static uint16_t kCommandId = 2002; + MavCmdRequestCameraImageCapture() { command = kCommandId; } + // Sequence number for missing CAMERA_IMAGE_CAPTURE message + float Number = 0; protected: virtual void pack(); virtual void unpack(); @@ -6801,41 +8749,82 @@ class MavCmdDoTriggerControl : public MavLinkCommand { public: const static uint16_t kCommandId = 2003; MavCmdDoTriggerControl() { command = kCommandId; } - // Trigger enable/disable (0 for disable, 1 for start) - float TriggerEnabledisable = 0; - // Shutter integration time (in ms) - float ShutterIntegrationTime = 0; + // Trigger enable/disable (0 for disable, 1 for start), -1 to ignore + float Enable = 0; + // 1 to reset the trigger sequence, -1 or 0 to ignore + float Reset = 0; + // 1 to pause triggering, but without switching the camera off or retracting it. + // -1 to ignore + float Pause = 0; protected: virtual void pack(); virtual void unpack(); }; -// Starts video capture +// Starts video capture (recording). Use NaN for reserved values. class MavCmdVideoStartCapture : public MavLinkCommand { public: const static uint16_t kCommandId = 2500; MavCmdVideoStartCapture() { command = kCommandId; } - // Camera ID (0 for all cameras), 1 for first, 2 for second, etc. - float CameraId = 0; - // Frames per second - float FramesPerSecond = 0; - // Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc), set to 0 - // if param 4/5 are used - float ResolutionMegapixels = 0; - // WIP: Resolution horizontal in pixels - float Wip = 0; - // WIP: Resolution horizontal in pixels - float Wip2 = 0; + // Video Stream ID (0 for all streams) + float StreamId = 0; + // Frequency CAMERA_CAPTURE_STATUS messages should be sent while recording (0 + // for no messages, otherwise frequency) + float StatusFrequency = 0; protected: virtual void pack(); virtual void unpack(); }; -// Stop the current video capture +// Stop the current video capture (recording). Use NaN for reserved values. class MavCmdVideoStopCapture : public MavLinkCommand { public: const static uint16_t kCommandId = 2501; MavCmdVideoStopCapture() { command = kCommandId; } - // WIP: Camera ID - float Wip = 0; + // Video Stream ID (0 for all streams) + float StreamId = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Start video streaming +class MavCmdVideoStartStreaming : public MavLinkCommand { +public: + const static uint16_t kCommandId = 2502; + MavCmdVideoStartStreaming() { command = kCommandId; } + // Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + float StreamId = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Stop the given video stream +class MavCmdVideoStopStreaming : public MavLinkCommand { +public: + const static uint16_t kCommandId = 2503; + MavCmdVideoStopStreaming() { command = kCommandId; } + // Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + float StreamId = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Request video stream information (VIDEO_STREAM_INFORMATION) +class MavCmdRequestVideoStreamInformation : public MavLinkCommand { +public: + const static uint16_t kCommandId = 2504; + MavCmdRequestVideoStreamInformation() { command = kCommandId; } + // Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + float StreamId = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Request video stream status (VIDEO_STREAM_STATUS) +class MavCmdRequestVideoStreamStatus : public MavLinkCommand { +public: + const static uint16_t kCommandId = 2505; + MavCmdRequestVideoStreamStatus() { command = kCommandId; } + // Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + float StreamId = 0; protected: virtual void pack(); virtual void unpack(); @@ -6866,25 +8855,36 @@ class MavCmdAirframeConfiguration : public MavLinkCommand { MavCmdAirframeConfiguration() { command = kCommandId; } // Landing gear ID (default: 0, -1 for all) float LandingGearId = 0; - // Landing gear position (Down: 0, Up: 1, NAN for no change) + // Landing gear position (Down: 0, Up: 1, NaN for no change) float LandingGearPosition = 0; protected: virtual void pack(); virtual void unpack(); }; +// Request to start/stop transmitting over the high latency telemetry +class MavCmdControlHighLatency : public MavLinkCommand { +public: + const static uint16_t kCommandId = 2600; + MavCmdControlHighLatency() { command = kCommandId; } + // Control transmission over high latency telemetry (0: stop, 1: start) + float Enable = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; // Create a panorama at the current position class MavCmdPanoramaCreate : public MavLinkCommand { public: const static uint16_t kCommandId = 2800; MavCmdPanoramaCreate() { command = kCommandId; } - // Viewing angle horizontal of the panorama (in degrees, +- 0.5 the total angle) - float ViewingAngleHorizontal = 0; - // Viewing angle vertical of panorama (in degrees) - float ViewingAngleVertical = 0; - // Speed of the horizontal rotation (in degrees per second) - float SpeedOfHorizontal = 0; - // Speed of the vertical rotation (in degrees per second) - float SpeedOfVertical = 0; + // Viewing angle horizontal of the panorama (+- 0.5 the total angle) + float HorizontalAngle = 0; + // Viewing angle vertical of panorama. + float VerticalAngle = 0; + // Speed of the horizontal rotation. + float HorizontalSpeed = 0; + // Speed of the vertical rotation. + float VerticalSpeed = 0; protected: virtual void pack(); virtual void unpack(); @@ -6894,15 +8894,31 @@ class MavCmdDoVtolTransition : public MavLinkCommand { public: const static uint16_t kCommandId = 3000; MavCmdDoVtolTransition() { command = kCommandId; } - // The target VTOL state, as defined by ENUM MAV_VTOL_STATE. Only MAV_VTOL_STATE_MC - // and MAV_VTOL_STATE_FW can be used. - float TheTargetVtol = 0; + // The target VTOL state. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be + // used. + float State = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Request authorization to arm the vehicle to a external entity, the arm authorizer +// is responsible to request all data that is needs from the vehicle before authorize +// or deny the request. If approved the progress of command_ack message should be +// set with period of time that this authorization is valid in seconds or in case +// it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. +class MavCmdArmAuthorizationRequest : public MavLinkCommand { +public: + const static uint16_t kCommandId = 3001; + MavCmdArmAuthorizationRequest() { command = kCommandId; } + // Vehicle system id, this way ground station can request arm authorization on + // behalf of any vehicle + float SystemId = 0; protected: virtual void pack(); virtual void unpack(); }; // This command sets the submode to standard guided when vehicle is in guided mode. -// The vehicle holds position and altitude and the user can input the desired velocites +// The vehicle holds position and altitude and the user can input the desired velocities // along all three axes. class MavCmdSetGuidedSubmodeStandard : public MavLinkCommand { public: @@ -6921,7 +8937,7 @@ class MavCmdSetGuidedSubmodeCircle : public MavLinkCommand { const static uint16_t kCommandId = 4001; MavCmdSetGuidedSubmodeCircle() { command = kCommandId; } // Radius of desired circle in CIRCLE_MODE - float RadiusOfDesired = 0; + float Radius = 0; // User defined float UserDefined = 0; // User defined @@ -6936,6 +8952,131 @@ class MavCmdSetGuidedSubmodeCircle : public MavLinkCommand { virtual void pack(); virtual void unpack(); }; +// Delay mission state machine until gate has been reached. +class MavCmdConditionGate : public MavLinkCommand { +public: + const static uint16_t kCommandId = 4501; + MavCmdConditionGate() { command = kCommandId; } + // Geometry: 0: orthogonal to path between previous and next waypoint. + float Geometry = 0; + // Altitude: 0: ignore altitude + float Altitude = 0; + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; + // Altitude + float Altitude2 = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Fence return point. There can only be one fence return point. +class MavCmdNavFenceReturnPoint : public MavLinkCommand { +public: + const static uint16_t kCommandId = 5000; + MavCmdNavFenceReturnPoint() { command = kCommandId; } + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; + // Altitude + float Altitude = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). +// The vehicle must stay within this area. Minimum of 3 vertices required. +class MavCmdNavFencePolygonVertexInclusion : public MavLinkCommand { +public: + const static uint16_t kCommandId = 5001; + MavCmdNavFencePolygonVertexInclusion() { command = kCommandId; } + // Polygon vertex count + float VertexCount = 0; + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). +// The vehicle must stay outside this area. Minimum of 3 vertices required. +class MavCmdNavFencePolygonVertexExclusion : public MavLinkCommand { +public: + const static uint16_t kCommandId = 5002; + MavCmdNavFencePolygonVertexExclusion() { command = kCommandId; } + // Polygon vertex count + float VertexCount = 0; + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Circular fence area. The vehicle must stay inside this area. +class MavCmdNavFenceCircleInclusion : public MavLinkCommand { +public: + const static uint16_t kCommandId = 5003; + MavCmdNavFenceCircleInclusion() { command = kCommandId; } + // Radius. + float Radius = 0; + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Circular fence area. The vehicle must stay outside this area. +class MavCmdNavFenceCircleExclusion : public MavLinkCommand { +public: + const static uint16_t kCommandId = 5004; + MavCmdNavFenceCircleExclusion() { command = kCommandId; } + // Radius. + float Radius = 0; + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Rally point. You can have multiple rally points defined. +class MavCmdNavRallyPoint : public MavLinkCommand { +public: + const static uint16_t kCommandId = 5100; + MavCmdNavRallyPoint() { command = kCommandId; } + // Latitude + float Latitude = 0; + // Longitude + float Longitude = 0; + // Altitude + float Altitude = 0; +protected: + virtual void pack(); + virtual void unpack(); +}; +// Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one +// message per every UAVCAN node that is online. Note that some of the response messages +// can be lost, which the receiver can detect easily by checking whether every received +// UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if +// not, this command should be sent again in order to request re-transmission of the +// node information messages. +class MavCmdUavcanGetNodeInfo : public MavLinkCommand { +public: + const static uint16_t kCommandId = 5200; + MavCmdUavcanGetNodeInfo() { command = kCommandId; } +protected: + virtual void pack(); + virtual void unpack(); +}; // Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach // the required release position and velocity. class MavCmdPayloadPrepareDeploy : public MavLinkCommand { @@ -6947,21 +9088,21 @@ class MavCmdPayloadPrepareDeploy : public MavLinkCommand { // deploy commands during execution, but allowing abort). 2: add payload deploy // to existing deployment list. float OperationMode = 0; - // Desired approach vector in degrees compass heading (0..360). A negative value - // indicates the system can define the approach vector at will. - float DesiredApproachVector = 0; - // Desired ground speed at release time. This can be overriden by the airframe + // Desired approach vector in compass heading. A negative value indicates the + // system can define the approach vector at will. + float ApproachVector = 0; + // Desired ground speed at release time. This can be overridden by the airframe // in case it needs to meet minimum airspeed. A negative value indicates the system // can define the ground speed at will. - float DesiredGroundSpeed = 0; - // Minimum altitude clearance to the release position in meters. A negative value - // indicates the system can define the clearance at will. - float MinimumAltitudeClearance = 0; + float GroundSpeed = 0; + // Minimum altitude clearance to the release position. A negative value indicates + // the system can define the clearance at will. + float AltitudeClearance = 0; // Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT float LatitudeUnscaledFor = 0; // Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT float LongitudeUnscaledFor = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -6973,7 +9114,7 @@ class MavCmdPayloadControlDeploy : public MavLinkCommand { const static uint16_t kCommandId = 30002; MavCmdPayloadControlDeploy() { command = kCommandId; } // Operation mode. 0: Abort deployment, continue normal mission. 1: switch to - // payload deploment mode. 100: delete first payload deployment request. 101: + // payload deployment mode. 100: delete first payload deployment request. 101: // delete all payload deployment requests. float OperationMode = 0; protected: @@ -6998,7 +9139,7 @@ class MavCmdWaypointUser1 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7022,7 +9163,7 @@ class MavCmdWaypointUser2 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7046,7 +9187,7 @@ class MavCmdWaypointUser3 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7070,7 +9211,7 @@ class MavCmdWaypointUser4 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7094,7 +9235,7 @@ class MavCmdWaypointUser5 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7118,7 +9259,7 @@ class MavCmdSpatialUser1 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7142,7 +9283,7 @@ class MavCmdSpatialUser2 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7166,7 +9307,7 @@ class MavCmdSpatialUser3 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7190,7 +9331,7 @@ class MavCmdSpatialUser4 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); @@ -7214,7 +9355,7 @@ class MavCmdSpatialUser5 : public MavLinkCommand { float LatitudeUnscaled = 0; // Longitude unscaled float LongitudeUnscaled = 0; - // Altitude, in meters AMSL + // Altitude (MSL), in meters float Altitude = 0; protected: virtual void pack(); diff --git a/MavLinkCom/include/MavLinkNode.hpp b/MavLinkCom/include/MavLinkNode.hpp index a03123dca..54ee613d0 100644 --- a/MavLinkCom/include/MavLinkNode.hpp +++ b/MavLinkCom/include/MavLinkNode.hpp @@ -74,6 +74,9 @@ namespace mavlinkcom { // wait a given amount of time for a heart beat, and return the decoded message. AsyncResult waitForHeartbeat(); + // send a single heartbeat + void sendOneHeartbeat(); + // Get the local system and component id int getLocalSystemId(); int getLocalComponentId(); diff --git a/MavLinkCom/mavlink/common/common.h b/MavLinkCom/mavlink/common/common.h index 8068fcfb4..7bb305017 100644 --- a/MavLinkCom/mavlink/common/common.h +++ b/MavLinkCom/mavlink/common/common.h @@ -1,1025 +1,1972 @@ -/** @file - * @brief MAVLink comm protocol generated from common.xml - * @see http://mavlink.org - */ -#pragma once -#ifndef MAVLINK_COMMON_H -#define MAVLINK_COMMON_H - -#ifndef MAVLINK_H - #error Wrong include order: MAVLINK_COMMON.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. -#endif - -#undef MAVLINK_THIS_XML_IDX -#define MAVLINK_THIS_XML_IDX 1 - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 0, 0, 0}, {1, 124, 31, 0, 0, 0}, {2, 137, 12, 0, 0, 0}, {4, 237, 14, 3, 12, 13}, {5, 217, 28, 1, 0, 0}, {6, 104, 3, 0, 0, 0}, {7, 119, 32, 0, 0, 0}, {11, 89, 6, 1, 4, 0}, {20, 214, 20, 3, 2, 3}, {21, 159, 2, 3, 0, 1}, {22, 220, 25, 0, 0, 0}, {23, 168, 23, 3, 4, 5}, {24, 24, 30, 0, 0, 0}, {25, 23, 101, 0, 0, 0}, {26, 170, 22, 0, 0, 0}, {27, 144, 26, 0, 0, 0}, {28, 67, 16, 0, 0, 0}, {29, 115, 14, 0, 0, 0}, {30, 39, 28, 0, 0, 0}, {31, 246, 32, 0, 0, 0}, {32, 185, 28, 0, 0, 0}, {33, 104, 28, 0, 0, 0}, {34, 237, 22, 0, 0, 0}, {35, 244, 22, 0, 0, 0}, {36, 222, 21, 0, 0, 0}, {37, 212, 6, 3, 4, 5}, {38, 9, 6, 3, 4, 5}, {39, 254, 37, 3, 32, 33}, {40, 230, 4, 3, 2, 3}, {41, 28, 4, 3, 2, 3}, {42, 28, 2, 0, 0, 0}, {43, 132, 2, 3, 0, 1}, {44, 221, 4, 3, 2, 3}, {45, 232, 2, 3, 0, 1}, {46, 11, 2, 0, 0, 0}, {47, 153, 3, 3, 0, 1}, {48, 41, 13, 1, 12, 0}, {49, 39, 12, 0, 0, 0}, {50, 78, 37, 3, 18, 19}, {51, 196, 4, 3, 2, 3}, {54, 15, 27, 3, 24, 25}, {55, 3, 25, 0, 0, 0}, {61, 167, 72, 0, 0, 0}, {62, 183, 26, 0, 0, 0}, {63, 119, 181, 0, 0, 0}, {64, 191, 225, 0, 0, 0}, {65, 118, 42, 0, 0, 0}, {66, 148, 6, 3, 2, 3}, {67, 21, 4, 0, 0, 0}, {69, 243, 11, 0, 0, 0}, {70, 124, 18, 3, 16, 17}, {73, 38, 37, 3, 32, 33}, {74, 20, 20, 0, 0, 0}, {75, 158, 35, 3, 30, 31}, {76, 152, 33, 3, 30, 31}, {77, 143, 3, 0, 0, 0}, {81, 106, 22, 0, 0, 0}, {82, 49, 39, 3, 36, 37}, {83, 22, 37, 0, 0, 0}, {84, 143, 53, 3, 50, 51}, {85, 140, 51, 0, 0, 0}, {86, 5, 53, 3, 50, 51}, {87, 150, 51, 0, 0, 0}, {89, 231, 28, 0, 0, 0}, {90, 183, 56, 0, 0, 0}, {91, 63, 42, 0, 0, 0}, {92, 54, 33, 0, 0, 0}, {93, 47, 81, 0, 0, 0}, {100, 175, 26, 0, 0, 0}, {101, 102, 32, 0, 0, 0}, {102, 158, 32, 0, 0, 0}, {103, 208, 20, 0, 0, 0}, {104, 56, 32, 0, 0, 0}, {105, 93, 62, 0, 0, 0}, {106, 138, 44, 0, 0, 0}, {107, 108, 64, 0, 0, 0}, {108, 32, 84, 0, 0, 0}, {109, 185, 9, 0, 0, 0}, {110, 84, 254, 3, 1, 2}, {111, 34, 16, 0, 0, 0}, {112, 174, 12, 0, 0, 0}, {113, 124, 36, 0, 0, 0}, {114, 237, 44, 0, 0, 0}, {115, 4, 64, 0, 0, 0}, {116, 76, 22, 0, 0, 0}, {117, 128, 6, 3, 4, 5}, {118, 56, 14, 0, 0, 0}, {119, 116, 12, 3, 10, 11}, {120, 134, 97, 0, 0, 0}, {121, 237, 2, 3, 0, 1}, {122, 203, 2, 3, 0, 1}, {123, 250, 113, 3, 0, 1}, {124, 87, 35, 0, 0, 0}, {125, 203, 6, 0, 0, 0}, {126, 220, 79, 0, 0, 0}, {127, 25, 35, 0, 0, 0}, {128, 226, 35, 0, 0, 0}, {129, 46, 22, 0, 0, 0}, {130, 29, 13, 0, 0, 0}, {131, 223, 255, 0, 0, 0}, {132, 85, 14, 0, 0, 0}, {133, 6, 18, 0, 0, 0}, {134, 229, 43, 0, 0, 0}, {135, 203, 8, 0, 0, 0}, {136, 1, 22, 0, 0, 0}, {137, 195, 14, 0, 0, 0}, {138, 109, 36, 0, 0, 0}, {139, 168, 43, 3, 41, 42}, {140, 181, 41, 0, 0, 0}, {141, 47, 32, 0, 0, 0}, {142, 72, 243, 0, 0, 0}, {143, 131, 14, 0, 0, 0}, {144, 127, 93, 0, 0, 0}, {146, 103, 100, 0, 0, 0}, {147, 154, 36, 0, 0, 0}, {148, 178, 60, 0, 0, 0}, {149, 200, 30, 0, 0, 0}, {230, 163, 42, 0, 0, 0}, {231, 105, 40, 0, 0, 0}, {232, 151, 63, 0, 0, 0}, {233, 35, 182, 0, 0, 0}, {234, 150, 40, 0, 0, 0}, {235, 179, 42, 0, 0, 0}, {241, 90, 32, 0, 0, 0}, {242, 104, 52, 0, 0, 0}, {243, 85, 53, 1, 52, 0}, {244, 95, 6, 0, 0, 0}, {245, 130, 2, 0, 0, 0}, {246, 184, 38, 0, 0, 0}, {247, 81, 19, 0, 0, 0}, {248, 8, 254, 3, 3, 4}, {249, 204, 36, 0, 0, 0}, {250, 49, 30, 0, 0, 0}, {251, 170, 18, 0, 0, 0}, {252, 44, 18, 0, 0, 0}, {253, 83, 51, 0, 0, 0}, {254, 46, 9, 0, 0, 0}, {256, 71, 42, 3, 8, 9}, {257, 131, 9, 0, 0, 0}, {258, 187, 32, 3, 0, 1}, {259, 49, 91, 0, 0, 0}, {260, 61, 32, 0, 0, 0}, {261, 179, 27, 0, 0, 0}, {262, 69, 31, 0, 0, 0}, {263, 133, 255, 0, 0, 0}, {264, 49, 28, 0, 0, 0}, {265, 26, 16, 0, 0, 0}, {266, 193, 255, 3, 2, 3}, {267, 35, 255, 3, 2, 3}, {268, 14, 4, 3, 2, 3}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_COMMON - -// ENUM DEFINITIONS - - -/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ -#ifndef HAVE_ENUM_MAV_AUTOPILOT -#define HAVE_ENUM_MAV_AUTOPILOT -typedef enum MAV_AUTOPILOT -{ - MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ - MAV_AUTOPILOT_RESERVED=1, /* Reserved for future use. | */ - MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ - MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */ - MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ - MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ - MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ - MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ - MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ - MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ - MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://pixhawk.ethz.ch/px4/ | */ - MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */ - MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */ - MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */ - MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */ - MAV_AUTOPILOT_ASLUAV=17, /* ASLUAV autopilot -- http://www.asl.ethz.ch | */ - MAV_AUTOPILOT_SMARTAP=18, /* SmartAP Autopilot - http://sky-drones.com | */ - MAV_AUTOPILOT_ENUM_END=19, /* | */ -} MAV_AUTOPILOT; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_TYPE -#define HAVE_ENUM_MAV_TYPE -typedef enum MAV_TYPE -{ - MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ - MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ - MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ - MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ - MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - MAV_TYPE_ROCKET=9, /* Rocket | */ - MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ - MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - MAV_TYPE_SUBMARINE=12, /* Submarine | */ - MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ - MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ - MAV_TYPE_TRICOPTER=15, /* Tricopter | */ - MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - MAV_TYPE_KITE=17, /* Kite | */ - MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ - MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */ - MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */ - MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL | */ - MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */ - MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */ - MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */ - MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */ - MAV_TYPE_GIMBAL=26, /* Onboard gimbal | */ - MAV_TYPE_ADSB=27, /* Onboard ADSB peripheral | */ - MAV_TYPE_ENUM_END=28, /* | */ -} MAV_TYPE; -#endif - -/** @brief These values define the type of firmware release. These values indicate the first version or release of this type. For example the first alpha release would be 64, the second would be 65. */ -#ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE -#define HAVE_ENUM_FIRMWARE_VERSION_TYPE -typedef enum FIRMWARE_VERSION_TYPE -{ - FIRMWARE_VERSION_TYPE_DEV=0, /* development release | */ - FIRMWARE_VERSION_TYPE_ALPHA=64, /* alpha release | */ - FIRMWARE_VERSION_TYPE_BETA=128, /* beta release | */ - FIRMWARE_VERSION_TYPE_RC=192, /* release candidate | */ - FIRMWARE_VERSION_TYPE_OFFICIAL=255, /* official stable release | */ - FIRMWARE_VERSION_TYPE_ENUM_END=256, /* | */ -} FIRMWARE_VERSION_TYPE; -#endif - -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG -typedef enum MAV_MODE_FLAG -{ - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ - MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ - MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ - MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ - MAV_MODE_FLAG_ENUM_END=129, /* | */ -} MAV_MODE_FLAG; -#endif - -/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -typedef enum MAV_MODE_FLAG_DECODE_POSITION -{ - MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ - MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ - MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixt bit: 00000100 | */ - MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ - MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ - MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ - MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ -} MAV_MODE_FLAG_DECODE_POSITION; -#endif - -/** @brief Override command, pauses current mission execution and moves immediately to a position */ -#ifndef HAVE_ENUM_MAV_GOTO -#define HAVE_ENUM_MAV_GOTO -typedef enum MAV_GOTO -{ - MAV_GOTO_DO_HOLD=0, /* Hold at the current position. | */ - MAV_GOTO_DO_CONTINUE=1, /* Continue with the next item in mission execution. | */ - MAV_GOTO_HOLD_AT_CURRENT_POSITION=2, /* Hold at the current position of the system | */ - MAV_GOTO_HOLD_AT_SPECIFIED_POSITION=3, /* Hold at the position specified in the parameters of the DO_HOLD action | */ - MAV_GOTO_ENUM_END=4, /* | */ -} MAV_GOTO; -#endif - -/** @brief These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it - simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. */ -#ifndef HAVE_ENUM_MAV_MODE -#define HAVE_ENUM_MAV_MODE -typedef enum MAV_MODE -{ - MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. No flag is set. | */ - MAV_MODE_MANUAL_DISARMED=64, /* System is allowed to be active, under manual (RC) control, no stabilization | */ - MAV_MODE_TEST_DISARMED=66, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ - MAV_MODE_STABILIZE_DISARMED=80, /* System is allowed to be active, under assisted RC control. | */ - MAV_MODE_GUIDED_DISARMED=88, /* System is allowed to be active, under autonomous control, manual setpoint | */ - MAV_MODE_AUTO_DISARMED=92, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */ - MAV_MODE_MANUAL_ARMED=192, /* System is allowed to be active, under manual (RC) control, no stabilization | */ - MAV_MODE_TEST_ARMED=194, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ - MAV_MODE_STABILIZE_ARMED=208, /* System is allowed to be active, under assisted RC control. | */ - MAV_MODE_GUIDED_ARMED=216, /* System is allowed to be active, under autonomous control, manual setpoint | */ - MAV_MODE_AUTO_ARMED=220, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */ - MAV_MODE_ENUM_END=221, /* | */ -} MAV_MODE; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_STATE -#define HAVE_ENUM_MAV_STATE -typedef enum MAV_STATE -{ - MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ - MAV_STATE_BOOT=1, /* System is booting up. | */ - MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ - MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ - MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ - MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ - MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ - MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ - MAV_STATE_ENUM_END=8, /* | */ -} MAV_STATE; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_COMPONENT -#define HAVE_ENUM_MAV_COMPONENT -typedef enum MAV_COMPONENT -{ - MAV_COMP_ID_ALL=0, /* | */ - MAV_COMP_ID_CAMERA=100, /* | */ - MAV_COMP_ID_SERVO1=140, /* | */ - MAV_COMP_ID_SERVO2=141, /* | */ - MAV_COMP_ID_SERVO3=142, /* | */ - MAV_COMP_ID_SERVO4=143, /* | */ - MAV_COMP_ID_SERVO5=144, /* | */ - MAV_COMP_ID_SERVO6=145, /* | */ - MAV_COMP_ID_SERVO7=146, /* | */ - MAV_COMP_ID_SERVO8=147, /* | */ - MAV_COMP_ID_SERVO9=148, /* | */ - MAV_COMP_ID_SERVO10=149, /* | */ - MAV_COMP_ID_SERVO11=150, /* | */ - MAV_COMP_ID_SERVO12=151, /* | */ - MAV_COMP_ID_SERVO13=152, /* | */ - MAV_COMP_ID_SERVO14=153, /* | */ - MAV_COMP_ID_GIMBAL=154, /* | */ - MAV_COMP_ID_LOG=155, /* | */ - MAV_COMP_ID_ADSB=156, /* | */ - MAV_COMP_ID_OSD=157, /* On Screen Display (OSD) devices for video links | */ - MAV_COMP_ID_PERIPHERAL=158, /* Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter sub-protocol | */ - MAV_COMP_ID_QX1_GIMBAL=159, /* | */ - MAV_COMP_ID_MAPPER=180, /* | */ - MAV_COMP_ID_MISSIONPLANNER=190, /* | */ - MAV_COMP_ID_PATHPLANNER=195, /* | */ - MAV_COMP_ID_IMU=200, /* | */ - MAV_COMP_ID_IMU_2=201, /* | */ - MAV_COMP_ID_IMU_3=202, /* | */ - MAV_COMP_ID_GPS=220, /* | */ - MAV_COMP_ID_UDP_BRIDGE=240, /* | */ - MAV_COMP_ID_UART_BRIDGE=241, /* | */ - MAV_COMP_ID_SYSTEM_CONTROL=250, /* | */ - MAV_COMPONENT_ENUM_END=251, /* | */ -} MAV_COMPONENT; -#endif - -/** @brief These encode the sensors whose status is sent as part of the SYS_STATUS message. */ -#ifndef HAVE_ENUM_MAV_SYS_STATUS_SENSOR -#define HAVE_ENUM_MAV_SYS_STATUS_SENSOR -typedef enum MAV_SYS_STATUS_SENSOR -{ - MAV_SYS_STATUS_SENSOR_3D_GYRO=1, /* 0x01 3D gyro | */ - MAV_SYS_STATUS_SENSOR_3D_ACCEL=2, /* 0x02 3D accelerometer | */ - MAV_SYS_STATUS_SENSOR_3D_MAG=4, /* 0x04 3D magnetometer | */ - MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE=8, /* 0x08 absolute pressure | */ - MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE=16, /* 0x10 differential pressure | */ - MAV_SYS_STATUS_SENSOR_GPS=32, /* 0x20 GPS | */ - MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW=64, /* 0x40 optical flow | */ - MAV_SYS_STATUS_SENSOR_VISION_POSITION=128, /* 0x80 computer vision position | */ - MAV_SYS_STATUS_SENSOR_LASER_POSITION=256, /* 0x100 laser based position | */ - MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH=512, /* 0x200 external ground truth (Vicon or Leica) | */ - MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL=1024, /* 0x400 3D angular rate control | */ - MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION=2048, /* 0x800 attitude stabilization | */ - MAV_SYS_STATUS_SENSOR_YAW_POSITION=4096, /* 0x1000 yaw position | */ - MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL=8192, /* 0x2000 z/altitude control | */ - MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL=16384, /* 0x4000 x/y position control | */ - MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS=32768, /* 0x8000 motor outputs / control | */ - MAV_SYS_STATUS_SENSOR_RC_RECEIVER=65536, /* 0x10000 rc receiver | */ - MAV_SYS_STATUS_SENSOR_3D_GYRO2=131072, /* 0x20000 2nd 3D gyro | */ - MAV_SYS_STATUS_SENSOR_3D_ACCEL2=262144, /* 0x40000 2nd 3D accelerometer | */ - MAV_SYS_STATUS_SENSOR_3D_MAG2=524288, /* 0x80000 2nd 3D magnetometer | */ - MAV_SYS_STATUS_GEOFENCE=1048576, /* 0x100000 geofence | */ - MAV_SYS_STATUS_AHRS=2097152, /* 0x200000 AHRS subsystem health | */ - MAV_SYS_STATUS_TERRAIN=4194304, /* 0x400000 Terrain subsystem health | */ - MAV_SYS_STATUS_REVERSE_MOTOR=8388608, /* 0x800000 Motors are reversed | */ - MAV_SYS_STATUS_LOGGING=16777216, /* 0x1000000 Logging | */ - MAV_SYS_STATUS_SENSOR_BATTERY=33554432, /* 0x2000000 Battery | */ - MAV_SYS_STATUS_SENSOR_ENUM_END=33554433, /* | */ -} MAV_SYS_STATUS_SENSOR; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_FRAME -#define HAVE_ENUM_MAV_FRAME -typedef enum MAV_FRAME -{ - MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) | */ - MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-up (x: north, y: east, z: down). | */ - MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ - MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */ - MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */ - MAV_FRAME_GLOBAL_INT=5, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL) | */ - MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. | */ - MAV_FRAME_LOCAL_OFFSET_NED=7, /* Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. | */ - MAV_FRAME_BODY_NED=8, /* Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. | */ - MAV_FRAME_BODY_OFFSET_NED=9, /* Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. | */ - MAV_FRAME_GLOBAL_TERRAIN_ALT=10, /* Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */ - MAV_FRAME_GLOBAL_TERRAIN_ALT_INT=11, /* Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */ - MAV_FRAME_ENUM_END=12, /* | */ -} MAV_FRAME; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE -#define HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE -typedef enum MAVLINK_DATA_STREAM_TYPE -{ - MAVLINK_DATA_STREAM_IMG_JPEG=1, /* | */ - MAVLINK_DATA_STREAM_IMG_BMP=2, /* | */ - MAVLINK_DATA_STREAM_IMG_RAW8U=3, /* | */ - MAVLINK_DATA_STREAM_IMG_RAW32U=4, /* | */ - MAVLINK_DATA_STREAM_IMG_PGM=5, /* | */ - MAVLINK_DATA_STREAM_IMG_PNG=6, /* | */ - MAVLINK_DATA_STREAM_TYPE_ENUM_END=7, /* | */ -} MAVLINK_DATA_STREAM_TYPE; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_ACTION -#define HAVE_ENUM_FENCE_ACTION -typedef enum FENCE_ACTION -{ - FENCE_ACTION_NONE=0, /* Disable fenced mode | */ - FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ - FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ - FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ - FENCE_ACTION_RTL=4, /* Switch to RTL (return to launch) mode and head for the return point. | */ - FENCE_ACTION_ENUM_END=5, /* | */ -} FENCE_ACTION; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_BREACH -#define HAVE_ENUM_FENCE_BREACH -typedef enum FENCE_BREACH -{ - FENCE_BREACH_NONE=0, /* No last fence breach | */ - FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ - FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ - FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ - FENCE_BREACH_ENUM_END=4, /* | */ -} FENCE_BREACH; -#endif - -/** @brief Enumeration of possible mount operation modes */ -#ifndef HAVE_ENUM_MAV_MOUNT_MODE -#define HAVE_ENUM_MAV_MOUNT_MODE -typedef enum MAV_MOUNT_MODE -{ - MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ - MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ - MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ - MAV_MOUNT_MODE_ENUM_END=5, /* | */ -} MAV_MOUNT_MODE; -#endif - -/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ -#ifndef HAVE_ENUM_MAV_CMD -#define HAVE_ENUM_MAV_CMD -typedef enum MAV_CMD -{ - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing). NaN for unchanged.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location. Else, this is desired yaw angle| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location. Else, this is desired yaw angle| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Abort Alt| Empty| Empty| Desired yaw angle. NaN for unchanged.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer. NaN for unchanged.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LAND_LOCAL=23, /* Land at local position (local frame only) |Landing target number (if available)| Maximum accepted offset from desired landing position [m] - computed magnitude from spherical coordinates: d = sqrt(x^2 + y^2 + z^2), which gives the maximum accepted distance between the desired landing position and the position where the vehicle is about to land| Landing descend rate [ms^-1]| Desired yaw angle [rad]| Y-axis position [m]| X-axis position [m]| Z-axis / ground level position [m]| */ - MAV_CMD_NAV_TAKEOFF_LOCAL=24, /* Takeoff from local position (local frame only) |Minimum pitch (if airspeed sensor present), desired pitch without sensor [rad]| Empty| Takeoff ascend rate [ms^-1]| Yaw angle [rad] (if magnetometer or another yaw estimation source present), ignored without one of these| Y-axis position [m]| X-axis position [m]| Z-axis position [m]| */ - MAV_CMD_NAV_FOLLOW=25, /* Vehicle following, i.e. this waypoint represents the position of a moving vehicle |Following logic to use (e.g. loitering or sinusoidal following) - depends on specific autopilot implementation| Ground speed of vehicle to be followed| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT=30, /* Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. |Climb or Descend (0 = Neutral, command completes when within 5m of this command's altitude, 1 = Climbing, command completes when at or above this command's altitude, 2 = Descending, command completes when at or below this command's altitude. | Empty| Empty| Empty| Empty| Empty| Desired altitude in meters| */ - MAV_CMD_NAV_LOITER_TO_ALT=31, /* Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. |Heading Required (0 = False)| Radius in meters. If positive loiter clockwise, negative counter-clockwise, 0 means no change to standard loiter.| Empty| Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_FOLLOW=32, /* Being following a target |System ID (the system ID of the FOLLOW_TARGET beacon). Send 0 to disable follow-me and return to the default position hold mode| RESERVED| RESERVED| altitude flag: 0: Keep current altitude, 1: keep altitude difference to target, 2: go to a fixed altitude above home| altitude| RESERVED| TTL in seconds in which the MAV should go to the default position hold mode after a message rx timeout| */ - MAV_CMD_DO_FOLLOW_REPOSITION=33, /* Reposition the MAV after a follow target command has been sent |Camera q1 (where 0 is on the ray from the camera to the tracking device)| Camera q2| Camera q3| Camera q4| altitude offset from target (m)| X offset from target (m)| Y offset from target (m)| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_VTOL_TAKEOFF=84, /* Takeoff from ground using VTOL mode |Empty| Empty| Empty| Yaw angle in degrees. NaN for unchanged.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_VTOL_LAND=85, /* Land using VTOL mode |Empty| Empty| Empty| Yaw angle in degrees. NaN for unchanged.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_DELAY=93, /* Delay the next navigation command a number of seconds or until a specified time |Delay in seconds (decimal, -1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| */ - MAV_CMD_NAV_PAYLOAD_PLACE=94, /* Descend and place payload. Vehicle descends until it detects a hanging payload has reached the ground, the gripper is opened to release the payload |Maximum distance to descend (meters)| Empty| Empty| Empty| Latitude (deg * 1E7)| Longitude (deg * 1E7)| Altitude (meters)| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Custom sub mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| absolute or relative [0,1]| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_ALTITUDE=186, /* Change altitude set point. |Altitude in meters| Mav frame of new altitude (see MAV_FRAME)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_LAND_START=189, /* Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ - MAV_CMD_DO_RALLY_LAND=190, /* Mission command to perform a landing from a rally point. |Break altitude (meters)| Landing speed (m/s)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_GO_AROUND=191, /* Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPOSITION=192, /* Reposition the vehicle to a specific WGS84 global position. |Ground speed, less than 0 (-1) for default| Bitmask of option flags, see the MAV_DO_REPOSITION_FLAGS enum.| Reserved| Yaw heading, NaN for unchanged. For planes indicates loiter direction (0: clockwise, 1: counter clockwise)| Latitude (deg * 1E7)| Longitude (deg * 1E7)| Altitude (meters)| */ - MAV_CMD_DO_PAUSE_CONTINUE=193, /* If in a GPS controlled position mode, hold the current position or continue. |0: Pause current mission or reposition command, hold current position. 1: Continue mission. A VTOL capable vehicle should enter hover mode (multicopter and VTOL planes). A plane should loiter with the default loiter radius.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_DO_SET_REVERSE=194, /* Set moving direction to forward or reverse. |Direction (0=Forward, 1=Reverse)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch (WIP: DEPRECATED: or lat in degrees) depending on mount mode.| roll (WIP: DEPRECATED: or lon in degrees) depending on mount mode.| yaw (WIP: DEPRECATED: or alt in meters) depending on mount mode.| WIP: alt in meters depending on mount mode.| WIP: latitude in degrees * 1E7, set if appropriate mount mode.| WIP: longitude in degrees * 1E7, set if appropriate mount mode.| MAV_MOUNT_MODE enum value| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable, 2=disable_floor_only)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test |motor sequence number (a number from 1 to max number of motors on the vehicle)| throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)| throttle| timeout (in seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_SET_YAW_SPEED=213, /* Sets a desired vehicle turn angle and speed change |yaw angle to adjust steering by in centidegress| speed - normalized to 0 .. 1| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| */ - MAV_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_GUIDED_LIMITS=222, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ - MAV_CMD_DO_ENGINE_CONTROL=223, /* Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines |0: Stop engine, 1:Start Engine| 0: Warm start, 1:Cold start. Controls use of choke where applicable| Height delay (meters). This is for commanding engine start only after the vehicle has gained the specified height. Used in VTOL vehicles during takeoff to start engine after the aircraft is off the ground. Zero for no delay.| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. |1: gyro calibration, 3: gyro temperature calibration| 1: magnetometer calibration| 1: ground pressure calibration| 1: radio RC calibration, 2: RC trim calibration| 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration| 1: APM: compass/motor interference calibration (PX4: airspeed calibration, deprecated), 2: airspeed calibration| 1: ESC calibration, 3: barometer temperature calibration| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer, 6: third magnetometer| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_UAVCAN=243, /* Trigger UAVCAN config. This command will be only accepted if in pre-flight mode. |1: Trigger actuator ID assignment and direction mapping.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: start logging with rate of param 3 in Hz (e.g. set to 1000 for 1000 Hz logging)| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot, 3: Reboot autopilot and keep it in the bootloader until upgraded.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer, 3: Reboot onboard computer and keep it in the bootloader until upgraded.| WIP: 0: Do nothing for camera, 1: Reboot onboard camera, 2: Shutdown onboard camera, 3: Reboot onboard camera and keep it in the bootloader until upgraded| WIP: 0: Do nothing for mount (e.g. gimbal), 1: Reboot mount, 2: Shutdown mount, 3: Reboot mount and keep it in the bootloader until upgraded| Reserved, send 0| Reserved, send 0| WIP: ID (e.g. camera ID -1 for all IDs)| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_GET_HOME_POSITION=410, /* Request the home position from the vehicle. |Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_GET_MESSAGE_INTERVAL=510, /* Request the interval between messages for a particular MAVLink message ID |The MAVLink message ID| */ - MAV_CMD_SET_MESSAGE_INTERVAL=511, /* Request the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM |The MAVLink message ID| The interval between two messages, in microseconds. Set to -1 to disable and 0 to request default rate.| */ - MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES=520, /* Request autopilot capabilities |1: Request autopilot version| Reserved (all remaining params)| */ - MAV_CMD_REQUEST_CAMERA_INFORMATION=521, /* WIP: Request camera information (CAMERA_INFORMATION) |Camera ID (0 for all cameras, 1 for first, 2 for second, etc.)| 0: No action 1: Request camera capabilities| Reserved (all remaining params)| */ - MAV_CMD_REQUEST_CAMERA_SETTINGS=522, /* WIP: Request camera settings (CAMERA_SETTINGS) |Camera ID (0 for all cameras, 1 for first, 2 for second, etc.)| 0: No Action 1: Request camera settings| Reserved (all remaining params)| */ - MAV_CMD_SET_CAMERA_SETTINGS_1=523, /* WIP: Set the camera settings part 1 (CAMERA_SETTINGS). Use NAN for values you don't want to change. |Camera ID (1 for first, 2 for second, etc.)| Aperture (1/value)| Shutter speed in seconds| ISO sensitivity| AE mode (Auto Exposure) (0: full auto 1: full manual 2: aperture priority 3: shutter priority)| EV value (when in auto exposure)| White balance (color temperature in K) (0: Auto WB)| */ - MAV_CMD_SET_CAMERA_SETTINGS_2=524, /* WIP: Set the camera settings part 2 (CAMERA_SETTINGS). Use NAN for values you don't want to change. |Camera ID (1 for first, 2 for second, etc.)| Camera mode (0: photo, 1: video)| Audio recording enabled (0: off 1: on)| Reserved for metering mode ID (Average, Center, Spot, etc.)| Reserved for image format ID (Jpeg/Raw/Jpeg+Raw)| Reserved for image quality ID (Compression)| Reserved for color mode ID (Neutral, Vivid, etc.)| */ - MAV_CMD_REQUEST_STORAGE_INFORMATION=525, /* WIP: Request storage information (STORAGE_INFORMATION) |Storage ID (0 for all, 1 for first, 2 for second, etc.)| 0: No Action 1: Request storage information| Reserved (all remaining params)| */ - MAV_CMD_STORAGE_FORMAT=526, /* WIP: Format a storage medium |Storage ID (1 for first, 2 for second, etc.)| 0: No action 1: Format storage| Reserved (all remaining params)| */ - MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS=527, /* WIP: Request camera capture status (CAMERA_CAPTURE_STATUS) |Camera ID (0 for all cameras, 1 for first, 2 for second, etc.)| 0: No Action 1: Request camera capture status| Reserved (all remaining params)| */ - MAV_CMD_REQUEST_FLIGHT_INFORMATION=528, /* WIP: Request flight information (FLIGHT_INFORMATION) |1: Request flight information| Reserved (all remaining params)| */ - MAV_CMD_RESET_CAMERA_SETTINGS=529, /* WIP: Reset all camera settings to Factory Default (CAMERA_SETTINGS) |Camera ID (0 for all cameras, 1 for first, 2 for second, etc.)| 0: No Action 1: Reset all settings| Reserved (all remaining params)| */ - MAV_CMD_IMAGE_START_CAPTURE=2000, /* WIP: Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. |Camera ID (0 for all cameras, 1 for first, 2 for second, etc.)| Duration between two consecutive pictures (in seconds)| Number of images to capture total - 0 for unlimited capture| Resolution horizontal in pixels (set to -1 for highest resolution possible)| Resolution vertical in pixels (set to -1 for highest resolution possible)| */ - MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* WIP: Stop image capture sequence |Camera ID (0 for all cameras, 1 for first, 2 for second, etc.)| Reserved| */ - MAV_CMD_DO_TRIGGER_CONTROL=2003, /* Enable or disable on-board camera triggering system. |Trigger enable/disable (0 for disable, 1 for start)| Shutter integration time (in ms)| Reserved| */ - MAV_CMD_VIDEO_START_CAPTURE=2500, /* WIP: Starts video capture (recording) |Camera ID (0 for all cameras, 1 for first, 2 for second, etc.)| Frames per second, set to -1 for highest framerate possible.| Resolution horizontal in pixels (set to -1 for highest resolution possible)| Resolution vertical in pixels (set to -1 for highest resolution possible)| Frequency CAMERA_CAPTURE_STATUS messages should be sent while recording (0 for no messages, otherwise time in Hz)| */ - MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* WIP: Stop the current video capture (recording) |Camera ID (0 for all cameras, 1 for first, 2 for second, etc.)| Reserved| */ - MAV_CMD_LOGGING_START=2510, /* Request to start streaming logging data over MAVLink (see also LOGGING_DATA message) |Format: 0: ULog| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ - MAV_CMD_LOGGING_STOP=2511, /* Request to stop streaming log data over MAVLink |Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ - MAV_CMD_AIRFRAME_CONFIGURATION=2520, /* |Landing gear ID (default: 0, -1 for all)| Landing gear position (Down: 0, Up: 1, NAN for no change)| Reserved, set to NAN| Reserved, set to NAN| Reserved, set to NAN| Reserved, set to NAN| Reserved, set to NAN| */ - MAV_CMD_PANORAMA_CREATE=2800, /* Create a panorama at the current position |Viewing angle horizontal of the panorama (in degrees, +- 0.5 the total angle)| Viewing angle vertical of panorama (in degrees)| Speed of the horizontal rotation (in degrees per second)| Speed of the vertical rotation (in degrees per second)| */ - MAV_CMD_DO_VTOL_TRANSITION=3000, /* Request VTOL transition |The target VTOL state, as defined by ENUM MAV_VTOL_STATE. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used.| */ - MAV_CMD_SET_GUIDED_SUBMODE_STANDARD=4000, /* This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocites along all three axes. - | */ - MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE=4001, /* This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. - |Radius of desired circle in CIRCLE_MODE| User defined| User defined| User defined| Unscaled target latitude of center of circle in CIRCLE_MODE| Unscaled target longitude of center of circle in CIRCLE_MODE| */ - MAV_CMD_NAV_FENCE_RETURN_POINT=5000, /* Fence return point. There can only be one fence return point. - |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION=5001, /* Fence vertex for an inclusion polygon. The vehicle must stay within this area. Minimum of 3 vertices required. - |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ - MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION=5002, /* Fence vertex for an exclusion polygon. The vehicle must stay outside this area. Minimum of 3 vertices required. - |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ - MAV_CMD_NAV_RALLY_POINT=5100, /* Rally point. You can have multiple rally points defined. - |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ - MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in degrees compass heading (0..360). A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overriden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position in meters. A negative value indicates the system can define the clearance at will.| Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Altitude, in meters AMSL| */ - MAV_CMD_PAYLOAD_CONTROL_DEPLOY=30002, /* Control the payload deployment. |Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deploment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_WAYPOINT_USER_1=31000, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_WAYPOINT_USER_2=31001, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_WAYPOINT_USER_3=31002, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_WAYPOINT_USER_4=31003, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_WAYPOINT_USER_5=31004, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_SPATIAL_USER_1=31005, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_SPATIAL_USER_2=31006, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_SPATIAL_USER_3=31007, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_SPATIAL_USER_4=31008, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_SPATIAL_USER_5=31009, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude, in meters AMSL| */ - MAV_CMD_USER_1=31010, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ - MAV_CMD_USER_2=31011, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ - MAV_CMD_USER_3=31012, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ - MAV_CMD_USER_4=31013, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ - MAV_CMD_USER_5=31014, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ - MAV_CMD_ENUM_END=31015, /* | */ -} MAV_CMD; -#endif - -/** @brief THIS INTERFACE IS DEPRECATED AS OF JULY 2015. Please use MESSAGE_INTERVAL instead. A data stream is not a fixed set of messages, but rather a - recommendation to the autopilot software. Individual autopilots may or may not obey - the recommended messages. */ -#ifndef HAVE_ENUM_MAV_DATA_STREAM -#define HAVE_ENUM_MAV_DATA_STREAM -typedef enum MAV_DATA_STREAM -{ - MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ - MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ - MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ - MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ - MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ - MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ - MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_ENUM_END=13, /* | */ -} MAV_DATA_STREAM; -#endif - -/** @brief The ROI (region of interest) for the vehicle. This can be - be used by the vehicle for camera/vehicle attitude alignment (see - MAV_CMD_NAV_ROI). */ -#ifndef HAVE_ENUM_MAV_ROI -#define HAVE_ENUM_MAV_ROI -typedef enum MAV_ROI -{ - MAV_ROI_NONE=0, /* No region of interest. | */ - MAV_ROI_WPNEXT=1, /* Point toward next MISSION. | */ - MAV_ROI_WPINDEX=2, /* Point toward given MISSION. | */ - MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ - MAV_ROI_TARGET=4, /* Point toward of given id. | */ - MAV_ROI_ENUM_END=5, /* | */ -} MAV_ROI; -#endif - -/** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */ -#ifndef HAVE_ENUM_MAV_CMD_ACK -#define HAVE_ENUM_MAV_CMD_ACK -typedef enum MAV_CMD_ACK -{ - MAV_CMD_ACK_OK=1, /* Command / mission item is ok. | */ - MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. | */ - MAV_CMD_ACK_ERR_ACCESS_DENIED=3, /* The system is refusing to accept this command from this source / communication partner. | */ - MAV_CMD_ACK_ERR_NOT_SUPPORTED=4, /* Command or mission item is not supported, other commands would be accepted. | */ - MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=5, /* The coordinate frame of this command / mission item is not supported. | */ - MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=6, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. | */ - MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=7, /* The X or latitude value is out of range. | */ - MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. | */ - MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. | */ - MAV_CMD_ACK_ENUM_END=10, /* | */ -} MAV_CMD_ACK; -#endif - -/** @brief Specifies the datatype of a MAVLink parameter. */ -#ifndef HAVE_ENUM_MAV_PARAM_TYPE -#define HAVE_ENUM_MAV_PARAM_TYPE -typedef enum MAV_PARAM_TYPE -{ - MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */ - MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */ - MAV_PARAM_TYPE_UINT16=3, /* 16-bit unsigned integer | */ - MAV_PARAM_TYPE_INT16=4, /* 16-bit signed integer | */ - MAV_PARAM_TYPE_UINT32=5, /* 32-bit unsigned integer | */ - MAV_PARAM_TYPE_INT32=6, /* 32-bit signed integer | */ - MAV_PARAM_TYPE_UINT64=7, /* 64-bit unsigned integer | */ - MAV_PARAM_TYPE_INT64=8, /* 64-bit signed integer | */ - MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */ - MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */ - MAV_PARAM_TYPE_ENUM_END=11, /* | */ -} MAV_PARAM_TYPE; -#endif - -/** @brief result from a mavlink command */ -#ifndef HAVE_ENUM_MAV_RESULT -#define HAVE_ENUM_MAV_RESULT -typedef enum MAV_RESULT -{ - MAV_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ - MAV_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ - MAV_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ - MAV_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ - MAV_RESULT_FAILED=4, /* Command executed, but failed | */ - MAV_RESULT_IN_PROGRESS=5, /* WIP: Command being executed | */ - MAV_RESULT_ENUM_END=6, /* | */ -} MAV_RESULT; -#endif - -/** @brief result in a mavlink mission ack */ -#ifndef HAVE_ENUM_MAV_MISSION_RESULT -#define HAVE_ENUM_MAV_MISSION_RESULT -typedef enum MAV_MISSION_RESULT -{ - MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */ - MAV_MISSION_ERROR=1, /* generic error / not accepting mission commands at all right now | */ - MAV_MISSION_UNSUPPORTED_FRAME=2, /* coordinate frame is not supported | */ - MAV_MISSION_UNSUPPORTED=3, /* command is not supported | */ - MAV_MISSION_NO_SPACE=4, /* mission item exceeds storage space | */ - MAV_MISSION_INVALID=5, /* one of the parameters has an invalid value | */ - MAV_MISSION_INVALID_PARAM1=6, /* param1 has an invalid value | */ - MAV_MISSION_INVALID_PARAM2=7, /* param2 has an invalid value | */ - MAV_MISSION_INVALID_PARAM3=8, /* param3 has an invalid value | */ - MAV_MISSION_INVALID_PARAM4=9, /* param4 has an invalid value | */ - MAV_MISSION_INVALID_PARAM5_X=10, /* x/param5 has an invalid value | */ - MAV_MISSION_INVALID_PARAM6_Y=11, /* y/param6 has an invalid value | */ - MAV_MISSION_INVALID_PARAM7=12, /* param7 has an invalid value | */ - MAV_MISSION_INVALID_SEQUENCE=13, /* received waypoint out of sequence | */ - MAV_MISSION_DENIED=14, /* not accepting any mission commands from this communication partner | */ - MAV_MISSION_RESULT_ENUM_END=15, /* | */ -} MAV_MISSION_RESULT; -#endif - -/** @brief Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. */ -#ifndef HAVE_ENUM_MAV_SEVERITY -#define HAVE_ENUM_MAV_SEVERITY -typedef enum MAV_SEVERITY -{ - MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */ - MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */ - MAV_SEVERITY_CRITICAL=2, /* Action must be taken immediately. Indicates failure in a primary system. | */ - MAV_SEVERITY_ERROR=3, /* Indicates an error in secondary/redundant systems. | */ - MAV_SEVERITY_WARNING=4, /* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */ - MAV_SEVERITY_NOTICE=5, /* An unusual event has occured, though not an error condition. This should be investigated for the root cause. | */ - MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */ - MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */ - MAV_SEVERITY_ENUM_END=8, /* | */ -} MAV_SEVERITY; -#endif - -/** @brief Power supply status flags (bitmask) */ -#ifndef HAVE_ENUM_MAV_POWER_STATUS -#define HAVE_ENUM_MAV_POWER_STATUS -typedef enum MAV_POWER_STATUS -{ - MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */ - MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */ - MAV_POWER_STATUS_USB_CONNECTED=4, /* USB power is connected | */ - MAV_POWER_STATUS_PERIPH_OVERCURRENT=8, /* peripheral supply is in over-current state | */ - MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */ - MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */ - MAV_POWER_STATUS_ENUM_END=33, /* | */ -} MAV_POWER_STATUS; -#endif - -/** @brief SERIAL_CONTROL device types */ -#ifndef HAVE_ENUM_SERIAL_CONTROL_DEV -#define HAVE_ENUM_SERIAL_CONTROL_DEV -typedef enum SERIAL_CONTROL_DEV -{ - SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */ - SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */ - SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */ - SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */ - SERIAL_CONTROL_DEV_SHELL=10, /* system shell | */ - SERIAL_CONTROL_DEV_ENUM_END=11, /* | */ -} SERIAL_CONTROL_DEV; -#endif - -/** @brief SERIAL_CONTROL flags (bitmask) */ -#ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG -#define HAVE_ENUM_SERIAL_CONTROL_FLAG -typedef enum SERIAL_CONTROL_FLAG -{ - SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */ - SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */ - SERIAL_CONTROL_FLAG_EXCLUSIVE=4, /* Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set | */ - SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */ - SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */ - SERIAL_CONTROL_FLAG_ENUM_END=17, /* | */ -} SERIAL_CONTROL_FLAG; -#endif - -/** @brief Enumeration of distance sensor types */ -#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR -#define HAVE_ENUM_MAV_DISTANCE_SENSOR -typedef enum MAV_DISTANCE_SENSOR -{ - MAV_DISTANCE_SENSOR_LASER=0, /* Laser rangefinder, e.g. LightWare SF02/F or PulsedLight units | */ - MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Ultrasound rangefinder, e.g. MaxBotix units | */ - MAV_DISTANCE_SENSOR_INFRARED=2, /* Infrared rangefinder, e.g. Sharp units | */ - MAV_DISTANCE_SENSOR_ENUM_END=3, /* | */ -} MAV_DISTANCE_SENSOR; -#endif - -/** @brief Enumeration of sensor orientation, according to its rotations */ -#ifndef HAVE_ENUM_MAV_SENSOR_ORIENTATION -#define HAVE_ENUM_MAV_SENSOR_ORIENTATION -typedef enum MAV_SENSOR_ORIENTATION -{ - MAV_SENSOR_ROTATION_NONE=0, /* Roll: 0, Pitch: 0, Yaw: 0 | */ - MAV_SENSOR_ROTATION_YAW_45=1, /* Roll: 0, Pitch: 0, Yaw: 45 | */ - MAV_SENSOR_ROTATION_YAW_90=2, /* Roll: 0, Pitch: 0, Yaw: 90 | */ - MAV_SENSOR_ROTATION_YAW_135=3, /* Roll: 0, Pitch: 0, Yaw: 135 | */ - MAV_SENSOR_ROTATION_YAW_180=4, /* Roll: 0, Pitch: 0, Yaw: 180 | */ - MAV_SENSOR_ROTATION_YAW_225=5, /* Roll: 0, Pitch: 0, Yaw: 225 | */ - MAV_SENSOR_ROTATION_YAW_270=6, /* Roll: 0, Pitch: 0, Yaw: 270 | */ - MAV_SENSOR_ROTATION_YAW_315=7, /* Roll: 0, Pitch: 0, Yaw: 315 | */ - MAV_SENSOR_ROTATION_ROLL_180=8, /* Roll: 180, Pitch: 0, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_180_YAW_45=9, /* Roll: 180, Pitch: 0, Yaw: 45 | */ - MAV_SENSOR_ROTATION_ROLL_180_YAW_90=10, /* Roll: 180, Pitch: 0, Yaw: 90 | */ - MAV_SENSOR_ROTATION_ROLL_180_YAW_135=11, /* Roll: 180, Pitch: 0, Yaw: 135 | */ - MAV_SENSOR_ROTATION_PITCH_180=12, /* Roll: 0, Pitch: 180, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_180_YAW_225=13, /* Roll: 180, Pitch: 0, Yaw: 225 | */ - MAV_SENSOR_ROTATION_ROLL_180_YAW_270=14, /* Roll: 180, Pitch: 0, Yaw: 270 | */ - MAV_SENSOR_ROTATION_ROLL_180_YAW_315=15, /* Roll: 180, Pitch: 0, Yaw: 315 | */ - MAV_SENSOR_ROTATION_ROLL_90=16, /* Roll: 90, Pitch: 0, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_90_YAW_45=17, /* Roll: 90, Pitch: 0, Yaw: 45 | */ - MAV_SENSOR_ROTATION_ROLL_90_YAW_90=18, /* Roll: 90, Pitch: 0, Yaw: 90 | */ - MAV_SENSOR_ROTATION_ROLL_90_YAW_135=19, /* Roll: 90, Pitch: 0, Yaw: 135 | */ - MAV_SENSOR_ROTATION_ROLL_270=20, /* Roll: 270, Pitch: 0, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_270_YAW_45=21, /* Roll: 270, Pitch: 0, Yaw: 45 | */ - MAV_SENSOR_ROTATION_ROLL_270_YAW_90=22, /* Roll: 270, Pitch: 0, Yaw: 90 | */ - MAV_SENSOR_ROTATION_ROLL_270_YAW_135=23, /* Roll: 270, Pitch: 0, Yaw: 135 | */ - MAV_SENSOR_ROTATION_PITCH_90=24, /* Roll: 0, Pitch: 90, Yaw: 0 | */ - MAV_SENSOR_ROTATION_PITCH_270=25, /* Roll: 0, Pitch: 270, Yaw: 0 | */ - MAV_SENSOR_ROTATION_PITCH_180_YAW_90=26, /* Roll: 0, Pitch: 180, Yaw: 90 | */ - MAV_SENSOR_ROTATION_PITCH_180_YAW_270=27, /* Roll: 0, Pitch: 180, Yaw: 270 | */ - MAV_SENSOR_ROTATION_ROLL_90_PITCH_90=28, /* Roll: 90, Pitch: 90, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_180_PITCH_90=29, /* Roll: 180, Pitch: 90, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_270_PITCH_90=30, /* Roll: 270, Pitch: 90, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_90_PITCH_180=31, /* Roll: 90, Pitch: 180, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_270_PITCH_180=32, /* Roll: 270, Pitch: 180, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_90_PITCH_270=33, /* Roll: 90, Pitch: 270, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_180_PITCH_270=34, /* Roll: 180, Pitch: 270, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_270_PITCH_270=35, /* Roll: 270, Pitch: 270, Yaw: 0 | */ - MAV_SENSOR_ROTATION_ROLL_90_PITCH_180_YAW_90=36, /* Roll: 90, Pitch: 180, Yaw: 90 | */ - MAV_SENSOR_ROTATION_ROLL_90_YAW_270=37, /* Roll: 90, Pitch: 0, Yaw: 270 | */ - MAV_SENSOR_ROTATION_ROLL_315_PITCH_315_YAW_315=38, /* Roll: 315, Pitch: 315, Yaw: 315 | */ - MAV_SENSOR_ORIENTATION_ENUM_END=39, /* | */ -} MAV_SENSOR_ORIENTATION; -#endif - -/** @brief Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot supports this capability. */ -#ifndef HAVE_ENUM_MAV_PROTOCOL_CAPABILITY -#define HAVE_ENUM_MAV_PROTOCOL_CAPABILITY -typedef enum MAV_PROTOCOL_CAPABILITY -{ - MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT=1, /* Autopilot supports MISSION float message type. | */ - MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT=2, /* Autopilot supports the new param float message type. | */ - MAV_PROTOCOL_CAPABILITY_MISSION_INT=4, /* Autopilot supports MISSION_INT scaled integer message type. | */ - MAV_PROTOCOL_CAPABILITY_COMMAND_INT=8, /* Autopilot supports COMMAND_INT scaled integer message type. | */ - MAV_PROTOCOL_CAPABILITY_PARAM_UNION=16, /* Autopilot supports the new param union message type. | */ - MAV_PROTOCOL_CAPABILITY_FTP=32, /* Autopilot supports the new FILE_TRANSFER_PROTOCOL message type. | */ - MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET=64, /* Autopilot supports commanding attitude offboard. | */ - MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED=128, /* Autopilot supports commanding position and velocity targets in local NED frame. | */ - MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT=256, /* Autopilot supports commanding position and velocity targets in global scaled integers. | */ - MAV_PROTOCOL_CAPABILITY_TERRAIN=512, /* Autopilot supports terrain protocol / data handling. | */ - MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET=1024, /* Autopilot supports direct actuator control. | */ - MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION=2048, /* Autopilot supports the flight termination command. | */ - MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION=4096, /* Autopilot supports onboard compass calibration. | */ - MAV_PROTOCOL_CAPABILITY_MAVLINK2=8192, /* Autopilot supports mavlink version 2. | */ - MAV_PROTOCOL_CAPABILITY_MISSION_FENCE=16384, /* Autopilot supports mission fence protocol. | */ - MAV_PROTOCOL_CAPABILITY_MISSION_RALLY=32768, /* Autopilot supports mission rally point protocol. | */ - MAV_PROTOCOL_CAPABILITY_ENUM_END=32769, /* | */ -} MAV_PROTOCOL_CAPABILITY; -#endif - -/** @brief Type of mission items being requested/sent in mission protocol. */ -#ifndef HAVE_ENUM_MAV_MISSION_TYPE -#define HAVE_ENUM_MAV_MISSION_TYPE -typedef enum MAV_MISSION_TYPE -{ - MAV_MISSION_TYPE_MISSION=0, /* Items are mission commands for main mission. | */ - MAV_MISSION_TYPE_FENCE=1, /* Specifies GeoFence area(s). Items are MAV_CMD_FENCE_ GeoFence items. | */ - MAV_MISSION_TYPE_RALLY=2, /* Specifies the rally points for the vehicle. Rally points are alternative RTL points. Items are MAV_CMD_RALLY_POINT rally point items. | */ - MAV_MISSION_TYPE_ALL=255, /* Only used in MISSION_CLEAR_ALL to clear all mission types. | */ - MAV_MISSION_TYPE_ENUM_END=256, /* | */ -} MAV_MISSION_TYPE; -#endif - -/** @brief Enumeration of estimator types */ -#ifndef HAVE_ENUM_MAV_ESTIMATOR_TYPE -#define HAVE_ENUM_MAV_ESTIMATOR_TYPE -typedef enum MAV_ESTIMATOR_TYPE -{ - MAV_ESTIMATOR_TYPE_NAIVE=1, /* This is a naive estimator without any real covariance feedback. | */ - MAV_ESTIMATOR_TYPE_VISION=2, /* Computer vision based estimate. Might be up to scale. | */ - MAV_ESTIMATOR_TYPE_VIO=3, /* Visual-inertial estimate. | */ - MAV_ESTIMATOR_TYPE_GPS=4, /* Plain GPS estimate. | */ - MAV_ESTIMATOR_TYPE_GPS_INS=5, /* Estimator integrating GPS and inertial sensing. | */ - MAV_ESTIMATOR_TYPE_ENUM_END=6, /* | */ -} MAV_ESTIMATOR_TYPE; -#endif - -/** @brief Enumeration of battery types */ -#ifndef HAVE_ENUM_MAV_BATTERY_TYPE -#define HAVE_ENUM_MAV_BATTERY_TYPE -typedef enum MAV_BATTERY_TYPE -{ - MAV_BATTERY_TYPE_UNKNOWN=0, /* Not specified. | */ - MAV_BATTERY_TYPE_LIPO=1, /* Lithium polymer battery | */ - MAV_BATTERY_TYPE_LIFE=2, /* Lithium-iron-phosphate battery | */ - MAV_BATTERY_TYPE_LION=3, /* Lithium-ION battery | */ - MAV_BATTERY_TYPE_NIMH=4, /* Nickel metal hydride battery | */ - MAV_BATTERY_TYPE_ENUM_END=5, /* | */ -} MAV_BATTERY_TYPE; -#endif - -/** @brief Enumeration of battery functions */ -#ifndef HAVE_ENUM_MAV_BATTERY_FUNCTION -#define HAVE_ENUM_MAV_BATTERY_FUNCTION -typedef enum MAV_BATTERY_FUNCTION -{ - MAV_BATTERY_FUNCTION_UNKNOWN=0, /* Battery function is unknown | */ - MAV_BATTERY_FUNCTION_ALL=1, /* Battery supports all flight systems | */ - MAV_BATTERY_FUNCTION_PROPULSION=2, /* Battery for the propulsion system | */ - MAV_BATTERY_FUNCTION_AVIONICS=3, /* Avionics battery | */ - MAV_BATTERY_TYPE_PAYLOAD=4, /* Payload battery | */ - MAV_BATTERY_FUNCTION_ENUM_END=5, /* | */ -} MAV_BATTERY_FUNCTION; -#endif - -/** @brief Enumeration of VTOL states */ -#ifndef HAVE_ENUM_MAV_VTOL_STATE -#define HAVE_ENUM_MAV_VTOL_STATE -typedef enum MAV_VTOL_STATE -{ - MAV_VTOL_STATE_UNDEFINED=0, /* MAV is not configured as VTOL | */ - MAV_VTOL_STATE_TRANSITION_TO_FW=1, /* VTOL is in transition from multicopter to fixed-wing | */ - MAV_VTOL_STATE_TRANSITION_TO_MC=2, /* VTOL is in transition from fixed-wing to multicopter | */ - MAV_VTOL_STATE_MC=3, /* VTOL is in multicopter state | */ - MAV_VTOL_STATE_FW=4, /* VTOL is in fixed-wing state | */ - MAV_VTOL_STATE_ENUM_END=5, /* | */ -} MAV_VTOL_STATE; -#endif - -/** @brief Enumeration of landed detector states */ -#ifndef HAVE_ENUM_MAV_LANDED_STATE -#define HAVE_ENUM_MAV_LANDED_STATE -typedef enum MAV_LANDED_STATE -{ - MAV_LANDED_STATE_UNDEFINED=0, /* MAV landed state is unknown | */ - MAV_LANDED_STATE_ON_GROUND=1, /* MAV is landed (on ground) | */ - MAV_LANDED_STATE_IN_AIR=2, /* MAV is in air | */ - MAV_LANDED_STATE_TAKEOFF=3, /* MAV currently taking off | */ - MAV_LANDED_STATE_LANDING=4, /* MAV currently landing | */ - MAV_LANDED_STATE_ENUM_END=5, /* | */ -} MAV_LANDED_STATE; -#endif - -/** @brief Enumeration of the ADSB altimeter types */ -#ifndef HAVE_ENUM_ADSB_ALTITUDE_TYPE -#define HAVE_ENUM_ADSB_ALTITUDE_TYPE -typedef enum ADSB_ALTITUDE_TYPE -{ - ADSB_ALTITUDE_TYPE_PRESSURE_QNH=0, /* Altitude reported from a Baro source using QNH reference | */ - ADSB_ALTITUDE_TYPE_GEOMETRIC=1, /* Altitude reported from a GNSS source | */ - ADSB_ALTITUDE_TYPE_ENUM_END=2, /* | */ -} ADSB_ALTITUDE_TYPE; -#endif - -/** @brief ADSB classification for the type of vehicle emitting the transponder signal */ -#ifndef HAVE_ENUM_ADSB_EMITTER_TYPE -#define HAVE_ENUM_ADSB_EMITTER_TYPE -typedef enum ADSB_EMITTER_TYPE -{ - ADSB_EMITTER_TYPE_NO_INFO=0, /* | */ - ADSB_EMITTER_TYPE_LIGHT=1, /* | */ - ADSB_EMITTER_TYPE_SMALL=2, /* | */ - ADSB_EMITTER_TYPE_LARGE=3, /* | */ - ADSB_EMITTER_TYPE_HIGH_VORTEX_LARGE=4, /* | */ - ADSB_EMITTER_TYPE_HEAVY=5, /* | */ - ADSB_EMITTER_TYPE_HIGHLY_MANUV=6, /* | */ - ADSB_EMITTER_TYPE_ROTOCRAFT=7, /* | */ - ADSB_EMITTER_TYPE_UNASSIGNED=8, /* | */ - ADSB_EMITTER_TYPE_GLIDER=9, /* | */ - ADSB_EMITTER_TYPE_LIGHTER_AIR=10, /* | */ - ADSB_EMITTER_TYPE_PARACHUTE=11, /* | */ - ADSB_EMITTER_TYPE_ULTRA_LIGHT=12, /* | */ - ADSB_EMITTER_TYPE_UNASSIGNED2=13, /* | */ - ADSB_EMITTER_TYPE_UAV=14, /* | */ - ADSB_EMITTER_TYPE_SPACE=15, /* | */ - ADSB_EMITTER_TYPE_UNASSGINED3=16, /* | */ - ADSB_EMITTER_TYPE_EMERGENCY_SURFACE=17, /* | */ - ADSB_EMITTER_TYPE_SERVICE_SURFACE=18, /* | */ - ADSB_EMITTER_TYPE_POINT_OBSTACLE=19, /* | */ - ADSB_EMITTER_TYPE_ENUM_END=20, /* | */ -} ADSB_EMITTER_TYPE; -#endif - -/** @brief These flags indicate status such as data validity of each data source. Set = data valid */ -#ifndef HAVE_ENUM_ADSB_FLAGS -#define HAVE_ENUM_ADSB_FLAGS -typedef enum ADSB_FLAGS -{ - ADSB_FLAGS_VALID_COORDS=1, /* | */ - ADSB_FLAGS_VALID_ALTITUDE=2, /* | */ - ADSB_FLAGS_VALID_HEADING=4, /* | */ - ADSB_FLAGS_VALID_VELOCITY=8, /* | */ - ADSB_FLAGS_VALID_CALLSIGN=16, /* | */ - ADSB_FLAGS_VALID_SQUAWK=32, /* | */ - ADSB_FLAGS_SIMULATED=64, /* | */ - ADSB_FLAGS_ENUM_END=65, /* | */ -} ADSB_FLAGS; -#endif - -/** @brief Bitmask of options for the MAV_CMD_DO_REPOSITION */ -#ifndef HAVE_ENUM_MAV_DO_REPOSITION_FLAGS -#define HAVE_ENUM_MAV_DO_REPOSITION_FLAGS -typedef enum MAV_DO_REPOSITION_FLAGS -{ - MAV_DO_REPOSITION_FLAGS_CHANGE_MODE=1, /* The aircraft should immediately transition into guided. This should not be set for follow me applications | */ - MAV_DO_REPOSITION_FLAGS_ENUM_END=2, /* | */ -} MAV_DO_REPOSITION_FLAGS; -#endif - -/** @brief Flags in EKF_STATUS message */ -#ifndef HAVE_ENUM_ESTIMATOR_STATUS_FLAGS -#define HAVE_ENUM_ESTIMATOR_STATUS_FLAGS -typedef enum ESTIMATOR_STATUS_FLAGS -{ - ESTIMATOR_ATTITUDE=1, /* True if the attitude estimate is good | */ - ESTIMATOR_VELOCITY_HORIZ=2, /* True if the horizontal velocity estimate is good | */ - ESTIMATOR_VELOCITY_VERT=4, /* True if the vertical velocity estimate is good | */ - ESTIMATOR_POS_HORIZ_REL=8, /* True if the horizontal position (relative) estimate is good | */ - ESTIMATOR_POS_HORIZ_ABS=16, /* True if the horizontal position (absolute) estimate is good | */ - ESTIMATOR_POS_VERT_ABS=32, /* True if the vertical position (absolute) estimate is good | */ - ESTIMATOR_POS_VERT_AGL=64, /* True if the vertical position (above ground) estimate is good | */ - ESTIMATOR_CONST_POS_MODE=128, /* True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) | */ - ESTIMATOR_PRED_POS_HORIZ_REL=256, /* True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate | */ - ESTIMATOR_PRED_POS_HORIZ_ABS=512, /* True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate | */ - ESTIMATOR_GPS_GLITCH=1024, /* True if the EKF has detected a GPS glitch | */ - ESTIMATOR_STATUS_FLAGS_ENUM_END=1025, /* | */ -} ESTIMATOR_STATUS_FLAGS; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE -#define HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE -typedef enum MOTOR_TEST_THROTTLE_TYPE -{ - MOTOR_TEST_THROTTLE_PERCENT=0, /* throttle as a percentage from 0 ~ 100 | */ - MOTOR_TEST_THROTTLE_PWM=1, /* throttle as an absolute PWM value (normally in range of 1000~2000) | */ - MOTOR_TEST_THROTTLE_PILOT=2, /* throttle pass-through from pilot's transmitter | */ - MOTOR_TEST_THROTTLE_TYPE_ENUM_END=3, /* | */ -} MOTOR_TEST_THROTTLE_TYPE; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_GPS_INPUT_IGNORE_FLAGS -#define HAVE_ENUM_GPS_INPUT_IGNORE_FLAGS -typedef enum GPS_INPUT_IGNORE_FLAGS -{ - GPS_INPUT_IGNORE_FLAG_ALT=1, /* ignore altitude field | */ - GPS_INPUT_IGNORE_FLAG_HDOP=2, /* ignore hdop field | */ - GPS_INPUT_IGNORE_FLAG_VDOP=4, /* ignore vdop field | */ - GPS_INPUT_IGNORE_FLAG_VEL_HORIZ=8, /* ignore horizontal velocity field (vn and ve) | */ - GPS_INPUT_IGNORE_FLAG_VEL_VERT=16, /* ignore vertical velocity field (vd) | */ - GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY=32, /* ignore speed accuracy field | */ - GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY=64, /* ignore horizontal accuracy field | */ - GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY=128, /* ignore vertical accuracy field | */ - GPS_INPUT_IGNORE_FLAGS_ENUM_END=129, /* | */ -} GPS_INPUT_IGNORE_FLAGS; -#endif - -/** @brief Possible actions an aircraft can take to avoid a collision. */ -#ifndef HAVE_ENUM_MAV_COLLISION_ACTION -#define HAVE_ENUM_MAV_COLLISION_ACTION -typedef enum MAV_COLLISION_ACTION -{ - MAV_COLLISION_ACTION_NONE=0, /* Ignore any potential collisions | */ - MAV_COLLISION_ACTION_REPORT=1, /* Report potential collision | */ - MAV_COLLISION_ACTION_ASCEND_OR_DESCEND=2, /* Ascend or Descend to avoid threat | */ - MAV_COLLISION_ACTION_MOVE_HORIZONTALLY=3, /* Move horizontally to avoid threat | */ - MAV_COLLISION_ACTION_MOVE_PERPENDICULAR=4, /* Aircraft to move perpendicular to the collision's velocity vector | */ - MAV_COLLISION_ACTION_RTL=5, /* Aircraft to fly directly back to its launch point | */ - MAV_COLLISION_ACTION_HOVER=6, /* Aircraft to stop in place | */ - MAV_COLLISION_ACTION_ENUM_END=7, /* | */ -} MAV_COLLISION_ACTION; -#endif - -/** @brief Aircraft-rated danger from this threat. */ -#ifndef HAVE_ENUM_MAV_COLLISION_THREAT_LEVEL -#define HAVE_ENUM_MAV_COLLISION_THREAT_LEVEL -typedef enum MAV_COLLISION_THREAT_LEVEL -{ - MAV_COLLISION_THREAT_LEVEL_NONE=0, /* Not a threat | */ - MAV_COLLISION_THREAT_LEVEL_LOW=1, /* Craft is mildly concerned about this threat | */ - MAV_COLLISION_THREAT_LEVEL_HIGH=2, /* Craft is panicing, and may take actions to avoid threat | */ - MAV_COLLISION_THREAT_LEVEL_ENUM_END=3, /* | */ -} MAV_COLLISION_THREAT_LEVEL; -#endif - -/** @brief Source of information about this collision. */ -#ifndef HAVE_ENUM_MAV_COLLISION_SRC -#define HAVE_ENUM_MAV_COLLISION_SRC -typedef enum MAV_COLLISION_SRC -{ - MAV_COLLISION_SRC_ADSB=0, /* ID field references ADSB_VEHICLE packets | */ - MAV_COLLISION_SRC_MAVLINK_GPS_GLOBAL_INT=1, /* ID field references MAVLink SRC ID | */ - MAV_COLLISION_SRC_ENUM_END=2, /* | */ -} MAV_COLLISION_SRC; -#endif - -/** @brief Type of GPS fix */ -#ifndef HAVE_ENUM_GPS_FIX_TYPE -#define HAVE_ENUM_GPS_FIX_TYPE -typedef enum GPS_FIX_TYPE -{ - GPS_FIX_TYPE_NO_GPS=0, /* No GPS connected | */ - GPS_FIX_TYPE_NO_FIX=1, /* No position information, GPS is connected | */ - GPS_FIX_TYPE_2D_FIX=2, /* 2D position | */ - GPS_FIX_TYPE_3D_FIX=3, /* 3D position | */ - GPS_FIX_TYPE_DGPS=4, /* DGPS/SBAS aided 3D position | */ - GPS_FIX_TYPE_RTK_FLOAT=5, /* RTK float, 3D position | */ - GPS_FIX_TYPE_RTK_FIXED=6, /* RTK Fixed, 3D position | */ - GPS_FIX_TYPE_STATIC=7, /* Static fixed, typically used for base stations | */ - GPS_FIX_TYPE_ENUM_END=8, /* | */ -} GPS_FIX_TYPE; -#endif - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - - -// base include - - -#undef MAVLINK_THIS_XML_IDX -#define MAVLINK_THIS_XML_IDX 1 - -#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX -# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_HIL_ACTUATOR_CONTROLS, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ALTITUDE, MAVLINK_MESSAGE_INFO_RESOURCE_REQUEST, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3, MAVLINK_MESSAGE_INFO_FOLLOW_TARGET, MAVLINK_MESSAGE_INFO_CONTROL_SYSTEM_STATE, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS, MAVLINK_MESSAGE_INFO_WIND_COV, MAVLINK_MESSAGE_INFO_GPS_INPUT, MAVLINK_MESSAGE_INFO_GPS_RTCM_DATA, MAVLINK_MESSAGE_INFO_HIGH_LATENCY, MAVLINK_MESSAGE_INFO_VIBRATION, MAVLINK_MESSAGE_INFO_HOME_POSITION, MAVLINK_MESSAGE_INFO_SET_HOME_POSITION, MAVLINK_MESSAGE_INFO_MESSAGE_INTERVAL, MAVLINK_MESSAGE_INFO_EXTENDED_SYS_STATE, MAVLINK_MESSAGE_INFO_ADSB_VEHICLE, MAVLINK_MESSAGE_INFO_COLLISION, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_SETUP_SIGNING, MAVLINK_MESSAGE_INFO_BUTTON_CHANGE, MAVLINK_MESSAGE_INFO_PLAY_TUNE, MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS, MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED, MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION, MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION, MAVLINK_MESSAGE_INFO_LOGGING_DATA, MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED, MAVLINK_MESSAGE_INFO_LOGGING_ACK} -# if MAVLINK_COMMAND_24BIT -# include "../mavlink_get_info.h" -# endif -#endif - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // MAVLINK_COMMON_H +/** @file + * @brief MAVLink comm protocol generated from common.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_COMMON_H +#define MAVLINK_COMMON_H + +#ifndef MAVLINK_H + #error Wrong include order: MAVLINK_COMMON.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + +#undef MAVLINK_THIS_XML_IDX +#define MAVLINK_THIS_XML_IDX 1 + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {1, 124, 31, 31, 0, 0, 0}, {2, 137, 12, 12, 0, 0, 0}, {4, 237, 14, 14, 3, 12, 13}, {5, 217, 28, 28, 1, 0, 0}, {6, 104, 3, 3, 0, 0, 0}, {7, 119, 32, 32, 0, 0, 0}, {8, 117, 36, 36, 0, 0, 0}, {11, 89, 6, 6, 1, 4, 0}, {20, 214, 20, 20, 3, 2, 3}, {21, 159, 2, 2, 3, 0, 1}, {22, 220, 25, 25, 0, 0, 0}, {23, 168, 23, 23, 3, 4, 5}, {24, 24, 30, 50, 0, 0, 0}, {25, 23, 101, 101, 0, 0, 0}, {26, 170, 22, 24, 0, 0, 0}, {27, 144, 26, 29, 0, 0, 0}, {28, 67, 16, 16, 0, 0, 0}, {29, 115, 14, 14, 0, 0, 0}, {30, 39, 28, 28, 0, 0, 0}, {31, 246, 32, 48, 0, 0, 0}, {32, 185, 28, 28, 0, 0, 0}, {33, 104, 28, 28, 0, 0, 0}, {34, 237, 22, 22, 0, 0, 0}, {35, 244, 22, 22, 0, 0, 0}, {36, 222, 21, 37, 0, 0, 0}, {37, 212, 6, 7, 3, 4, 5}, {38, 9, 6, 7, 3, 4, 5}, {39, 254, 37, 38, 3, 32, 33}, {40, 230, 4, 5, 3, 2, 3}, {41, 28, 4, 4, 3, 2, 3}, {42, 28, 2, 2, 0, 0, 0}, {43, 132, 2, 3, 3, 0, 1}, {44, 221, 4, 5, 3, 2, 3}, {45, 232, 2, 3, 3, 0, 1}, {46, 11, 2, 2, 0, 0, 0}, {47, 153, 3, 4, 3, 0, 1}, {48, 41, 13, 21, 1, 12, 0}, {49, 39, 12, 20, 0, 0, 0}, {50, 78, 37, 37, 3, 18, 19}, {51, 196, 4, 5, 3, 2, 3}, {52, 132, 7, 7, 0, 0, 0}, {54, 15, 27, 27, 3, 24, 25}, {55, 3, 25, 25, 0, 0, 0}, {61, 167, 72, 72, 0, 0, 0}, {62, 183, 26, 26, 0, 0, 0}, {63, 119, 181, 181, 0, 0, 0}, {64, 191, 225, 225, 0, 0, 0}, {65, 118, 42, 42, 0, 0, 0}, {66, 148, 6, 6, 3, 2, 3}, {67, 21, 4, 4, 0, 0, 0}, {69, 243, 11, 11, 1, 10, 0}, {70, 124, 18, 38, 3, 16, 17}, {73, 38, 37, 38, 3, 32, 33}, {74, 20, 20, 20, 0, 0, 0}, {75, 158, 35, 35, 3, 30, 31}, {76, 152, 33, 33, 3, 30, 31}, {77, 143, 3, 10, 3, 8, 9}, {81, 106, 22, 22, 0, 0, 0}, {82, 49, 39, 39, 3, 36, 37}, {83, 22, 37, 37, 0, 0, 0}, {84, 143, 53, 53, 3, 50, 51}, {85, 140, 51, 51, 0, 0, 0}, {86, 5, 53, 53, 3, 50, 51}, {87, 150, 51, 51, 0, 0, 0}, {89, 231, 28, 28, 0, 0, 0}, {90, 183, 56, 56, 0, 0, 0}, {91, 63, 42, 42, 0, 0, 0}, {92, 54, 33, 33, 0, 0, 0}, {93, 47, 81, 81, 0, 0, 0}, {100, 175, 26, 34, 0, 0, 0}, {101, 102, 32, 117, 0, 0, 0}, {102, 158, 32, 117, 0, 0, 0}, {103, 208, 20, 57, 0, 0, 0}, {104, 56, 32, 116, 0, 0, 0}, {105, 93, 62, 63, 0, 0, 0}, {106, 138, 44, 44, 0, 0, 0}, {107, 108, 64, 64, 0, 0, 0}, {108, 32, 84, 84, 0, 0, 0}, {109, 185, 9, 9, 0, 0, 0}, {110, 84, 254, 254, 3, 1, 2}, {111, 34, 16, 16, 0, 0, 0}, {112, 174, 12, 12, 0, 0, 0}, {113, 124, 36, 36, 0, 0, 0}, {114, 237, 44, 44, 0, 0, 0}, {115, 4, 64, 64, 0, 0, 0}, {116, 76, 22, 24, 0, 0, 0}, {117, 128, 6, 6, 3, 4, 5}, {118, 56, 14, 14, 0, 0, 0}, {119, 116, 12, 12, 3, 10, 11}, {120, 134, 97, 97, 0, 0, 0}, {121, 237, 2, 2, 3, 0, 1}, {122, 203, 2, 2, 3, 0, 1}, {123, 250, 113, 113, 3, 0, 1}, {124, 87, 35, 35, 0, 0, 0}, {125, 203, 6, 6, 0, 0, 0}, {126, 220, 79, 79, 0, 0, 0}, {127, 25, 35, 35, 0, 0, 0}, {128, 226, 35, 35, 0, 0, 0}, {129, 46, 22, 24, 0, 0, 0}, {130, 29, 13, 13, 0, 0, 0}, {131, 223, 255, 255, 0, 0, 0}, {132, 85, 14, 38, 0, 0, 0}, {133, 6, 18, 18, 0, 0, 0}, {134, 229, 43, 43, 0, 0, 0}, {135, 203, 8, 8, 0, 0, 0}, {136, 1, 22, 22, 0, 0, 0}, {137, 195, 14, 14, 0, 0, 0}, {138, 109, 36, 120, 0, 0, 0}, {139, 168, 43, 43, 3, 41, 42}, {140, 181, 41, 41, 0, 0, 0}, {141, 47, 32, 32, 0, 0, 0}, {142, 72, 243, 243, 0, 0, 0}, {143, 131, 14, 14, 0, 0, 0}, {144, 127, 93, 93, 0, 0, 0}, {146, 103, 100, 100, 0, 0, 0}, {147, 154, 36, 41, 0, 0, 0}, {148, 178, 60, 78, 0, 0, 0}, {149, 200, 30, 60, 0, 0, 0}, {162, 189, 8, 9, 0, 0, 0}, {230, 163, 42, 42, 0, 0, 0}, {231, 105, 40, 40, 0, 0, 0}, {232, 151, 63, 65, 0, 0, 0}, {233, 35, 182, 182, 0, 0, 0}, {234, 150, 40, 40, 0, 0, 0}, {235, 179, 42, 42, 0, 0, 0}, {241, 90, 32, 32, 0, 0, 0}, {242, 104, 52, 60, 0, 0, 0}, {243, 85, 53, 61, 1, 52, 0}, {244, 95, 6, 6, 0, 0, 0}, {245, 130, 2, 2, 0, 0, 0}, {246, 184, 38, 38, 0, 0, 0}, {247, 81, 19, 19, 0, 0, 0}, {248, 8, 254, 254, 3, 3, 4}, {249, 204, 36, 36, 0, 0, 0}, {250, 49, 30, 30, 0, 0, 0}, {251, 170, 18, 18, 0, 0, 0}, {252, 44, 18, 18, 0, 0, 0}, {253, 83, 51, 51, 0, 0, 0}, {254, 46, 9, 9, 0, 0, 0}, {256, 71, 42, 42, 3, 8, 9}, {257, 131, 9, 9, 0, 0, 0}, {258, 187, 32, 232, 3, 0, 1}, {259, 92, 235, 235, 0, 0, 0}, {260, 146, 5, 13, 0, 0, 0}, {261, 179, 27, 27, 0, 0, 0}, {262, 12, 18, 18, 0, 0, 0}, {263, 133, 255, 255, 0, 0, 0}, {264, 49, 28, 28, 0, 0, 0}, {265, 26, 16, 20, 0, 0, 0}, {266, 193, 255, 255, 3, 2, 3}, {267, 35, 255, 255, 3, 2, 3}, {268, 14, 4, 4, 3, 2, 3}, {269, 109, 213, 213, 0, 0, 0}, {270, 59, 19, 19, 0, 0, 0}, {299, 19, 96, 96, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}, {301, 243, 58, 58, 0, 0, 0}, {310, 28, 17, 17, 0, 0, 0}, {311, 95, 116, 116, 0, 0, 0}, {320, 243, 20, 20, 3, 2, 3}, {321, 88, 2, 2, 3, 0, 1}, {322, 243, 149, 149, 0, 0, 0}, {323, 78, 147, 147, 3, 0, 1}, {324, 132, 146, 146, 0, 0, 0}, {330, 23, 158, 167, 0, 0, 0}, {331, 91, 230, 232, 0, 0, 0}, {332, 236, 239, 239, 0, 0, 0}, {333, 231, 109, 109, 0, 0, 0}, {334, 135, 14, 14, 0, 0, 0}, {335, 225, 24, 24, 0, 0, 0}, {340, 99, 70, 70, 0, 0, 0}, {350, 232, 20, 252, 0, 0, 0}, {360, 11, 25, 25, 0, 0, 0}, {365, 36, 255, 255, 0, 0, 0}, {370, 98, 73, 73, 0, 0, 0}, {371, 161, 50, 50, 0, 0, 0}, {375, 251, 140, 140, 0, 0, 0}, {380, 232, 20, 20, 0, 0, 0}, {385, 147, 133, 133, 3, 2, 3}, {390, 156, 238, 238, 0, 0, 0}, {395, 231, 222, 222, 0, 0, 0}, {400, 110, 254, 254, 3, 4, 5}, {401, 183, 6, 6, 3, 4, 5}, {9000, 113, 137, 137, 0, 0, 0}, {12900, 197, 22, 22, 0, 0, 0}, {12901, 16, 37, 37, 0, 0, 0}, {12902, 181, 31, 31, 0, 0, 0}, {12903, 149, 24, 24, 0, 0, 0}, {12904, 238, 21, 21, 0, 0, 0}, {12905, 56, 21, 21, 0, 0, 0}, {12915, 67, 252, 252, 0, 0, 0}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_COMMON + +// ENUM DEFINITIONS + + +/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ +#ifndef HAVE_ENUM_MAV_AUTOPILOT +#define HAVE_ENUM_MAV_AUTOPILOT +typedef enum MAV_AUTOPILOT +{ + MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ + MAV_AUTOPILOT_RESERVED=1, /* Reserved for future use. | */ + MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ + MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilot - Plane/Copter/Rover/Sub/Tracker, http://ardupilot.org | */ + MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ + MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ + MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ + MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ + MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ + MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ + MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://px4.io/ | */ + MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */ + MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */ + MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */ + MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */ + MAV_AUTOPILOT_ASLUAV=17, /* ASLUAV autopilot -- http://www.asl.ethz.ch | */ + MAV_AUTOPILOT_SMARTAP=18, /* SmartAP Autopilot - http://sky-drones.com | */ + MAV_AUTOPILOT_AIRRAILS=19, /* AirRails - http://uaventure.com | */ + MAV_AUTOPILOT_ENUM_END=20, /* | */ +} MAV_AUTOPILOT; +#endif + +/** @brief MAVLINK component type reported in HEARTBEAT message. Flight controllers must report the type of the vehicle on which they are mounted (e.g. MAV_TYPE_OCTOROTOR). All other components must report a value appropriate for their type (e.g. a camera must use MAV_TYPE_CAMERA). */ +#ifndef HAVE_ENUM_MAV_TYPE +#define HAVE_ENUM_MAV_TYPE +typedef enum MAV_TYPE +{ + MAV_TYPE_GENERIC=0, /* Generic micro air vehicle | */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ + MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ + MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ + MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ + MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + MAV_TYPE_ROCKET=9, /* Rocket | */ + MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ + MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + MAV_TYPE_SUBMARINE=12, /* Submarine | */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ + MAV_TYPE_TRICOPTER=15, /* Tricopter | */ + MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + MAV_TYPE_KITE=17, /* Kite | */ + MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ + MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */ + MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */ + MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL | */ + MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */ + MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */ + MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */ + MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */ + MAV_TYPE_GIMBAL=26, /* Gimbal | */ + MAV_TYPE_ADSB=27, /* ADSB system | */ + MAV_TYPE_PARAFOIL=28, /* Steerable, nonrigid airfoil | */ + MAV_TYPE_DODECAROTOR=29, /* Dodecarotor | */ + MAV_TYPE_CAMERA=30, /* Camera | */ + MAV_TYPE_CHARGING_STATION=31, /* Charging station | */ + MAV_TYPE_FLARM=32, /* FLARM collision avoidance system | */ + MAV_TYPE_SERVO=33, /* Servo | */ + MAV_TYPE_ENUM_END=34, /* | */ +} MAV_TYPE; +#endif + +/** @brief These values define the type of firmware release. These values indicate the first version or release of this type. For example the first alpha release would be 64, the second would be 65. */ +#ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE +#define HAVE_ENUM_FIRMWARE_VERSION_TYPE +typedef enum FIRMWARE_VERSION_TYPE +{ + FIRMWARE_VERSION_TYPE_DEV=0, /* development release | */ + FIRMWARE_VERSION_TYPE_ALPHA=64, /* alpha release | */ + FIRMWARE_VERSION_TYPE_BETA=128, /* beta release | */ + FIRMWARE_VERSION_TYPE_RC=192, /* release candidate | */ + FIRMWARE_VERSION_TYPE_OFFICIAL=255, /* official stable release | */ + FIRMWARE_VERSION_TYPE_ENUM_END=256, /* | */ +} FIRMWARE_VERSION_TYPE; +#endif + +/** @brief Flags to report failure cases over the high latency telemtry. */ +#ifndef HAVE_ENUM_HL_FAILURE_FLAG +#define HAVE_ENUM_HL_FAILURE_FLAG +typedef enum HL_FAILURE_FLAG +{ + HL_FAILURE_FLAG_GPS=1, /* GPS failure. | */ + HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE=2, /* Differential pressure sensor failure. | */ + HL_FAILURE_FLAG_ABSOLUTE_PRESSURE=4, /* Absolute pressure sensor failure. | */ + HL_FAILURE_FLAG_3D_ACCEL=8, /* Accelerometer sensor failure. | */ + HL_FAILURE_FLAG_3D_GYRO=16, /* Gyroscope sensor failure. | */ + HL_FAILURE_FLAG_3D_MAG=32, /* Magnetometer sensor failure. | */ + HL_FAILURE_FLAG_TERRAIN=64, /* Terrain subsystem failure. | */ + HL_FAILURE_FLAG_BATTERY=128, /* Battery failure/critical low battery. | */ + HL_FAILURE_FLAG_RC_RECEIVER=256, /* RC receiver failure/no rc connection. | */ + HL_FAILURE_FLAG_OFFBOARD_LINK=512, /* Offboard link failure. | */ + HL_FAILURE_FLAG_ENGINE=1024, /* Engine failure. | */ + HL_FAILURE_FLAG_GEOFENCE=2048, /* Geofence violation. | */ + HL_FAILURE_FLAG_ESTIMATOR=4096, /* Estimator failure, for example measurement rejection or large variances. | */ + HL_FAILURE_FLAG_MISSION=8192, /* Mission failure. | */ + HL_FAILURE_FLAG_ENUM_END=8193, /* | */ +} HL_FAILURE_FLAG; +#endif + +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +typedef enum MAV_MODE_FLAG +{ + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies waypoints / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ + MAV_MODE_FLAG_ENUM_END=129, /* | */ +} MAV_MODE_FLAG; +#endif + +/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +typedef enum MAV_MODE_FLAG_DECODE_POSITION +{ + MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ + MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ + MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixth bit: 00000100 | */ + MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ + MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ + MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ + MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ +} MAV_MODE_FLAG_DECODE_POSITION; +#endif + +/** @brief Actions that may be specified in MAV_CMD_OVERRIDE_GOTO to override mission execution. */ +#ifndef HAVE_ENUM_MAV_GOTO +#define HAVE_ENUM_MAV_GOTO +typedef enum MAV_GOTO +{ + MAV_GOTO_DO_HOLD=0, /* Hold at the current position. | */ + MAV_GOTO_DO_CONTINUE=1, /* Continue with the next item in mission execution. | */ + MAV_GOTO_HOLD_AT_CURRENT_POSITION=2, /* Hold at the current position of the system | */ + MAV_GOTO_HOLD_AT_SPECIFIED_POSITION=3, /* Hold at the position specified in the parameters of the DO_HOLD action | */ + MAV_GOTO_ENUM_END=4, /* | */ +} MAV_GOTO; +#endif + +/** @brief These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it + simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. */ +#ifndef HAVE_ENUM_MAV_MODE +#define HAVE_ENUM_MAV_MODE +typedef enum MAV_MODE +{ + MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. No flag is set. | */ + MAV_MODE_MANUAL_DISARMED=64, /* System is allowed to be active, under manual (RC) control, no stabilization | */ + MAV_MODE_TEST_DISARMED=66, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ + MAV_MODE_STABILIZE_DISARMED=80, /* System is allowed to be active, under assisted RC control. | */ + MAV_MODE_GUIDED_DISARMED=88, /* System is allowed to be active, under autonomous control, manual setpoint | */ + MAV_MODE_AUTO_DISARMED=92, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) | */ + MAV_MODE_MANUAL_ARMED=192, /* System is allowed to be active, under manual (RC) control, no stabilization | */ + MAV_MODE_TEST_ARMED=194, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ + MAV_MODE_STABILIZE_ARMED=208, /* System is allowed to be active, under assisted RC control. | */ + MAV_MODE_GUIDED_ARMED=216, /* System is allowed to be active, under autonomous control, manual setpoint | */ + MAV_MODE_AUTO_ARMED=220, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) | */ + MAV_MODE_ENUM_END=221, /* | */ +} MAV_MODE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_STATE +#define HAVE_ENUM_MAV_STATE +typedef enum MAV_STATE +{ + MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ + MAV_STATE_BOOT=1, /* System is booting up. | */ + MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ + MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ + MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ + MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ + MAV_STATE_FLIGHT_TERMINATION=8, /* System is terminating itself. | */ + MAV_STATE_ENUM_END=9, /* | */ +} MAV_STATE; +#endif + +/** @brief Component ids (values) for the different types and instances of onboard hardware/software that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance systems etc.). + Components must use the appropriate ID in their source address when sending messages. Components can also use IDs to determine if they are the intended recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate messages that must be processed by all components. + When creating new entries, components that can have multiple instances (e.g. cameras, servos etc.) should be allocated sequential values. An appropriate number of values should be left free after these components to allow the number of instances to be expanded. */ +#ifndef HAVE_ENUM_MAV_COMPONENT +#define HAVE_ENUM_MAV_COMPONENT +typedef enum MAV_COMPONENT +{ + MAV_COMP_ID_ALL=0, /* Target id (target_component) used to broadcast messages to all components of the receiving system. Components should attempt to process messages with this component ID and forward to components on any other interfaces. Note: This is not a valid *source* component id for a message. | */ + MAV_COMP_ID_AUTOPILOT1=1, /* System flight controller component ("autopilot"). Only one autopilot is expected in a particular system. | */ + MAV_COMP_ID_USER1=25, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER2=26, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER3=27, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER4=28, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER5=29, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER6=30, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER7=31, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER8=32, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER9=33, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER10=34, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER11=35, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER12=36, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER13=37, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER14=38, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER15=39, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USE16=40, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER17=41, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER18=42, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER19=43, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER20=44, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER21=45, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER22=46, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER23=47, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER24=48, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER25=49, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER26=50, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER27=51, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER28=52, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER29=53, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER30=54, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER31=55, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER32=56, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER33=57, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER34=58, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER35=59, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER36=60, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER37=61, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER38=62, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER39=63, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER40=64, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER41=65, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER42=66, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER43=67, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER44=68, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER45=69, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER46=70, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER47=71, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER48=72, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER49=73, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER50=74, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER51=75, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER52=76, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER53=77, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER54=78, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER55=79, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER56=80, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER57=81, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER58=82, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER59=83, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER60=84, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER61=85, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER62=86, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER63=87, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER64=88, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER65=89, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER66=90, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER67=91, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER68=92, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER69=93, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER70=94, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER71=95, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER72=96, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER73=97, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER74=98, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER75=99, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_CAMERA=100, /* Camera #1. | */ + MAV_COMP_ID_CAMERA2=101, /* Camera #2. | */ + MAV_COMP_ID_CAMERA3=102, /* Camera #3. | */ + MAV_COMP_ID_CAMERA4=103, /* Camera #4. | */ + MAV_COMP_ID_CAMERA5=104, /* Camera #5. | */ + MAV_COMP_ID_CAMERA6=105, /* Camera #6. | */ + MAV_COMP_ID_SERVO1=140, /* Servo #1. | */ + MAV_COMP_ID_SERVO2=141, /* Servo #2. | */ + MAV_COMP_ID_SERVO3=142, /* Servo #3. | */ + MAV_COMP_ID_SERVO4=143, /* Servo #4. | */ + MAV_COMP_ID_SERVO5=144, /* Servo #5. | */ + MAV_COMP_ID_SERVO6=145, /* Servo #6. | */ + MAV_COMP_ID_SERVO7=146, /* Servo #7. | */ + MAV_COMP_ID_SERVO8=147, /* Servo #8. | */ + MAV_COMP_ID_SERVO9=148, /* Servo #9. | */ + MAV_COMP_ID_SERVO10=149, /* Servo #10. | */ + MAV_COMP_ID_SERVO11=150, /* Servo #11. | */ + MAV_COMP_ID_SERVO12=151, /* Servo #12. | */ + MAV_COMP_ID_SERVO13=152, /* Servo #13. | */ + MAV_COMP_ID_SERVO14=153, /* Servo #14. | */ + MAV_COMP_ID_GIMBAL=154, /* Gimbal #1. | */ + MAV_COMP_ID_LOG=155, /* Logging component. | */ + MAV_COMP_ID_ADSB=156, /* Automatic Dependent Surveillance-Broadcast (ADS-B) component. | */ + MAV_COMP_ID_OSD=157, /* On Screen Display (OSD) devices for video links. | */ + MAV_COMP_ID_PERIPHERAL=158, /* Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. | */ + MAV_COMP_ID_QX1_GIMBAL=159, /* Gimbal ID for QX1. | */ + MAV_COMP_ID_FLARM=160, /* FLARM collision alert component. | */ + MAV_COMP_ID_GIMBAL2=171, /* Gimbal #2. | */ + MAV_COMP_ID_GIMBAL3=172, /* Gimbal #3. | */ + MAV_COMP_ID_GIMBAL4=173, /* Gimbal #4 | */ + MAV_COMP_ID_GIMBAL5=174, /* Gimbal #5. | */ + MAV_COMP_ID_GIMBAL6=175, /* Gimbal #6. | */ + MAV_COMP_ID_MISSIONPLANNER=190, /* Component that can generate/supply a mission flight plan (e.g. GCS or developer API). | */ + MAV_COMP_ID_PATHPLANNER=195, /* Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). | */ + MAV_COMP_ID_OBSTACLE_AVOIDANCE=196, /* Component that plans a collision free path between two points. | */ + MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY=197, /* Component that provides position estimates using VIO techniques. | */ + MAV_COMP_ID_IMU=200, /* Inertial Measurement Unit (IMU) #1. | */ + MAV_COMP_ID_IMU_2=201, /* Inertial Measurement Unit (IMU) #2. | */ + MAV_COMP_ID_IMU_3=202, /* Inertial Measurement Unit (IMU) #3. | */ + MAV_COMP_ID_GPS=220, /* GPS #1. | */ + MAV_COMP_ID_GPS2=221, /* GPS #2. | */ + MAV_COMP_ID_UDP_BRIDGE=240, /* Component to bridge MAVLink to UDP (i.e. from a UART). | */ + MAV_COMP_ID_UART_BRIDGE=241, /* Component to bridge to UART (i.e. from UDP). | */ + MAV_COMP_ID_TUNNEL_NODE=242, /* Component handling TUNNEL messages (e.g. vendor specific GUI of a component). | */ + MAV_COMP_ID_SYSTEM_CONTROL=250, /* Component for handling system messages (e.g. to ARM, takeoff, etc.). | */ + MAV_COMPONENT_ENUM_END=251, /* | */ +} MAV_COMPONENT; +#endif + +/** @brief These encode the sensors whose status is sent as part of the SYS_STATUS message. */ +#ifndef HAVE_ENUM_MAV_SYS_STATUS_SENSOR +#define HAVE_ENUM_MAV_SYS_STATUS_SENSOR +typedef enum MAV_SYS_STATUS_SENSOR +{ + MAV_SYS_STATUS_SENSOR_3D_GYRO=1, /* 0x01 3D gyro | */ + MAV_SYS_STATUS_SENSOR_3D_ACCEL=2, /* 0x02 3D accelerometer | */ + MAV_SYS_STATUS_SENSOR_3D_MAG=4, /* 0x04 3D magnetometer | */ + MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE=8, /* 0x08 absolute pressure | */ + MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE=16, /* 0x10 differential pressure | */ + MAV_SYS_STATUS_SENSOR_GPS=32, /* 0x20 GPS | */ + MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW=64, /* 0x40 optical flow | */ + MAV_SYS_STATUS_SENSOR_VISION_POSITION=128, /* 0x80 computer vision position | */ + MAV_SYS_STATUS_SENSOR_LASER_POSITION=256, /* 0x100 laser based position | */ + MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH=512, /* 0x200 external ground truth (Vicon or Leica) | */ + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL=1024, /* 0x400 3D angular rate control | */ + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION=2048, /* 0x800 attitude stabilization | */ + MAV_SYS_STATUS_SENSOR_YAW_POSITION=4096, /* 0x1000 yaw position | */ + MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL=8192, /* 0x2000 z/altitude control | */ + MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL=16384, /* 0x4000 x/y position control | */ + MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS=32768, /* 0x8000 motor outputs / control | */ + MAV_SYS_STATUS_SENSOR_RC_RECEIVER=65536, /* 0x10000 rc receiver | */ + MAV_SYS_STATUS_SENSOR_3D_GYRO2=131072, /* 0x20000 2nd 3D gyro | */ + MAV_SYS_STATUS_SENSOR_3D_ACCEL2=262144, /* 0x40000 2nd 3D accelerometer | */ + MAV_SYS_STATUS_SENSOR_3D_MAG2=524288, /* 0x80000 2nd 3D magnetometer | */ + MAV_SYS_STATUS_GEOFENCE=1048576, /* 0x100000 geofence | */ + MAV_SYS_STATUS_AHRS=2097152, /* 0x200000 AHRS subsystem health | */ + MAV_SYS_STATUS_TERRAIN=4194304, /* 0x400000 Terrain subsystem health | */ + MAV_SYS_STATUS_REVERSE_MOTOR=8388608, /* 0x800000 Motors are reversed | */ + MAV_SYS_STATUS_LOGGING=16777216, /* 0x1000000 Logging | */ + MAV_SYS_STATUS_SENSOR_BATTERY=33554432, /* 0x2000000 Battery | */ + MAV_SYS_STATUS_SENSOR_PROXIMITY=67108864, /* 0x4000000 Proximity | */ + MAV_SYS_STATUS_SENSOR_SATCOM=134217728, /* 0x8000000 Satellite Communication | */ + MAV_SYS_STATUS_SENSOR_ENUM_END=134217729, /* | */ +} MAV_SYS_STATUS_SENSOR; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_FRAME +#define HAVE_ENUM_MAV_FRAME +typedef enum MAV_FRAME +{ + MAV_FRAME_GLOBAL=0, /* Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). | */ + MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-down (x: north, y: east, z: down). | */ + MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */ + MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-up (x: east, y: north, z: up). | */ + MAV_FRAME_GLOBAL_INT=5, /* Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL). | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. | */ + MAV_FRAME_LOCAL_OFFSET_NED=7, /* Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. | */ + MAV_FRAME_BODY_NED=8, /* Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. | */ + MAV_FRAME_BODY_OFFSET_NED=9, /* Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. | */ + MAV_FRAME_GLOBAL_TERRAIN_ALT=10, /* Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */ + MAV_FRAME_GLOBAL_TERRAIN_ALT_INT=11, /* Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */ + MAV_FRAME_BODY_FRD=12, /* Body fixed frame of reference, Z-down (x: forward, y: right, z: down). | */ + MAV_FRAME_BODY_FLU=13, /* Body fixed frame of reference, Z-up (x: forward, y: left, z: up). | */ + MAV_FRAME_MOCAP_NED=14, /* Odometry local coordinate frame of data given by a motion capture system, Z-down (x: north, y: east, z: down). | */ + MAV_FRAME_MOCAP_ENU=15, /* Odometry local coordinate frame of data given by a motion capture system, Z-up (x: east, y: north, z: up). | */ + MAV_FRAME_VISION_NED=16, /* Odometry local coordinate frame of data given by a vision estimation system, Z-down (x: north, y: east, z: down). | */ + MAV_FRAME_VISION_ENU=17, /* Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: east, y: north, z: up). | */ + MAV_FRAME_ESTIM_NED=18, /* Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: north, y: east, z: down). | */ + MAV_FRAME_ESTIM_ENU=19, /* Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: east, y: noth, z: up). | */ + MAV_FRAME_LOCAL_FRD=20, /* Forward, Right, Down coordinate frame. This is a local frame with Z-down and arbitrary F/R alignment (i.e. not aligned with NED/earth frame). | */ + MAV_FRAME_LOCAL_FLU=21, /* Forward, Left, Up coordinate frame. This is a local frame with Z-up and arbitrary F/L alignment (i.e. not aligned with ENU/earth frame). | */ + MAV_FRAME_ENUM_END=22, /* | */ +} MAV_FRAME; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE +#define HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE +typedef enum MAVLINK_DATA_STREAM_TYPE +{ + MAVLINK_DATA_STREAM_IMG_JPEG=1, /* | */ + MAVLINK_DATA_STREAM_IMG_BMP=2, /* | */ + MAVLINK_DATA_STREAM_IMG_RAW8U=3, /* | */ + MAVLINK_DATA_STREAM_IMG_RAW32U=4, /* | */ + MAVLINK_DATA_STREAM_IMG_PGM=5, /* | */ + MAVLINK_DATA_STREAM_IMG_PNG=6, /* | */ + MAVLINK_DATA_STREAM_TYPE_ENUM_END=7, /* | */ +} MAVLINK_DATA_STREAM_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_FENCE_ACTION +#define HAVE_ENUM_FENCE_ACTION +typedef enum FENCE_ACTION +{ + FENCE_ACTION_NONE=0, /* Disable fenced mode | */ + FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ + FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ + FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ + FENCE_ACTION_RTL=4, /* Switch to RTL (return to launch) mode and head for the return point. | */ + FENCE_ACTION_ENUM_END=5, /* | */ +} FENCE_ACTION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_FENCE_BREACH +#define HAVE_ENUM_FENCE_BREACH +typedef enum FENCE_BREACH +{ + FENCE_BREACH_NONE=0, /* No last fence breach | */ + FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ + FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ + FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ + FENCE_BREACH_ENUM_END=4, /* | */ +} FENCE_BREACH; +#endif + +/** @brief Actions being taken to mitigate/prevent fence breach */ +#ifndef HAVE_ENUM_FENCE_MITIGATE +#define HAVE_ENUM_FENCE_MITIGATE +typedef enum FENCE_MITIGATE +{ + FENCE_MITIGATE_UNKNOWN=0, /* Unknown | */ + FENCE_MITIGATE_NONE=1, /* No actions being taken | */ + FENCE_MITIGATE_VEL_LIMIT=2, /* Velocity limiting active to prevent breach | */ + FENCE_MITIGATE_ENUM_END=3, /* | */ +} FENCE_MITIGATE; +#endif + +/** @brief Enumeration of possible mount operation modes */ +#ifndef HAVE_ENUM_MAV_MOUNT_MODE +#define HAVE_ENUM_MAV_MOUNT_MODE +typedef enum MAV_MOUNT_MODE +{ + MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ + MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ + MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ + MAV_MOUNT_MODE_ENUM_END=5, /* | */ +} MAV_MOUNT_MODE; +#endif + +/** @brief Generalized UAVCAN node health */ +#ifndef HAVE_ENUM_UAVCAN_NODE_HEALTH +#define HAVE_ENUM_UAVCAN_NODE_HEALTH +typedef enum UAVCAN_NODE_HEALTH +{ + UAVCAN_NODE_HEALTH_OK=0, /* The node is functioning properly. | */ + UAVCAN_NODE_HEALTH_WARNING=1, /* A critical parameter went out of range or the node has encountered a minor failure. | */ + UAVCAN_NODE_HEALTH_ERROR=2, /* The node has encountered a major failure. | */ + UAVCAN_NODE_HEALTH_CRITICAL=3, /* The node has suffered a fatal malfunction. | */ + UAVCAN_NODE_HEALTH_ENUM_END=4, /* | */ +} UAVCAN_NODE_HEALTH; +#endif + +/** @brief Generalized UAVCAN node mode */ +#ifndef HAVE_ENUM_UAVCAN_NODE_MODE +#define HAVE_ENUM_UAVCAN_NODE_MODE +typedef enum UAVCAN_NODE_MODE +{ + UAVCAN_NODE_MODE_OPERATIONAL=0, /* The node is performing its primary functions. | */ + UAVCAN_NODE_MODE_INITIALIZATION=1, /* The node is initializing; this mode is entered immediately after startup. | */ + UAVCAN_NODE_MODE_MAINTENANCE=2, /* The node is under maintenance. | */ + UAVCAN_NODE_MODE_SOFTWARE_UPDATE=3, /* The node is in the process of updating its software. | */ + UAVCAN_NODE_MODE_OFFLINE=7, /* The node is no longer available online. | */ + UAVCAN_NODE_MODE_ENUM_END=8, /* | */ +} UAVCAN_NODE_MODE; +#endif + +/** @brief Flags to indicate the status of camera storage. */ +#ifndef HAVE_ENUM_STORAGE_STATUS +#define HAVE_ENUM_STORAGE_STATUS +typedef enum STORAGE_STATUS +{ + STORAGE_STATUS_EMPTY=0, /* Storage is missing (no microSD card loaded for example.) | */ + STORAGE_STATUS_UNFORMATTED=1, /* Storage present but unformatted. | */ + STORAGE_STATUS_READY=2, /* Storage present and ready. | */ + STORAGE_STATUS_NOT_SUPPORTED=3, /* Camera does not supply storage status information. Capacity information in STORAGE_INFORMATION fields will be ignored. | */ + STORAGE_STATUS_ENUM_END=4, /* | */ +} STORAGE_STATUS; +#endif + +/** @brief Yaw behaviour during orbit flight. */ +#ifndef HAVE_ENUM_ORBIT_YAW_BEHAVIOUR +#define HAVE_ENUM_ORBIT_YAW_BEHAVIOUR +typedef enum ORBIT_YAW_BEHAVIOUR +{ + ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER=0, /* Vehicle front points to the center (default). | */ + ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING=1, /* Vehicle front holds heading when message received. | */ + ORBIT_YAW_BEHAVIOUR_UNCONTROLLED=2, /* Yaw uncontrolled. | */ + ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE=3, /* Vehicle front follows flight path (tangential to circle). | */ + ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED=4, /* Yaw controlled by RC input. | */ + ORBIT_YAW_BEHAVIOUR_ENUM_END=5, /* | */ +} ORBIT_YAW_BEHAVIOUR; +#endif + +/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. See https://mavlink.io/en/guide/xml_schema.html#MAV_CMD for information about the structure of the MAV_CMD entries */ +#ifndef HAVE_ENUM_MAV_CMD +#define HAVE_ENUM_MAV_CMD +typedef enum MAV_CMD +{ + MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. |Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached)| 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at waypoint (rotary wing). NaN for unchanged.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time |Empty| Empty| Radius around waypoint. If positive loiter clockwise, else counter-clockwise| Desired yaw angle. NaN for unchanged.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Number of turns.| Empty| Radius around waypoint. If positive loiter clockwise, else counter-clockwise| Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location. Else, this is desired yaw angle. NaN for unchanged.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this waypoint for X seconds |Loiter time.| Empty| Radius around waypoint. If positive loiter clockwise, else counter-clockwise.| Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location. Else, this is desired yaw angle. NaN for unchanged.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_LAND=21, /* Land at location. |Minimum target altitude if landing is aborted (0 = undefined/use system default).| Precision land mode.| Empty.| Desired yaw angle. NaN for unchanged.| Latitude.| Longitude.| Landing altitude (ground level in current frame).| */ + MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer. NaN for unchanged.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LAND_LOCAL=23, /* Land at local position (local frame only) |Landing target number (if available)| Maximum accepted offset from desired landing position - computed magnitude from spherical coordinates: d = sqrt(x^2 + y^2 + z^2), which gives the maximum accepted distance between the desired landing position and the position where the vehicle is about to land| Landing descend rate| Desired yaw angle| Y-axis position| X-axis position| Z-axis / ground level position| */ + MAV_CMD_NAV_TAKEOFF_LOCAL=24, /* Takeoff from local position (local frame only) |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Takeoff ascend rate| Yaw angle (if magnetometer or another yaw estimation source present), ignored without one of these| Y-axis position| X-axis position| Z-axis position| */ + MAV_CMD_NAV_FOLLOW=25, /* Vehicle following, i.e. this waypoint represents the position of a moving vehicle |Following logic to use (e.g. loitering or sinusoidal following) - depends on specific autopilot implementation| Ground speed of vehicle to be followed| Radius around waypoint. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT=30, /* Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. |Climb or Descend (0 = Neutral, command completes when within 5m of this command's altitude, 1 = Climbing, command completes when at or above this command's altitude, 2 = Descending, command completes when at or below this command's altitude.| Empty| Empty| Empty| Empty| Empty| Desired altitude| */ + MAV_CMD_NAV_LOITER_TO_ALT=31, /* Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. |Heading Required (0 = False)| Radius. If positive loiter clockwise, negative counter-clockwise, 0 means no change to standard loiter.| Empty| Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_FOLLOW=32, /* Begin following a target |System ID (of the FOLLOW_TARGET beacon). Send 0 to disable follow-me and return to the default position hold mode.| RESERVED| RESERVED| Altitude mode: 0: Keep current altitude, 1: keep altitude difference to target, 2: go to a fixed altitude above home.| Altitude above home. (used if mode=2)| RESERVED| Time to land in which the MAV should go to the default position hold mode after a message RX timeout.| */ + MAV_CMD_DO_FOLLOW_REPOSITION=33, /* Reposition the MAV after a follow target command has been sent |Camera q1 (where 0 is on the ray from the camera to the tracking device)| Camera q2| Camera q3| Camera q4| altitude offset from target| X offset from target| Y offset from target| */ + MAV_CMD_DO_ORBIT=34, /* Start orbiting on the circumference of a circle defined by the parameters. Setting any value NaN results in using defaults. |Radius of the circle. positive: Orbit clockwise. negative: Orbit counter-clockwise.| Tangential Velocity. NaN: Vehicle configuration default.| Yaw behavior of the vehicle.| Reserved (e.g. for dynamic center beacon options)| Center point latitude (if no MAV_FRAME specified) / X coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting.| Center point longitude (if no MAV_FRAME specified) / Y coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting.| Center point altitude (MSL) (if no MAV_FRAME specified) / Z coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting.| */ + MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode.| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to waypoint using a spline path. |Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_VTOL_TAKEOFF=84, /* Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. |Empty| Front transition heading.| Empty| Yaw angle. NaN for unchanged.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_VTOL_LAND=85, /* Land using VTOL mode |Empty| Empty| Approach altitude (with the same reference as the Altitude field). NaN if unspecified.| Yaw angle. NaN for unchanged.| Latitude| Longitude| Altitude (ground level)| */ + MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_DELAY=93, /* Delay the next navigation command a number of seconds or until a specified time |Delay (-1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| */ + MAV_CMD_NAV_PAYLOAD_PLACE=94, /* Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload. |Maximum distance to descend.| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate.| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance.| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle, 0 is north| angular speed| direction: -1: counter clockwise, 1: clockwise| 0: absolute angle, 1: relative offset| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Custom sub mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed, 2=Climb Speed, 3=Descent Speed)| Speed (-1 indicates no change)| Throttle (-1 indicates no change)| 0: absolute, 1: relative| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay instance number.| Setting. (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cycles with a desired period. |Relay instance number.| Cycle count.| Cycle time.| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo instance number.| Pulse Width Modulation.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo instance number.| Pulse Width Modulation.| Cycle count.| Cycle time.| Empty| Empty| Empty| */ + MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CHANGE_ALTITUDE=186, /* Change altitude set point. |Altitude.| Frame of new altitude.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_LAND_START=189, /* Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ + MAV_CMD_DO_RALLY_LAND=190, /* Mission command to perform a landing from a rally point. |Break altitude| Landing speed| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GO_AROUND=191, /* Mission command to safely abort an autonomous landing. |Altitude| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPOSITION=192, /* Reposition the vehicle to a specific WGS84 global position. |Ground speed, less than 0 (-1) for default| Bitmask of option flags.| Reserved| Yaw heading, NaN for unchanged. For planes indicates loiter direction (0: clockwise, 1: counter clockwise)| Latitude| Longitude| Altitude (meters)| */ + MAV_CMD_DO_PAUSE_CONTINUE=193, /* If in a GPS controlled position mode, hold the current position or continue. |0: Pause current mission or reposition command, hold current position. 1: Continue mission. A VTOL capable vehicle should enter hover mode (multicopter and VTOL planes). A plane should loiter with the default loiter radius.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_DO_SET_REVERSE=194, /* Set moving direction to forward or reverse. |Direction (0=Forward, 1=Reverse)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI_LOCATION=195, /* Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET=196, /* Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint| */ + MAV_CMD_DO_SET_ROI_NONE=197, /* Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode.| Waypoint index/ target ID (depends on param 1).| Region of interest index. (allows a vehicle to manage multiple ROI's)| Empty| MAV_ROI_WPNEXT: pitch offset from next waypoint, MAV_ROI_LOCATION: latitude| MAV_ROI_WPNEXT: roll offset from next waypoint, MAV_ROI_LOCATION: longitude| MAV_ROI_WPNEXT: yaw offset from next waypoint, MAV_ROI_LOCATION: altitude| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). |Modes: P, TV, AV, M, Etc.| Shutter speed: Divisor number for one second.| Aperture: F stop number.| ISO number e.g. 80, 100, 200, Etc.| Exposure type enumerator.| Command Identity.| Main engine cut-off time before camera trigger. (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Control digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Test shot identifier. If set to 1, image will only be captured, but not counted towards internal frame count.| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| roll input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| pitch input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| yaw input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch depending on mount mode (degrees or degrees/second depending on pitch input).| roll depending on mount mode (degrees or degrees/second depending on roll input).| yaw depending on mount mode (degrees or degrees/second depending on yaw input).| altitude depending on mount mode.| latitude, set if appropriate mount mode.| longitude, set if appropriate mount mode.| Mount mode.| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera. |Camera trigger distance. 0 to stop triggering.| Camera shutter integration time. -1 or 0 to ignore| Trigger camera once immediately. (0 = no trigger, 1 = trigger)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable, 2=disable_floor_only)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test. |Motor instance number. (from 1 to max number of motors on the vehicle)| Throttle type.| Throttle.| Timeout.| Motor count. (number of motors to test to test in sequence, waiting for the timeout above between them; 0=1 motor, 1=1 motor, 2=2 motors...)| Motor test order.| Empty| */ + MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight. |Inverted flight. (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_SET_YAW_SPEED=213, /* Sets a desired vehicle turn angle and speed change. |Yaw angle to adjust steering by.| Speed.| Final angle. (0=absolute, 1=relative)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL=214, /* Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. |Camera trigger cycle time. -1 or 0 to ignore.| Camera shutter integration time. Should be less than trigger cycle time. -1 or 0 to ignore.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |quaternion param q1, w (1 in null-rotation)| quaternion param q2, x (0 in null-rotation)| quaternion param q3, y (0 in null-rotation)| quaternion param q4, z (0 in null-rotation)| Empty| Empty| Empty| */ + MAV_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GUIDED_LIMITS=222, /* Set limits for external control |Timeout - maximum time that external controller will be allowed to control vehicle. 0 means no timeout.| Altitude (MSL) min - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit.| Altitude (MSL) max - if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit.| Horizontal move limit - if vehicle moves more than this distance from its location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal move limit.| Empty| Empty| Empty| */ + MAV_CMD_DO_ENGINE_CONTROL=223, /* Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines |0: Stop engine, 1:Start Engine| 0: Warm start, 1:Cold start. Controls use of choke where applicable| Height delay. This is for commanding engine start only after the vehicle has gained the specified height. Used in VTOL vehicles during takeoff to start engine after the aircraft is off the ground. Zero for no delay.| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MISSION_CURRENT=224, /* Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). |Mission sequence value to set| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. |1: gyro calibration, 3: gyro temperature calibration| 1: magnetometer calibration| 1: ground pressure calibration| 1: radio RC calibration, 2: RC trim calibration| 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration| 1: APM: compass/motor interference calibration (PX4: airspeed calibration, deprecated), 2: airspeed calibration| 1: ESC calibration, 3: barometer temperature calibration| */ + MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer, 6: third magnetometer| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + MAV_CMD_PREFLIGHT_UAVCAN=243, /* Trigger UAVCAN config. This command will be only accepted if in pre-flight mode. |1: Trigger actuator ID assignment and direction mapping.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: logging rate (e.g. set to 1000 for 1000 Hz logging)| Reserved| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot, 3: Reboot autopilot and keep it in the bootloader until upgraded.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer, 3: Reboot onboard computer and keep it in the bootloader until upgraded.| WIP: 0: Do nothing for camera, 1: Reboot onboard camera, 2: Shutdown onboard camera, 3: Reboot onboard camera and keep it in the bootloader until upgraded| WIP: 0: Do nothing for mount (e.g. gimbal), 1: Reboot mount, 2: Shutdown mount, 3: Reboot mount and keep it in the bootloader until upgraded| Reserved, send 0| Reserved, send 0| WIP: ID (e.g. camera ID -1 for all IDs)| */ + MAV_CMD_OVERRIDE_GOTO=252, /* Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position. |MAV_GOTO_DO_HOLD: pause mission and either hold or move to specified position (depending on param2), MAV_GOTO_DO_CONTINUE: resume mission.| MAV_GOTO_HOLD_AT_CURRENT_POSITION: hold at current position, MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position.| Coordinate frame of hold point.| Desired yaw angle.| Latitude / X position.| Longitude / Y position.| Altitude / Z position.| */ + MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |0: disarm, 1: arm| 0: arm-disarm unless prevented by safety checks (i.e. when landed), 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ILLUMINATOR_ON_OFF=405, /* Turns illuminators ON/OFF. An illuminator is a light source that is used for lighting up dark areas external to the sytstem: e.g. a torch or searchlight (as opposed to a light source for illuminating the system itself, e.g. an indicator light). |0: Illuminators OFF, 1: Illuminators ON| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_GET_HOME_POSITION=410, /* Request the home position from the vehicle. |Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing. |0:Spektrum.| RC type.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_GET_MESSAGE_INTERVAL=510, /* Request the interval between messages for a particular MAVLink message ID. The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message. |The MAVLink message ID| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_MESSAGE_INTERVAL=511, /* Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM. |The MAVLink message ID| The interval between two messages. Set to -1 to disable and 0 to request default rate.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Target address of message stream (if message has target address fields). 0: Flight-stack default (recommended), 1: address of requestor, 2: broadcast.| */ + MAV_CMD_REQUEST_MESSAGE=512, /* Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). |The MAVLink message ID of the requested message.| Index id (if appropriate). The use of this parameter (if any), must be defined in the requested message.| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| Target address for requested message (if message has target address fields). 0: Flight-stack default, 1: address of requestor, 2: broadcast.| */ + MAV_CMD_REQUEST_PROTOCOL_VERSION=519, /* Request MAVLink protocol version compatibility |1: Request supported protocol versions by all nodes on the network| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES=520, /* Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message |1: Request autopilot version| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_CAMERA_INFORMATION=521, /* Request camera information (CAMERA_INFORMATION). |0: No action 1: Request camera capabilities| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_CAMERA_SETTINGS=522, /* Request camera settings (CAMERA_SETTINGS). |0: No Action 1: Request camera settings| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_STORAGE_INFORMATION=525, /* Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage. |Storage ID (0 for all, 1 for first, 2 for second, etc.)| 0: No Action 1: Request storage information| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_STORAGE_FORMAT=526, /* Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage. |Storage ID (1 for first, 2 for second, etc.)| 0: No action 1: Format storage| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS=527, /* Request camera capture status (CAMERA_CAPTURE_STATUS) |0: No Action 1: Request camera capture status| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_FLIGHT_INFORMATION=528, /* Request flight information (FLIGHT_INFORMATION) |1: Request flight information| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_RESET_CAMERA_SETTINGS=529, /* Reset all camera settings to Factory Default |0: No Action 1: Reset all settings| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_CAMERA_MODE=530, /* Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming. |Reserved (Set to 0)| Camera mode| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_CAMERA_ZOOM=531, /* Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). Use NaN for reserved values. |Zoom type| Zoom value. The range of valid values depend on the zoom type.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_CAMERA_FOCUS=532, /* Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). Use NaN for reserved values. |Focus type| Focus value| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_JUMP_TAG=600, /* Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. |Tag.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_JUMP_TAG=601, /* Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. |Target tag to jump to.| Repeat count.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. |Reserved (Set to 0)| Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds).| Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE.| Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1). Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted. Use 0 to ignore it.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* Stop image capture sequence Use NaN for reserved values. |Reserved (Set to 0)| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE=2002, /* Re-request a CAMERA_IMAGE_CAPTURE message. Use NaN for reserved values. |Sequence number for missing CAMERA_IMAGE_CAPTURE message| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_TRIGGER_CONTROL=2003, /* Enable or disable on-board camera triggering system. |Trigger enable/disable (0 for disable, 1 for start), -1 to ignore| 1 to reset the trigger sequence, -1 or 0 to ignore| 1 to pause triggering, but without switching the camera off or retracting it. -1 to ignore| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_VIDEO_START_CAPTURE=2500, /* Starts video capture (recording). Use NaN for reserved values. |Video Stream ID (0 for all streams)| Frequency CAMERA_CAPTURE_STATUS messages should be sent while recording (0 for no messages, otherwise frequency)| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* Stop the current video capture (recording). Use NaN for reserved values. |Video Stream ID (0 for all streams)| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_VIDEO_START_STREAMING=2502, /* Start video streaming |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Reserved| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_VIDEO_STOP_STREAMING=2503, /* Stop the given video stream |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Reserved| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION=2504, /* Request video stream information (VIDEO_STREAM_INFORMATION) |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_VIDEO_STREAM_STATUS=2505, /* Request video stream status (VIDEO_STREAM_STATUS) |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_LOGGING_START=2510, /* Request to start streaming logging data over MAVLink (see also LOGGING_DATA message) |Format: 0: ULog| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ + MAV_CMD_LOGGING_STOP=2511, /* Request to stop streaming log data over MAVLink |Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ + MAV_CMD_AIRFRAME_CONFIGURATION=2520, /* |Landing gear ID (default: 0, -1 for all)| Landing gear position (Down: 0, Up: 1, NaN for no change)| Reserved, set to NaN| Reserved, set to NaN| Reserved, set to NaN| Reserved, set to NaN| Reserved, set to NaN| */ + MAV_CMD_CONTROL_HIGH_LATENCY=2600, /* Request to start/stop transmitting over the high latency telemetry |Control transmission over high latency telemetry (0: stop, 1: start)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_PANORAMA_CREATE=2800, /* Create a panorama at the current position |Viewing angle horizontal of the panorama (+- 0.5 the total angle)| Viewing angle vertical of panorama.| Speed of the horizontal rotation.| Speed of the vertical rotation.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_VTOL_TRANSITION=3000, /* Request VTOL transition |The target VTOL state. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ARM_AUTHORIZATION_REQUEST=3001, /* Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. + |Vehicle system id, this way ground station can request arm authorization on behalf of any vehicle| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_GUIDED_SUBMODE_STANDARD=4000, /* This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes. + |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE=4001, /* This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. + |Radius of desired circle in CIRCLE_MODE| User defined| User defined| User defined| Unscaled target latitude of center of circle in CIRCLE_MODE| Unscaled target longitude of center of circle in CIRCLE_MODE| Reserved (default:0)| */ + MAV_CMD_CONDITION_GATE=4501, /* Delay mission state machine until gate has been reached. |Geometry: 0: orthogonal to path between previous and next waypoint.| Altitude: 0: ignore altitude| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_FENCE_RETURN_POINT=5000, /* Fence return point. There can only be one fence return point. + |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION=5001, /* Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. + |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION=5002, /* Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. + |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION=5003, /* Circular fence area. The vehicle must stay inside this area. + |Radius.| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION=5004, /* Circular fence area. The vehicle must stay outside this area. + |Radius.| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + MAV_CMD_NAV_RALLY_POINT=5100, /* Rally point. You can have multiple rally points defined. + |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ + MAV_CMD_UAVCAN_GET_NODE_INFO=5200, /* Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages. |Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ + MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in compass heading. A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overridden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position. A negative value indicates the system can define the clearance at will.| Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Altitude (MSL), in meters| */ + MAV_CMD_PAYLOAD_CONTROL_DEPLOY=30002, /* Control the payload deployment. |Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deployment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_WAYPOINT_USER_1=31000, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_WAYPOINT_USER_2=31001, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_WAYPOINT_USER_3=31002, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_WAYPOINT_USER_4=31003, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_WAYPOINT_USER_5=31004, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_SPATIAL_USER_1=31005, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_SPATIAL_USER_2=31006, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_SPATIAL_USER_3=31007, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_SPATIAL_USER_4=31008, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_SPATIAL_USER_5=31009, /* User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL), in meters| */ + MAV_CMD_USER_1=31010, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ + MAV_CMD_USER_2=31011, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ + MAV_CMD_USER_3=31012, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ + MAV_CMD_USER_4=31013, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ + MAV_CMD_USER_5=31014, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ + MAV_CMD_ENUM_END=31015, /* | */ +} MAV_CMD; +#endif + +/** @brief A data stream is not a fixed set of messages, but rather a + recommendation to the autopilot software. Individual autopilots may or may not obey + the recommended messages. */ +#ifndef HAVE_ENUM_MAV_DATA_STREAM +#define HAVE_ENUM_MAV_DATA_STREAM +typedef enum MAV_DATA_STREAM +{ + MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ + MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ + MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ + MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ + MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ + MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ + MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_ENUM_END=13, /* | */ +} MAV_DATA_STREAM; +#endif + +/** @brief The ROI (region of interest) for the vehicle. This can be + be used by the vehicle for camera/vehicle attitude alignment (see + MAV_CMD_NAV_ROI). */ +#ifndef HAVE_ENUM_MAV_ROI +#define HAVE_ENUM_MAV_ROI +typedef enum MAV_ROI +{ + MAV_ROI_NONE=0, /* No region of interest. | */ + MAV_ROI_WPNEXT=1, /* Point toward next waypoint, with optional pitch/roll/yaw offset. | */ + MAV_ROI_WPINDEX=2, /* Point toward given waypoint. | */ + MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ + MAV_ROI_TARGET=4, /* Point toward of given id. | */ + MAV_ROI_ENUM_END=5, /* | */ +} MAV_ROI; +#endif + +/** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */ +#ifndef HAVE_ENUM_MAV_CMD_ACK +#define HAVE_ENUM_MAV_CMD_ACK +typedef enum MAV_CMD_ACK +{ + MAV_CMD_ACK_OK=1, /* Command / mission item is ok. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_ACCESS_DENIED=3, /* The system is refusing to accept this command from this source / communication partner. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_NOT_SUPPORTED=4, /* Command or mission item is not supported, other commands would be accepted. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=5, /* The coordinate frame of this command / mission item is not supported. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=6, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=7, /* The X or latitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ENUM_END=10, /* | */ +} MAV_CMD_ACK; +#endif + +/** @brief Specifies the datatype of a MAVLink parameter. */ +#ifndef HAVE_ENUM_MAV_PARAM_TYPE +#define HAVE_ENUM_MAV_PARAM_TYPE +typedef enum MAV_PARAM_TYPE +{ + MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */ + MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */ + MAV_PARAM_TYPE_UINT16=3, /* 16-bit unsigned integer | */ + MAV_PARAM_TYPE_INT16=4, /* 16-bit signed integer | */ + MAV_PARAM_TYPE_UINT32=5, /* 32-bit unsigned integer | */ + MAV_PARAM_TYPE_INT32=6, /* 32-bit signed integer | */ + MAV_PARAM_TYPE_UINT64=7, /* 64-bit unsigned integer | */ + MAV_PARAM_TYPE_INT64=8, /* 64-bit signed integer | */ + MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */ + MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */ + MAV_PARAM_TYPE_ENUM_END=11, /* | */ +} MAV_PARAM_TYPE; +#endif + +/** @brief Specifies the datatype of a MAVLink extended parameter. */ +#ifndef HAVE_ENUM_MAV_PARAM_EXT_TYPE +#define HAVE_ENUM_MAV_PARAM_EXT_TYPE +typedef enum MAV_PARAM_EXT_TYPE +{ + MAV_PARAM_EXT_TYPE_UINT8=1, /* 8-bit unsigned integer | */ + MAV_PARAM_EXT_TYPE_INT8=2, /* 8-bit signed integer | */ + MAV_PARAM_EXT_TYPE_UINT16=3, /* 16-bit unsigned integer | */ + MAV_PARAM_EXT_TYPE_INT16=4, /* 16-bit signed integer | */ + MAV_PARAM_EXT_TYPE_UINT32=5, /* 32-bit unsigned integer | */ + MAV_PARAM_EXT_TYPE_INT32=6, /* 32-bit signed integer | */ + MAV_PARAM_EXT_TYPE_UINT64=7, /* 64-bit unsigned integer | */ + MAV_PARAM_EXT_TYPE_INT64=8, /* 64-bit signed integer | */ + MAV_PARAM_EXT_TYPE_REAL32=9, /* 32-bit floating-point | */ + MAV_PARAM_EXT_TYPE_REAL64=10, /* 64-bit floating-point | */ + MAV_PARAM_EXT_TYPE_CUSTOM=11, /* Custom Type | */ + MAV_PARAM_EXT_TYPE_ENUM_END=12, /* | */ +} MAV_PARAM_EXT_TYPE; +#endif + +/** @brief Result from a MAVLink command (MAV_CMD) */ +#ifndef HAVE_ENUM_MAV_RESULT +#define HAVE_ENUM_MAV_RESULT +typedef enum MAV_RESULT +{ + MAV_RESULT_ACCEPTED=0, /* Command is valid (is supported and has valid parameters), and was executed. | */ + MAV_RESULT_TEMPORARILY_REJECTED=1, /* Command is valid, but cannot be executed at this time. This is used to indicate a problem that should be fixed just by waiting (e.g. a state machine is busy, can't arm because have not got GPS lock, etc.). Retrying later should work. | */ + MAV_RESULT_DENIED=2, /* Command is invalid (is supported but has invalid parameters). Retrying same command and parameters will not work. | */ + MAV_RESULT_UNSUPPORTED=3, /* Command is not supported (unknown). | */ + MAV_RESULT_FAILED=4, /* Command is valid, but execution has failed. This is used to indicate any non-temporary or unexpected problem, i.e. any problem that must be fixed before the command can succeed/be retried. For example, attempting to write a file when out of memory, attempting to arm when sensors are not calibrated, etc. | */ + MAV_RESULT_IN_PROGRESS=5, /* Command is valid and is being executed. This will be followed by further progress updates, i.e. the component may send further COMMAND_ACK messages with result MAV_RESULT_IN_PROGRESS (at a rate decided by the implementation), and must terminate by sending a COMMAND_ACK message with final result of the operation. The COMMAND_ACK.progress field can be used to indicate the progress of the operation. There is no need for the sender to retry the command, but if done during execution, the component will return MAV_RESULT_IN_PROGRESS with an updated progress. | */ + MAV_RESULT_ENUM_END=6, /* | */ +} MAV_RESULT; +#endif + +/** @brief Result of mission operation (in a MISSION_ACK message). */ +#ifndef HAVE_ENUM_MAV_MISSION_RESULT +#define HAVE_ENUM_MAV_MISSION_RESULT +typedef enum MAV_MISSION_RESULT +{ + MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */ + MAV_MISSION_ERROR=1, /* Generic error / not accepting mission commands at all right now. | */ + MAV_MISSION_UNSUPPORTED_FRAME=2, /* Coordinate frame is not supported. | */ + MAV_MISSION_UNSUPPORTED=3, /* Command is not supported. | */ + MAV_MISSION_NO_SPACE=4, /* Mission item exceeds storage space. | */ + MAV_MISSION_INVALID=5, /* One of the parameters has an invalid value. | */ + MAV_MISSION_INVALID_PARAM1=6, /* param1 has an invalid value. | */ + MAV_MISSION_INVALID_PARAM2=7, /* param2 has an invalid value. | */ + MAV_MISSION_INVALID_PARAM3=8, /* param3 has an invalid value. | */ + MAV_MISSION_INVALID_PARAM4=9, /* param4 has an invalid value. | */ + MAV_MISSION_INVALID_PARAM5_X=10, /* x / param5 has an invalid value. | */ + MAV_MISSION_INVALID_PARAM6_Y=11, /* y / param6 has an invalid value. | */ + MAV_MISSION_INVALID_PARAM7=12, /* z / param7 has an invalid value. | */ + MAV_MISSION_INVALID_SEQUENCE=13, /* Mission item received out of sequence | */ + MAV_MISSION_DENIED=14, /* Not accepting any mission commands from this communication partner. | */ + MAV_MISSION_OPERATION_CANCELLED=15, /* Current mission operation cancelled (e.g. mission upload, mission download). | */ + MAV_MISSION_RESULT_ENUM_END=16, /* | */ +} MAV_MISSION_RESULT; +#endif + +/** @brief Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. */ +#ifndef HAVE_ENUM_MAV_SEVERITY +#define HAVE_ENUM_MAV_SEVERITY +typedef enum MAV_SEVERITY +{ + MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */ + MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */ + MAV_SEVERITY_CRITICAL=2, /* Action must be taken immediately. Indicates failure in a primary system. | */ + MAV_SEVERITY_ERROR=3, /* Indicates an error in secondary/redundant systems. | */ + MAV_SEVERITY_WARNING=4, /* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */ + MAV_SEVERITY_NOTICE=5, /* An unusual event has occurred, though not an error condition. This should be investigated for the root cause. | */ + MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */ + MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */ + MAV_SEVERITY_ENUM_END=8, /* | */ +} MAV_SEVERITY; +#endif + +/** @brief Power supply status flags (bitmask) */ +#ifndef HAVE_ENUM_MAV_POWER_STATUS +#define HAVE_ENUM_MAV_POWER_STATUS +typedef enum MAV_POWER_STATUS +{ + MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */ + MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */ + MAV_POWER_STATUS_USB_CONNECTED=4, /* USB power is connected | */ + MAV_POWER_STATUS_PERIPH_OVERCURRENT=8, /* peripheral supply is in over-current state | */ + MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */ + MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */ + MAV_POWER_STATUS_ENUM_END=33, /* | */ +} MAV_POWER_STATUS; +#endif + +/** @brief SERIAL_CONTROL device types */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_DEV +#define HAVE_ENUM_SERIAL_CONTROL_DEV +typedef enum SERIAL_CONTROL_DEV +{ + SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */ + SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */ + SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */ + SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */ + SERIAL_CONTROL_DEV_SHELL=10, /* system shell | */ + SERIAL_CONTROL_DEV_ENUM_END=11, /* | */ +} SERIAL_CONTROL_DEV; +#endif + +/** @brief SERIAL_CONTROL flags (bitmask) */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG +#define HAVE_ENUM_SERIAL_CONTROL_FLAG +typedef enum SERIAL_CONTROL_FLAG +{ + SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */ + SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */ + SERIAL_CONTROL_FLAG_EXCLUSIVE=4, /* Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set | */ + SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */ + SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */ + SERIAL_CONTROL_FLAG_ENUM_END=17, /* | */ +} SERIAL_CONTROL_FLAG; +#endif + +/** @brief Enumeration of distance sensor types */ +#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR +#define HAVE_ENUM_MAV_DISTANCE_SENSOR +typedef enum MAV_DISTANCE_SENSOR +{ + MAV_DISTANCE_SENSOR_LASER=0, /* Laser rangefinder, e.g. LightWare SF02/F or PulsedLight units | */ + MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Ultrasound rangefinder, e.g. MaxBotix units | */ + MAV_DISTANCE_SENSOR_INFRARED=2, /* Infrared rangefinder, e.g. Sharp units | */ + MAV_DISTANCE_SENSOR_RADAR=3, /* Radar type, e.g. uLanding units | */ + MAV_DISTANCE_SENSOR_UNKNOWN=4, /* Broken or unknown type, e.g. analog units | */ + MAV_DISTANCE_SENSOR_ENUM_END=5, /* | */ +} MAV_DISTANCE_SENSOR; +#endif + +/** @brief Enumeration of sensor orientation, according to its rotations */ +#ifndef HAVE_ENUM_MAV_SENSOR_ORIENTATION +#define HAVE_ENUM_MAV_SENSOR_ORIENTATION +typedef enum MAV_SENSOR_ORIENTATION +{ + MAV_SENSOR_ROTATION_NONE=0, /* Roll: 0, Pitch: 0, Yaw: 0 | */ + MAV_SENSOR_ROTATION_YAW_45=1, /* Roll: 0, Pitch: 0, Yaw: 45 | */ + MAV_SENSOR_ROTATION_YAW_90=2, /* Roll: 0, Pitch: 0, Yaw: 90 | */ + MAV_SENSOR_ROTATION_YAW_135=3, /* Roll: 0, Pitch: 0, Yaw: 135 | */ + MAV_SENSOR_ROTATION_YAW_180=4, /* Roll: 0, Pitch: 0, Yaw: 180 | */ + MAV_SENSOR_ROTATION_YAW_225=5, /* Roll: 0, Pitch: 0, Yaw: 225 | */ + MAV_SENSOR_ROTATION_YAW_270=6, /* Roll: 0, Pitch: 0, Yaw: 270 | */ + MAV_SENSOR_ROTATION_YAW_315=7, /* Roll: 0, Pitch: 0, Yaw: 315 | */ + MAV_SENSOR_ROTATION_ROLL_180=8, /* Roll: 180, Pitch: 0, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_45=9, /* Roll: 180, Pitch: 0, Yaw: 45 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_90=10, /* Roll: 180, Pitch: 0, Yaw: 90 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_135=11, /* Roll: 180, Pitch: 0, Yaw: 135 | */ + MAV_SENSOR_ROTATION_PITCH_180=12, /* Roll: 0, Pitch: 180, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_225=13, /* Roll: 180, Pitch: 0, Yaw: 225 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_270=14, /* Roll: 180, Pitch: 0, Yaw: 270 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_315=15, /* Roll: 180, Pitch: 0, Yaw: 315 | */ + MAV_SENSOR_ROTATION_ROLL_90=16, /* Roll: 90, Pitch: 0, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_90_YAW_45=17, /* Roll: 90, Pitch: 0, Yaw: 45 | */ + MAV_SENSOR_ROTATION_ROLL_90_YAW_90=18, /* Roll: 90, Pitch: 0, Yaw: 90 | */ + MAV_SENSOR_ROTATION_ROLL_90_YAW_135=19, /* Roll: 90, Pitch: 0, Yaw: 135 | */ + MAV_SENSOR_ROTATION_ROLL_270=20, /* Roll: 270, Pitch: 0, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_270_YAW_45=21, /* Roll: 270, Pitch: 0, Yaw: 45 | */ + MAV_SENSOR_ROTATION_ROLL_270_YAW_90=22, /* Roll: 270, Pitch: 0, Yaw: 90 | */ + MAV_SENSOR_ROTATION_ROLL_270_YAW_135=23, /* Roll: 270, Pitch: 0, Yaw: 135 | */ + MAV_SENSOR_ROTATION_PITCH_90=24, /* Roll: 0, Pitch: 90, Yaw: 0 | */ + MAV_SENSOR_ROTATION_PITCH_270=25, /* Roll: 0, Pitch: 270, Yaw: 0 | */ + MAV_SENSOR_ROTATION_PITCH_180_YAW_90=26, /* Roll: 0, Pitch: 180, Yaw: 90 | */ + MAV_SENSOR_ROTATION_PITCH_180_YAW_270=27, /* Roll: 0, Pitch: 180, Yaw: 270 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_90=28, /* Roll: 90, Pitch: 90, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_180_PITCH_90=29, /* Roll: 180, Pitch: 90, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_270_PITCH_90=30, /* Roll: 270, Pitch: 90, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_180=31, /* Roll: 90, Pitch: 180, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_270_PITCH_180=32, /* Roll: 270, Pitch: 180, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_270=33, /* Roll: 90, Pitch: 270, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_180_PITCH_270=34, /* Roll: 180, Pitch: 270, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_270_PITCH_270=35, /* Roll: 270, Pitch: 270, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_180_YAW_90=36, /* Roll: 90, Pitch: 180, Yaw: 90 | */ + MAV_SENSOR_ROTATION_ROLL_90_YAW_270=37, /* Roll: 90, Pitch: 0, Yaw: 270 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293=38, /* Roll: 90, Pitch: 68, Yaw: 293 | */ + MAV_SENSOR_ROTATION_PITCH_315=39, /* Pitch: 315 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_315=40, /* Roll: 90, Pitch: 315 | */ + MAV_SENSOR_ROTATION_CUSTOM=100, /* Custom orientation | */ + MAV_SENSOR_ORIENTATION_ENUM_END=101, /* | */ +} MAV_SENSOR_ORIENTATION; +#endif + +/** @brief Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot supports this capability. */ +#ifndef HAVE_ENUM_MAV_PROTOCOL_CAPABILITY +#define HAVE_ENUM_MAV_PROTOCOL_CAPABILITY +typedef enum MAV_PROTOCOL_CAPABILITY +{ + MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT=1, /* Autopilot supports MISSION float message type. | */ + MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT=2, /* Autopilot supports the new param float message type. | */ + MAV_PROTOCOL_CAPABILITY_MISSION_INT=4, /* Autopilot supports MISSION_INT scaled integer message type. | */ + MAV_PROTOCOL_CAPABILITY_COMMAND_INT=8, /* Autopilot supports COMMAND_INT scaled integer message type. | */ + MAV_PROTOCOL_CAPABILITY_PARAM_UNION=16, /* Autopilot supports the new param union message type. | */ + MAV_PROTOCOL_CAPABILITY_FTP=32, /* Autopilot supports the new FILE_TRANSFER_PROTOCOL message type. | */ + MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET=64, /* Autopilot supports commanding attitude offboard. | */ + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED=128, /* Autopilot supports commanding position and velocity targets in local NED frame. | */ + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT=256, /* Autopilot supports commanding position and velocity targets in global scaled integers. | */ + MAV_PROTOCOL_CAPABILITY_TERRAIN=512, /* Autopilot supports terrain protocol / data handling. | */ + MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET=1024, /* Autopilot supports direct actuator control. | */ + MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION=2048, /* Autopilot supports the flight termination command. | */ + MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION=4096, /* Autopilot supports onboard compass calibration. | */ + MAV_PROTOCOL_CAPABILITY_MAVLINK2=8192, /* Autopilot supports MAVLink version 2. | */ + MAV_PROTOCOL_CAPABILITY_MISSION_FENCE=16384, /* Autopilot supports mission fence protocol. | */ + MAV_PROTOCOL_CAPABILITY_MISSION_RALLY=32768, /* Autopilot supports mission rally point protocol. | */ + MAV_PROTOCOL_CAPABILITY_FLIGHT_INFORMATION=65536, /* Autopilot supports the flight information protocol. | */ + MAV_PROTOCOL_CAPABILITY_ENUM_END=65537, /* | */ +} MAV_PROTOCOL_CAPABILITY; +#endif + +/** @brief Type of mission items being requested/sent in mission protocol. */ +#ifndef HAVE_ENUM_MAV_MISSION_TYPE +#define HAVE_ENUM_MAV_MISSION_TYPE +typedef enum MAV_MISSION_TYPE +{ + MAV_MISSION_TYPE_MISSION=0, /* Items are mission commands for main mission. | */ + MAV_MISSION_TYPE_FENCE=1, /* Specifies GeoFence area(s). Items are MAV_CMD_NAV_FENCE_ GeoFence items. | */ + MAV_MISSION_TYPE_RALLY=2, /* Specifies the rally points for the vehicle. Rally points are alternative RTL points. Items are MAV_CMD_NAV_RALLY_POINT rally point items. | */ + MAV_MISSION_TYPE_ALL=255, /* Only used in MISSION_CLEAR_ALL to clear all mission types. | */ + MAV_MISSION_TYPE_ENUM_END=256, /* | */ +} MAV_MISSION_TYPE; +#endif + +/** @brief Enumeration of estimator types */ +#ifndef HAVE_ENUM_MAV_ESTIMATOR_TYPE +#define HAVE_ENUM_MAV_ESTIMATOR_TYPE +typedef enum MAV_ESTIMATOR_TYPE +{ + MAV_ESTIMATOR_TYPE_UNKNOWN=0, /* Unknown type of the estimator. | */ + MAV_ESTIMATOR_TYPE_NAIVE=1, /* This is a naive estimator without any real covariance feedback. | */ + MAV_ESTIMATOR_TYPE_VISION=2, /* Computer vision based estimate. Might be up to scale. | */ + MAV_ESTIMATOR_TYPE_VIO=3, /* Visual-inertial estimate. | */ + MAV_ESTIMATOR_TYPE_GPS=4, /* Plain GPS estimate. | */ + MAV_ESTIMATOR_TYPE_GPS_INS=5, /* Estimator integrating GPS and inertial sensing. | */ + MAV_ESTIMATOR_TYPE_MOCAP=6, /* Estimate from external motion capturing system. | */ + MAV_ESTIMATOR_TYPE_LIDAR=7, /* Estimator based on lidar sensor input. | */ + MAV_ESTIMATOR_TYPE_AUTOPILOT=8, /* Estimator on autopilot. | */ + MAV_ESTIMATOR_TYPE_ENUM_END=9, /* | */ +} MAV_ESTIMATOR_TYPE; +#endif + +/** @brief Enumeration of battery types */ +#ifndef HAVE_ENUM_MAV_BATTERY_TYPE +#define HAVE_ENUM_MAV_BATTERY_TYPE +typedef enum MAV_BATTERY_TYPE +{ + MAV_BATTERY_TYPE_UNKNOWN=0, /* Not specified. | */ + MAV_BATTERY_TYPE_LIPO=1, /* Lithium polymer battery | */ + MAV_BATTERY_TYPE_LIFE=2, /* Lithium-iron-phosphate battery | */ + MAV_BATTERY_TYPE_LION=3, /* Lithium-ION battery | */ + MAV_BATTERY_TYPE_NIMH=4, /* Nickel metal hydride battery | */ + MAV_BATTERY_TYPE_ENUM_END=5, /* | */ +} MAV_BATTERY_TYPE; +#endif + +/** @brief Enumeration of battery functions */ +#ifndef HAVE_ENUM_MAV_BATTERY_FUNCTION +#define HAVE_ENUM_MAV_BATTERY_FUNCTION +typedef enum MAV_BATTERY_FUNCTION +{ + MAV_BATTERY_FUNCTION_UNKNOWN=0, /* Battery function is unknown | */ + MAV_BATTERY_FUNCTION_ALL=1, /* Battery supports all flight systems | */ + MAV_BATTERY_FUNCTION_PROPULSION=2, /* Battery for the propulsion system | */ + MAV_BATTERY_FUNCTION_AVIONICS=3, /* Avionics battery | */ + MAV_BATTERY_TYPE_PAYLOAD=4, /* Payload battery | */ + MAV_BATTERY_FUNCTION_ENUM_END=5, /* | */ +} MAV_BATTERY_FUNCTION; +#endif + +/** @brief Enumeration for battery charge states. */ +#ifndef HAVE_ENUM_MAV_BATTERY_CHARGE_STATE +#define HAVE_ENUM_MAV_BATTERY_CHARGE_STATE +typedef enum MAV_BATTERY_CHARGE_STATE +{ + MAV_BATTERY_CHARGE_STATE_UNDEFINED=0, /* Low battery state is not provided | */ + MAV_BATTERY_CHARGE_STATE_OK=1, /* Battery is not in low state. Normal operation. | */ + MAV_BATTERY_CHARGE_STATE_LOW=2, /* Battery state is low, warn and monitor close. | */ + MAV_BATTERY_CHARGE_STATE_CRITICAL=3, /* Battery state is critical, return or abort immediately. | */ + MAV_BATTERY_CHARGE_STATE_EMERGENCY=4, /* Battery state is too low for ordinary abort sequence. Perform fastest possible emergency stop to prevent damage. | */ + MAV_BATTERY_CHARGE_STATE_FAILED=5, /* Battery failed, damage unavoidable. | */ + MAV_BATTERY_CHARGE_STATE_UNHEALTHY=6, /* Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. | */ + MAV_BATTERY_CHARGE_STATE_CHARGING=7, /* Battery is charging. | */ + MAV_BATTERY_CHARGE_STATE_ENUM_END=8, /* | */ +} MAV_BATTERY_CHARGE_STATE; +#endif + +/** @brief Smart battery supply status/fault flags (bitmask) for health indication. */ +#ifndef HAVE_ENUM_MAV_SMART_BATTERY_FAULT +#define HAVE_ENUM_MAV_SMART_BATTERY_FAULT +typedef enum MAV_SMART_BATTERY_FAULT +{ + MAV_SMART_BATTERY_FAULT_DEEP_DISCHARGE=1, /* Battery has deep discharged. | */ + MAV_SMART_BATTERY_FAULT_SPIKES=2, /* Voltage spikes. | */ + MAV_SMART_BATTERY_FAULT_SINGLE_CELL_FAIL=4, /* Single cell has failed. | */ + MAV_SMART_BATTERY_FAULT_OVER_CURRENT=8, /* Over-current fault. | */ + MAV_SMART_BATTERY_FAULT_OVER_TEMPERATURE=16, /* Over-temperature fault. | */ + MAV_SMART_BATTERY_FAULT_UNDER_TEMPERATURE=32, /* Under-temperature fault. | */ + MAV_SMART_BATTERY_FAULT_ENUM_END=33, /* | */ +} MAV_SMART_BATTERY_FAULT; +#endif + +/** @brief Enumeration of VTOL states */ +#ifndef HAVE_ENUM_MAV_VTOL_STATE +#define HAVE_ENUM_MAV_VTOL_STATE +typedef enum MAV_VTOL_STATE +{ + MAV_VTOL_STATE_UNDEFINED=0, /* MAV is not configured as VTOL | */ + MAV_VTOL_STATE_TRANSITION_TO_FW=1, /* VTOL is in transition from multicopter to fixed-wing | */ + MAV_VTOL_STATE_TRANSITION_TO_MC=2, /* VTOL is in transition from fixed-wing to multicopter | */ + MAV_VTOL_STATE_MC=3, /* VTOL is in multicopter state | */ + MAV_VTOL_STATE_FW=4, /* VTOL is in fixed-wing state | */ + MAV_VTOL_STATE_ENUM_END=5, /* | */ +} MAV_VTOL_STATE; +#endif + +/** @brief Enumeration of landed detector states */ +#ifndef HAVE_ENUM_MAV_LANDED_STATE +#define HAVE_ENUM_MAV_LANDED_STATE +typedef enum MAV_LANDED_STATE +{ + MAV_LANDED_STATE_UNDEFINED=0, /* MAV landed state is unknown | */ + MAV_LANDED_STATE_ON_GROUND=1, /* MAV is landed (on ground) | */ + MAV_LANDED_STATE_IN_AIR=2, /* MAV is in air | */ + MAV_LANDED_STATE_TAKEOFF=3, /* MAV currently taking off | */ + MAV_LANDED_STATE_LANDING=4, /* MAV currently landing | */ + MAV_LANDED_STATE_ENUM_END=5, /* | */ +} MAV_LANDED_STATE; +#endif + +/** @brief Enumeration of the ADSB altimeter types */ +#ifndef HAVE_ENUM_ADSB_ALTITUDE_TYPE +#define HAVE_ENUM_ADSB_ALTITUDE_TYPE +typedef enum ADSB_ALTITUDE_TYPE +{ + ADSB_ALTITUDE_TYPE_PRESSURE_QNH=0, /* Altitude reported from a Baro source using QNH reference | */ + ADSB_ALTITUDE_TYPE_GEOMETRIC=1, /* Altitude reported from a GNSS source | */ + ADSB_ALTITUDE_TYPE_ENUM_END=2, /* | */ +} ADSB_ALTITUDE_TYPE; +#endif + +/** @brief ADSB classification for the type of vehicle emitting the transponder signal */ +#ifndef HAVE_ENUM_ADSB_EMITTER_TYPE +#define HAVE_ENUM_ADSB_EMITTER_TYPE +typedef enum ADSB_EMITTER_TYPE +{ + ADSB_EMITTER_TYPE_NO_INFO=0, /* | */ + ADSB_EMITTER_TYPE_LIGHT=1, /* | */ + ADSB_EMITTER_TYPE_SMALL=2, /* | */ + ADSB_EMITTER_TYPE_LARGE=3, /* | */ + ADSB_EMITTER_TYPE_HIGH_VORTEX_LARGE=4, /* | */ + ADSB_EMITTER_TYPE_HEAVY=5, /* | */ + ADSB_EMITTER_TYPE_HIGHLY_MANUV=6, /* | */ + ADSB_EMITTER_TYPE_ROTOCRAFT=7, /* | */ + ADSB_EMITTER_TYPE_UNASSIGNED=8, /* | */ + ADSB_EMITTER_TYPE_GLIDER=9, /* | */ + ADSB_EMITTER_TYPE_LIGHTER_AIR=10, /* | */ + ADSB_EMITTER_TYPE_PARACHUTE=11, /* | */ + ADSB_EMITTER_TYPE_ULTRA_LIGHT=12, /* | */ + ADSB_EMITTER_TYPE_UNASSIGNED2=13, /* | */ + ADSB_EMITTER_TYPE_UAV=14, /* | */ + ADSB_EMITTER_TYPE_SPACE=15, /* | */ + ADSB_EMITTER_TYPE_UNASSGINED3=16, /* | */ + ADSB_EMITTER_TYPE_EMERGENCY_SURFACE=17, /* | */ + ADSB_EMITTER_TYPE_SERVICE_SURFACE=18, /* | */ + ADSB_EMITTER_TYPE_POINT_OBSTACLE=19, /* | */ + ADSB_EMITTER_TYPE_ENUM_END=20, /* | */ +} ADSB_EMITTER_TYPE; +#endif + +/** @brief These flags indicate status such as data validity of each data source. Set = data valid */ +#ifndef HAVE_ENUM_ADSB_FLAGS +#define HAVE_ENUM_ADSB_FLAGS +typedef enum ADSB_FLAGS +{ + ADSB_FLAGS_VALID_COORDS=1, /* | */ + ADSB_FLAGS_VALID_ALTITUDE=2, /* | */ + ADSB_FLAGS_VALID_HEADING=4, /* | */ + ADSB_FLAGS_VALID_VELOCITY=8, /* | */ + ADSB_FLAGS_VALID_CALLSIGN=16, /* | */ + ADSB_FLAGS_VALID_SQUAWK=32, /* | */ + ADSB_FLAGS_SIMULATED=64, /* | */ + ADSB_FLAGS_VERTICAL_VELOCITY_VALID=128, /* | */ + ADSB_FLAGS_BARO_VALID=256, /* | */ + ADSB_FLAGS_SOURCE_UAT=32768, /* | */ + ADSB_FLAGS_ENUM_END=32769, /* | */ +} ADSB_FLAGS; +#endif + +/** @brief Bitmap of options for the MAV_CMD_DO_REPOSITION */ +#ifndef HAVE_ENUM_MAV_DO_REPOSITION_FLAGS +#define HAVE_ENUM_MAV_DO_REPOSITION_FLAGS +typedef enum MAV_DO_REPOSITION_FLAGS +{ + MAV_DO_REPOSITION_FLAGS_CHANGE_MODE=1, /* The aircraft should immediately transition into guided. This should not be set for follow me applications | */ + MAV_DO_REPOSITION_FLAGS_ENUM_END=2, /* | */ +} MAV_DO_REPOSITION_FLAGS; +#endif + +/** @brief Flags in EKF_STATUS message */ +#ifndef HAVE_ENUM_ESTIMATOR_STATUS_FLAGS +#define HAVE_ENUM_ESTIMATOR_STATUS_FLAGS +typedef enum ESTIMATOR_STATUS_FLAGS +{ + ESTIMATOR_ATTITUDE=1, /* True if the attitude estimate is good | */ + ESTIMATOR_VELOCITY_HORIZ=2, /* True if the horizontal velocity estimate is good | */ + ESTIMATOR_VELOCITY_VERT=4, /* True if the vertical velocity estimate is good | */ + ESTIMATOR_POS_HORIZ_REL=8, /* True if the horizontal position (relative) estimate is good | */ + ESTIMATOR_POS_HORIZ_ABS=16, /* True if the horizontal position (absolute) estimate is good | */ + ESTIMATOR_POS_VERT_ABS=32, /* True if the vertical position (absolute) estimate is good | */ + ESTIMATOR_POS_VERT_AGL=64, /* True if the vertical position (above ground) estimate is good | */ + ESTIMATOR_CONST_POS_MODE=128, /* True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) | */ + ESTIMATOR_PRED_POS_HORIZ_REL=256, /* True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate | */ + ESTIMATOR_PRED_POS_HORIZ_ABS=512, /* True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate | */ + ESTIMATOR_GPS_GLITCH=1024, /* True if the EKF has detected a GPS glitch | */ + ESTIMATOR_ACCEL_ERROR=2048, /* True if the EKF has detected bad accelerometer data | */ + ESTIMATOR_STATUS_FLAGS_ENUM_END=2049, /* | */ +} ESTIMATOR_STATUS_FLAGS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MOTOR_TEST_ORDER +#define HAVE_ENUM_MOTOR_TEST_ORDER +typedef enum MOTOR_TEST_ORDER +{ + MOTOR_TEST_ORDER_DEFAULT=0, /* default autopilot motor test method | */ + MOTOR_TEST_ORDER_SEQUENCE=1, /* motor numbers are specified as their index in a predefined vehicle-specific sequence | */ + MOTOR_TEST_ORDER_BOARD=2, /* motor numbers are specified as the output as labeled on the board | */ + MOTOR_TEST_ORDER_ENUM_END=3, /* | */ +} MOTOR_TEST_ORDER; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE +#define HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE +typedef enum MOTOR_TEST_THROTTLE_TYPE +{ + MOTOR_TEST_THROTTLE_PERCENT=0, /* throttle as a percentage from 0 ~ 100 | */ + MOTOR_TEST_THROTTLE_PWM=1, /* throttle as an absolute PWM value (normally in range of 1000~2000) | */ + MOTOR_TEST_THROTTLE_PILOT=2, /* throttle pass-through from pilot's transmitter | */ + MOTOR_TEST_COMPASS_CAL=3, /* per-motor compass calibration test | */ + MOTOR_TEST_THROTTLE_TYPE_ENUM_END=4, /* | */ +} MOTOR_TEST_THROTTLE_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GPS_INPUT_IGNORE_FLAGS +#define HAVE_ENUM_GPS_INPUT_IGNORE_FLAGS +typedef enum GPS_INPUT_IGNORE_FLAGS +{ + GPS_INPUT_IGNORE_FLAG_ALT=1, /* ignore altitude field | */ + GPS_INPUT_IGNORE_FLAG_HDOP=2, /* ignore hdop field | */ + GPS_INPUT_IGNORE_FLAG_VDOP=4, /* ignore vdop field | */ + GPS_INPUT_IGNORE_FLAG_VEL_HORIZ=8, /* ignore horizontal velocity field (vn and ve) | */ + GPS_INPUT_IGNORE_FLAG_VEL_VERT=16, /* ignore vertical velocity field (vd) | */ + GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY=32, /* ignore speed accuracy field | */ + GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY=64, /* ignore horizontal accuracy field | */ + GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY=128, /* ignore vertical accuracy field | */ + GPS_INPUT_IGNORE_FLAGS_ENUM_END=129, /* | */ +} GPS_INPUT_IGNORE_FLAGS; +#endif + +/** @brief Possible actions an aircraft can take to avoid a collision. */ +#ifndef HAVE_ENUM_MAV_COLLISION_ACTION +#define HAVE_ENUM_MAV_COLLISION_ACTION +typedef enum MAV_COLLISION_ACTION +{ + MAV_COLLISION_ACTION_NONE=0, /* Ignore any potential collisions | */ + MAV_COLLISION_ACTION_REPORT=1, /* Report potential collision | */ + MAV_COLLISION_ACTION_ASCEND_OR_DESCEND=2, /* Ascend or Descend to avoid threat | */ + MAV_COLLISION_ACTION_MOVE_HORIZONTALLY=3, /* Move horizontally to avoid threat | */ + MAV_COLLISION_ACTION_MOVE_PERPENDICULAR=4, /* Aircraft to move perpendicular to the collision's velocity vector | */ + MAV_COLLISION_ACTION_RTL=5, /* Aircraft to fly directly back to its launch point | */ + MAV_COLLISION_ACTION_HOVER=6, /* Aircraft to stop in place | */ + MAV_COLLISION_ACTION_ENUM_END=7, /* | */ +} MAV_COLLISION_ACTION; +#endif + +/** @brief Aircraft-rated danger from this threat. */ +#ifndef HAVE_ENUM_MAV_COLLISION_THREAT_LEVEL +#define HAVE_ENUM_MAV_COLLISION_THREAT_LEVEL +typedef enum MAV_COLLISION_THREAT_LEVEL +{ + MAV_COLLISION_THREAT_LEVEL_NONE=0, /* Not a threat | */ + MAV_COLLISION_THREAT_LEVEL_LOW=1, /* Craft is mildly concerned about this threat | */ + MAV_COLLISION_THREAT_LEVEL_HIGH=2, /* Craft is panicking, and may take actions to avoid threat | */ + MAV_COLLISION_THREAT_LEVEL_ENUM_END=3, /* | */ +} MAV_COLLISION_THREAT_LEVEL; +#endif + +/** @brief Source of information about this collision. */ +#ifndef HAVE_ENUM_MAV_COLLISION_SRC +#define HAVE_ENUM_MAV_COLLISION_SRC +typedef enum MAV_COLLISION_SRC +{ + MAV_COLLISION_SRC_ADSB=0, /* ID field references ADSB_VEHICLE packets | */ + MAV_COLLISION_SRC_MAVLINK_GPS_GLOBAL_INT=1, /* ID field references MAVLink SRC ID | */ + MAV_COLLISION_SRC_ENUM_END=2, /* | */ +} MAV_COLLISION_SRC; +#endif + +/** @brief Type of GPS fix */ +#ifndef HAVE_ENUM_GPS_FIX_TYPE +#define HAVE_ENUM_GPS_FIX_TYPE +typedef enum GPS_FIX_TYPE +{ + GPS_FIX_TYPE_NO_GPS=0, /* No GPS connected | */ + GPS_FIX_TYPE_NO_FIX=1, /* No position information, GPS is connected | */ + GPS_FIX_TYPE_2D_FIX=2, /* 2D position | */ + GPS_FIX_TYPE_3D_FIX=3, /* 3D position | */ + GPS_FIX_TYPE_DGPS=4, /* DGPS/SBAS aided 3D position | */ + GPS_FIX_TYPE_RTK_FLOAT=5, /* RTK float, 3D position | */ + GPS_FIX_TYPE_RTK_FIXED=6, /* RTK Fixed, 3D position | */ + GPS_FIX_TYPE_STATIC=7, /* Static fixed, typically used for base stations | */ + GPS_FIX_TYPE_PPP=8, /* PPP, 3D position. | */ + GPS_FIX_TYPE_ENUM_END=9, /* | */ +} GPS_FIX_TYPE; +#endif + +/** @brief RTK GPS baseline coordinate system, used for RTK corrections */ +#ifndef HAVE_ENUM_RTK_BASELINE_COORDINATE_SYSTEM +#define HAVE_ENUM_RTK_BASELINE_COORDINATE_SYSTEM +typedef enum RTK_BASELINE_COORDINATE_SYSTEM +{ + RTK_BASELINE_COORDINATE_SYSTEM_ECEF=0, /* Earth-centered, Earth-fixed | */ + RTK_BASELINE_COORDINATE_SYSTEM_NED=1, /* RTK basestation centered, north, east, down | */ + RTK_BASELINE_COORDINATE_SYSTEM_ENUM_END=2, /* | */ +} RTK_BASELINE_COORDINATE_SYSTEM; +#endif + +/** @brief Type of landing target */ +#ifndef HAVE_ENUM_LANDING_TARGET_TYPE +#define HAVE_ENUM_LANDING_TARGET_TYPE +typedef enum LANDING_TARGET_TYPE +{ + LANDING_TARGET_TYPE_LIGHT_BEACON=0, /* Landing target signaled by light beacon (ex: IR-LOCK) | */ + LANDING_TARGET_TYPE_RADIO_BEACON=1, /* Landing target signaled by radio beacon (ex: ILS, NDB) | */ + LANDING_TARGET_TYPE_VISION_FIDUCIAL=2, /* Landing target represented by a fiducial marker (ex: ARTag) | */ + LANDING_TARGET_TYPE_VISION_OTHER=3, /* Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square) | */ + LANDING_TARGET_TYPE_ENUM_END=4, /* | */ +} LANDING_TARGET_TYPE; +#endif + +/** @brief Direction of VTOL transition */ +#ifndef HAVE_ENUM_VTOL_TRANSITION_HEADING +#define HAVE_ENUM_VTOL_TRANSITION_HEADING +typedef enum VTOL_TRANSITION_HEADING +{ + VTOL_TRANSITION_HEADING_VEHICLE_DEFAULT=0, /* Respect the heading configuration of the vehicle. | */ + VTOL_TRANSITION_HEADING_NEXT_WAYPOINT=1, /* Use the heading pointing towards the next waypoint. | */ + VTOL_TRANSITION_HEADING_TAKEOFF=2, /* Use the heading on takeoff (while sitting on the ground). | */ + VTOL_TRANSITION_HEADING_SPECIFIED=3, /* Use the specified heading in parameter 4. | */ + VTOL_TRANSITION_HEADING_ANY=4, /* Use the current heading when reaching takeoff altitude (potentially facing the wind when weather-vaning is active). | */ + VTOL_TRANSITION_HEADING_ENUM_END=5, /* | */ +} VTOL_TRANSITION_HEADING; +#endif + +/** @brief Camera capability flags (Bitmap) */ +#ifndef HAVE_ENUM_CAMERA_CAP_FLAGS +#define HAVE_ENUM_CAMERA_CAP_FLAGS +typedef enum CAMERA_CAP_FLAGS +{ + CAMERA_CAP_FLAGS_CAPTURE_VIDEO=1, /* Camera is able to record video | */ + CAMERA_CAP_FLAGS_CAPTURE_IMAGE=2, /* Camera is able to capture images | */ + CAMERA_CAP_FLAGS_HAS_MODES=4, /* Camera has separate Video and Image/Photo modes (MAV_CMD_SET_CAMERA_MODE) | */ + CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE=8, /* Camera can capture images while in video mode | */ + CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE=16, /* Camera can capture videos while in Photo/Image mode | */ + CAMERA_CAP_FLAGS_HAS_IMAGE_SURVEY_MODE=32, /* Camera has image survey mode (MAV_CMD_SET_CAMERA_MODE) | */ + CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM=64, /* Camera has basic zoom control (MAV_CMD_SET_CAMERA_ZOOM) | */ + CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS=128, /* Camera has basic focus control (MAV_CMD_SET_CAMERA_FOCUS) | */ + CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM=256, /* Camera has video streaming capabilities (use MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION for video streaming info) | */ + CAMERA_CAP_FLAGS_ENUM_END=257, /* | */ +} CAMERA_CAP_FLAGS; +#endif + +/** @brief Stream status flags (Bitmap) */ +#ifndef HAVE_ENUM_VIDEO_STREAM_STATUS_FLAGS +#define HAVE_ENUM_VIDEO_STREAM_STATUS_FLAGS +typedef enum VIDEO_STREAM_STATUS_FLAGS +{ + VIDEO_STREAM_STATUS_FLAGS_RUNNING=1, /* Stream is active (running) | */ + VIDEO_STREAM_STATUS_FLAGS_THERMAL=2, /* Stream is thermal imaging | */ + VIDEO_STREAM_STATUS_FLAGS_ENUM_END=3, /* | */ +} VIDEO_STREAM_STATUS_FLAGS; +#endif + +/** @brief Video stream types */ +#ifndef HAVE_ENUM_VIDEO_STREAM_TYPE +#define HAVE_ENUM_VIDEO_STREAM_TYPE +typedef enum VIDEO_STREAM_TYPE +{ + VIDEO_STREAM_TYPE_RTSP=0, /* Stream is RTSP | */ + VIDEO_STREAM_TYPE_RTPUDP=1, /* Stream is RTP UDP (URI gives the port number) | */ + VIDEO_STREAM_TYPE_TCP_MPEG=2, /* Stream is MPEG on TCP | */ + VIDEO_STREAM_TYPE_MPEG_TS_H264=3, /* Stream is h.264 on MPEG TS (URI gives the port number) | */ + VIDEO_STREAM_TYPE_ENUM_END=4, /* | */ +} VIDEO_STREAM_TYPE; +#endif + +/** @brief Zoom types for MAV_CMD_SET_CAMERA_ZOOM */ +#ifndef HAVE_ENUM_CAMERA_ZOOM_TYPE +#define HAVE_ENUM_CAMERA_ZOOM_TYPE +typedef enum CAMERA_ZOOM_TYPE +{ + ZOOM_TYPE_STEP=0, /* Zoom one step increment (-1 for wide, 1 for tele) | */ + ZOOM_TYPE_CONTINUOUS=1, /* Continuous zoom up/down until stopped (-1 for wide, 1 for tele, 0 to stop zooming) | */ + ZOOM_TYPE_RANGE=2, /* Zoom value as proportion of full camera range (a value between 0.0 and 100.0) | */ + ZOOM_TYPE_FOCAL_LENGTH=3, /* Zoom value/variable focal length in milimetres. Note that there is no message to get the valid zoom range of the camera, so this can type can only be used for cameras where the zoom range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera) | */ + CAMERA_ZOOM_TYPE_ENUM_END=4, /* | */ +} CAMERA_ZOOM_TYPE; +#endif + +/** @brief Focus types for MAV_CMD_SET_CAMERA_FOCUS */ +#ifndef HAVE_ENUM_SET_FOCUS_TYPE +#define HAVE_ENUM_SET_FOCUS_TYPE +typedef enum SET_FOCUS_TYPE +{ + FOCUS_TYPE_STEP=0, /* Focus one step increment (-1 for focusing in, 1 for focusing out towards infinity). | */ + FOCUS_TYPE_CONTINUOUS=1, /* Continuous focus up/down until stopped (-1 for focusing in, 1 for focusing out towards infinity, 0 to stop focusing) | */ + FOCUS_TYPE_RANGE=2, /* Focus value as proportion of full camera focus range (a value between 0.0 and 100.0) | */ + FOCUS_TYPE_METERS=3, /* Focus value in metres. Note that there is no message to get the valid focus range of the camera, so this can type can only be used for cameras where the range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera). | */ + SET_FOCUS_TYPE_ENUM_END=4, /* | */ +} SET_FOCUS_TYPE; +#endif + +/** @brief Result from a PARAM_EXT_SET message. */ +#ifndef HAVE_ENUM_PARAM_ACK +#define HAVE_ENUM_PARAM_ACK +typedef enum PARAM_ACK +{ + PARAM_ACK_ACCEPTED=0, /* Parameter value ACCEPTED and SET | */ + PARAM_ACK_VALUE_UNSUPPORTED=1, /* Parameter value UNKNOWN/UNSUPPORTED | */ + PARAM_ACK_FAILED=2, /* Parameter failed to set | */ + PARAM_ACK_IN_PROGRESS=3, /* Parameter value received but not yet validated or set. A subsequent PARAM_EXT_ACK will follow once operation is completed with the actual result. These are for parameters that may take longer to set. Instead of waiting for an ACK and potentially timing out, you will immediately receive this response to let you know it was received. | */ + PARAM_ACK_ENUM_END=4, /* | */ +} PARAM_ACK; +#endif + +/** @brief Camera Modes. */ +#ifndef HAVE_ENUM_CAMERA_MODE +#define HAVE_ENUM_CAMERA_MODE +typedef enum CAMERA_MODE +{ + CAMERA_MODE_IMAGE=0, /* Camera is in image/photo capture mode. | */ + CAMERA_MODE_VIDEO=1, /* Camera is in video capture mode. | */ + CAMERA_MODE_IMAGE_SURVEY=2, /* Camera is in image survey capture mode. It allows for camera controller to do specific settings for surveys. | */ + CAMERA_MODE_ENUM_END=3, /* | */ +} CAMERA_MODE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ARM_AUTH_DENIED_REASON +#define HAVE_ENUM_MAV_ARM_AUTH_DENIED_REASON +typedef enum MAV_ARM_AUTH_DENIED_REASON +{ + MAV_ARM_AUTH_DENIED_REASON_GENERIC=0, /* Not a specific reason | */ + MAV_ARM_AUTH_DENIED_REASON_NONE=1, /* Authorizer will send the error as string to GCS | */ + MAV_ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT=2, /* At least one waypoint have a invalid value | */ + MAV_ARM_AUTH_DENIED_REASON_TIMEOUT=3, /* Timeout in the authorizer process(in case it depends on network) | */ + MAV_ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE=4, /* Airspace of the mission in use by another vehicle, second result parameter can have the waypoint id that caused it to be denied. | */ + MAV_ARM_AUTH_DENIED_REASON_BAD_WEATHER=5, /* Weather is not good to fly | */ + MAV_ARM_AUTH_DENIED_REASON_ENUM_END=6, /* | */ +} MAV_ARM_AUTH_DENIED_REASON; +#endif + +/** @brief RC type */ +#ifndef HAVE_ENUM_RC_TYPE +#define HAVE_ENUM_RC_TYPE +typedef enum RC_TYPE +{ + RC_TYPE_SPEKTRUM_DSM2=0, /* Spektrum DSM2 | */ + RC_TYPE_SPEKTRUM_DSMX=1, /* Spektrum DSMX | */ + RC_TYPE_ENUM_END=2, /* | */ +} RC_TYPE; +#endif + +/** @brief Bitmap to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 9 is set the floats afx afy afz should be interpreted as force instead of acceleration. */ +#ifndef HAVE_ENUM_POSITION_TARGET_TYPEMASK +#define HAVE_ENUM_POSITION_TARGET_TYPEMASK +typedef enum POSITION_TARGET_TYPEMASK +{ + POSITION_TARGET_TYPEMASK_X_IGNORE=1, /* Ignore position x | */ + POSITION_TARGET_TYPEMASK_Y_IGNORE=2, /* Ignore position y | */ + POSITION_TARGET_TYPEMASK_Z_IGNORE=4, /* Ignore position z | */ + POSITION_TARGET_TYPEMASK_VX_IGNORE=8, /* Ignore velocity x | */ + POSITION_TARGET_TYPEMASK_VY_IGNORE=16, /* Ignore velocity y | */ + POSITION_TARGET_TYPEMASK_VZ_IGNORE=32, /* Ignore velocity z | */ + POSITION_TARGET_TYPEMASK_AX_IGNORE=64, /* Ignore acceleration x | */ + POSITION_TARGET_TYPEMASK_AY_IGNORE=128, /* Ignore acceleration y | */ + POSITION_TARGET_TYPEMASK_AZ_IGNORE=256, /* Ignore acceleration z | */ + POSITION_TARGET_TYPEMASK_FORCE_SET=512, /* Use force instead of acceleration | */ + POSITION_TARGET_TYPEMASK_YAW_IGNORE=1024, /* Ignore yaw | */ + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE=2048, /* Ignore yaw rate | */ + POSITION_TARGET_TYPEMASK_ENUM_END=2049, /* | */ +} POSITION_TARGET_TYPEMASK; +#endif + +/** @brief Airborne status of UAS. */ +#ifndef HAVE_ENUM_UTM_FLIGHT_STATE +#define HAVE_ENUM_UTM_FLIGHT_STATE +typedef enum UTM_FLIGHT_STATE +{ + UTM_FLIGHT_STATE_UNKNOWN=1, /* The flight state can't be determined. | */ + UTM_FLIGHT_STATE_GROUND=2, /* UAS on ground. | */ + UTM_FLIGHT_STATE_AIRBORNE=3, /* UAS airborne. | */ + UTM_FLIGHT_STATE_EMERGENCY=16, /* UAS is in an emergency flight state. | */ + UTM_FLIGHT_STATE_NOCTRL=32, /* UAS has no active controls. | */ + UTM_FLIGHT_STATE_ENUM_END=33, /* | */ +} UTM_FLIGHT_STATE; +#endif + +/** @brief Flags for the global position report. */ +#ifndef HAVE_ENUM_UTM_DATA_AVAIL_FLAGS +#define HAVE_ENUM_UTM_DATA_AVAIL_FLAGS +typedef enum UTM_DATA_AVAIL_FLAGS +{ + UTM_DATA_AVAIL_FLAGS_TIME_VALID=1, /* The field time contains valid data. | */ + UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE=2, /* The field uas_id contains valid data. | */ + UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE=4, /* The fields lat, lon and h_acc contain valid data. | */ + UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE=8, /* The fields alt and v_acc contain valid data. | */ + UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE=16, /* The field relative_alt contains valid data. | */ + UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE=32, /* The fields vx and vy contain valid data. | */ + UTM_DATA_AVAIL_FLAGS_VERTICAL_VELO_AVAILABLE=64, /* The field vz contains valid data. | */ + UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE=128, /* The fields next_lat, next_lon and next_alt contain valid data. | */ + UTM_DATA_AVAIL_FLAGS_ENUM_END=129, /* | */ +} UTM_DATA_AVAIL_FLAGS; +#endif + +/** @brief Cellular network radio type */ +#ifndef HAVE_ENUM_CELLULAR_NETWORK_RADIO_TYPE +#define HAVE_ENUM_CELLULAR_NETWORK_RADIO_TYPE +typedef enum CELLULAR_NETWORK_RADIO_TYPE +{ + CELLULAR_NETWORK_RADIO_TYPE_NONE=0, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_GSM=1, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_CDMA=2, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_WCDMA=3, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_LTE=4, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_ENUM_END=5, /* | */ +} CELLULAR_NETWORK_RADIO_TYPE; +#endif + +/** @brief These flags encode the cellular network status */ +#ifndef HAVE_ENUM_CELLULAR_NETWORK_STATUS_FLAG +#define HAVE_ENUM_CELLULAR_NETWORK_STATUS_FLAG +typedef enum CELLULAR_NETWORK_STATUS_FLAG +{ + CELLULAR_NETWORK_STATUS_FLAG_ROAMING=1, /* Roaming is active | */ + CELLULAR_NETWORK_STATUS_FLAG_ENUM_END=2, /* | */ +} CELLULAR_NETWORK_STATUS_FLAG; +#endif + +/** @brief Precision land modes (used in MAV_CMD_NAV_LAND). */ +#ifndef HAVE_ENUM_PRECISION_LAND_MODE +#define HAVE_ENUM_PRECISION_LAND_MODE +typedef enum PRECISION_LAND_MODE +{ + PRECISION_LAND_MODE_DISABLED=0, /* Normal (non-precision) landing. | */ + PRECISION_LAND_MODE_OPPORTUNISTIC=1, /* Use precision landing if beacon detected when land command accepted, otherwise land normally. | */ + PRECISION_LAND_MODE_REQUIRED=2, /* Use precision landing, searching for beacon if not found when land command accepted (land normally if beacon cannot be found). | */ + PRECISION_LAND_MODE_ENUM_END=3, /* | */ +} PRECISION_LAND_MODE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_PARACHUTE_ACTION +#define HAVE_ENUM_PARACHUTE_ACTION +typedef enum PARACHUTE_ACTION +{ + PARACHUTE_DISABLE=0, /* Disable parachute release. | */ + PARACHUTE_ENABLE=1, /* Enable parachute release. | */ + PARACHUTE_RELEASE=2, /* Release parachute. | */ + PARACHUTE_ACTION_ENUM_END=3, /* | */ +} PARACHUTE_ACTION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_TUNNEL_PAYLOAD_TYPE +#define HAVE_ENUM_MAV_TUNNEL_PAYLOAD_TYPE +typedef enum MAV_TUNNEL_PAYLOAD_TYPE +{ + MAV_TUNNEL_PAYLOAD_TYPE_UNKNOWN=0, /* Encoding of payload unknown. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED0=200, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED1=201, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED2=202, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED3=203, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED4=204, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED5=205, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED6=206, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED7=207, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED8=208, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED9=209, /* Registered for STorM32 gimbal controller. | */ + MAV_TUNNEL_PAYLOAD_TYPE_ENUM_END=210, /* | */ +} MAV_TUNNEL_PAYLOAD_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_ID_TYPE +#define HAVE_ENUM_MAV_ODID_ID_TYPE +typedef enum MAV_ODID_ID_TYPE +{ + MAV_ODID_ID_TYPE_NONE=0, /* No type defined. | */ + MAV_ODID_ID_TYPE_SERIAL_NUMBER=1, /* Manufacturer Serial Number (ANSI/CTA-2063 format). | */ + MAV_ODID_ID_TYPE_CAA_REGISTRATION_ID=2, /* CAA (Civil Aviation Authority) registered ID. Format: [ICAO Country Code].[CAA Assigned ID]. | */ + MAV_ODID_ID_TYPE_UTM_ASSIGNED_UUID=3, /* UTM (Unmanned Traffic Management) assigned UUID (RFC4122). | */ + MAV_ODID_ID_TYPE_ENUM_END=4, /* | */ +} MAV_ODID_ID_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_UA_TYPE +#define HAVE_ENUM_MAV_ODID_UA_TYPE +typedef enum MAV_ODID_UA_TYPE +{ + MAV_ODID_UA_TYPE_NONE=0, /* No UA (Unmanned Aircraft) type defined. | */ + MAV_ODID_UA_TYPE_AEROPLANE=1, /* Aeroplane/Airplane. Fixed wing. | */ + MAV_ODID_UA_TYPE_ROTORCRAFT=2, /* Rotorcraft (including Multirotor). | */ + MAV_ODID_UA_TYPE_GYROPLANE=3, /* Gyroplane. | */ + MAV_ODID_UA_TYPE_VTOL=4, /* VTOL (Vertical Take-Off and Landing). Fixed wing aircraft that can take off vertically. | */ + MAV_ODID_UA_TYPE_ORNITHOPTER=5, /* Ornithopter. | */ + MAV_ODID_UA_TYPE_GLIDER=6, /* Glider. | */ + MAV_ODID_UA_TYPE_KITE=7, /* Kite. | */ + MAV_ODID_UA_TYPE_FREE_BALLOON=8, /* Free Balloon. | */ + MAV_ODID_UA_TYPE_CAPTIVE_BALLOON=9, /* Captive Balloon. | */ + MAV_ODID_UA_TYPE_AIRSHIP=10, /* Airship. E.g. a blimp. | */ + MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE=11, /* Free Fall/Parachute. | */ + MAV_ODID_UA_TYPE_ROCKET=12, /* Rocket. | */ + MAV_ODID_UA_TYPE_TETHERED_POWERED_AIRCRAFT=13, /* Tethered powered aircraft. | */ + MAV_ODID_UA_TYPE_GROUND_OBSTACLE=14, /* Ground Obstacle. | */ + MAV_ODID_UA_TYPE_OTHER=15, /* Other type of aircraft not listed earlier. | */ + MAV_ODID_UA_TYPE_ENUM_END=16, /* | */ +} MAV_ODID_UA_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_STATUS +#define HAVE_ENUM_MAV_ODID_STATUS +typedef enum MAV_ODID_STATUS +{ + MAV_ODID_STATUS_UNDECLARED=0, /* The status of the (UA) Unmanned Aircraft is undefined. | */ + MAV_ODID_STATUS_GROUND=1, /* The UA is on the ground. | */ + MAV_ODID_STATUS_AIRBORNE=2, /* The UA is in the air. | */ + MAV_ODID_STATUS_ENUM_END=3, /* | */ +} MAV_ODID_STATUS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_HEIGHT_REF +#define HAVE_ENUM_MAV_ODID_HEIGHT_REF +typedef enum MAV_ODID_HEIGHT_REF +{ + MAV_ODID_HEIGHT_REF_OVER_TAKEOFF=0, /* The height field is relative to the take-off location. | */ + MAV_ODID_HEIGHT_REF_OVER_GROUND=1, /* The height field is relative to ground. | */ + MAV_ODID_HEIGHT_REF_ENUM_END=2, /* | */ +} MAV_ODID_HEIGHT_REF; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_HOR_ACC +#define HAVE_ENUM_MAV_ODID_HOR_ACC +typedef enum MAV_ODID_HOR_ACC +{ + MAV_ODID_HOR_ACC_UNKNOWN=0, /* The horizontal accuracy is unknown. | */ + MAV_ODID_HOR_ACC_10NM=1, /* The horizontal accuracy is smaller than 10 Nautical Miles. 18.52 km. | */ + MAV_ODID_HOR_ACC_4NM=2, /* The horizontal accuracy is smaller than 4 Nautical Miles. 7.408 km. | */ + MAV_ODID_HOR_ACC_2NM=3, /* The horizontal accuracy is smaller than 2 Nautical Miles. 3.704 km. | */ + MAV_ODID_HOR_ACC_1NM=4, /* The horizontal accuracy is smaller than 1 Nautical Miles. 1.852 km. | */ + MAV_ODID_HOR_ACC_0_5NM=5, /* The horizontal accuracy is smaller than 0.5 Nautical Miles. 926 m. | */ + MAV_ODID_HOR_ACC_0_3NM=6, /* The horizontal accuracy is smaller than 0.3 Nautical Miles. 555.6 m. | */ + MAV_ODID_HOR_ACC_0_1NM=7, /* The horizontal accuracy is smaller than 0.1 Nautical Miles. 185.2 m. | */ + MAV_ODID_HOR_ACC_0_05NM=8, /* The horizontal accuracy is smaller than 0.05 Nautical Miles. 92.6 m. | */ + MAV_ODID_HOR_ACC_30_METER=9, /* The horizontal accuracy is smaller than 30 meter. | */ + MAV_ODID_HOR_ACC_10_METER=10, /* The horizontal accuracy is smaller than 10 meter. | */ + MAV_ODID_HOR_ACC_3_METER=11, /* The horizontal accuracy is smaller than 3 meter. | */ + MAV_ODID_HOR_ACC_1_METER=12, /* The horizontal accuracy is smaller than 1 meter. | */ + MAV_ODID_HOR_ACC_ENUM_END=13, /* | */ +} MAV_ODID_HOR_ACC; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_VER_ACC +#define HAVE_ENUM_MAV_ODID_VER_ACC +typedef enum MAV_ODID_VER_ACC +{ + MAV_ODID_VER_ACC_UNKNOWN=0, /* The vertical accuracy is unknown. | */ + MAV_ODID_VER_ACC_150_METER=1, /* The vertical accuracy is smaller than 150 meter. | */ + MAV_ODID_VER_ACC_45_METER=2, /* The vertical accuracy is smaller than 45 meter. | */ + MAV_ODID_VER_ACC_25_METER=3, /* The vertical accuracy is smaller than 25 meter. | */ + MAV_ODID_VER_ACC_10_METER=4, /* The vertical accuracy is smaller than 10 meter. | */ + MAV_ODID_VER_ACC_3_METER=5, /* The vertical accuracy is smaller than 3 meter. | */ + MAV_ODID_VER_ACC_1_METER=6, /* The vertical accuracy is smaller than 1 meter. | */ + MAV_ODID_VER_ACC_ENUM_END=7, /* | */ +} MAV_ODID_VER_ACC; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_SPEED_ACC +#define HAVE_ENUM_MAV_ODID_SPEED_ACC +typedef enum MAV_ODID_SPEED_ACC +{ + MAV_ODID_SPEED_ACC_UNKNOWN=0, /* The speed accuracy is unknown. | */ + MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND=1, /* The speed accuracy is smaller than 10 meters per second. | */ + MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND=2, /* The speed accuracy is smaller than 3 meters per second. | */ + MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND=3, /* The speed accuracy is smaller than 1 meters per second. | */ + MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND=4, /* The speed accuracy is smaller than 0.3 meters per second. | */ + MAV_ODID_SPEED_ACC_ENUM_END=5, /* | */ +} MAV_ODID_SPEED_ACC; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_TIME_ACC +#define HAVE_ENUM_MAV_ODID_TIME_ACC +typedef enum MAV_ODID_TIME_ACC +{ + MAV_ODID_TIME_ACC_UNKNOWN=0, /* The timestamp accuracy is unknown. | */ + MAV_ODID_TIME_ACC_0_1_SECOND=1, /* The timestamp accuracy is smaller than 0.1 second. | */ + MAV_ODID_TIME_ACC_0_2_SECOND=2, /* The timestamp accuracy is smaller than 0.2 second. | */ + MAV_ODID_TIME_ACC_0_3_SECOND=3, /* The timestamp accuracy is smaller than 0.3 second. | */ + MAV_ODID_TIME_ACC_0_4_SECOND=4, /* The timestamp accuracy is smaller than 0.4 second. | */ + MAV_ODID_TIME_ACC_0_5_SECOND=5, /* The timestamp accuracy is smaller than 0.5 second. | */ + MAV_ODID_TIME_ACC_0_6_SECOND=6, /* The timestamp accuracy is smaller than 0.6 second. | */ + MAV_ODID_TIME_ACC_0_7_SECOND=7, /* The timestamp accuracy is smaller than 0.7 second. | */ + MAV_ODID_TIME_ACC_0_8_SECOND=8, /* The timestamp accuracy is smaller than 0.8 second. | */ + MAV_ODID_TIME_ACC_0_9_SECOND=9, /* The timestamp accuracy is smaller than 0.9 second. | */ + MAV_ODID_TIME_ACC_1_0_SECOND=10, /* The timestamp accuracy is smaller than 1.0 second. | */ + MAV_ODID_TIME_ACC_1_1_SECOND=11, /* The timestamp accuracy is smaller than 1.1 second. | */ + MAV_ODID_TIME_ACC_1_2_SECOND=12, /* The timestamp accuracy is smaller than 1.2 second. | */ + MAV_ODID_TIME_ACC_1_3_SECOND=13, /* The timestamp accuracy is smaller than 1.3 second. | */ + MAV_ODID_TIME_ACC_1_4_SECOND=14, /* The timestamp accuracy is smaller than 1.4 second. | */ + MAV_ODID_TIME_ACC_1_5_SECOND=15, /* The timestamp accuracy is smaller than 1.5 second. | */ + MAV_ODID_TIME_ACC_ENUM_END=16, /* | */ +} MAV_ODID_TIME_ACC; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_AUTH_TYPE +#define HAVE_ENUM_MAV_ODID_AUTH_TYPE +typedef enum MAV_ODID_AUTH_TYPE +{ + MAV_ODID_AUTH_TYPE_NONE=0, /* No authentication type is specified. | */ + MAV_ODID_AUTH_TYPE_UAS_ID_SIGNATURE=1, /* Signature for the UAS (Unmanned Aircraft System) ID. | */ + MAV_ODID_AUTH_TYPE_OPERATOR_ID_SIGNATURE=2, /* Signature for the Operator ID. | */ + MAV_ODID_AUTH_TYPE_MESSAGE_SET_SIGNATURE=3, /* Signature for the entire message set. | */ + MAV_ODID_AUTH_TYPE_NETWORK_REMOTE_ID=4, /* Authentication is provided by Network Remote ID. | */ + MAV_ODID_AUTH_TYPE_ENUM_END=5, /* | */ +} MAV_ODID_AUTH_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_DESC_TYPE +#define HAVE_ENUM_MAV_ODID_DESC_TYPE +typedef enum MAV_ODID_DESC_TYPE +{ + MAV_ODID_DESC_TYPE_TEXT=0, /* Free-form text description of the purpose of the flight. | */ + MAV_ODID_DESC_TYPE_ENUM_END=1, /* | */ +} MAV_ODID_DESC_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_LOCATION_SRC +#define HAVE_ENUM_MAV_ODID_LOCATION_SRC +typedef enum MAV_ODID_LOCATION_SRC +{ + MAV_ODID_LOCATION_SRC_TAKEOFF=0, /* The location of the operator is the same as the take-off location. | */ + MAV_ODID_LOCATION_SRC_LIVE_GNSS=1, /* The location of the operator is based on live GNSS data. | */ + MAV_ODID_LOCATION_SRC_FIXED=2, /* The location of the operator is a fixed location. | */ + MAV_ODID_LOCATION_SRC_ENUM_END=3, /* | */ +} MAV_ODID_LOCATION_SRC; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_OPERATOR_ID_TYPE +#define HAVE_ENUM_MAV_ODID_OPERATOR_ID_TYPE +typedef enum MAV_ODID_OPERATOR_ID_TYPE +{ + MAV_ODID_OPERATOR_ID_TYPE_CAA=0, /* CAA (Civil Aviation Authority) registered operator ID. | */ + MAV_ODID_OPERATOR_ID_TYPE_ENUM_END=1, /* | */ +} MAV_ODID_OPERATOR_ID_TYPE; +#endif + +/** @brief Tune formats (used for vehicle buzzer/tone generation). */ +#ifndef HAVE_ENUM_TUNE_FORMAT +#define HAVE_ENUM_TUNE_FORMAT +typedef enum TUNE_FORMAT +{ + TUNE_FORMAT_QBASIC1_1=1, /* Format is QBasic 1.1 Play: https://www.qbasic.net/en/reference/qb11/Statement/PLAY-006.htm. | */ + TUNE_FORMAT_MML_MODERN=2, /* Format is Modern Music Markup Language (MML): https://en.wikipedia.org/wiki/Music_Macro_Language#Modern_MML. | */ + TUNE_FORMAT_ENUM_END=3, /* | */ +} TUNE_FORMAT; +#endif + +/** @brief Component capability flags (Bitmap) */ +#ifndef HAVE_ENUM_COMPONENT_CAP_FLAGS +#define HAVE_ENUM_COMPONENT_CAP_FLAGS +typedef enum COMPONENT_CAP_FLAGS +{ + COMPONENT_CAP_FLAGS_PARAM=1, /* Component has parameters, and supports the parameter protocol (PARAM messages). | */ + COMPONENT_CAP_FLAGS_PARAM_EXT=2, /* Component has parameters, and supports the extended parameter protocol (PARAM_EXT messages). | */ + COMPONENT_CAP_FLAGS_ENUM_END=3, /* | */ +} COMPONENT_CAP_FLAGS; +#endif + +/** @brief Type of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html */ +#ifndef HAVE_ENUM_AIS_TYPE +#define HAVE_ENUM_AIS_TYPE +typedef enum AIS_TYPE +{ + AIS_TYPE_UNKNOWN=0, /* Not available (default). | */ + AIS_TYPE_RESERVED_1=1, /* | */ + AIS_TYPE_RESERVED_2=2, /* | */ + AIS_TYPE_RESERVED_3=3, /* | */ + AIS_TYPE_RESERVED_4=4, /* | */ + AIS_TYPE_RESERVED_5=5, /* | */ + AIS_TYPE_RESERVED_6=6, /* | */ + AIS_TYPE_RESERVED_7=7, /* | */ + AIS_TYPE_RESERVED_8=8, /* | */ + AIS_TYPE_RESERVED_9=9, /* | */ + AIS_TYPE_RESERVED_10=10, /* | */ + AIS_TYPE_RESERVED_11=11, /* | */ + AIS_TYPE_RESERVED_12=12, /* | */ + AIS_TYPE_RESERVED_13=13, /* | */ + AIS_TYPE_RESERVED_14=14, /* | */ + AIS_TYPE_RESERVED_15=15, /* | */ + AIS_TYPE_RESERVED_16=16, /* | */ + AIS_TYPE_RESERVED_17=17, /* | */ + AIS_TYPE_RESERVED_18=18, /* | */ + AIS_TYPE_RESERVED_19=19, /* | */ + AIS_TYPE_WIG=20, /* Wing In Ground effect. | */ + AIS_TYPE_WIG_HAZARDOUS_A=21, /* | */ + AIS_TYPE_WIG_HAZARDOUS_B=22, /* | */ + AIS_TYPE_WIG_HAZARDOUS_C=23, /* | */ + AIS_TYPE_WIG_HAZARDOUS_D=24, /* | */ + AIS_TYPE_WIG_RESERVED_1=25, /* | */ + AIS_TYPE_WIG_RESERVED_2=26, /* | */ + AIS_TYPE_WIG_RESERVED_3=27, /* | */ + AIS_TYPE_WIG_RESERVED_4=28, /* | */ + AIS_TYPE_WIG_RESERVED_5=29, /* | */ + AIS_TYPE_FISHING=30, /* | */ + AIS_TYPE_TOWING=31, /* | */ + AIS_TYPE_TOWING_LARGE=32, /* Towing: length exceeds 200m or breadth exceeds 25m. | */ + AIS_TYPE_DREDGING=33, /* Dredging or other underwater ops. | */ + AIS_TYPE_DIVING=34, /* | */ + AIS_TYPE_MILITARY=35, /* | */ + AIS_TYPE_SAILING=36, /* | */ + AIS_TYPE_PLEASURE=37, /* | */ + AIS_TYPE_RESERVED_20=38, /* | */ + AIS_TYPE_RESERVED_21=39, /* | */ + AIS_TYPE_HSC=40, /* High Speed Craft. | */ + AIS_TYPE_HSC_HAZARDOUS_A=41, /* | */ + AIS_TYPE_HSC_HAZARDOUS_B=42, /* | */ + AIS_TYPE_HSC_HAZARDOUS_C=43, /* | */ + AIS_TYPE_HSC_HAZARDOUS_D=44, /* | */ + AIS_TYPE_HSC_RESERVED_1=45, /* | */ + AIS_TYPE_HSC_RESERVED_2=46, /* | */ + AIS_TYPE_HSC_RESERVED_3=47, /* | */ + AIS_TYPE_HSC_RESERVED_4=48, /* | */ + AIS_TYPE_HSC_UNKNOWN=49, /* | */ + AIS_TYPE_PILOT=50, /* | */ + AIS_TYPE_SAR=51, /* Search And Rescue vessel. | */ + AIS_TYPE_TUG=52, /* | */ + AIS_TYPE_PORT_TENDER=53, /* | */ + AIS_TYPE_ANTI_POLLUTION=54, /* Anti-pollution equipment. | */ + AIS_TYPE_LAW_ENFORCEMENT=55, /* | */ + AIS_TYPE_SPARE_LOCAL_1=56, /* | */ + AIS_TYPE_SPARE_LOCAL_2=57, /* | */ + AIS_TYPE_MEDICAL_TRANSPORT=58, /* | */ + AIS_TYPE_NONECOMBATANT=59, /* Noncombatant ship according to RR Resolution No. 18. | */ + AIS_TYPE_PASSENGER=60, /* | */ + AIS_TYPE_PASSENGER_HAZARDOUS_A=61, /* | */ + AIS_TYPE_PASSENGER_HAZARDOUS_B=62, /* | */ + AIS_TYPE_AIS_TYPE_PASSENGER_HAZARDOUS_C=63, /* | */ + AIS_TYPE_PASSENGER_HAZARDOUS_D=64, /* | */ + AIS_TYPE_PASSENGER_RESERVED_1=65, /* | */ + AIS_TYPE_PASSENGER_RESERVED_2=66, /* | */ + AIS_TYPE_PASSENGER_RESERVED_3=67, /* | */ + AIS_TYPE_AIS_TYPE_PASSENGER_RESERVED_4=68, /* | */ + AIS_TYPE_PASSENGER_UNKNOWN=69, /* | */ + AIS_TYPE_CARGO=70, /* | */ + AIS_TYPE_CARGO_HAZARDOUS_A=71, /* | */ + AIS_TYPE_CARGO_HAZARDOUS_B=72, /* | */ + AIS_TYPE_CARGO_HAZARDOUS_C=73, /* | */ + AIS_TYPE_CARGO_HAZARDOUS_D=74, /* | */ + AIS_TYPE_CARGO_RESERVED_1=75, /* | */ + AIS_TYPE_CARGO_RESERVED_2=76, /* | */ + AIS_TYPE_CARGO_RESERVED_3=77, /* | */ + AIS_TYPE_CARGO_RESERVED_4=78, /* | */ + AIS_TYPE_CARGO_UNKNOWN=79, /* | */ + AIS_TYPE_TANKER=80, /* | */ + AIS_TYPE_TANKER_HAZARDOUS_A=81, /* | */ + AIS_TYPE_TANKER_HAZARDOUS_B=82, /* | */ + AIS_TYPE_TANKER_HAZARDOUS_C=83, /* | */ + AIS_TYPE_TANKER_HAZARDOUS_D=84, /* | */ + AIS_TYPE_TANKER_RESERVED_1=85, /* | */ + AIS_TYPE_TANKER_RESERVED_2=86, /* | */ + AIS_TYPE_TANKER_RESERVED_3=87, /* | */ + AIS_TYPE_TANKER_RESERVED_4=88, /* | */ + AIS_TYPE_TANKER_UNKNOWN=89, /* | */ + AIS_TYPE_OTHER=90, /* | */ + AIS_TYPE_OTHER_HAZARDOUS_A=91, /* | */ + AIS_TYPE_OTHER_HAZARDOUS_B=92, /* | */ + AIS_TYPE_OTHER_HAZARDOUS_C=93, /* | */ + AIS_TYPE_OTHER_HAZARDOUS_D=94, /* | */ + AIS_TYPE_OTHER_RESERVED_1=95, /* | */ + AIS_TYPE_OTHER_RESERVED_2=96, /* | */ + AIS_TYPE_OTHER_RESERVED_3=97, /* | */ + AIS_TYPE_OTHER_RESERVED_4=98, /* | */ + AIS_TYPE_OTHER_UNKNOWN=99, /* | */ + AIS_TYPE_ENUM_END=100, /* | */ +} AIS_TYPE; +#endif + +/** @brief Navigational status of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html */ +#ifndef HAVE_ENUM_AIS_NAV_STATUS +#define HAVE_ENUM_AIS_NAV_STATUS +typedef enum AIS_NAV_STATUS +{ + UNDER_WAY=0, /* Under way using engine. | */ + AIS_NAV_ANCHORED=1, /* | */ + AIS_NAV_UN_COMMANDED=2, /* | */ + AIS_NAV_RESTRICTED_MANOEUVERABILITY=3, /* | */ + AIS_NAV_DRAUGHT_CONSTRAINED=4, /* | */ + AIS_NAV_MOORED=5, /* | */ + AIS_NAV_AGROUND=6, /* | */ + AIS_NAV_FISHING=7, /* | */ + AIS_NAV_SAILING=8, /* | */ + AIS_NAV_RESERVED_HSC=9, /* | */ + AIS_NAV_RESERVED_WIG=10, /* | */ + AIS_NAV_RESERVED_1=11, /* | */ + AIS_NAV_RESERVED_2=12, /* | */ + AIS_NAV_RESERVED_3=13, /* | */ + AIS_NAV_AIS_SART=14, /* Search And Rescue Transponder. | */ + AIS_NAV_UNKNOWN=15, /* Not available (default). | */ + AIS_NAV_STATUS_ENUM_END=16, /* | */ +} AIS_NAV_STATUS; +#endif + +/** @brief These flags are used in the AIS_VESSEL.fields bitmask to indicate validity of data in the other message fields. When set, the data is valid. */ +#ifndef HAVE_ENUM_AIS_FLAGS +#define HAVE_ENUM_AIS_FLAGS +typedef enum AIS_FLAGS +{ + AIS_FLAGS_POSITION_ACCURACY=1, /* 1 = Position accuracy less than 10m, 0 = position accuracy greater than 10m. | */ + AIS_FLAGS_VALID_COG=2, /* | */ + AIS_FLAGS_VALID_VELOCITY=4, /* | */ + AIS_FLAGS_HIGH_VELOCITY=8, /* 1 = Velocity over 52.5765m/s (102.2 knots) | */ + AIS_FLAGS_VALID_TURN_RATE=16, /* | */ + AIS_FLAGS_TURN_RATE_SIGN_ONLY=32, /* Only the sign of the returned turn rate value is valid, either greater than 5deg/30s or less than -5deg/30s | */ + AIS_FLAGS_VALID_DIMENSIONS=64, /* | */ + AIS_FLAGS_LARGE_BOW_DIMENSION=128, /* Distance to bow is larger than 511m | */ + AIS_FLAGS_LARGE_STERN_DIMENSION=256, /* Distance to stern is larger than 511m | */ + AIS_FLAGS_LARGE_PORT_DIMENSION=512, /* Distance to port side is larger than 63m | */ + AIS_FLAGS_LARGE_STARBOARD_DIMENSION=1024, /* Distance to starboard side is larger than 63m | */ + AIS_FLAGS_VALID_CALLSIGN=2048, /* | */ + AIS_FLAGS_VALID_NAME=4096, /* | */ + AIS_FLAGS_ENUM_END=4097, /* | */ +} AIS_FLAGS; +#endif + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +// base include + + +#undef MAVLINK_THIS_XML_IDX +#define MAVLINK_THIS_XML_IDX 1 + +#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_LINK_NODE_STATUS, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT, MAVLINK_MESSAGE_INFO_MISSION_CHANGED, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_HIL_ACTUATOR_CONTROLS, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ALTITUDE, MAVLINK_MESSAGE_INFO_RESOURCE_REQUEST, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3, MAVLINK_MESSAGE_INFO_FOLLOW_TARGET, MAVLINK_MESSAGE_INFO_CONTROL_SYSTEM_STATE, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS, MAVLINK_MESSAGE_INFO_WIND_COV, MAVLINK_MESSAGE_INFO_GPS_INPUT, MAVLINK_MESSAGE_INFO_GPS_RTCM_DATA, MAVLINK_MESSAGE_INFO_HIGH_LATENCY, MAVLINK_MESSAGE_INFO_HIGH_LATENCY2, MAVLINK_MESSAGE_INFO_VIBRATION, MAVLINK_MESSAGE_INFO_HOME_POSITION, MAVLINK_MESSAGE_INFO_SET_HOME_POSITION, MAVLINK_MESSAGE_INFO_MESSAGE_INTERVAL, MAVLINK_MESSAGE_INFO_EXTENDED_SYS_STATE, MAVLINK_MESSAGE_INFO_ADSB_VEHICLE, MAVLINK_MESSAGE_INFO_COLLISION, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_SETUP_SIGNING, MAVLINK_MESSAGE_INFO_BUTTON_CHANGE, MAVLINK_MESSAGE_INFO_PLAY_TUNE, MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS, MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED, MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION, MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION, MAVLINK_MESSAGE_INFO_LOGGING_DATA, MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED, MAVLINK_MESSAGE_INFO_LOGGING_ACK, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS, MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION, MAVLINK_MESSAGE_INFO_AIS_VESSEL, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE, MAVLINK_MESSAGE_INFO_PARAM_EXT_SET, MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK, MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE, MAVLINK_MESSAGE_INFO_ODOMETRY, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER, MAVLINK_MESSAGE_INFO_CELLULAR_STATUS, MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS, MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY, MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS, MAVLINK_MESSAGE_INFO_STATUSTEXT_LONG, MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO, MAVLINK_MESSAGE_INFO_SMART_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS, MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET, MAVLINK_MESSAGE_INFO_TUNNEL, MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS, MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION, MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2, MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES, MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK} +# define MAVLINK_MESSAGE_NAMES {{ "ACTUATOR_CONTROL_TARGET", 140 }, { "ACTUATOR_OUTPUT_STATUS", 375 }, { "ADSB_VEHICLE", 246 }, { "AIS_VESSEL", 301 }, { "ALTITUDE", 141 }, { "ATTITUDE", 30 }, { "ATTITUDE_QUATERNION", 31 }, { "ATTITUDE_QUATERNION_COV", 61 }, { "ATTITUDE_TARGET", 83 }, { "ATT_POS_MOCAP", 138 }, { "AUTH_KEY", 7 }, { "AUTOPILOT_VERSION", 148 }, { "BATTERY_STATUS", 147 }, { "BUTTON_CHANGE", 257 }, { "CAMERA_CAPTURE_STATUS", 262 }, { "CAMERA_IMAGE_CAPTURED", 263 }, { "CAMERA_INFORMATION", 259 }, { "CAMERA_SETTINGS", 260 }, { "CAMERA_TRIGGER", 112 }, { "CELLULAR_STATUS", 334 }, { "CHANGE_OPERATOR_CONTROL", 5 }, { "CHANGE_OPERATOR_CONTROL_ACK", 6 }, { "COLLISION", 247 }, { "COMMAND_ACK", 77 }, { "COMMAND_INT", 75 }, { "COMMAND_LONG", 76 }, { "COMPONENT_INFORMATION", 395 }, { "CONTROL_SYSTEM_STATE", 146 }, { "DATA_STREAM", 67 }, { "DATA_TRANSMISSION_HANDSHAKE", 130 }, { "DEBUG", 254 }, { "DEBUG_FLOAT_ARRAY", 350 }, { "DEBUG_VECT", 250 }, { "DISTANCE_SENSOR", 132 }, { "ENCAPSULATED_DATA", 131 }, { "ESTIMATOR_STATUS", 230 }, { "EXTENDED_SYS_STATE", 245 }, { "FENCE_STATUS", 162 }, { "FILE_TRANSFER_PROTOCOL", 110 }, { "FLIGHT_INFORMATION", 264 }, { "FOLLOW_TARGET", 144 }, { "GLOBAL_POSITION_INT", 33 }, { "GLOBAL_POSITION_INT_COV", 63 }, { "GLOBAL_VISION_POSITION_ESTIMATE", 101 }, { "GPS2_RAW", 124 }, { "GPS2_RTK", 128 }, { "GPS_GLOBAL_ORIGIN", 49 }, { "GPS_INJECT_DATA", 123 }, { "GPS_INPUT", 232 }, { "GPS_RAW_INT", 24 }, { "GPS_RTCM_DATA", 233 }, { "GPS_RTK", 127 }, { "GPS_STATUS", 25 }, { "HEARTBEAT", 0 }, { "HIGHRES_IMU", 105 }, { "HIGH_LATENCY", 234 }, { "HIGH_LATENCY2", 235 }, { "HIL_ACTUATOR_CONTROLS", 93 }, { "HIL_CONTROLS", 91 }, { "HIL_GPS", 113 }, { "HIL_OPTICAL_FLOW", 114 }, { "HIL_RC_INPUTS_RAW", 92 }, { "HIL_SENSOR", 107 }, { "HIL_STATE", 90 }, { "HIL_STATE_QUATERNION", 115 }, { "HOME_POSITION", 242 }, { "ISBD_LINK_STATUS", 335 }, { "LANDING_TARGET", 149 }, { "LINK_NODE_STATUS", 8 }, { "LOCAL_POSITION_NED", 32 }, { "LOCAL_POSITION_NED_COV", 64 }, { "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", 89 }, { "LOGGING_ACK", 268 }, { "LOGGING_DATA", 266 }, { "LOGGING_DATA_ACKED", 267 }, { "LOG_DATA", 120 }, { "LOG_ENTRY", 118 }, { "LOG_ERASE", 121 }, { "LOG_REQUEST_DATA", 119 }, { "LOG_REQUEST_END", 122 }, { "LOG_REQUEST_LIST", 117 }, { "MANUAL_CONTROL", 69 }, { "MANUAL_SETPOINT", 81 }, { "MEMORY_VECT", 249 }, { "MESSAGE_INTERVAL", 244 }, { "MISSION_ACK", 47 }, { "MISSION_CHANGED", 52 }, { "MISSION_CLEAR_ALL", 45 }, { "MISSION_COUNT", 44 }, { "MISSION_CURRENT", 42 }, { "MISSION_ITEM", 39 }, { "MISSION_ITEM_INT", 73 }, { "MISSION_ITEM_REACHED", 46 }, { "MISSION_REQUEST", 40 }, { "MISSION_REQUEST_INT", 51 }, { "MISSION_REQUEST_LIST", 43 }, { "MISSION_REQUEST_PARTIAL_LIST", 37 }, { "MISSION_SET_CURRENT", 41 }, { "MISSION_WRITE_PARTIAL_LIST", 38 }, { "MOUNT_ORIENTATION", 265 }, { "NAMED_VALUE_FLOAT", 251 }, { "NAMED_VALUE_INT", 252 }, { "NAV_CONTROLLER_OUTPUT", 62 }, { "OBSTACLE_DISTANCE", 330 }, { "ODOMETRY", 331 }, { "ONBOARD_COMPUTER_STATUS", 390 }, { "OPEN_DRONE_ID_AUTHENTICATION", 12902 }, { "OPEN_DRONE_ID_BASIC_ID", 12900 }, { "OPEN_DRONE_ID_LOCATION", 12901 }, { "OPEN_DRONE_ID_MESSAGE_PACK", 12915 }, { "OPEN_DRONE_ID_OPERATOR_ID", 12905 }, { "OPEN_DRONE_ID_SELF_ID", 12903 }, { "OPEN_DRONE_ID_SYSTEM", 12904 }, { "OPTICAL_FLOW", 100 }, { "OPTICAL_FLOW_RAD", 106 }, { "ORBIT_EXECUTION_STATUS", 360 }, { "PARAM_EXT_ACK", 324 }, { "PARAM_EXT_REQUEST_LIST", 321 }, { "PARAM_EXT_REQUEST_READ", 320 }, { "PARAM_EXT_SET", 323 }, { "PARAM_EXT_VALUE", 322 }, { "PARAM_MAP_RC", 50 }, { "PARAM_REQUEST_LIST", 21 }, { "PARAM_REQUEST_READ", 20 }, { "PARAM_SET", 23 }, { "PARAM_VALUE", 22 }, { "PING", 4 }, { "PLAY_TUNE", 258 }, { "PLAY_TUNE_V2", 400 }, { "POSITION_TARGET_GLOBAL_INT", 87 }, { "POSITION_TARGET_LOCAL_NED", 85 }, { "POWER_STATUS", 125 }, { "PROTOCOL_VERSION", 300 }, { "RADIO_STATUS", 109 }, { "RAW_IMU", 27 }, { "RAW_PRESSURE", 28 }, { "RC_CHANNELS", 65 }, { "RC_CHANNELS_OVERRIDE", 70 }, { "RC_CHANNELS_RAW", 35 }, { "RC_CHANNELS_SCALED", 34 }, { "REQUEST_DATA_STREAM", 66 }, { "RESOURCE_REQUEST", 142 }, { "SAFETY_ALLOWED_AREA", 55 }, { "SAFETY_SET_ALLOWED_AREA", 54 }, { "SCALED_IMU", 26 }, { "SCALED_IMU2", 116 }, { "SCALED_IMU3", 129 }, { "SCALED_PRESSURE", 29 }, { "SCALED_PRESSURE2", 137 }, { "SCALED_PRESSURE3", 143 }, { "SERIAL_CONTROL", 126 }, { "SERVO_OUTPUT_RAW", 36 }, { "SETUP_SIGNING", 256 }, { "SET_ACTUATOR_CONTROL_TARGET", 139 }, { "SET_ATTITUDE_TARGET", 82 }, { "SET_GPS_GLOBAL_ORIGIN", 48 }, { "SET_HOME_POSITION", 243 }, { "SET_MODE", 11 }, { "SET_POSITION_TARGET_GLOBAL_INT", 86 }, { "SET_POSITION_TARGET_LOCAL_NED", 84 }, { "SIM_STATE", 108 }, { "SMART_BATTERY_INFO", 370 }, { "SMART_BATTERY_STATUS", 371 }, { "STATUSTEXT", 253 }, { "STATUSTEXT_LONG", 365 }, { "STORAGE_INFORMATION", 261 }, { "SUPPORTED_TUNES", 401 }, { "SYSTEM_TIME", 2 }, { "SYS_STATUS", 1 }, { "TERRAIN_CHECK", 135 }, { "TERRAIN_DATA", 134 }, { "TERRAIN_REPORT", 136 }, { "TERRAIN_REQUEST", 133 }, { "TIMESYNC", 111 }, { "TIME_ESTIMATE_TO_TARGET", 380 }, { "TRAJECTORY_REPRESENTATION_BEZIER", 333 }, { "TRAJECTORY_REPRESENTATION_WAYPOINTS", 332 }, { "TUNNEL", 385 }, { "UAVCAN_NODE_INFO", 311 }, { "UAVCAN_NODE_STATUS", 310 }, { "UTM_GLOBAL_POSITION", 340 }, { "V2_EXTENSION", 248 }, { "VFR_HUD", 74 }, { "VIBRATION", 241 }, { "VICON_POSITION_ESTIMATE", 104 }, { "VIDEO_STREAM_INFORMATION", 269 }, { "VIDEO_STREAM_STATUS", 270 }, { "VISION_POSITION_ESTIMATE", 102 }, { "VISION_SPEED_ESTIMATE", 103 }, { "WHEEL_DISTANCE", 9000 }, { "WIFI_CONFIG_AP", 299 }, { "WIND_COV", 231 }} +# if MAVLINK_COMMAND_24BIT +# include "../mavlink_get_info.h" +# endif +#endif + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // MAVLINK_COMMON_H diff --git a/MavLinkCom/mavlink/common/version.h b/MavLinkCom/mavlink/common/version.h index b091bda37..8263f9234 100644 --- a/MavLinkCom/mavlink/common/version.h +++ b/MavLinkCom/mavlink/common/version.h @@ -1,14 +1,14 @@ -/** @file - * @brief MAVLink comm protocol built from common.xml - * @see http://mavlink.org - */ -#pragma once - -#ifndef MAVLINK_VERSION_H -#define MAVLINK_VERSION_H - -#define MAVLINK_BUILD_DATE "Fri Apr 28 2017" -#define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" -#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 - -#endif // MAVLINK_VERSION_H +/** @file + * @brief MAVLink comm protocol built from common.xml + * @see http://mavlink.org + */ +#pragma once + +#ifndef MAVLINK_VERSION_H +#define MAVLINK_VERSION_H + +#define MAVLINK_BUILD_DATE "Mon Dec 09 2019" +#define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" +#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 + +#endif // MAVLINK_VERSION_H diff --git a/MavLinkCom/mavlink/mavlink_conversions.h b/MavLinkCom/mavlink/mavlink_conversions.h index 9baee264b..a859146f8 100644 --- a/MavLinkCom/mavlink/mavlink_conversions.h +++ b/MavLinkCom/mavlink/mavlink_conversions.h @@ -1,209 +1,212 @@ -#pragma once - -/* enable math defines on Windows */ -#ifdef _MSC_VER -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES -#endif -#endif -#include - -#ifndef M_PI_2 - #define M_PI_2 ((float)asin(1)) -#endif - -/** - * @file mavlink_conversions.h - * - * These conversion functions follow the NASA rotation standards definition file - * available online. - * - * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free - * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free) - * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the - * protocol as widely as possible. - * - * @author James Goppert - * @author Thomas Gubler - */ - - -/** - * Converts a quaternion to a rotation matrix - * - * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) - * @param dcm a 3x3 rotation matrix - */ -MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3]) -{ - double a = quaternion[0]; - double b = quaternion[1]; - double c = quaternion[2]; - double d = quaternion[3]; - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm[0][0] = aSq + bSq - cSq - dSq; - dcm[0][1] = 2 * (b * c - a * d); - dcm[0][2] = 2 * (a * c + b * d); - dcm[1][0] = 2 * (b * c + a * d); - dcm[1][1] = aSq - bSq + cSq - dSq; - dcm[1][2] = 2 * (c * d - a * b); - dcm[2][0] = 2 * (b * d - a * c); - dcm[2][1] = 2 * (a * b + c * d); - dcm[2][2] = aSq - bSq - cSq + dSq; -} - - -/** - * Converts a rotation matrix to euler angles - * - * @param dcm a 3x3 rotation matrix - * @param roll the roll angle in radians - * @param pitch the pitch angle in radians - * @param yaw the yaw angle in radians - */ -MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) -{ - float phi, theta, psi; - theta = asin(-dcm[2][0]); - - if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { - phi = 0.0f; - psi = (atan2f(dcm[1][2] - dcm[0][1], - dcm[0][2] + dcm[1][1]) + phi); - - } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { - phi = 0.0f; - psi = atan2f(dcm[1][2] - dcm[0][1], - dcm[0][2] + dcm[1][1] - phi); - - } else { - phi = atan2f(dcm[2][1], dcm[2][2]); - psi = atan2f(dcm[1][0], dcm[0][0]); - } - - *roll = phi; - *pitch = theta; - *yaw = psi; -} - - -/** - * Converts a quaternion to euler angles - * - * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) - * @param roll the roll angle in radians - * @param pitch the pitch angle in radians - * @param yaw the yaw angle in radians - */ -MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw) -{ - float dcm[3][3]; - mavlink_quaternion_to_dcm(quaternion, dcm); - mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw); -} - - -/** - * Converts euler angles to a quaternion - * - * @param roll the roll angle in radians - * @param pitch the pitch angle in radians - * @param yaw the yaw angle in radians - * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) - */ -MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4]) -{ - float cosPhi_2 = cosf(roll / 2); - float sinPhi_2 = sinf(roll / 2); - float cosTheta_2 = cosf(pitch / 2); - float sinTheta_2 = sinf(pitch / 2); - float cosPsi_2 = cosf(yaw / 2); - float sinPsi_2 = sinf(yaw / 2); - quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - - -/** - * Converts a rotation matrix to a quaternion - * Reference: - * - Shoemake, Quaternions, - * http://www.cs.ucr.edu/~vbz/resources/quatut.pdf - * - * @param dcm a 3x3 rotation matrix - * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) - */ -MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4]) -{ - float tr = dcm[0][0] + dcm[1][1] + dcm[2][2]; - if (tr > 0.0f) { - float s = sqrtf(tr + 1.0f); - quaternion[0] = s * 0.5f; - s = 0.5f / s; - quaternion[1] = (dcm[2][1] - dcm[1][2]) * s; - quaternion[2] = (dcm[0][2] - dcm[2][0]) * s; - quaternion[3] = (dcm[1][0] - dcm[0][1]) * s; - } else { - /* Find maximum diagonal element in dcm - * store index in dcm_i */ - int dcm_i = 0; - int i; - for (i = 1; i < 3; i++) { - if (dcm[i][i] > dcm[dcm_i][dcm_i]) { - dcm_i = i; - } - } - - int dcm_j = (dcm_i + 1) % 3; - int dcm_k = (dcm_i + 2) % 3; - - float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] - - dcm[dcm_k][dcm_k]) + 1.0f); - quaternion[dcm_i + 1] = s * 0.5f; - s = 0.5f / s; - quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s; - quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s; - quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s; - } -} - - -/** - * Converts euler angles to a rotation matrix - * - * @param roll the roll angle in radians - * @param pitch the pitch angle in radians - * @param yaw the yaw angle in radians - * @param dcm a 3x3 rotation matrix - */ -MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3]) -{ - float cosPhi = cosf(roll); - float sinPhi = sinf(roll); - float cosThe = cosf(pitch); - float sinThe = sinf(pitch); - float cosPsi = cosf(yaw); - float sinPsi = sinf(yaw); - - dcm[0][0] = cosThe * cosPsi; - dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm[1][0] = cosThe * sinPsi; - dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm[2][0] = -sinThe; - dcm[2][1] = sinPhi * cosThe; - dcm[2][2] = cosPhi * cosThe; -} - +#pragma once + +#ifndef MAVLINK_NO_CONVERSION_HELPERS + +/* enable math defines on Windows */ +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif +#include + +#ifndef M_PI_2 + #define M_PI_2 ((float)asin(1)) +#endif + +/** + * @file mavlink_conversions.h + * + * These conversion functions follow the NASA rotation standards definition file + * available online. + * + * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free + * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free) + * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the + * protocol as widely as possible. + * + * @author James Goppert + * @author Thomas Gubler + */ + + +/** + * Converts a quaternion to a rotation matrix + * + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + * @param dcm a 3x3 rotation matrix + */ +MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3]) +{ + double a = quaternion[0]; + double b = quaternion[1]; + double c = quaternion[2]; + double d = quaternion[3]; + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + dcm[0][0] = aSq + bSq - cSq - dSq; + dcm[0][1] = 2 * (b * c - a * d); + dcm[0][2] = 2 * (a * c + b * d); + dcm[1][0] = 2 * (b * c + a * d); + dcm[1][1] = aSq - bSq + cSq - dSq; + dcm[1][2] = 2 * (c * d - a * b); + dcm[2][0] = 2 * (b * d - a * c); + dcm[2][1] = 2 * (a * b + c * d); + dcm[2][2] = aSq - bSq - cSq + dSq; +} + + +/** + * Converts a rotation matrix to euler angles + * + * @param dcm a 3x3 rotation matrix + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + */ +MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) +{ + float phi, theta, psi; + theta = asinf(-dcm[2][0]); + + if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = (atan2f(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1]) + phi); + + } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = atan2f(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1] - phi); + + } else { + phi = atan2f(dcm[2][1], dcm[2][2]); + psi = atan2f(dcm[1][0], dcm[0][0]); + } + + *roll = phi; + *pitch = theta; + *yaw = psi; +} + + +/** + * Converts a quaternion to euler angles + * + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + */ +MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw) +{ + float dcm[3][3]; + mavlink_quaternion_to_dcm(quaternion, dcm); + mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw); +} + + +/** + * Converts euler angles to a quaternion + * + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + */ +MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4]) +{ + float cosPhi_2 = cosf(roll / 2); + float sinPhi_2 = sinf(roll / 2); + float cosTheta_2 = cosf(pitch / 2); + float sinTheta_2 = sinf(pitch / 2); + float cosPsi_2 = cosf(yaw / 2); + float sinPsi_2 = sinf(yaw / 2); + quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); +} + + +/** + * Converts a rotation matrix to a quaternion + * Reference: + * - Shoemake, Quaternions, + * http://www.cs.ucr.edu/~vbz/resources/quatut.pdf + * + * @param dcm a 3x3 rotation matrix + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + */ +MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4]) +{ + float tr = dcm[0][0] + dcm[1][1] + dcm[2][2]; + if (tr > 0.0f) { + float s = sqrtf(tr + 1.0f); + quaternion[0] = s * 0.5f; + s = 0.5f / s; + quaternion[1] = (dcm[2][1] - dcm[1][2]) * s; + quaternion[2] = (dcm[0][2] - dcm[2][0]) * s; + quaternion[3] = (dcm[1][0] - dcm[0][1]) * s; + } else { + /* Find maximum diagonal element in dcm + * store index in dcm_i */ + int dcm_i = 0; + int i; + for (i = 1; i < 3; i++) { + if (dcm[i][i] > dcm[dcm_i][dcm_i]) { + dcm_i = i; + } + } + + int dcm_j = (dcm_i + 1) % 3; + int dcm_k = (dcm_i + 2) % 3; + + float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] - + dcm[dcm_k][dcm_k]) + 1.0f); + quaternion[dcm_i + 1] = s * 0.5f; + s = 0.5f / s; + quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s; + quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s; + quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s; + } +} + + +/** + * Converts euler angles to a rotation matrix + * + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + * @param dcm a 3x3 rotation matrix + */ +MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3]) +{ + float cosPhi = cosf(roll); + float sinPhi = sinf(roll); + float cosThe = cosf(pitch); + float sinThe = sinf(pitch); + float cosPsi = cosf(yaw); + float sinPsi = sinf(yaw); + + dcm[0][0] = cosThe * cosPsi; + dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; + dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; + + dcm[1][0] = cosThe * sinPsi; + dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; + dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; + + dcm[2][0] = -sinThe; + dcm[2][1] = sinPhi * cosThe; + dcm[2][2] = cosPhi * cosThe; +} + +#endif // MAVLINK_NO_CONVERSION_HELPERS diff --git a/MavLinkCom/mavlink/mavlink_get_info.h b/MavLinkCom/mavlink/mavlink_get_info.h index 111810589..51cb577f2 100644 --- a/MavLinkCom/mavlink/mavlink_get_info.h +++ b/MavLinkCom/mavlink/mavlink_get_info.h @@ -1,37 +1,71 @@ -#pragma once - -#ifdef MAVLINK_USE_MESSAGE_INFO -#define MAVLINK_HAVE_GET_MESSAGE_INFO -/* - return the message_info struct for a message -*/ -MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg) -{ - static const mavlink_message_info_t mavlink_message_info[] = MAVLINK_MESSAGE_INFO; - /* - use a bisection search to find the right entry. A perfect hash may be better - Note that this assumes the table is sorted with primary key msgid - */ - uint32_t msgid = msg->msgid; - uint32_t low=0, high=sizeof(mavlink_message_info)/sizeof(mavlink_message_info[0]); - while (low < high) { - uint32_t mid = (low+1+high)/2; - if (msgid < mavlink_message_info[mid].msgid) { - high = mid-1; - continue; - } - if (msgid > mavlink_message_info[mid].msgid) { - low = mid; - continue; - } - low = mid; - break; - } - if (mavlink_message_info[low].msgid == msgid) { - return &mavlink_message_info[low]; - } - return NULL; -} -#endif // MAVLINK_USE_MESSAGE_INFO - - +#pragma once + +#ifdef MAVLINK_USE_MESSAGE_INFO +#define MAVLINK_HAVE_GET_MESSAGE_INFO + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_id(uint32_t msgid) +{ + static const mavlink_message_info_t mavlink_message_info[] = MAVLINK_MESSAGE_INFO; + /* + use a bisection search to find the right entry. A perfect hash may be better + Note that this assumes the table is sorted with primary key msgid + */ + uint32_t low=0, high=sizeof(mavlink_message_info)/sizeof(mavlink_message_info[0]); + while (low < high) { + uint32_t mid = (low+1+high)/2; + if (msgid < mavlink_message_info[mid].msgid) { + high = mid-1; + continue; + } + if (msgid > mavlink_message_info[mid].msgid) { + low = mid; + continue; + } + low = mid; + break; + } + if (mavlink_message_info[low].msgid == msgid) { + return &mavlink_message_info[low]; + } + return NULL; +} + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg) +{ + return mavlink_get_message_info_by_id(msg->msgid); +} + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_name(const char *name) +{ + static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES; + /* + use a bisection search to find the right entry. A perfect hash may be better + Note that this assumes the table is sorted with primary key name + */ + uint32_t low=0, high=sizeof(mavlink_message_names)/sizeof(mavlink_message_names[0]); + while (low < high) { + uint32_t mid = (low+1+high)/2; + int cmp = strcmp(mavlink_message_names[mid].name, name); + if (cmp == 0) { + return mavlink_get_message_info_by_id(mavlink_message_names[mid].msgid); + } + if (cmp > 0) { + high = mid-1; + } else { + low = mid; + } + } + return NULL; +} +#endif // MAVLINK_USE_MESSAGE_INFO + + diff --git a/MavLinkCom/mavlink/mavlink_helpers.h b/MavLinkCom/mavlink/mavlink_helpers.h index 0107cc153..fed04d688 100644 --- a/MavLinkCom/mavlink/mavlink_helpers.h +++ b/MavLinkCom/mavlink/mavlink_helpers.h @@ -1,1119 +1,1133 @@ -#pragma once - -#include "string.h" -#include "checksum.h" -#include "mavlink_types.h" -#include "mavlink_conversions.h" -#include - -#ifndef MAVLINK_HELPER -#define MAVLINK_HELPER -#endif - -#include "mavlink_sha256.h" - -#ifdef MAVLINK_USE_CXX_NAMESPACE -namespace mavlink { -#endif - -/* - * Internal function to give access to the channel status for each channel - */ -#ifndef MAVLINK_GET_CHANNEL_STATUS -MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) -{ -#ifdef MAVLINK_EXTERNAL_RX_STATUS - // No m_mavlink_status array defined in function, - // has to be defined externally -#else - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; -#endif - return &m_mavlink_status[chan]; -} -#endif - -/* - * Internal function to give access to the channel buffer for each channel - */ -#ifndef MAVLINK_GET_CHANNEL_BUFFER -MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) -{ - -#ifdef MAVLINK_EXTERNAL_RX_BUFFER - // No m_mavlink_buffer array defined in function, - // has to be defined externally -#else - static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; -#endif - return &m_mavlink_buffer[chan]; -} -#endif // MAVLINK_GET_CHANNEL_BUFFER - -/** - * @brief Reset the status of a channel. - */ -MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) -{ - mavlink_status_t *status = mavlink_get_channel_status(chan); - status->parse_state = MAVLINK_PARSE_STATE_IDLE; -} - -/** - * @brief create a signature block for a packet - */ -MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing, - uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN], - const uint8_t *header, uint8_t header_len, - const uint8_t *packet, uint8_t packet_len, - const uint8_t crc[2]) -{ - mavlink_sha256_ctx ctx; - union { - uint64_t t64; - uint8_t t8[8]; - } tstamp; - if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) { - return 0; - } - signature[0] = signing->link_id; - tstamp.t64 = signing->timestamp; - memcpy(&signature[1], tstamp.t8, 6); - signing->timestamp++; - - mavlink_sha256_init(&ctx); - mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); - mavlink_sha256_update(&ctx, header, header_len); - mavlink_sha256_update(&ctx, packet, packet_len); - mavlink_sha256_update(&ctx, crc, 2); - mavlink_sha256_update(&ctx, signature, 7); - mavlink_sha256_final_48(&ctx, &signature[7]); - - return MAVLINK_SIGNATURE_BLOCK_LEN; -} - -/** - * return new packet length for trimming payload of any trailing zero - * bytes. Used in MAVLink2 to give simple support for variable length - * arrays. - */ -MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length) -{ - while (length > 1 && payload[length-1] == 0) { - length--; - } - return length; -} - -/** - * @brief check a signature block for a packet - */ -MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, - mavlink_signing_streams_t *signing_streams, - const mavlink_message_t *msg) -{ - if (signing == NULL) { - return true; - } - const uint8_t *p = (const uint8_t *)&msg->magic; - const uint8_t *psig = msg->signature; - const uint8_t *incoming_signature = psig+7; - mavlink_sha256_ctx ctx; - uint8_t signature[6]; - uint16_t i; - - mavlink_sha256_init(&ctx); - mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); - mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len); - mavlink_sha256_update(&ctx, msg->ck, 2); - mavlink_sha256_update(&ctx, psig, 1+6); - mavlink_sha256_final_48(&ctx, signature); - if (memcmp(signature, incoming_signature, 6) != 0) { - return false; - } - - // now check timestamp - union tstamp { - uint64_t t64; - uint8_t t8[8]; - } tstamp; - uint8_t link_id = psig[0]; - tstamp.t64 = 0; - memcpy(tstamp.t8, psig+1, 6); - - if (signing_streams == NULL) { - return false; - } - - // find stream - for (i=0; inum_signing_streams; i++) { - if (msg->sysid == signing_streams->stream[i].sysid && - msg->compid == signing_streams->stream[i].compid && - link_id == signing_streams->stream[i].link_id) { - break; - } - } - if (i == signing_streams->num_signing_streams) { - if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) { - // over max number of streams - return false; - } - // new stream. Only accept if timestamp is not more than 1 minute old - if (tstamp.t64 + 6000*1000UL < signing->timestamp) { - return false; - } - // add new stream - signing_streams->stream[i].sysid = msg->sysid; - signing_streams->stream[i].compid = msg->compid; - signing_streams->stream[i].link_id = link_id; - signing_streams->num_signing_streams++; - } else { - union tstamp last_tstamp; - last_tstamp.t64 = 0; - memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6); - if (tstamp.t64 <= last_tstamp.t64) { - // repeating old timestamp - return false; - } - } - - // remember last timestamp - memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6); - - // our next timestamp must be at least this timestamp - if (tstamp.t64 > signing->timestamp) { - signing->timestamp = tstamp.t64; - } - return true; -} - - -/** - * @brief Finalize a MAVLink message with channel assignment - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. This function - * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack - * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. - * - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length - */ -MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra) -{ - bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; - bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); - uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0; - uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1; - uint8_t buf[MAVLINK_CORE_HEADER_LEN+1]; - if (mavlink1) { - msg->magic = MAVLINK_STX_MAVLINK1; - header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1; - } else { - msg->magic = MAVLINK_STX; - } - msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length); - msg->sysid = system_id; - msg->compid = component_id; - msg->incompat_flags = 0; - if (signing) { - msg->incompat_flags |= MAVLINK_IFLAG_SIGNED; - } - msg->compat_flags = 0; - msg->seq = status->current_tx_seq; - status->current_tx_seq = status->current_tx_seq + 1; - - // form the header as a byte array for the crc - buf[0] = msg->magic; - buf[1] = msg->len; - if (mavlink1) { - buf[2] = msg->seq; - buf[3] = msg->sysid; - buf[4] = msg->compid; - buf[5] = msg->msgid & 0xFF; - } else { - buf[2] = msg->incompat_flags; - buf[3] = msg->compat_flags; - buf[4] = msg->seq; - buf[5] = msg->sysid; - buf[6] = msg->compid; - buf[7] = msg->msgid & 0xFF; - buf[8] = (msg->msgid >> 8) & 0xFF; - buf[9] = (msg->msgid >> 16) & 0xFF; - } - - msg->checksum = crc_calculate(&buf[1], header_len-1); - crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); - crc_accumulate(crc_extra, &msg->checksum); - mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); - - if (signing) { - mavlink_sign_packet(status->signing, - msg->signature, - (const uint8_t *)buf, header_len, - (const uint8_t *)_MAV_PAYLOAD(msg), msg->len, - (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len); - } - - return msg->len + header_len + 2 + signature_len; -} - -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra) -{ - mavlink_status_t *status = mavlink_get_channel_status(chan); - return mavlink_finalize_message_buffer(msg, system_id, component_id, status, min_length, length, crc_extra); -} - -/** - * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel - */ -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t min_length, uint8_t length, uint8_t crc_extra) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra); -} - -static inline void _mav_parse_error(mavlink_status_t *status) -{ - status->parse_error++; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); - -/** - * @brief Finalize a MAVLink message with channel assignment and send - */ -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid, - const char *packet, - uint8_t min_length, uint8_t length, uint8_t crc_extra) -{ - uint16_t checksum; - uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; - uint8_t ck[2]; - mavlink_status_t *status = mavlink_get_channel_status(chan); - uint8_t header_len = MAVLINK_CORE_HEADER_LEN; - uint8_t signature_len = 0; - uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; - bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; - bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); - - if (mavlink1) { - length = min_length; - if (msgid > 255) { - // can't send 16 bit messages - _mav_parse_error(status); - return; - } - header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; - buf[0] = MAVLINK_STX_MAVLINK1; - buf[1] = length; - buf[2] = status->current_tx_seq; - buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; - buf[5] = msgid & 0xFF; - } else { - uint8_t incompat_flags = 0; - if (signing) { - incompat_flags |= MAVLINK_IFLAG_SIGNED; - } - length = _mav_trim_payload(packet, length); - buf[0] = MAVLINK_STX; - buf[1] = length; - buf[2] = incompat_flags; - buf[3] = 0; // compat_flags - buf[4] = status->current_tx_seq; - buf[5] = mavlink_system.sysid; - buf[6] = mavlink_system.compid; - buf[7] = msgid & 0xFF; - buf[8] = (msgid >> 8) & 0xFF; - buf[9] = (msgid >> 16) & 0xFF; - } - status->current_tx_seq++; - checksum = crc_calculate((const uint8_t*)&buf[1], header_len); - crc_accumulate_buffer(&checksum, packet, length); - crc_accumulate(crc_extra, &checksum); - ck[0] = (uint8_t)(checksum & 0xFF); - ck[1] = (uint8_t)(checksum >> 8); - - if (signing) { - // possibly add a signature - signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1, - (const uint8_t *)packet, length, ck); - } - - MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); - _mavlink_send_uart(chan, (const char *)buf, header_len+1); - _mavlink_send_uart(chan, packet, length); - _mavlink_send_uart(chan, (const char *)ck, 2); - if (signature_len != 0) { - _mavlink_send_uart(chan, (const char *)signature, signature_len); - } - MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); -} - -/** - * @brief re-send a message over a uart channel - * this is more stack efficient than re-marshalling the message - * If the message is signed then the original signature is also sent - */ -MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg) -{ - uint8_t ck[2]; - - ck[0] = (uint8_t)(msg->checksum & 0xFF); - ck[1] = (uint8_t)(msg->checksum >> 8); - // XXX use the right sequence here - - uint8_t header_len; - uint8_t signature_len; - - if (msg->magic == MAVLINK_STX_MAVLINK1) { - header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1; - signature_len = 0; - MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len); - // we can't send the structure directly as it has extra mavlink2 elements in it - uint8_t buf[MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1]; - buf[0] = msg->magic; - buf[1] = msg->len; - buf[2] = msg->seq; - buf[3] = msg->sysid; - buf[4] = msg->compid; - buf[5] = msg->msgid & 0xFF; - _mavlink_send_uart(chan, (const char*)buf, header_len); - } else { - header_len = MAVLINK_CORE_HEADER_LEN + 1; - signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; - MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len); - uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1]; - buf[0] = msg->magic; - buf[1] = msg->len; - buf[2] = msg->incompat_flags; - buf[3] = msg->compat_flags; - buf[4] = msg->seq; - buf[5] = msg->sysid; - buf[6] = msg->compid; - buf[7] = msg->msgid & 0xFF; - buf[8] = (msg->msgid >> 8) & 0xFF; - buf[9] = (msg->msgid >> 16) & 0xFF; - _mavlink_send_uart(chan, (const char *)buf, header_len); - } - _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); - _mavlink_send_uart(chan, (const char *)ck, 2); - if (signature_len != 0) { - _mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN); - } - MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a message to send it over a serial byte stream - */ -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg) -{ - uint8_t signature_len, header_len; - uint8_t *ck; - uint8_t length = msg->len; - - if (msg->magic == MAVLINK_STX_MAVLINK1) { - signature_len = 0; - header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; - buf[0] = msg->magic; - buf[1] = length; - buf[2] = msg->seq; - buf[3] = msg->sysid; - buf[4] = msg->compid; - buf[5] = msg->msgid & 0xFF; - memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len); - ck = buf + header_len + 1 + (uint16_t)msg->len; - } else { - length = _mav_trim_payload(_MAV_PAYLOAD(msg), length); - header_len = MAVLINK_CORE_HEADER_LEN; - buf[0] = msg->magic; - buf[1] = length; - buf[2] = msg->incompat_flags; - buf[3] = msg->compat_flags; - buf[4] = msg->seq; - buf[5] = msg->sysid; - buf[6] = msg->compid; - buf[7] = msg->msgid & 0xFF; - buf[8] = (msg->msgid >> 8) & 0xFF; - buf[9] = (msg->msgid >> 16) & 0xFF; - memcpy(&buf[10], _MAV_PAYLOAD(msg), length); - ck = buf + header_len + 1 + (uint16_t)length; - signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; - } - ck[0] = (uint8_t)(msg->checksum & 0xFF); - ck[1] = (uint8_t)(msg->checksum >> 8); - if (signature_len > 0) { - memcpy(&ck[2], msg->signature, signature_len); - } - - return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len; -} - -union __mavlink_bitfield { - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; -}; - - -MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) -{ - crc_init(&msg->checksum); -} - -MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) -{ - crc_accumulate(c, &msg->checksum); -} - -/* - return the crc_entry value for a msgid -*/ -#ifndef MAVLINK_GET_MSG_ENTRY -MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid) -{ - static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS; - /* - use a bisection search to find the right entry. A perfect hash may be better - Note that this assumes the table is sorted by msgid - */ - uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]); - while (low < high) { - uint32_t mid = (low+1+high)/2; - if (msgid < mavlink_message_crcs[mid].msgid) { - high = mid-1; - continue; - } - if (msgid > mavlink_message_crcs[mid].msgid) { - low = mid; - continue; - } - low = mid; - break; - } - if (mavlink_message_crcs[low].msgid != msgid) { - // msgid is not in the table - return NULL; - } - return &mavlink_message_crcs[low]; -} -#endif // MAVLINK_GET_MSG_ENTRY - -/* - return the crc_extra value for a message -*/ -MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg) -{ - const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); - return e?e->crc_extra:0; -} - -/* - return the expected message length -*/ -#define MAVLINK_HAVE_EXPECTED_MESSAGE_LENGTH -MAVLINK_HELPER uint8_t mavlink_expected_message_length(const mavlink_message_t *msg) -{ - const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); - return e?e->msg_len:0; -} - -/** - * This is a varient of mavlink_frame_char() but with caller supplied - * parsing buffers. It is useful when you want to create a MAVLink - * parser in a library that doesn't use any global variables - * - * @param rxmsg parsing message buffer - * @param status parsing starus buffer - * @param c The char to parse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @param returnStats if a message was decoded, this is filled with the channel's stats - * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC - * - * A typical use scenario of this function call is: - * - * @code - * #include - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, - mavlink_status_t* status, - uint8_t c, - mavlink_message_t* r_message, - mavlink_status_t* r_mavlink_status) -{ - /* Enable this option to check the length of each message. - This allows invalid messages to be caught much sooner. Use if the transmission - medium is prone to missing (or extra) characters (e.g. a radio that fades in - and out). Only use if the channel will only contain messages types listed in - the headers. - */ -#ifdef MAVLINK_CHECK_MESSAGE_LENGTH -#ifndef MAVLINK_MESSAGE_LENGTH - static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid] -#endif -#endif - - int bufferIndex = 0; - - status->msg_received = MAVLINK_FRAMING_INCOMPLETE; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - rxmsg->magic = c; - status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1; - mavlink_start_checksum(rxmsg); - } else if (c == MAVLINK_STX_MAVLINK1) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - rxmsg->magic = c; - status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received -/* Support shorter buffers than the - default maximum packet size */ -#if (MAVLINK_MAX_PAYLOAD_LEN < 255) - || c > MAVLINK_MAX_PAYLOAD_LEN -#endif - ) - { - status->buffer_overrun++; - _mav_parse_error(status); - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { - rxmsg->incompat_flags = 0; - rxmsg->compat_flags = 0; - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS; - } else { - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->incompat_flags = c; - if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) { - // message includes an incompatible feature flag - _mav_parse_error(status); - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - break; - } - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS; - break; - - case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS: - rxmsg->compat_flags = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); - if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; -#ifdef MAVLINK_CHECK_MESSAGE_LENGTH - if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid)) - { - _mav_parse_error(status); - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - break; - } -#endif - } else { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID1: - rxmsg->msgid |= c<<8; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2; - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID2: - rxmsg->msgid |= c<<16; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; -#ifdef MAVLINK_CHECK_MESSAGE_LENGTH - if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid)) - { - _mav_parse_error(status); - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - break; - } -#endif - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID3: - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: { - const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid); - uint8_t crc_extra = e?e->crc_extra:0; - mavlink_update_checksum(rxmsg, crc_extra); - if (c != (rxmsg->checksum & 0xFF)) { - status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1; - } else { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - } - rxmsg->ck[0] = c; - - // zero-fill the packet to cope with short incoming packets - if (e && status->packet_idx < e->msg_len) { - memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->msg_len - status->packet_idx); - } - break; - } - - case MAVLINK_PARSE_STATE_GOT_CRC1: - case MAVLINK_PARSE_STATE_GOT_BAD_CRC1: - if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) { - // got a bad CRC message - status->msg_received = MAVLINK_FRAMING_BAD_CRC; - } else { - // Successfully got message - status->msg_received = MAVLINK_FRAMING_OK; - } - rxmsg->ck[1] = c; - - if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) { - status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT; - status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN; - - // If the CRC is already wrong, don't overwrite msg_received, - // otherwise we can end up with garbage flagged as valid. - if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { - status->msg_received = MAVLINK_FRAMING_INCOMPLETE; - } - } else { - if (status->signing && - (status->signing->accept_unsigned_callback == NULL || - !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { - - // If the CRC is already wrong, don't overwrite msg_received. - if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { - status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; - } - } - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - } - break; - case MAVLINK_PARSE_STATE_SIGNATURE_WAIT: - rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c; - status->signature_wait--; - if (status->signature_wait == 0) { - // we have the whole signature, check it is OK - bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg); - if (!sig_ok && - (status->signing->accept_unsigned_callback && - status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { - // accepted via application level override - sig_ok = true; - } - if (sig_ok) { - status->msg_received = MAVLINK_FRAMING_OK; - } else { - status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; - } - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == MAVLINK_FRAMING_OK) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg - r_mavlink_status->parse_state = status->parse_state; - r_mavlink_status->packet_idx = status->packet_idx; - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - r_mavlink_status->flags = status->flags; - status->parse_error = 0; - - if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) { - /* - the CRC came out wrong. We now need to overwrite the - msg CRC with the one on the wire so that if the - caller decides to forward the message anyway that - mavlink_msg_to_send_buffer() won't overwrite the - checksum - */ - r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8); - } - - return status->msg_received; -} - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. This function will return 0, 1 or - * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC) - * - * Messages are parsed into an internal buffer (one for each channel). When a complete - * message is received it is copies into *returnMsg and the channel's status is - * copied into *returnStats. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to parse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @param returnStats if a message was decoded, this is filled with the channel's stats - * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC - * - * A typical use scenario of this function call is: - * - * @code - * #include - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan), - mavlink_get_channel_status(chan), - c, - r_message, - r_mavlink_status); -} - -/** - * Set the protocol version - */ -MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version) -{ - mavlink_status_t *status = mavlink_get_channel_status(chan); - if (version > 1) { - status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); - } else { - status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } -} - -/** - * Get the protocol version - * - * @return 1 for v1, 2 for v2 - */ -MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan) -{ - mavlink_status_t *status = mavlink_get_channel_status(chan); - if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) { - return 1; - } else { - return 2; - } -} - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. This function will return 0 or 1. - * - * Messages are parsed into an internal buffer (one for each channel). When a complete - * message is received it is copies into *returnMsg and the channel's status is - * copied into *returnStats. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to parse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @param returnStats if a message was decoded, this is filled with the channel's stats - * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC - * - * A typical use scenario of this function call is: - * - * @code - * #include - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status); - if (msg_received == MAVLINK_FRAMING_BAD_CRC || - msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) { - // we got a bad CRC. Treat as a parse failure - mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); - mavlink_status_t* status = mavlink_get_channel_status(chan); - _mav_parse_error(status); - status->msg_received = MAVLINK_FRAMING_INCOMPLETE; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - return 0; - } - return msg_received; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) -{ - uint16_t bits_remain = bits; - // Transform number into network order - int32_t v; - uint8_t i_bit_index, i_byte_index, curr_bits_n; -#if MAVLINK_NEED_BYTE_SWAP - union { - int32_t i; - uint8_t b[4]; - } bin, bout; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - v = bout.i; -#else - v = b; -#endif - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (uint8_t)(8 - i_bit_index)) - { - // Enough space - curr_bits_n = (uint8_t)bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -// To make MAVLink work on your MCU, define comm_send_ch() if you wish -// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a -// whole packet at a time - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - */ - -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) -{ -#ifdef MAVLINK_SEND_UART_BYTES - /* this is the more efficient approach, if the platform - defines it */ - MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len); -#else - /* fallback to one byte at a time */ - uint16_t i; - for (i = 0; i < len; i++) { - comm_send_ch(chan, (uint8_t)buf[i]); - } -#endif -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -#ifdef MAVLINK_USE_CXX_NAMESPACE -} // namespace mavlink -#endif +#pragma once + +#include "string.h" +#include "checksum.h" +#include "mavlink_types.h" +#include "mavlink_conversions.h" +#include + +#ifndef MAVLINK_HELPER +#define MAVLINK_HELPER +#endif + +#include "mavlink_sha256.h" + +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + +/* + * Internal function to give access to the channel status for each channel + */ +#ifndef MAVLINK_GET_CHANNEL_STATUS +MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ +#ifdef MAVLINK_EXTERNAL_RX_STATUS + // No m_mavlink_status array defined in function, + // has to be defined externally +#else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +#endif + return &m_mavlink_status[chan]; +} +#endif + +/* + * Internal function to give access to the channel buffer for each channel + */ +#ifndef MAVLINK_GET_CHANNEL_BUFFER +MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) +{ + +#ifdef MAVLINK_EXTERNAL_RX_BUFFER + // No m_mavlink_buffer array defined in function, + // has to be defined externally +#else + static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; +#endif + return &m_mavlink_buffer[chan]; +} +#endif // MAVLINK_GET_CHANNEL_BUFFER + +/* Enable this option to check the length of each message. + This allows invalid messages to be caught much sooner. Use if the transmission + medium is prone to missing (or extra) characters (e.g. a radio that fades in + and out). Only use if the channel will only contain messages types listed in + the headers. +*/ +//#define MAVLINK_CHECK_MESSAGE_LENGTH + +/** + * @brief Reset the status of a channel. + */ +MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + status->parse_state = MAVLINK_PARSE_STATE_IDLE; +} + +/** + * @brief create a signature block for a packet + */ +MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing, + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN], + const uint8_t *header, uint8_t header_len, + const uint8_t *packet, uint8_t packet_len, + const uint8_t crc[2]) +{ + mavlink_sha256_ctx ctx; + union { + uint64_t t64; + uint8_t t8[8]; + } tstamp; + if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) { + return 0; + } + signature[0] = signing->link_id; + tstamp.t64 = signing->timestamp; + memcpy(&signature[1], tstamp.t8, 6); + signing->timestamp++; + + mavlink_sha256_init(&ctx); + mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); + mavlink_sha256_update(&ctx, header, header_len); + mavlink_sha256_update(&ctx, packet, packet_len); + mavlink_sha256_update(&ctx, crc, 2); + mavlink_sha256_update(&ctx, signature, 7); + mavlink_sha256_final_48(&ctx, &signature[7]); + + return MAVLINK_SIGNATURE_BLOCK_LEN; +} + +/** + * @brief Trim payload of any trailing zero-populated bytes (MAVLink 2 only). + * + * @param payload Serialised payload buffer. + * @param length Length of full-width payload buffer. + * @return Length of payload after zero-filled bytes are trimmed. + */ +MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length) +{ + while (length > 1 && payload[length-1] == 0) { + length--; + } + return length; +} + +/** + * @brief check a signature block for a packet + */ +MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, + mavlink_signing_streams_t *signing_streams, + const mavlink_message_t *msg) +{ + if (signing == NULL) { + return true; + } + const uint8_t *p = (const uint8_t *)&msg->magic; + const uint8_t *psig = msg->signature; + const uint8_t *incoming_signature = psig+7; + mavlink_sha256_ctx ctx; + uint8_t signature[6]; + uint16_t i; + + mavlink_sha256_init(&ctx); + mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); + mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len); + mavlink_sha256_update(&ctx, msg->ck, 2); + mavlink_sha256_update(&ctx, psig, 1+6); + mavlink_sha256_final_48(&ctx, signature); + if (memcmp(signature, incoming_signature, 6) != 0) { + return false; + } + + // now check timestamp + union tstamp { + uint64_t t64; + uint8_t t8[8]; + } tstamp; + uint8_t link_id = psig[0]; + tstamp.t64 = 0; + memcpy(tstamp.t8, psig+1, 6); + + if (signing_streams == NULL) { + return false; + } + + // find stream + for (i=0; inum_signing_streams; i++) { + if (msg->sysid == signing_streams->stream[i].sysid && + msg->compid == signing_streams->stream[i].compid && + link_id == signing_streams->stream[i].link_id) { + break; + } + } + if (i == signing_streams->num_signing_streams) { + if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) { + // over max number of streams + return false; + } + // new stream. Only accept if timestamp is not more than 1 minute old + if (tstamp.t64 + 6000*1000UL < signing->timestamp) { + return false; + } + // add new stream + signing_streams->stream[i].sysid = msg->sysid; + signing_streams->stream[i].compid = msg->compid; + signing_streams->stream[i].link_id = link_id; + signing_streams->num_signing_streams++; + } else { + union tstamp last_tstamp; + last_tstamp.t64 = 0; + memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6); + if (tstamp.t64 <= last_tstamp.t64) { + // repeating old timestamp + return false; + } + } + + // remember last timestamp + memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6); + + // our next timestamp must be at least this timestamp + if (tstamp.t64 > signing->timestamp) { + signing->timestamp = tstamp.t64; + } + return true; +} + + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length + */ +MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra) +{ + bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; + bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); + uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0; + uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1; + uint8_t buf[MAVLINK_CORE_HEADER_LEN+1]; + if (mavlink1) { + msg->magic = MAVLINK_STX_MAVLINK1; + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1; + } else { + msg->magic = MAVLINK_STX; + } + msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length); + msg->sysid = system_id; + msg->compid = component_id; + msg->incompat_flags = 0; + if (signing) { + msg->incompat_flags |= MAVLINK_IFLAG_SIGNED; + } + msg->compat_flags = 0; + msg->seq = status->current_tx_seq; + status->current_tx_seq = status->current_tx_seq + 1; + + // form the header as a byte array for the crc + buf[0] = msg->magic; + buf[1] = msg->len; + if (mavlink1) { + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + } else { + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + } + + uint16_t checksum = crc_calculate(&buf[1], header_len-1); + crc_accumulate_buffer(&checksum, _MAV_PAYLOAD(msg), msg->len); + crc_accumulate(crc_extra, &checksum); + mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + + msg->checksum = checksum; + + if (signing) { + mavlink_sign_packet(status->signing, + msg->signature, + (const uint8_t *)buf, header_len, + (const uint8_t *)_MAV_PAYLOAD(msg), msg->len, + (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len); + } + + return msg->len + header_len + 2 + signature_len; +} + +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + return mavlink_finalize_message_buffer(msg, system_id, component_id, status, min_length, length, crc_extra); +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + */ +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t min_length, uint8_t length, uint8_t crc_extra) +{ + return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra); +} + +static inline void _mav_parse_error(mavlink_status_t *status) +{ + status->parse_error++; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); + +/** + * @brief Finalize a MAVLink message with channel assignment and send + */ +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid, + const char *packet, + uint8_t min_length, uint8_t length, uint8_t crc_extra) +{ + uint16_t checksum; + uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; + uint8_t ck[2]; + mavlink_status_t *status = mavlink_get_channel_status(chan); + uint8_t header_len = MAVLINK_CORE_HEADER_LEN; + uint8_t signature_len = 0; + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; + bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; + bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); + + if (mavlink1) { + length = min_length; + if (msgid > 255) { + // can't send 16 bit messages + _mav_parse_error(status); + return; + } + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; + buf[0] = MAVLINK_STX_MAVLINK1; + buf[1] = length; + buf[2] = status->current_tx_seq; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid & 0xFF; + } else { + uint8_t incompat_flags = 0; + if (signing) { + incompat_flags |= MAVLINK_IFLAG_SIGNED; + } + length = _mav_trim_payload(packet, length); + buf[0] = MAVLINK_STX; + buf[1] = length; + buf[2] = incompat_flags; + buf[3] = 0; // compat_flags + buf[4] = status->current_tx_seq; + buf[5] = mavlink_system.sysid; + buf[6] = mavlink_system.compid; + buf[7] = msgid & 0xFF; + buf[8] = (msgid >> 8) & 0xFF; + buf[9] = (msgid >> 16) & 0xFF; + } + status->current_tx_seq++; + checksum = crc_calculate((const uint8_t*)&buf[1], header_len); + crc_accumulate_buffer(&checksum, packet, length); + crc_accumulate(crc_extra, &checksum); + ck[0] = (uint8_t)(checksum & 0xFF); + ck[1] = (uint8_t)(checksum >> 8); + + if (signing) { + // possibly add a signature + signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1, + (const uint8_t *)packet, length, ck); + } + + MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); + _mavlink_send_uart(chan, (const char *)buf, header_len+1); + _mavlink_send_uart(chan, packet, length); + _mavlink_send_uart(chan, (const char *)ck, 2); + if (signature_len != 0) { + _mavlink_send_uart(chan, (const char *)signature, signature_len); + } + MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); +} + +/** + * @brief re-send a message over a uart channel + * this is more stack efficient than re-marshalling the message + * If the message is signed then the original signature is also sent + */ +MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg) +{ + uint8_t ck[2]; + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + // XXX use the right sequence here + + uint8_t header_len; + uint8_t signature_len; + + if (msg->magic == MAVLINK_STX_MAVLINK1) { + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1; + signature_len = 0; + MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len); + // we can't send the structure directly as it has extra mavlink2 elements in it + uint8_t buf[MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1]; + buf[0] = msg->magic; + buf[1] = msg->len; + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + _mavlink_send_uart(chan, (const char*)buf, header_len); + } else { + header_len = MAVLINK_CORE_HEADER_LEN + 1; + signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; + MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len); + uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1]; + buf[0] = msg->magic; + buf[1] = msg->len; + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + _mavlink_send_uart(chan, (const char *)buf, header_len); + } + _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); + _mavlink_send_uart(chan, (const char *)ck, 2); + if (signature_len != 0) { + _mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN); + } + MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a message to send it over a serial byte stream + */ +MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg) +{ + uint8_t signature_len, header_len; + uint8_t *ck; + uint8_t length = msg->len; + + if (msg->magic == MAVLINK_STX_MAVLINK1) { + signature_len = 0; + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; + buf[0] = msg->magic; + buf[1] = length; + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len); + ck = buf + header_len + 1 + (uint16_t)msg->len; + } else { + length = _mav_trim_payload(_MAV_PAYLOAD(msg), length); + header_len = MAVLINK_CORE_HEADER_LEN; + buf[0] = msg->magic; + buf[1] = length; + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + memcpy(&buf[10], _MAV_PAYLOAD(msg), length); + ck = buf + header_len + 1 + (uint16_t)length; + signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; + } + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + if (signature_len > 0) { + memcpy(&ck[2], msg->signature, signature_len); + } + + return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len; +} + +union __mavlink_bitfield { + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; +}; + + +MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) +{ + uint16_t crcTmp = 0; + crc_init(&crcTmp); + msg->checksum = crcTmp; +} + +MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + uint16_t checksum = msg->checksum; + crc_accumulate(c, &checksum); + msg->checksum = checksum; +} + +/* + return the crc_entry value for a msgid +*/ +#ifndef MAVLINK_GET_MSG_ENTRY +MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid) +{ + static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS; + /* + use a bisection search to find the right entry. A perfect hash may be better + Note that this assumes the table is sorted by msgid + */ + uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]) - 1; + while (low < high) { + uint32_t mid = (low+1+high)/2; + if (msgid < mavlink_message_crcs[mid].msgid) { + high = mid-1; + continue; + } + if (msgid > mavlink_message_crcs[mid].msgid) { + low = mid; + continue; + } + low = mid; + break; + } + if (mavlink_message_crcs[low].msgid != msgid) { + // msgid is not in the table + return NULL; + } + return &mavlink_message_crcs[low]; +} +#endif // MAVLINK_GET_MSG_ENTRY + +/* + return the crc_extra value for a message +*/ +MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->crc_extra:0; +} + +/* + return the min message length +*/ +#define MAVLINK_HAVE_MIN_MESSAGE_LENGTH +MAVLINK_HELPER uint8_t mavlink_min_message_length(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->min_msg_len:0; +} + +/* + return the max message length (including extensions) +*/ +#define MAVLINK_HAVE_MAX_MESSAGE_LENGTH +MAVLINK_HELPER uint8_t mavlink_max_message_length(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->max_msg_len:0; +} + +/** + * This is a variant of mavlink_frame_char() but with caller supplied + * parsing buffers. It is useful when you want to create a MAVLink + * parser in a library that doesn't use any global variables + * + * @param rxmsg parsing message buffer + * @param status parsing status buffer + * @param c The char to parse + * + * @param r_message NULL if no message could be decoded, otherwise the message data + * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats + * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC + * + */ +MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, + mavlink_status_t* status, + uint8_t c, + mavlink_message_t* r_message, + mavlink_status_t* r_mavlink_status) +{ + int bufferIndex = 0; + + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + rxmsg->magic = c; + status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1; + mavlink_start_checksum(rxmsg); + } else if (c == MAVLINK_STX_MAVLINK1) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + rxmsg->magic = c; + status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received +/* Support shorter buffers than the + default maximum packet size */ +#if (MAVLINK_MAX_PAYLOAD_LEN < 255) + || c > MAVLINK_MAX_PAYLOAD_LEN +#endif + ) + { + status->buffer_overrun++; + _mav_parse_error(status); + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { + rxmsg->incompat_flags = 0; + rxmsg->compat_flags = 0; + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->incompat_flags = c; + if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) { + // message includes an incompatible feature flag + _mav_parse_error(status); + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS; + break; + + case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS: + rxmsg->compat_flags = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); + if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { + if(rxmsg->len > 0) { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len < mavlink_min_message_length(rxmsg) || + rxmsg->len > mavlink_max_message_length(rxmsg)) { + _mav_parse_error(status); + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } +#endif + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID1: + rxmsg->msgid |= c<<8; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2; + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID2: + rxmsg->msgid |= ((uint32_t)c)<<16; + mavlink_update_checksum(rxmsg, c); + if(rxmsg->len > 0){ + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len < mavlink_min_message_length(rxmsg) || + rxmsg->len > mavlink_max_message_length(rxmsg)) + { + _mav_parse_error(status); + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } +#endif + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID3: + _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: { + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid); + uint8_t crc_extra = e?e->crc_extra:0; + mavlink_update_checksum(rxmsg, crc_extra); + if (c != (rxmsg->checksum & 0xFF)) { + status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + rxmsg->ck[0] = c; + + // zero-fill the packet to cope with short incoming packets + if (e && status->packet_idx < e->max_msg_len) { + memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx); + } + break; + } + + case MAVLINK_PARSE_STATE_GOT_CRC1: + case MAVLINK_PARSE_STATE_GOT_BAD_CRC1: + if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) { + // got a bad CRC message + status->msg_received = MAVLINK_FRAMING_BAD_CRC; + } else { + // Successfully got message + status->msg_received = MAVLINK_FRAMING_OK; + } + rxmsg->ck[1] = c; + + if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) { + status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT; + status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN; + + // If the CRC is already wrong, don't overwrite msg_received, + // otherwise we can end up with garbage flagged as valid. + if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; + } + } else { + if (status->signing && + (status->signing->accept_unsigned_callback == NULL || + !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { + + // If the CRC is already wrong, don't overwrite msg_received. + if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { + status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; + } + } + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (r_message != NULL) { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + } + break; + case MAVLINK_PARSE_STATE_SIGNATURE_WAIT: + rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c; + status->signature_wait--; + if (status->signature_wait == 0) { + // we have the whole signature, check it is OK + bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg); + if (!sig_ok && + (status->signing->accept_unsigned_callback && + status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { + // accepted via application level override + sig_ok = true; + } + if (sig_ok) { + status->msg_received = MAVLINK_FRAMING_OK; + } else { + status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; + } + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (r_message !=NULL) { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == MAVLINK_FRAMING_OK) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + if (r_message != NULL) { + r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg + } + if (r_mavlink_status != NULL) { + r_mavlink_status->parse_state = status->parse_state; + r_mavlink_status->packet_idx = status->packet_idx; + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + r_mavlink_status->flags = status->flags; + } + status->parse_error = 0; + + if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) { + /* + the CRC came out wrong. We now need to overwrite the + msg CRC with the one on the wire so that if the + caller decides to forward the message anyway that + mavlink_msg_to_send_buffer() won't overwrite the + checksum + */ + if (r_message != NULL) { + r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8); + } + } + + return status->msg_received; +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. This function will return 0, 1 or + * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC) + * + * Messages are parsed into an internal buffer (one for each channel). When a complete + * message is received it is copies into *r_message and the channel's status is + * copied into *r_mavlink_status. + * + * @param chan ID of the channel to be parsed. + * A channel is not a physical message channel like a serial port, but a logical partition of + * the communication streams. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to parse + * + * @param r_message NULL if no message could be decoded, otherwise the message data + * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats + * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC + * + * A typical use scenario of this function call is: + * + * @code + * #include + * + * mavlink_status_t status; + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_frame_char(chan, byte, &msg, &status) != MAVLINK_FRAMING_INCOMPLETE) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ + return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan), + mavlink_get_channel_status(chan), + c, + r_message, + r_mavlink_status); +} + +/** + * Set the protocol version + */ +MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + if (version > 1) { + status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + } else { + status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } +} + +/** + * Get the protocol version + * + * @return 1 for v1, 2 for v2 + */ +MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) { + return 1; + } else { + return 2; + } +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. This function will return 0 or 1. + * + * Messages are parsed into an internal buffer (one for each channel). When a complete + * message is received it is copies into *r_message and the channel's status is + * copied into *r_mavlink_status. + * + * @param chan ID of the channel to be parsed. + * A channel is not a physical message channel like a serial port, but a logical partition of + * the communication streams. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to parse + * + * @param r_message NULL if no message could be decoded, otherwise the message data + * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats + * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC + * + * A typical use scenario of this function call is: + * + * @code + * #include + * + * mavlink_status_t status; + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg, &status)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ + uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status); + if (msg_received == MAVLINK_FRAMING_BAD_CRC || + msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) { + // we got a bad CRC. Treat as a parse failure + mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); + mavlink_status_t* status = mavlink_get_channel_status(chan); + _mav_parse_error(status); + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + mavlink_start_checksum(rxmsg); + } + return 0; + } + return msg_received; +} + +/** + * @brief Put a bitfield of length 1-32 bit into the buffer + * + * @param b the value to add, will be encoded in the bitfield + * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. + * @param packet_index the position in the packet (the index of the first byte to use) + * @param bit_index the position in the byte (the index of the first bit to use) + * @param buffer packet buffer to write into + * @return new position of the last used byte in the buffer + */ +MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) +{ + uint16_t bits_remain = bits; + // Transform number into network order + int32_t v; + uint8_t i_bit_index, i_byte_index, curr_bits_n; +#if MAVLINK_NEED_BYTE_SWAP + union { + int32_t i; + uint8_t b[4]; + } bin, bout; + bin.i = b; + bout.b[0] = bin.b[3]; + bout.b[1] = bin.b[2]; + bout.b[2] = bin.b[1]; + bout.b[3] = bin.b[0]; + v = bout.i; +#else + v = b; +#endif + + // buffer in + // 01100000 01000000 00000000 11110001 + // buffer out + // 11110001 00000000 01000000 01100000 + + // Existing partly filled byte (four free slots) + // 0111xxxx + + // Mask n free bits + // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 + // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 + + // Shift n bits into the right position + // out = in >> n; + + // Mask and shift bytes + i_bit_index = bit_index; + i_byte_index = packet_index; + if (bit_index > 0) + { + // If bits were available at start, they were available + // in the byte before the current index + i_byte_index--; + } + + // While bits have not been packed yet + while (bits_remain > 0) + { + // Bits still have to be packed + // there can be more than 8 bits, so + // we might have to pack them into more than one byte + + // First pack everything we can into the current 'open' byte + //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 + //FIXME + if (bits_remain <= (uint8_t)(8 - i_bit_index)) + { + // Enough space + curr_bits_n = (uint8_t)bits_remain; + } + else + { + curr_bits_n = (8 - i_bit_index); + } + + // Pack these n bits into the current byte + // Mask out whatever was at that position with ones (xxx11111) + buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); + // Put content to this position, by masking out the non-used part + buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); + + // Increment the bit index + i_bit_index += curr_bits_n; + + // Now proceed to the next byte, if necessary + bits_remain -= curr_bits_n; + if (bits_remain > 0) + { + // Offer another 8 bits / one byte + i_byte_index++; + i_bit_index = 0; + } + } + + *r_bit_index = i_bit_index; + // If a partly filled byte is present, mark this as consumed + if (i_bit_index != 7) i_byte_index++; + return i_byte_index - packet_index; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define comm_send_ch() if you wish +// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a +// whole packet at a time + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + */ + +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) +{ +#ifdef MAVLINK_SEND_UART_BYTES + /* this is the more efficient approach, if the platform + defines it */ + MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len); +#else + /* fallback to one byte at a time */ + uint16_t i; + for (i = 0; i < len; i++) { + comm_send_ch(chan, (uint8_t)buf[i]); + } +#endif +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif diff --git a/MavLinkCom/mavlink/mavlink_sha256.h b/MavLinkCom/mavlink/mavlink_sha256.h index 317feb9e1..ff22b5f5c 100644 --- a/MavLinkCom/mavlink/mavlink_sha256.h +++ b/MavLinkCom/mavlink/mavlink_sha256.h @@ -1,252 +1,252 @@ -#pragma once - -/* - sha-256 implementation for MAVLink based on Heimdal sources, with - modifications to suit mavlink headers - */ -/* - * Copyright (c) 1995 - 2001 Kungliga Tekniska H�gskolan - * (Royal Institute of Technology, Stockholm, Sweden). - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * - * 3. Neither the name of the Institute nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - * SUCH DAMAGE. - */ - -/* - allow implementation to provide their own sha256 with the same API -*/ -#ifndef HAVE_MAVLINK_SHA256 - -#ifdef MAVLINK_USE_CXX_NAMESPACE -namespace mavlink { -#endif - -#ifndef MAVLINK_HELPER -#define MAVLINK_HELPER -#endif - -typedef struct { - unsigned int sz[2]; - uint32_t counter[8]; - union { - unsigned char save_bytes[64]; - uint32_t save_u32[16]; - } u; -} mavlink_sha256_ctx; - -#define Ch(x,y,z) (((x) & (y)) ^ ((~(x)) & (z))) -#define Maj(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z))) - -#define ROTR(x,n) (((x)>>(n)) | ((x) << (32 - (n)))) - -#define Sigma0(x) (ROTR(x,2) ^ ROTR(x,13) ^ ROTR(x,22)) -#define Sigma1(x) (ROTR(x,6) ^ ROTR(x,11) ^ ROTR(x,25)) -#define sigma0(x) (ROTR(x,7) ^ ROTR(x,18) ^ ((x)>>3)) -#define sigma1(x) (ROTR(x,17) ^ ROTR(x,19) ^ ((x)>>10)) - -#define A m->counter[0] -#define B m->counter[1] -#define C m->counter[2] -#define D m->counter[3] -#define E m->counter[4] -#define F m->counter[5] -#define G m->counter[6] -#define H m->counter[7] - -static const uint32_t mavlink_sha256_constant_256[64] = { - 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, - 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, - 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, - 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, - 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, - 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, - 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, - 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, - 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, - 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, - 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, - 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, - 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, - 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, - 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, - 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 -}; - -MAVLINK_HELPER void mavlink_sha256_init(mavlink_sha256_ctx *m) -{ - m->sz[0] = 0; - m->sz[1] = 0; - A = 0x6a09e667; - B = 0xbb67ae85; - C = 0x3c6ef372; - D = 0xa54ff53a; - E = 0x510e527f; - F = 0x9b05688c; - G = 0x1f83d9ab; - H = 0x5be0cd19; -} - -static inline void mavlink_sha256_calc(mavlink_sha256_ctx *m, uint32_t *in) -{ - uint32_t AA, BB, CC, DD, EE, FF, GG, HH; - uint32_t data[64]; - int i; - - AA = A; - BB = B; - CC = C; - DD = D; - EE = E; - FF = F; - GG = G; - HH = H; - - for (i = 0; i < 16; ++i) - data[i] = in[i]; - for (i = 16; i < 64; ++i) - data[i] = sigma1(data[i-2]) + data[i-7] + - sigma0(data[i-15]) + data[i - 16]; - - for (i = 0; i < 64; i++) { - uint32_t T1, T2; - - T1 = HH + Sigma1(EE) + Ch(EE, FF, GG) + mavlink_sha256_constant_256[i] + data[i]; - T2 = Sigma0(AA) + Maj(AA,BB,CC); - - HH = GG; - GG = FF; - FF = EE; - EE = DD + T1; - DD = CC; - CC = BB; - BB = AA; - AA = T1 + T2; - } - - A += AA; - B += BB; - C += CC; - D += DD; - E += EE; - F += FF; - G += GG; - H += HH; -} - -MAVLINK_HELPER void mavlink_sha256_update(mavlink_sha256_ctx *m, const void *v, uint32_t len) -{ - const unsigned char *p = (const unsigned char *)v; - uint32_t old_sz = m->sz[0]; - uint32_t offset; - - m->sz[0] += len * 8; - if (m->sz[0] < old_sz) - ++m->sz[1]; - offset = (old_sz / 8) % 64; - while(len > 0){ - uint32_t l = 64 - offset; - if (len < l) { - l = len; - } - memcpy(m->u.save_bytes + offset, p, l); - offset += l; - p += l; - len -= l; - if(offset == 64){ - int i; - uint32_t current[16]; - const uint32_t *u = m->u.save_u32; - for (i = 0; i < 16; i++){ - const uint8_t *p1 = (const uint8_t *)&u[i]; - uint8_t *p2 = (uint8_t *)¤t[i]; - p2[0] = p1[3]; - p2[1] = p1[2]; - p2[2] = p1[1]; - p2[3] = p1[0]; - } - mavlink_sha256_calc(m, current); - offset = 0; - } - } -} - -/* - get first 48 bits of final sha256 hash - */ -MAVLINK_HELPER void mavlink_sha256_final_48(mavlink_sha256_ctx *m, uint8_t result[6]) -{ - unsigned char zeros[72]; - unsigned offset = (m->sz[0] / 8) % 64; - unsigned int dstart = (120 - offset - 1) % 64 + 1; - uint8_t *p = (uint8_t *)&m->counter[0]; - - *zeros = 0x80; - memset (zeros + 1, 0, sizeof(zeros) - 1); - zeros[dstart+7] = (m->sz[0] >> 0) & 0xff; - zeros[dstart+6] = (m->sz[0] >> 8) & 0xff; - zeros[dstart+5] = (m->sz[0] >> 16) & 0xff; - zeros[dstart+4] = (m->sz[0] >> 24) & 0xff; - zeros[dstart+3] = (m->sz[1] >> 0) & 0xff; - zeros[dstart+2] = (m->sz[1] >> 8) & 0xff; - zeros[dstart+1] = (m->sz[1] >> 16) & 0xff; - zeros[dstart+0] = (m->sz[1] >> 24) & 0xff; - - mavlink_sha256_update(m, zeros, dstart + 8); - - // this ordering makes the result consistent with taking the first - // 6 bytes of more conventional sha256 functions. It assumes - // little-endian ordering of m->counter - result[0] = p[3]; - result[1] = p[2]; - result[2] = p[1]; - result[3] = p[0]; - result[4] = p[7]; - result[5] = p[6]; -} - -// prevent conflicts with users of the header -#undef A -#undef B -#undef C -#undef D -#undef E -#undef F -#undef G -#undef H -#undef Ch -#undef ROTR -#undef Sigma0 -#undef Sigma1 -#undef sigma0 -#undef sigma1 - -#ifdef MAVLINK_USE_CXX_NAMESPACE -} // namespace mavlink -#endif - -#endif // HAVE_MAVLINK_SHA256 +#pragma once + +/* + sha-256 implementation for MAVLink based on Heimdal sources, with + modifications to suit mavlink headers + */ +/* + * Copyright (c) 1995 - 2001 Kungliga Tekniska H�gskolan + * (Royal Institute of Technology, Stockholm, Sweden). + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the Institute nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + */ + +/* + allow implementation to provide their own sha256 with the same API +*/ +#ifndef HAVE_MAVLINK_SHA256 + +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + +#ifndef MAVLINK_HELPER +#define MAVLINK_HELPER +#endif + +typedef struct { + uint32_t sz[2]; + uint32_t counter[8]; + union { + unsigned char save_bytes[64]; + uint32_t save_u32[16]; + } u; +} mavlink_sha256_ctx; + +#define Ch(x,y,z) (((x) & (y)) ^ ((~(x)) & (z))) +#define Maj(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z))) + +#define ROTR(x,n) (((x)>>(n)) | ((x) << (32 - (n)))) + +#define Sigma0(x) (ROTR(x,2) ^ ROTR(x,13) ^ ROTR(x,22)) +#define Sigma1(x) (ROTR(x,6) ^ ROTR(x,11) ^ ROTR(x,25)) +#define sigma0(x) (ROTR(x,7) ^ ROTR(x,18) ^ ((x)>>3)) +#define sigma1(x) (ROTR(x,17) ^ ROTR(x,19) ^ ((x)>>10)) + +#define A m->counter[0] +#define B m->counter[1] +#define C m->counter[2] +#define D m->counter[3] +#define E m->counter[4] +#define F m->counter[5] +#define G m->counter[6] +#define H m->counter[7] + +static const uint32_t mavlink_sha256_constant_256[64] = { + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, + 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, + 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, + 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, + 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, + 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, + 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, + 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, + 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, + 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, + 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, + 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 +}; + +MAVLINK_HELPER void mavlink_sha256_init(mavlink_sha256_ctx *m) +{ + m->sz[0] = 0; + m->sz[1] = 0; + A = 0x6a09e667; + B = 0xbb67ae85; + C = 0x3c6ef372; + D = 0xa54ff53a; + E = 0x510e527f; + F = 0x9b05688c; + G = 0x1f83d9ab; + H = 0x5be0cd19; +} + +static inline void mavlink_sha256_calc(mavlink_sha256_ctx *m, uint32_t *in) +{ + uint32_t AA, BB, CC, DD, EE, FF, GG, HH; + uint32_t data[64]; + int i; + + AA = A; + BB = B; + CC = C; + DD = D; + EE = E; + FF = F; + GG = G; + HH = H; + + for (i = 0; i < 16; ++i) + data[i] = in[i]; + for (i = 16; i < 64; ++i) + data[i] = sigma1(data[i-2]) + data[i-7] + + sigma0(data[i-15]) + data[i - 16]; + + for (i = 0; i < 64; i++) { + uint32_t T1, T2; + + T1 = HH + Sigma1(EE) + Ch(EE, FF, GG) + mavlink_sha256_constant_256[i] + data[i]; + T2 = Sigma0(AA) + Maj(AA,BB,CC); + + HH = GG; + GG = FF; + FF = EE; + EE = DD + T1; + DD = CC; + CC = BB; + BB = AA; + AA = T1 + T2; + } + + A += AA; + B += BB; + C += CC; + D += DD; + E += EE; + F += FF; + G += GG; + H += HH; +} + +MAVLINK_HELPER void mavlink_sha256_update(mavlink_sha256_ctx *m, const void *v, uint32_t len) +{ + const unsigned char *p = (const unsigned char *)v; + uint32_t old_sz = m->sz[0]; + uint32_t offset; + + m->sz[0] += len * 8; + if (m->sz[0] < old_sz) + ++m->sz[1]; + offset = (old_sz / 8) % 64; + while(len > 0){ + uint32_t l = 64 - offset; + if (len < l) { + l = len; + } + memcpy(m->u.save_bytes + offset, p, l); + offset += l; + p += l; + len -= l; + if(offset == 64){ + int i; + uint32_t current[16]; + const uint32_t *u = m->u.save_u32; + for (i = 0; i < 16; i++){ + const uint8_t *p1 = (const uint8_t *)&u[i]; + uint8_t *p2 = (uint8_t *)¤t[i]; + p2[0] = p1[3]; + p2[1] = p1[2]; + p2[2] = p1[1]; + p2[3] = p1[0]; + } + mavlink_sha256_calc(m, current); + offset = 0; + } + } +} + +/* + get first 48 bits of final sha256 hash + */ +MAVLINK_HELPER void mavlink_sha256_final_48(mavlink_sha256_ctx *m, uint8_t result[6]) +{ + unsigned char zeros[72]; + unsigned offset = (m->sz[0] / 8) % 64; + unsigned int dstart = (120 - offset - 1) % 64 + 1; + uint8_t *p = (uint8_t *)&m->counter[0]; + + *zeros = 0x80; + memset (zeros + 1, 0, sizeof(zeros) - 1); + zeros[dstart+7] = (m->sz[0] >> 0) & 0xff; + zeros[dstart+6] = (m->sz[0] >> 8) & 0xff; + zeros[dstart+5] = (m->sz[0] >> 16) & 0xff; + zeros[dstart+4] = (m->sz[0] >> 24) & 0xff; + zeros[dstart+3] = (m->sz[1] >> 0) & 0xff; + zeros[dstart+2] = (m->sz[1] >> 8) & 0xff; + zeros[dstart+1] = (m->sz[1] >> 16) & 0xff; + zeros[dstart+0] = (m->sz[1] >> 24) & 0xff; + + mavlink_sha256_update(m, zeros, dstart + 8); + + // this ordering makes the result consistent with taking the first + // 6 bytes of more conventional sha256 functions. It assumes + // little-endian ordering of m->counter + result[0] = p[3]; + result[1] = p[2]; + result[2] = p[1]; + result[3] = p[0]; + result[4] = p[7]; + result[5] = p[6]; +} + +// prevent conflicts with users of the header +#undef A +#undef B +#undef C +#undef D +#undef E +#undef F +#undef G +#undef H +#undef Ch +#undef ROTR +#undef Sigma0 +#undef Sigma1 +#undef sigma0 +#undef sigma1 + +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif + +#endif // HAVE_MAVLINK_SHA256 diff --git a/MavLinkCom/mavlink/mavlink_types.h b/MavLinkCom/mavlink/mavlink_types.h index 08a650bc4..0531e44da 100644 --- a/MavLinkCom/mavlink/mavlink_types.h +++ b/MavLinkCom/mavlink/mavlink_types.h @@ -1,298 +1,299 @@ -#pragma once - -// Visual Studio versions before 2010 don't have stdint.h, so we just error out. -#if (defined _MSC_VER) && (_MSC_VER < 1600) -#error "The C-MAVLink implementation requires Visual Studio 2010 or greater" -#endif - -#include -#include - -#ifdef MAVLINK_USE_CXX_NAMESPACE -namespace mavlink { -#endif - -// Macro to define packed structures -#ifdef __GNUC__ - #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed)) -#else - #define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) ) -#endif - -#ifndef MAVLINK_MAX_PAYLOAD_LEN -// it is possible to override this, but be careful! -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length -#endif - -#define MAVLINK_CORE_HEADER_LEN 9 ///< Length of core header (of the comm. layer) -#define MAVLINK_CORE_HEADER_MAVLINK1_LEN 5 ///< Length of MAVLink1 core header (of the comm. layer) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and stx -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) - -#define MAVLINK_SIGNATURE_BLOCK_LEN 13 - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN) ///< Maximum packet length - -/** - * Old-style 4 byte param union - * - * This struct is the data format to be used when sending - * parameters. The parameter should be copied to the native - * type (without type conversion) - * and re-instanted on the receiving side using the - * native type as well. - */ -MAVPACKED( -typedef struct param_union { - union { - float param_float; - int32_t param_int32; - uint32_t param_uint32; - int16_t param_int16; - uint16_t param_uint16; - int8_t param_int8; - uint8_t param_uint8; - uint8_t bytes[4]; - }; - uint8_t type; -}) mavlink_param_union_t; - - -/** - * New-style 8 byte param union - * mavlink_param_union_double_t will be 8 bytes long, and treated as needing 8 byte alignment for the purposes of MAVLink 1.0 field ordering. - * The mavlink_param_union_double_t will be treated as a little-endian structure. - * - * If is_double is 1 then the type is a double, and the remaining 63 bits are the double, with the lowest bit of the mantissa zero. - * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the - * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union. - * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above, - * as bitfield ordering isn't consistent between platforms. The above is intended to be for gcc on x86, - * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number, - * and the bits pulled out using the shifts/masks. -*/ -MAVPACKED( -typedef struct param_union_extended { - union { - struct { - uint8_t is_double:1; - uint8_t mavlink_type:7; - union { - char c; - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; - float f; - uint8_t align[7]; - }; - }; - uint8_t data[8]; - }; -}) mavlink_param_union_double_t; - -/** - * This structure is required to make the mavlink_send_xxx convenience functions - * work, as it tells the library what the current system and component ID are. - */ -MAVPACKED( -typedef struct __mavlink_system { - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function -}) mavlink_system_t; - -MAVPACKED( -typedef struct __mavlink_message { - uint16_t checksum; ///< sent at end of packet - uint8_t magic; ///< protocol magic marker - uint8_t len; ///< Length of payload - uint8_t incompat_flags; ///< flags that must be understood - uint8_t compat_flags; ///< flags that can be ignored if not understood - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint32_t msgid:24; ///< ID of message in payload - uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; - uint8_t ck[2]; ///< incoming checksum bytes - uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; -}) mavlink_message_t; - -typedef enum { - MAVLINK_TYPE_CHAR = 0, - MAVLINK_TYPE_UINT8_T = 1, - MAVLINK_TYPE_INT8_T = 2, - MAVLINK_TYPE_UINT16_T = 3, - MAVLINK_TYPE_INT16_T = 4, - MAVLINK_TYPE_UINT32_T = 5, - MAVLINK_TYPE_INT32_T = 6, - MAVLINK_TYPE_UINT64_T = 7, - MAVLINK_TYPE_INT64_T = 8, - MAVLINK_TYPE_FLOAT = 9, - MAVLINK_TYPE_DOUBLE = 10 -} mavlink_message_type_t; - -#define MAVLINK_MAX_FIELDS 64 - -typedef struct __mavlink_field_info { - const char *name; // name of this field - const char *print_format; // printing format hint, or NULL - mavlink_message_type_t type; // type of this field - unsigned int array_length; // if non-zero, field is an array - unsigned int wire_offset; // offset of each field in the payload - unsigned int structure_offset; // offset in a C structure -} mavlink_field_info_t; - -// note that in this structure the order of fields is the order -// in the XML file, not necessary the wire order -typedef struct __mavlink_message_info { - uint32_t msgid; // message ID - const char *name; // name of the message - unsigned num_fields; // how many fields in this message - mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information -} mavlink_message_info_t; - -#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0]))) -#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0]))) - -// checksum is immediately after the payload bytes -#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) -#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) - -typedef enum { - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3 -} mavlink_channel_t; - -/* - * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number - * of buffers they will use. If more are used, then the result will be - * a stack overrun - */ -#ifndef MAVLINK_COMM_NUM_BUFFERS -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -# define MAVLINK_COMM_NUM_BUFFERS 16 -#else -# define MAVLINK_COMM_NUM_BUFFERS 4 -#endif -#endif - -typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS, - MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID1, - MAVLINK_PARSE_STATE_GOT_MSGID2, - MAVLINK_PARSE_STATE_GOT_MSGID3, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1, - MAVLINK_PARSE_STATE_GOT_BAD_CRC1, - MAVLINK_PARSE_STATE_SIGNATURE_WAIT -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef enum { - MAVLINK_FRAMING_INCOMPLETE=0, - MAVLINK_FRAMING_OK=1, - MAVLINK_FRAMING_BAD_CRC=2, - MAVLINK_FRAMING_BAD_SIGNATURE=3 -} mavlink_framing_t; - -#define MAVLINK_STATUS_FLAG_IN_MAVLINK1 1 // last incoming packet was MAVLink1 -#define MAVLINK_STATUS_FLAG_OUT_MAVLINK1 2 // generate MAVLink1 by default -#define MAVLINK_STATUS_FLAG_IN_SIGNED 4 // last incoming packet was signed and validated -#define MAVLINK_STATUS_FLAG_IN_BADSIG 8 // last incoming packet had a bad signature - -#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol - -typedef struct __mavlink_status { - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops - uint8_t flags; ///< MAVLINK_STATUS_FLAG_* - uint8_t signature_wait; ///< number of signature bytes left to receive - struct __mavlink_signing *signing; ///< optional signing state - struct __mavlink_signing_streams *signing_streams; ///< global record of stream timestamps -} mavlink_status_t; - -/* - a callback function to allow for accepting unsigned packets - */ -typedef bool (*mavlink_accept_unsigned_t)(const mavlink_status_t *status, uint32_t msgid); - -/* - flags controlling signing - */ -#define MAVLINK_SIGNING_FLAG_SIGN_OUTGOING 1 - -/* - state of MAVLink signing for this channel - */ -typedef struct __mavlink_signing { - uint8_t flags; ///< MAVLINK_SIGNING_FLAG_* - uint8_t link_id; - uint64_t timestamp; - uint8_t secret_key[32]; - mavlink_accept_unsigned_t accept_unsigned_callback; -} mavlink_signing_t; - -/* - timestamp state of each logical signing stream. This needs to be the same structure for all - connections in order to be secure - */ -#ifndef MAVLINK_MAX_SIGNING_STREAMS -#define MAVLINK_MAX_SIGNING_STREAMS 16 -#endif -typedef struct __mavlink_signing_streams { - uint16_t num_signing_streams; - struct __mavlink_signing_stream { - uint8_t link_id; - uint8_t sysid; - uint8_t compid; - uint8_t timestamp_bytes[6]; - } stream[MAVLINK_MAX_SIGNING_STREAMS]; -} mavlink_signing_streams_t; - - -#define MAVLINK_BIG_ENDIAN 0 -#define MAVLINK_LITTLE_ENDIAN 1 - -#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM 1 -#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT 2 - -/* - entry in table of information about each message type - */ -typedef struct __mavlink_msg_entry { - uint32_t msgid; - uint8_t crc_extra; - uint8_t msg_len; - uint8_t flags; // MAV_MSG_ENTRY_FLAG_* - uint8_t target_system_ofs; // payload offset to target_system, or 0 - uint8_t target_component_ofs; // payload offset to target_component, or 0 -} mavlink_msg_entry_t; - -/* - incompat_flags bits - */ -#define MAVLINK_IFLAG_SIGNED 0x01 -#define MAVLINK_IFLAG_MASK 0x01 // mask of all understood bits - -#ifdef MAVLINK_USE_CXX_NAMESPACE -} // namespace mavlink -#endif +#pragma once + +// Visual Studio versions before 2010 don't have stdint.h, so we just error out. +#if (defined _MSC_VER) && (_MSC_VER < 1600) +#error "The C-MAVLink implementation requires Visual Studio 2010 or greater" +#endif + +#include +#include + +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + +// Macro to define packed structures +#ifdef __GNUC__ + #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed)) +#else + #define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) ) +#endif + +#ifndef MAVLINK_MAX_PAYLOAD_LEN +// it is possible to override this, but be careful! +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length +#endif + +#define MAVLINK_CORE_HEADER_LEN 9 ///< Length of core header (of the comm. layer) +#define MAVLINK_CORE_HEADER_MAVLINK1_LEN 5 ///< Length of MAVLink1 core header (of the comm. layer) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and stx +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) + +#define MAVLINK_SIGNATURE_BLOCK_LEN 13 + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN) ///< Maximum packet length + +/** + * Old-style 4 byte param union + * + * This struct is the data format to be used when sending + * parameters. The parameter should be copied to the native + * type (without type conversion) + * and re-instanted on the receiving side using the + * native type as well. + */ +MAVPACKED( +typedef struct param_union { + union { + float param_float; + int32_t param_int32; + uint32_t param_uint32; + int16_t param_int16; + uint16_t param_uint16; + int8_t param_int8; + uint8_t param_uint8; + uint8_t bytes[4]; + }; + uint8_t type; +}) mavlink_param_union_t; + + +/** + * New-style 8 byte param union + * mavlink_param_union_double_t will be 8 bytes long, and treated as needing 8 byte alignment for the purposes of MAVLink 1.0 field ordering. + * The mavlink_param_union_double_t will be treated as a little-endian structure. + * + * If is_double is 1 then the type is a double, and the remaining 63 bits are the double, with the lowest bit of the mantissa zero. + * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the + * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union. + * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above, + * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86, + * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number, + * and the bits pulled out using the shifts/masks. +*/ +MAVPACKED( +typedef struct param_union_extended { + union { + struct { + uint8_t is_double:1; + uint8_t mavlink_type:7; + union { + char c; + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; + float f; + uint8_t align[7]; + }; + }; + uint8_t data[8]; + }; +}) mavlink_param_union_double_t; + +/** + * This structure is required to make the mavlink_send_xxx convenience functions + * work, as it tells the library what the current system and component ID are. + */ +MAVPACKED( +typedef struct __mavlink_system { + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function +}) mavlink_system_t; + +MAVPACKED( +typedef struct __mavlink_message { + uint16_t checksum; ///< sent at end of packet + uint8_t magic; ///< protocol magic marker + uint8_t len; ///< Length of payload + uint8_t incompat_flags; ///< flags that must be understood + uint8_t compat_flags; ///< flags that can be ignored if not understood + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint32_t msgid:24; ///< ID of message in payload + uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; + uint8_t ck[2]; ///< incoming checksum bytes + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; +}) mavlink_message_t; + +typedef enum { + MAVLINK_TYPE_CHAR = 0, + MAVLINK_TYPE_UINT8_T = 1, + MAVLINK_TYPE_INT8_T = 2, + MAVLINK_TYPE_UINT16_T = 3, + MAVLINK_TYPE_INT16_T = 4, + MAVLINK_TYPE_UINT32_T = 5, + MAVLINK_TYPE_INT32_T = 6, + MAVLINK_TYPE_UINT64_T = 7, + MAVLINK_TYPE_INT64_T = 8, + MAVLINK_TYPE_FLOAT = 9, + MAVLINK_TYPE_DOUBLE = 10 +} mavlink_message_type_t; + +#define MAVLINK_MAX_FIELDS 64 + +typedef struct __mavlink_field_info { + const char *name; // name of this field + const char *print_format; // printing format hint, or NULL + mavlink_message_type_t type; // type of this field + unsigned int array_length; // if non-zero, field is an array + unsigned int wire_offset; // offset of each field in the payload + unsigned int structure_offset; // offset in a C structure +} mavlink_field_info_t; + +// note that in this structure the order of fields is the order +// in the XML file, not necessary the wire order +typedef struct __mavlink_message_info { + uint32_t msgid; // message ID + const char *name; // name of the message + unsigned num_fields; // how many fields in this message + mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information +} mavlink_message_info_t; + +#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0]))) +#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0]))) + +// checksum is immediately after the payload bytes +#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) +#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) + +typedef enum { + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3 +} mavlink_channel_t; + +/* + * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number + * of buffers they will use. If more are used, then the result will be + * a stack overrun + */ +#ifndef MAVLINK_COMM_NUM_BUFFERS +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) +# define MAVLINK_COMM_NUM_BUFFERS 16 +#else +# define MAVLINK_COMM_NUM_BUFFERS 4 +#endif +#endif + +typedef enum { + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS, + MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID1, + MAVLINK_PARSE_STATE_GOT_MSGID2, + MAVLINK_PARSE_STATE_GOT_MSGID3, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1, + MAVLINK_PARSE_STATE_GOT_BAD_CRC1, + MAVLINK_PARSE_STATE_SIGNATURE_WAIT +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef enum { + MAVLINK_FRAMING_INCOMPLETE=0, + MAVLINK_FRAMING_OK=1, + MAVLINK_FRAMING_BAD_CRC=2, + MAVLINK_FRAMING_BAD_SIGNATURE=3 +} mavlink_framing_t; + +#define MAVLINK_STATUS_FLAG_IN_MAVLINK1 1 // last incoming packet was MAVLink1 +#define MAVLINK_STATUS_FLAG_OUT_MAVLINK1 2 // generate MAVLink1 by default +#define MAVLINK_STATUS_FLAG_IN_SIGNED 4 // last incoming packet was signed and validated +#define MAVLINK_STATUS_FLAG_IN_BADSIG 8 // last incoming packet had a bad signature + +#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol + +typedef struct __mavlink_status { + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops + uint8_t flags; ///< MAVLINK_STATUS_FLAG_* + uint8_t signature_wait; ///< number of signature bytes left to receive + struct __mavlink_signing *signing; ///< optional signing state + struct __mavlink_signing_streams *signing_streams; ///< global record of stream timestamps +} mavlink_status_t; + +/* + a callback function to allow for accepting unsigned packets + */ +typedef bool (*mavlink_accept_unsigned_t)(const mavlink_status_t *status, uint32_t msgid); + +/* + flags controlling signing + */ +#define MAVLINK_SIGNING_FLAG_SIGN_OUTGOING 1 ///< Enable outgoing signing + +/* + state of MAVLink signing for this channel + */ +typedef struct __mavlink_signing { + uint8_t flags; ///< MAVLINK_SIGNING_FLAG_* + uint8_t link_id; ///< Same as MAVLINK_CHANNEL + uint64_t timestamp; ///< Timestamp, in microseconds since UNIX epoch GMT + uint8_t secret_key[32]; + mavlink_accept_unsigned_t accept_unsigned_callback; +} mavlink_signing_t; + +/* + timestamp state of each logical signing stream. This needs to be the same structure for all + connections in order to be secure + */ +#ifndef MAVLINK_MAX_SIGNING_STREAMS +#define MAVLINK_MAX_SIGNING_STREAMS 16 +#endif +typedef struct __mavlink_signing_streams { + uint16_t num_signing_streams; + struct __mavlink_signing_stream { + uint8_t link_id; ///< ID of the link (MAVLINK_CHANNEL) + uint8_t sysid; ///< Remote system ID + uint8_t compid; ///< Remote component ID + uint8_t timestamp_bytes[6]; ///< Timestamp, in microseconds since UNIX epoch GMT + } stream[MAVLINK_MAX_SIGNING_STREAMS]; +} mavlink_signing_streams_t; + + +#define MAVLINK_BIG_ENDIAN 0 +#define MAVLINK_LITTLE_ENDIAN 1 + +#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM 1 +#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT 2 + +/* + entry in table of information about each message type + */ +typedef struct __mavlink_msg_entry { + uint32_t msgid; + uint8_t crc_extra; + uint8_t min_msg_len; // minimum message length + uint8_t max_msg_len; // maximum message length (e.g. including mavlink2 extensions) + uint8_t flags; // MAV_MSG_ENTRY_FLAG_* + uint8_t target_system_ofs; // payload offset to target_system, or 0 + uint8_t target_component_ofs; // payload offset to target_component, or 0 +} mavlink_msg_entry_t; + +/* + incompat_flags bits + */ +#define MAVLINK_IFLAG_SIGNED 0x01 +#define MAVLINK_IFLAG_MASK 0x01 // mask of all understood bits + +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif diff --git a/MavLinkCom/mavlink/message_definitions/common.xml b/MavLinkCom/mavlink/message_definitions/common.xml new file mode 100644 index 000000000..7863575fb --- /dev/null +++ b/MavLinkCom/mavlink/message_definitions/common.xml @@ -0,0 +1,6213 @@ + + + 3 + 0 + + + Micro air vehicle / autopilot classes. This identifies the individual model. + + Generic autopilot, full support for everything + + + Reserved for future use. + + + SLUGS autopilot, http://slugsuav.soe.ucsc.edu + + + ArduPilot - Plane/Copter/Rover/Sub/Tracker, http://ardupilot.org + + + OpenPilot, http://openpilot.org + + + Generic autopilot only supporting simple waypoints + + + Generic autopilot supporting waypoints and other simple navigation commands + + + Generic autopilot supporting the full mission command set + + + No valid autopilot, e.g. a GCS or other MAVLink component + + + PPZ UAV - http://nongnu.org/paparazzi + + + UAV Dev Board + + + FlexiPilot + + + PX4 Autopilot - http://px4.io/ + + + SMACCMPilot - http://smaccmpilot.org + + + AutoQuad -- http://autoquad.org + + + Armazila -- http://armazila.com + + + Aerob -- http://aerob.ru + + + ASLUAV autopilot -- http://www.asl.ethz.ch + + + SmartAP Autopilot - http://sky-drones.com + + + AirRails - http://uaventure.com + + + + MAVLINK component type reported in HEARTBEAT message. Flight controllers must report the type of the vehicle on which they are mounted (e.g. MAV_TYPE_OCTOROTOR). All other components must report a value appropriate for their type (e.g. a camera must use MAV_TYPE_CAMERA). + + Generic micro air vehicle + + + Fixed wing aircraft. + + + Quadrotor + + + Coaxial helicopter + + + Normal helicopter with tail rotor. + + + Ground installation + + + Operator control unit / ground control station + + + Airship, controlled + + + Free balloon, uncontrolled + + + Rocket + + + Ground rover + + + Surface vessel, boat, ship + + + Submarine + + + Hexarotor + + + Octorotor + + + Tricopter + + + Flapping wing + + + Kite + + + Onboard companion controller + + + Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. + + + Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. + + + Tiltrotor VTOL + + + + VTOL reserved 2 + + + VTOL reserved 3 + + + VTOL reserved 4 + + + VTOL reserved 5 + + + Gimbal + + + ADSB system + + + Steerable, nonrigid airfoil + + + Dodecarotor + + + Camera + + + Charging station + + + FLARM collision avoidance system + + + Servo + + + + These values define the type of firmware release. These values indicate the first version or release of this type. For example the first alpha release would be 64, the second would be 65. + + development release + + + alpha release + + + beta release + + + release candidate + + + official stable release + + + + Flags to report failure cases over the high latency telemtry. + + GPS failure. + + + Differential pressure sensor failure. + + + Absolute pressure sensor failure. + + + Accelerometer sensor failure. + + + Gyroscope sensor failure. + + + Magnetometer sensor failure. + + + Terrain subsystem failure. + + + Battery failure/critical low battery. + + + RC receiver failure/no rc connection. + + + Offboard link failure. + + + Engine failure. + + + Geofence violation. + + + Estimator failure, for example measurement rejection or large variances. + + + Mission failure. + + + + These flags encode the MAV mode. + + 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. + + + 0b01000000 remote control input is enabled. + + + 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. + + + 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. + + + 0b00001000 guided mode enabled, system flies waypoints / mission items. + + + 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. + + + 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. + + + 0b00000001 Reserved for future use. + + + + These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. + + First bit: 10000000 + + + Second bit: 01000000 + + + Third bit: 00100000 + + + Fourth bit: 00010000 + + + Fifth bit: 00001000 + + + Sixth bit: 00000100 + + + Seventh bit: 00000010 + + + Eighth bit: 00000001 + + + + Actions that may be specified in MAV_CMD_OVERRIDE_GOTO to override mission execution. + + Hold at the current position. + + + Continue with the next item in mission execution. + + + Hold at the current position of the system + + + Hold at the position specified in the parameters of the DO_HOLD action + + + + These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it + simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. + + System is not ready to fly, booting, calibrating, etc. No flag is set. + + + System is allowed to be active, under assisted RC control. + + + System is allowed to be active, under assisted RC control. + + + System is allowed to be active, under manual (RC) control, no stabilization + + + System is allowed to be active, under manual (RC) control, no stabilization + + + System is allowed to be active, under autonomous control, manual setpoint + + + System is allowed to be active, under autonomous control, manual setpoint + + + System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) + + + System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) + + + UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. + + + UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. + + + + + Uninitialized system, state is unknown. + + + System is booting up. + + + System is calibrating and not flight-ready. + + + System is grounded and on standby. It can be launched any time. + + + System is active and might be already airborne. Motors are engaged. + + + System is in a non-normal flight mode. It can however still navigate. + + + System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. + + + System just initialized its power-down sequence, will shut down now. + + + System is terminating itself. + + + + Component ids (values) for the different types and instances of onboard hardware/software that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance systems etc.). + Components must use the appropriate ID in their source address when sending messages. Components can also use IDs to determine if they are the intended recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate messages that must be processed by all components. + When creating new entries, components that can have multiple instances (e.g. cameras, servos etc.) should be allocated sequential values. An appropriate number of values should be left free after these components to allow the number of instances to be expanded. + + Target id (target_component) used to broadcast messages to all components of the receiving system. Components should attempt to process messages with this component ID and forward to components on any other interfaces. Note: This is not a valid *source* component id for a message. + + + System flight controller component ("autopilot"). Only one autopilot is expected in a particular system. + + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Camera #1. + + + Camera #2. + + + Camera #3. + + + Camera #4. + + + Camera #5. + + + Camera #6. + + + Servo #1. + + + Servo #2. + + + Servo #3. + + + Servo #4. + + + Servo #5. + + + Servo #6. + + + Servo #7. + + + Servo #8. + + + Servo #9. + + + Servo #10. + + + Servo #11. + + + Servo #12. + + + Servo #13. + + + Servo #14. + + + Gimbal #1. + + + Logging component. + + + Automatic Dependent Surveillance-Broadcast (ADS-B) component. + + + On Screen Display (OSD) devices for video links. + + + Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. + + + All gimbals should use MAV_COMP_ID_GIMBAL. + Gimbal ID for QX1. + + + FLARM collision alert component. + + + Gimbal #2. + + + Gimbal #3. + + + Gimbal #4 + + + Gimbal #5. + + + Gimbal #6. + + + Component that can generate/supply a mission flight plan (e.g. GCS or developer API). + + + Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). + + + Component that plans a collision free path between two points. + + + Component that provides position estimates using VIO techniques. + + + Inertial Measurement Unit (IMU) #1. + + + Inertial Measurement Unit (IMU) #2. + + + Inertial Measurement Unit (IMU) #3. + + + GPS #1. + + + GPS #2. + + + Component to bridge MAVLink to UDP (i.e. from a UART). + + + Component to bridge to UART (i.e. from UDP). + + + Component handling TUNNEL messages (e.g. vendor specific GUI of a component). + + + System control does not require a separate component ID. + Component for handling system messages (e.g. to ARM, takeoff, etc.). + + + + These encode the sensors whose status is sent as part of the SYS_STATUS message. + + 0x01 3D gyro + + + 0x02 3D accelerometer + + + 0x04 3D magnetometer + + + 0x08 absolute pressure + + + 0x10 differential pressure + + + 0x20 GPS + + + 0x40 optical flow + + + 0x80 computer vision position + + + 0x100 laser based position + + + 0x200 external ground truth (Vicon or Leica) + + + 0x400 3D angular rate control + + + 0x800 attitude stabilization + + + 0x1000 yaw position + + + 0x2000 z/altitude control + + + 0x4000 x/y position control + + + 0x8000 motor outputs / control + + + 0x10000 rc receiver + + + 0x20000 2nd 3D gyro + + + 0x40000 2nd 3D accelerometer + + + 0x80000 2nd 3D magnetometer + + + 0x100000 geofence + + + 0x200000 AHRS subsystem health + + + 0x400000 Terrain subsystem health + + + 0x800000 Motors are reversed + + + 0x1000000 Logging + + + 0x2000000 Battery + + + 0x4000000 Proximity + + + 0x8000000 Satellite Communication + + + + + Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). + + + Local coordinate frame, Z-down (x: north, y: east, z: down). + + + NOT a coordinate frame, indicates a mission command. + + + Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. + + + Local coordinate frame, Z-up (x: east, y: north, z: up). + + + Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL). + + + Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. + + + Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. + + + + Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. + + + + Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. + + + Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. + + + Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. + + + Body fixed frame of reference, Z-down (x: forward, y: right, z: down). + + + Body fixed frame of reference, Z-up (x: forward, y: left, z: up). + + + + Odometry local coordinate frame of data given by a motion capture system, Z-down (x: north, y: east, z: down). + + + + Odometry local coordinate frame of data given by a motion capture system, Z-up (x: east, y: north, z: up). + + + + Odometry local coordinate frame of data given by a vision estimation system, Z-down (x: north, y: east, z: down). + + + + Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: east, y: north, z: up). + + + + Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: north, y: east, z: down). + + + + Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: east, y: noth, z: up). + + + Forward, Right, Down coordinate frame. This is a local frame with Z-down and arbitrary F/R alignment (i.e. not aligned with NED/earth frame). + + + Forward, Left, Up coordinate frame. This is a local frame with Z-up and arbitrary F/L alignment (i.e. not aligned with ENU/earth frame). + + + + + + + + + + + + + + + + + + + + + + + + + + Disable fenced mode + + + Switched to guided mode to return point (fence point 0) + + + Report fence breach, but don't take action + + + Switched to guided mode to return point (fence point 0) with manual throttle control + + + Switch to RTL (return to launch) mode and head for the return point. + + + + + No last fence breach + + + Breached minimum altitude + + + Breached maximum altitude + + + Breached fence boundary + + + + + Actions being taken to mitigate/prevent fence breach + + Unknown + + + No actions being taken + + + Velocity limiting active to prevent breach + + + + + Enumeration of possible mount operation modes + + Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization + + + Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. + + + Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization + + + Load neutral position and start RC Roll,Pitch,Yaw control with stabilization + + + Load neutral position and start to point to Lat,Lon,Alt + + + + + Generalized UAVCAN node health + + The node is functioning properly. + + + A critical parameter went out of range or the node has encountered a minor failure. + + + The node has encountered a major failure. + + + The node has suffered a fatal malfunction. + + + + + Generalized UAVCAN node mode + + The node is performing its primary functions. + + + The node is initializing; this mode is entered immediately after startup. + + + The node is under maintenance. + + + The node is in the process of updating its software. + + + The node is no longer available online. + + + + Flags to indicate the status of camera storage. + + Storage is missing (no microSD card loaded for example.) + + + Storage present but unformatted. + + + Storage present and ready. + + + Camera does not supply storage status information. Capacity information in STORAGE_INFORMATION fields will be ignored. + + + + Yaw behaviour during orbit flight. + + Vehicle front points to the center (default). + + + Vehicle front holds heading when message received. + + + Yaw uncontrolled. + + + Vehicle front follows flight path (tangential to circle). + + + Yaw controlled by RC input. + + + + + + + + Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. See https://mavlink.io/en/guide/xml_schema.html#MAV_CMD for information about the structure of the MAV_CMD entries + + Navigate to waypoint. + Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached) + 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. + Desired yaw angle at waypoint (rotary wing). NaN for unchanged. + Latitude + Longitude + Altitude + + + Loiter around this waypoint an unlimited amount of time + Empty + Empty + Radius around waypoint. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. NaN for unchanged. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X turns + Number of turns. + Empty + Radius around waypoint. If positive loiter clockwise, else counter-clockwise + Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location. Else, this is desired yaw angle. NaN for unchanged. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X seconds + Loiter time. + Empty + Radius around waypoint. If positive loiter clockwise, else counter-clockwise. + Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location. Else, this is desired yaw angle. NaN for unchanged. + Latitude + Longitude + Altitude + + + Return to launch location + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Land at location. + Minimum target altitude if landing is aborted (0 = undefined/use system default). + Precision land mode. + Empty. + Desired yaw angle. NaN for unchanged. + Latitude. + Longitude. + Landing altitude (ground level in current frame). + + + Takeoff from ground / hand + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Empty + Yaw angle (if magnetometer present), ignored without magnetometer. NaN for unchanged. + Latitude + Longitude + Altitude + + + Land at local position (local frame only) + Landing target number (if available) + Maximum accepted offset from desired landing position - computed magnitude from spherical coordinates: d = sqrt(x^2 + y^2 + z^2), which gives the maximum accepted distance between the desired landing position and the position where the vehicle is about to land + Landing descend rate + Desired yaw angle + Y-axis position + X-axis position + Z-axis / ground level position + + + Takeoff from local position (local frame only) + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Takeoff ascend rate + Yaw angle (if magnetometer or another yaw estimation source present), ignored without one of these + Y-axis position + X-axis position + Z-axis position + + + Vehicle following, i.e. this waypoint represents the position of a moving vehicle + Following logic to use (e.g. loitering or sinusoidal following) - depends on specific autopilot implementation + Ground speed of vehicle to be followed + Radius around waypoint. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. + Climb or Descend (0 = Neutral, command completes when within 5m of this command's altitude, 1 = Climbing, command completes when at or above this command's altitude, 2 = Descending, command completes when at or below this command's altitude. + Empty + Empty + Empty + Empty + Empty + Desired altitude + + + Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. + Heading Required (0 = False) + Radius. If positive loiter clockwise, negative counter-clockwise, 0 means no change to standard loiter. + Empty + Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location + Latitude + Longitude + Altitude + + + Begin following a target + System ID (of the FOLLOW_TARGET beacon). Send 0 to disable follow-me and return to the default position hold mode. + RESERVED + RESERVED + Altitude mode: 0: Keep current altitude, 1: keep altitude difference to target, 2: go to a fixed altitude above home. + Altitude above home. (used if mode=2) + RESERVED + Time to land in which the MAV should go to the default position hold mode after a message RX timeout. + + + Reposition the MAV after a follow target command has been sent + Camera q1 (where 0 is on the ray from the camera to the tracking device) + Camera q2 + Camera q3 + Camera q4 + altitude offset from target + X offset from target + Y offset from target + + + + + Start orbiting on the circumference of a circle defined by the parameters. Setting any value NaN results in using defaults. + Radius of the circle. positive: Orbit clockwise. negative: Orbit counter-clockwise. + Tangential Velocity. NaN: Vehicle configuration default. + Yaw behavior of the vehicle. + Reserved (e.g. for dynamic center beacon options) + Center point latitude (if no MAV_FRAME specified) / X coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting. + Center point longitude (if no MAV_FRAME specified) / Y coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting. + Center point altitude (MSL) (if no MAV_FRAME specified) / Z coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting. + + + + Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Region of interest mode. + Waypoint index/ target ID. (see MAV_ROI enum) + ROI index (allows a vehicle to manage multiple ROI's) + Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z + + + Control autonomous path planning on the MAV. + 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning + 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid + Empty + Yaw angle at goal + Latitude/X of goal + Longitude/Y of goal + Altitude/Z of goal + + + Navigate to waypoint using a spline path. + Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Empty + Empty + Empty + Latitude/X of goal + Longitude/Y of goal + Altitude/Z of goal + + + Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. + Empty + Front transition heading. + Empty + Yaw angle. NaN for unchanged. + Latitude + Longitude + Altitude + + + Land using VTOL mode + Empty + Empty + Approach altitude (with the same reference as the Altitude field). NaN if unspecified. + Yaw angle. NaN for unchanged. + Latitude + Longitude + Altitude (ground level) + + + + hand control over to an external controller + On / Off (> 0.5f on) + Empty + Empty + Empty + Empty + Empty + Empty + + + Delay the next navigation command a number of seconds or until a specified time + Delay (-1 to enable time-of-day fields) + hour (24h format, UTC, -1 to ignore) + minute (24h format, UTC, -1 to ignore) + second (24h format, UTC) + Empty + Empty + Empty + + + Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload. + Maximum distance to descend. + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Delay mission state machine. + Delay + Empty + Empty + Empty + Empty + Empty + Empty + + + Ascend/descend at rate. Delay mission state machine until desired altitude reached. + Descent / Ascend rate. + Empty + Empty + Empty + Empty + Empty + Finish Altitude + + + Delay mission state machine until within desired distance of next NAV point. + Distance. + Empty + Empty + Empty + Empty + Empty + Empty + + + Reach a certain target angle. + target angle, 0 is north + angular speed + direction: -1: counter clockwise, 1: clockwise + 0: absolute angle, 1: relative offset + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Set system mode. + Mode + Custom mode - this is system specific, please refer to the individual autopilot specifications for details. + Custom sub mode - this is system specific, please refer to the individual autopilot specifications for details. + Empty + Empty + Empty + Empty + + + Jump to the desired command in the mission list. Repeat this action only the specified number of times + Sequence number + Repeat count + Empty + Empty + Empty + Empty + Empty + + + Change speed and/or throttle set points. + Speed type (0=Airspeed, 1=Ground Speed, 2=Climb Speed, 3=Descent Speed) + Speed (-1 indicates no change) + Throttle (-1 indicates no change) + 0: absolute, 1: relative + Empty + Empty + Empty + + + Changes the home location either to the current location or a specified location. + Use current (1=use current location, 0=use specified location) + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. + Parameter number + Parameter value + Empty + Empty + Empty + Empty + Empty + + + Set a relay to a condition. + Relay instance number. + Setting. (1=on, 0=off, others possible depending on system hardware) + Empty + Empty + Empty + Empty + Empty + + + Cycle a relay on and off for a desired number of cycles with a desired period. + Relay instance number. + Cycle count. + Cycle time. + Empty + Empty + Empty + Empty + + + Set a servo to a desired PWM value. + Servo instance number. + Pulse Width Modulation. + Empty + Empty + Empty + Empty + Empty + + + Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. + Servo instance number. + Pulse Width Modulation. + Cycle count. + Cycle time. + Empty + Empty + Empty + + + Terminate flight immediately + Flight termination activated if > 0.5 + Empty + Empty + Empty + Empty + Empty + Empty + + + Change altitude set point. + Altitude. + Frame of new altitude. + Empty + Empty + Empty + Empty + Empty + + + Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence. + Empty + Empty + Empty + Empty + Latitude + Longitude + Empty + + + Mission command to perform a landing from a rally point. + Break altitude + Landing speed + Empty + Empty + Empty + Empty + Empty + + + Mission command to safely abort an autonomous landing. + Altitude + Empty + Empty + Empty + Empty + Empty + Empty + + + Reposition the vehicle to a specific WGS84 global position. + Ground speed, less than 0 (-1) for default + Bitmask of option flags. + Reserved + Yaw heading, NaN for unchanged. For planes indicates loiter direction (0: clockwise, 1: counter clockwise) + Latitude + Longitude + Altitude (meters) + + + If in a GPS controlled position mode, hold the current position or continue. + 0: Pause current mission or reposition command, hold current position. 1: Continue mission. A VTOL capable vehicle should enter hover mode (multicopter and VTOL planes). A plane should loiter with the default loiter radius. + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + + + Set moving direction to forward or reverse. + Direction (0=Forward, 1=Reverse) + Empty + Empty + Empty + Empty + Empty + Empty + + + Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Empty + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Empty + Empty + Empty + Empty + pitch offset from next waypoint + roll offset from next waypoint + yaw offset from next waypoint + + + Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Control onboard camera system. + Camera ID (-1 for all) + Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw + Transmission mode: 0: video stream, >0: single images every n seconds + Recording: 0: disabled, 1: enabled compressed, 2: enabled raw + Empty + Empty + Empty + + + + Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Region of interest mode. + Waypoint index/ target ID (depends on param 1). + Region of interest index. (allows a vehicle to manage multiple ROI's) + Empty + MAV_ROI_WPNEXT: pitch offset from next waypoint, MAV_ROI_LOCATION: latitude + MAV_ROI_WPNEXT: roll offset from next waypoint, MAV_ROI_LOCATION: longitude + MAV_ROI_WPNEXT: yaw offset from next waypoint, MAV_ROI_LOCATION: altitude + + + + Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). + Modes: P, TV, AV, M, Etc. + Shutter speed: Divisor number for one second. + Aperture: F stop number. + ISO number e.g. 80, 100, 200, Etc. + Exposure type enumerator. + Command Identity. + Main engine cut-off time before camera trigger. (0 means no cut-off) + + + Control digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). + Session control e.g. show/hide lens + Zoom's absolute position + Zooming step value to offset zoom from the current position + Focus Locking, Unlocking or Re-locking + Shooting Command + Command Identity + Test shot identifier. If set to 1, image will only be captured, but not counted towards internal frame count. + + + + Mission command to configure a camera or antenna mount + Mount operation mode + stabilize roll? (1 = yes, 0 = no) + stabilize pitch? (1 = yes, 0 = no) + stabilize yaw? (1 = yes, 0 = no) + roll input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame) + pitch input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame) + yaw input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame) + + + + Mission command to control a camera or antenna mount + pitch depending on mount mode (degrees or degrees/second depending on pitch input). + roll depending on mount mode (degrees or degrees/second depending on roll input). + yaw depending on mount mode (degrees or degrees/second depending on yaw input). + altitude depending on mount mode. + latitude, set if appropriate mount mode. + longitude, set if appropriate mount mode. + Mount mode. + + + Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera. + Camera trigger distance. 0 to stop triggering. + Camera shutter integration time. -1 or 0 to ignore + Trigger camera once immediately. (0 = no trigger, 1 = trigger) + Empty + Empty + Empty + Empty + + + Mission command to enable the geofence + enable? (0=disable, 1=enable, 2=disable_floor_only) + Empty + Empty + Empty + Empty + Empty + Empty + + + Mission command to trigger a parachute + action + Empty + Empty + Empty + Empty + Empty + Empty + + + Mission command to perform motor test. + Motor instance number. (from 1 to max number of motors on the vehicle) + Throttle type. + Throttle. + Timeout. + Motor count. (number of motors to test to test in sequence, waiting for the timeout above between them; 0=1 motor, 1=1 motor, 2=2 motors...) + Motor test order. + Empty + + + Change to/from inverted flight. + Inverted flight. (0=normal, 1=inverted) + Empty + Empty + Empty + Empty + Empty + Empty + + + + Sets a desired vehicle turn angle and speed change. + Yaw angle to adjust steering by. + Speed. + Final angle. (0=absolute, 1=relative) + Empty + Empty + Empty + Empty + + + Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. + Camera trigger cycle time. -1 or 0 to ignore. + Camera shutter integration time. Should be less than trigger cycle time. -1 or 0 to ignore. + Empty + Empty + Empty + Empty + Empty + + + Mission command to control a camera or antenna mount, using a quaternion as reference. + quaternion param q1, w (1 in null-rotation) + quaternion param q2, x (0 in null-rotation) + quaternion param q3, y (0 in null-rotation) + quaternion param q4, z (0 in null-rotation) + Empty + Empty + Empty + + + set id of master controller + System ID + Component ID + Empty + Empty + Empty + Empty + Empty + + + Set limits for external control + Timeout - maximum time that external controller will be allowed to control vehicle. 0 means no timeout. + Altitude (MSL) min - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit. + Altitude (MSL) max - if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit. + Horizontal move limit - if vehicle moves more than this distance from its location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal move limit. + Empty + Empty + Empty + + + Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines + 0: Stop engine, 1:Start Engine + 0: Warm start, 1:Cold start. Controls use of choke where applicable + Height delay. This is for commanding engine start only after the vehicle has gained the specified height. Used in VTOL vehicles during takeoff to start engine after the aircraft is off the ground. Zero for no delay. + Empty + Empty + Empty + Empty + Empty + + + Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). + Mission sequence value to set + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the DO commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. + 1: gyro calibration, 3: gyro temperature calibration + 1: magnetometer calibration + 1: ground pressure calibration + 1: radio RC calibration, 2: RC trim calibration + 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration + 1: APM: compass/motor interference calibration (PX4: airspeed calibration, deprecated), 2: airspeed calibration + 1: ESC calibration, 3: barometer temperature calibration + + + Set sensor offsets. This command will be only accepted if in pre-flight mode. + Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer, 6: third magnetometer + X axis offset (or generic dimension 1), in the sensor's raw units + Y axis offset (or generic dimension 2), in the sensor's raw units + Z axis offset (or generic dimension 3), in the sensor's raw units + Generic dimension 4, in the sensor's raw units + Generic dimension 5, in the sensor's raw units + Generic dimension 6, in the sensor's raw units + + + Trigger UAVCAN config. This command will be only accepted if in pre-flight mode. + 1: Trigger actuator ID assignment and direction mapping. + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + + + Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. + Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults + Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults + Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: logging rate (e.g. set to 1000 for 1000 Hz logging) + Reserved + Empty + Empty + Empty + + + Request the reboot or shutdown of system components. + 0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot, 3: Reboot autopilot and keep it in the bootloader until upgraded. + 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer, 3: Reboot onboard computer and keep it in the bootloader until upgraded. + WIP: 0: Do nothing for camera, 1: Reboot onboard camera, 2: Shutdown onboard camera, 3: Reboot onboard camera and keep it in the bootloader until upgraded + WIP: 0: Do nothing for mount (e.g. gimbal), 1: Reboot mount, 2: Shutdown mount, 3: Reboot mount and keep it in the bootloader until upgraded + Reserved, send 0 + Reserved, send 0 + WIP: ID (e.g. camera ID -1 for all IDs) + + + Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position. + MAV_GOTO_DO_HOLD: pause mission and either hold or move to specified position (depending on param2), MAV_GOTO_DO_CONTINUE: resume mission. + MAV_GOTO_HOLD_AT_CURRENT_POSITION: hold at current position, MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position. + Coordinate frame of hold point. + Desired yaw angle. + Latitude / X position. + Longitude / Y position. + Altitude / Z position. + + + start running a mission + first_item: the first mission item to run + last_item: the last mission item to run (after this item is run, the mission ends) + + + Arms / Disarms a component + 0: disarm, 1: arm + 0: arm-disarm unless prevented by safety checks (i.e. when landed), 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight) + + + + + Turns illuminators ON/OFF. An illuminator is a light source that is used for lighting up dark areas external to the sytstem: e.g. a torch or searchlight (as opposed to a light source for illuminating the system itself, e.g. an indicator light). + 0: Illuminators OFF, 1: Illuminators ON + + + Request the home position from the vehicle. + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + + + Starts receiver pairing. + 0:Spektrum. + RC type. + + + Request the interval between messages for a particular MAVLink message ID. The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message. + The MAVLink message ID + + + Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM. + The MAVLink message ID + The interval between two messages. Set to -1 to disable and 0 to request default rate. + Target address of message stream (if message has target address fields). 0: Flight-stack default (recommended), 1: address of requestor, 2: broadcast. + + + Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). + The MAVLink message ID of the requested message. + Index id (if appropriate). The use of this parameter (if any), must be defined in the requested message. + The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0). + The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0). + The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0). + The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0). + Target address for requested message (if message has target address fields). 0: Flight-stack default, 1: address of requestor, 2: broadcast. + + + Request MAVLink protocol version compatibility + 1: Request supported protocol versions by all nodes on the network + Reserved (all remaining params) + + + Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message + 1: Request autopilot version + Reserved (all remaining params) + + + Request camera information (CAMERA_INFORMATION). + 0: No action 1: Request camera capabilities + Reserved (all remaining params) + + + Request camera settings (CAMERA_SETTINGS). + 0: No Action 1: Request camera settings + Reserved (all remaining params) + + + Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage. + Storage ID (0 for all, 1 for first, 2 for second, etc.) + 0: No Action 1: Request storage information + Reserved (all remaining params) + + + Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage. + Storage ID (1 for first, 2 for second, etc.) + 0: No action 1: Format storage + Reserved (all remaining params) + + + Request camera capture status (CAMERA_CAPTURE_STATUS) + 0: No Action 1: Request camera capture status + Reserved (all remaining params) + + + + + Request flight information (FLIGHT_INFORMATION) + 1: Request flight information + Reserved (all remaining params) + + + Reset all camera settings to Factory Default + 0: No Action 1: Reset all settings + Reserved (all remaining params) + + + Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming. + Reserved (Set to 0) + Camera mode + Reserved (all remaining params) + + + + + Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). Use NaN for reserved values. + Zoom type + Zoom value. The range of valid values depend on the zoom type. + Reserved (all remaining params) + + + + + Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). Use NaN for reserved values. + Focus type + Focus value + Reserved (all remaining params) + + + Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. + Tag. + + + Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. + Target tag to jump to. + Repeat count. + + + Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. + Reserved (Set to 0) + Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds). + Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE. + Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1). Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted. Use 0 to ignore it. + Reserved (all remaining params) + + + Stop image capture sequence Use NaN for reserved values. + Reserved (Set to 0) + Reserved (all remaining params) + + + + + Re-request a CAMERA_IMAGE_CAPTURE message. Use NaN for reserved values. + Sequence number for missing CAMERA_IMAGE_CAPTURE message + Reserved (all remaining params) + + + Enable or disable on-board camera triggering system. + Trigger enable/disable (0 for disable, 1 for start), -1 to ignore + 1 to reset the trigger sequence, -1 or 0 to ignore + 1 to pause triggering, but without switching the camera off or retracting it. -1 to ignore + + + Starts video capture (recording). Use NaN for reserved values. + Video Stream ID (0 for all streams) + Frequency CAMERA_CAPTURE_STATUS messages should be sent while recording (0 for no messages, otherwise frequency) + Reserved (all remaining params) + + + Stop the current video capture (recording). Use NaN for reserved values. + Video Stream ID (0 for all streams) + Reserved (all remaining params) + + + + + Start video streaming + Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + Reserved + + + + + Stop the given video stream + Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + Reserved + + + + + Request video stream information (VIDEO_STREAM_INFORMATION) + Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + Reserved (all remaining params) + + + + + Request video stream status (VIDEO_STREAM_STATUS) + Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + Reserved (all remaining params) + + + Request to start streaming logging data over MAVLink (see also LOGGING_DATA message) + Format: 0: ULog + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + + + Request to stop streaming log data over MAVLink + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + + + + Landing gear ID (default: 0, -1 for all) + Landing gear position (Down: 0, Up: 1, NaN for no change) + Reserved, set to NaN + Reserved, set to NaN + Reserved, set to NaN + Reserved, set to NaN + Reserved, set to NaN + + + Request to start/stop transmitting over the high latency telemetry + Control transmission over high latency telemetry (0: stop, 1: start) + Empty + Empty + Empty + Empty + Empty + Empty + + + Create a panorama at the current position + Viewing angle horizontal of the panorama (+- 0.5 the total angle) + Viewing angle vertical of panorama. + Speed of the horizontal rotation. + Speed of the vertical rotation. + + + Request VTOL transition + The target VTOL state. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used. + + + Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. + + Vehicle system id, this way ground station can request arm authorization on behalf of any vehicle + + + This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes. + + + + This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. + + Radius of desired circle in CIRCLE_MODE + User defined + User defined + User defined + Unscaled target latitude of center of circle in CIRCLE_MODE + Unscaled target longitude of center of circle in CIRCLE_MODE + + + + + Delay mission state machine until gate has been reached. + Geometry: 0: orthogonal to path between previous and next waypoint. + Altitude: 0: ignore altitude + Empty + Empty + Latitude + Longitude + Altitude + + + Fence return point. There can only be one fence return point. + + Reserved + Reserved + Reserved + Reserved + Latitude + Longitude + Altitude + + + Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. + + Polygon vertex count + Reserved + Reserved + Reserved + Latitude + Longitude + Reserved + + + Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. + + Polygon vertex count + Reserved + Reserved + Reserved + Latitude + Longitude + Reserved + + + Circular fence area. The vehicle must stay inside this area. + + Radius. + Reserved + Reserved + Reserved + Latitude + Longitude + Reserved + + + Circular fence area. The vehicle must stay outside this area. + + Radius. + Reserved + Reserved + Reserved + Latitude + Longitude + Reserved + + + Rally point. You can have multiple rally points defined. + + Reserved + Reserved + Reserved + Reserved + Latitude + Longitude + Altitude + + + + + Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages. + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + + + + + + Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. + Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list. + Desired approach vector in compass heading. A negative value indicates the system can define the approach vector at will. + Desired ground speed at release time. This can be overridden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will. + Minimum altitude clearance to the release position. A negative value indicates the system can define the clearance at will. + Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT + Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT + Altitude (MSL), in meters + + + Control the payload deployment. + Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deployment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests. + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + + + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL), in meters + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + + + + A data stream is not a fixed set of messages, but rather a + recommendation to the autopilot software. Individual autopilots may or may not obey + the recommended messages. + + Enable all data streams + + + Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. + + + Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS + + + Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW + + + Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. + + + Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. + + + Dependent on the autopilot + + + Dependent on the autopilot + + + Dependent on the autopilot + + + + + The ROI (region of interest) for the vehicle. This can be + be used by the vehicle for camera/vehicle attitude alignment (see + MAV_CMD_NAV_ROI). + + No region of interest. + + + Point toward next waypoint, with optional pitch/roll/yaw offset. + + + Point toward given waypoint. + + + Point toward fixed location. + + + Point toward of given id. + + + + ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. + + Command / mission item is ok. + + + Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. + + + The system is refusing to accept this command from this source / communication partner. + + + Command or mission item is not supported, other commands would be accepted. + + + The coordinate frame of this command / mission item is not supported. + + + The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. + + + The X or latitude value is out of range. + + + The Y or longitude value is out of range. + + + The Z or altitude value is out of range. + + + + Specifies the datatype of a MAVLink parameter. + + 8-bit unsigned integer + + + 8-bit signed integer + + + 16-bit unsigned integer + + + 16-bit signed integer + + + 32-bit unsigned integer + + + 32-bit signed integer + + + 64-bit unsigned integer + + + 64-bit signed integer + + + 32-bit floating-point + + + 64-bit floating-point + + + + Specifies the datatype of a MAVLink extended parameter. + + 8-bit unsigned integer + + + 8-bit signed integer + + + 16-bit unsigned integer + + + 16-bit signed integer + + + 32-bit unsigned integer + + + 32-bit signed integer + + + 64-bit unsigned integer + + + 64-bit signed integer + + + 32-bit floating-point + + + 64-bit floating-point + + + Custom Type + + + + Result from a MAVLink command (MAV_CMD) + + Command is valid (is supported and has valid parameters), and was executed. + + + Command is valid, but cannot be executed at this time. This is used to indicate a problem that should be fixed just by waiting (e.g. a state machine is busy, can't arm because have not got GPS lock, etc.). Retrying later should work. + + + Command is invalid (is supported but has invalid parameters). Retrying same command and parameters will not work. + + + Command is not supported (unknown). + + + Command is valid, but execution has failed. This is used to indicate any non-temporary or unexpected problem, i.e. any problem that must be fixed before the command can succeed/be retried. For example, attempting to write a file when out of memory, attempting to arm when sensors are not calibrated, etc. + + + Command is valid and is being executed. This will be followed by further progress updates, i.e. the component may send further COMMAND_ACK messages with result MAV_RESULT_IN_PROGRESS (at a rate decided by the implementation), and must terminate by sending a COMMAND_ACK message with final result of the operation. The COMMAND_ACK.progress field can be used to indicate the progress of the operation. There is no need for the sender to retry the command, but if done during execution, the component will return MAV_RESULT_IN_PROGRESS with an updated progress. + + + + Result of mission operation (in a MISSION_ACK message). + + mission accepted OK + + + Generic error / not accepting mission commands at all right now. + + + Coordinate frame is not supported. + + + Command is not supported. + + + Mission item exceeds storage space. + + + One of the parameters has an invalid value. + + + param1 has an invalid value. + + + param2 has an invalid value. + + + param3 has an invalid value. + + + param4 has an invalid value. + + + x / param5 has an invalid value. + + + y / param6 has an invalid value. + + + z / param7 has an invalid value. + + + Mission item received out of sequence + + + Not accepting any mission commands from this communication partner. + + + Current mission operation cancelled (e.g. mission upload, mission download). + + + + Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. + + System is unusable. This is a "panic" condition. + + + Action should be taken immediately. Indicates error in non-critical systems. + + + Action must be taken immediately. Indicates failure in a primary system. + + + Indicates an error in secondary/redundant systems. + + + Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. + + + An unusual event has occurred, though not an error condition. This should be investigated for the root cause. + + + Normal operational messages. Useful for logging. No action is required for these messages. + + + Useful non-operational messages that can assist in debugging. These should not occur during normal operation. + + + + Power supply status flags (bitmask) + + main brick power supply valid + + + main servo power supply valid for FMU + + + USB power is connected + + + peripheral supply is in over-current state + + + hi-power peripheral supply is in over-current state + + + Power status has changed since boot + + + + SERIAL_CONTROL device types + + First telemetry port + + + Second telemetry port + + + First GPS port + + + Second GPS port + + + system shell + + + + SERIAL_CONTROL flags (bitmask) + + Set if this is a reply + + + Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message + + + Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set + + + Block on writes to the serial port + + + Send multiple replies until port is drained + + + + Enumeration of distance sensor types + + Laser rangefinder, e.g. LightWare SF02/F or PulsedLight units + + + Ultrasound rangefinder, e.g. MaxBotix units + + + Infrared rangefinder, e.g. Sharp units + + + Radar type, e.g. uLanding units + + + Broken or unknown type, e.g. analog units + + + + Enumeration of sensor orientation, according to its rotations + + Roll: 0, Pitch: 0, Yaw: 0 + + + Roll: 0, Pitch: 0, Yaw: 45 + + + Roll: 0, Pitch: 0, Yaw: 90 + + + Roll: 0, Pitch: 0, Yaw: 135 + + + Roll: 0, Pitch: 0, Yaw: 180 + + + Roll: 0, Pitch: 0, Yaw: 225 + + + Roll: 0, Pitch: 0, Yaw: 270 + + + Roll: 0, Pitch: 0, Yaw: 315 + + + Roll: 180, Pitch: 0, Yaw: 0 + + + Roll: 180, Pitch: 0, Yaw: 45 + + + Roll: 180, Pitch: 0, Yaw: 90 + + + Roll: 180, Pitch: 0, Yaw: 135 + + + Roll: 0, Pitch: 180, Yaw: 0 + + + Roll: 180, Pitch: 0, Yaw: 225 + + + Roll: 180, Pitch: 0, Yaw: 270 + + + Roll: 180, Pitch: 0, Yaw: 315 + + + Roll: 90, Pitch: 0, Yaw: 0 + + + Roll: 90, Pitch: 0, Yaw: 45 + + + Roll: 90, Pitch: 0, Yaw: 90 + + + Roll: 90, Pitch: 0, Yaw: 135 + + + Roll: 270, Pitch: 0, Yaw: 0 + + + Roll: 270, Pitch: 0, Yaw: 45 + + + Roll: 270, Pitch: 0, Yaw: 90 + + + Roll: 270, Pitch: 0, Yaw: 135 + + + Roll: 0, Pitch: 90, Yaw: 0 + + + Roll: 0, Pitch: 270, Yaw: 0 + + + Roll: 0, Pitch: 180, Yaw: 90 + + + Roll: 0, Pitch: 180, Yaw: 270 + + + Roll: 90, Pitch: 90, Yaw: 0 + + + Roll: 180, Pitch: 90, Yaw: 0 + + + Roll: 270, Pitch: 90, Yaw: 0 + + + Roll: 90, Pitch: 180, Yaw: 0 + + + Roll: 270, Pitch: 180, Yaw: 0 + + + Roll: 90, Pitch: 270, Yaw: 0 + + + Roll: 180, Pitch: 270, Yaw: 0 + + + Roll: 270, Pitch: 270, Yaw: 0 + + + Roll: 90, Pitch: 180, Yaw: 90 + + + Roll: 90, Pitch: 0, Yaw: 270 + + + Roll: 90, Pitch: 68, Yaw: 293 + + + Pitch: 315 + + + Roll: 90, Pitch: 315 + + + Custom orientation + + + + Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot supports this capability. + + Autopilot supports MISSION float message type. + + + Autopilot supports the new param float message type. + + + Autopilot supports MISSION_INT scaled integer message type. + + + Autopilot supports COMMAND_INT scaled integer message type. + + + Autopilot supports the new param union message type. + + + Autopilot supports the new FILE_TRANSFER_PROTOCOL message type. + + + Autopilot supports commanding attitude offboard. + + + Autopilot supports commanding position and velocity targets in local NED frame. + + + Autopilot supports commanding position and velocity targets in global scaled integers. + + + Autopilot supports terrain protocol / data handling. + + + Autopilot supports direct actuator control. + + + Autopilot supports the flight termination command. + + + Autopilot supports onboard compass calibration. + + + Autopilot supports MAVLink version 2. + + + Autopilot supports mission fence protocol. + + + Autopilot supports mission rally point protocol. + + + Autopilot supports the flight information protocol. + + + + Type of mission items being requested/sent in mission protocol. + + Items are mission commands for main mission. + + + Specifies GeoFence area(s). Items are MAV_CMD_NAV_FENCE_ GeoFence items. + + + Specifies the rally points for the vehicle. Rally points are alternative RTL points. Items are MAV_CMD_NAV_RALLY_POINT rally point items. + + + Only used in MISSION_CLEAR_ALL to clear all mission types. + + + + Enumeration of estimator types + + Unknown type of the estimator. + + + This is a naive estimator without any real covariance feedback. + + + Computer vision based estimate. Might be up to scale. + + + Visual-inertial estimate. + + + Plain GPS estimate. + + + Estimator integrating GPS and inertial sensing. + + + Estimate from external motion capturing system. + + + Estimator based on lidar sensor input. + + + Estimator on autopilot. + + + + Enumeration of battery types + + Not specified. + + + Lithium polymer battery + + + Lithium-iron-phosphate battery + + + Lithium-ION battery + + + Nickel metal hydride battery + + + + Enumeration of battery functions + + Battery function is unknown + + + Battery supports all flight systems + + + Battery for the propulsion system + + + Avionics battery + + + Payload battery + + + + Enumeration for battery charge states. + + Low battery state is not provided + + + Battery is not in low state. Normal operation. + + + Battery state is low, warn and monitor close. + + + Battery state is critical, return or abort immediately. + + + Battery state is too low for ordinary abort sequence. Perform fastest possible emergency stop to prevent damage. + + + Battery failed, damage unavoidable. + + + Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. + + + Battery is charging. + + + + Smart battery supply status/fault flags (bitmask) for health indication. + + Battery has deep discharged. + + + Voltage spikes. + + + Single cell has failed. + + + Over-current fault. + + + Over-temperature fault. + + + Under-temperature fault. + + + + Enumeration of VTOL states + + MAV is not configured as VTOL + + + VTOL is in transition from multicopter to fixed-wing + + + VTOL is in transition from fixed-wing to multicopter + + + VTOL is in multicopter state + + + VTOL is in fixed-wing state + + + + Enumeration of landed detector states + + MAV landed state is unknown + + + MAV is landed (on ground) + + + MAV is in air + + + MAV currently taking off + + + MAV currently landing + + + + Enumeration of the ADSB altimeter types + + Altitude reported from a Baro source using QNH reference + + + Altitude reported from a GNSS source + + + + ADSB classification for the type of vehicle emitting the transponder signal + + + + + + + + + + + + + + + + + + + + + + + These flags indicate status such as data validity of each data source. Set = data valid + + + + + + + + + + + + + Bitmap of options for the MAV_CMD_DO_REPOSITION + + The aircraft should immediately transition into guided. This should not be set for follow me applications + + + + + Flags in EKF_STATUS message + + True if the attitude estimate is good + + + True if the horizontal velocity estimate is good + + + True if the vertical velocity estimate is good + + + True if the horizontal position (relative) estimate is good + + + True if the horizontal position (absolute) estimate is good + + + True if the vertical position (absolute) estimate is good + + + True if the vertical position (above ground) estimate is good + + + True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) + + + True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate + + + True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate + + + True if the EKF has detected a GPS glitch + + + True if the EKF has detected bad accelerometer data + + + + + + default autopilot motor test method + + + motor numbers are specified as their index in a predefined vehicle-specific sequence + + + motor numbers are specified as the output as labeled on the board + + + + + + throttle as a percentage from 0 ~ 100 + + + throttle as an absolute PWM value (normally in range of 1000~2000) + + + throttle pass-through from pilot's transmitter + + + per-motor compass calibration test + + + + + + ignore altitude field + + + ignore hdop field + + + ignore vdop field + + + ignore horizontal velocity field (vn and ve) + + + ignore vertical velocity field (vd) + + + ignore speed accuracy field + + + ignore horizontal accuracy field + + + ignore vertical accuracy field + + + + Possible actions an aircraft can take to avoid a collision. + + Ignore any potential collisions + + + Report potential collision + + + Ascend or Descend to avoid threat + + + Move horizontally to avoid threat + + + Aircraft to move perpendicular to the collision's velocity vector + + + Aircraft to fly directly back to its launch point + + + Aircraft to stop in place + + + + Aircraft-rated danger from this threat. + + Not a threat + + + Craft is mildly concerned about this threat + + + Craft is panicking, and may take actions to avoid threat + + + + Source of information about this collision. + + ID field references ADSB_VEHICLE packets + + + ID field references MAVLink SRC ID + + + + Type of GPS fix + + No GPS connected + + + No position information, GPS is connected + + + 2D position + + + 3D position + + + DGPS/SBAS aided 3D position + + + RTK float, 3D position + + + RTK Fixed, 3D position + + + Static fixed, typically used for base stations + + + PPP, 3D position. + + + + RTK GPS baseline coordinate system, used for RTK corrections + + Earth-centered, Earth-fixed + + + RTK basestation centered, north, east, down + + + + Type of landing target + + Landing target signaled by light beacon (ex: IR-LOCK) + + + Landing target signaled by radio beacon (ex: ILS, NDB) + + + Landing target represented by a fiducial marker (ex: ARTag) + + + Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square) + + + + Direction of VTOL transition + + Respect the heading configuration of the vehicle. + + + Use the heading pointing towards the next waypoint. + + + Use the heading on takeoff (while sitting on the ground). + + + Use the specified heading in parameter 4. + + + Use the current heading when reaching takeoff altitude (potentially facing the wind when weather-vaning is active). + + + + Camera capability flags (Bitmap) + + Camera is able to record video + + + Camera is able to capture images + + + Camera has separate Video and Image/Photo modes (MAV_CMD_SET_CAMERA_MODE) + + + Camera can capture images while in video mode + + + Camera can capture videos while in Photo/Image mode + + + Camera has image survey mode (MAV_CMD_SET_CAMERA_MODE) + + + Camera has basic zoom control (MAV_CMD_SET_CAMERA_ZOOM) + + + Camera has basic focus control (MAV_CMD_SET_CAMERA_FOCUS) + + + Camera has video streaming capabilities (use MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION for video streaming info) + + + + Stream status flags (Bitmap) + + Stream is active (running) + + + Stream is thermal imaging + + + + Video stream types + + Stream is RTSP + + + Stream is RTP UDP (URI gives the port number) + + + Stream is MPEG on TCP + + + Stream is h.264 on MPEG TS (URI gives the port number) + + + + Zoom types for MAV_CMD_SET_CAMERA_ZOOM + + Zoom one step increment (-1 for wide, 1 for tele) + + + Continuous zoom up/down until stopped (-1 for wide, 1 for tele, 0 to stop zooming) + + + Zoom value as proportion of full camera range (a value between 0.0 and 100.0) + + + Zoom value/variable focal length in milimetres. Note that there is no message to get the valid zoom range of the camera, so this can type can only be used for cameras where the zoom range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera) + + + + Focus types for MAV_CMD_SET_CAMERA_FOCUS + + Focus one step increment (-1 for focusing in, 1 for focusing out towards infinity). + + + Continuous focus up/down until stopped (-1 for focusing in, 1 for focusing out towards infinity, 0 to stop focusing) + + + Focus value as proportion of full camera focus range (a value between 0.0 and 100.0) + + + Focus value in metres. Note that there is no message to get the valid focus range of the camera, so this can type can only be used for cameras where the range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera). + + + + Result from a PARAM_EXT_SET message. + + Parameter value ACCEPTED and SET + + + Parameter value UNKNOWN/UNSUPPORTED + + + Parameter failed to set + + + Parameter value received but not yet validated or set. A subsequent PARAM_EXT_ACK will follow once operation is completed with the actual result. These are for parameters that may take longer to set. Instead of waiting for an ACK and potentially timing out, you will immediately receive this response to let you know it was received. + + + + Camera Modes. + + Camera is in image/photo capture mode. + + + Camera is in video capture mode. + + + Camera is in image survey capture mode. It allows for camera controller to do specific settings for surveys. + + + + + Not a specific reason + + + Authorizer will send the error as string to GCS + + + At least one waypoint have a invalid value + + + Timeout in the authorizer process(in case it depends on network) + + + Airspace of the mission in use by another vehicle, second result parameter can have the waypoint id that caused it to be denied. + + + Weather is not good to fly + + + + RC type + + Spektrum DSM2 + + + Spektrum DSMX + + + + Bitmap to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 9 is set the floats afx afy afz should be interpreted as force instead of acceleration. + + Ignore position x + + + Ignore position y + + + Ignore position z + + + Ignore velocity x + + + Ignore velocity y + + + Ignore velocity z + + + Ignore acceleration x + + + Ignore acceleration y + + + Ignore acceleration z + + + Use force instead of acceleration + + + Ignore yaw + + + Ignore yaw rate + + + + Airborne status of UAS. + + The flight state can't be determined. + + + UAS on ground. + + + UAS airborne. + + + UAS is in an emergency flight state. + + + UAS has no active controls. + + + + Flags for the global position report. + + The field time contains valid data. + + + The field uas_id contains valid data. + + + The fields lat, lon and h_acc contain valid data. + + + The fields alt and v_acc contain valid data. + + + The field relative_alt contains valid data. + + + The fields vx and vy contain valid data. + + + The field vz contains valid data. + + + The fields next_lat, next_lon and next_alt contain valid data. + + + + Cellular network radio type + + + + + + + + These flags encode the cellular network status + + Roaming is active + + + + Precision land modes (used in MAV_CMD_NAV_LAND). + + Normal (non-precision) landing. + + + Use precision landing if beacon detected when land command accepted, otherwise land normally. + + + Use precision landing, searching for beacon if not found when land command accepted (land normally if beacon cannot be found). + + + + + + Disable parachute release. + + + Enable parachute release. + + + Release parachute. + + + + + Encoding of payload unknown. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + + + No type defined. + + + Manufacturer Serial Number (ANSI/CTA-2063 format). + + + CAA (Civil Aviation Authority) registered ID. Format: [ICAO Country Code].[CAA Assigned ID]. + + + UTM (Unmanned Traffic Management) assigned UUID (RFC4122). + + + + + No UA (Unmanned Aircraft) type defined. + + + Aeroplane/Airplane. Fixed wing. + + + Rotorcraft (including Multirotor). + + + Gyroplane. + + + VTOL (Vertical Take-Off and Landing). Fixed wing aircraft that can take off vertically. + + + Ornithopter. + + + Glider. + + + Kite. + + + Free Balloon. + + + Captive Balloon. + + + Airship. E.g. a blimp. + + + Free Fall/Parachute. + + + Rocket. + + + Tethered powered aircraft. + + + Ground Obstacle. + + + Other type of aircraft not listed earlier. + + + + + The status of the (UA) Unmanned Aircraft is undefined. + + + The UA is on the ground. + + + The UA is in the air. + + + + + The height field is relative to the take-off location. + + + The height field is relative to ground. + + + + + The horizontal accuracy is unknown. + + + The horizontal accuracy is smaller than 10 Nautical Miles. 18.52 km. + + + The horizontal accuracy is smaller than 4 Nautical Miles. 7.408 km. + + + The horizontal accuracy is smaller than 2 Nautical Miles. 3.704 km. + + + The horizontal accuracy is smaller than 1 Nautical Miles. 1.852 km. + + + The horizontal accuracy is smaller than 0.5 Nautical Miles. 926 m. + + + The horizontal accuracy is smaller than 0.3 Nautical Miles. 555.6 m. + + + The horizontal accuracy is smaller than 0.1 Nautical Miles. 185.2 m. + + + The horizontal accuracy is smaller than 0.05 Nautical Miles. 92.6 m. + + + The horizontal accuracy is smaller than 30 meter. + + + The horizontal accuracy is smaller than 10 meter. + + + The horizontal accuracy is smaller than 3 meter. + + + The horizontal accuracy is smaller than 1 meter. + + + + + The vertical accuracy is unknown. + + + The vertical accuracy is smaller than 150 meter. + + + The vertical accuracy is smaller than 45 meter. + + + The vertical accuracy is smaller than 25 meter. + + + The vertical accuracy is smaller than 10 meter. + + + The vertical accuracy is smaller than 3 meter. + + + The vertical accuracy is smaller than 1 meter. + + + + + The speed accuracy is unknown. + + + The speed accuracy is smaller than 10 meters per second. + + + The speed accuracy is smaller than 3 meters per second. + + + The speed accuracy is smaller than 1 meters per second. + + + The speed accuracy is smaller than 0.3 meters per second. + + + + + The timestamp accuracy is unknown. + + + The timestamp accuracy is smaller than 0.1 second. + + + The timestamp accuracy is smaller than 0.2 second. + + + The timestamp accuracy is smaller than 0.3 second. + + + The timestamp accuracy is smaller than 0.4 second. + + + The timestamp accuracy is smaller than 0.5 second. + + + The timestamp accuracy is smaller than 0.6 second. + + + The timestamp accuracy is smaller than 0.7 second. + + + The timestamp accuracy is smaller than 0.8 second. + + + The timestamp accuracy is smaller than 0.9 second. + + + The timestamp accuracy is smaller than 1.0 second. + + + The timestamp accuracy is smaller than 1.1 second. + + + The timestamp accuracy is smaller than 1.2 second. + + + The timestamp accuracy is smaller than 1.3 second. + + + The timestamp accuracy is smaller than 1.4 second. + + + The timestamp accuracy is smaller than 1.5 second. + + + + + No authentication type is specified. + + + Signature for the UAS (Unmanned Aircraft System) ID. + + + Signature for the Operator ID. + + + Signature for the entire message set. + + + Authentication is provided by Network Remote ID. + + + + + Free-form text description of the purpose of the flight. + + + + + The location of the operator is the same as the take-off location. + + + The location of the operator is based on live GNSS data. + + + The location of the operator is a fixed location. + + + + + CAA (Civil Aviation Authority) registered operator ID. + + + + Tune formats (used for vehicle buzzer/tone generation). + + Format is QBasic 1.1 Play: https://www.qbasic.net/en/reference/qb11/Statement/PLAY-006.htm. + + + Format is Modern Music Markup Language (MML): https://en.wikipedia.org/wiki/Music_Macro_Language#Modern_MML. + + + + Component capability flags (Bitmap) + + Component has parameters, and supports the parameter protocol (PARAM messages). + + + Component has parameters, and supports the extended parameter protocol (PARAM_EXT messages). + + + + + Type of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html + + Not available (default). + + + + + + + + + + + + + + + + + + + + + + Wing In Ground effect. + + + + + + + + + + + + + + Towing: length exceeds 200m or breadth exceeds 25m. + + + Dredging or other underwater ops. + + + + + + + + + High Speed Craft. + + + + + + + + + + + + + Search And Rescue vessel. + + + + + Anti-pollution equipment. + + + + + + + Noncombatant ship according to RR Resolution No. 18. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Navigational status of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html + + Under way using engine. + + + + + + + + + + + + + + + + Search And Rescue Transponder. + + + Not available (default). + + + + + These flags are used in the AIS_VESSEL.fields bitmask to indicate validity of data in the other message fields. When set, the data is valid. + + 1 = Position accuracy less than 10m, 0 = position accuracy greater than 10m. + + + + + 1 = Velocity over 52.5765m/s (102.2 knots) + + + + Only the sign of the returned turn rate value is valid, either greater than 5deg/30s or less than -5deg/30s + + + + Distance to bow is larger than 511m + + + Distance to stern is larger than 511m + + + Distance to port side is larger than 63m + + + Distance to starboard side is larger than 63m + + + + + + + + The heartbeat message shows that a system or component is present and responding. The type and autopilot fields (along with the message component id), allow the receiving system to treat further messages from this system appropriately (e.g. by laying out the user interface based on the autopilot). This microservice is documented at https://mavlink.io/en/services/heartbeat.html + Vehicle or component type. For a flight controller component the vehicle type (quadrotor, helicopter, etc.). For other components the component type (e.g. camera, gimbal, etc.). This should be used in preference to component id for identifying the component type. + Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers. + System mode bitmap. + A bitfield for use for autopilot-specific flags + System status flag. + MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version + + + The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows whether the system is currently active or not and if an emergency occurred. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occurred it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. + Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. + Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. + Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy. + Maximum usage in percent of the mainloop time. Values: [0-1000] - should always be below 1000 + Battery voltage, UINT16_MAX: Voltage not sent by autopilot + Battery current, -1: Current not sent by autopilot + Battery energy remaining, -1: Battery remaining energy not sent by autopilot + Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + Autopilot-specific errors + Autopilot-specific errors + Autopilot-specific errors + Autopilot-specific errors + + + The system time is the time of the master clock, typically the computer clock of the main onboard computer. + Timestamp (UNIX epoch time). + Timestamp (time since system boot). + + + + A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. The ping microservice is documented at https://mavlink.io/en/services/ping.html + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + PING sequence + 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system + 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component. + + + Request to control this MAV + System the GCS requests control for + 0: request control of this MAV, 1: Release control of this MAV + 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + + + Accept / deny control of this MAV + ID of the GCS this message + 0: request control of this MAV, 1: Release control of this MAV + 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + + Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. + key + + + + + Status generated in each node in the communication chain and injected into MAVLink stream. + Timestamp (time since system boot). + Remaining free transmit buffer space + Remaining free receive buffer space + Transmit rate + Receive rate + Number of bytes that could not be parsed correctly. + Transmit buffer overflows. This number wraps around as it reaches UINT16_MAX + Receive buffer overflows. This number wraps around as it reaches UINT16_MAX + Messages sent + Messages received (estimated from counting seq) + Messages lost (estimated from counting seq) + + + Use COMMAND_LONG with MAV_CMD_DO_SET_MODE instead + Set the system mode, as defined by enum MAV_MODE. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. + The system setting the mode + The new base mode. + The new autopilot-specific mode. This field can be ignored by an autopilot. + + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also https://mavlink.io/en/services/parameter.html for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + + + Request all parameters of this component. After this request, all parameters are emitted. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Onboard parameter value + Onboard parameter type. + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value (write new value to permanent storage). IMPORTANT: The receiving component should acknowledge the new parameter value by sending a PARAM_VALUE message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Onboard parameter value + Onboard parameter type. + + + The global position, as returned by the Global Positioning System (GPS). This is + NOT the global position estimate of the system, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + GPS fix type. + Latitude (WGS84, EGM96 ellipsoid) + Longitude (WGS84, EGM96 ellipsoid) + Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. + GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS ground speed. If unknown, set to: UINT16_MAX + Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + Number of satellites visible. If unknown, set to 255 + + Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + Position uncertainty. Positive for up. + Altitude uncertainty. Positive for up. + Speed uncertainty. Positive for up. + Heading / track uncertainty + + + The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. + Number of satellites visible + Global satellite ID + 0: Satellite not used, 1: used for localization + Elevation (0: right on top of receiver, 90: on the horizon) of satellite + Direction of satellite, 0: 0 deg, 255: 360 deg. + Signal to noise ratio of satellite + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (time since system boot). + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + X Magnetic field + Y Magnetic field + Z Magnetic field + + Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + + + The RAW IMU readings for a 9DOF sensor, which is identified by the id (default IMU1). This message should always contain the true raw values without any scaling to allow data capture and system debugging. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + X acceleration (raw) + Y acceleration (raw) + Z acceleration (raw) + Angular speed around X axis (raw) + Angular speed around Y axis (raw) + Angular speed around Z axis (raw) + X Magnetic field (raw) + Y Magnetic field (raw) + Z Magnetic field (raw) + + Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + + + The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Absolute pressure (raw) + Differential pressure 1 (raw, 0 if nonexistent) + Differential pressure 2 (raw, 0 if nonexistent) + Raw Temperature measurement (raw) + + + The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. + Timestamp (time since system boot). + Absolute pressure + Differential pressure 1 + Temperature + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). + Timestamp (time since system boot). + Roll angle (-pi..+pi) + Pitch angle (-pi..+pi) + Yaw angle (-pi..+pi) + Roll angular speed + Pitch angular speed + Yaw angular speed + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right), expressed as quaternion. Quaternion order is w, x, y, z and a zero rotation would be expressed as (1 0 0 0). + Timestamp (time since system boot). + Quaternion component 1, w (1 in null-rotation) + Quaternion component 2, x (0 in null-rotation) + Quaternion component 3, y (0 in null-rotation) + Quaternion component 4, z (0 in null-rotation) + Roll angular speed + Pitch angular speed + Yaw angular speed + + Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame, NED / north-east-down convention) + Timestamp (time since system boot). + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up). It + is designed as scaled integer message since the resolution of float is not sufficient. + Timestamp (time since system boot). + Latitude, expressed + Longitude, expressed + Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. + Altitude above ground + Ground X Speed (Latitude, positive north) + Ground Y Speed (Longitude, positive east) + Ground Z Speed (Altitude, positive down) + Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + + + The scaled values of the RC channels received: (-100%) -10000, (0%) 0, (100%) 10000. Channels that are inactive should be set to UINT16_MAX. + Timestamp (time since system boot). + Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + RC channel 1 value scaled. + RC channel 2 value scaled. + RC channel 3 value scaled. + RC channel 4 value scaled. + RC channel 5 value scaled. + RC channel 6 value scaled. + RC channel 7 value scaled. + RC channel 8 value scaled. + Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + + + The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. A value of UINT16_MAX implies the channel is unused. Individual receivers/transmitters might violate this specification. + Timestamp (time since system boot). + Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + RC channel 1 value. + RC channel 2 value. + RC channel 3 value. + RC channel 4 value. + RC channel 5 value. + RC channel 6 value. + RC channel 7 value. + RC channel 8 value. + Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + + + Superseded by ACTUATOR_OUTPUT_STATUS. The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + Servo output 1 value + Servo output 2 value + Servo output 3 value + Servo output 4 value + Servo output 5 value + Servo output 6 value + Servo output 7 value + Servo output 8 value + + Servo output 9 value + Servo output 10 value + Servo output 11 value + Servo output 12 value + Servo output 13 value + Servo output 14 value + Servo output 15 value + Servo output 16 value + + + Request a partial list of mission items from the system/component. https://mavlink.io/en/services/mission.html. If start and end index are the same, just send one waypoint. + System ID + Component ID + Start index + End index, -1 by default (-1: send list to end). Else a valid index of the list + + Mission type. + + + This message is sent to the MAV to write a partial list. If start index == end index, only one item will be transmitted / updated. If the start index is NOT 0 and above the current list size, this request should be REJECTED! + System ID + Component ID + Start index. Must be smaller / equal to the largest index of the current onboard list. + End index, equal or greater than start index. + + Mission type. + + + Message encoding a mission item. This message is emitted to announce + the presence of a mission item and to set a mission item on the system. The mission item can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). See also https://mavlink.io/en/services/mission.html. + System ID + Component ID + Sequence + The coordinate system of the waypoint. + The scheduled action for the waypoint. + false:0, true:1 + Autocontinue to next waypoint + PARAM1, see MAV_CMD enum + PARAM2, see MAV_CMD enum + PARAM3, see MAV_CMD enum + PARAM4, see MAV_CMD enum + PARAM5 / local: X coordinate, global: latitude + PARAM6 / local: Y coordinate, global: longitude + PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + + Mission type. + + + Request the information of the mission item with the sequence number seq. The response of the system to this message should be a MISSION_ITEM message. https://mavlink.io/en/services/mission.html + System ID + Component ID + Sequence + + Mission type. + + + Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). + System ID + Component ID + Sequence + + + Message that announces the sequence number of the current active mission item. The MAV will fly towards this mission item. + Sequence + + + Request the overall list of mission items from the system/component. + System ID + Component ID + + Mission type. + + + This message is emitted as response to MISSION_REQUEST_LIST by the MAV and to initiate a write transaction. The GCS can then request the individual mission item based on the knowledge of the total number of waypoints. + System ID + Component ID + Number of mission items in the sequence + + Mission type. + + + Delete all mission items at once. + System ID + Component ID + + Mission type. + + + A certain mission item has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. + Sequence + + + Acknowledgment message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). + System ID + Component ID + Mission result. + + Mission type. + + + Sets the GPS co-ordinates of the vehicle local origin (0,0,0) position. Vehicle should emit GPS_GLOBAL_ORIGIN irrespective of whether the origin is changed. This enables transform between the local coordinate frame and the global (GPS) coordinate frame, which may be necessary when (for example) indoor and outdoor settings are connected and the MAV should move from in- to outdoor. + System ID + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + + + Publishes the GPS co-ordinates of the vehicle local origin (0,0,0) position. Emitted whenever a new GPS-Local position mapping is requested or set - e.g. following SET_GPS_GLOBAL_ORIGIN message. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + + + Bind a RC channel to a parameter. The parameter should change according to the RC channel value. + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + Index of parameter RC channel. Not equal to the RC channel id. Typically corresponds to a potentiometer-knob on the RC. + Initial parameter value + Scale, maps the RC range [-1, 1] to a parameter value + Minimum param value. The protocol does not define if this overwrites an onboard minimum value. (Depends on implementation) + Maximum param value. The protocol does not define if this overwrites an onboard maximum value. (Depends on implementation) + + + Request the information of the mission item with the sequence number seq. The response of the system to this message should be a MISSION_ITEM_INT message. https://mavlink.io/en/services/mission.html + System ID + Component ID + Sequence + + Mission type. + + + + + A broadcast message to notify any ground station or SDK if a mission, geofence or safe points have changed on the vehicle. + Start index for partial mission change (-1 for all items). + End index of a partial mission change. -1 is a synonym for the last mission item (i.e. selects all items from start_index). Ignore field if start_index=-1. + System ID of the author of the new mission. + Compnent ID of the author of the new mission. + Mission type. + + + Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. + System ID + Component ID + Coordinate frame. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Read out the safety zone the MAV currently assumes. + Coordinate frame. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right), expressed as quaternion. Quaternion order is w, x, y, z and a zero rotation would be expressed as (1 0 0 0). + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + Roll angular speed + Pitch angular speed + Yaw angular speed + Row-major representation of a 3x3 attitude covariance matrix (states: roll, pitch, yaw; first three entries are the first ROW, next three entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + + + The state of the fixed wing navigation and position controller. + Current desired roll + Current desired pitch + Current desired heading + Bearing to current waypoint/target + Distance to active waypoint + Current altitude error + Current airspeed error + Current crosstrack error on x-y plane + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up). It is designed as scaled integer message since the resolution of float is not sufficient. NOTE: This message is intended for onboard networks / companion computers and higher-bandwidth links and optimized for accuracy and completeness. Please use the GLOBAL_POSITION_INT message for a minimal subset. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Class id of the estimator this estimate originated from. + Latitude + Longitude + Altitude in meters above MSL + Altitude above ground + Ground X Speed (Latitude) + Ground Y Speed (Longitude) + Ground Z Speed (Altitude) + Row-major representation of a 6x6 position and velocity 6x6 cross-covariance matrix (states: lat, lon, alt, vx, vy, vz; first six entries are the first ROW, next six entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame, NED / north-east-down convention) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Class id of the estimator this estimate originated from. + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + X Acceleration + Y Acceleration + Z Acceleration + Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + + + The PPM values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. A value of UINT16_MAX implies the channel is unused. Individual receivers/transmitters might violate this specification. + Timestamp (time since system boot). + Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + RC channel 1 value. + RC channel 2 value. + RC channel 3 value. + RC channel 4 value. + RC channel 5 value. + RC channel 6 value. + RC channel 7 value. + RC channel 8 value. + RC channel 9 value. + RC channel 10 value. + RC channel 11 value. + RC channel 12 value. + RC channel 13 value. + RC channel 14 value. + RC channel 15 value. + RC channel 16 value. + RC channel 17 value. + RC channel 18 value. + Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + + + + Request a data stream. + The target requested to send the message stream. + The target requested to send the message stream. + The ID of the requested data stream + The requested message rate + 1 to start sending, 0 to stop sending. + + + + Data stream status information. + The ID of the requested data stream + The message rate + 1 stream is enabled, 0 stream is stopped. + + + This message provides an API for manually controlling the vehicle using standard joystick axes nomenclature, along with a joystick-like input device. Unused axes can be disabled an buttons are also transmit as boolean values of their + The system to be controlled. + X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. + Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. + Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. Positive values are positive thrust, negative values are negative thrust. + R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. + A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + + + The RAW values of the RC channels sent to the MAV to override info received from the RC radio. A value of UINT16_MAX means no change to that channel. A value of 0 means control of that channel should be released back to the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + System ID + Component ID + RC channel 1 value. A value of UINT16_MAX means to ignore this field. + RC channel 2 value. A value of UINT16_MAX means to ignore this field. + RC channel 3 value. A value of UINT16_MAX means to ignore this field. + RC channel 4 value. A value of UINT16_MAX means to ignore this field. + RC channel 5 value. A value of UINT16_MAX means to ignore this field. + RC channel 6 value. A value of UINT16_MAX means to ignore this field. + RC channel 7 value. A value of UINT16_MAX means to ignore this field. + RC channel 8 value. A value of UINT16_MAX means to ignore this field. + + RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. + RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. + + + Message encoding a mission item. This message is emitted to announce + the presence of a mission item and to set a mission item on the system. The mission item can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). See also https://mavlink.io/en/services/mission.html. + System ID + Component ID + Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + The coordinate system of the waypoint. + The scheduled action for the waypoint. + false:0, true:1 + Autocontinue to next waypoint + PARAM1, see MAV_CMD enum + PARAM2, see MAV_CMD enum + PARAM3, see MAV_CMD enum + PARAM4, see MAV_CMD enum + PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + + Mission type. + + + Metrics typically displayed on a HUD for fixed wing aircraft. + Current indicated airspeed (IAS). + Current ground speed. + Current heading in compass units (0-360, 0=north). + Current throttle setting (0 to 100). + Current altitude (MSL). + Current climb rate. + + + Message encoding a command with parameters as scaled integers. Scaling depends on the actual command value. The command microservice is documented at https://mavlink.io/en/services/command.html + System ID + Component ID + The coordinate system of the COMMAND. + The scheduled action for the mission item. + false:0, true:1 + autocontinue to next wp + PARAM1, see MAV_CMD enum + PARAM2, see MAV_CMD enum + PARAM3, see MAV_CMD enum + PARAM4, see MAV_CMD enum + PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame). + + + Send a command with up to seven parameters to the MAV. The command microservice is documented at https://mavlink.io/en/services/command.html + System which should execute the command + Component which should execute the command, 0 for all components + Command ID (of command to send). + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1 (for the specific command). + Parameter 2 (for the specific command). + Parameter 3 (for the specific command). + Parameter 4 (for the specific command). + Parameter 5 (for the specific command). + Parameter 6 (for the specific command). + Parameter 7 (for the specific command). + + + Report status of a command. Includes feedback whether the command was executed. The command microservice is documented at https://mavlink.io/en/services/command.html + Command ID (of acknowledged command). + Result of command. + + WIP: Also used as result_param1, it can be set with a enum containing the errors reasons of why the command was denied or the progress percentage or 255 if unknown the progress when result is MAV_RESULT_IN_PROGRESS. + WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + WIP: System which requested the command to be executed + WIP: Component which requested the command to be executed + + + Setpoint in roll, pitch, yaw and thrust from the operator + Timestamp (time since system boot). + Desired roll rate + Desired pitch rate + Desired yaw rate + Collective thrust, normalized to 0 .. 1 + Flight mode switch position, 0.. 255 + Override mode switch position, 0.. 255 + + + Sets a desired vehicle attitude. Used by an external controller to command the vehicle (manual controller or other system). + Timestamp (time since system boot). + System ID + Component ID + Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + Body roll rate + Body pitch rate + Body yaw rate + Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + + + Reports the current commanded attitude of the vehicle as specified by the autopilot. This should match the commands sent in a SET_ATTITUDE_TARGET message if the vehicle is being controlled this way. + Timestamp (time since system boot). + Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + Body roll rate + Body pitch rate + Body yaw rate + Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + + + Sets a desired vehicle position in a local north-east-down coordinate frame. Used by an external controller to command the vehicle (manual controller or other system). + Timestamp (time since system boot). + System ID + Component ID + Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + Bitmap to indicate which dimensions should be ignored by the vehicle. + X Position in NED frame + Y Position in NED frame + Z Position in NED frame (note, altitude is negative in NED) + X velocity in NED frame + Y velocity in NED frame + Z velocity in NED frame + X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + yaw setpoint + yaw rate setpoint + + + Reports the current commanded vehicle position, velocity, and acceleration as specified by the autopilot. This should match the commands sent in SET_POSITION_TARGET_LOCAL_NED if the vehicle is being controlled this way. + Timestamp (time since system boot). + Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + Bitmap to indicate which dimensions should be ignored by the vehicle. + X Position in NED frame + Y Position in NED frame + Z Position in NED frame (note, altitude is negative in NED) + X velocity in NED frame + Y velocity in NED frame + Z velocity in NED frame + X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + yaw setpoint + yaw rate setpoint + + + Sets a desired vehicle position, velocity, and/or acceleration in a global coordinate system (WGS84). Used by an external controller to command the vehicle (manual controller or other system). + Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + System ID + Component ID + Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + Bitmap to indicate which dimensions should be ignored by the vehicle. + X Position in WGS84 frame + Y Position in WGS84 frame + Altitude (MSL, Relative to home, or AGL - depending on frame) + X velocity in NED frame + Y velocity in NED frame + Z velocity in NED frame + X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + yaw setpoint + yaw rate setpoint + + + Reports the current commanded vehicle position, velocity, and acceleration as specified by the autopilot. This should match the commands sent in SET_POSITION_TARGET_GLOBAL_INT if the vehicle is being controlled this way. + Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + Bitmap to indicate which dimensions should be ignored by the vehicle. + X Position in WGS84 frame + Y Position in WGS84 frame + Altitude (MSL, AGL or relative to home altitude, depending on frame) + X velocity in NED frame + Y velocity in NED frame + Z velocity in NED frame + X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + yaw setpoint + yaw rate setpoint + + + The offset in X, Y, Z and yaw between the LOCAL_POSITION_NED messages of MAV X and the global coordinate frame in NED coordinates. Coordinate frame is right-handed, Z-axis down (aeronautical frame, NED / north-east-down convention) + Timestamp (time since system boot). + X Position + Y Position + Z Position + Roll + Pitch + Yaw + + + Suffers from missing airspeed fields and singularities due to Euler angles + Sent from simulation to autopilot. This packet is useful for high throughput applications such as hardware in the loop simulations. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Roll angle + Pitch angle + Yaw angle + Body frame roll / phi angular speed + Body frame pitch / theta angular speed + Body frame yaw / psi angular speed + Latitude + Longitude + Altitude + Ground X Speed (Latitude) + Ground Y Speed (Longitude) + Ground Z Speed (Altitude) + X acceleration + Y acceleration + Z acceleration + + + Sent from autopilot to simulation. Hardware in the loop control outputs + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Control output -1 .. 1 + Control output -1 .. 1 + Control output -1 .. 1 + Throttle 0 .. 1 + Aux 1, -1 .. 1 + Aux 2, -1 .. 1 + Aux 3, -1 .. 1 + Aux 4, -1 .. 1 + System mode. + Navigation mode (MAV_NAV_MODE) + + + Sent from simulation to autopilot. The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + RC channel 1 value + RC channel 2 value + RC channel 3 value + RC channel 4 value + RC channel 5 value + RC channel 6 value + RC channel 7 value + RC channel 8 value + RC channel 9 value + RC channel 10 value + RC channel 11 value + RC channel 12 value + Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + + + Sent from autopilot to simulation. Hardware in the loop control outputs (replacement for HIL_CONTROLS) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Control outputs -1 .. 1. Channel assignment depends on the simulated hardware. + System mode. Includes arming state. + Flags as bitfield, reserved for future use. + + + Optical flow from a flow sensor (e.g. optical mouse sensor) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Sensor ID + Flow in x-sensor direction + Flow in y-sensor direction + Flow in x-sensor direction, angular-speed compensated + Flow in y-sensor direction, angular-speed compensated + Optical flow quality / confidence. 0: bad, 255: maximum quality + Ground distance. Positive value: distance known. Negative value: Unknown distance + + Flow rate about X axis + Flow rate about Y axis + + + Global position/attitude estimate from a vision source. + Timestamp (UNIX time or since system boot) + Global X position + Global Y position + Global Z position + Roll angle + Pitch angle + Yaw angle + + Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + + + Local position/attitude estimate from a vision source. + Timestamp (UNIX time or time since system boot) + Local X position + Local Y position + Local Z position + Roll angle + Pitch angle + Yaw angle + + Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + + + Speed estimate from a vision source. + Timestamp (UNIX time or time since system boot) + Global X speed + Global Y speed + Global Z speed + + Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + + + Global position estimate from a Vicon motion system source. + Timestamp (UNIX time or time since system boot) + Global X position + Global Y position + Global Z position + Roll angle + Pitch angle + Yaw angle + + Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + + + The IMU readings in SI units in NED body frame + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + X Magnetic field + Y Magnetic field + Z Magnetic field + Absolute pressure + Differential pressure + Altitude calculated from pressure + Temperature + Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + + Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + + + Optical flow from an angular rate flow sensor (e.g. PX4FLOW or mouse sensor) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Sensor ID + Integration time. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + Flow around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + Flow around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + RH rotation around X axis + RH rotation around Y axis + RH rotation around Z axis + Temperature + Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + Time since the distance was sampled. + Distance to the center of the flow field. Positive value (including zero): distance known. Negative value: Unknown distance. + + + The IMU readings in SI units in NED body frame + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis in body frame + Angular speed around Y axis in body frame + Angular speed around Z axis in body frame + X Magnetic field + Y Magnetic field + Z Magnetic field + Absolute pressure + Differential pressure (airspeed) + Altitude calculated from pressure + Temperature + Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + + + Status of simulation environment, if used + True attitude quaternion component 1, w (1 in null-rotation) + True attitude quaternion component 2, x (0 in null-rotation) + True attitude quaternion component 3, y (0 in null-rotation) + True attitude quaternion component 4, z (0 in null-rotation) + Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + Latitude + Longitude + Altitude + Horizontal position standard deviation + Vertical position standard deviation + True velocity in north direction in earth-fixed NED frame + True velocity in east direction in earth-fixed NED frame + True velocity in down direction in earth-fixed NED frame + + + Status generated by radio and injected into MAVLink stream. + Local (message sender) recieved signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + Remaining free transmitter buffer space. + Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + Count of radio packet receive errors (since boot). + Count of error corrected radio packets (since boot). + + + File transfer message + Network ID (0 for broadcast) + System ID (0 for broadcast) + Component ID (0 for broadcast) + Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + + + Time synchronization message. + Time sync timestamp 1 + Time sync timestamp 2 + + + Camera-IMU triggering and synchronisation message. + Timestamp for image frame (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Image frame sequence + + + The global position, as returned by the Global Positioning System (GPS). This is + NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + GPS HDOP horizontal dilution of position. If unknown, set to: 65535 + GPS VDOP vertical dilution of position. If unknown, set to: 65535 + GPS ground speed. If unknown, set to: 65535 + GPS velocity in north direction in earth-fixed NED frame + GPS velocity in east direction in earth-fixed NED frame + GPS velocity in down direction in earth-fixed NED frame + Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 + Number of satellites visible. If unknown, set to 255 + + + Simulated optical flow from a flow sensor (e.g. PX4FLOW or optical mouse sensor) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Sensor ID + Integration time. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + RH rotation around X axis + RH rotation around Y axis + RH rotation around Z axis + Temperature + Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + Time since the distance was sampled. + Distance to the center of the flow field. Positive value (including zero): distance known. Negative value: Unknown distance. + + + Sent from simulation to autopilot, avoids in contrast to HIL_STATE singularities. This packet is useful for high throughput applications such as hardware in the loop simulations. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) + Body frame roll / phi angular speed + Body frame pitch / theta angular speed + Body frame yaw / psi angular speed + Latitude + Longitude + Altitude + Ground X Speed (Latitude) + Ground Y Speed (Longitude) + Ground Z Speed (Altitude) + Indicated airspeed + True airspeed + X acceleration + Y acceleration + Z acceleration + + + The RAW IMU readings for secondary 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (time since system boot). + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + X Magnetic field + Y Magnetic field + Z Magnetic field + + Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + + + Request a list of available logs. On some systems calling this may stop on-board logging until LOG_REQUEST_END is called. + System ID + Component ID + First log id (0 for first available) + Last log id (0xffff for last available) + + + Reply to LOG_REQUEST_LIST + Log id + Total number of logs + High log number + UTC timestamp of log since 1970, or 0 if not available + Size of the log (may be approximate) + + + Request a chunk of a log + System ID + Component ID + Log id (from LOG_ENTRY reply) + Offset into the log + Number of bytes + + + Reply to LOG_REQUEST_DATA + Log id (from LOG_ENTRY reply) + Offset into the log + Number of bytes (zero for end of log) + log data + + + Erase all logs + System ID + Component ID + + + Stop log transfer and resume normal logging + System ID + Component ID + + + Data for injecting into the onboard GPS (used for DGPS) + System ID + Component ID + Data length + Raw data (110 is enough for 12 satellites of RTCMv2) + + + Second GPS data. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + GPS fix type. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX + GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + GPS ground speed. If unknown, set to: UINT16_MAX + Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + Number of satellites visible. If unknown, set to 255 + Number of DGPS satellites + Age of DGPS info + + + Power supply status + 5V rail voltage. + Servo rail voltage. + Bitmap of power supply status flags. + + + Control a serial port. This can be used for raw access to an onboard serial peripheral such as a GPS or telemetry radio. It is designed to make it possible to update the devices firmware via MAVLink messages or change the devices settings. A message with zero bytes can be used to change just the baudrate. + Serial control device type. + Bitmap of serial control flags. + Timeout for reply data + Baudrate of transfer. Zero means no change. + how many bytes in this transfer + serial data + + + RTK GPS data. Gives information on the relative baseline calculation the GPS is reporting + Time since boot of last baseline message received. + Identification of connected RTK receiver. + GPS Week Number of last baseline + GPS Time of Week of last baseline + GPS-specific health report for RTK data. + Rate of baseline messages being received by GPS + Current number of sats used for RTK calculation. + Coordinate system of baseline + Current baseline in ECEF x or NED north component. + Current baseline in ECEF y or NED east component. + Current baseline in ECEF z or NED down component. + Current estimate of baseline accuracy. + Current number of integer ambiguity hypotheses. + + + RTK GPS data. Gives information on the relative baseline calculation the GPS is reporting + Time since boot of last baseline message received. + Identification of connected RTK receiver. + GPS Week Number of last baseline + GPS Time of Week of last baseline + GPS-specific health report for RTK data. + Rate of baseline messages being received by GPS + Current number of sats used for RTK calculation. + Coordinate system of baseline + Current baseline in ECEF x or NED north component. + Current baseline in ECEF y or NED east component. + Current baseline in ECEF z or NED down component. + Current estimate of baseline accuracy. + Current number of integer ambiguity hypotheses. + + + The RAW IMU readings for 3rd 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (time since system boot). + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + X Magnetic field + Y Magnetic field + Z Magnetic field + + Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + + + Handshake message to initiate, control and stop image streaming when using the Image Transmission Protocol: https://mavlink.io/en/services/image_transmission.html. + Type of requested/acknowledged data. + total data size (set on ACK only). + Width of a matrix or image. + Height of a matrix or image. + Number of packets being sent (set on ACK only). + Payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only). + JPEG quality. Values: [1-100]. + + + Data packet for images sent using the Image Transmission Protocol: https://mavlink.io/en/services/image_transmission.html. + sequence number (starting with 0 on every transmission) + image data bytes + + + Distance sensor information for an onboard rangefinder. + Timestamp (time since system boot). + Minimum distance the sensor can measure + Maximum distance the sensor can measure + Current distance reading + Type of distance sensor. + Onboard ID of the sensor + Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 + Measurement variance. Max standard deviation is 6cm. 255 if unknown. + + Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + + + Request for terrain data and terrain status + Latitude of SW corner of first grid + Longitude of SW corner of first grid + Grid spacing + Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + + + Terrain data sent from GCS. The lat/lon and grid_spacing must be the same as a lat/lon from a TERRAIN_REQUEST + Latitude of SW corner of first grid + Longitude of SW corner of first grid + Grid spacing + bit within the terrain request mask + Terrain data MSL + + + Request that the vehicle report terrain height at the given location. Used by GCS to check if vehicle has all terrain data needed for a mission. + Latitude + Longitude + + + Response from a TERRAIN_CHECK request + Latitude + Longitude + grid spacing (zero if terrain at this location unavailable) + Terrain height MSL + Current vehicle height above lat/lon terrain height + Number of 4x4 terrain blocks waiting to be received or read from disk + Number of 4x4 terrain blocks in memory + + + Barometer readings for 2nd barometer + Timestamp (time since system boot). + Absolute pressure + Differential pressure + Temperature measurement + + + Motion capture attitude and position + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + X position (NED) + Y position (NED) + Z position (NED) + + Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + + + Set the vehicle attitude and body angular rates. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + System ID + Component ID + Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + + + Set the vehicle attitude and body angular rates. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + + + The current system altitude. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + This altitude measure is initialized on system boot and monotonic (it is never reset, but represents the local altitude change). The only guarantee on this field is that it will never be reset and is consistent within a flight. The recommended value for this field is the uncorrected barometric altitude at boot time. This altitude will also drift and vary between flights. + This altitude measure is strictly above mean sea level and might be non-monotonic (it might reset on events like GPS lock or when a new QNH value is set). It should be the altitude to which global altitude waypoints are compared to. Note that it is *not* the GPS altitude, however, most GPS modules already output MSL by default and not the WGS84 altitude. + This is the local altitude in the local coordinate frame. It is not the altitude above home, but in reference to the coordinate origin (0, 0, 0). It is up-positive. + This is the altitude above the home position. It resets on each change of the current home position. + This is the altitude above terrain. It might be fed by a terrain database or an altimeter. Values smaller than -1000 should be interpreted as unknown. + This is not the altitude, but the clear space below the system according to the fused clearance estimate. It generally should max out at the maximum range of e.g. the laser altimeter. It is generally a moving target. A negative value indicates no measurement available. + + + The autopilot is requesting a resource (file, binary, other type of data) + Request ID. This ID should be re-used when sending back URI contents + The type of requested URI. 0 = a file via URL. 1 = a UAVCAN binary + The requested unique resource identifier (URI). It is not necessarily a straight domain name (depends on the URI type enum) + The way the autopilot wants to receive the URI. 0 = MAVLink FTP. 1 = binary stream. + The storage path the autopilot wants the URI to be stored in. Will only be valid if the transfer_type has a storage associated (e.g. MAVLink FTP). + + + Barometer readings for 3rd barometer + Timestamp (time since system boot). + Absolute pressure + Differential pressure + Temperature measurement + + + Current motion information from a designated system + Timestamp (time since system boot). + bit positions for tracker reporting capabilities (POS = 0, VEL = 1, ACCEL = 2, ATT + RATES = 3) + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL) + target velocity (0,0,0) for unknown + linear target acceleration (0,0,0) for unknown + (1 0 0 0 for unknown) + (0 0 0 for unknown) + eph epv + button states or switches of a tracker device + + + The smoothed, monotonic system state used to feed the control loops of the system. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + X acceleration in body frame + Y acceleration in body frame + Z acceleration in body frame + X velocity in body frame + Y velocity in body frame + Z velocity in body frame + X position in local frame + Y position in local frame + Z position in local frame + Airspeed, set to -1 if unknown + Variance of body velocity estimate + Variance in local position + The attitude, represented as Quaternion + Angular rate in roll axis + Angular rate in pitch axis + Angular rate in yaw axis + + + Battery information. Updates GCS with flight controller battery status. Use SMART_BATTERY_* messages instead for smart batteries. + Battery ID + Function of the battery + Type (chemistry) of the battery + Temperature of the battery. INT16_MAX for unknown temperature. + Battery voltage of cells. Cells above the valid cell count for this battery should have the UINT16_MAX value. + Battery current, -1: autopilot does not measure the current + Consumed charge, -1: autopilot does not provide consumption estimate + Consumed energy, -1: autopilot does not provide energy consumption estimate + Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + + Remaining battery time, 0: autopilot does not provide remaining battery time estimate + State for extent of discharge, provided by autopilot for warning or external reactions + + + Version and capability of autopilot software. This should be emitted in response to a MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES command. + Bitmap of capabilities + Firmware version number + Middleware version number + Operating system version number + HW / board version (last 8 bytes should be silicon ID, if any) + Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + ID of the board vendor + ID of the product + UID if provided by hardware (see uid2) + + UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) + + + The location of a landing target. See: https://mavlink.io/en/services/landing_target.html + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + The ID of the target if multiple targets are present + Coordinate frame used for following fields. + X-axis angular offset of the target from the center of the image + Y-axis angular offset of the target from the center of the image + Distance to the target from the vehicle + Size of target along x-axis + Size of target along y-axis + + X Position of the landing target in MAV_FRAME + Y Position of the landing target in MAV_FRAME + Z Position of the landing target in MAV_FRAME + Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + Type of landing target + Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). + + + + Status of geo-fencing. Sent in extended status stream when fencing enabled. + Breach status (0 if currently inside fence, 1 if outside). + Number of fence breaches. + Last breach type. + Time (since boot) of last breach. + + Active action to prevent fence breach + + + + Estimator status message including flags, innovation test ratios and estimated accuracies. The flags message is an integer bitmask containing information on which EKF outputs are valid. See the ESTIMATOR_STATUS_FLAGS enum definition for further information. The innovation test ratios show the magnitude of the sensor innovation divided by the innovation check threshold. Under normal operation the innovation test ratios should be below 0.5 with occasional values up to 1.0. Values greater than 1.0 should be rare under normal operation and indicate that a measurement has been rejected by the filter. The user should be notified if an innovation test ratio greater than 1.0 is recorded. Notifications for values in the range between 0.5 and 1.0 should be optional and controllable by the user. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Bitmap indicating which EKF outputs are valid. + Velocity innovation test ratio + Horizontal position innovation test ratio + Vertical position innovation test ratio + Magnetometer innovation test ratio + Height above terrain innovation test ratio + True airspeed innovation test ratio + Horizontal position 1-STD accuracy relative to the EKF local origin + Vertical position 1-STD accuracy relative to the EKF local origin + + + Wind covariance estimate from vehicle. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Wind in X (NED) direction + Wind in Y (NED) direction + Wind in Z (NED) direction + Variability of the wind in XY. RMS of a 1 Hz lowpassed wind estimate. + Variability of the wind in Z. RMS of a 1 Hz lowpassed wind estimate. + Altitude (MSL) that this measurement was taken at + Horizontal speed 1-STD accuracy + Vertical speed 1-STD accuracy + + + GPS sensor input message. This is a raw sensor value sent by the GPS. This is NOT the global position estimate of the system. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + ID of the GPS for multiple GPS inputs + Bitmap indicating which GPS input flags fields to ignore. All other fields must be provided. + GPS time (from start of GPS week) + GPS week number + 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + GPS HDOP horizontal dilution of position + GPS VDOP vertical dilution of position + GPS velocity in north direction in earth-fixed NED frame + GPS velocity in east direction in earth-fixed NED frame + GPS velocity in down direction in earth-fixed NED frame + GPS speed accuracy + GPS horizontal accuracy + GPS vertical accuracy + Number of satellites visible. + + Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + + + RTCM message for injecting into the onboard GPS (used for DGPS) + LSB: 1 means message is fragmented, next 2 bits are the fragment ID, the remaining 5 bits are used for the sequence ID. Messages are only to be flushed to the GPS when the entire message has been reconstructed on the autopilot. The fragment ID specifies which order the fragments should be assembled into a buffer, while the sequence ID is used to detect a mismatch between different buffers. The buffer is considered fully reconstructed when either all 4 fragments are present, or all the fragments before the first fragment with a non full payload is received. This management is used to ensure that normal GPS operation doesn't corrupt RTCM data, and to recover from a unreliable transport delivery order. + data length + RTCM message (may be fragmented) + + + Message appropriate for high latency connections like Iridium + Bitmap of enabled system modes. + A bitfield for use for autopilot-specific flags. + The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + roll + pitch + heading + throttle (percentage) + heading setpoint + Latitude + Longitude + Altitude above mean sea level + Altitude setpoint relative to the home position + airspeed + airspeed setpoint + groundspeed + climb rate + Number of satellites visible. If unknown, set to 255 + GPS Fix type. + Remaining battery (percentage) + Autopilot temperature (degrees C) + Air temperature (degrees C) from airspeed sensor + failsafe (each bit represents a failsafe where 0=ok, 1=failsafe active (bit0:RC, bit1:batt, bit2:GPS, bit3:GCS, bit4:fence) + current waypoint number + distance to target + + + + + Message appropriate for high latency connections like Iridium (version 2) + Timestamp (milliseconds since boot or Unix epoch) + Type of the MAV (quadrotor, helicopter, etc.) + Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers. + A bitfield for use for autopilot-specific flags (2 byte version). + Latitude + Longitude + Altitude above mean sea level + Altitude setpoint + Heading + Heading setpoint + Distance to target waypoint or position + Throttle + Airspeed + Airspeed setpoint + Groundspeed + Windspeed + Wind heading + Maximum error horizontal position since last message + Maximum error vertical position since last message + Air temperature from airspeed sensor + Maximum climb rate magnitude since last message + Battery level (-1 if field not provided). + Current waypoint number + Bitmap of failure flags. + Field for custom payload. + Field for custom payload. + Field for custom payload. + + + Vibration levels and accelerometer clipping + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Vibration levels on X-axis + Vibration levels on Y-axis + Vibration levels on Z-axis + first accelerometer clipping count + second accelerometer clipping count + third accelerometer clipping count + + + This message can be requested by sending the MAV_CMD_GET_HOME_POSITION command. The position the system will return to and land on. The position is set automatically by the system during the takeoff in case it was not explicitly set by the operator before or after. The position the system will return to and land on. The global and local positions encode the position in the respective coordinate frames, while the q parameter encodes the orientation of the surface. Under normal conditions it describes the heading and terrain slope, which can be used by the aircraft to adjust the approach. The approach 3D vector describes the point to which the system should fly in normal flight mode and then perform a landing sequence along the vector. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + Local X position of this position in the local coordinate frame + Local Y position of this position in the local coordinate frame + Local Z position of this position in the local coordinate frame + World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + + + The position the system will return to and land on. The position is set automatically by the system during the takeoff in case it was not explicitly set by the operator before or after. The global and local positions encode the position in the respective coordinate frames, while the q parameter encodes the orientation of the surface. Under normal conditions it describes the heading and terrain slope, which can be used by the aircraft to adjust the approach. The approach 3D vector describes the point to which the system should fly in normal flight mode and then perform a landing sequence along the vector. + System ID. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + Local X position of this position in the local coordinate frame + Local Y position of this position in the local coordinate frame + Local Z position of this position in the local coordinate frame + World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + + + The interval between messages for a particular MAVLink message ID. This message is the response to the MAV_CMD_GET_MESSAGE_INTERVAL command. This interface replaces DATA_STREAM. + The ID of the requested MAVLink message. v1.0 is limited to 254 messages. + The interval between two messages. A value of -1 indicates this stream is disabled, 0 indicates it is not available, > 0 indicates the interval at which it is sent. + + + Provides state for additional features + The VTOL state if applicable. Is set to MAV_VTOL_STATE_UNDEFINED if UAV is not in VTOL configuration. + The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + + + The location and information of an ADSB vehicle + ICAO address + Latitude + Longitude + ADSB altitude type. + Altitude(ASL) + Course over ground + The horizontal velocity + The vertical velocity. Positive is up + The callsign, 8+null + ADSB emitter type. + Time since last communication in seconds + Bitmap to indicate various statuses including valid data fields + Squawk code + + + Information about a potential collision + Collision data source + Unique identifier, domain based on src field + Action that is being taken to avoid this collision + How concerned the aircraft is about this collision + Estimated time until collision occurs + Closest vertical distance between vehicle and object + Closest horizontal distance between vehicle and object + + + Message implementing parts of the V2 payload specs in V1 frames for transitional support. + Network ID (0 for broadcast) + System ID (0 for broadcast) + Component ID (0 for broadcast) + A code that identifies the software component that understands this message (analogous to USB device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/definition_files/extension_message_ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + Variable length payload. The length must be encoded in the payload as part of the message_type protocol, e.g. by including the length as payload data, or by terminating the payload data with a non-zero marker. This is required in order to reconstruct zero-terminated payloads that are (or otherwise would be) trimmed by MAVLink 2 empty-byte truncation. The entire content of the payload block is opaque unless you understand the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the MAVLink specification. + + + Send raw controller memory. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Starting address of the debug variables + Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + Memory contents at specified address + + + To debug something using a named 3D vector. + Name + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + x + y + z + + + Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Timestamp (time since system boot). + Name of the debug variable + Floating point value + + + Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Timestamp (time since system boot). + Name of the debug variable + Signed integer value + + + Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). + Severity of status. Relies on the definitions within RFC-5424. + Status text message, without null termination character + + + Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. + Timestamp (time since system boot). + index of debug variable + DEBUG value + + + + Setup a MAVLink2 signing key. If called with secret_key of all zero and zero initial_timestamp will disable signing + system id of the target + component ID of the target + signing key + initial timestamp + + + Report button state change. + Timestamp (time since system boot). + Time of last change of button state. + Bitmap for state of buttons. + + + New version explicitly defines format. More interoperable. + Control vehicle tone generation (buzzer). + System ID + Component ID + tune in board specific format + + tune extension (appended to tune) + + + Information about a camera + Timestamp (time since system boot). + Name of the camera vendor + Name of the camera model + Version of the camera firmware (v << 24 & 0xff = Dev, v << 16 & 0xff = Patch, v << 8 & 0xff = Minor, v & 0xff = Major) + Focal length + Image sensor size horizontal + Image sensor size vertical + Horizontal image resolution + Vertical image resolution + Reserved for a lens ID + Bitmap of camera capability flags. + Camera definition version (iteration) + Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + + + Settings of a camera, can be requested using MAV_CMD_REQUEST_CAMERA_SETTINGS. + Timestamp (time since system boot). + Camera mode + + Current zoom level (0.0 to 100.0, NaN if not known) + Current focus level (0.0 to 100.0, NaN if not known) + + + Information about a storage medium. This message is sent in response to a request and whenever the status of the storage changes (STORAGE_STATUS). + Timestamp (time since system boot). + Storage ID (1 for first, 2 for second, etc.) + Number of storage devices + Status of storage + Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + Read speed. + Write speed. + + + Information about the status of a capture. + Timestamp (time since system boot). + Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + Current status of video capturing (0: idle, 1: capture in progress) + Image capture interval + Time since recording started + Available storage capacity. + + + Information about a captured image + Timestamp (time since system boot). + Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + Camera ID (1 for first, 2 for second, etc.) + Latitude where image was taken + Longitude where capture was taken + Altitude (MSL) where image was taken + Altitude above ground + Quaternion of camera orientation (w, x, y, z order, zero-rotation is 0, 0, 0, 0) + Zero based index of this image (image count since armed -1) + Boolean indicating success (1) or failure (0) while capturing this image. + URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + + + + + Information about flight since last arming. + Timestamp (time since system boot). + Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + Universally unique identifier (UUID) of flight, should correspond to name of log files + + + Orientation of a mount + Timestamp (time since system boot). + Roll in global frame (set to NaN for invalid). + Pitch in global frame (set to NaN for invalid). + Yaw relative to vehicle (set to NaN for invalid). + + Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + + + A message containing logged data (see also MAV_CMD_LOGGING_START) + system ID of the target + component ID of the target + sequence number (can wrap) + data length + offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + logged data + + + A message containing logged data which requires a LOGGING_ACK to be sent back + system ID of the target + component ID of the target + sequence number (can wrap) + data length + offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + logged data + + + An ack for a LOGGING_DATA_ACKED message + system ID of the target + component ID of the target + sequence number (must match the one in LOGGING_DATA_ACKED) + + + + + Information about video stream + Video Stream ID (1 for first, 2 for second, etc.) + Number of streams available. + Type of stream. + Bitmap of stream status flags. + Frame rate. + Horizontal resolution. + Vertical resolution. + Bit rate. + Video image rotation clockwise. + Horizontal Field of view. + Stream name. + Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + + + + + Information about the status of a video stream. + Video Stream ID (1 for first, 2 for second, etc.) + Bitmap of stream status flags + Frame rate + Horizontal resolution + Vertical resolution + Bit rate + Video image rotation clockwise + Horizontal Field of view + + + Configure AP SSID and Password. + Name of Wi-Fi network (SSID). Leave it blank to leave it unchanged. + Password. Leave it blank for an open AP. + + + + + Version and capability of protocol version. This message is the response to REQUEST_PROTOCOL_VERSION and is used as part of the handshaking to establish which MAVLink version should be used on the network. Every node should respond to REQUEST_PROTOCOL_VERSION to enable the handshaking. Library implementers should consider adding this into the default decoding state machine to allow the protocol core to respond directly. + Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + Minimum MAVLink version supported + Maximum MAVLink version supported (set to the same value as version by default) + The first 8 bytes (not characters printed in hex!) of the git hash. + The first 8 bytes (not characters printed in hex!) of the git hash. + + + + + The location and information of an AIS vessel + Mobile Marine Service Identifier, 9 decimal digits + Latitude + Longitude + Course over ground + True heading + Speed over ground + Turn rate + Navigational status + Type of vessels + Distance from lat/lon location to bow + Distance from lat/lon location to stern + Distance from lat/lon location to port side + Distance from lat/lon location to starboard side + The vessel callsign + The vessel name + Time since last communication in seconds + Bitmask to indicate various statuses including valid data fields + + + + General status information of an UAVCAN node. Please refer to the definition of the UAVCAN message "uavcan.protocol.NodeStatus" for the background information. The UAVCAN specification is available at http://uavcan.org. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Time since the start-up of the node. + Generalized node health status. + Generalized operating mode. + Not used currently. + Vendor-specific status information. + + + General information describing a particular UAVCAN node. Please refer to the definition of the UAVCAN service "uavcan.protocol.GetNodeInfo" for the background information. This message should be emitted by the system whenever a new node appears online, or an existing node reboots. Additionally, it can be emitted upon request from the other end of the MAVLink channel (see MAV_CMD_UAVCAN_GET_NODE_INFO). It is also not prohibited to emit this message unconditionally at a low frequency. The UAVCAN specification is available at http://uavcan.org. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Time since the start-up of the node. + Node name string. For example, "sapog.px4.io". + Hardware major version number. + Hardware minor version number. + Hardware unique 128-bit ID. + Software major version number. + Software minor version number. + Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + + + Request to read the value of a parameter with the either the param_id string id or param_index. + System ID + Component ID + Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + + + Request all parameters of this component. After this request, all parameters are emitted. + System ID + Component ID + + + Emit the value of a parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows them to re-request missing parameters after a loss or timeout. + Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter value + Parameter type. + Total number of parameters + Index of this parameter + + + Set a parameter value. In order to deal with message loss (and retransmission of PARAM_EXT_SET), when setting a parameter value and the new value is the same as the current value, you will immediately get a PARAM_ACK_ACCEPTED response. If the current state is PARAM_ACK_IN_PROGRESS, you will accordingly receive a PARAM_ACK_IN_PROGRESS in response. + System ID + Component ID + Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter value + Parameter type. + + + Response from a PARAM_EXT_SET message. + Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + Parameter type. + Result code. + + + Obstacle distances in front of the sensor, starting from the left in increment degrees to the right + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Class id of the distance sensor type. + Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + Minimum distance the sensor can measure. + Maximum distance the sensor can measure. + + Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + + + Odometry message to communicate odometry information with an external interface. Fits ROS REP 147 standard for aerial vehicles (http://www.ros.org/reps/rep-0147.html). + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Coordinate frame of reference for the pose data. + Coordinate frame of reference for the velocity in free space (twist) data. + X Position + Y Position + Z Position + Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + X linear speed + Y linear speed + Z linear speed + Roll angular speed + Pitch angular speed + Yaw angular speed + Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + + Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + Type of estimator that is providing the odometry. + + + + + Describe a trajectory using an array of up-to 5 waypoints in the local frame. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Number of valid points (up-to 5 waypoints are possible) + X-coordinate of waypoint, set to NaN if not being used + Y-coordinate of waypoint, set to NaN if not being used + Z-coordinate of waypoint, set to NaN if not being used + X-velocity of waypoint, set to NaN if not being used + Y-velocity of waypoint, set to NaN if not being used + Z-velocity of waypoint, set to NaN if not being used + X-acceleration of waypoint, set to NaN if not being used + Y-acceleration of waypoint, set to NaN if not being used + Z-acceleration of waypoint, set to NaN if not being used + Yaw angle, set to NaN if not being used + Yaw rate, set to NaN if not being used + Scheduled action for each waypoint, UINT16_MAX if not being used. + + + + + Describe a trajectory using an array of up-to 5 bezier points in the local frame. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Number of valid points (up-to 5 waypoints are possible) + X-coordinate of starting bezier point, set to NaN if not being used + Y-coordinate of starting bezier point, set to NaN if not being used + Z-coordinate of starting bezier point, set to NaN if not being used + Bezier time horizon, set to NaN if velocity/acceleration should not be incorporated + Yaw, set to NaN for unchanged + + + + Report current used cellular network status + Status bitmap + Cellular network radio type: gsm, cdma, lte... + Cellular network RSSI/RSRP in dBm, absolute value + Mobile country code. If unknown, set to: UINT16_MAX + Mobile network code. If unknown, set to: UINT16_MAX + Location area code. If unknown, set to: 0 + Cell ID. If unknown, set to: UINT32_MAX + + + Status of the Iridium SBD link. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Number of failed SBD sessions. + Number of successful SBD sessions. + Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + 1: Ring call pending, 0: No call pending. + 1: Transmission session pending, 0: No transmission session pending. + 1: Receiving session pending, 0: No receiving session pending. + + + + + The global position resulting from GPS and sensor fusion. + Time of applicability of position (microseconds since UNIX epoch). + Unique UAS ID. + Latitude (WGS84) + Longitude (WGS84) + Altitude (WGS84) + Altitude above ground + Ground X speed (latitude, positive north) + Ground Y speed (longitude, positive east) + Ground Z speed (altitude, positive down) + Horizontal position uncertainty (standard deviation) + Altitude uncertainty (standard deviation) + Speed uncertainty (standard deviation) + Next waypoint, latitude (WGS84) + Next waypoint, longitude (WGS84) + Next waypoint, altitude (WGS84) + Time until next update. Set to 0 if unknown or in data driven mode. + Flight state + Bitwise OR combination of the data available flags. + + + Large debug/prototyping array. The message uses the maximum available payload for data. The array_id and name fields are used to discriminate between messages in code and in user interfaces (respectively). Do not use in production code. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Name, for human-friendly display in a Ground Control Station + Unique ID used to discriminate between arrays + + data + + + + + Vehicle status report that is sent out while orbit execution is in progress (see MAV_CMD_DO_ORBIT). + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + The coordinate system of the fields: x, y, z. + X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + Altitude of center point. Coordinate system depends on frame field. + + + Status text message (use only for important status and error messages). The full message payload can be used for status text, but we recommend that updates be kept concise. Note: The message is intended as a less restrictive replacement for STATUSTEXT. + Severity of status. Relies on the definitions within RFC-5424. + Status text message, without null termination character. + + + + + + Smart Battery information (static/infrequent update). Use for updates from: smart battery to flight stack, flight stack to GCS. Use instead of BATTERY_STATUS for smart batteries. + Battery ID + Capacity when full according to manufacturer, -1: field not provided. + Capacity when full (accounting for battery degradation), -1: field not provided. + Charge/discharge cycle count. -1: field not provided. + Serial number. -1: field not provided. + Static device name. Encode as manufacturer and product names separated using an underscore. + Battery weight. 0: field not provided. + Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + + + + + Smart Battery information (dynamic). Use for updates from: smart battery to flight stack, flight stack to GCS. Use instead of BATTERY_STATUS for smart batteries. + Battery ID + Remaining battery energy. Values: [0-100], -1: field not provided. + Battery current (through all cells/loads). Positive if discharging, negative if charging. UINT16_MAX: field not provided. + Battery temperature. -1: field not provided. + Fault/health indications. + Estimated remaining battery time. -1: field not provided. + The cell number of the first index in the 'voltages' array field. Using this field allows you to specify cell voltages for batteries with more than 16 cells. + Individual cell voltages. Batteries with more 16 cells can use the cell_offset field to specify the cell offset for the array specified in the current message . Index values above the valid cell count for this battery should have the UINT16_MAX value. + + + The raw values of the actuator outputs (e.g. on Pixhawk, from MAIN, AUX ports). This message supersedes SERVO_OUTPUT_RAW. + Timestamp (since system boot). + Active outputs + Servo / motor output array values. Zero values indicate unused channels. + + + + + Time/duration estimates for various events and actions given the current vehicle state and position. + Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + + + + + Message for transporting "arbitrary" variable-length data from one component to another (broadcast is not forbidden, but discouraged). The encoding of the data is usually extension specific, i.e. determined by the source, and is usually not documented as part of the MAVLink specification. + System ID (can be 0 for broadcast, but this is discouraged) + Component ID (can be 0 for broadcast, but this is discouraged) + A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + Length of the data transported in payload + Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + + + + + Hardware status sent by an onboard computer. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + Time since system boot. + Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + Temperature of the board. A value of INT8_MAX implies the field is unused. + Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + Fan speeds. A value of INT16_MAX implies the field is unused. + Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + + + + + Information about a component. For camera components instead use CAMERA_INFORMATION, and for autopilots use AUTOPILOT_VERSION. Components including GCSes should consider supporting requests of this message via MAV_CMD_REQUEST_MESSAGE. + Timestamp (time since system boot). + Name of the component vendor + Name of the component model + Version of the component firmware (v << 24 & 0xff = Dev, v << 16 & 0xff = Patch, v << 8 & 0xff = Minor, v & 0xff = Major) + Version of the component hardware (v << 24 & 0xff = Dev, v << 16 & 0xff = Patch, v << 8 & 0xff = Minor, v & 0xff = Major) + Bitmap of component capability flags. + Component definition version (iteration) + Component definition URI (if any, otherwise only basic functions will be available). The XML format is not yet specified and work in progress. + + + + + Play vehicle tone/tune (buzzer). Supersedes message PLAY_TUNE. + System ID + Component ID + Tune format + Tune definition as a NULL-terminated string. + + + + + Tune formats supported by vehicle. This should be emitted as response to MAV_CMD_REQUEST_MESSAGE. + System ID + Component ID + Bitfield of supported tune formats. + + + + Cumulative distance traveled for each reported wheel. + Timestamp (synced to UNIX time or since system boot). + Number of wheels reported. + Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + + + + + Data for filling the OpenDroneID Basic ID message. This and the below messages are primarily meant for feeding data to/from an OpenDroneID implementation. E.g. https://github.com/opendroneid/opendroneid-core-c + Indicates the format for the uas_id field of this message. + Indicates the type of UA (Unmanned Aircraft). + UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + + + + + Data for filling the OpenDroneID Location message. The float data types are 32-bit IEEE 754. The Location message provides the location, altitude, direction and speed of the aircraft. + Indicates whether the Unmanned Aircraft is on the ground or in the air. + Direction over ground (not heading, but direction of movement) in degrees * 100: 0.0 - 359.99 degrees. If unknown: 361.00 degrees. + Ground speed. Positive only. If unknown: 255.00 m/s. If speed is larger than 254.25 m/s, use 254.25 m/s. + The vertical speed. Up is positive. If unknown: 63.00 m/s. If speed is larger than 62.00 m/s, use 62.00 m/s. + Current latitude of the UA (Unmanned Aircraft). If unknown: 0 deg (both Lat/Lon). + Current longitude of the UA (Unmanned Aircraft). If unknown: 0 deg (both Lat/Lon). + The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + The geodetic altitude as defined by WGS84. If unknown: -1000 m. + Indicates the reference point for the height field. + The current height of the UA (Unmanned Aircraft) above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + The accuracy of the horizontal position. + The accuracy of the vertical position. + The accuracy of the barometric altitude. + The accuracy of the horizontal and vertical speed. + Seconds after the full hour. Typically the GPS outputs a time of week value in milliseconds. That value can be easily converted for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + The accuracy of the timestamps. + + + + + Data for filling the OpenDroneID Authentication message. The Authentication Message defines a field that can provide a means of authenticity for the identity of the UAS (Unmanned Aircraft System). The Authentication message can have two different formats. Five data pages are supported. For data page 0, the fields PageCount, Length and TimeStamp are present and AuthData is only 17 bytes. For data page 1 through 4, PageCount,Length and TimeStamp are not present and the size of AuthData is 23 bytes. + Indicates the type of authentication. + Allowed range is 0 - 4. + This field is only present for page 0. Allowed range is 0 - 5. + This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + + + + + Data for filling the OpenDroneID Self ID message. The Self ID Message is an opportunity for the operator to (optionally) declare their identity and purpose of the flight. This message can provide additional information that could reduce the threat profile of a UA (Unmanned Aircraft) flying in a particular area or manner. + Indicates the type of the description field. + Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + + + + + Data for filling the OpenDroneID System message. The System Message contains general system information including the operator location and possible aircraft group information. + Specifies the location source for the operator location. + Latitude of the operator. If unknown: 0 deg (both Lat/Lon). + Longitude of the operator. If unknown: 0 deg (both Lat/Lon). + Number of aircraft in the area, group or formation (default 1). + Radius of the cylindrical area of the group or formation (default 0). + Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + Area Operations Floor relative to WGS84. If unknown: -1000 m. + + + + + Data for filling the OpenDroneID Operator ID message, which contains the CAA (Civil Aviation Authority) issued operator ID. + Indicates the type of the operator_id field. + Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + + + + + + An OpenDroneID message pack is a container for multiple encoded OpenDroneID messages (i.e. not in the format given for the above messages descriptions but after encoding into the compressed OpenDroneID byte format). Used e.g. when transmitting on Bluetooth 5.0 Long Range/Extended Advertising or on WiFi Neighbor Aware Networking. + This field must currently always be equal to 25 bytes, since all encoded OpenDroneID messages are specificed to have this length. + Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + + + diff --git a/MavLinkCom/mavlink/protocol.h b/MavLinkCom/mavlink/protocol.h index 8550277b3..22bd0ca0d 100644 --- a/MavLinkCom/mavlink/protocol.h +++ b/MavLinkCom/mavlink/protocol.h @@ -234,9 +234,9 @@ _MAV_PUT_ARRAY(int64_t, i64) _MAV_PUT_ARRAY(float, f) _MAV_PUT_ARRAY(double, d) -#define _MAV_RETURN_char(msg, wire_offset) (const char)_MAV_PAYLOAD(msg)[wire_offset] -#define _MAV_RETURN_int8_t(msg, wire_offset) (const int8_t)_MAV_PAYLOAD(msg)[wire_offset] -#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset] +#define _MAV_RETURN_char(msg, wire_offset) (char)_MAV_PAYLOAD(msg)[wire_offset] +#define _MAV_RETURN_int8_t(msg, wire_offset) (int8_t)_MAV_PAYLOAD(msg)[wire_offset] +#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset] #if MAVLINK_NEED_BYTE_SWAP #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \ diff --git a/MavLinkCom/src/MavLinkConnection.cpp b/MavLinkCom/src/MavLinkConnection.cpp index 2150e4a96..0d1b93ee9 100644 --- a/MavLinkCom/src/MavLinkConnection.cpp +++ b/MavLinkCom/src/MavLinkConnection.cpp @@ -32,12 +32,16 @@ std::shared_ptr MavLinkConnection::connectTcp(const std::str return MavLinkConnectionImpl::connectTcp(nodeName, localAddr, remoteIpAddr, remotePort); } +void MavLinkConnection::acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort) +{ + pImpl->acceptTcp(shared_from_this(), nodeName, localAddr, listeningPort); +} + void MavLinkConnection::startListening(const std::string& nodeName, std::shared_ptr connectedPort) { pImpl->startListening(shared_from_this(), nodeName, connectedPort); } - // log every message that is "sent" using sendMessage. void MavLinkConnection::startLoggingSendMessage(std::shared_ptr log) { @@ -104,6 +108,12 @@ void MavLinkConnection::unsubscribe(int id) pImpl->unsubscribe(id); } + +bool MavLinkConnection::isPublishThread() const +{ + return pImpl->isPublishThread(); +} + MavLinkConnection::~MavLinkConnection() { pImpl->close(); pImpl = nullptr; diff --git a/MavLinkCom/src/MavLinkMessages.cpp b/MavLinkCom/src/MavLinkMessages.cpp index bc43732f8..32aa684c8 100644 --- a/MavLinkCom/src/MavLinkMessages.cpp +++ b/MavLinkCom/src/MavLinkMessages.cpp @@ -208,6 +208,54 @@ std::string MavLinkAuthKey::toJSon() { return ss.str(); } +int MavLinkLinkNodeStatus::pack(char* buffer) const { + pack_uint64_t(buffer, reinterpret_cast(&this->timestamp), 0); + pack_uint32_t(buffer, reinterpret_cast(&this->tx_rate), 8); + pack_uint32_t(buffer, reinterpret_cast(&this->rx_rate), 12); + pack_uint32_t(buffer, reinterpret_cast(&this->messages_sent), 16); + pack_uint32_t(buffer, reinterpret_cast(&this->messages_received), 20); + pack_uint32_t(buffer, reinterpret_cast(&this->messages_lost), 24); + pack_uint16_t(buffer, reinterpret_cast(&this->rx_parse_err), 28); + pack_uint16_t(buffer, reinterpret_cast(&this->tx_overflows), 30); + pack_uint16_t(buffer, reinterpret_cast(&this->rx_overflows), 32); + pack_uint8_t(buffer, reinterpret_cast(&this->tx_buf), 34); + pack_uint8_t(buffer, reinterpret_cast(&this->rx_buf), 35); + return 36; +} + +int MavLinkLinkNodeStatus::unpack(const char* buffer) { + unpack_uint64_t(buffer, reinterpret_cast(&this->timestamp), 0); + unpack_uint32_t(buffer, reinterpret_cast(&this->tx_rate), 8); + unpack_uint32_t(buffer, reinterpret_cast(&this->rx_rate), 12); + unpack_uint32_t(buffer, reinterpret_cast(&this->messages_sent), 16); + unpack_uint32_t(buffer, reinterpret_cast(&this->messages_received), 20); + unpack_uint32_t(buffer, reinterpret_cast(&this->messages_lost), 24); + unpack_uint16_t(buffer, reinterpret_cast(&this->rx_parse_err), 28); + unpack_uint16_t(buffer, reinterpret_cast(&this->tx_overflows), 30); + unpack_uint16_t(buffer, reinterpret_cast(&this->rx_overflows), 32); + unpack_uint8_t(buffer, reinterpret_cast(&this->tx_buf), 34); + unpack_uint8_t(buffer, reinterpret_cast(&this->rx_buf), 35); + return 36; +} + +std::string MavLinkLinkNodeStatus::toJSon() { + std::ostringstream ss; + ss << "{ \"name\": \"LINK_NODE_STATUS\", \"id\": 8, \"timestamp\":" << timestamp << ", \"msg\": {"; + ss << "\"timestamp\":" << this->timestamp; + ss << ", \"tx_rate\":" << this->tx_rate; + ss << ", \"rx_rate\":" << this->rx_rate; + ss << ", \"messages_sent\":" << this->messages_sent; + ss << ", \"messages_received\":" << this->messages_received; + ss << ", \"messages_lost\":" << this->messages_lost; + ss << ", \"rx_parse_err\":" << this->rx_parse_err; + ss << ", \"tx_overflows\":" << this->tx_overflows; + ss << ", \"rx_overflows\":" << this->rx_overflows; + ss << ", \"tx_buf\":" << static_cast(this->tx_buf); + ss << ", \"rx_buf\":" << static_cast(this->rx_buf); + ss << "} },"; + return ss.str(); +} + int MavLinkSetMode::pack(char* buffer) const { pack_uint32_t(buffer, reinterpret_cast(&this->custom_mode), 0); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 4); @@ -351,7 +399,12 @@ int MavLinkGpsRawInt::pack(char* buffer) const { pack_uint16_t(buffer, reinterpret_cast(&this->cog), 26); pack_uint8_t(buffer, reinterpret_cast(&this->fix_type), 28); pack_uint8_t(buffer, reinterpret_cast(&this->satellites_visible), 29); - return 30; + pack_int32_t(buffer, reinterpret_cast(&this->alt_ellipsoid), 30); + pack_uint32_t(buffer, reinterpret_cast(&this->h_acc), 34); + pack_uint32_t(buffer, reinterpret_cast(&this->v_acc), 38); + pack_uint32_t(buffer, reinterpret_cast(&this->vel_acc), 42); + pack_uint32_t(buffer, reinterpret_cast(&this->hdg_acc), 46); + return 50; } int MavLinkGpsRawInt::unpack(const char* buffer) { @@ -365,7 +418,12 @@ int MavLinkGpsRawInt::unpack(const char* buffer) { unpack_uint16_t(buffer, reinterpret_cast(&this->cog), 26); unpack_uint8_t(buffer, reinterpret_cast(&this->fix_type), 28); unpack_uint8_t(buffer, reinterpret_cast(&this->satellites_visible), 29); - return 30; + unpack_int32_t(buffer, reinterpret_cast(&this->alt_ellipsoid), 30); + unpack_uint32_t(buffer, reinterpret_cast(&this->h_acc), 34); + unpack_uint32_t(buffer, reinterpret_cast(&this->v_acc), 38); + unpack_uint32_t(buffer, reinterpret_cast(&this->vel_acc), 42); + unpack_uint32_t(buffer, reinterpret_cast(&this->hdg_acc), 46); + return 50; } std::string MavLinkGpsRawInt::toJSon() { @@ -381,6 +439,11 @@ std::string MavLinkGpsRawInt::toJSon() { ss << ", \"cog\":" << this->cog; ss << ", \"fix_type\":" << static_cast(this->fix_type); ss << ", \"satellites_visible\":" << static_cast(this->satellites_visible); + ss << ", \"alt_ellipsoid\":" << this->alt_ellipsoid; + ss << ", \"h_acc\":" << this->h_acc; + ss << ", \"v_acc\":" << this->v_acc; + ss << ", \"vel_acc\":" << this->vel_acc; + ss << ", \"hdg_acc\":" << this->hdg_acc; ss << "} },"; return ss.str(); } @@ -429,7 +492,8 @@ int MavLinkScaledImu::pack(char* buffer) const { pack_int16_t(buffer, reinterpret_cast(&this->xmag), 16); pack_int16_t(buffer, reinterpret_cast(&this->ymag), 18); pack_int16_t(buffer, reinterpret_cast(&this->zmag), 20); - return 22; + pack_int16_t(buffer, reinterpret_cast(&this->temperature), 22); + return 24; } int MavLinkScaledImu::unpack(const char* buffer) { @@ -443,7 +507,8 @@ int MavLinkScaledImu::unpack(const char* buffer) { unpack_int16_t(buffer, reinterpret_cast(&this->xmag), 16); unpack_int16_t(buffer, reinterpret_cast(&this->ymag), 18); unpack_int16_t(buffer, reinterpret_cast(&this->zmag), 20); - return 22; + unpack_int16_t(buffer, reinterpret_cast(&this->temperature), 22); + return 24; } std::string MavLinkScaledImu::toJSon() { @@ -459,6 +524,7 @@ std::string MavLinkScaledImu::toJSon() { ss << ", \"xmag\":" << this->xmag; ss << ", \"ymag\":" << this->ymag; ss << ", \"zmag\":" << this->zmag; + ss << ", \"temperature\":" << this->temperature; ss << "} },"; return ss.str(); } @@ -474,7 +540,9 @@ int MavLinkRawImu::pack(char* buffer) const { pack_int16_t(buffer, reinterpret_cast(&this->xmag), 20); pack_int16_t(buffer, reinterpret_cast(&this->ymag), 22); pack_int16_t(buffer, reinterpret_cast(&this->zmag), 24); - return 26; + pack_uint8_t(buffer, reinterpret_cast(&this->id), 26); + pack_int16_t(buffer, reinterpret_cast(&this->temperature), 27); + return 29; } int MavLinkRawImu::unpack(const char* buffer) { @@ -488,7 +556,9 @@ int MavLinkRawImu::unpack(const char* buffer) { unpack_int16_t(buffer, reinterpret_cast(&this->xmag), 20); unpack_int16_t(buffer, reinterpret_cast(&this->ymag), 22); unpack_int16_t(buffer, reinterpret_cast(&this->zmag), 24); - return 26; + unpack_uint8_t(buffer, reinterpret_cast(&this->id), 26); + unpack_int16_t(buffer, reinterpret_cast(&this->temperature), 27); + return 29; } std::string MavLinkRawImu::toJSon() { @@ -504,6 +574,8 @@ std::string MavLinkRawImu::toJSon() { ss << ", \"xmag\":" << this->xmag; ss << ", \"ymag\":" << this->ymag; ss << ", \"zmag\":" << this->zmag; + ss << ", \"id\":" << static_cast(this->id); + ss << ", \"temperature\":" << this->temperature; ss << "} },"; return ss.str(); } @@ -610,7 +682,8 @@ int MavLinkAttitudeQuaternion::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->rollspeed), 20); pack_float(buffer, reinterpret_cast(&this->pitchspeed), 24); pack_float(buffer, reinterpret_cast(&this->yawspeed), 28); - return 32; + pack_float_array(4, buffer, reinterpret_cast(&this->repr_offset_q[0]), 32); + return 48; } int MavLinkAttitudeQuaternion::unpack(const char* buffer) { @@ -622,7 +695,8 @@ int MavLinkAttitudeQuaternion::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->rollspeed), 20); unpack_float(buffer, reinterpret_cast(&this->pitchspeed), 24); unpack_float(buffer, reinterpret_cast(&this->yawspeed), 28); - return 32; + unpack_float_array(4, buffer, reinterpret_cast(&this->repr_offset_q[0]), 32); + return 48; } std::string MavLinkAttitudeQuaternion::toJSon() { @@ -636,6 +710,7 @@ std::string MavLinkAttitudeQuaternion::toJSon() { ss << ", \"rollspeed\":" << float_tostring(this->rollspeed); ss << ", \"pitchspeed\":" << float_tostring(this->pitchspeed); ss << ", \"yawspeed\":" << float_tostring(this->yawspeed); + ss << ", \"repr_offset_q\":" << "[" << float_array_tostring(4, reinterpret_cast(&this->repr_offset_q[0])) << "]"; ss << "} },"; return ss.str(); } @@ -824,15 +899,15 @@ int MavLinkServoOutputRaw::pack(char* buffer) const { pack_uint16_t(buffer, reinterpret_cast(&this->servo6_raw), 14); pack_uint16_t(buffer, reinterpret_cast(&this->servo7_raw), 16); pack_uint16_t(buffer, reinterpret_cast(&this->servo8_raw), 18); - pack_uint16_t(buffer, reinterpret_cast(&this->servo9_raw), 20); - pack_uint16_t(buffer, reinterpret_cast(&this->servo10_raw), 22); - pack_uint16_t(buffer, reinterpret_cast(&this->servo11_raw), 24); - pack_uint16_t(buffer, reinterpret_cast(&this->servo12_raw), 26); - pack_uint16_t(buffer, reinterpret_cast(&this->servo13_raw), 28); - pack_uint16_t(buffer, reinterpret_cast(&this->servo14_raw), 30); - pack_uint16_t(buffer, reinterpret_cast(&this->servo15_raw), 32); - pack_uint16_t(buffer, reinterpret_cast(&this->servo16_raw), 34); - pack_uint8_t(buffer, reinterpret_cast(&this->port), 36); + pack_uint8_t(buffer, reinterpret_cast(&this->port), 20); + pack_uint16_t(buffer, reinterpret_cast(&this->servo9_raw), 21); + pack_uint16_t(buffer, reinterpret_cast(&this->servo10_raw), 23); + pack_uint16_t(buffer, reinterpret_cast(&this->servo11_raw), 25); + pack_uint16_t(buffer, reinterpret_cast(&this->servo12_raw), 27); + pack_uint16_t(buffer, reinterpret_cast(&this->servo13_raw), 29); + pack_uint16_t(buffer, reinterpret_cast(&this->servo14_raw), 31); + pack_uint16_t(buffer, reinterpret_cast(&this->servo15_raw), 33); + pack_uint16_t(buffer, reinterpret_cast(&this->servo16_raw), 35); return 37; } @@ -846,15 +921,15 @@ int MavLinkServoOutputRaw::unpack(const char* buffer) { unpack_uint16_t(buffer, reinterpret_cast(&this->servo6_raw), 14); unpack_uint16_t(buffer, reinterpret_cast(&this->servo7_raw), 16); unpack_uint16_t(buffer, reinterpret_cast(&this->servo8_raw), 18); - unpack_uint16_t(buffer, reinterpret_cast(&this->servo9_raw), 20); - unpack_uint16_t(buffer, reinterpret_cast(&this->servo10_raw), 22); - unpack_uint16_t(buffer, reinterpret_cast(&this->servo11_raw), 24); - unpack_uint16_t(buffer, reinterpret_cast(&this->servo12_raw), 26); - unpack_uint16_t(buffer, reinterpret_cast(&this->servo13_raw), 28); - unpack_uint16_t(buffer, reinterpret_cast(&this->servo14_raw), 30); - unpack_uint16_t(buffer, reinterpret_cast(&this->servo15_raw), 32); - unpack_uint16_t(buffer, reinterpret_cast(&this->servo16_raw), 34); - unpack_uint8_t(buffer, reinterpret_cast(&this->port), 36); + unpack_uint8_t(buffer, reinterpret_cast(&this->port), 20); + unpack_uint16_t(buffer, reinterpret_cast(&this->servo9_raw), 21); + unpack_uint16_t(buffer, reinterpret_cast(&this->servo10_raw), 23); + unpack_uint16_t(buffer, reinterpret_cast(&this->servo11_raw), 25); + unpack_uint16_t(buffer, reinterpret_cast(&this->servo12_raw), 27); + unpack_uint16_t(buffer, reinterpret_cast(&this->servo13_raw), 29); + unpack_uint16_t(buffer, reinterpret_cast(&this->servo14_raw), 31); + unpack_uint16_t(buffer, reinterpret_cast(&this->servo15_raw), 33); + unpack_uint16_t(buffer, reinterpret_cast(&this->servo16_raw), 35); return 37; } @@ -870,6 +945,7 @@ std::string MavLinkServoOutputRaw::toJSon() { ss << ", \"servo6_raw\":" << this->servo6_raw; ss << ", \"servo7_raw\":" << this->servo7_raw; ss << ", \"servo8_raw\":" << this->servo8_raw; + ss << ", \"port\":" << static_cast(this->port); ss << ", \"servo9_raw\":" << this->servo9_raw; ss << ", \"servo10_raw\":" << this->servo10_raw; ss << ", \"servo11_raw\":" << this->servo11_raw; @@ -878,7 +954,6 @@ std::string MavLinkServoOutputRaw::toJSon() { ss << ", \"servo14_raw\":" << this->servo14_raw; ss << ", \"servo15_raw\":" << this->servo15_raw; ss << ", \"servo16_raw\":" << this->servo16_raw; - ss << ", \"port\":" << static_cast(this->port); ss << "} },"; return ss.str(); } @@ -888,7 +963,8 @@ int MavLinkMissionRequestPartialList::pack(char* buffer) const { pack_int16_t(buffer, reinterpret_cast(&this->end_index), 2); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 4); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 5); - return 6; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 6); + return 7; } int MavLinkMissionRequestPartialList::unpack(const char* buffer) { @@ -896,7 +972,8 @@ int MavLinkMissionRequestPartialList::unpack(const char* buffer) { unpack_int16_t(buffer, reinterpret_cast(&this->end_index), 2); unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 4); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 5); - return 6; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 6); + return 7; } std::string MavLinkMissionRequestPartialList::toJSon() { @@ -906,6 +983,7 @@ std::string MavLinkMissionRequestPartialList::toJSon() { ss << ", \"end_index\":" << this->end_index; ss << ", \"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -915,7 +993,8 @@ int MavLinkMissionWritePartialList::pack(char* buffer) const { pack_int16_t(buffer, reinterpret_cast(&this->end_index), 2); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 4); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 5); - return 6; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 6); + return 7; } int MavLinkMissionWritePartialList::unpack(const char* buffer) { @@ -923,7 +1002,8 @@ int MavLinkMissionWritePartialList::unpack(const char* buffer) { unpack_int16_t(buffer, reinterpret_cast(&this->end_index), 2); unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 4); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 5); - return 6; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 6); + return 7; } std::string MavLinkMissionWritePartialList::toJSon() { @@ -933,6 +1013,7 @@ std::string MavLinkMissionWritePartialList::toJSon() { ss << ", \"end_index\":" << this->end_index; ss << ", \"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -952,7 +1033,8 @@ int MavLinkMissionItem::pack(char* buffer) const { pack_uint8_t(buffer, reinterpret_cast(&this->frame), 34); pack_uint8_t(buffer, reinterpret_cast(&this->current), 35); pack_uint8_t(buffer, reinterpret_cast(&this->autocontinue), 36); - return 37; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 37); + return 38; } int MavLinkMissionItem::unpack(const char* buffer) { @@ -970,7 +1052,8 @@ int MavLinkMissionItem::unpack(const char* buffer) { unpack_uint8_t(buffer, reinterpret_cast(&this->frame), 34); unpack_uint8_t(buffer, reinterpret_cast(&this->current), 35); unpack_uint8_t(buffer, reinterpret_cast(&this->autocontinue), 36); - return 37; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 37); + return 38; } std::string MavLinkMissionItem::toJSon() { @@ -990,6 +1073,7 @@ std::string MavLinkMissionItem::toJSon() { ss << ", \"frame\":" << static_cast(this->frame); ss << ", \"current\":" << static_cast(this->current); ss << ", \"autocontinue\":" << static_cast(this->autocontinue); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -998,14 +1082,16 @@ int MavLinkMissionRequest::pack(char* buffer) const { pack_uint16_t(buffer, reinterpret_cast(&this->seq), 0); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 2); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 3); - return 4; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 4); + return 5; } int MavLinkMissionRequest::unpack(const char* buffer) { unpack_uint16_t(buffer, reinterpret_cast(&this->seq), 0); unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 2); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 3); - return 4; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 4); + return 5; } std::string MavLinkMissionRequest::toJSon() { @@ -1014,6 +1100,7 @@ std::string MavLinkMissionRequest::toJSon() { ss << "\"seq\":" << this->seq; ss << ", \"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -1063,13 +1150,15 @@ std::string MavLinkMissionCurrent::toJSon() { int MavLinkMissionRequestList::pack(char* buffer) const { pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 0); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 1); - return 2; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 2); + return 3; } int MavLinkMissionRequestList::unpack(const char* buffer) { unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 0); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 1); - return 2; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 2); + return 3; } std::string MavLinkMissionRequestList::toJSon() { @@ -1077,6 +1166,7 @@ std::string MavLinkMissionRequestList::toJSon() { ss << "{ \"name\": \"MISSION_REQUEST_LIST\", \"id\": 43, \"timestamp\":" << timestamp << ", \"msg\": {"; ss << "\"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -1085,14 +1175,16 @@ int MavLinkMissionCount::pack(char* buffer) const { pack_uint16_t(buffer, reinterpret_cast(&this->count), 0); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 2); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 3); - return 4; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 4); + return 5; } int MavLinkMissionCount::unpack(const char* buffer) { unpack_uint16_t(buffer, reinterpret_cast(&this->count), 0); unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 2); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 3); - return 4; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 4); + return 5; } std::string MavLinkMissionCount::toJSon() { @@ -1101,6 +1193,7 @@ std::string MavLinkMissionCount::toJSon() { ss << "\"count\":" << this->count; ss << ", \"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -1108,13 +1201,15 @@ std::string MavLinkMissionCount::toJSon() { int MavLinkMissionClearAll::pack(char* buffer) const { pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 0); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 1); - return 2; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 2); + return 3; } int MavLinkMissionClearAll::unpack(const char* buffer) { unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 0); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 1); - return 2; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 2); + return 3; } std::string MavLinkMissionClearAll::toJSon() { @@ -1122,6 +1217,7 @@ std::string MavLinkMissionClearAll::toJSon() { ss << "{ \"name\": \"MISSION_CLEAR_ALL\", \"id\": 45, \"timestamp\":" << timestamp << ", \"msg\": {"; ss << "\"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -1148,14 +1244,16 @@ int MavLinkMissionAck::pack(char* buffer) const { pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 0); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 1); pack_uint8_t(buffer, reinterpret_cast(&this->type), 2); - return 3; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 3); + return 4; } int MavLinkMissionAck::unpack(const char* buffer) { unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 0); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 1); unpack_uint8_t(buffer, reinterpret_cast(&this->type), 2); - return 3; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 3); + return 4; } std::string MavLinkMissionAck::toJSon() { @@ -1164,6 +1262,7 @@ std::string MavLinkMissionAck::toJSon() { ss << "\"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); ss << ", \"type\":" << static_cast(this->type); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -1173,7 +1272,8 @@ int MavLinkSetGpsGlobalOrigin::pack(char* buffer) const { pack_int32_t(buffer, reinterpret_cast(&this->longitude), 4); pack_int32_t(buffer, reinterpret_cast(&this->altitude), 8); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 12); - return 13; + pack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 13); + return 21; } int MavLinkSetGpsGlobalOrigin::unpack(const char* buffer) { @@ -1181,7 +1281,8 @@ int MavLinkSetGpsGlobalOrigin::unpack(const char* buffer) { unpack_int32_t(buffer, reinterpret_cast(&this->longitude), 4); unpack_int32_t(buffer, reinterpret_cast(&this->altitude), 8); unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 12); - return 13; + unpack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 13); + return 21; } std::string MavLinkSetGpsGlobalOrigin::toJSon() { @@ -1191,6 +1292,7 @@ std::string MavLinkSetGpsGlobalOrigin::toJSon() { ss << ", \"longitude\":" << this->longitude; ss << ", \"altitude\":" << this->altitude; ss << ", \"target_system\":" << static_cast(this->target_system); + ss << ", \"time_usec\":" << this->time_usec; ss << "} },"; return ss.str(); } @@ -1199,14 +1301,16 @@ int MavLinkGpsGlobalOrigin::pack(char* buffer) const { pack_int32_t(buffer, reinterpret_cast(&this->latitude), 0); pack_int32_t(buffer, reinterpret_cast(&this->longitude), 4); pack_int32_t(buffer, reinterpret_cast(&this->altitude), 8); - return 12; + pack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 12); + return 20; } int MavLinkGpsGlobalOrigin::unpack(const char* buffer) { unpack_int32_t(buffer, reinterpret_cast(&this->latitude), 0); unpack_int32_t(buffer, reinterpret_cast(&this->longitude), 4); unpack_int32_t(buffer, reinterpret_cast(&this->altitude), 8); - return 12; + unpack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 12); + return 20; } std::string MavLinkGpsGlobalOrigin::toJSon() { @@ -1215,6 +1319,7 @@ std::string MavLinkGpsGlobalOrigin::toJSon() { ss << "\"latitude\":" << this->latitude; ss << ", \"longitude\":" << this->longitude; ss << ", \"altitude\":" << this->altitude; + ss << ", \"time_usec\":" << this->time_usec; ss << "} },"; return ss.str(); } @@ -1265,14 +1370,16 @@ int MavLinkMissionRequestInt::pack(char* buffer) const { pack_uint16_t(buffer, reinterpret_cast(&this->seq), 0); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 2); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 3); - return 4; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 4); + return 5; } int MavLinkMissionRequestInt::unpack(const char* buffer) { unpack_uint16_t(buffer, reinterpret_cast(&this->seq), 0); unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 2); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 3); - return 4; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 4); + return 5; } std::string MavLinkMissionRequestInt::toJSon() { @@ -1281,6 +1388,37 @@ std::string MavLinkMissionRequestInt::toJSon() { ss << "\"seq\":" << this->seq; ss << ", \"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); + ss << ", \"mission_type\":" << static_cast(this->mission_type); + ss << "} },"; + return ss.str(); +} + +int MavLinkMissionChanged::pack(char* buffer) const { + pack_int16_t(buffer, reinterpret_cast(&this->start_index), 0); + pack_int16_t(buffer, reinterpret_cast(&this->end_index), 2); + pack_uint8_t(buffer, reinterpret_cast(&this->origin_sysid), 4); + pack_uint8_t(buffer, reinterpret_cast(&this->origin_compid), 5); + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 6); + return 7; +} + +int MavLinkMissionChanged::unpack(const char* buffer) { + unpack_int16_t(buffer, reinterpret_cast(&this->start_index), 0); + unpack_int16_t(buffer, reinterpret_cast(&this->end_index), 2); + unpack_uint8_t(buffer, reinterpret_cast(&this->origin_sysid), 4); + unpack_uint8_t(buffer, reinterpret_cast(&this->origin_compid), 5); + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 6); + return 7; +} + +std::string MavLinkMissionChanged::toJSon() { + std::ostringstream ss; + ss << "{ \"name\": \"MISSION_CHANGED\", \"id\": 52, \"timestamp\":" << timestamp << ", \"msg\": {"; + ss << "\"start_index\":" << this->start_index; + ss << ", \"end_index\":" << this->end_index; + ss << ", \"origin_sysid\":" << static_cast(this->origin_sysid); + ss << ", \"origin_compid\":" << static_cast(this->origin_compid); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -1707,7 +1845,17 @@ int MavLinkRcChannelsOverride::pack(char* buffer) const { pack_uint16_t(buffer, reinterpret_cast(&this->chan8_raw), 14); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 16); pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 17); - return 18; + pack_uint16_t(buffer, reinterpret_cast(&this->chan9_raw), 18); + pack_uint16_t(buffer, reinterpret_cast(&this->chan10_raw), 20); + pack_uint16_t(buffer, reinterpret_cast(&this->chan11_raw), 22); + pack_uint16_t(buffer, reinterpret_cast(&this->chan12_raw), 24); + pack_uint16_t(buffer, reinterpret_cast(&this->chan13_raw), 26); + pack_uint16_t(buffer, reinterpret_cast(&this->chan14_raw), 28); + pack_uint16_t(buffer, reinterpret_cast(&this->chan15_raw), 30); + pack_uint16_t(buffer, reinterpret_cast(&this->chan16_raw), 32); + pack_uint16_t(buffer, reinterpret_cast(&this->chan17_raw), 34); + pack_uint16_t(buffer, reinterpret_cast(&this->chan18_raw), 36); + return 38; } int MavLinkRcChannelsOverride::unpack(const char* buffer) { @@ -1721,7 +1869,17 @@ int MavLinkRcChannelsOverride::unpack(const char* buffer) { unpack_uint16_t(buffer, reinterpret_cast(&this->chan8_raw), 14); unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 16); unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 17); - return 18; + unpack_uint16_t(buffer, reinterpret_cast(&this->chan9_raw), 18); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan10_raw), 20); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan11_raw), 22); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan12_raw), 24); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan13_raw), 26); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan14_raw), 28); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan15_raw), 30); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan16_raw), 32); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan17_raw), 34); + unpack_uint16_t(buffer, reinterpret_cast(&this->chan18_raw), 36); + return 38; } std::string MavLinkRcChannelsOverride::toJSon() { @@ -1737,6 +1895,16 @@ std::string MavLinkRcChannelsOverride::toJSon() { ss << ", \"chan8_raw\":" << this->chan8_raw; ss << ", \"target_system\":" << static_cast(this->target_system); ss << ", \"target_component\":" << static_cast(this->target_component); + ss << ", \"chan9_raw\":" << this->chan9_raw; + ss << ", \"chan10_raw\":" << this->chan10_raw; + ss << ", \"chan11_raw\":" << this->chan11_raw; + ss << ", \"chan12_raw\":" << this->chan12_raw; + ss << ", \"chan13_raw\":" << this->chan13_raw; + ss << ", \"chan14_raw\":" << this->chan14_raw; + ss << ", \"chan15_raw\":" << this->chan15_raw; + ss << ", \"chan16_raw\":" << this->chan16_raw; + ss << ", \"chan17_raw\":" << this->chan17_raw; + ss << ", \"chan18_raw\":" << this->chan18_raw; ss << "} },"; return ss.str(); } @@ -1756,7 +1924,8 @@ int MavLinkMissionItemInt::pack(char* buffer) const { pack_uint8_t(buffer, reinterpret_cast(&this->frame), 34); pack_uint8_t(buffer, reinterpret_cast(&this->current), 35); pack_uint8_t(buffer, reinterpret_cast(&this->autocontinue), 36); - return 37; + pack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 37); + return 38; } int MavLinkMissionItemInt::unpack(const char* buffer) { @@ -1774,7 +1943,8 @@ int MavLinkMissionItemInt::unpack(const char* buffer) { unpack_uint8_t(buffer, reinterpret_cast(&this->frame), 34); unpack_uint8_t(buffer, reinterpret_cast(&this->current), 35); unpack_uint8_t(buffer, reinterpret_cast(&this->autocontinue), 36); - return 37; + unpack_uint8_t(buffer, reinterpret_cast(&this->mission_type), 37); + return 38; } std::string MavLinkMissionItemInt::toJSon() { @@ -1794,6 +1964,7 @@ std::string MavLinkMissionItemInt::toJSon() { ss << ", \"frame\":" << static_cast(this->frame); ss << ", \"current\":" << static_cast(this->current); ss << ", \"autocontinue\":" << static_cast(this->autocontinue); + ss << ", \"mission_type\":" << static_cast(this->mission_type); ss << "} },"; return ss.str(); } @@ -1936,13 +2107,21 @@ std::string MavLinkCommandLong::toJSon() { int MavLinkCommandAck::pack(char* buffer) const { pack_uint16_t(buffer, reinterpret_cast(&this->command), 0); pack_uint8_t(buffer, reinterpret_cast(&this->result), 2); - return 3; + pack_uint8_t(buffer, reinterpret_cast(&this->progress), 3); + pack_int32_t(buffer, reinterpret_cast(&this->result_param2), 4); + pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 8); + pack_uint8_t(buffer, reinterpret_cast(&this->target_component), 9); + return 10; } int MavLinkCommandAck::unpack(const char* buffer) { unpack_uint16_t(buffer, reinterpret_cast(&this->command), 0); unpack_uint8_t(buffer, reinterpret_cast(&this->result), 2); - return 3; + unpack_uint8_t(buffer, reinterpret_cast(&this->progress), 3); + unpack_int32_t(buffer, reinterpret_cast(&this->result_param2), 4); + unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 8); + unpack_uint8_t(buffer, reinterpret_cast(&this->target_component), 9); + return 10; } std::string MavLinkCommandAck::toJSon() { @@ -1950,6 +2129,10 @@ std::string MavLinkCommandAck::toJSon() { ss << "{ \"name\": \"COMMAND_ACK\", \"id\": 77, \"timestamp\":" << timestamp << ", \"msg\": {"; ss << "\"command\":" << this->command; ss << ", \"result\":" << static_cast(this->result); + ss << ", \"progress\":" << static_cast(this->progress); + ss << ", \"result_param2\":" << this->result_param2; + ss << ", \"target_system\":" << static_cast(this->target_system); + ss << ", \"target_component\":" << static_cast(this->target_component); ss << "} },"; return ss.str(); } @@ -2548,7 +2731,9 @@ int MavLinkOpticalFlow::pack(char* buffer) const { pack_int16_t(buffer, reinterpret_cast(&this->flow_y), 22); pack_uint8_t(buffer, reinterpret_cast(&this->sensor_id), 24); pack_uint8_t(buffer, reinterpret_cast(&this->quality), 25); - return 26; + pack_float(buffer, reinterpret_cast(&this->flow_rate_x), 26); + pack_float(buffer, reinterpret_cast(&this->flow_rate_y), 30); + return 34; } int MavLinkOpticalFlow::unpack(const char* buffer) { @@ -2560,7 +2745,9 @@ int MavLinkOpticalFlow::unpack(const char* buffer) { unpack_int16_t(buffer, reinterpret_cast(&this->flow_y), 22); unpack_uint8_t(buffer, reinterpret_cast(&this->sensor_id), 24); unpack_uint8_t(buffer, reinterpret_cast(&this->quality), 25); - return 26; + unpack_float(buffer, reinterpret_cast(&this->flow_rate_x), 26); + unpack_float(buffer, reinterpret_cast(&this->flow_rate_y), 30); + return 34; } std::string MavLinkOpticalFlow::toJSon() { @@ -2574,6 +2761,8 @@ std::string MavLinkOpticalFlow::toJSon() { ss << ", \"flow_y\":" << this->flow_y; ss << ", \"sensor_id\":" << static_cast(this->sensor_id); ss << ", \"quality\":" << static_cast(this->quality); + ss << ", \"flow_rate_x\":" << float_tostring(this->flow_rate_x); + ss << ", \"flow_rate_y\":" << float_tostring(this->flow_rate_y); ss << "} },"; return ss.str(); } @@ -2586,7 +2775,9 @@ int MavLinkGlobalVisionPositionEstimate::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->roll), 20); pack_float(buffer, reinterpret_cast(&this->pitch), 24); pack_float(buffer, reinterpret_cast(&this->yaw), 28); - return 32; + pack_float_array(21, buffer, reinterpret_cast(&this->covariance[0]), 32); + pack_uint8_t(buffer, reinterpret_cast(&this->reset_counter), 116); + return 117; } int MavLinkGlobalVisionPositionEstimate::unpack(const char* buffer) { @@ -2597,7 +2788,9 @@ int MavLinkGlobalVisionPositionEstimate::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->roll), 20); unpack_float(buffer, reinterpret_cast(&this->pitch), 24); unpack_float(buffer, reinterpret_cast(&this->yaw), 28); - return 32; + unpack_float_array(21, buffer, reinterpret_cast(&this->covariance[0]), 32); + unpack_uint8_t(buffer, reinterpret_cast(&this->reset_counter), 116); + return 117; } std::string MavLinkGlobalVisionPositionEstimate::toJSon() { @@ -2610,6 +2803,8 @@ std::string MavLinkGlobalVisionPositionEstimate::toJSon() { ss << ", \"roll\":" << float_tostring(this->roll); ss << ", \"pitch\":" << float_tostring(this->pitch); ss << ", \"yaw\":" << float_tostring(this->yaw); + ss << ", \"covariance\":" << "[" << float_array_tostring(21, reinterpret_cast(&this->covariance[0])) << "]"; + ss << ", \"reset_counter\":" << static_cast(this->reset_counter); ss << "} },"; return ss.str(); } @@ -2622,7 +2817,9 @@ int MavLinkVisionPositionEstimate::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->roll), 20); pack_float(buffer, reinterpret_cast(&this->pitch), 24); pack_float(buffer, reinterpret_cast(&this->yaw), 28); - return 32; + pack_float_array(21, buffer, reinterpret_cast(&this->covariance[0]), 32); + pack_uint8_t(buffer, reinterpret_cast(&this->reset_counter), 116); + return 117; } int MavLinkVisionPositionEstimate::unpack(const char* buffer) { @@ -2633,7 +2830,9 @@ int MavLinkVisionPositionEstimate::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->roll), 20); unpack_float(buffer, reinterpret_cast(&this->pitch), 24); unpack_float(buffer, reinterpret_cast(&this->yaw), 28); - return 32; + unpack_float_array(21, buffer, reinterpret_cast(&this->covariance[0]), 32); + unpack_uint8_t(buffer, reinterpret_cast(&this->reset_counter), 116); + return 117; } std::string MavLinkVisionPositionEstimate::toJSon() { @@ -2646,6 +2845,8 @@ std::string MavLinkVisionPositionEstimate::toJSon() { ss << ", \"roll\":" << float_tostring(this->roll); ss << ", \"pitch\":" << float_tostring(this->pitch); ss << ", \"yaw\":" << float_tostring(this->yaw); + ss << ", \"covariance\":" << "[" << float_array_tostring(21, reinterpret_cast(&this->covariance[0])) << "]"; + ss << ", \"reset_counter\":" << static_cast(this->reset_counter); ss << "} },"; return ss.str(); } @@ -2655,7 +2856,9 @@ int MavLinkVisionSpeedEstimate::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->x), 8); pack_float(buffer, reinterpret_cast(&this->y), 12); pack_float(buffer, reinterpret_cast(&this->z), 16); - return 20; + pack_float_array(9, buffer, reinterpret_cast(&this->covariance[0]), 20); + pack_uint8_t(buffer, reinterpret_cast(&this->reset_counter), 56); + return 57; } int MavLinkVisionSpeedEstimate::unpack(const char* buffer) { @@ -2663,7 +2866,9 @@ int MavLinkVisionSpeedEstimate::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->x), 8); unpack_float(buffer, reinterpret_cast(&this->y), 12); unpack_float(buffer, reinterpret_cast(&this->z), 16); - return 20; + unpack_float_array(9, buffer, reinterpret_cast(&this->covariance[0]), 20); + unpack_uint8_t(buffer, reinterpret_cast(&this->reset_counter), 56); + return 57; } std::string MavLinkVisionSpeedEstimate::toJSon() { @@ -2673,6 +2878,8 @@ std::string MavLinkVisionSpeedEstimate::toJSon() { ss << ", \"x\":" << float_tostring(this->x); ss << ", \"y\":" << float_tostring(this->y); ss << ", \"z\":" << float_tostring(this->z); + ss << ", \"covariance\":" << "[" << float_array_tostring(9, reinterpret_cast(&this->covariance[0])) << "]"; + ss << ", \"reset_counter\":" << static_cast(this->reset_counter); ss << "} },"; return ss.str(); } @@ -2685,7 +2892,8 @@ int MavLinkViconPositionEstimate::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->roll), 20); pack_float(buffer, reinterpret_cast(&this->pitch), 24); pack_float(buffer, reinterpret_cast(&this->yaw), 28); - return 32; + pack_float_array(21, buffer, reinterpret_cast(&this->covariance[0]), 32); + return 116; } int MavLinkViconPositionEstimate::unpack(const char* buffer) { @@ -2696,7 +2904,8 @@ int MavLinkViconPositionEstimate::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->roll), 20); unpack_float(buffer, reinterpret_cast(&this->pitch), 24); unpack_float(buffer, reinterpret_cast(&this->yaw), 28); - return 32; + unpack_float_array(21, buffer, reinterpret_cast(&this->covariance[0]), 32); + return 116; } std::string MavLinkViconPositionEstimate::toJSon() { @@ -2709,6 +2918,7 @@ std::string MavLinkViconPositionEstimate::toJSon() { ss << ", \"roll\":" << float_tostring(this->roll); ss << ", \"pitch\":" << float_tostring(this->pitch); ss << ", \"yaw\":" << float_tostring(this->yaw); + ss << ", \"covariance\":" << "[" << float_array_tostring(21, reinterpret_cast(&this->covariance[0])) << "]"; ss << "} },"; return ss.str(); } @@ -2729,7 +2939,8 @@ int MavLinkHighresImu::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->pressure_alt), 52); pack_float(buffer, reinterpret_cast(&this->temperature), 56); pack_uint16_t(buffer, reinterpret_cast(&this->fields_updated), 60); - return 62; + pack_uint8_t(buffer, reinterpret_cast(&this->id), 62); + return 63; } int MavLinkHighresImu::unpack(const char* buffer) { @@ -2748,7 +2959,8 @@ int MavLinkHighresImu::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->pressure_alt), 52); unpack_float(buffer, reinterpret_cast(&this->temperature), 56); unpack_uint16_t(buffer, reinterpret_cast(&this->fields_updated), 60); - return 62; + unpack_uint8_t(buffer, reinterpret_cast(&this->id), 62); + return 63; } std::string MavLinkHighresImu::toJSon() { @@ -2769,6 +2981,7 @@ std::string MavLinkHighresImu::toJSon() { ss << ", \"pressure_alt\":" << float_tostring(this->pressure_alt); ss << ", \"temperature\":" << float_tostring(this->temperature); ss << ", \"fields_updated\":" << this->fields_updated; + ss << ", \"id\":" << static_cast(this->id); ss << "} },"; return ss.str(); } @@ -3246,7 +3459,8 @@ int MavLinkScaledImu2::pack(char* buffer) const { pack_int16_t(buffer, reinterpret_cast(&this->xmag), 16); pack_int16_t(buffer, reinterpret_cast(&this->ymag), 18); pack_int16_t(buffer, reinterpret_cast(&this->zmag), 20); - return 22; + pack_int16_t(buffer, reinterpret_cast(&this->temperature), 22); + return 24; } int MavLinkScaledImu2::unpack(const char* buffer) { @@ -3260,7 +3474,8 @@ int MavLinkScaledImu2::unpack(const char* buffer) { unpack_int16_t(buffer, reinterpret_cast(&this->xmag), 16); unpack_int16_t(buffer, reinterpret_cast(&this->ymag), 18); unpack_int16_t(buffer, reinterpret_cast(&this->zmag), 20); - return 22; + unpack_int16_t(buffer, reinterpret_cast(&this->temperature), 22); + return 24; } std::string MavLinkScaledImu2::toJSon() { @@ -3276,6 +3491,7 @@ std::string MavLinkScaledImu2::toJSon() { ss << ", \"xmag\":" << this->xmag; ss << ", \"ymag\":" << this->ymag; ss << ", \"zmag\":" << this->zmag; + ss << ", \"temperature\":" << this->temperature; ss << "} },"; return ss.str(); } @@ -3690,7 +3906,8 @@ int MavLinkScaledImu3::pack(char* buffer) const { pack_int16_t(buffer, reinterpret_cast(&this->xmag), 16); pack_int16_t(buffer, reinterpret_cast(&this->ymag), 18); pack_int16_t(buffer, reinterpret_cast(&this->zmag), 20); - return 22; + pack_int16_t(buffer, reinterpret_cast(&this->temperature), 22); + return 24; } int MavLinkScaledImu3::unpack(const char* buffer) { @@ -3704,7 +3921,8 @@ int MavLinkScaledImu3::unpack(const char* buffer) { unpack_int16_t(buffer, reinterpret_cast(&this->xmag), 16); unpack_int16_t(buffer, reinterpret_cast(&this->ymag), 18); unpack_int16_t(buffer, reinterpret_cast(&this->zmag), 20); - return 22; + unpack_int16_t(buffer, reinterpret_cast(&this->temperature), 22); + return 24; } std::string MavLinkScaledImu3::toJSon() { @@ -3720,6 +3938,7 @@ std::string MavLinkScaledImu3::toJSon() { ss << ", \"xmag\":" << this->xmag; ss << ", \"ymag\":" << this->ymag; ss << ", \"zmag\":" << this->zmag; + ss << ", \"temperature\":" << this->temperature; ss << "} },"; return ss.str(); } @@ -3790,7 +4009,10 @@ int MavLinkDistanceSensor::pack(char* buffer) const { pack_uint8_t(buffer, reinterpret_cast(&this->id), 11); pack_uint8_t(buffer, reinterpret_cast(&this->orientation), 12); pack_uint8_t(buffer, reinterpret_cast(&this->covariance), 13); - return 14; + pack_float(buffer, reinterpret_cast(&this->horizontal_fov), 14); + pack_float(buffer, reinterpret_cast(&this->vertical_fov), 18); + pack_float_array(4, buffer, reinterpret_cast(&this->quaternion[0]), 22); + return 38; } int MavLinkDistanceSensor::unpack(const char* buffer) { @@ -3802,7 +4024,10 @@ int MavLinkDistanceSensor::unpack(const char* buffer) { unpack_uint8_t(buffer, reinterpret_cast(&this->id), 11); unpack_uint8_t(buffer, reinterpret_cast(&this->orientation), 12); unpack_uint8_t(buffer, reinterpret_cast(&this->covariance), 13); - return 14; + unpack_float(buffer, reinterpret_cast(&this->horizontal_fov), 14); + unpack_float(buffer, reinterpret_cast(&this->vertical_fov), 18); + unpack_float_array(4, buffer, reinterpret_cast(&this->quaternion[0]), 22); + return 38; } std::string MavLinkDistanceSensor::toJSon() { @@ -3816,6 +4041,9 @@ std::string MavLinkDistanceSensor::toJSon() { ss << ", \"id\":" << static_cast(this->id); ss << ", \"orientation\":" << static_cast(this->orientation); ss << ", \"covariance\":" << static_cast(this->covariance); + ss << ", \"horizontal_fov\":" << float_tostring(this->horizontal_fov); + ss << ", \"vertical_fov\":" << float_tostring(this->vertical_fov); + ss << ", \"quaternion\":" << "[" << float_array_tostring(4, reinterpret_cast(&this->quaternion[0])) << "]"; ss << "} },"; return ss.str(); } @@ -3967,7 +4195,8 @@ int MavLinkAttPosMocap::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->x), 24); pack_float(buffer, reinterpret_cast(&this->y), 28); pack_float(buffer, reinterpret_cast(&this->z), 32); - return 36; + pack_float_array(21, buffer, reinterpret_cast(&this->covariance[0]), 36); + return 120; } int MavLinkAttPosMocap::unpack(const char* buffer) { @@ -3976,7 +4205,8 @@ int MavLinkAttPosMocap::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->x), 24); unpack_float(buffer, reinterpret_cast(&this->y), 28); unpack_float(buffer, reinterpret_cast(&this->z), 32); - return 36; + unpack_float_array(21, buffer, reinterpret_cast(&this->covariance[0]), 36); + return 120; } std::string MavLinkAttPosMocap::toJSon() { @@ -3987,6 +4217,7 @@ std::string MavLinkAttPosMocap::toJSon() { ss << ", \"x\":" << float_tostring(this->x); ss << ", \"y\":" << float_tostring(this->y); ss << ", \"z\":" << float_tostring(this->z); + ss << ", \"covariance\":" << "[" << float_array_tostring(21, reinterpret_cast(&this->covariance[0])) << "]"; ss << "} },"; return ss.str(); } @@ -4262,7 +4493,9 @@ int MavLinkBatteryStatus::pack(char* buffer) const { pack_uint8_t(buffer, reinterpret_cast(&this->battery_function), 33); pack_uint8_t(buffer, reinterpret_cast(&this->type), 34); pack_int8_t(buffer, reinterpret_cast(&this->battery_remaining), 35); - return 36; + pack_int32_t(buffer, reinterpret_cast(&this->time_remaining), 36); + pack_uint8_t(buffer, reinterpret_cast(&this->charge_state), 40); + return 41; } int MavLinkBatteryStatus::unpack(const char* buffer) { @@ -4275,7 +4508,9 @@ int MavLinkBatteryStatus::unpack(const char* buffer) { unpack_uint8_t(buffer, reinterpret_cast(&this->battery_function), 33); unpack_uint8_t(buffer, reinterpret_cast(&this->type), 34); unpack_int8_t(buffer, reinterpret_cast(&this->battery_remaining), 35); - return 36; + unpack_int32_t(buffer, reinterpret_cast(&this->time_remaining), 36); + unpack_uint8_t(buffer, reinterpret_cast(&this->charge_state), 40); + return 41; } std::string MavLinkBatteryStatus::toJSon() { @@ -4290,6 +4525,8 @@ std::string MavLinkBatteryStatus::toJSon() { ss << ", \"battery_function\":" << static_cast(this->battery_function); ss << ", \"type\":" << static_cast(this->type); ss << ", \"battery_remaining\":" << static_cast(this->battery_remaining); + ss << ", \"time_remaining\":" << this->time_remaining; + ss << ", \"charge_state\":" << static_cast(this->charge_state); ss << "} },"; return ss.str(); } @@ -4306,7 +4543,8 @@ int MavLinkAutopilotVersion::pack(char* buffer) const { pack_uint8_t_array(8, buffer, reinterpret_cast(&this->flight_custom_version[0]), 36); pack_uint8_t_array(8, buffer, reinterpret_cast(&this->middleware_custom_version[0]), 44); pack_uint8_t_array(8, buffer, reinterpret_cast(&this->os_custom_version[0]), 52); - return 60; + pack_uint8_t_array(18, buffer, reinterpret_cast(&this->uid2[0]), 60); + return 78; } int MavLinkAutopilotVersion::unpack(const char* buffer) { @@ -4321,7 +4559,8 @@ int MavLinkAutopilotVersion::unpack(const char* buffer) { unpack_uint8_t_array(8, buffer, reinterpret_cast(&this->flight_custom_version[0]), 36); unpack_uint8_t_array(8, buffer, reinterpret_cast(&this->middleware_custom_version[0]), 44); unpack_uint8_t_array(8, buffer, reinterpret_cast(&this->os_custom_version[0]), 52); - return 60; + unpack_uint8_t_array(18, buffer, reinterpret_cast(&this->uid2[0]), 60); + return 78; } std::string MavLinkAutopilotVersion::toJSon() { @@ -4338,6 +4577,7 @@ std::string MavLinkAutopilotVersion::toJSon() { ss << ", \"flight_custom_version\":" << "[" << uint8_t_array_tostring(8, reinterpret_cast(&this->flight_custom_version[0])) << "]"; ss << ", \"middleware_custom_version\":" << "[" << uint8_t_array_tostring(8, reinterpret_cast(&this->middleware_custom_version[0])) << "]"; ss << ", \"os_custom_version\":" << "[" << uint8_t_array_tostring(8, reinterpret_cast(&this->os_custom_version[0])) << "]"; + ss << ", \"uid2\":" << "[" << uint8_t_array_tostring(18, reinterpret_cast(&this->uid2[0])) << "]"; ss << "} },"; return ss.str(); } @@ -4351,7 +4591,13 @@ int MavLinkLandingTarget::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->size_y), 24); pack_uint8_t(buffer, reinterpret_cast(&this->target_num), 28); pack_uint8_t(buffer, reinterpret_cast(&this->frame), 29); - return 30; + pack_float(buffer, reinterpret_cast(&this->x), 30); + pack_float(buffer, reinterpret_cast(&this->y), 34); + pack_float(buffer, reinterpret_cast(&this->z), 38); + pack_float_array(4, buffer, reinterpret_cast(&this->q[0]), 42); + pack_uint8_t(buffer, reinterpret_cast(&this->type), 58); + pack_uint8_t(buffer, reinterpret_cast(&this->position_valid), 59); + return 60; } int MavLinkLandingTarget::unpack(const char* buffer) { @@ -4363,7 +4609,13 @@ int MavLinkLandingTarget::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->size_y), 24); unpack_uint8_t(buffer, reinterpret_cast(&this->target_num), 28); unpack_uint8_t(buffer, reinterpret_cast(&this->frame), 29); - return 30; + unpack_float(buffer, reinterpret_cast(&this->x), 30); + unpack_float(buffer, reinterpret_cast(&this->y), 34); + unpack_float(buffer, reinterpret_cast(&this->z), 38); + unpack_float_array(4, buffer, reinterpret_cast(&this->q[0]), 42); + unpack_uint8_t(buffer, reinterpret_cast(&this->type), 58); + unpack_uint8_t(buffer, reinterpret_cast(&this->position_valid), 59); + return 60; } std::string MavLinkLandingTarget::toJSon() { @@ -4377,6 +4629,42 @@ std::string MavLinkLandingTarget::toJSon() { ss << ", \"size_y\":" << float_tostring(this->size_y); ss << ", \"target_num\":" << static_cast(this->target_num); ss << ", \"frame\":" << static_cast(this->frame); + ss << ", \"x\":" << float_tostring(this->x); + ss << ", \"y\":" << float_tostring(this->y); + ss << ", \"z\":" << float_tostring(this->z); + ss << ", \"q\":" << "[" << float_array_tostring(4, reinterpret_cast(&this->q[0])) << "]"; + ss << ", \"type\":" << static_cast(this->type); + ss << ", \"position_valid\":" << static_cast(this->position_valid); + ss << "} },"; + return ss.str(); +} + +int MavLinkFenceStatus::pack(char* buffer) const { + pack_uint32_t(buffer, reinterpret_cast(&this->breach_time), 0); + pack_uint16_t(buffer, reinterpret_cast(&this->breach_count), 4); + pack_uint8_t(buffer, reinterpret_cast(&this->breach_status), 6); + pack_uint8_t(buffer, reinterpret_cast(&this->breach_type), 7); + pack_uint8_t(buffer, reinterpret_cast(&this->breach_mitigation), 8); + return 9; +} + +int MavLinkFenceStatus::unpack(const char* buffer) { + unpack_uint32_t(buffer, reinterpret_cast(&this->breach_time), 0); + unpack_uint16_t(buffer, reinterpret_cast(&this->breach_count), 4); + unpack_uint8_t(buffer, reinterpret_cast(&this->breach_status), 6); + unpack_uint8_t(buffer, reinterpret_cast(&this->breach_type), 7); + unpack_uint8_t(buffer, reinterpret_cast(&this->breach_mitigation), 8); + return 9; +} + +std::string MavLinkFenceStatus::toJSon() { + std::ostringstream ss; + ss << "{ \"name\": \"FENCE_STATUS\", \"id\": 162, \"timestamp\":" << timestamp << ", \"msg\": {"; + ss << "\"breach_time\":" << this->breach_time; + ss << ", \"breach_count\":" << this->breach_count; + ss << ", \"breach_status\":" << static_cast(this->breach_status); + ss << ", \"breach_type\":" << static_cast(this->breach_type); + ss << ", \"breach_mitigation\":" << static_cast(this->breach_mitigation); ss << "} },"; return ss.str(); } @@ -4487,7 +4775,8 @@ int MavLinkGpsInput::pack(char* buffer) const { pack_uint8_t(buffer, reinterpret_cast(&this->gps_id), 60); pack_uint8_t(buffer, reinterpret_cast(&this->fix_type), 61); pack_uint8_t(buffer, reinterpret_cast(&this->satellites_visible), 62); - return 63; + pack_uint16_t(buffer, reinterpret_cast(&this->yaw), 63); + return 65; } int MavLinkGpsInput::unpack(const char* buffer) { @@ -4509,7 +4798,8 @@ int MavLinkGpsInput::unpack(const char* buffer) { unpack_uint8_t(buffer, reinterpret_cast(&this->gps_id), 60); unpack_uint8_t(buffer, reinterpret_cast(&this->fix_type), 61); unpack_uint8_t(buffer, reinterpret_cast(&this->satellites_visible), 62); - return 63; + unpack_uint16_t(buffer, reinterpret_cast(&this->yaw), 63); + return 65; } std::string MavLinkGpsInput::toJSon() { @@ -4533,6 +4823,7 @@ std::string MavLinkGpsInput::toJSon() { ss << ", \"gps_id\":" << static_cast(this->gps_id); ss << ", \"fix_type\":" << static_cast(this->fix_type); ss << ", \"satellites_visible\":" << static_cast(this->satellites_visible); + ss << ", \"yaw\":" << this->yaw; ss << "} },"; return ss.str(); } @@ -4562,83 +4853,71 @@ std::string MavLinkGpsRtcmData::toJSon() { } int MavLinkHighLatency::pack(char* buffer) const { - pack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 0); - pack_uint32_t(buffer, reinterpret_cast(&this->custom_mode), 8); - pack_int32_t(buffer, reinterpret_cast(&this->latitude), 12); - pack_int32_t(buffer, reinterpret_cast(&this->longitude), 16); - pack_int16_t(buffer, reinterpret_cast(&this->roll), 20); - pack_int16_t(buffer, reinterpret_cast(&this->pitch), 22); - pack_uint16_t(buffer, reinterpret_cast(&this->heading), 24); - pack_int16_t(buffer, reinterpret_cast(&this->roll_sp), 26); - pack_int16_t(buffer, reinterpret_cast(&this->pitch_sp), 28); - pack_int16_t(buffer, reinterpret_cast(&this->heading_sp), 30); - pack_int16_t(buffer, reinterpret_cast(&this->altitude_home), 32); - pack_int16_t(buffer, reinterpret_cast(&this->altitude_amsl), 34); - pack_int16_t(buffer, reinterpret_cast(&this->altitude_sp), 36); - pack_uint16_t(buffer, reinterpret_cast(&this->wp_distance), 38); - pack_uint8_t(buffer, reinterpret_cast(&this->base_mode), 40); - pack_uint8_t(buffer, reinterpret_cast(&this->landed_state), 41); - pack_int8_t(buffer, reinterpret_cast(&this->throttle), 42); - pack_uint8_t(buffer, reinterpret_cast(&this->airspeed), 43); - pack_uint8_t(buffer, reinterpret_cast(&this->airspeed_sp), 44); - pack_uint8_t(buffer, reinterpret_cast(&this->groundspeed), 45); - pack_int8_t(buffer, reinterpret_cast(&this->climb_rate), 46); - pack_uint8_t(buffer, reinterpret_cast(&this->gps_nsat), 47); - pack_uint8_t(buffer, reinterpret_cast(&this->gps_fix_type), 48); - pack_uint8_t(buffer, reinterpret_cast(&this->battery_remaining), 49); - pack_int8_t(buffer, reinterpret_cast(&this->temperature), 50); - pack_int8_t(buffer, reinterpret_cast(&this->temperature_air), 51); - pack_uint8_t(buffer, reinterpret_cast(&this->failsafe), 52); - pack_uint8_t(buffer, reinterpret_cast(&this->wp_num), 53); - return 54; + pack_uint32_t(buffer, reinterpret_cast(&this->custom_mode), 0); + pack_int32_t(buffer, reinterpret_cast(&this->latitude), 4); + pack_int32_t(buffer, reinterpret_cast(&this->longitude), 8); + pack_int16_t(buffer, reinterpret_cast(&this->roll), 12); + pack_int16_t(buffer, reinterpret_cast(&this->pitch), 14); + pack_uint16_t(buffer, reinterpret_cast(&this->heading), 16); + pack_int16_t(buffer, reinterpret_cast(&this->heading_sp), 18); + pack_int16_t(buffer, reinterpret_cast(&this->altitude_amsl), 20); + pack_int16_t(buffer, reinterpret_cast(&this->altitude_sp), 22); + pack_uint16_t(buffer, reinterpret_cast(&this->wp_distance), 24); + pack_uint8_t(buffer, reinterpret_cast(&this->base_mode), 26); + pack_uint8_t(buffer, reinterpret_cast(&this->landed_state), 27); + pack_int8_t(buffer, reinterpret_cast(&this->throttle), 28); + pack_uint8_t(buffer, reinterpret_cast(&this->airspeed), 29); + pack_uint8_t(buffer, reinterpret_cast(&this->airspeed_sp), 30); + pack_uint8_t(buffer, reinterpret_cast(&this->groundspeed), 31); + pack_int8_t(buffer, reinterpret_cast(&this->climb_rate), 32); + pack_uint8_t(buffer, reinterpret_cast(&this->gps_nsat), 33); + pack_uint8_t(buffer, reinterpret_cast(&this->gps_fix_type), 34); + pack_uint8_t(buffer, reinterpret_cast(&this->battery_remaining), 35); + pack_int8_t(buffer, reinterpret_cast(&this->temperature), 36); + pack_int8_t(buffer, reinterpret_cast(&this->temperature_air), 37); + pack_uint8_t(buffer, reinterpret_cast(&this->failsafe), 38); + pack_uint8_t(buffer, reinterpret_cast(&this->wp_num), 39); + return 40; } int MavLinkHighLatency::unpack(const char* buffer) { - unpack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 0); - unpack_uint32_t(buffer, reinterpret_cast(&this->custom_mode), 8); - unpack_int32_t(buffer, reinterpret_cast(&this->latitude), 12); - unpack_int32_t(buffer, reinterpret_cast(&this->longitude), 16); - unpack_int16_t(buffer, reinterpret_cast(&this->roll), 20); - unpack_int16_t(buffer, reinterpret_cast(&this->pitch), 22); - unpack_uint16_t(buffer, reinterpret_cast(&this->heading), 24); - unpack_int16_t(buffer, reinterpret_cast(&this->roll_sp), 26); - unpack_int16_t(buffer, reinterpret_cast(&this->pitch_sp), 28); - unpack_int16_t(buffer, reinterpret_cast(&this->heading_sp), 30); - unpack_int16_t(buffer, reinterpret_cast(&this->altitude_home), 32); - unpack_int16_t(buffer, reinterpret_cast(&this->altitude_amsl), 34); - unpack_int16_t(buffer, reinterpret_cast(&this->altitude_sp), 36); - unpack_uint16_t(buffer, reinterpret_cast(&this->wp_distance), 38); - unpack_uint8_t(buffer, reinterpret_cast(&this->base_mode), 40); - unpack_uint8_t(buffer, reinterpret_cast(&this->landed_state), 41); - unpack_int8_t(buffer, reinterpret_cast(&this->throttle), 42); - unpack_uint8_t(buffer, reinterpret_cast(&this->airspeed), 43); - unpack_uint8_t(buffer, reinterpret_cast(&this->airspeed_sp), 44); - unpack_uint8_t(buffer, reinterpret_cast(&this->groundspeed), 45); - unpack_int8_t(buffer, reinterpret_cast(&this->climb_rate), 46); - unpack_uint8_t(buffer, reinterpret_cast(&this->gps_nsat), 47); - unpack_uint8_t(buffer, reinterpret_cast(&this->gps_fix_type), 48); - unpack_uint8_t(buffer, reinterpret_cast(&this->battery_remaining), 49); - unpack_int8_t(buffer, reinterpret_cast(&this->temperature), 50); - unpack_int8_t(buffer, reinterpret_cast(&this->temperature_air), 51); - unpack_uint8_t(buffer, reinterpret_cast(&this->failsafe), 52); - unpack_uint8_t(buffer, reinterpret_cast(&this->wp_num), 53); - return 54; + unpack_uint32_t(buffer, reinterpret_cast(&this->custom_mode), 0); + unpack_int32_t(buffer, reinterpret_cast(&this->latitude), 4); + unpack_int32_t(buffer, reinterpret_cast(&this->longitude), 8); + unpack_int16_t(buffer, reinterpret_cast(&this->roll), 12); + unpack_int16_t(buffer, reinterpret_cast(&this->pitch), 14); + unpack_uint16_t(buffer, reinterpret_cast(&this->heading), 16); + unpack_int16_t(buffer, reinterpret_cast(&this->heading_sp), 18); + unpack_int16_t(buffer, reinterpret_cast(&this->altitude_amsl), 20); + unpack_int16_t(buffer, reinterpret_cast(&this->altitude_sp), 22); + unpack_uint16_t(buffer, reinterpret_cast(&this->wp_distance), 24); + unpack_uint8_t(buffer, reinterpret_cast(&this->base_mode), 26); + unpack_uint8_t(buffer, reinterpret_cast(&this->landed_state), 27); + unpack_int8_t(buffer, reinterpret_cast(&this->throttle), 28); + unpack_uint8_t(buffer, reinterpret_cast(&this->airspeed), 29); + unpack_uint8_t(buffer, reinterpret_cast(&this->airspeed_sp), 30); + unpack_uint8_t(buffer, reinterpret_cast(&this->groundspeed), 31); + unpack_int8_t(buffer, reinterpret_cast(&this->climb_rate), 32); + unpack_uint8_t(buffer, reinterpret_cast(&this->gps_nsat), 33); + unpack_uint8_t(buffer, reinterpret_cast(&this->gps_fix_type), 34); + unpack_uint8_t(buffer, reinterpret_cast(&this->battery_remaining), 35); + unpack_int8_t(buffer, reinterpret_cast(&this->temperature), 36); + unpack_int8_t(buffer, reinterpret_cast(&this->temperature_air), 37); + unpack_uint8_t(buffer, reinterpret_cast(&this->failsafe), 38); + unpack_uint8_t(buffer, reinterpret_cast(&this->wp_num), 39); + return 40; } std::string MavLinkHighLatency::toJSon() { std::ostringstream ss; ss << "{ \"name\": \"HIGH_LATENCY\", \"id\": 234, \"timestamp\":" << timestamp << ", \"msg\": {"; - ss << "\"time_usec\":" << this->time_usec; - ss << ", \"custom_mode\":" << this->custom_mode; + ss << "\"custom_mode\":" << this->custom_mode; ss << ", \"latitude\":" << this->latitude; ss << ", \"longitude\":" << this->longitude; ss << ", \"roll\":" << this->roll; ss << ", \"pitch\":" << this->pitch; ss << ", \"heading\":" << this->heading; - ss << ", \"roll_sp\":" << this->roll_sp; - ss << ", \"pitch_sp\":" << this->pitch_sp; ss << ", \"heading_sp\":" << this->heading_sp; - ss << ", \"altitude_home\":" << this->altitude_home; ss << ", \"altitude_amsl\":" << this->altitude_amsl; ss << ", \"altitude_sp\":" << this->altitude_sp; ss << ", \"wp_distance\":" << this->wp_distance; @@ -4660,6 +4939,102 @@ std::string MavLinkHighLatency::toJSon() { return ss.str(); } +int MavLinkHighLatency2::pack(char* buffer) const { + pack_uint32_t(buffer, reinterpret_cast(&this->timestamp), 0); + pack_int32_t(buffer, reinterpret_cast(&this->latitude), 4); + pack_int32_t(buffer, reinterpret_cast(&this->longitude), 8); + pack_uint16_t(buffer, reinterpret_cast(&this->custom_mode), 12); + pack_int16_t(buffer, reinterpret_cast(&this->altitude), 14); + pack_int16_t(buffer, reinterpret_cast(&this->target_altitude), 16); + pack_uint16_t(buffer, reinterpret_cast(&this->target_distance), 18); + pack_uint16_t(buffer, reinterpret_cast(&this->wp_num), 20); + pack_uint16_t(buffer, reinterpret_cast(&this->failure_flags), 22); + pack_uint8_t(buffer, reinterpret_cast(&this->type), 24); + pack_uint8_t(buffer, reinterpret_cast(&this->autopilot), 25); + pack_uint8_t(buffer, reinterpret_cast(&this->heading), 26); + pack_uint8_t(buffer, reinterpret_cast(&this->target_heading), 27); + pack_uint8_t(buffer, reinterpret_cast(&this->throttle), 28); + pack_uint8_t(buffer, reinterpret_cast(&this->airspeed), 29); + pack_uint8_t(buffer, reinterpret_cast(&this->airspeed_sp), 30); + pack_uint8_t(buffer, reinterpret_cast(&this->groundspeed), 31); + pack_uint8_t(buffer, reinterpret_cast(&this->windspeed), 32); + pack_uint8_t(buffer, reinterpret_cast(&this->wind_heading), 33); + pack_uint8_t(buffer, reinterpret_cast(&this->eph), 34); + pack_uint8_t(buffer, reinterpret_cast(&this->epv), 35); + pack_int8_t(buffer, reinterpret_cast(&this->temperature_air), 36); + pack_int8_t(buffer, reinterpret_cast(&this->climb_rate), 37); + pack_int8_t(buffer, reinterpret_cast(&this->battery), 38); + pack_int8_t(buffer, reinterpret_cast(&this->custom0), 39); + pack_int8_t(buffer, reinterpret_cast(&this->custom1), 40); + pack_int8_t(buffer, reinterpret_cast(&this->custom2), 41); + return 42; +} + +int MavLinkHighLatency2::unpack(const char* buffer) { + unpack_uint32_t(buffer, reinterpret_cast(&this->timestamp), 0); + unpack_int32_t(buffer, reinterpret_cast(&this->latitude), 4); + unpack_int32_t(buffer, reinterpret_cast(&this->longitude), 8); + unpack_uint16_t(buffer, reinterpret_cast(&this->custom_mode), 12); + unpack_int16_t(buffer, reinterpret_cast(&this->altitude), 14); + unpack_int16_t(buffer, reinterpret_cast(&this->target_altitude), 16); + unpack_uint16_t(buffer, reinterpret_cast(&this->target_distance), 18); + unpack_uint16_t(buffer, reinterpret_cast(&this->wp_num), 20); + unpack_uint16_t(buffer, reinterpret_cast(&this->failure_flags), 22); + unpack_uint8_t(buffer, reinterpret_cast(&this->type), 24); + unpack_uint8_t(buffer, reinterpret_cast(&this->autopilot), 25); + unpack_uint8_t(buffer, reinterpret_cast(&this->heading), 26); + unpack_uint8_t(buffer, reinterpret_cast(&this->target_heading), 27); + unpack_uint8_t(buffer, reinterpret_cast(&this->throttle), 28); + unpack_uint8_t(buffer, reinterpret_cast(&this->airspeed), 29); + unpack_uint8_t(buffer, reinterpret_cast(&this->airspeed_sp), 30); + unpack_uint8_t(buffer, reinterpret_cast(&this->groundspeed), 31); + unpack_uint8_t(buffer, reinterpret_cast(&this->windspeed), 32); + unpack_uint8_t(buffer, reinterpret_cast(&this->wind_heading), 33); + unpack_uint8_t(buffer, reinterpret_cast(&this->eph), 34); + unpack_uint8_t(buffer, reinterpret_cast(&this->epv), 35); + unpack_int8_t(buffer, reinterpret_cast(&this->temperature_air), 36); + unpack_int8_t(buffer, reinterpret_cast(&this->climb_rate), 37); + unpack_int8_t(buffer, reinterpret_cast(&this->battery), 38); + unpack_int8_t(buffer, reinterpret_cast(&this->custom0), 39); + unpack_int8_t(buffer, reinterpret_cast(&this->custom1), 40); + unpack_int8_t(buffer, reinterpret_cast(&this->custom2), 41); + return 42; +} + +std::string MavLinkHighLatency2::toJSon() { + std::ostringstream ss; + ss << "{ \"name\": \"HIGH_LATENCY2\", \"id\": 235, \"timestamp\":" << timestamp << ", \"msg\": {"; + ss << "\"timestamp\":" << this->timestamp; + ss << ", \"latitude\":" << this->latitude; + ss << ", \"longitude\":" << this->longitude; + ss << ", \"custom_mode\":" << this->custom_mode; + ss << ", \"altitude\":" << this->altitude; + ss << ", \"target_altitude\":" << this->target_altitude; + ss << ", \"target_distance\":" << this->target_distance; + ss << ", \"wp_num\":" << this->wp_num; + ss << ", \"failure_flags\":" << this->failure_flags; + ss << ", \"type\":" << static_cast(this->type); + ss << ", \"autopilot\":" << static_cast(this->autopilot); + ss << ", \"heading\":" << static_cast(this->heading); + ss << ", \"target_heading\":" << static_cast(this->target_heading); + ss << ", \"throttle\":" << static_cast(this->throttle); + ss << ", \"airspeed\":" << static_cast(this->airspeed); + ss << ", \"airspeed_sp\":" << static_cast(this->airspeed_sp); + ss << ", \"groundspeed\":" << static_cast(this->groundspeed); + ss << ", \"windspeed\":" << static_cast(this->windspeed); + ss << ", \"wind_heading\":" << static_cast(this->wind_heading); + ss << ", \"eph\":" << static_cast(this->eph); + ss << ", \"epv\":" << static_cast(this->epv); + ss << ", \"temperature_air\":" << static_cast(this->temperature_air); + ss << ", \"climb_rate\":" << static_cast(this->climb_rate); + ss << ", \"battery\":" << static_cast(this->battery); + ss << ", \"custom0\":" << static_cast(this->custom0); + ss << ", \"custom1\":" << static_cast(this->custom1); + ss << ", \"custom2\":" << static_cast(this->custom2); + ss << "} },"; + return ss.str(); +} + int MavLinkVibration::pack(char* buffer) const { pack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 0); pack_float(buffer, reinterpret_cast(&this->vibration_x), 8); @@ -4707,7 +5082,8 @@ int MavLinkHomePosition::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->approach_x), 40); pack_float(buffer, reinterpret_cast(&this->approach_y), 44); pack_float(buffer, reinterpret_cast(&this->approach_z), 48); - return 52; + pack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 52); + return 60; } int MavLinkHomePosition::unpack(const char* buffer) { @@ -4721,7 +5097,8 @@ int MavLinkHomePosition::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->approach_x), 40); unpack_float(buffer, reinterpret_cast(&this->approach_y), 44); unpack_float(buffer, reinterpret_cast(&this->approach_z), 48); - return 52; + unpack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 52); + return 60; } std::string MavLinkHomePosition::toJSon() { @@ -4737,6 +5114,7 @@ std::string MavLinkHomePosition::toJSon() { ss << ", \"approach_x\":" << float_tostring(this->approach_x); ss << ", \"approach_y\":" << float_tostring(this->approach_y); ss << ", \"approach_z\":" << float_tostring(this->approach_z); + ss << ", \"time_usec\":" << this->time_usec; ss << "} },"; return ss.str(); } @@ -4753,7 +5131,8 @@ int MavLinkSetHomePosition::pack(char* buffer) const { pack_float(buffer, reinterpret_cast(&this->approach_y), 44); pack_float(buffer, reinterpret_cast(&this->approach_z), 48); pack_uint8_t(buffer, reinterpret_cast(&this->target_system), 52); - return 53; + pack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 53); + return 61; } int MavLinkSetHomePosition::unpack(const char* buffer) { @@ -4768,7 +5147,8 @@ int MavLinkSetHomePosition::unpack(const char* buffer) { unpack_float(buffer, reinterpret_cast(&this->approach_y), 44); unpack_float(buffer, reinterpret_cast(&this->approach_z), 48); unpack_uint8_t(buffer, reinterpret_cast(&this->target_system), 52); - return 53; + unpack_uint64_t(buffer, reinterpret_cast(&this->time_usec), 53); + return 61; } std::string MavLinkSetHomePosition::toJSon() { @@ -4785,6 +5165,7 @@ std::string MavLinkSetHomePosition::toJSon() { ss << ", \"approach_y\":" << float_tostring(this->approach_y); ss << ", \"approach_z\":" << float_tostring(this->approach_z); ss << ", \"target_system\":" << static_cast(this->target_system); + ss << ", \"time_usec\":" << this->time_usec; ss << "} },"; return ss.str(); } @@ -5101,71 +5482,41 @@ std::string MavLinkDebug::toJSon() { return ss.str(); } -int MavLinkProtocolVersion::pack(char* buffer) const { - pack_uint16_t(buffer, reinterpret_cast(&this->version), 0); - pack_uint16_t(buffer, reinterpret_cast(&this->min_version), 2); - pack_uint16_t(buffer, reinterpret_cast(&this->max_version), 4); - pack_uint8_t_array(8, buffer, reinterpret_cast(&this->spec_version_hash[0]), 6); - pack_uint8_t_array(8, buffer, reinterpret_cast(&this->library_version_hash[0]), 14); - return 22; -} -int MavLinkProtocolVersion::unpack(const char* buffer) { - - unpack_uint16_t(buffer, reinterpret_cast(&this->version), 0); - unpack_uint16_t(buffer, reinterpret_cast(&this->min_version), 2); - unpack_uint16_t(buffer, reinterpret_cast(&this->max_version), 4); - unpack_uint8_t_array(8, buffer, reinterpret_cast(&this->spec_version_hash[0]), 6); - unpack_uint8_t_array(8, buffer, reinterpret_cast(&this->library_version_hash[0]), 14); - return 22; -} - -std::string MavLinkProtocolVersion::toJSon() { - std::ostringstream ss; - ss << "{ \"name\": \"DEBUG\", \"id\": 300, \"timestamp\":" << timestamp << ", \"msg\": {"; - ss << "\"version\":" << this->version; - ss << ", \"min_version\":" << this->min_version; - ss << ", \"max_version\":" << this->max_version; - ss << ", \"spec_version_hash\":" << "[" << uint8_t_array_tostring(8, reinterpret_cast(&this->spec_version_hash[0])) << "]"; - ss << ", \"library_version_hash\":" << "[" << uint8_t_array_tostring(8, reinterpret_cast(&this->library_version_hash[0])) << "]"; - ss << "} },"; - return ss.str(); -} - void MavCmdNavWaypoint::pack() { - param1 = HoldTimeDecimal; - param2 = AcceptanceRadiusMeters; - param3 = p0ToPass; - param4 = DesiredYawAngle; + param1 = Hold; + param2 = AcceptRadius; + param3 = PassRadius; + param4 = Yaw; param5 = Latitude; param6 = Longitude; param7 = Altitude; } void MavCmdNavWaypoint::unpack() { - HoldTimeDecimal = param1; - AcceptanceRadiusMeters = param2; - p0ToPass = param3; - DesiredYawAngle = param4; + Hold = param1; + AcceptRadius = param2; + PassRadius = param3; + Yaw = param4; Latitude = param5; Longitude = param6; Altitude = param7; } void MavCmdNavLoiterUnlim::pack() { - param3 = RadiusAroundMission; - param4 = DesiredYawAngle; + param3 = Radius; + param4 = Yaw; param5 = Latitude; param6 = Longitude; param7 = Altitude; } void MavCmdNavLoiterUnlim::unpack() { - RadiusAroundMission = param3; - DesiredYawAngle = param4; + Radius = param3; + Yaw = param4; Latitude = param5; Longitude = param6; Altitude = param7; } void MavCmdNavLoiterTurns::pack() { param1 = Turns; - param3 = RadiusAroundMission; + param3 = Radius; param4 = ExitXtrackLocation; param5 = Latitude; param6 = Longitude; @@ -5173,23 +5524,23 @@ void MavCmdNavLoiterTurns::pack() { } void MavCmdNavLoiterTurns::unpack() { Turns = param1; - RadiusAroundMission = param3; + Radius = param3; ExitXtrackLocation = param4; Latitude = param5; Longitude = param6; Altitude = param7; } void MavCmdNavLoiterTime::pack() { - param1 = Seconds; - param3 = RadiusAroundMission; + param1 = Time; + param3 = Radius; param4 = ExitXtrackLocation; param5 = Latitude; param6 = Longitude; param7 = Altitude; } void MavCmdNavLoiterTime::unpack() { - Seconds = param1; - RadiusAroundMission = param3; + Time = param1; + Radius = param3; ExitXtrackLocation = param4; Latitude = param5; Longitude = param6; @@ -5201,240 +5552,274 @@ void MavCmdNavReturnToLaunch::unpack() { } void MavCmdNavLand::pack() { param1 = AbortAlt; - param4 = DesiredYawAngle; + param2 = LandMode; + param4 = YawAngle; param5 = Latitude; param6 = Longitude; - param7 = Altitude; + param7 = LandingAltitude; } void MavCmdNavLand::unpack() { AbortAlt = param1; - DesiredYawAngle = param4; + LandMode = param2; + YawAngle = param4; Latitude = param5; Longitude = param6; - Altitude = param7; + LandingAltitude = param7; } void MavCmdNavTakeoff::pack() { - param1 = MinimumPitch; - param4 = YawAngle; + param1 = Pitch; + param4 = Yaw; param5 = Latitude; param6 = Longitude; param7 = Altitude; } void MavCmdNavTakeoff::unpack() { - MinimumPitch = param1; - YawAngle = param4; + Pitch = param1; + Yaw = param4; Latitude = param5; Longitude = param6; Altitude = param7; } void MavCmdNavLandLocal::pack() { - param1 = LandingTargetNumber; - param2 = MaximumAcceptedOffset; - param3 = LandingDescendRate; - param4 = DesiredYawAngle; - param5 = Y; - param6 = X; - param7 = Z; + param1 = Target; + param2 = Offset; + param3 = DescendRate; + param4 = Yaw; + param5 = YPosition; + param6 = XPosition; + param7 = ZPosition; } void MavCmdNavLandLocal::unpack() { - LandingTargetNumber = param1; - MaximumAcceptedOffset = param2; - LandingDescendRate = param3; - DesiredYawAngle = param4; - Y = param5; - X = param6; - Z = param7; + Target = param1; + Offset = param2; + DescendRate = param3; + Yaw = param4; + YPosition = param5; + XPosition = param6; + ZPosition = param7; } void MavCmdNavTakeoffLocal::pack() { - param1 = MinimumPitch; - param3 = TakeoffAscendRate; - param4 = YawAngle; - param5 = Y; - param6 = X; - param7 = Z; + param1 = Pitch; + param3 = AscendRate; + param4 = Yaw; + param5 = YPosition; + param6 = XPosition; + param7 = ZPosition; } void MavCmdNavTakeoffLocal::unpack() { - MinimumPitch = param1; - TakeoffAscendRate = param3; - YawAngle = param4; - Y = param5; - X = param6; - Z = param7; + Pitch = param1; + AscendRate = param3; + Yaw = param4; + YPosition = param5; + XPosition = param6; + ZPosition = param7; } void MavCmdNavFollow::pack() { - param1 = FollowingLogicTo; - param2 = GroundSpeedOf; - param3 = RadiusAroundMission; - param4 = DesiredYawAngle; + param1 = Following; + param2 = GroundSpeed; + param3 = Radius; + param4 = Yaw; param5 = Latitude; param6 = Longitude; param7 = Altitude; } void MavCmdNavFollow::unpack() { - FollowingLogicTo = param1; - GroundSpeedOf = param2; - RadiusAroundMission = param3; - DesiredYawAngle = param4; + Following = param1; + GroundSpeed = param2; + Radius = param3; + Yaw = param4; Latitude = param5; Longitude = param6; Altitude = param7; } void MavCmdNavContinueAndChangeAlt::pack() { - param1 = ClimbOrDescend; - param7 = DesiredAltitudeMeters; + param1 = Action; + param7 = Altitude; } void MavCmdNavContinueAndChangeAlt::unpack() { - ClimbOrDescend = param1; - DesiredAltitudeMeters = param7; + Action = param1; + Altitude = param7; } void MavCmdNavLoiterToAlt::pack() { param1 = HeadingRequired; - param2 = RadiusMeters; - param4 = ExitXtrackLocation; + param2 = Radius; + param4 = XtrackLocation; param5 = Latitude; param6 = Longitude; param7 = Altitude; } void MavCmdNavLoiterToAlt::unpack() { HeadingRequired = param1; - RadiusMeters = param2; - ExitXtrackLocation = param4; + Radius = param2; + XtrackLocation = param4; Latitude = param5; Longitude = param6; Altitude = param7; } void MavCmdDoFollow::pack() { param1 = SystemId; - param4 = AltitudeFlag; + param4 = AltitudeMode; param5 = Altitude; - param7 = TtlSecondsWhich; + param7 = TimeToLand; } void MavCmdDoFollow::unpack() { SystemId = param1; - AltitudeFlag = param4; + AltitudeMode = param4; Altitude = param5; - TtlSecondsWhich = param7; + TimeToLand = param7; } void MavCmdDoFollowReposition::pack() { - param1 = CameraQ1; - param2 = CameraQ2; - param3 = CameraQ3; - param4 = CameraQ4; - param5 = AltitudeOffsetTarget; - param6 = XOffsetTarget; - param7 = YOffsetTarget; + param1 = CameraQp1; + param2 = CameraQp2; + param3 = CameraQp3; + param4 = CameraQp4; + param5 = AltitudeOffset; + param6 = XOffset; + param7 = YOffset; } void MavCmdDoFollowReposition::unpack() { - CameraQ1 = param1; - CameraQ2 = param2; - CameraQ3 = param3; - CameraQ4 = param4; - AltitudeOffsetTarget = param5; - XOffsetTarget = param6; - YOffsetTarget = param7; + CameraQp1 = param1; + CameraQp2 = param2; + CameraQp3 = param3; + CameraQp4 = param4; + AltitudeOffset = param5; + XOffset = param6; + YOffset = param7; +} +void MavCmdDoOrbit::pack() { + param1 = Radius; + param2 = Velocity; + param3 = YawBehavior; + param5 = CenterPointLatitude; + param6 = CenterPointLongitude; + param7 = CenterPointAltitude; +} +void MavCmdDoOrbit::unpack() { + Radius = param1; + Velocity = param2; + YawBehavior = param3; + CenterPointLatitude = param5; + CenterPointLongitude = param6; + CenterPointAltitude = param7; } void MavCmdNavRoi::pack() { - param1 = RegionOfIntereset; - param2 = MissionIndexTarget; + param1 = RoiMode; + param2 = WpIndex; param3 = RoiIndex; - param5 = XLocationOf; + param5 = X; param6 = Y; param7 = Z; } void MavCmdNavRoi::unpack() { - RegionOfIntereset = param1; - MissionIndexTarget = param2; + RoiMode = param1; + WpIndex = param2; RoiIndex = param3; - XLocationOf = param5; + X = param5; Y = param6; Z = param7; } void MavCmdNavPathplanning::pack() { - param1 = p0; - param2 = p02; - param4 = YawAngleAt; - param5 = LatitudexOfGoal; - param6 = LongitudeyOfGoal; - param7 = AltitudezOfGoal; + param1 = LocalCtrl; + param2 = GlobalCtrl; + param4 = Yaw; + param5 = LatitudepxOfGoal; + param6 = LongitudepyOfGoal; + param7 = AltitudepzOfGoal; } void MavCmdNavPathplanning::unpack() { - p0 = param1; - p02 = param2; - YawAngleAt = param4; - LatitudexOfGoal = param5; - LongitudeyOfGoal = param6; - AltitudezOfGoal = param7; + LocalCtrl = param1; + GlobalCtrl = param2; + Yaw = param4; + LatitudepxOfGoal = param5; + LongitudepyOfGoal = param6; + AltitudepzOfGoal = param7; } void MavCmdNavSplineWaypoint::pack() { - param1 = HoldTimeDecimal; - param5 = LatitudexOfGoal; - param6 = LongitudeyOfGoal; - param7 = AltitudezOfGoal; + param1 = Hold; + param5 = LatitudepxOfGoal; + param6 = LongitudepyOfGoal; + param7 = AltitudepzOfGoal; } void MavCmdNavSplineWaypoint::unpack() { - HoldTimeDecimal = param1; - LatitudexOfGoal = param5; - LongitudeyOfGoal = param6; - AltitudezOfGoal = param7; + Hold = param1; + LatitudepxOfGoal = param5; + LongitudepyOfGoal = param6; + AltitudepzOfGoal = param7; } void MavCmdNavVtolTakeoff::pack() { - param4 = YawAngleDegrees; + param2 = TransitionHeading; + param4 = YawAngle; param5 = Latitude; param6 = Longitude; param7 = Altitude; } void MavCmdNavVtolTakeoff::unpack() { - YawAngleDegrees = param4; + TransitionHeading = param2; + YawAngle = param4; Latitude = param5; Longitude = param6; Altitude = param7; } void MavCmdNavVtolLand::pack() { - param4 = YawAngleDegrees; + param3 = Altitude; + param4 = Yaw; param5 = Latitude; param6 = Longitude; - param7 = Altitude; + param7 = Altitude2; } void MavCmdNavVtolLand::unpack() { - YawAngleDegrees = param4; + Altitude = param3; + Yaw = param4; Latitude = param5; Longitude = param6; - Altitude = param7; + Altitude2 = param7; } void MavCmdNavGuidedEnable::pack() { - param1 = OnOff; + param1 = Enable; } void MavCmdNavGuidedEnable::unpack() { - OnOff = param1; + Enable = param1; } void MavCmdNavDelay::pack() { - param1 = DelaySeconds; + param1 = Delay; param2 = Hour; param3 = Minute; param4 = Second; } void MavCmdNavDelay::unpack() { - DelaySeconds = param1; + Delay = param1; Hour = param2; Minute = param3; Second = param4; } +void MavCmdNavPayloadPlace::pack() { + param1 = MaxDescent; + param5 = Latitude; + param6 = Longitude; + param7 = Altitude; +} +void MavCmdNavPayloadPlace::unpack() { + MaxDescent = param1; + Latitude = param5; + Longitude = param6; + Altitude = param7; +} void MavCmdNavLast::pack() { } void MavCmdNavLast::unpack() { } void MavCmdConditionDelay::pack() { - param1 = DelaySeconds; + param1 = Delay; } void MavCmdConditionDelay::unpack() { - DelaySeconds = param1; + Delay = param1; } void MavCmdConditionChangeAlt::pack() { - param1 = DescentAscend; + param1 = Rate; param7 = FinishAltitude; } void MavCmdConditionChangeAlt::unpack() { - DescentAscend = param1; + Rate = param1; FinishAltitude = param7; } void MavCmdConditionDistance::pack() { @@ -5444,16 +5829,16 @@ void MavCmdConditionDistance::unpack() { Distance = param1; } void MavCmdConditionYaw::pack() { - param1 = TargetAngle; - param2 = SpeedDuringYaw; + param1 = Angle; + param2 = AngularSpeed; param3 = Direction; - param4 = RelativeOffsetOr; + param4 = Relative; } void MavCmdConditionYaw::unpack() { - TargetAngle = param1; - SpeedDuringYaw = param2; + Angle = param1; + AngularSpeed = param2; Direction = param3; - RelativeOffsetOr = param4; + Relative = param4; } void MavCmdConditionLast::pack() { } @@ -5462,32 +5847,32 @@ void MavCmdConditionLast::unpack() { void MavCmdDoSetMode::pack() { param1 = Mode; param2 = CustomMode; - param3 = CustomSubMode; + param3 = CustomSubmode; } void MavCmdDoSetMode::unpack() { Mode = param1; CustomMode = param2; - CustomSubMode = param3; + CustomSubmode = param3; } void MavCmdDoJump::pack() { - param1 = SequenceNumber; - param2 = RepeatCount; + param1 = Number; + param2 = Repeat; } void MavCmdDoJump::unpack() { - SequenceNumber = param1; - RepeatCount = param2; + Number = param1; + Repeat = param2; } void MavCmdDoChangeSpeed::pack() { param1 = SpeedType; param2 = Speed; param3 = Throttle; - param4 = AbsoluteOrRelative; + param4 = Relative; } void MavCmdDoChangeSpeed::unpack() { SpeedType = param1; Speed = param2; Throttle = param3; - AbsoluteOrRelative = param4; + Relative = param4; } void MavCmdDoSetHome::pack() { param1 = UseCurrent; @@ -5502,64 +5887,64 @@ void MavCmdDoSetHome::unpack() { Altitude = param7; } void MavCmdDoSetParameter::pack() { - param1 = ParameterNumber; - param2 = ParameterValue; + param1 = Number; + param2 = Value; } void MavCmdDoSetParameter::unpack() { - ParameterNumber = param1; - ParameterValue = param2; + Number = param1; + Value = param2; } void MavCmdDoSetRelay::pack() { - param1 = RelayNumber; + param1 = Instance; param2 = Setting; } void MavCmdDoSetRelay::unpack() { - RelayNumber = param1; + Instance = param1; Setting = param2; } void MavCmdDoRepeatRelay::pack() { - param1 = RelayNumber; - param2 = CycleCount; - param3 = CycleTime; + param1 = Instance; + param2 = Count; + param3 = Time; } void MavCmdDoRepeatRelay::unpack() { - RelayNumber = param1; - CycleCount = param2; - CycleTime = param3; + Instance = param1; + Count = param2; + Time = param3; } void MavCmdDoSetServo::pack() { - param1 = ServoNumber; + param1 = Instance; param2 = Pwm; } void MavCmdDoSetServo::unpack() { - ServoNumber = param1; + Instance = param1; Pwm = param2; } void MavCmdDoRepeatServo::pack() { - param1 = ServoNumber; + param1 = Instance; param2 = Pwm; - param3 = CycleCount; - param4 = CycleTime; + param3 = Count; + param4 = Time; } void MavCmdDoRepeatServo::unpack() { - ServoNumber = param1; + Instance = param1; Pwm = param2; - CycleCount = param3; - CycleTime = param4; + Count = param3; + Time = param4; } void MavCmdDoFlighttermination::pack() { - param1 = FlightTerminationActivated; + param1 = Terminate; } void MavCmdDoFlighttermination::unpack() { - FlightTerminationActivated = param1; + Terminate = param1; } void MavCmdDoChangeAltitude::pack() { - param1 = AltitudeMeters; - param2 = MavFrameOf; + param1 = Altitude; + param2 = Frame; } void MavCmdDoChangeAltitude::unpack() { - AltitudeMeters = param1; - MavFrameOf = param2; + Altitude = param1; + Frame = param2; } void MavCmdDoLandStart::pack() { param5 = Latitude; @@ -5570,12 +5955,12 @@ void MavCmdDoLandStart::unpack() { Longitude = param6; } void MavCmdDoRallyLand::pack() { - param1 = BreakAltitude; - param2 = LandingSpeed; + param1 = Altitude; + param2 = Speed; } void MavCmdDoRallyLand::unpack() { - BreakAltitude = param1; - LandingSpeed = param2; + Altitude = param1; + Speed = param2; } void MavCmdDoGoAround::pack() { param1 = Altitude; @@ -5584,130 +5969,166 @@ void MavCmdDoGoAround::unpack() { Altitude = param1; } void MavCmdDoReposition::pack() { - param1 = GroundSpeed; - param2 = BitmaskOfOption; - param4 = YawHeading; + param1 = Speed; + param2 = Bitmask; + param4 = Yaw; param5 = Latitude; param6 = Longitude; param7 = Altitude; } void MavCmdDoReposition::unpack() { - GroundSpeed = param1; - BitmaskOfOption = param2; - YawHeading = param4; + Speed = param1; + Bitmask = param2; + Yaw = param4; Latitude = param5; Longitude = param6; Altitude = param7; } void MavCmdDoPauseContinue::pack() { - param1 = p0; + param1 = Continue; } void MavCmdDoPauseContinue::unpack() { - p0 = param1; + Continue = param1; } void MavCmdDoSetReverse::pack() { - param1 = Direction; + param1 = Reverse; } void MavCmdDoSetReverse::unpack() { - Direction = param1; + Reverse = param1; +} +void MavCmdDoSetRoiLocation::pack() { + param5 = Latitude; + param6 = Longitude; + param7 = Altitude; +} +void MavCmdDoSetRoiLocation::unpack() { + Latitude = param5; + Longitude = param6; + Altitude = param7; +} +void MavCmdDoSetRoiWpnextOffset::pack() { + param5 = PitchOffset; + param6 = RollOffset; + param7 = YawOffset; +} +void MavCmdDoSetRoiWpnextOffset::unpack() { + PitchOffset = param5; + RollOffset = param6; + YawOffset = param7; +} +void MavCmdDoSetRoiNone::pack() { +} +void MavCmdDoSetRoiNone::unpack() { } void MavCmdDoControlVideo::pack() { - param1 = CameraId; + param1 = Id; param2 = Transmission; - param3 = TransmissionMode; + param3 = Interval; param4 = Recording; } void MavCmdDoControlVideo::unpack() { - CameraId = param1; + Id = param1; Transmission = param2; - TransmissionMode = param3; + Interval = param3; Recording = param4; } void MavCmdDoSetRoi::pack() { - param1 = RegionOfIntereset; - param2 = MissionIndexTarget; + param1 = RoiMode; + param2 = WpIndex; param3 = RoiIndex; - param5 = XLocationOf; - param6 = Y; - param7 = Z; + param5 = MavRoiWpnext; + param6 = MavRoiWpnext2; + param7 = MavRoiWpnext3; } void MavCmdDoSetRoi::unpack() { - RegionOfIntereset = param1; - MissionIndexTarget = param2; + RoiMode = param1; + WpIndex = param2; RoiIndex = param3; - XLocationOf = param5; - Y = param6; - Z = param7; + MavRoiWpnext = param5; + MavRoiWpnext2 = param6; + MavRoiWpnext3 = param7; } void MavCmdDoDigicamConfigure::pack() { - param1 = Modes; + param1 = Mode; param2 = ShutterSpeed; param3 = Aperture; - param4 = IsoNumberE; - param5 = ExposureTypeEnumerator; + param4 = Iso; + param5 = Exposure; param6 = CommandIdentity; - param7 = MainEngineCut; + param7 = EngineCutpoff; } void MavCmdDoDigicamConfigure::unpack() { - Modes = param1; + Mode = param1; ShutterSpeed = param2; Aperture = param3; - IsoNumberE = param4; - ExposureTypeEnumerator = param5; + Iso = param4; + Exposure = param5; CommandIdentity = param6; - MainEngineCut = param7; + EngineCutpoff = param7; } void MavCmdDoDigicamControl::pack() { - param1 = SessionControlE; - param2 = ZoomsAbsolutePosition; - param3 = ZoomingStepValue; - param4 = FocusLocking; - param5 = ShootingCommand; + param1 = SessionControl; + param2 = ZoomAbsolute; + param3 = ZoomRelative; + param4 = Focus; + param5 = ShootCommand; param6 = CommandIdentity; + param7 = ShotId; } void MavCmdDoDigicamControl::unpack() { - SessionControlE = param1; - ZoomsAbsolutePosition = param2; - ZoomingStepValue = param3; - FocusLocking = param4; - ShootingCommand = param5; + SessionControl = param1; + ZoomAbsolute = param2; + ZoomRelative = param3; + Focus = param4; + ShootCommand = param5; CommandIdentity = param6; + ShotId = param7; } void MavCmdDoMountConfigure::pack() { - param1 = MountOperationMode; + param1 = Mode; param2 = StabilizeRoll; param3 = StabilizePitch; param4 = StabilizeYaw; + param5 = RollInput; + param6 = PitchInput; + param7 = YawInput; } void MavCmdDoMountConfigure::unpack() { - MountOperationMode = param1; + Mode = param1; StabilizeRoll = param2; StabilizePitch = param3; StabilizeYaw = param4; + RollInput = param5; + PitchInput = param6; + YawInput = param7; } void MavCmdDoMountControl::pack() { - param1 = Pitch; - param2 = Roll; - param3 = Yaw; - param4 = Wip; - param5 = Wip2; - param6 = Wip3; - param7 = MavMountModeEnumValue; + param1 = PitchDependingMount; + param2 = RollDependingMount; + param3 = YawDependingMount; + param4 = Altitude; + param5 = Latitude; + param6 = Longitude; + param7 = Mode; } void MavCmdDoMountControl::unpack() { - Pitch = param1; - Roll = param2; - Yaw = param3; - Wip = param4; - Wip2 = param5; - Wip3 = param6; - MavMountModeEnumValue = param7; + PitchDependingMount = param1; + RollDependingMount = param2; + YawDependingMount = param3; + Altitude = param4; + Latitude = param5; + Longitude = param6; + Mode = param7; } void MavCmdDoSetCamTriggDist::pack() { - param1 = CameraTriggerDistance; + param1 = Distance; + param2 = Shutter; + param3 = Trigger; } void MavCmdDoSetCamTriggDist::unpack() { - CameraTriggerDistance = param1; + Distance = param1; + Shutter = param2; + Trigger = param3; } void MavCmdDoFenceEnable::pack() { param1 = Enable; @@ -5722,16 +6143,20 @@ void MavCmdDoParachute::unpack() { Action = param1; } void MavCmdDoMotorTest::pack() { - param1 = MotorSequenceNumber; + param1 = Instance; param2 = ThrottleType; param3 = Throttle; param4 = Timeout; + param5 = MotorCount; + param6 = TestOrder; } void MavCmdDoMotorTest::unpack() { - MotorSequenceNumber = param1; + Instance = param1; ThrottleType = param2; Throttle = param3; Timeout = param4; + MotorCount = param5; + TestOrder = param6; } void MavCmdDoInvertedFlight::pack() { param1 = Inverted; @@ -5739,25 +6164,35 @@ void MavCmdDoInvertedFlight::pack() { void MavCmdDoInvertedFlight::unpack() { Inverted = param1; } -void MavCmdDoSetPositionYawThrust::pack() { - param1 = YawAngleTo; - param2 = Thrust; +void MavCmdNavSetYawSpeed::pack() { + param1 = Yaw; + param2 = Speed; + param3 = Angle; +} +void MavCmdNavSetYawSpeed::unpack() { + Yaw = param1; + Speed = param2; + Angle = param3; +} +void MavCmdDoSetCamTriggInterval::pack() { + param1 = TriggerCycle; + param2 = ShutterIntegration; } -void MavCmdDoSetPositionYawThrust::unpack() { - YawAngleTo = param1; - Thrust = param2; +void MavCmdDoSetCamTriggInterval::unpack() { + TriggerCycle = param1; + ShutterIntegration = param2; } void MavCmdDoMountControlQuat::pack() { - param1 = Q1; - param2 = Q2; - param3 = Q3; - param4 = Q4; + param1 = Qp1; + param2 = Qp2; + param3 = Qp3; + param4 = Qp4; } void MavCmdDoMountControlQuat::unpack() { - Q1 = param1; - Q2 = param2; - Q3 = param3; - Q4 = param4; + Qp1 = param1; + Qp2 = param2; + Qp3 = param3; + Qp4 = param4; } void MavCmdDoGuidedMaster::pack() { param1 = SystemId; @@ -5769,111 +6204,119 @@ void MavCmdDoGuidedMaster::unpack() { } void MavCmdDoGuidedLimits::pack() { param1 = Timeout; - param2 = AbsoluteAltitudeMin; - param3 = AbsoluteAltitudeMax; - param4 = HorizontalMoveLimit; + param2 = MinAltitude; + param3 = MaxAltitude; + param4 = HorizpMoveLimit; } void MavCmdDoGuidedLimits::unpack() { Timeout = param1; - AbsoluteAltitudeMin = param2; - AbsoluteAltitudeMax = param3; - HorizontalMoveLimit = param4; + MinAltitude = param2; + MaxAltitude = param3; + HorizpMoveLimit = param4; } void MavCmdDoEngineControl::pack() { - param1 = p0; - param2 = p02; + param1 = StartEngine; + param2 = ColdStart; param3 = HeightDelay; } void MavCmdDoEngineControl::unpack() { - p0 = param1; - p02 = param2; + StartEngine = param1; + ColdStart = param2; HeightDelay = param3; } +void MavCmdDoSetMissionCurrent::pack() { + param1 = Number; +} +void MavCmdDoSetMissionCurrent::unpack() { + Number = param1; +} void MavCmdDoLast::pack() { } void MavCmdDoLast::unpack() { } void MavCmdPreflightCalibration::pack() { - param1 = GyroCalibration; - param2 = MagnetometerCalibration; + param1 = GyroTemperature; + param2 = Magnetometer; param3 = GroundPressure; - param4 = RadioCalibration; - param5 = AccelerometerCalibration; - param6 = CompassmotorInterferenceCalibration; + param4 = RemoteControl; + param5 = Accelerometer; + param6 = CompmotOrAirspeed; + param7 = EscOrBaro; } void MavCmdPreflightCalibration::unpack() { - GyroCalibration = param1; - MagnetometerCalibration = param2; + GyroTemperature = param1; + Magnetometer = param2; GroundPressure = param3; - RadioCalibration = param4; - AccelerometerCalibration = param5; - CompassmotorInterferenceCalibration = param6; + RemoteControl = param4; + Accelerometer = param5; + CompmotOrAirspeed = param6; + EscOrBaro = param7; } void MavCmdPreflightSetSensorOffsets::pack() { - param1 = SensorToAdjust; - param2 = XAxisOffset; - param3 = YAxisOffset; - param4 = ZAxisOffset; - param5 = GenericDimension4; - param6 = GenericDimension5; - param7 = GenericDimension6; + param1 = SensorType; + param2 = XOffset; + param3 = YOffset; + param4 = ZOffset; + param5 = P4thDimension; + param6 = P5thDimension; + param7 = P6thDimension; } void MavCmdPreflightSetSensorOffsets::unpack() { - SensorToAdjust = param1; - XAxisOffset = param2; - YAxisOffset = param3; - ZAxisOffset = param4; - GenericDimension4 = param5; - GenericDimension5 = param6; - GenericDimension6 = param7; + SensorType = param1; + XOffset = param2; + YOffset = param3; + ZOffset = param4; + P4thDimension = param5; + P5thDimension = param6; + P6thDimension = param7; } void MavCmdPreflightUavcan::pack() { - param1 = p1; + param1 = ActuatorId; } void MavCmdPreflightUavcan::unpack() { - p1 = param1; + ActuatorId = param1; } void MavCmdPreflightStorage::pack() { param1 = ParameterStorage; param2 = MissionStorage; - param3 = OnboardLogging; + param3 = LoggingRate; } void MavCmdPreflightStorage::unpack() { ParameterStorage = param1; MissionStorage = param2; - OnboardLogging = param3; + LoggingRate = param3; } void MavCmdPreflightRebootShutdown::pack() { - param1 = p0; - param2 = p02; + param1 = Autopilot; + param2 = Companion; param3 = Wip; param4 = Wip2; param7 = Wip3; } void MavCmdPreflightRebootShutdown::unpack() { - p0 = param1; - p02 = param2; + Autopilot = param1; + Companion = param2; Wip = param3; Wip2 = param4; Wip3 = param7; } void MavCmdOverrideGoto::pack() { - param1 = MavGotoDoHold; - param2 = MavGotoHoldAtCurrentPosition; - param3 = MavFrameCoordinateFrame; - param4 = DesiredYawAngle; - param5 = LatitudeX; - param6 = LongitudeY; - param7 = AltitudeZ; + param1 = Continue; + param2 = Position; + param3 = Frame; + param4 = Yaw; + param5 = LatitudePX; + param6 = LongitudePY; + param7 = AltitudePZ; } void MavCmdOverrideGoto::unpack() { - MavGotoDoHold = param1; - MavGotoHoldAtCurrentPosition = param2; - MavFrameCoordinateFrame = param3; - DesiredYawAngle = param4; - LatitudeX = param5; - LongitudeY = param6; - AltitudeZ = param7; + Continue = param1; + Position = param2; + Frame = param3; + Yaw = param4; + LatitudePX = param5; + LongitudePY = param6; + AltitudePZ = param7; } void MavCmdMissionStart::pack() { param1 = FirstItem; @@ -5884,172 +6327,226 @@ void MavCmdMissionStart::unpack() { LastItem = param2; } void MavCmdComponentArmDisarm::pack() { - param1 = p1ToArm; + param1 = Arm; + param2 = Force; } void MavCmdComponentArmDisarm::unpack() { - p1ToArm = param1; + Arm = param1; + Force = param2; +} +void MavCmdIlluminatorOnOff::pack() { + param1 = Enable; +} +void MavCmdIlluminatorOnOff::unpack() { + Enable = param1; } void MavCmdGetHomePosition::pack() { } void MavCmdGetHomePosition::unpack() { } void MavCmdStartRxPair::pack() { - param1 = p0; - param2 = p02; + param1 = Spektrum; + param2 = RcType; } void MavCmdStartRxPair::unpack() { - p0 = param1; - p02 = param2; + Spektrum = param1; + RcType = param2; } void MavCmdGetMessageInterval::pack() { - param1 = TheMavlinkMessage; + param1 = MessageId; } void MavCmdGetMessageInterval::unpack() { - TheMavlinkMessage = param1; + MessageId = param1; } void MavCmdSetMessageInterval::pack() { - param1 = TheMavlinkMessage; - param2 = TheIntervalBetween; + param1 = MessageId; + param2 = Interval; + param3 = ResponseTarget; } void MavCmdSetMessageInterval::unpack() { - TheMavlinkMessage = param1; - TheIntervalBetween = param2; + MessageId = param1; + Interval = param2; + ResponseTarget = param3; +} +void MavCmdRequestMessage::pack() { + param1 = MessageId; + param2 = IndexId; + param3 = TheUseOf; + param4 = TheUseOf2; + param5 = TheUseOf3; + param6 = TheUseOf4; + param7 = ResponseTarget; +} +void MavCmdRequestMessage::unpack() { + MessageId = param1; + IndexId = param2; + TheUseOf = param3; + TheUseOf2 = param4; + TheUseOf3 = param5; + TheUseOf4 = param6; + ResponseTarget = param7; +} +void MavCmdRequestProtocolVersion::pack() { + param1 = Protocol; +} +void MavCmdRequestProtocolVersion::unpack() { + Protocol = param1; } void MavCmdRequestAutopilotCapabilities::pack() { - param1 = p1; + param1 = Version; } void MavCmdRequestAutopilotCapabilities::unpack() { - p1 = param1; + Version = param1; } void MavCmdRequestCameraInformation::pack() { - param1 = p1; - param2 = CameraId; + param1 = Capabilities; } void MavCmdRequestCameraInformation::unpack() { - p1 = param1; - CameraId = param2; + Capabilities = param1; } void MavCmdRequestCameraSettings::pack() { - param1 = p1; - param2 = CameraId; + param1 = Settings; } void MavCmdRequestCameraSettings::unpack() { - p1 = param1; - CameraId = param2; -} -void MavCmdSetCameraSettings1::pack() { - param1 = CameraId; - param2 = Aperture; - param3 = ApertureLocked; - param4 = ShutterSpeedS; - param5 = ShutterSpeedLocked; - param6 = IsoSensitivity; - param7 = IsoSensitivityLocked; -} -void MavCmdSetCameraSettings1::unpack() { - CameraId = param1; - Aperture = param2; - ApertureLocked = param3; - ShutterSpeedS = param4; - ShutterSpeedLocked = param5; - IsoSensitivity = param6; - IsoSensitivityLocked = param7; -} -void MavCmdSetCameraSettings2::pack() { - param1 = CameraId; - param2 = WhiteBalanceLocked; - param3 = WhiteBalance; - param4 = ReservedForCamera; - param5 = ReservedForColor; - param6 = ReservedForImage; -} -void MavCmdSetCameraSettings2::unpack() { - CameraId = param1; - WhiteBalanceLocked = param2; - WhiteBalance = param3; - ReservedForCamera = param4; - ReservedForColor = param5; - ReservedForImage = param6; + Settings = param1; } void MavCmdRequestStorageInformation::pack() { - param1 = p1; - param2 = StorageId; + param1 = StorageId; + param2 = Information; } void MavCmdRequestStorageInformation::unpack() { - p1 = param1; - StorageId = param2; + StorageId = param1; + Information = param2; } void MavCmdStorageFormat::pack() { - param1 = p1; - param2 = StorageId; + param1 = StorageId; + param2 = Format; } void MavCmdStorageFormat::unpack() { - p1 = param1; - StorageId = param2; + StorageId = param1; + Format = param2; } void MavCmdRequestCameraCaptureStatus::pack() { - param1 = p1; - param2 = CameraId; + param1 = CaptureStatus; } void MavCmdRequestCameraCaptureStatus::unpack() { - p1 = param1; - CameraId = param2; + CaptureStatus = param1; } void MavCmdRequestFlightInformation::pack() { - param1 = p1; + param1 = FlightInformation; } void MavCmdRequestFlightInformation::unpack() { - p1 = param1; + FlightInformation = param1; +} +void MavCmdResetCameraSettings::pack() { + param1 = Reset; +} +void MavCmdResetCameraSettings::unpack() { + Reset = param1; +} +void MavCmdSetCameraMode::pack() { + param2 = CameraMode; +} +void MavCmdSetCameraMode::unpack() { + CameraMode = param2; +} +void MavCmdSetCameraZoom::pack() { + param1 = ZoomType; + param2 = ZoomValue; +} +void MavCmdSetCameraZoom::unpack() { + ZoomType = param1; + ZoomValue = param2; +} +void MavCmdSetCameraFocus::pack() { + param1 = FocusType; + param2 = FocusValue; +} +void MavCmdSetCameraFocus::unpack() { + FocusType = param1; + FocusValue = param2; +} +void MavCmdJumpTag::pack() { + param1 = Tag; +} +void MavCmdJumpTag::unpack() { + Tag = param1; +} +void MavCmdDoJumpTag::pack() { + param1 = Tag; + param2 = Repeat; +} +void MavCmdDoJumpTag::unpack() { + Tag = param1; + Repeat = param2; } void MavCmdImageStartCapture::pack() { - param1 = DurationBetweenTwo; - param2 = NumberOfImages; - param3 = ResolutionMegapixels; - param4 = Wip; - param5 = Wip2; - param6 = Wip3; + param2 = Interval; + param3 = CaptureCount; + param4 = SequenceNumber; } void MavCmdImageStartCapture::unpack() { - DurationBetweenTwo = param1; - NumberOfImages = param2; - ResolutionMegapixels = param3; - Wip = param4; - Wip2 = param5; - Wip3 = param6; + Interval = param2; + CaptureCount = param3; + SequenceNumber = param4; } void MavCmdImageStopCapture::pack() { - param1 = CameraId; } void MavCmdImageStopCapture::unpack() { - CameraId = param1; +} +void MavCmdRequestCameraImageCapture::pack() { + param1 = Number; +} +void MavCmdRequestCameraImageCapture::unpack() { + Number = param1; } void MavCmdDoTriggerControl::pack() { - param1 = TriggerEnabledisable; - param2 = ShutterIntegrationTime; + param1 = Enable; + param2 = Reset; + param3 = Pause; } void MavCmdDoTriggerControl::unpack() { - TriggerEnabledisable = param1; - ShutterIntegrationTime = param2; + Enable = param1; + Reset = param2; + Pause = param3; } void MavCmdVideoStartCapture::pack() { - param1 = CameraId; - param2 = FramesPerSecond; - param3 = ResolutionMegapixels; - param4 = Wip; - param5 = Wip2; + param1 = StreamId; + param2 = StatusFrequency; } void MavCmdVideoStartCapture::unpack() { - CameraId = param1; - FramesPerSecond = param2; - ResolutionMegapixels = param3; - Wip = param4; - Wip2 = param5; + StreamId = param1; + StatusFrequency = param2; } void MavCmdVideoStopCapture::pack() { - param1 = Wip; + param1 = StreamId; } void MavCmdVideoStopCapture::unpack() { - Wip = param1; + StreamId = param1; +} +void MavCmdVideoStartStreaming::pack() { + param1 = StreamId; +} +void MavCmdVideoStartStreaming::unpack() { + StreamId = param1; +} +void MavCmdVideoStopStreaming::pack() { + param1 = StreamId; +} +void MavCmdVideoStopStreaming::unpack() { + StreamId = param1; +} +void MavCmdRequestVideoStreamInformation::pack() { + param1 = StreamId; +} +void MavCmdRequestVideoStreamInformation::unpack() { + StreamId = param1; +} +void MavCmdRequestVideoStreamStatus::pack() { + param1 = StreamId; +} +void MavCmdRequestVideoStreamStatus::unpack() { + StreamId = param1; } void MavCmdLoggingStart::pack() { param1 = Format; @@ -6069,30 +6566,42 @@ void MavCmdAirframeConfiguration::unpack() { LandingGearId = param1; LandingGearPosition = param2; } +void MavCmdControlHighLatency::pack() { + param1 = Enable; +} +void MavCmdControlHighLatency::unpack() { + Enable = param1; +} void MavCmdPanoramaCreate::pack() { - param1 = ViewingAngleHorizontal; - param2 = ViewingAngleVertical; - param3 = SpeedOfHorizontal; - param4 = SpeedOfVertical; + param1 = HorizontalAngle; + param2 = VerticalAngle; + param3 = HorizontalSpeed; + param4 = VerticalSpeed; } void MavCmdPanoramaCreate::unpack() { - ViewingAngleHorizontal = param1; - ViewingAngleVertical = param2; - SpeedOfHorizontal = param3; - SpeedOfVertical = param4; + HorizontalAngle = param1; + VerticalAngle = param2; + HorizontalSpeed = param3; + VerticalSpeed = param4; } void MavCmdDoVtolTransition::pack() { - param1 = TheTargetVtol; + param1 = State; } void MavCmdDoVtolTransition::unpack() { - TheTargetVtol = param1; + State = param1; +} +void MavCmdArmAuthorizationRequest::pack() { + param1 = SystemId; +} +void MavCmdArmAuthorizationRequest::unpack() { + SystemId = param1; } void MavCmdSetGuidedSubmodeStandard::pack() { } void MavCmdSetGuidedSubmodeStandard::unpack() { } void MavCmdSetGuidedSubmodeCircle::pack() { - param1 = RadiusOfDesired; + param1 = Radius; param2 = UserDefined; param3 = UserDefined2; param4 = UserDefined3; @@ -6100,27 +6609,105 @@ void MavCmdSetGuidedSubmodeCircle::pack() { param6 = UnscaledTargetLongitude; } void MavCmdSetGuidedSubmodeCircle::unpack() { - RadiusOfDesired = param1; + Radius = param1; UserDefined = param2; UserDefined2 = param3; UserDefined3 = param4; UnscaledTargetLatitude = param5; UnscaledTargetLongitude = param6; } +void MavCmdConditionGate::pack() { + param1 = Geometry; + param2 = Altitude; + param5 = Latitude; + param6 = Longitude; + param7 = Altitude2; +} +void MavCmdConditionGate::unpack() { + Geometry = param1; + Altitude = param2; + Latitude = param5; + Longitude = param6; + Altitude2 = param7; +} +void MavCmdNavFenceReturnPoint::pack() { + param5 = Latitude; + param6 = Longitude; + param7 = Altitude; +} +void MavCmdNavFenceReturnPoint::unpack() { + Latitude = param5; + Longitude = param6; + Altitude = param7; +} +void MavCmdNavFencePolygonVertexInclusion::pack() { + param1 = VertexCount; + param5 = Latitude; + param6 = Longitude; +} +void MavCmdNavFencePolygonVertexInclusion::unpack() { + VertexCount = param1; + Latitude = param5; + Longitude = param6; +} +void MavCmdNavFencePolygonVertexExclusion::pack() { + param1 = VertexCount; + param5 = Latitude; + param6 = Longitude; +} +void MavCmdNavFencePolygonVertexExclusion::unpack() { + VertexCount = param1; + Latitude = param5; + Longitude = param6; +} +void MavCmdNavFenceCircleInclusion::pack() { + param1 = Radius; + param5 = Latitude; + param6 = Longitude; +} +void MavCmdNavFenceCircleInclusion::unpack() { + Radius = param1; + Latitude = param5; + Longitude = param6; +} +void MavCmdNavFenceCircleExclusion::pack() { + param1 = Radius; + param5 = Latitude; + param6 = Longitude; +} +void MavCmdNavFenceCircleExclusion::unpack() { + Radius = param1; + Latitude = param5; + Longitude = param6; +} +void MavCmdNavRallyPoint::pack() { + param5 = Latitude; + param6 = Longitude; + param7 = Altitude; +} +void MavCmdNavRallyPoint::unpack() { + Latitude = param5; + Longitude = param6; + Altitude = param7; +} +void MavCmdUavcanGetNodeInfo::pack() { +} +void MavCmdUavcanGetNodeInfo::unpack() { +} void MavCmdPayloadPrepareDeploy::pack() { param1 = OperationMode; - param2 = DesiredApproachVector; - param3 = DesiredGroundSpeed; - param4 = MinimumAltitudeClearance; + param2 = ApproachVector; + param3 = GroundSpeed; + param4 = AltitudeClearance; param5 = LatitudeUnscaledFor; param6 = LongitudeUnscaledFor; param7 = Altitude; } void MavCmdPayloadPrepareDeploy::unpack() { OperationMode = param1; - DesiredApproachVector = param2; - DesiredGroundSpeed = param3; - MinimumAltitudeClearance = param4; + ApproachVector = param2; + GroundSpeed = param3; + AltitudeClearance = param4; LatitudeUnscaledFor = param5; LongitudeUnscaledFor = param6; Altitude = param7; @@ -6425,6 +7012,9 @@ MavLinkMessageBase* MavLinkMessageBase::lookup(const MavLinkMessage& msg) { case MavLinkMessageIds::MAVLINK_MSG_ID_AUTH_KEY: result = new MavLinkAuthKey(); break; + case MavLinkMessageIds::MAVLINK_MSG_ID_LINK_NODE_STATUS: + result = new MavLinkLinkNodeStatus(); + break; case MavLinkMessageIds::MAVLINK_MSG_ID_SET_MODE: result = new MavLinkSetMode(); break; @@ -6524,6 +7114,9 @@ MavLinkMessageBase* MavLinkMessageBase::lookup(const MavLinkMessage& msg) { case MavLinkMessageIds::MAVLINK_MSG_ID_MISSION_REQUEST_INT: result = new MavLinkMissionRequestInt(); break; + case MavLinkMessageIds::MAVLINK_MSG_ID_MISSION_CHANGED: + result = new MavLinkMissionChanged(); + break; case MavLinkMessageIds::MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA: result = new MavLinkSafetySetAllowedArea(); break; @@ -6755,6 +7348,9 @@ MavLinkMessageBase* MavLinkMessageBase::lookup(const MavLinkMessage& msg) { case MavLinkMessageIds::MAVLINK_MSG_ID_LANDING_TARGET: result = new MavLinkLandingTarget(); break; + case MavLinkMessageIds::MAVLINK_MSG_ID_FENCE_STATUS: + result = new MavLinkFenceStatus(); + break; case MavLinkMessageIds::MAVLINK_MSG_ID_ESTIMATOR_STATUS: result = new MavLinkEstimatorStatus(); break; @@ -6770,6 +7366,9 @@ MavLinkMessageBase* MavLinkMessageBase::lookup(const MavLinkMessage& msg) { case MavLinkMessageIds::MAVLINK_MSG_ID_HIGH_LATENCY: result = new MavLinkHighLatency(); break; + case MavLinkMessageIds::MAVLINK_MSG_ID_HIGH_LATENCY2: + result = new MavLinkHighLatency2(); + break; case MavLinkMessageIds::MAVLINK_MSG_ID_VIBRATION: result = new MavLinkVibration(); break; @@ -6812,9 +7411,6 @@ MavLinkMessageBase* MavLinkMessageBase::lookup(const MavLinkMessage& msg) { case MavLinkMessageIds::MAVLINK_MSG_ID_DEBUG: result = new MavLinkDebug(); break; - case MavLinkMessageIds::MAVLINK_MSG_ID_PROTOCOL_VERSION: - result = new MavLinkProtocolVersion(); - break; default: break; } diff --git a/MavLinkCom/src/MavLinkNode.cpp b/MavLinkCom/src/MavLinkNode.cpp index f2eff75b6..ee45bf59b 100644 --- a/MavLinkCom/src/MavLinkNode.cpp +++ b/MavLinkCom/src/MavLinkNode.cpp @@ -75,6 +75,10 @@ AsyncResult MavLinkNode::waitForHeartbeat() { return pImpl->waitForHeartbeat(); } +void MavLinkNode::sendOneHeartbeat() { + return pImpl->sendOneHeartbeat(); +} + void MavLinkNode::setMessageInterval(int msgId, int frequency) { pImpl->setMessageInterval(msgId, frequency); diff --git a/MavLinkCom/src/impl/AdHocConnectionImpl.cpp b/MavLinkCom/src/impl/AdHocConnectionImpl.cpp index 33b9d1f93..e08ddc3a3 100644 --- a/MavLinkCom/src/impl/AdHocConnectionImpl.cpp +++ b/MavLinkCom/src/impl/AdHocConnectionImpl.cpp @@ -176,6 +176,7 @@ void AdHocConnectionImpl::unsubscribe(int id) void AdHocConnectionImpl::readPackets() { //CurrentThread::setMaximumPriority(); + CurrentThread::setThreadName("MavLinkThread"); std::shared_ptr safePort = this->port; const int MAXBUFFER = 512; uint8_t* buffer = new uint8_t[MAXBUFFER]; @@ -275,6 +276,7 @@ void AdHocConnectionImpl::drainQueue() void AdHocConnectionImpl::publishPackets() { //CurrentThread::setMaximumPriority(); + CurrentThread::setThreadName("MavLinkThread"); while (!closed) { drainQueue(); diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index a7f2b75f8..2e4655b7b 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -28,6 +28,7 @@ MavLinkConnectionImpl::MavLinkConnectionImpl() // todo: if we support signing then initialize // mavlink_intermediate_status_.signing callbacks } + std::string MavLinkConnectionImpl::getName() { return name; } @@ -85,6 +86,21 @@ std::shared_ptr MavLinkConnectionImpl::connectTcp(const std: return createConnection(nodeName, socket); } +void MavLinkConnectionImpl::acceptTcp(std::shared_ptr parent, const std::string& nodeName, const std::string& localAddr, int listeningPort) +{ + std::string local = localAddr; + close(); + std::shared_ptr socket = std::make_shared(); + + port = socket; // this is so that a call to close() can cancel this blocking accept call. + socket->accept(localAddr, listeningPort); + + socket->setNonBlocking(); + socket->setNoDelay(); + + parent->startListening(nodeName, socket); +} + std::shared_ptr MavLinkConnectionImpl::connectSerial(const std::string& nodeName, const std::string& portName, int baudRate, const std::string& initString) { std::shared_ptr serial = std::make_shared(); @@ -105,9 +121,11 @@ void MavLinkConnectionImpl::startListening(std::shared_ptr pa { name = nodeName; con_ = parent; - close(); + if (port != connectedPort) { + close(); + port = connectedPort; + } closed = false; - port = connectedPort; Utils::cleanupThread(read_thread); read_thread = std::thread{ &MavLinkConnectionImpl::readPackets, this }; @@ -254,7 +272,7 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg) int msglen = 0; if (entry != nullptr) { crc_extra = entry->crc_extra; - msglen = entry->msg_len; + msglen = entry->min_msg_len; } if (msg.msgid == MavLinkTelemetry::kMessageId) { msglen = 28; // mavlink doesn't know about our custom telemetry message. @@ -328,6 +346,7 @@ int MavLinkConnectionImpl::subscribe(MessageHandler handler) snapshot_stale = true; return entry.id; } + void MavLinkConnectionImpl::unsubscribe(int id) { std::lock_guard guard(listener_mutex); @@ -373,6 +392,7 @@ void MavLinkConnectionImpl::join(std::shared_ptr remote, bool void MavLinkConnectionImpl::readPackets() { //CurrentThread::setMaximumPriority(); + CurrentThread::setThreadName("MavLinkThread"); std::shared_ptr safePort = this->port; mavlink_message_t msg; mavlink_message_t msgBuffer; // intermediate state. @@ -410,8 +430,6 @@ void MavLinkConnectionImpl::readPackets() } else if (frame_state == MAVLINK_FRAMING_OK) { - int msgId = msg.msgid; - // pick up the sysid/compid of the remote node we are connected to. if (other_system_id == -1) { other_system_id = msg.sysid; @@ -536,6 +554,8 @@ void MavLinkConnectionImpl::drainQueue() void MavLinkConnectionImpl::publishPackets() { //CurrentThread::setMaximumPriority(); + CurrentThread::setThreadName("MavLinkThread"); + publish_thread_id_ = std::this_thread::get_id(); while (!closed) { drainQueue(); @@ -546,6 +566,11 @@ void MavLinkConnectionImpl::publishPackets() } } +bool MavLinkConnectionImpl::isPublishThread() const +{ + return std::this_thread::get_id() == publish_thread_id_; +} + void MavLinkConnectionImpl::getTelemetry(MavLinkTelemetry& result) { std::lock_guard guard(telemetry_mutex_); diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp index daa83dfc8..3f4325167 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp @@ -36,6 +36,7 @@ namespace mavlinkcom_impl { static std::shared_ptr connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort); static std::shared_ptr connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort); static std::shared_ptr connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort); + void acceptTcp(std::shared_ptr parent, const std::string& nodeName, const std::string& localAddr, int listeningPort); std::string getName(); int getTargetComponentId(); @@ -55,6 +56,7 @@ namespace mavlinkcom_impl { void getTelemetry(MavLinkTelemetry& result); void ignoreMessage(uint8_t message_id); int prepareForSending(MavLinkMessage& msg); + bool isPublishThread() const; private: static std::shared_ptr createConnection(const std::string& nodeName, std::shared_ptr port); void joinLeftSubscriber(std::shared_ptr remote, std::shared_ptrcon, const MavLinkMessage& msg); @@ -91,6 +93,7 @@ namespace mavlinkcom_impl { mavlink_utils::Semaphore msg_available_; bool waiting_for_msg_ = false; bool supports_mavlink2_ = false; + std::thread::id publish_thread_id_; bool signing_ = false; mavlink_status_t mavlink_intermediate_status_; mavlink_status_t mavlink_status_; diff --git a/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp b/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp index 78065788d..0fc7c86d6 100644 --- a/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp @@ -97,6 +97,7 @@ bool MavLinkFtpClientImpl::isSupported() { // request capabilities, it will respond with AUTOPILOT_VERSION. MavLinkAutopilotVersion ver; + assertNotPublishingThread(); if (!getCapabilities().wait(5000, &ver)) { throw std::runtime_error(Utils::stringf("Five second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES")); } diff --git a/MavLinkCom/src/impl/MavLinkNodeImpl.cpp b/MavLinkCom/src/impl/MavLinkNodeImpl.cpp index 3f5a005d2..4772c747f 100644 --- a/MavLinkCom/src/impl/MavLinkNodeImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkNodeImpl.cpp @@ -5,6 +5,7 @@ #include "Utils.hpp" #include "MavLinkMessages.hpp" #include "Semaphore.hpp" +#include "ThreadUtils.hpp" using namespace mavlink_utils; @@ -48,26 +49,9 @@ void MavLinkNodeImpl::startHeartbeat() void MavLinkNodeImpl::sendHeartbeat() { + CurrentThread::setThreadName("MavLinkThread"); while (heartbeat_running_) { - MavLinkHeartbeat heartbeat; - // send a heart beat so that the remote node knows we are still alive - // (otherwise drone will trigger a failsafe operation). - heartbeat.autopilot = static_cast(MAV_AUTOPILOT::MAV_AUTOPILOT_GENERIC); - heartbeat.type = static_cast(MAV_TYPE::MAV_TYPE_GCS); - heartbeat.mavlink_version = 3; - heartbeat.base_mode = 0; // ignored by PX4 - heartbeat.custom_mode = 0; // ignored by PX4 - heartbeat.system_status = 0; // ignored by PX4 - try - { - sendMessage(heartbeat); - } - catch (std::exception& e) - { - // ignore any failures here because we are running in our own thread here. - Utils::log(Utils::stringf("Caught and ignoring exception sending heartbeat: %s", e.what())); - } - + sendOneHeartbeat(); std::this_thread::sleep_for(std::chrono::milliseconds(heartbeatMilliseconds)); } } @@ -85,7 +69,7 @@ void MavLinkNodeImpl::handleMessage(std::shared_ptr connectio { req_cap_ = true; MavCmdRequestAutopilotCapabilities cmd{}; - cmd.p1 = 1; + cmd.param1 = 1; sendCommand(cmd); } break; @@ -95,7 +79,7 @@ void MavLinkNodeImpl::handleMessage(std::shared_ptr connectio break; } - // this is for the subclasses to play with. We put nothing here so we are not dependent on the + // this is for the subclasses to play with. We put nothing here so we are not dependent on the // subclasses remembering to call this base implementation. } @@ -136,6 +120,7 @@ AsyncResult MavLinkNodeImpl::getCapabilities() int subscription = con->subscribe([=](std::shared_ptr connection, const MavLinkMessage& m) { unused(connection); + unused(m); result.setResult(cap_); }); @@ -143,7 +128,7 @@ AsyncResult MavLinkNodeImpl::getCapabilities() // request capabilities, it will respond with AUTOPILOT_VERSION. MavCmdRequestAutopilotCapabilities cmd{}; - cmd.p1 = 1; + cmd.param1 = 1; sendCommand(cmd); return result; @@ -171,16 +156,40 @@ AsyncResult MavLinkNodeImpl::waitForHeartbeat() } }); result.setState(subscription); - + return result; } +void MavLinkNodeImpl::sendOneHeartbeat() +{ + MavLinkHeartbeat heartbeat; + // send a heart beat so that the remote node knows we are still alive + // (otherwise drone will trigger a failsafe operation). + heartbeat.autopilot = static_cast(MAV_AUTOPILOT::MAV_AUTOPILOT_GENERIC); + heartbeat.type = static_cast(MAV_TYPE::MAV_TYPE_GCS); + heartbeat.mavlink_version = 3; + heartbeat.base_mode = 0; // ignored by PX4 + heartbeat.custom_mode = 0; // ignored by PX4 + heartbeat.system_status = 0; // ignored by PX4 + try + { + sendMessage(heartbeat); + } + catch (std::exception& e) + { + // ignore any failures here because we are running in our own thread here. + Utils::log(Utils::stringf("Caught and ignoring exception sending heartbeat: %s", e.what())); + } +} + + + void MavLinkNodeImpl::setMessageInterval(int msgId, int frequency) { float intervalMicroseconds = 1000000.0f / frequency; MavCmdSetMessageInterval cmd{}; - cmd.TheMavlinkMessage = static_cast(msgId); - cmd.TheIntervalBetween = intervalMicroseconds; + cmd.MessageId = static_cast(msgId); + cmd.Interval = intervalMicroseconds; sendCommand(cmd); } @@ -288,6 +297,15 @@ float PackParameter(uint8_t type, float param_value) return pu.f; } +void MavLinkNodeImpl::assertNotPublishingThread() +{ + auto con = ensureConnection(); + if (con->isPublishThread()) + { + throw std::runtime_error("Cannot perform blocking operation on the connection publish thread"); + } +} + std::vector MavLinkNodeImpl::getParamList() { @@ -298,6 +316,8 @@ std::vector MavLinkNodeImpl::getParamList() size_t paramCount = 0; auto con = ensureConnection(); + assertNotPublishingThread(); + int subscription = con->subscribe([&](std::shared_ptr connection, const MavLinkMessage& message) { unused(connection); if (message.msgid == MavLinkParamValue::kMessageId) @@ -324,7 +344,7 @@ std::vector MavLinkNodeImpl::getParamList() } }); - //MAVLINK_MSG_ID_PARAM_REQUEST_LIST + //MAVLINK_MSG_ID_PARAM_REQUEST_LIST MavLinkParamRequestList cmd; cmd.target_system = getTargetSystemId(); cmd.target_component = getTargetComponentId(); @@ -345,7 +365,7 @@ std::vector MavLinkNodeImpl::getParamList() std::vector missing; for (size_t i = 0; i < paramCount; i++) - { + { // nested loop is inefficient, but it is needed because UDP also doesn't guarantee in-order delivery bool found = false; for (auto iter = result.begin(), end = result.end(); iter != end; iter++) @@ -497,6 +517,7 @@ AsyncResult MavLinkNodeImpl::setParameter(MavLinkParameter p) size++; // we can include the null terminator. } auto con = ensureConnection(); + assertNotPublishingThread(); AsyncResult result([=](int state) { con->unsubscribe(state); }); @@ -571,8 +592,9 @@ void MavLinkNodeImpl::sendCommand(MavLinkCommand& command) try { sendMessage(cmd); } - catch (std::exception e) { + catch (const std::exception& e) { // silently fail since we are on a background thread here... + unused(e); } } diff --git a/MavLinkCom/src/impl/MavLinkNodeImpl.hpp b/MavLinkCom/src/impl/MavLinkNodeImpl.hpp index c1c6f9f0b..295c3f2e6 100644 --- a/MavLinkCom/src/impl/MavLinkNodeImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkNodeImpl.hpp @@ -15,7 +15,7 @@ namespace mavlinkcom_impl { { public: MavLinkNodeImpl(int localSystemId, int localComponentId); - ~MavLinkNodeImpl(); + virtual ~MavLinkNodeImpl(); void connect(std::shared_ptr connection); void close(); @@ -69,6 +69,8 @@ namespace mavlinkcom_impl { AsyncResult waitForHeartbeat(); + void sendOneHeartbeat(); + // Encode and send the given message to the connected node void sendMessage(MavLinkMessageBase& msg); @@ -84,6 +86,7 @@ namespace mavlinkcom_impl { protected: // this is called for all messages received on the connection. virtual void handleMessage(std::shared_ptr connection, const MavLinkMessage& message); + void assertNotPublishingThread(); private: void sendHeartbeat(); AsyncResult getParameterByIndex(int16_t index); diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp index 738d98987..53a366037 100644 --- a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp @@ -486,7 +486,7 @@ void MavLinkVehicleImpl::writeMessage(MavLinkMessageBase& msg, bool update_stats AsyncResult MavLinkVehicleImpl::armDisarm(bool arm) { MavCmdComponentArmDisarm cmd{}; - cmd.p1ToArm = arm ? 1.0f : 0.0f; + cmd.Arm = arm ? 1.0f : 0.0f; return sendCommandAndWaitForAck(cmd); } @@ -497,8 +497,8 @@ AsyncResult MavLinkVehicleImpl::takeoff(float z, float pitch, float yaw) float targetAlt = vehicle_state_.home.global_pos.alt - deltaZ; Utils::log(Utils::stringf("Take off to %f", targetAlt)); MavCmdNavTakeoff cmd{}; - cmd.MinimumPitch = pitch; - cmd.YawAngle = yaw; + cmd.Pitch = pitch; + cmd.Yaw = yaw; cmd.Latitude = INFINITY; cmd.Longitude = INFINITY; cmd.Altitude = targetAlt; @@ -531,10 +531,10 @@ AsyncResult MavLinkVehicleImpl::land(float yaw, float lat, float lon, floa { MavCmdNavLand cmd{}; cmd.AbortAlt = 1; - cmd.DesiredYawAngle = yaw; + cmd.YawAngle = yaw; cmd.Latitude = lat; cmd.Longitude = lon; - cmd.Altitude = altitude; + cmd.LandingAltitude = altitude; return sendCommandAndWaitForAck(cmd); } @@ -547,7 +547,8 @@ AsyncResult MavLinkVehicleImpl::returnToHome() bool MavLinkVehicleImpl::getRcSwitch(int channel, float threshold) { if (threshold < -1 || threshold > 1) { - throw std::runtime_error(Utils::stringf("RC channel threshold is out of range, expecting -1 < threshold < 1, but got %f", threshold)); + auto msg = Utils::stringf("RC channel threshold is out of range, expecting -1 < threshold < 1, but got %f", threshold); + throw std::runtime_error(msg); } // if threshold < 0 then the threshold is inverted. if (channel > 0 && channel < 18) { @@ -585,7 +586,7 @@ void MavLinkVehicleImpl::releaseControl() control_request_sent_ = false; vehicle_state_.controls.offboard = false; MavCmdNavGuidedEnable cmd{}; - cmd.OnOff = 0; + cmd.Enable = 0; sendCommand(cmd); } @@ -612,7 +613,7 @@ void MavLinkVehicleImpl::checkOffboard() // now the command should succeed. bool r = false; MavCmdNavGuidedEnable cmd{}; - cmd.OnOff = 1; + cmd.Enable = 1; // Note: we can't wait for ACK here, I've tried it. The ACK takes too long to get back to // us by which time the PX4 times out offboard mode!! sendCommand(cmd); @@ -694,7 +695,7 @@ AsyncResult MavLinkVehicleImpl::setMode(int mode, int customMode, int cust MavCmdDoSetMode cmd{}; cmd.Mode = static_cast(mode); cmd.CustomMode = static_cast(customMode); - cmd.CustomSubMode = static_cast(customSubMode); + cmd.CustomSubmode = static_cast(customSubMode); return sendCommandAndWaitForAck(cmd); } @@ -738,8 +739,9 @@ AsyncResult MavLinkVehicleImpl::loiter() bool MavLinkVehicleImpl::isLocalControlSupported() { MavLinkAutopilotVersion cap; + assertNotPublishingThread(); if (!getCapabilities().wait(2000, &cap)) { - throw std::runtime_error(Utils::stringf("Two second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES")); + throw std::runtime_error("Two second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES"); } return (cap.capabilities & static_cast(MAV_PROTOCOL_CAPABILITY::MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED)) != 0; } @@ -838,8 +840,9 @@ void MavLinkVehicleImpl::moveToLocalPosition(float x, float y, float z, bool isY bool MavLinkVehicleImpl::isAttitudeControlSupported() { MavLinkAutopilotVersion cap; + assertNotPublishingThread(); if (!getCapabilities().wait(5000, &cap)) { - throw std::runtime_error(Utils::stringf("Five second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES")); + throw std::runtime_error("Five second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES"); } return (cap.capabilities & static_cast(MAV_PROTOCOL_CAPABILITY::MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET)) != 0; } diff --git a/MavLinkCom/src/impl/MavLinkVideoStreamImpl.cpp b/MavLinkCom/src/impl/MavLinkVideoStreamImpl.cpp index 6fa119a4f..a4d08a101 100644 --- a/MavLinkCom/src/impl/MavLinkVideoStreamImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkVideoStreamImpl.cpp @@ -102,9 +102,9 @@ bool MavLinkVideoClientImpl::readNextFrame(MavLinkVideoClient::MavLinkVideoFrame void MavLinkVideoClientImpl::requestVideo(int camera_id, float every_n_sec, bool save_locally) { MavCmdDoControlVideo cmd{}; - cmd.CameraId = static_cast(camera_id); + cmd.Id = static_cast(camera_id); cmd.Transmission = 1.0f; - cmd.TransmissionMode = every_n_sec; + cmd.Interval = every_n_sec; cmd.Recording = save_locally ? 1.0f : 0.0f; sendCommand(cmd); } diff --git a/MavLinkCom/src/serial_com/SerialPort.hpp b/MavLinkCom/src/serial_com/SerialPort.hpp index 49101d36f..9ed32d5d1 100644 --- a/MavLinkCom/src/serial_com/SerialPort.hpp +++ b/MavLinkCom/src/serial_com/SerialPort.hpp @@ -37,7 +37,7 @@ class SerialPort : public Port { public: SerialPort(); - ~SerialPort(); + virtual ~SerialPort(); // open the serial port virtual int connect(const char* portName, int baudRate); diff --git a/MavLinkCom/src/serial_com/TcpClientPort.cpp b/MavLinkCom/src/serial_com/TcpClientPort.cpp index ad9b21440..10568ba6a 100644 --- a/MavLinkCom/src/serial_com/TcpClientPort.cpp +++ b/MavLinkCom/src/serial_com/TcpClientPort.cpp @@ -23,14 +23,20 @@ using namespace mavlink_utils; typedef int socklen_t; static bool socket_initialized_ = false; +inline int GetSocketError() +{ + return WSAGetLastError(); +} #else // posix -#include +#include #include #include #include +#include #include +#include #include #include #include @@ -39,7 +45,7 @@ typedef int SOCKET; const int INVALID_SOCKET = -1; const int ERROR_ACCESS_DENIED = EACCES; -inline int WSAGetLastError() { +inline int GetSocketError() { return errno; } const int SOCKET_ERROR = -1; @@ -51,6 +57,7 @@ class TcpClientPort::TcpSocketImpl { SocketInit init; SOCKET sock = INVALID_SOCKET; + SOCKET accept_sock = INVALID_SOCKET; sockaddr_in localaddr; sockaddr_in remoteaddr; bool closed_ = true; @@ -80,7 +87,8 @@ class TcpClientPort::TcpSocketImpl std::string serviceName = std::to_string(port); int rc = getaddrinfo(ipAddress.c_str(), serviceName.c_str(), &hints, &result); if (rc != 0) { - throw std::runtime_error(Utils::stringf("TcpClientPort getaddrinfo failed with error: %d\n", rc)); + auto msg = Utils::stringf("TcpClientPort getaddrinfo failed with error: %d\n", rc); + throw std::runtime_error(msg); } for (struct addrinfo *ptr = result; ptr != NULL; ptr = ptr->ai_next) { @@ -98,7 +106,8 @@ class TcpClientPort::TcpSocketImpl freeaddrinfo(result); if (!found) { - throw std::runtime_error(Utils::stringf("TcpClientPort could not resolve ip address for '%s:%d'\n", ipAddress.c_str(), port)); + auto msg = Utils::stringf("TcpClientPort could not resolve ip address for '%s:%d'\n", ipAddress.c_str(), port); + throw std::runtime_error(msg); } } @@ -114,14 +123,16 @@ class TcpClientPort::TcpSocketImpl int rc = bind(sock, reinterpret_cast(&localaddr), addrlen); if (rc < 0) { - int hr = WSAGetLastError(); - throw std::runtime_error(Utils::stringf("TcpClientPort socket bind failed with error: %d\n", hr)); + int hr = GetSocketError(); + auto msg = Utils::stringf("TcpClientPort socket bind failed with error: %d\n", hr); + throw std::runtime_error(msg); } rc = ::connect(sock, reinterpret_cast(&remoteaddr), addrlen); if (rc != 0) { - int hr = WSAGetLastError(); - throw std::runtime_error(Utils::stringf("TcpClientPort socket connect failed with error: %d\n", hr)); + int hr = GetSocketError(); + auto msg = Utils::stringf("TcpClientPort socket connect failed with error: %d\n", hr); + throw std::runtime_error(msg); } closed_ = false; @@ -130,50 +141,119 @@ class TcpClientPort::TcpSocketImpl void accept(const std::string& localHost, int localPort) { - SOCKET local = socket(AF_INET, SOCK_STREAM, 0); + accept_sock = socket(AF_INET, SOCK_STREAM, 0); resolveAddress(localHost, localPort, localaddr); // bind socket to local address. socklen_t addrlen = sizeof(sockaddr_in); - int rc = ::bind(local, reinterpret_cast(&localaddr), addrlen); + int rc = ::bind(accept_sock, reinterpret_cast(&localaddr), addrlen); if (rc < 0) { - int hr = WSAGetLastError(); - throw std::runtime_error(Utils::stringf("TcpClientPort socket bind failed with error: %d\n", hr)); + int hr = GetSocketError(); + auto msg = Utils::stringf("TcpClientPort socket bind failed with error: %d\n", hr); + throw std::runtime_error(msg); } // start listening for incoming connection - rc = ::listen(local, 1); + rc = ::listen(accept_sock, 1); if (rc < 0) { - int hr = WSAGetLastError(); - throw std::runtime_error(Utils::stringf("TcpClientPort socket listen failed with error: %d\n", hr)); + int hr = GetSocketError(); + auto msg = Utils::stringf("TcpClientPort socket listen failed with error: %d\n", hr); + throw std::runtime_error(msg); } // accept 1 - sock = ::accept(local, reinterpret_cast(&remoteaddr), &addrlen); + sock = ::accept(accept_sock, reinterpret_cast(&remoteaddr), &addrlen); if (sock == INVALID_SOCKET) { - int hr = WSAGetLastError(); - throw std::runtime_error(Utils::stringf("TcpClientPort accept failed with error: %d\n", hr)); + int hr = GetSocketError(); + auto msg = Utils::stringf("TcpClientPort accept failed with error: %d\n", hr); + throw std::runtime_error(msg); } +#ifdef _WIN32 + // don't need to accept any more, so we can close this one. + ::closesocket(accept_sock); +#else + int fd = static_cast(accept_sock); + ::close(fd); +#endif + accept_sock = INVALID_SOCKET; + closed_ = false; } + void setNonBlocking() + { +#ifdef _WIN32 + unsigned long mode = 1; + int rc = ioctlsocket(sock, FIONBIO, &mode); +#else + int fd = static_cast(sock); + int flags = fcntl(fd, F_GETFL, 0); + if (flags == -1) { + auto msg = Utils::stringf("fcntl failed with error: %d\n", errno); + throw std::runtime_error(msg); + } + flags |= O_NONBLOCK; + int rc = fcntl(fd, F_SETFL, flags); +#endif + if (rc != 0) { + rc = GetSocketError(); + auto msg = Utils::stringf("TcpClientPort setNonBlocking failed with error: %d\n", rc); + throw std::runtime_error(msg); + } + } + + void setNoDelay() + { + int flags = 1; +#ifdef _WIN32 + int rc = ::setsockopt(sock, IPPROTO_TCP, TCP_NODELAY, reinterpret_cast(&flags), sizeof(flags)); +#else + int fd = static_cast(sock); + int rc = ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, reinterpret_cast(&flags), sizeof(flags)); +#endif + + if (rc != 0) + { + rc = GetSocketError(); + auto msg = Utils::stringf("TcpClientPort set TCP_NODELAY failed: %d\n", rc); + throw std::runtime_error(msg); + } + } + // write to the serial port int write(const uint8_t* ptr, int count) { - socklen_t addrlen = sizeof(sockaddr_in); int hr = send(sock, reinterpret_cast(ptr), count, 0); if (hr == SOCKET_ERROR) { - throw std::runtime_error(Utils::stringf("TcpClientPort socket send failed with error: %d\n", hr)); + hr = checkerror(); + auto msg = Utils::stringf("TcpClientPort socket send failed with error: %d\n", hr); + throw std::runtime_error(msg); } return hr; } + int checkerror() { + int hr = GetSocketError(); +#ifdef _WIN32 + if (hr == WSAECONNRESET) + { + close(); + } +#else + if (hr == ECONNREFUSED || hr == ENOTCONN) + { + close(); + } +#endif + return hr; + } + int read(uint8_t* result, int bytesToRead) { int bytesRead = 0; @@ -181,28 +261,31 @@ class TcpClientPort::TcpSocketImpl while (!closed_) { - socklen_t addrlen = sizeof(sockaddr_in); int rc = recv(sock, reinterpret_cast(result), bytesToRead, 0); if (rc < 0) { + int hr = checkerror(); #ifdef _WIN32 - int hr = WSAGetLastError(); if (hr == WSAEMSGSIZE) { - // message was too large for the buffer, no problem, return what we have. + // message was too large for the buffer, no problem, return what we have. } - else if (hr == WSAECONNRESET || hr == ERROR_IO_PENDING) + else if (hr == ERROR_IO_PENDING) { // try again - this can happen if server recreates the socket on their side. continue; } + else if (hr == WSAEINTR) + { + // skip this, it is was interrupted, and if user is closing the port closed_ will be true. + continue; + } else #else - int hr = errno; if (hr == EINTR) { - // skip this, it is was interrupted. - continue; + // try again - this can happen if server recreates the socket on their side. + continue; } else #endif @@ -213,7 +296,6 @@ class TcpClientPort::TcpSocketImpl if (rc == 0) { - //printf("Connection closed\n"); return -1; } else @@ -227,16 +309,28 @@ class TcpClientPort::TcpSocketImpl void close() { - if (!closed_) { - closed_ = true; + closed_ = true; + if (sock != INVALID_SOCKET) + { +#ifdef _WIN32 + closesocket(sock); +#else + int fd = static_cast(sock); + ::close(fd); +#endif + sock = INVALID_SOCKET; + } + if (accept_sock != INVALID_SOCKET) + { #ifdef _WIN32 - closesocket(sock); + closesocket(accept_sock); #else - int fd = static_cast(sock); - ::close(fd); + int fd = static_cast(accept_sock); + ::close(fd); #endif - } + accept_sock = INVALID_SOCKET; + } } std::string remoteAddress() { @@ -305,4 +399,14 @@ int TcpClientPort::remotePort() int TcpClientPort::getRssi(const char* ifaceName) { return impl_->getRssi(ifaceName); -} \ No newline at end of file +} + +void TcpClientPort::setNoDelay() +{ + impl_->setNoDelay(); +} + +void TcpClientPort::setNonBlocking() +{ + impl_->setNonBlocking(); +} diff --git a/MavLinkCom/src/serial_com/TcpClientPort.hpp b/MavLinkCom/src/serial_com/TcpClientPort.hpp index f84a11679..bcca77138 100644 --- a/MavLinkCom/src/serial_com/TcpClientPort.hpp +++ b/MavLinkCom/src/serial_com/TcpClientPort.hpp @@ -10,7 +10,7 @@ class TcpClientPort : public Port { public: TcpClientPort(); - ~TcpClientPort(); + virtual ~TcpClientPort(); // Connect can set you up two different ways. Pass 0 for local port to get any free local // port. localHost allows you to be specific about which local adapter to use in case you @@ -34,6 +34,9 @@ class TcpClientPort : public Port std::string remoteAddress(); int remotePort(); + void setNonBlocking(); + void setNoDelay(); + private: class TcpSocketImpl; std::unique_ptr impl_; diff --git a/MavLinkCom/src/serial_com/UdpClientPort.cpp b/MavLinkCom/src/serial_com/UdpClientPort.cpp index 8024dd3bb..833461ba5 100644 --- a/MavLinkCom/src/serial_com/UdpClientPort.cpp +++ b/MavLinkCom/src/serial_com/UdpClientPort.cpp @@ -23,6 +23,9 @@ using namespace mavlink_utils; typedef int socklen_t; static bool socket_initialized_ = false; +inline int GetSocketError() { + return WSAGetLastError(); +} #else // posix @@ -39,7 +42,7 @@ typedef int SOCKET; const int INVALID_SOCKET = -1; const int ERROR_ACCESS_DENIED = EACCES; -inline int WSAGetLastError() { +inline int GetSocketError() { return errno; } const int SOCKET_ERROR = -1; @@ -82,7 +85,8 @@ class UdpClientPort::UdpSocketImpl std::string serviceName = std::to_string(port); int rc = getaddrinfo(ipAddress.c_str(), serviceName.c_str(), &hints, &result); if (rc != 0) { - throw std::runtime_error(Utils::stringf("UdpClientPort getaddrinfo failed with error: %d\n", rc)); + auto msg = Utils::stringf("UdpClientPort getaddrinfo failed with error: %d\n", rc); + throw std::runtime_error(msg); } for (struct addrinfo *ptr = result; ptr != NULL; ptr = ptr->ai_next) { @@ -100,8 +104,8 @@ class UdpClientPort::UdpSocketImpl freeaddrinfo(result); if (!found) { - - throw std::runtime_error(Utils::stringf("UdpClientPort could not resolve ip address for '%s:%d'\n", ipAddress.c_str(), port)); + auto msg = Utils::stringf("UdpClientPort could not resolve ip address for '%s:%d'\n", ipAddress.c_str(), port); + throw std::runtime_error(msg); } } @@ -125,8 +129,9 @@ class UdpClientPort::UdpSocketImpl int rc = bind(sock, reinterpret_cast(&localaddr), addrlen); if (rc < 0) { - int hr = WSAGetLastError(); - throw std::runtime_error(Utils::stringf("UdpClientPort socket bind failed with error: %d\n", hr)); + int hr = GetSocketError(); + auto msg = Utils::stringf("UdpClientPort socket bind failed with error: %d\n", hr); + throw std::runtime_error(msg); return hr; } @@ -136,9 +141,10 @@ class UdpClientPort::UdpSocketImpl rc = ::connect(sock, reinterpret_cast(&remoteaddr), addrlen); if (rc < 0) { - int hr = WSAGetLastError(); - throw std::runtime_error(Utils::stringf("UdpClientPort socket could not connect to remote host at %s:%d, error: %d\n", - remoteHost.c_str(), remotePort, hr)); + int hr = GetSocketError(); + auto msg = Utils::stringf("UdpClientPort socket could not connect to remote host at %s:%d, error: %d\n", + remoteHost.c_str(), remotePort, hr); + throw std::runtime_error(msg); return hr; } } @@ -146,6 +152,22 @@ class UdpClientPort::UdpSocketImpl return 0; } + int checkerror() { + int hr = GetSocketError(); +#ifdef _WIN32 + if (hr == WSAECONNRESET) + { + close(); + } +#else + if (hr == ECONNREFUSED || hr == ENOTCONN) + { + close(); + } +#endif + return hr; + } + // write to the serial port int write(const uint8_t* ptr, int count) { @@ -164,10 +186,11 @@ class UdpClientPort::UdpSocketImpl #endif if (hr == SOCKET_ERROR) { - hr = WSAGetLastError(); + hr = checkerror(); // perhaps the client is gone, and may want to come back on a different port, in which case let's reset our remote port to allow that. remoteaddr.sin_port = 0; - throw std::runtime_error(Utils::stringf("UdpClientPort socket send failed with error: %d\n", hr)); + auto msg = Utils::stringf("UdpClientPort socket send failed with error: %d\n", hr); + throw std::runtime_error(msg); } return hr; @@ -186,13 +209,13 @@ class UdpClientPort::UdpSocketImpl int rc = recvfrom(sock, reinterpret_cast(result), bytesToRead, 0, reinterpret_cast(&other), &addrlen); if (rc < 0) { - int hr = WSAGetLastError(); + int hr = checkerror(); #ifdef _WIN32 if (hr == WSAEMSGSIZE) { // message was too large for the buffer, no problem, return what we have. } - else if (hr == WSAECONNRESET || hr == ERROR_IO_PENDING) + else if (hr == ERROR_IO_PENDING) { // try again - this can happen if server recreates the socket on their side. continue; @@ -208,10 +231,6 @@ class UdpClientPort::UdpSocketImpl // skip this, it is was interrupted, and if user is closing the port closed_ will be true. continue; } - else if (hr == ECONNRESET) { - // try again - this can happen if server recreates the socket on their side. - continue; - } else #endif { @@ -235,7 +254,6 @@ class UdpClientPort::UdpSocketImpl if (rc == 0) { - //printf("Connection closed\n"); return -1; } else diff --git a/MavLinkCom/src/serial_com/UdpClientPort.hpp b/MavLinkCom/src/serial_com/UdpClientPort.hpp index 9908ee5e6..0c83c494a 100644 --- a/MavLinkCom/src/serial_com/UdpClientPort.hpp +++ b/MavLinkCom/src/serial_com/UdpClientPort.hpp @@ -10,7 +10,7 @@ class UdpClientPort : public Port { public: UdpClientPort(); - ~UdpClientPort(); + virtual ~UdpClientPort(); // Connect can set you up two different ways. Pass 0 for local port to get any free local // port and pass a fixed remotePort if you want to send to a specific remote port. diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index 8bdce564a..172d01286 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -58,6 +58,7 @@ Code + @@ -66,7 +67,6 @@ Code - Code diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index fe141d380..5f5793052 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -15,42 +15,118 @@ def __init__(self, ip = "", port = 41451, timeout_value = 3600): if (ip == ""): ip = "127.0.0.1" self.client = msgpackrpc.Client(msgpackrpc.Address(ip, port), timeout = timeout_value, pack_encoding = 'utf-8', unpack_encoding = 'utf-8') - + # ----------------------------------- Common vehicle APIs --------------------------------------------- def reset(self): + """ + Reset the vehicle to its original starting state + + Note that you must call `enableApiControl` and `armDisarm` again after the call to reset + """ self.client.call('reset') def ping(self): + """ + If connection is established then this call will return true otherwise it will be blocked until timeout + + Returns: + bool: + """ return self.client.call('ping') def getClientVersion(self): return 1 # sync with C++ client + def getServerVersion(self): return self.client.call('getServerVersion') + def getMinRequiredServerVersion(self): return 1 # sync with C++ client + def getMinRequiredClientVersion(self): return self.client.call('getMinRequiredClientVersion') # basic flight control def enableApiControl(self, is_enabled, vehicle_name = ''): - return self.client.call('enableApiControl', is_enabled, vehicle_name) + """ + Enables or disables API control for vehicle corresponding to vehicle_name + + Args: + is_enabled (bool): True to enable, False to disable API control + vehicle_name (str, optional): Name of the vehicle to send this command to + """ + self.client.call('enableApiControl', is_enabled, vehicle_name) + def isApiControlEnabled(self, vehicle_name = ''): + """ + Returns true if API control is established. + + If false (which is default) then API calls would be ignored. After a successful call to `enableApiControl`, `isApiControlEnabled` should return true. + + Args: + vehicle_name (str, optional): Name of the vehicle + + Returns: + bool: If API control is enabled + """ return self.client.call('isApiControlEnabled', vehicle_name) + def armDisarm(self, arm, vehicle_name = ''): + """ + Arms or disarms vehicle + + Args: + arm (bool): True to arm, False to disarm the vehicle + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + bool: Success + """ return self.client.call('armDisarm', arm, vehicle_name) - + def simPause(self, is_paused): + """ + Pauses simulation + + Args: + is_paused (bool): True to pause the simulation, False to release + """ self.client.call('simPause', is_paused) + def simIsPause(self): + """ + Returns true if the simulation is paused + + Returns: + bool: If the simulation is paused + """ return self.client.call("simIsPaused") + def simContinueForTime(self, seconds): + """ + Continue the simulation for the specified number of seconds + + Args: + seconds (float): Time to run the simulation for + """ self.client.call('simContinueForTime', seconds) def getHomeGeoPoint(self, vehicle_name = ''): + """ + Get the Home location of the vehicle + + Args: + vehicle_name (str, optional): Name of vehicle to get home location of + + Returns: + GeoPoint: Home location of the vehicle + """ return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint', vehicle_name)) def confirmConnection(self): + """ + Checks state of connection every 1 sec and reports it in Console so user can see the progress for connection. + """ if self.ping(): print("Connected!") else: @@ -59,7 +135,7 @@ def confirmConnection(self): client_ver = self.getClientVersion() server_min_ver = self.getMinRequiredServerVersion() client_min_ver = self.getMinRequiredClientVersion() - + ver_info = "Client Ver:" + str(client_ver) + " (Min Req: " + str(client_min_ver) + \ "), Server Ver:" + str(server_ver) + " (Min Req: " + str(server_min_ver) + ")" @@ -73,20 +149,84 @@ def confirmConnection(self): print(ver_info) print('') + def simSwapTextures(self, tags, tex_id = 0, component_id = 0, material_id = 0): + """ + Runtime Swap Texture API + + See https://microsoft.github.io/AirSim/retexturing/ for details + + Args: + tags (str): string of "," or ", " delimited tags to identify on which actors to perform the swap + tex_id (int, optional): indexes the array of textures assigned to each actor undergoing a swap + + If out-of-bounds for some object's texture set, it will be taken modulo the number of textures that were available + component_id (int, optional): + material_id (int, optional): + + Returns: + list[str]: List of objects which matched the provided tags and had the texture swap perfomed + """ + return self.client.call("simSwapTextures", tags, tex_id, component_id, material_id) + # time-of-day control def simSetTimeOfDay(self, is_enabled, start_datetime = "", is_start_datetime_dst = False, celestial_clock_speed = 1, update_interval_secs = 60, move_sun = True): - return self.client.call('simSetTimeOfDay', is_enabled, start_datetime, is_start_datetime_dst, celestial_clock_speed, update_interval_secs, move_sun) + """ + Control the position of Sun in the environment + + Sun's position is computed using the coordinates specified in `OriginGeopoint` in settings for the date-time specified in the argument, + else if the string is empty, current date & time is used + + Args: + is_enabled (bool): True to enable time-of-day effect, False to reset the position to original + start_datetime (str, optional): Date & Time in %Y-%m-%d %H:%M:%S format, e.g. `2018-02-12 15:20:00` + is_start_datetime_dst (bool, optional): True to adjust for Daylight Savings Time + celestial_clock_speed (float, optional): Run celestial clock faster or slower than simulation clock + E.g. Value 100 means for every 1 second of simulation clock, Sun's position is advanced by 100 seconds + so Sun will move in sky much faster + update_interval_secs (float, optional): Interval to update the Sun's position + move_sun (bool, optional): Whether or not to move the Sun + """ + self.client.call('simSetTimeOfDay', is_enabled, start_datetime, is_start_datetime_dst, celestial_clock_speed, update_interval_secs, move_sun) # weather def simEnableWeather(self, enable): - return self.client.call('simEnableWeather', enable) + """ + Enable Weather effects. Needs to be called before using `simSetWeatherParameter` API + + Args: + enable (bool): True to enable, False to disable + """ + self.client.call('simEnableWeather', enable) + def simSetWeatherParameter(self, param, val): - return self.client.call('simSetWeatherParameter', param, val) + """ + Enable various weather effects + + Args: + param (WeatherParameter): Weather effect to be enabled + val (float): Intensity of the effect, Range 0-1 + """ + self.client.call('simSetWeatherParameter', param, val) # camera control # simGetImage returns compressed png in array of bytes # image_type uses one of the ImageType members def simGetImage(self, camera_name, image_type, vehicle_name = ''): + """ + Get a single image + + Returns bytes of png format image which can be dumped into abinary file to create .png image + `string_to_uint8_array()` can be used to convert into Numpy unit8 array + See https://microsoft.github.io/AirSim/image_apis/ for details + + Args: + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + image_type (ImageType): Type of image required + vehicle_name (str, optional): Name of the vehicle with the camera + + Returns: + Binary string literal of compressed png image + """ # todo: in future remove below, it's only for compatibility to pre v1.2 camera_name = str(camera_name) @@ -100,172 +240,464 @@ def simGetImage(self, camera_name, image_type, vehicle_name = ''): # simGetImage returns compressed png in array of bytes # image_type uses one of the ImageType members def simGetImages(self, requests, vehicle_name = ''): + """ + Get multiple images + + See https://microsoft.github.io/AirSim/image_apis/ for details and examples + + Args: + requests (list[ImageRequest]): Images required + vehicle_name (str, optional): Name of vehicle associated with the camera + + Returns: + list[ImageResponse]: + """ responses_raw = self.client.call('simGetImages', requests, vehicle_name) return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw] + # gets the static meshes in the unreal scene + def simGetMeshPositionVertexBuffers(self): + """ + Returns the static meshes that make up the scene + + See https://microsoft.github.io/AirSim/meshes/ for details and how to use this + + Returns: + list[MeshPositionVertexBuffersResponse]: + """ + responses_raw = self.client.call('simGetMeshPositionVertexBuffers') + return [MeshPositionVertexBuffersResponse.from_msgpack(response_raw) for response_raw in responses_raw] + def simGetCollisionInfo(self, vehicle_name = ''): + """ + Args: + vehicle_name (str, optional): Name of the Vehicle to get the info of + + Returns: + CollisionInfo: + """ return CollisionInfo.from_msgpack(self.client.call('simGetCollisionInfo', vehicle_name)) def simSetVehiclePose(self, pose, ignore_collison, vehicle_name = ''): + """ + Set the pose of the vehicle + + If you don't want to change position (or orientation) then just set components of position (or orientation) to floating point nan values + + Args: + pose (Pose): Desired Pose pf the vehicle + ignore_collision (bool): Whether to ignore any collision or not + vehicle_name (str, optional): Name of the vehicle to move + """ self.client.call('simSetVehiclePose', pose, ignore_collison, vehicle_name) + def simGetVehiclePose(self, vehicle_name = ''): + """ + Args: + vehicle_name (str, optional): Name of the vehicle to get the Pose of + + Returns: + Pose: + """ pose = self.client.call('simGetVehiclePose', vehicle_name) return Pose.from_msgpack(pose) + + def simSetTraceLine(self, color_rgba, thickness=1.0, vehicle_name = ''): + """ + Modify the color and thickness of the line when Tracing is enabled + + Tracing can be enabled by pressing T in the Editor or setting `EnableTrace` to `True` in the Vehicle Settings + + Args: + color_rgba (list): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of the line + vehicle_name (string, optional): Name of the vehicle to set Trace line values for + """ + self.client.call('simSetTraceLine', color_rgba, thickness, vehicle_name) + def simGetObjectPose(self, object_name): + """ + Args: + object_name (str): Object to get the Pose of + + Returns: + Pose: + """ pose = self.client.call('simGetObjectPose', object_name) return Pose.from_msgpack(pose) + def simSetObjectPose(self, object_name, pose, teleport = True): + """ + Set the pose of the object(actor) in the environment + + The specified actor must have Mobility set to movable, otherwise there will be undefined behaviour. + See https://www.unrealengine.com/en-US/blog/moving-physical-objects for details on how to set Mobility and the effect of Teleport parameter + + Args: + object_name (str): Name of the object(actor) to move + pose (Pose): Desired Pose of the object + teleport (bool, optional): Whether to move the object immediately without affecting their velocity + + Returns: + bool: If the move was successful + """ return self.client.call('simSetObjectPose', object_name, pose, teleport) def simListSceneObjects(self, name_regex = '.*'): + """ + Lists the objects present in the environment + + Default behaviour is to list all objects, regex can be used to return smaller list of matching objects or actors + + Args: + name_regex (str, optional): String to match actor names against, e.g. "Cylinder.*" + + Returns: + list[str]: List containing all the names + """ return self.client.call('simListSceneObjects', name_regex) def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False): + """ + Set segmentation ID for specific objects + + See https://microsoft.github.io/AirSim/image_apis/#segmentation for details + + Args: + mesh_name (str): Name of the mesh to set the ID of (supports regex) + object_id (int): Object ID to be set, range 0-255 + + RBG values for IDs can be seen at https://microsoft.github.io/AirSim/seg_rgbs.txt + is_name_regex (bool, optional): Whether the mesh name is a regex + + Returns: + bool: If the mesh was found + """ return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex) + def simGetSegmentationObjectID(self, mesh_name): + """ + Returns Object ID for the given mesh name + + Mapping of Object IDs to RGB values can be seen at https://microsoft.github.io/AirSim/seg_rgbs.txt + + Args: + mesh_name (str): Name of the mesh to get the ID of + """ return self.client.call('simGetSegmentationObjectID', mesh_name) + def simPrintLogMessage(self, message, message_param = "", severity = 0): - return self.client.call('simPrintLogMessage', message, message_param, severity) + """ + Prints the specified message in the simulator's window. + + If message_param is supplied, then it's printed next to the message and in that case if this API is called with same message value + but different message_param again then previous line is overwritten with new line (instead of API creating new line on display). + + For example, `simPrintLogMessage("Iteration: ", to_string(i))` keeps updating same line on display when API is called with different values of i. + The valid values of severity parameter is 0 to 3 inclusive that corresponds to different colors. + + Args: + message (str): Message to be printed + message_param (str, optional): Parameter to be printed next to the message + severity (int, optional): Range 0-3, inclusive, corresponding to the severity of the message + """ + self.client.call('simPrintLogMessage', message, message_param, severity) def simGetCameraInfo(self, camera_name, vehicle_name = ''): + """ + Get details about the camera + + Args: + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + vehicle_name (str, optional): Vehicle which the camera is associated with + + Returns: + CameraInfo: + """ # TODO: below str() conversion is only needed for legacy reason and should be removed in future return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name)) - def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = ''): + + def simSetCameraPose(self, camera_name, pose, vehicle_name = ''): + """ + - Control the pose of a selected camera + + Args: + camera_name (str): Name of the camera to be controlled + pose (Pose): Pose representing the desired position and orientation of the camera + vehicle_name (str, optional): Name of vehicle which the camera corresponds to + """ + # TODO: below str() conversion is only needed for legacy reason and should be removed in future + self.client.call('simSetCameraPose', str(camera_name), pose, vehicle_name) + + def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = ''): + """ + - Control the field of view of a selected camera + + Args: + camera_name (str): Name of the camera to be controlled + fov_degrees (float): Value of field of view in degrees + vehicle_name (str, optional): Name of vehicle which the camera corresponds to + """ # TODO: below str() conversion is only needed for legacy reason and should be removed in future - self.client.call('simSetCameraOrientation', str(camera_name), orientation, vehicle_name) + self.client.call('simSetCameraFov', str(camera_name), fov_degrees, vehicle_name) def simGetGroundTruthKinematics(self, vehicle_name = ''): + """ + Get Ground truth kinematics of the vehicle + + Args: + vehicle_name (str, optional): Name of the vehicle + + Returns: + KinematicsState: Ground truth of the vehicle + """ kinematics_state = self.client.call('simGetGroundTruthKinematics', vehicle_name) return KinematicsState.from_msgpack(kinematics_state) simGetGroundTruthKinematics.__annotations__ = {'return': KinematicsState} + def simGetGroundTruthEnvironment(self, vehicle_name = ''): + """ + Get ground truth environment state + + Args: + vehicle_name (str, optional): Name of the vehicle + + Returns: + EnvironmentState: Ground truth environment state + """ env_state = self.client.call('simGetGroundTruthEnvironment', vehicle_name) return EnvironmentState.from_msgpack(env_state) simGetGroundTruthEnvironment.__annotations__ = {'return': EnvironmentState} # sensor APIs def getImuData(self, imu_name = '', vehicle_name = ''): + """ + Args: + imu_name (str, optional): Name of IMU to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + ImuData: + """ return ImuData.from_msgpack(self.client.call('getImuData', imu_name, vehicle_name)) def getBarometerData(self, barometer_name = '', vehicle_name = ''): + """ + Args: + barometer_name (str, optional): Name of Barometer to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + BarometerData: + """ return BarometerData.from_msgpack(self.client.call('getBarometerData', barometer_name, vehicle_name)) def getMagnetometerData(self, magnetometer_name = '', vehicle_name = ''): + """ + Args: + magnetometer_name (str, optional): Name of Magnetometer to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + MagnetometerData: + """ return MagnetometerData.from_msgpack(self.client.call('getMagnetometerData', magnetometer_name, vehicle_name)) def getGpsData(self, gps_name = '', vehicle_name = ''): + """ + Args: + gps_name (str, optional): Name of GPS to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + GpsData: + """ return GpsData.from_msgpack(self.client.call('getGpsData', gps_name, vehicle_name)) - def getDistanceSensorData(self, lidar_name = '', vehicle_name = ''): + def getDistanceSensorData(self, distance_sensor_name = '', vehicle_name = ''): + """ + Args: + distance_sensor_name (str, optional): Name of Distance Sensor to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + DistanceSensorData: + """ return DistanceSensorData.from_msgpack(self.client.call('getDistanceSensorData', distance_sensor_name, vehicle_name)) def getLidarData(self, lidar_name = '', vehicle_name = ''): + """ + Args: + lidar_name (str, optional): Name of Lidar to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + LidarData: + """ return LidarData.from_msgpack(self.client.call('getLidarData', lidar_name, vehicle_name)) - + def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): + """ + Returns Segmentation ID of each point's collided object in the last Lidar update + + Args: + lidar_name (str, optional): Name of Lidar sensor + vehicle_name (str, optional): Name of the vehicle wth the sensor + + Returns: + list[int]: Segmentation IDs of the objects + """ return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name) - #----------- APIs to control ACharacter in scene ----------/ - def simCharSetFaceExpression(self, expression_name, value, character_name = ""): - self.client.call('simCharSetFaceExpression', expression_name, value, character_name) - def simCharGetFaceExpression(self, expression_name, character_name = ""): - return self.client.call('simCharGetFaceExpression', expression_name, character_name) - def simCharGetAvailableFaceExpressions(self): - return self.client.call('simCharGetAvailableFaceExpressions') - def simCharSetSkinDarkness(self, value, character_name = ""): - self.client.call('simCharSetSkinDarkness', value, character_name) - def simCharGetSkinDarkness(self, character_name = ""): - return self.client.call('simCharGetSkinDarkness', character_name) - def simCharSetSkinAgeing(self, value, character_name = ""): - self.client.call('simCharSetSkinAgeing', value, character_name) - def simCharGetSkinAgeing(self, character_name = ""): - return self.client.call('simCharGetSkinAgeing', character_name) - def simCharSetHeadRotation(self, q, character_name = ""): - self.client.call('simCharSetHeadRotation', q, character_name) - def simCharGetHeadRotation(self, character_name = ""): - return self.client.call('simCharGetHeadRotation', character_name) - def simCharSetBonePose(self, bone_name, pose, character_name = ""): - self.client.call('simCharSetBonePose', bone_name, pose, character_name) - def simCharGetBonePose(self, bone_name, character_name = ""): - return self.client.call('simCharGetBonePose', bone_name, character_name) - def simCharResetBonePose(self, bone_name, character_name = ""): - self.client.call('simCharResetBonePose', bone_name, character_name) - def simCharSetFacePreset(self, preset_name, value, character_name = ""): - self.client.call('simCharSetFacePreset', preset_name, value, character_name) - def simCharSetFacePresets(self, presets, character_name = ""): - self.client.call('simSetFacePresets', presets, character_name) - def simCharSetBonePoses(self, poses, character_name = ""): - self.client.call('simSetBonePoses', poses, character_name) - def simCharGetBonePoses(self, bone_names, character_name = ""): - return self.client.call('simGetBonePoses', bone_names, character_name) + # Plotting APIs + def simFlushPersistentMarkers(self): + """ + Clear any persistent markers - those plotted with setting `is_persistent=True` in the APIs below + """ + self.client.call('simFlushPersistentMarkers') + + def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, duration = -1.0, is_persistent = False): + """ + Plot a list of 3D points in World NED frame + + Args: + points (list[Vector3r]): List of Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + size (float, optional): Size of plotted point + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotPoints', points, color_rgba, size, duration, is_persistent) + + def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False): + """ + Plots a line strip in World NED frame, defined from points[0] to points[1], points[1] to points[2], ... , points[n-2] to points[n-1] + + Args: + points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of line + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotLineStrip', points, color_rgba, thickness, duration, is_persistent) + + def simPlotLineList(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False): + """ + Plots a line strip in World NED frame, defined from points[0] to points[1], points[2] to points[3], ... , points[n-2] to points[n-1] + + Args: + points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects. Must be even + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of line + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotLineList', points, color_rgba, thickness, duration, is_persistent) + + def simPlotArrows(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, arrow_size = 2.0, duration = -1.0, is_persistent = False): + """ + Plots a list of arrows in World NED frame, defined from points_start[0] to points_end[0], points_start[1] to points_end[1], ... , points_start[n-1] to points_end[n-1] + + Args: + points_start (list[Vector3r]): List of 3D start positions of arrow start positions, specified as Vector3r objects + points_end (list[Vector3r]): List of 3D end positions of arrow start positions, specified as Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of line + arrow_size (float, optional): Size of arrow head + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotArrows', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) + + + def simPlotStrings(self, strings, positions, scale = 5, color_rgba=[1.0, 0.0, 0.0, 1.0], duration = -1.0): + """ + Plots a list of strings at desired positions in World NED frame. + + Args: + strings (list[String], optional): List of strings to plot + positions (list[Vector3r]): List of positions where the strings should be plotted. Should be in one-to-one correspondence with the strings' list + scale (float, optional): Font scale of transform name + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + duration (float, optional): Duration (seconds) to plot for + """ + self.client.call('simPlotStrings', strings, positions, scale, color_rgba, duration) + + def simPlotTransforms(self, poses, scale = 5.0, thickness = 5.0, duration = -1.0, is_persistent = False): + """ + Plots a list of transforms in World NED frame. + + Args: + poses (list[Pose]): List of Pose objects representing the transforms to plot + scale (float, optional): Length of transforms' axes + thickness (float, optional): Thickness of transforms' axes + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotTransforms', poses, scale, thickness, duration, is_persistent) + + def simPlotTransformsWithNames(self, poses, names, tf_scale = 5.0, tf_thickness = 5.0, text_scale = 10.0, text_color_rgba = [1.0, 0.0, 0.0, 1.0], duration = -1.0): + """ + Plots a list of transforms with their names in World NED frame. + + Args: + poses (list[Pose]): List of Pose objects representing the transforms to plot + names (list[string]): List of strings with one-to-one correspondence to list of poses + tf_scale (float, optional): Length of transforms' axes + tf_thickness (float, optional): Thickness of transforms' axes + text_scale (float, optional): Font scale of transform name + text_color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 for the transform name + duration (float, optional): Duration (seconds) to plot for + """ + self.client.call('simPlotTransformsWithNames', poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration) def cancelLastTask(self, vehicle_name = ''): + """ + Cancel previous Async task + + Args: + vehicle_name (str, optional): Name of the vehicle + """ self.client.call('cancelLastTask', vehicle_name) + def waitOnLastTask(self, timeout_sec = float('nan')): + """ + Wait for the last Async task to complete + + Args: + timeout_sec (float, optional): Time for the task to complete + + Returns: + bool: Result of the last task + + True if the task completed without cancellation or timeout + """ return self.client.call('waitOnLastTask', timeout_sec) - # legacy handling - # TODO: remove below legacy wrappers in future major releases - upgrade_api_help = "\nPlease see https://github.com/Microsoft/AirSim/blob/master/docs/upgrade_apis.md for more info." - def simGetPose(self): - logging.warning("simGetPose API is renamed to simGetVehiclePose. Please update your code." + self.upgrade_api_help) - return self.simGetVehiclePose() - def simSetPose(self, pose, ignore_collison): - logging.warning("simSetPose API is renamed to simSetVehiclePose. Please update your code." + self.upgrade_api_help) - return self.simSetVehiclePose(pose, ignore_collison) - def getCollisionInfo(self): - logging.warning("getCollisionInfo API is renamed to simGetCollisionInfo. Please update your code." + self.upgrade_api_help) - return self.simGetCollisionInfo() - def getCameraInfo(self, camera_id): - logging.warning("getCameraInfo API is renamed to simGetCameraInfo. Please update your code." + self.upgrade_api_help) - return self.simGetCameraInfo(camera_id) - def setCameraOrientation(self, camera_id, orientation): - logging.warning("setCameraOrientation API is renamed to simSetCameraOrientation. Please update your code." + self.upgrade_api_help) - return self.simSetCameraOrientation(camera_id, orientation) - def getPosition(self): - logging.warning("getPosition API is deprecated. For ground-truth please use simGetGroundTruthKinematics() API." + self.upgrade_api_help) - return self.simGetGroundTruthKinematics().position - def getVelocity(self): - logging.warning("getVelocity API is deprecated. For ground-truth please use simGetGroundTruthKinematics() API." + self.upgrade_api_help) - return self.simGetGroundTruthKinematics().linear_velocity - def getOrientation(self): - logging.warning("getOrientation API is deprecated. For ground-truth please use simGetGroundTruthKinematics() API." + self.upgrade_api_help) - return self.simGetGroundTruthKinematics().orientation - def getLandedState(self): - raise Exception("getLandedState API is deprecated. Please use getMultirotorState() API") - def getGpsLocation(self): - logging.warning("getGpsLocation API is deprecated. For ground-truth please use simGetGroundTruthKinematics() API." + self.upgrade_api_help) - return self.simGetGroundTruthEnvironment().geo_point - def takeoff(self, max_wait_seconds = 15): - raise Exception("takeoff API is deprecated. Please use takeoffAsync() API." + self.upgrade_api_help) - def land(self, max_wait_seconds = 60): - raise Exception("land API is deprecated. Please use landAsync() API." + self.upgrade_api_help) - def goHome(self): - raise Exception("goHome API is deprecated. Please use goHomeAsync() API." + self.upgrade_api_help) - def hover(self): - raise Exception("hover API is deprecated. Please use hoverAsync() API." + self.upgrade_api_help) - def moveByAngleZ(self, pitch, roll, z, yaw, duration): - raise Exception("moveByAngleZ API is deprecated. Please use moveByAngleZAsync() API." + self.upgrade_api_help) - def moveByAngleThrottle(self, pitch, roll, throttle, yaw_rate, duration): - raise Exception("moveByAngleThrottle API is deprecated. Please use moveByAngleThrottleAsync() API." + self.upgrade_api_help) - def moveByVelocity(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): - raise Exception("moveByVelocity API is deprecated. Please use moveByVelocityAsync() API." + self.upgrade_api_help) - def moveByVelocityZ(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): - raise Exception("moveByVelocityZ API is deprecated. Please use moveByVelocityZAsync() API." + self.upgrade_api_help) - def moveOnPath(self, path, velocity, max_wait_seconds = 60, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1): - raise Exception("moveOnPath API is deprecated. Please use moveOnPathAsync() API." + self.upgrade_api_help) - def moveToZ(self, z, velocity, max_wait_seconds = 60, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1): - raise Exception("moveToZ API is deprecated. Please use moveToZAsync() API." + self.upgrade_api_help) - def moveToPosition(self, x, y, z, velocity, max_wait_seconds = 60, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1): - raise Exception("moveToPosition API is deprecated. Please use moveToPositionAsync() API." + self.upgrade_api_help) - def moveByManual(self, vx_max, vy_max, z_min, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): - raise Exception("moveByManual API is deprecated. Please use moveByManualAsync() API." + self.upgrade_api_help) - def rotateToYaw(self, yaw, max_wait_seconds = 60, margin = 5): - raise Exception("rotateToYaw API is deprecated. Please use rotateToYawAsync() API." + self.upgrade_api_help) - def rotateByYawRate(self, yaw_rate, duration): - raise Exception("rotateByYawRate API is deprecated. Please use rotateByYawRateAsync() API." + self.upgrade_api_help) - def setRCData(self, rcdata = RCData()): - raise Exception("setRCData API is deprecated. Please use moveByRC() API." + self.upgrade_api_help) + # Recording APIs + def startRecording(self): + """ + Start Recording + + Recording will be done according to the settings + """ + self.client.call('startRecording') + + def stopRecording(self): + """ + Stop Recording + """ + self.client.call('stopRecording') + + def isRecording(self): + """ + Whether Recording is running or not + + Returns: + bool: True if Recording, else False + """ + return self.client.call('isRecording') # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): @@ -273,56 +705,410 @@ def __init__(self, ip = "", port = 41451, timeout_value = 3600): super(MultirotorClient, self).__init__(ip, port, timeout_value) def takeoffAsync(self, timeout_sec = 20, vehicle_name = ''): - return self.client.call_async('takeoff', timeout_sec, vehicle_name) + """ + Takeoff vehicle to 3m above ground. Vehicle should not be moving when this API is used + + Args: + timeout_sec (int, optional): Timeout for the vehicle to reach desired altitude + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('takeoff', timeout_sec, vehicle_name) + def landAsync(self, timeout_sec = 60, vehicle_name = ''): - return self.client.call_async('land', timeout_sec, vehicle_name) + """ + Land the vehicle + + Args: + timeout_sec (int, optional): Timeout for the vehicle to land + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('land', timeout_sec, vehicle_name) + def goHomeAsync(self, timeout_sec = 3e+38, vehicle_name = ''): + """ + Return vehicle to Home i.e. Launch location + + Args: + timeout_sec (int, optional): Timeout for the vehicle to reach desired altitude + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ return self.client.call_async('goHome', timeout_sec, vehicle_name) # APIs for control def moveByAngleZAsync(self, pitch, roll, z, yaw, duration, vehicle_name = ''): return self.client.call_async('moveByAngleZ', pitch, roll, z, yaw, duration, vehicle_name) + def moveByAngleThrottleAsync(self, pitch, roll, throttle, yaw_rate, duration, vehicle_name = ''): return self.client.call_async('moveByAngleThrottle', pitch, roll, throttle, yaw_rate, duration, vehicle_name) + def moveByVelocityAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + """ + Args: + vx (float): desired velocity in world (NED) X axis + vy (float): desired velocity in world (NED) Y axis + vz (float): desired velocity in world (NED) Z axis + duration (float): Desired amount of time (seconds), to send this command for + drivetrain (DrivetrainType, optional): + yaw_mode (YawMode, optional): + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ return self.client.call_async('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name) + def moveByVelocityZAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): return self.client.call_async('moveByVelocityZ', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name) - def moveOnPathAsync(self, path, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), + + def moveOnPathAsync(self, path, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveOnPath', path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) - def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), + + def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + def moveByManualAsync(self, vx_max, vy_max, z_min, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): - """Read current RC state and use it to control the vehicles. + """ + - Read current RC state and use it to control the vehicles. Parameters sets up the constraints on velocity and minimum altitude while flying. If RC state is detected to violate these constraints then that RC state would be ignored. - :param vx_max: max velocity allowed in x direction - :param vy_max: max velocity allowed in y direction - :param vz_max: max velocity allowed in z direction - :param z_min: min z allowed for vehicle position - :param duration: after this duration vehicle would switch back to non-manual mode - :param drivetrain: when ForwardOnly, vehicle rotates itself so that its front is always facing the direction of travel. If MaxDegreeOfFreedom then it doesn't do that (crab-like movement) - :param yaw_mode: Specifies if vehicle should face at given angle (is_rate=False) or should be rotating around its axis at given rate (is_rate=True) + Args: + vx_max (float): max velocity allowed in x direction + vy_max (float): max velocity allowed in y direction + vz_max (float): max velocity allowed in z direction + z_min (float): min z allowed for vehicle position + duration (float): after this duration vehicle would switch back to non-manual mode + drivetrain (DrivetrainType): when ForwardOnly, vehicle rotates itself so that its front is always facing the direction of travel. If MaxDegreeOfFreedom then it doesn't do that (crab-like movement) + yaw_mode (YawMode): Specifies if vehicle should face at given angle (is_rate=False) or should be rotating around its axis at given rate (is_rate=True) + vehicle_name (str, optional): Name of the multirotor to send this command to + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ return self.client.call_async('moveByManual', vx_max, vy_max, z_min, duration, drivetrain, yaw_mode, vehicle_name) + def rotateToYawAsync(self, yaw, timeout_sec = 3e+38, margin = 5, vehicle_name = ''): return self.client.call_async('rotateToYaw', yaw, timeout_sec, margin, vehicle_name) + def rotateByYawRateAsync(self, yaw_rate, duration, vehicle_name = ''): return self.client.call_async('rotateByYawRate', yaw_rate, duration, vehicle_name) + def hoverAsync(self, vehicle_name = ''): return self.client.call_async('hover', vehicle_name) def moveByRC(self, rcdata = RCData(), vehicle_name = ''): return self.client.call('moveByRC', rcdata, vehicle_name) - + + # low-level control API + def moveByMotorPWMsAsync(self, front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name = ''): + """ + - Directly control the motors using PWM values + + Args: + front_right_pwm (float): PWM value for the front right motor (between 0.0 to 1.0) + rear_left_pwm (float): PWM value for the rear left motor (between 0.0 to 1.0) + front_left_pwm (float): PWM value for the front left motor (between 0.0 to 1.0) + rear_right_pwm (float): PWM value for the rear right motor (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByMotorPWMs', front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name) + + def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll angle, pitch angle, and yaw angle set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, vehicle_name) + + def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll angle, pitch angle, and yaw angle are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawThrottle', roll, -pitch, -yaw, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw_rate (float): Desired yaw rate, in radian per second. + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw_rate (float): Desired yaw rate, in radian per second. + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawrateZ', roll, -pitch, -yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll_rate (float): Desired roll rate, in radians / second + pitch_rate (float): Desired pitch rate, in radians / second + yaw_rate (float): Desired yaw rate, in radians / second + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByAngleRatesZ', roll_rate, -pitch_rate, -yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll_rate (float): Desired roll rate, in radians / second + pitch_rate (float): Desired pitch rate, in radians / second + yaw_rate (float): Desired yaw rate, in radians / second + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByAngleRatesThrottle', roll_rate, -pitch_rate, -yaw_rate, throttle, duration, vehicle_name) + + def setAngleRateControllerGains(self, angle_rate_gains=AngleRateControllerGains(), vehicle_name = ''): + """ + - Modifying these gains will have an affect on *ALL* move*() APIs. + This is because any velocity setpoint is converted to an angle level setpoint which is tracked with an angle level controllers. + That angle level setpoint is itself tracked with and angle rate controller. + - This function should only be called if the default angle rate control PID gains need to be modified. + + Args: + angle_rate_gains (AngleRateControllerGains): + - Correspond to the roll, pitch, yaw axes, defined in the body frame. + - Pass AngleRateControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setAngleRateControllerGains', *(angle_rate_gains.to_lists()+(vehicle_name,))) + + def setAngleLevelControllerGains(self, angle_level_gains=AngleLevelControllerGains(), vehicle_name = ''): + """ + - Sets angle level controller gains (used by any API setting angle references - for ex: moveByRollPitchYawZAsync(), moveByRollPitchYawThrottleAsync(), etc) + - Modifying these gains will also affect the behaviour of moveByVelocityAsync() API. + This is because the AirSim flight controller will track velocity setpoints by converting them to angle set points. + - This function should only be called if the default angle level control PID gains need to be modified. + - Passing AngleLevelControllerGains() sets gains to default airsim values. + + Args: + angle_level_gains (AngleLevelControllerGains): + - Correspond to the roll, pitch, yaw axes, defined in the body frame. + - Pass AngleLevelControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setAngleLevelControllerGains', *(angle_level_gains.to_lists()+(vehicle_name,))) + + def setVelocityControllerGains(self, velocity_gains=VelocityControllerGains(), vehicle_name = ''): + """ + - Sets velocity controller gains for moveByVelocityAsync(). + - This function should only be called if the default velocity control PID gains need to be modified. + - Passing VelocityControllerGains() sets gains to default airsim values. + + Args: + velocity_gains (VelocityControllerGains): + - Correspond to the world X, Y, Z axes. + - Pass VelocityControllerGains() to reset gains to default recommended values. + - Modifying velocity controller gains will have an affect on the behaviour of moveOnSplineAsync() and moveOnSplineVelConstraintsAsync(), as they both use velocity control to track the trajectory. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setVelocityControllerGains', *(velocity_gains.to_lists()+(vehicle_name,))) + + + def setPositionControllerGains(self, position_gains=PositionControllerGains(), vehicle_name = ''): + """ + Sets position controller gains for moveByPositionAsync. + This function should only be called if the default position control PID gains need to be modified. + + Args: + position_gains (PositionControllerGains): + - Correspond to the X, Y, Z axes. + - Pass PositionControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setPositionControllerGains', *(position_gains.to_lists()+(vehicle_name,))) + # query vehicle state def getMultirotorState(self, vehicle_name = ''): + """ + Args: + vehicle_name (str, optional): Vehicle to get the state of + + Returns: + MultirotorState: + """ return MultirotorState.from_msgpack(self.client.call('getMultirotorState', vehicle_name)) getMultirotorState.__annotations__ = {'return': MultirotorState} @@ -333,11 +1119,33 @@ def __init__(self, ip = "", port = 41451, timeout_value = 3600): super(CarClient, self).__init__(ip, port, timeout_value) def setCarControls(self, controls, vehicle_name = ''): + """ + Control the car using throttle, steering, brake, etc. + + Args: + controls (CarControls): Struct containing control values + vehicle_name (str, optional): Name of vehicle to be controlled + """ self.client.call('setCarControls', controls, vehicle_name) def getCarState(self, vehicle_name = ''): + """ + Args: + vehicle_name (str, optional): Name of vehicle + + Returns: + CarState: + """ state_raw = self.client.call('getCarState', vehicle_name) return CarState.from_msgpack(state_raw) + def getCarControls(self, vehicle_name=''): + """ + Args: + vehicle_name (str, optional): Name of vehicle + + Returns: + CarControls: + """ controls_raw = self.client.call('getCarControls', vehicle_name) - return CarControls.from_msgpack(controls_raw) \ No newline at end of file + return CarControls.from_msgpack(controls_raw) diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index ed1c98f4b..14f73f168 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -19,7 +19,7 @@ def from_msgpack(cls, encoded): return obj -class ImageType: +class ImageType: Scene = 0 DepthPlanner = 1 DepthPerspective = 2 @@ -32,7 +32,7 @@ class ImageType: class DrivetrainType: MaxDegreeOfFreedom = 0 ForwardOnly = 1 - + class LandedState: Landed = 0 Flying = 1 @@ -71,13 +71,13 @@ def __sub__(self, other): def __truediv__(self, other): if type(other) in [int, float] + np.sctypes['int'] + np.sctypes['uint'] + np.sctypes['float']: return Vector3r( self.x_val / other, self.y_val / other, self.z_val / other) - else: + else: raise TypeError('unsupported operand type(s) for /: %s and %s' % ( str(type(self)), str(type(other))) ) def __mul__(self, other): if type(other) in [int, float] + np.sctypes['int'] + np.sctypes['uint'] + np.sctypes['float']: return Vector3r(self.x_val*other, self.y_val*other, self.z_val*other) - else: + else: raise TypeError('unsupported operand type(s) for *: %s and %s' % ( str(type(self)), str(type(other))) ) def dot(self, other): @@ -139,12 +139,12 @@ def __mul__(self, other): else: raise TypeError('unsupported operand type(s) for *: %s and %s' % ( str(type(self)), str(type(other))) ) - def __truediv__(self, other): - if type(other) == type(self): + def __truediv__(self, other): + if type(other) == type(self): return self * other.inverse() elif type(other) in [int, float] + np.sctypes['int'] + np.sctypes['uint'] + np.sctypes['float']: return Quaternionr( self.x_val / other, self.y_val / other, self.z_val / other, self.w_val / other) - else: + else: raise TypeError('unsupported operand type(s) for /: %s and %s' % ( str(type(self)), str(type(other))) ) def dot(self, other): @@ -172,7 +172,7 @@ def rotate(self, other): else: raise ValueError('length of the other Quaternionr must be 1') else: - raise TypeError('unsupported operand type(s) for \'rotate\': %s and %s' % ( str(type(self)), str(type(other))) ) + raise TypeError('unsupported operand type(s) for \'rotate\': %s and %s' % ( str(type(self)), str(type(other))) ) def conjugate(self): return Quaternionr(-self.x_val, -self.y_val, -self.z_val, self.w_val) @@ -240,18 +240,18 @@ class RCData(MsgpackMixin): def __init__(self, timestamp = 0, pitch = 0.0, roll = 0.0, throttle = 0.0, yaw = 0.0, switch1 = 0, switch2 = 0, switch3 = 0, switch4 = 0, switch5 = 0, switch6 = 0, switch7 = 0, switch8 = 0, is_initialized = False, is_valid = False): self.timestamp = timestamp - self.pitch = pitch + self.pitch = pitch self.roll = roll - self.throttle = throttle - self.yaw = yaw - self.switch1 = switch1 - self.switch2 = switch2 - self.switch3 = switch3 - self.switch4 = switch4 + self.throttle = throttle + self.yaw = yaw + self.switch1 = switch1 + self.switch2 = switch2 + self.switch3 = switch3 + self.switch4 = switch4 self.switch5 = switch5 - self.switch6 = switch6 - self.switch7 = switch7 - self.switch8 = switch8 + self.switch6 = switch6 + self.switch7 = switch7 + self.switch8 = switch8 self.is_initialized = is_initialized self.is_valid = is_valid @@ -291,7 +291,7 @@ class CarControls(MsgpackMixin): manual_gear = 0 gear_immediate = True - def __init__(self, throttle = 0, steering = 0, brake = 0, + def __init__(self, throttle = 0, steering = 0, brake = 0, handbrake = False, is_manual_gear = False, manual_gear = 0, gear_immediate = True): self.throttle = throttle self.steering = steering @@ -304,13 +304,13 @@ def __init__(self, throttle = 0, steering = 0, brake = 0, def set_throttle(self, throttle_val, forward): if (forward): - is_manual_gear = False - manual_gear = 0 - throttle = abs(throttle_val) + self.is_manual_gear = False + self.manual_gear = 0 + self.throttle = abs(throttle_val) else: - is_manual_gear = False - manual_gear = -1 - throttle = - abs(throttle_val) + self.is_manual_gear = False + self.manual_gear = -1 + self.throttle = - abs(throttle_val) class KinematicsState(MsgpackMixin): position = Vector3r() @@ -334,17 +334,20 @@ class CarState(MsgpackMixin): rpm = 0.0 maxrpm = 0.0 handbrake = False - collision = CollisionInfo(); + collision = CollisionInfo() kinematics_estimated = KinematicsState() timestamp = np.uint64(0) class MultirotorState(MsgpackMixin): - collision = CollisionInfo(); + collision = CollisionInfo() kinematics_estimated = KinematicsState() gps_location = GeoPoint() timestamp = np.uint64(0) landed_state = LandedState.Landed rc_data = RCData() + ready = False + ready_message = "" + can_arm = False class ProjectionMatrix(MsgpackMixin): matrix = [] @@ -382,15 +385,123 @@ class GnssFixType(MsgpackMixin): GNSS_FIX_2D_FIX = 2 GNSS_FIX_3D_FIX = 3 -class GnssReport(MsgpackMixin): - geo_point = GeoPoint(); +class GnssReport(MsgpackMixin): + geo_point = GeoPoint() eph = 0.0 - epv = 0.0; - velocity = Vector3r(); - fix_type = GnssFixType(); - time_utc = np.uint64(0); + epv = 0.0 + velocity = Vector3r() + fix_type = GnssFixType() + time_utc = np.uint64(0) class GpsData(MsgpackMixin): time_stamp = np.uint64(0) gnss = GnssReport() - is_valid = False \ No newline at end of file + is_valid = False + +class DistanceSensorData(MsgpackMixin): + time_stamp = np.uint64(0) + distance = 0.0 + min_distance = 0.0 + max_distance = 0.0 + relative_pose = Pose() + +class PIDGains(): + """ + Struct to store values of PID gains. Used to transmit controller gain values while instantiating + AngleLevel/AngleRate/Velocity/PositionControllerGains objects. + + Attributes: + kP (float): Proportional gain + kI (float): Integrator gain + kD (float): Derivative gain + """ + def __init__(self, kp, ki, kd): + self.kp = kp + self.ki = ki + self.kd = kd + + def to_list(self): + return [self.kp, self.ki, self.kd] + +class AngleRateControllerGains(): + """ + Struct to contain controller gains used by angle level PID controller + + Attributes: + roll_gains (PIDGains): kP, kI, kD for roll axis + pitch_gains (PIDGains): kP, kI, kD for pitch axis + yaw_gains (PIDGains): kP, kI, kD for yaw axis + """ + def __init__(self, roll_gains = PIDGains(0.25, 0, 0), + pitch_gains = PIDGains(0.25, 0, 0), + yaw_gains = PIDGains(0.25, 0, 0)): + self.roll_gains = roll_gains + self.pitch_gains = pitch_gains + self.yaw_gains = yaw_gains + + def to_lists(self): + return [self.roll_gains.kp, self.pitch_gains.kp, self.yaw_gains.kp], [self.roll_gains.ki, self.pitch_gains.ki, self.yaw_gains.ki], [self.roll_gains.kd, self.pitch_gains.kd, self.yaw_gains.kd] + +class AngleLevelControllerGains(): + """ + Struct to contain controller gains used by angle rate PID controller + + Attributes: + roll_gains (PIDGains): kP, kI, kD for roll axis + pitch_gains (PIDGains): kP, kI, kD for pitch axis + yaw_gains (PIDGains): kP, kI, kD for yaw axis + """ + def __init__(self, roll_gains = PIDGains(2.5, 0, 0), + pitch_gains = PIDGains(2.5, 0, 0), + yaw_gains = PIDGains(2.5, 0, 0)): + self.roll_gains = roll_gains + self.pitch_gains = pitch_gains + self.yaw_gains = yaw_gains + + def to_lists(self): + return [self.roll_gains.kp, self.pitch_gains.kp, self.yaw_gains.kp], [self.roll_gains.ki, self.pitch_gains.ki, self.yaw_gains.ki], [self.roll_gains.kd, self.pitch_gains.kd, self.yaw_gains.kd] + +class VelocityControllerGains(): + """ + Struct to contain controller gains used by velocity PID controller + + Attributes: + x_gains (PIDGains): kP, kI, kD for X axis + y_gains (PIDGains): kP, kI, kD for Y axis + z_gains (PIDGains): kP, kI, kD for Z axis + """ + def __init__(self, x_gains = PIDGains(0.2, 0, 0), + y_gains = PIDGains(0.2, 0, 0), + z_gains = PIDGains(2.0, 2.0, 0)): + self.x_gains = x_gains + self.y_gains = y_gains + self.z_gains = z_gains + + def to_lists(self): + return [self.x_gains.kp, self.y_gains.kp, self.z_gains.kp], [self.x_gains.ki, self.y_gains.ki, self.z_gains.ki], [self.x_gains.kd, self.y_gains.kd, self.z_gains.kd] + +class PositionControllerGains(): + """ + Struct to contain controller gains used by position PID controller + + Attributes: + x_gains (PIDGains): kP, kI, kD for X axis + y_gains (PIDGains): kP, kI, kD for Y axis + z_gains (PIDGains): kP, kI, kD for Z axis + """ + def __init__(self, x_gains = PIDGains(0.25, 0, 0), + y_gains = PIDGains(0.25, 0, 0), + z_gains = PIDGains(0.25, 0, 0)): + self.x_gains = x_gains + self.y_gains = y_gains + self.z_gains = z_gains + + def to_lists(self): + return [self.x_gains.kp, self.y_gains.kp, self.z_gains.kp], [self.x_gains.ki, self.y_gains.ki, self.z_gains.ki], [self.x_gains.kd, self.y_gains.kd, self.z_gains.kd] + +class MeshPositionVertexBuffersResponse(MsgpackMixin): + position = Vector3r() + orientation = Quaternionr() + vertices = 0.0 + indices = 0.0 + name = '' diff --git a/PythonClient/build_api_docs.sh b/PythonClient/build_api_docs.sh new file mode 100755 index 000000000..548e7d951 --- /dev/null +++ b/PythonClient/build_api_docs.sh @@ -0,0 +1,2 @@ +cd docs; +make html; diff --git a/PythonClient/car/distance_sensor_multi.py b/PythonClient/car/distance_sensor_multi.py new file mode 100644 index 000000000..5e4660ba6 --- /dev/null +++ b/PythonClient/car/distance_sensor_multi.py @@ -0,0 +1,49 @@ +import airsim +import time + +''' +An example script showing usage of Distance sensor to measure distance between 2 Car vehicles +Settings - + +{ + "SettingsVersion": 1.2, + "SimMode": "Car", + "Vehicles": { + "Car1": { + "VehicleType": "PhysXCar", + "AutoCreate": true, + "Sensors": { + "Distance": { + "SensorType": 5, + "Enabled" : true, + "DrawDebugPoints": true + } + } + }, + "Car2": { + "VehicleType": "PhysXCar", + "AutoCreate": true, + "X": 10, "Y": 0, "Z": 0, + "Sensors": { + "Distance": { + "SensorType": 5, + "Enabled" : true, + "DrawDebugPoints": true + } + } + } + } +} + +Car2 is placed in front of Car 1 + +''' + +client = airsim.CarClient() +client.confirmConnection() + +while True: + data_car1 = client.getDistanceSensorData(vehicle_name="Car1") + data_car2 = client.getDistanceSensorData(vehicle_name="Car2") + print(f"Distance sensor data: Car1: {data_car1.distance}, Car2: {data_car2.distance}") + time.sleep(1.0) \ No newline at end of file diff --git a/PythonClient/computer_vision/character_control.py b/PythonClient/computer_vision/character_control.py deleted file mode 100644 index 81cf8c6ed..000000000 --- a/PythonClient/computer_vision/character_control.py +++ /dev/null @@ -1,77 +0,0 @@ -# This script expects object available in UE environment of type AAirSimCharater -# In settings.json first activate computer vision mode: -# https://github.com/Microsoft/AirSim/blob/master/docs/image_apis.md#computer-vision-mode - -import setup_path -import airsim - - -import pprint -import os -import time - -pp = pprint.PrettyPrinter(indent=4) - -client = airsim.VehicleClient() -client.confirmConnection() -client.reset() - -airsim.wait_key('Press any key to set skin age to 1') -client.simCharSetSkinAgeing(1) - -airsim.wait_key('Press any key to set skin color to 0.9') -client.simCharSetSkinDarkness(0.9) - -#airsim.wait_key('Press any key to set face expression') -#client.simCharSetFaceExpression("BlendShapeNode_Smile", 1); - -airsim.wait_key('Press any key to set bone pose') -client.reset() -jaw_pose = airsim.Pose() -jaw_pose.position = airsim.Vector3r(0.002, 0.001, -0.003) -jaw_pose.orientation = airsim.to_quaternion(0, 0, -.15) -client.simCharSetBonePose( "Jaw", jaw_pose); - -airsim.wait_key('Press any key to set preset') -client.reset() -for x in range(0, 10, 3): - client.simCharSetFacePreset("FACS_0" + str(x), 5); - time.sleep(1) - -airsim.wait_key('Press any key to set multiple presets') -presets = {"Phoneme_l":0.5, "Phoneme_ae": 1, "Phoneme_ooo":0.0} -client.simCharSetFacePresets(presets) - -airsim.wait_key('Press any key to turn head around') -client.reset() -for pitch in range(-5, 5, 5): - for yaw in range(-10, 10, 2): - q = airsim.to_quaternion(pitch/10.0, 0, yaw/10.0) - client.simCharSetHeadRotation(q) - time.sleep(0.1) - -airsim.wait_key('Press any key to get images') -for x in range(3): # do few times - responses = client.simGetImages([ - airsim.ImageRequest("0", airsim.ImageType.DepthVis), - airsim.ImageRequest("0", airsim.ImageType.Segmentation), - airsim.ImageRequest("0", airsim.ImageType.Scene), - airsim.ImageRequest("0", airsim.ImageType.SurfaceNormals)]) - - for i, response in enumerate(responses): - if response.pixels_as_float: - print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_float), pprint.pformat(response.camera_position))) - airsim.write_pfm(os.path.normpath('/temp/cv_mode_' + str(x) + "_" + str(i) + '.pfm'), airsim.get_pfm_array(response)) - else: - print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_uint8), pprint.pformat(response.camera_position))) - airsim.write_file(os.path.normpath('/temp/cv_mode_' + str(x) + "_" + str(i) + '.png'), response.image_data_uint8) - - pose = client.simGetVehiclePose() - pose.position.x_val = pose.position.x_val + 1 - pose.position.y_val = pose.position.y_val - 0.5 - pose.position.z_val = pose.position.z_val - 0.5 - client.simSetVehiclePose(pose, True) - - time.sleep(3) - -client.reset() \ No newline at end of file diff --git a/PythonClient/computer_vision/cv_mode.py b/PythonClient/computer_vision/cv_mode.py index 188080ba2..0691c0de7 100644 --- a/PythonClient/computer_vision/cv_mode.py +++ b/PythonClient/computer_vision/cv_mode.py @@ -13,8 +13,9 @@ client = airsim.VehicleClient() client.confirmConnection() -airsim.wait_key('Press any key to set camera-0 gimble to 15-degree pitch') -client.simSetCameraOrientation("0", airsim.to_quaternion(0.261799, 0, 0)); #radians +airsim.wait_key('Press any key to set camera-0 gimbal to 15-degree pitch') +camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.261799, 0, 0)) #radians +client.simSetCameraPose("0", camera_pose) airsim.wait_key('Press any key to get camera parameters') for camera_name in range(5): @@ -48,4 +49,4 @@ time.sleep(3) # currently reset() doesn't work in CV mode. Below is the workaround -client.simSetPose(airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True) \ No newline at end of file +client.simSetPose(airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True) diff --git a/PythonClient/computer_vision/plot_markers.py b/PythonClient/computer_vision/plot_markers.py new file mode 100644 index 000000000..d005ff833 --- /dev/null +++ b/PythonClient/computer_vision/plot_markers.py @@ -0,0 +1,63 @@ +import setup_path +import airsim +from airsim import Vector3r, Quaternionr, Pose +from airsim.utils import to_quaternion +import numpy as np +import time + +client = airsim.VehicleClient() +client.confirmConnection() + +# plot red arrows for 30 seconds +client.simPlotArrows(points_start = [Vector3r(x,y,0) for x, y in zip(np.linspace(0,10,20), np.linspace(0,0,20))], + points_end = [Vector3r(x,y,0) for x, y in zip(np.linspace(0,10,20), np.linspace(10,10,20))], + color_rgba = [1.0, 0.0, 1.0, 1.0], duration = 30.0, arrow_size = 10, thickness = 1) + +# plot magenta arrows for 15 seconds +client.simPlotArrows(points_start = [Vector3r(x,y,-3) for x, y in zip(np.linspace(0,10,20), np.linspace(0,0,20))], + points_end = [Vector3r(x,y,-5) for x, y in zip(np.linspace(0,10,20), np.linspace(10,20,20))], + color_rgba = [1.0, 1.0, 0.0, 1.0], duration = 15.0, arrow_size = 20, thickness = 3) + +# plot red arrows for 10 seconds +client.simPlotArrows(points_start = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(0,0,20), np.linspace(-3,-10, 20))], + points_end = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(10,20,20), np.linspace(-5,-8, 20))], + color_rgba = [1.0, 0.0, 0.0, 1.0], duration = 10.0, arrow_size = 100, thickness = 5) + +# plot 2 white arrows which are persistent +client.simPlotArrows(points_start = [Vector3r(x,y,-2) for x, y in zip(np.linspace(0,10,20), np.linspace(0,20,20))], + points_end = [Vector3r(x,y,-5) for x, y in zip(np.linspace(3,17,20), np.linspace(5,28,20))], + color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 5.0, arrow_size = 100, thickness = 1, is_persistent = True) + +# plot points +client.simPlotPoints(points = [Vector3r(x,y,-5) for x, y in zip(np.linspace(0,-10,20), np.linspace(0,-20,20))], color_rgba=[1.0, 0.0, 0.0, 1.0], size = 25, duration = 20.0, is_persistent = False) +client.simPlotPoints(points = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,-10,20), np.linspace(0,-20,20), np.linspace(0,-5,20))], color_rgba=[0.0, 0.0, 1.0, 1.0], size = 10, duration = 20.0, is_persistent = False) +client.simPlotPoints(points = [Vector3r(x,y,z) for x, y, z in zip(np.linspace(0,10,20), np.linspace(0,-20,20), np.linspace(0,-7,20))], color_rgba=[1.0, 0.0, 1.0, 1.0], size = 15, duration = 20.0, is_persistent = False) + +# plot line strip. 0-1, 1-2, 2-3 +client.simPlotLineStrip(points = [Vector3r(x,y,-5) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5, duration = 30.0, is_persistent = False) + +# plot line list. 0-1, 2-3, 4-5. Must be even. +client.simPlotLineList(points = [Vector3r(x,y,-7) for x, y in zip(np.linspace(0,-10,10), np.linspace(0,-20,10))], color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5, duration = 30.0, is_persistent = False) + +# plot transforms +client.simPlotStrings(strings = ["Microsoft AirSim" for i in range(5)], positions = [Vector3r(x,y,-1) for x, y in zip(np.linspace(0,5,5), np.linspace(0,0,5))], + scale = 1, color_rgba=[1.0, 1.0, 1.0, 1.0], duration = 1200.0) + +# client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=yaw)) for x, y, yaw in zip(np.linspace(0,10,10), np.linspace(0,0,10), np.linspace(0,np.pi,10))], +# scale = 35, thickness = 5, duration = 1200.0, is_persistent = False) + +# client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=roll, yaw=0.0)) for x, y, roll in zip(np.linspace(0,10,10), np.linspace(1,1,10), np.linspace(0,np.pi,10))], +# scale = 35, thickness = 5, duration = 1200.0, is_persistent = False) + +client.simPlotTransformsWithNames(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=yaw)) for x, y, yaw in zip(np.linspace(0,10,10), np.linspace(0,0,10), np.linspace(0,np.pi,10))], + names=["tf_yaw_" + str(idx) for idx in range(10)], tf_scale = 35, tf_thickness = 5, text_scale = 1, text_color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 1200.0) + +client.simPlotTransformsWithNames(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=roll, yaw=0.0)) for x, y, roll in zip(np.linspace(0,10,10), np.linspace(1,1,10), np.linspace(0,np.pi,10))], + names=["tf_roll_" + str(idx) for idx in range(10)], tf_scale = 35, tf_thickness = 5, text_scale = 1, text_color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 1200.0) + +client.simPlotTransformsWithNames(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=pitch, roll=0.0, yaw=0.0)) for x, y, pitch in zip(np.linspace(0,10,10), np.linspace(-1,-1,10), np.linspace(0,np.pi,10))], + names=["tf_pitch_" + str(idx) for idx in range(10)], tf_scale = 35, tf_thickness = 5, text_scale = 1, text_color_rgba = [1.0, 1.0, 1.0, 1.0], duration = 1200.0) + +time.sleep(20.0) + +client.simFlushPersistentMarkers() \ No newline at end of file diff --git a/PythonClient/docs/Makefile b/PythonClient/docs/Makefile new file mode 100644 index 000000000..298ea9e21 --- /dev/null +++ b/PythonClient/docs/Makefile @@ -0,0 +1,19 @@ +# Minimal makefile for Sphinx documentation +# + +# You can set these variables from the command line. +SPHINXOPTS = +SPHINXBUILD = sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) \ No newline at end of file diff --git a/PythonClient/docs/conf.py b/PythonClient/docs/conf.py new file mode 100644 index 000000000..e7bfe2273 --- /dev/null +++ b/PythonClient/docs/conf.py @@ -0,0 +1,204 @@ +# -*- coding: utf-8 -*- +# +# Configuration file for the Sphinx documentation builder. +# +# This file does only contain a selection of the most common options. For a +# full list see the documentation: +# http://www.sphinx-doc.org/en/master/config + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys +sys.path.insert(0, os.path.abspath('..')) + +import sphinx_rtd_theme + +# -- Project information ----------------------------------------------------- + +project = u'airsim' +copyright = u'2020, Shital Shah, Ratnesh Madaan, Sai Vemprala, Nicholas Gyde' +author = u'Shital Shah, Ratnesh Madaan, Sai Vemprala, Nicholas Gyde' + +# The short X.Y version +version = u'' +# The full version, including alpha/beta/rc tags +release = u'1.2.8' + + +# -- General configuration --------------------------------------------------- + +# If your documentation needs a minimal Sphinx version, state it here. +# +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.autosummary', + 'sphinx.ext.autosectionlabel', + 'sphinx.ext.doctest', + 'sphinx.ext.intersphinx', + 'sphinx.ext.todo', + 'sphinx.ext.mathjax', + 'sphinx.ext.coverage', + 'sphinx.ext.napoleon', + 'sphinx.ext.viewcode', + 'sphinx_rtd_theme' +] + +autodoc_default_flags = ['members'] +autosummary_generate = True +autosectionlabel_prefix_document = True +autosectionlabel_maxdepth = 4 + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# +# source_suffix = ['.rst', '.md'] +source_suffix = '.rst' + +# The master toctree document. +master_doc = 'index' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = [u'_build', 'Thumbs.db', '.DS_Store'] + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = None + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +# html_theme = 'alabaster' +html_theme = "sphinx_rtd_theme" + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# +# html_theme_options = {} + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +# Custom sidebar templates, must be a dictionary that maps document names +# to template names. +# +# The default sidebars (for documents that don't match any pattern) are +# defined by theme itself. Builtin themes are using these templates by +# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', +# 'searchbox.html']``. +# +# html_sidebars = {} + + +# -- Options for HTMLHelp output --------------------------------------------- + +# Output file base name for HTML help builder. +htmlhelp_basename = 'airsimdoc' + + +# -- Options for LaTeX output ------------------------------------------------ + +latex_elements = { + # The paper size ('letterpaper' or 'a4paper'). + # + # 'papersize': 'letterpaper', + + # The font size ('10pt', '11pt' or '12pt'). + # + # 'pointsize': '10pt', + + # Additional stuff for the LaTeX preamble. + # + # 'preamble': '', + + # Latex figure (float) alignment + # + # 'figure_align': 'htbp', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + (master_doc, 'airsim.tex', u'airsim Documentation', + u'Ratnesh Madaan, Matthew Brown, Nicholas Gyde', 'manual'), +] + + +# -- Options for manual page output ------------------------------------------ + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + (master_doc, 'airsim', u'airsim Documentation', + [author], 1) +] + + +# -- Options for Texinfo output ---------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + (master_doc, 'airsim', u'airsim Documentation', + author, 'airsim', 'One line description of project.', + 'Miscellaneous'), +] + + +# -- Options for Epub output ------------------------------------------------- + +# Bibliographic Dublin Core info. +epub_title = project + +# The unique identifier of the text. This can be a ISBN number +# or the project homepage. +# +# epub_identifier = '' + +# A unique identification for the text. +# +# epub_uid = '' + +# A list of files that should not be packed into the epub file. +epub_exclude_files = ['search.html'] + + +# -- Extension configuration ------------------------------------------------- + +# -- Options for intersphinx extension --------------------------------------- + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {'https://docs.python.org/': None} + +# -- Options for todo extension ---------------------------------------------- + +# If true, `todo` and `todoList` produce output, else they produce nothing. +todo_include_todos = True diff --git a/PythonClient/docs/index.rst b/PythonClient/docs/index.rst new file mode 100644 index 000000000..c256e3fcd --- /dev/null +++ b/PythonClient/docs/index.rst @@ -0,0 +1,42 @@ +.. airsim documentation master file, created by + sphinx-quickstart on Sun Aug 4 14:58:26 2019. + You can adapt this file completely to your liking, but it should at least + contain the root `toctree` directive. + +airsim +========================================= +This page documents `airsim`_, the python package to be used for `Microsoft AirSim`_. + +.. _`airsim`: https://pypi.org/project/airsim/ +.. _`Microsoft AirSim`: https://github.com/microsoft/AirSim + +.. toctree:: + :maxdepth: 3 + +* :ref:`genindex` +* :ref:`modindex` + +.. autoclass:: airsim.client.VehicleClient + :members: + :undoc-members: + :show-inheritance: + +.. autoclass:: airsim.client.MultirotorClient + :members: + :undoc-members: + :show-inheritance: + +.. autoclass:: airsim.client.CarClient + :members: + :undoc-members: + :show-inheritance: + +.. automodule:: airsim.types + :members: + :undoc-members: + :show-inheritance: + +.. automodule:: airsim.utils + :members: + :undoc-members: + :show-inheritance: \ No newline at end of file diff --git a/PythonClient/docs/make.bat b/PythonClient/docs/make.bat new file mode 100644 index 000000000..27f573b87 --- /dev/null +++ b/PythonClient/docs/make.bat @@ -0,0 +1,35 @@ +@ECHO OFF + +pushd %~dp0 + +REM Command file for Sphinx documentation + +if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build +) +set SOURCEDIR=. +set BUILDDIR=_build + +if "%1" == "" goto help + +%SPHINXBUILD% >NUL 2>NUL +if errorlevel 9009 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.http://sphinx-doc.org/ + exit /b 1 +) + +%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% +goto end + +:help +%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% + +:end +popd diff --git a/PythonClient/imitation_learning/README.md b/PythonClient/imitation_learning/README.md index 7f7c96694..c8c769156 100644 --- a/PythonClient/imitation_learning/README.md +++ b/PythonClient/imitation_learning/README.md @@ -6,7 +6,7 @@ The code in this section is based on the [Autonomous Driving Cookbook](https://g ## Prerequisites * Operating system: Windows 10 * GPU: Nvidia GTX 1080 or higher (recommended) -* Software: Unreal Engine 4.18 and Visual Studio 2017 (see [upgrade instructions](../../docs/unreal_upgrade.md)) +* Software: Unreal Engine 4.24 and Visual Studio 2019 (see [upgrade instructions](../../docs/unreal_upgrade.md)) * Development: CUDA 9.0 and python 3.5. * Python libraries: Keras 2.1.2, TensorFlow 1.6.0. * Note: Newer versions of keras or tensorflow are recommended but can cause syntax errors. diff --git a/PythonClient/install_packages.bat b/PythonClient/install_packages.bat deleted file mode 100644 index 721bdec5d..000000000 --- a/PythonClient/install_packages.bat +++ /dev/null @@ -1,3 +0,0 @@ -conda install opencv -pip install opencv-python -pip install msgpack-rpc-python \ No newline at end of file diff --git a/PythonClient/multirotor/arm.py b/PythonClient/multirotor/arm.py new file mode 100644 index 000000000..addf1fe6a --- /dev/null +++ b/PythonClient/multirotor/arm.py @@ -0,0 +1,6 @@ +import setup_path +import airsim + +client = airsim.MultirotorClient() +client.confirmConnection() +client.armDisarm(True) diff --git a/PythonClient/multirotor/box.py b/PythonClient/multirotor/box.py index f366496b0..83b2c7d12 100644 --- a/PythonClient/multirotor/box.py +++ b/PythonClient/multirotor/box.py @@ -1,4 +1,4 @@ -import setup_path +import setup_path import airsim import sys @@ -11,7 +11,6 @@ client.takeoffAsync().join() print("Flying a small square box using moveByVelocityZ") -print("Try pressing 't' in the AirSim view to see a pink trace of the flight") # AirSim uses NED coordinates so negative axis is up. # z of -7 is 7 meters above the original launch point. @@ -46,3 +45,4 @@ client.moveByVelocityZAsync(vx, vy,z,duration, airsim.DrivetrainType.MaxDegreeOfFreedom, airsim.YawMode(False, 0)).join() time.sleep(delay) client.hoverAsync().join() +client.landAsync().join() diff --git a/PythonClient/multirotor/clock_speed.py b/PythonClient/multirotor/clock_speed.py index ee4f981be..774f0bc7f 100644 --- a/PythonClient/multirotor/clock_speed.py +++ b/PythonClient/multirotor/clock_speed.py @@ -1,4 +1,4 @@ -import setup_path +import setup_path import airsim import time @@ -12,7 +12,7 @@ client.enableApiControl(True) client.armDisarm(True) -client.moveByVelocityZAsync(0, 0, -2, 3).join() +client.moveByVelocityZAsync(0, 0, -20, 3).join() while True: diff --git a/PythonClient/multirotor/disarm.py b/PythonClient/multirotor/disarm.py index bf9f4fe5a..9cee1045e 100644 --- a/PythonClient/multirotor/disarm.py +++ b/PythonClient/multirotor/disarm.py @@ -1,4 +1,4 @@ -import setup_path +import setup_path import airsim client = airsim.MultirotorClient() diff --git a/PythonClient/multirotor/gimbal.py b/PythonClient/multirotor/gimbal.py index fef1aeb52..45beb6a23 100644 --- a/PythonClient/multirotor/gimbal.py +++ b/PythonClient/multirotor/gimbal.py @@ -17,7 +17,9 @@ for i in range(5): client.moveToPositionAsync(float(-50.00), float( 50.26), float( -20.58), float( 3.5)) time.sleep(6) - client.simSetCameraOrientation("0", airsim.to_quaternion(0.5, 0.5, 0.1)) + camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.5, 0.5, 0.1)) + client.simSetCameraPose("0", camera_pose) client.moveToPositionAsync(float(50.00), float( -50.26), float( -10.58), float( 3.5)) time.sleep(6) - client.simSetCameraOrientation("0", airsim.to_quaternion(-0.5, -0.5, -0.1)) \ No newline at end of file + camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(-0.5, -0.5, -0.1)) + client.simSetCameraPose("0", camera_pose) diff --git a/PythonClient/multirotor/orbit.py b/PythonClient/multirotor/orbit.py index 9e6b3b996..78d9a23b3 100644 --- a/PythonClient/multirotor/orbit.py +++ b/PythonClient/multirotor/orbit.py @@ -53,7 +53,7 @@ def __init__(self, radius = 2, altitude = 10, speed = 2, iterations = 1, center start = time.time() count = 0 while count < 100: - pos = self.home + pos = self.client.getMultirotorState().kinematics_estimated.position if abs(pos.z_val - self.home.z_val) > 1: count = 0 self.home = pos @@ -63,7 +63,7 @@ def __init__(self, radius = 2, altitude = 10, speed = 2, iterations = 1, center else: count += 1 - self.center = self.client.getMultirotorState().kinematics_estimated.position + self.center = pos self.center.x_val += cx self.center.y_val += cy diff --git a/PythonClient/multirotor/params.txt b/PythonClient/multirotor/params.txt new file mode 100644 index 000000000..da7bb5515 --- /dev/null +++ b/PythonClient/multirotor/params.txt @@ -0,0 +1,5 @@ +param set LPE_LAT 47.641468 +param set LPE_LON -122.140165 +param set COM_OBL_ACT 1 +param set NAV_RCL_ACT 0 +param set NAV_DLL_ACT 0 diff --git a/PythonClient/multirotor/path.py b/PythonClient/multirotor/path.py index 5c2dc7f08..d36fc6a90 100644 --- a/PythonClient/multirotor/path.py +++ b/PythonClient/multirotor/path.py @@ -1,33 +1,56 @@ -import setup_path +import setup_path import airsim import sys import time +print("""This script is designed to fly on the streets of the Neighborhood environment +and assumes the unreal position of the drone is [160, -1500, 120].""") + client = airsim.MultirotorClient() client.confirmConnection() client.enableApiControl(True) +print("arming the drone...") client.armDisarm(True) -landed = client.getMultirotorState().landed_state -if landed == airsim.LandedState.Landed: +state = client.getMultirotorState() +if state.landed_state == airsim.LandedState.Landed: print("taking off...") client.takeoffAsync().join() else: client.hoverAsync().join() +time.sleep(1) + +state = client.getMultirotorState() +if state.landed_state == airsim.LandedState.Landed: + print("take off failed...") + sys.exit(1) + # AirSim uses NED coordinates so negative axis is up. # z of -7 is 7 meters above the original launch point. z = -7 +print("make sure we are hovering at 7 meters...") +client.moveToZAsync(z, 1).join() # see https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo # this method is async and we are not waiting for the result since we are passing timeout_sec=0. -result = client.moveOnPathAsync([airsim.Vector3r(0,-253,z),airsim.Vector3r(125,-253,z),airsim.Vector3r(125,0,z),airsim.Vector3r(0,0,z),airsim.Vector3r(0,0,-20)], - 12, 120, + +print("flying on path...") +result = client.moveOnPathAsync([airsim.Vector3r(125,0,z), + airsim.Vector3r(125,-130,z), + airsim.Vector3r(0,-130,z), + airsim.Vector3r(0,0,z)], + 12, 120, airsim.DrivetrainType.ForwardOnly, airsim.YawMode(False,0), 20, 1).join() + +# drone will over-shoot so we bring it back to the start point before landing. client.moveToPositionAsync(0,0,z,1).join() +print("landing...") client.landAsync().join() +print("disarming...") client.armDisarm(False) client.enableApiControl(False) +print("done.") diff --git a/PythonClient/multirotor/set_trace_line.py b/PythonClient/multirotor/set_trace_line.py new file mode 100644 index 000000000..9324af8ec --- /dev/null +++ b/PythonClient/multirotor/set_trace_line.py @@ -0,0 +1,46 @@ +# Please add "EnableTrace": true to your setting.json as shown below + +# { +# "SettingsVersion": 1.2, +# "SimMode": "Multirotor", +# "Vehicles": { +# "Drone": { +# "VehicleType": "SimpleFlight", +# "EnableTrace": true +# } +# } +# } + +import setup_path +import airsim +import time + +# connect to the AirSim simulator +client = airsim.MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +client.takeoffAsync().join() +client.hoverAsync().join() + +vehicleControl = client.moveByVelocityAsync(1, 1, 0, 12) + +client.simSetTraceLine([1.0, 0.0, 0.0, 1.0], 5) +time.sleep(2) +client.simSetTraceLine([0.0, 1.0, 0.0, 0.8], 10) +time.sleep(2) +client.simSetTraceLine([0.0, 0.0, 1.0, 0.6], 20) +time.sleep(2) +client.simSetTraceLine([1.0, 1.0, 0.0, 0.4], 30) +time.sleep(2) +client.simSetTraceLine([0.0, 1.0, 1.0, 0.2], 40) +time.sleep(2) +client.simSetTraceLine([1.0, 0.0, 1.0, 0.1], 50) +time.sleep(2) + +vehicleControl.join() + +client.armDisarm(False) +client.takeoffAsync().join() +client.enableApiControl(False) diff --git a/PythonClient/multirotor/state.py b/PythonClient/multirotor/state.py new file mode 100644 index 000000000..7731c2491 --- /dev/null +++ b/PythonClient/multirotor/state.py @@ -0,0 +1,20 @@ +import setup_path +import airsim +import pprint + +def print_state(): + print("===============================================================") + state = client.getMultirotorState() + print("state: %s" % pprint.pformat(state)) + return state + + +client = airsim.MultirotorClient() +state = print_state() + +if state.ready: + print("drone is ready!") +else: + print("drone is not ready!") +if state.ready_message: + print(state.ready_message) diff --git a/PythonClient/ros/car_image_raw.py b/PythonClient/ros/car_image_raw.py deleted file mode 100644 index 4ffe5bece..000000000 --- a/PythonClient/ros/car_image_raw.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/usr/bin/env python - -# Example ROS node for publishing AirSim images. - -import setup_path -import airsim - -import rospy - -# ROS Image message -from sensor_msgs.msg import Image - -def airpub(): - pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1) - rospy.init_node('image_raw', anonymous=True) - rate = rospy.Rate(10) # 10hz - - # connect to the AirSim simulator - client = airsim.CarClient() - client.confirmConnection() - - while not rospy.is_shutdown(): - # get camera images from the car - responses = client.simGetImages([ - airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGB array - - for response in responses: - img_rgb_string = response.image_data_uint8 - - # Populate image message - msg=Image() - msg.header.stamp = rospy.Time.now() - msg.header.frame_id = "frameId" - msg.encoding = "rgb8" - msg.height = 360 # resolution should match values in settings.json - msg.width = 640 - msg.data = img_rgb_string - msg.is_bigendian = 0 - msg.step = msg.width * 3 - - # log time and size of published image - rospy.loginfo(len(response.image_data_uint8)) - # publish image message - pub.publish(msg) - # sleep until next cycle - rate.sleep() - - -if __name__ == '__main__': - try: - airpub() - except rospy.ROSInterruptException: - pass diff --git a/PythonClient/ros/car_pose.py b/PythonClient/ros/car_pose.py deleted file mode 100644 index 789a635e3..000000000 --- a/PythonClient/ros/car_pose.py +++ /dev/null @@ -1,60 +0,0 @@ -#!/usr/bin/env python - -import setup_path -import airsim - -import rospy -import tf -from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped - -import time - - -def airpub(): - pub = rospy.Publisher("airsimPose", PoseStamped, queue_size=1) - rospy.init_node('airpub', anonymous=True) - rate = rospy.Rate(10) # 10hz - - # connect to the AirSim simulator - client = airsim.CarClient() - client.confirmConnection() - -# start = time.time() - - - while not rospy.is_shutdown(): - - # get state of the car - car_state = client.getCarState() - pos = car_state.kinematics_estimated.position - orientation = car_state.kinematics_estimated.orientation -# milliseconds = (time.time() - start) * 1000 - - - # populate PoseStamped ros message - simPose = PoseStamped() - simPose.pose.position.x = pos.x_val - simPose.pose.position.y = pos.y_val - simPose.pose.position.z = pos.z_val - simPose.pose.orientation.w = orientation.w_val - simPose.pose.orientation.x = orientation.x_val - simPose.pose.orientation.y = orientation.y_val - simPose.pose.orientation.z = orientation.z_val - simPose.header.stamp = rospy.Time.now() - simPose.header.seq = 1 - simPose.header.frame_id = "simFrame" - - # log PoseStamped message - rospy.loginfo(simPose) - #publish PoseStamped message - pub.publish(simPose) - # sleeps until next cycle - rate.sleep() - - -if __name__ == '__main__': - try: - airpub() - except rospy.ROSInterruptException: - pass diff --git a/PythonClient/ros/drone_image_raw.py b/PythonClient/ros/drone_image_raw.py deleted file mode 100644 index b41ba6248..000000000 --- a/PythonClient/ros/drone_image_raw.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python - -# Example ROS node for publishing AirSim images. - -# AirSim Python API -import setup_path -import airsim - -import rospy - -# ROS Image message -from sensor_msgs.msg import Image - -def airpub(): - pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1) - rospy.init_node('image_raw', anonymous=True) - rate = rospy.Rate(10) # 10hz - - # connect to the AirSim simulator - client = airsim.MultirotorClient() - client.confirmConnection() - - while not rospy.is_shutdown(): - # get camera images from the car - responses = client.simGetImages([ - airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGB array - - for response in responses: - img_rgb_string = response.image_data_uint8 - - # Populate image message - msg=Image() - msg.header.stamp = rospy.Time.now() - msg.header.frame_id = "frameId" - msg.encoding = "rgb8" - msg.height = 360 # resolution should match values in settings.json - msg.width = 640 - msg.data = img_rgb_string - msg.is_bigendian = 0 - msg.step = msg.width * 3 - - # log time and size of published image - rospy.loginfo(len(response.image_data_uint8)) - # publish image message - pub.publish(msg) - # sleep until next cycle - rate.sleep() - - -if __name__ == '__main__': - try: - airpub() - except rospy.ROSInterruptException: - pass diff --git a/PythonClient/ros/setup_path.py b/PythonClient/ros/setup_path.py deleted file mode 100644 index 362113fea..000000000 --- a/PythonClient/ros/setup_path.py +++ /dev/null @@ -1,52 +0,0 @@ -# Import this module to automatically setup path to local airsim module -# This module first tries to see if airsim module is installed via pip -# If it does then we don't do anything else -# Else we look up grand-parent folder to see if it has airsim folder -# and if it does then we add that in sys.path - -import os,sys,inspect,logging - -#this class simply tries to see if airsim -class SetupPath: - @staticmethod - def getDirLevels(path): - path_norm = os.path.normpath(path) - return len(path_norm.split(os.sep)) - - @staticmethod - def getCurrentPath(): - cur_filepath = os.path.abspath(inspect.getfile(inspect.currentframe())) - return os.path.dirname(cur_filepath) - - @staticmethod - def getGrandParentDir(): - cur_path = SetupPath.getCurrentPath() - if SetupPath.getDirLevels(cur_path) >= 2: - return os.path.dirname(os.path.dirname(cur_path)) - return '' - - @staticmethod - def getParentDir(): - cur_path = SetupPath.getCurrentPath() - if SetupPath.getDirLevels(cur_path) >= 1: - return os.path.dirname(cur_path) - return '' - - @staticmethod - def addAirSimModulePath(): - # if airsim module is installed then don't do anything else - #import pkgutil - #airsim_loader = pkgutil.find_loader('airsim') - #if airsim_loader is not None: - # return - - parent = SetupPath.getParentDir() - if parent != '': - airsim_path = os.path.join(parent, 'airsim') - client_path = os.path.join(airsim_path, 'client.py') - if os.path.exists(client_path): - sys.path.insert(0, parent) - else: - logging.warning("airsim module not found in parent folder. Using installed package (pip install airsim).") - -SetupPath.addAirSimModulePath() diff --git a/PythonClient/setup.py b/PythonClient/setup.py index 13b7b0d90..43521d757 100644 --- a/PythonClient/setup.py +++ b/PythonClient/setup.py @@ -5,7 +5,7 @@ setuptools.setup( name="airsim", - version="1.2.3", + version="1.2.8", author="Shital Shah", author_email="shitals@microsoft.com", description="Open source simulator based on Unreal Engine for autonomous vehicles from Microsoft AI & Research", @@ -20,6 +20,6 @@ "Operating System :: OS Independent", ), install_requires=[ - 'msgpack-rpc-python', 'numpy' + 'msgpack-rpc-python', 'numpy', 'opencv-contrib-python' ] -) \ No newline at end of file +) diff --git a/README.md b/README.md index b196622b0..663b901e8 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Welcome to AirSim -AirSim is a simulator for drones, cars and more, built on [Unreal Engine](https://www.unrealengine.com/) (we now also have an experimental [Unity](https://unity3d.com/) release). It is open-source, cross platform, and supports hardware-in-loop with popular flight controllers such as PX4 for physically and visually realistic simulations. It is developed as an Unreal plugin that can simply be dropped into any Unreal environment. Similarly, we have an experimental release for a Unity plugin. +AirSim is a simulator for drones, cars and more, built on [Unreal Engine](https://www.unrealengine.com/) (we now also have an experimental [Unity](https://unity3d.com/) release). It is open-source, cross platform, and supports hardware-in-loop with popular flight controllers such as PX4 for physically and visually realistic simulations. It is developed as an Unreal plugin that can simply be dropped into any Unreal environment. Similarly, we have an experimental release for a Unity plugin. Our goal is to develop AirSim as a platform for AI research to experiment with deep learning, computer vision and reinforcement learning algorithms for autonomous vehicles. For this purpose, AirSim also exposes APIs to retrieve data and control vehicles in a platform independent way. @@ -15,29 +15,34 @@ Cars in AirSim [![AirSim Car Demo Video](docs/images/car_demo_video.png)](https://youtu.be/gnz1X3UNM5Y) ## What's New -* A ROS wrapper for multirotors is available. See [airsim_ros_pkgs](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_ros_pkgs) for the ROS API, and [airsim_tutorial_pkgs](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_tutorial_pkgs) for tutorials. -* [Added sensor APIs for Barometer, IMU, GPS, Magnetometer, Distance Sensor](https://microsoft.github.io/AirSim/docs/sensors.md) -* Added support for [docker in ubuntu](https://microsoft.github.io/AirSim/docs/docker_ubuntu) -* Added Weather Effects and [APIs](https://microsoft.github.io/AirSim/docs/apis#weather-apis) -* Added [Time of Day API](https://microsoft.github.io/AirSim/docs/apis#time-of-day-api) -* An experimental integration of [AirSim on Unity](https://github.com/Microsoft/AirSim/tree/master/Unity) is now available. Learn more in [Unity blog post](https://blogs.unity3d.com/2018/11/14/airsim-on-unity-experiment-with-autonomous-vehicle-simulation). -* [New environments](https://github.com/Microsoft/AirSim/releases/tag/v1.2.1): Forest, Plains (windmill farm), TalkingHeads (human head simulation), TrapCam (animal detection via camera) -* Highly efficient [NoDisplay view mode](https://microsoft.github.io/AirSim/docs/settings#viewmode) to turn off main screen rendering so you can capture images at high rate -* [Lidar Sensor](https://microsoft.github.io/AirSim/docs/lidar) -* Case Study: [Formula Student Technion Driverless](https://github.com/Microsoft/AirSim/wiki/technion) -* [Multi-Vehicle Capability](https://microsoft.github.io/AirSim/docs/multi_vehicle) -* [ROS publisher](https://github.com/Microsoft/AirSim/pull/1135) - -For complete list of changes, view our [Changelog](CHANGELOG.md) + +* Latest release `v1.3.1` for [Windows](https://github.com/microsoft/AirSim/releases/tag/v1.3.1-windows) and [Linux](https://github.com/microsoft/AirSim/releases/tag/v1.3.1-linux) +* Upgraded to Unreal Engine 4.24, Visual Studio 2019, Clang 8, C++ 17 standard +* Mac OSX Catalina support +* Updated [airsim](https://pypi.org/project/airsim/) Python package, with lots of new APIs +* [Removed legacy API wrappers](https://github.com/microsoft/AirSim/pull/2494) +* [Support for latest PX4 stable release](https://microsoft.github.io/AirSim/px4_setup/) +* Support for [ArduPilot](https://ardupilot.org/ardupilot/) - [Copter, Rover vehicles](https://ardupilot.org/dev/docs/sitl-with-airsim.html) +* [Updated Unity support](https://microsoft.github.io/AirSim/Unity/) +* [Removed simChar* APIs](https://github.com/microsoft/AirSim/pull/2493) +* [Plotting APIs for Debugging](https://github.com/microsoft/AirSim/pull/2304) +* ROS wrapper for multirotors is available. See [airsim_ros_pkgs](https://microsoft.github.io/AirSim/airsim_ros_pkgs/) for the ROS API, and [airsim_tutorial_pkgs](https://microsoft.github.io/AirSim/airsim_tutorial_pkgs/) for tutorials. +* Added support for [docker in ubuntu](https://microsoft.github.io/AirSim/docker_ubuntu/) + +For complete list of changes, view our [Changelog](docs/CHANGELOG.md) ## How to Get It ### Windows -* [Download binaries](https://microsoft.github.io/AirSim/docs/use_precompiled) -* [Build it](https://microsoft.github.io/AirSim/docs/build_windows) +* [Download binaries](https://github.com/Microsoft/AirSim/releases) +* [Build it](https://microsoft.github.io/AirSim/build_windows) ### Linux -* [Build it](https://microsoft.github.io/AirSim/docs/build_linux) +* [Download binaries](https://github.com/Microsoft/AirSim/releases) +* [Build it](https://microsoft.github.io/AirSim/build_linux) + +### macOS +* [Build it](https://microsoft.github.io/AirSim/build_linux) [![Build Status](https://travis-ci.org/Microsoft/AirSim.svg?branch=master)](https://travis-ci.org/Microsoft/AirSim) @@ -51,7 +56,7 @@ View our [detailed documentation](https://microsoft.github.io/AirSim/) on all as If you have remote control (RC) as shown below, you can manually control the drone in the simulator. For cars, you can use arrow keys to drive manually. -[More details](https://microsoft.github.io/AirSim/docs/remote_control/) +[More details](https://microsoft.github.io/AirSim/remote_control/) ![record screenshot](docs/images/AirSimDroneManual.gif) @@ -64,9 +69,9 @@ AirSim exposes APIs so you can interact with the vehicle in the simulation progr These APIs are also available as part of a separate, independent cross-platform library, so you can deploy them on a companion computer on your vehicle. This way you can write and test your code in the simulator, and later execute it on the real vehicles. Transfer learning and related research is one of our focus areas. -Note that you can use [SimMode setting](https://microsoft.github.io/AirSim/docs/settings#simmode) to specify the default vehicle or the new [ComputerVision mode](https://microsoft.github.io/AirSim/docs/image_apis#computer-vision-mode-1) so you don't get prompted each time you start AirSim. +Note that you can use [SimMode setting](https://microsoft.github.io/AirSim/settings#simmode) to specify the default vehicle or the new [ComputerVision mode](https://microsoft.github.io/AirSim/image_apis#computer-vision-mode-1) so you don't get prompted each time you start AirSim. -[More details](https://microsoft.github.io/AirSim/docs/apis/) +[More details](https://microsoft.github.io/AirSim/apis/) ### Gathering training data @@ -74,17 +79,17 @@ There are two ways you can generate training data from AirSim for deep learning. ![record screenshot](docs/images/record_data.png) -A better way to generate training data exactly the way you want is by accessing the APIs. This allows you to be in full control of how, what, where and when you want to log data. +A better way to generate training data exactly the way you want is by accessing the APIs. This allows you to be in full control of how, what, where and when you want to log data. ### Computer Vision mode -Yet another way to use AirSim is the so-called "Computer Vision" mode. In this mode, you don't have vehicles or physics. You can use the keyboard to move around the scene, or use APIs to position available cameras in any arbitrary pose, and collect images such as depth, disparity, surface normals or object segmentation. +Yet another way to use AirSim is the so-called "Computer Vision" mode. In this mode, you don't have vehicles or physics. You can use the keyboard to move around the scene, or use APIs to position available cameras in any arbitrary pose, and collect images such as depth, disparity, surface normals or object segmentation. -[More details](https://microsoft.github.io/AirSim/docs/image_apis/) +[More details](https://microsoft.github.io/AirSim/image_apis/) ### Weather Effects -Press F10 to see various options available for weather effects. You can also control the weather using [APIs](https://microsoft.github.io/AirSim/docs/apis#weather-apis). Press F1 to see other options available. +Press F10 to see various options available for weather effects. You can also control the weather using [APIs](https://microsoft.github.io/AirSim/apis#weather-apis). Press F1 to see other options available. ![record screenshot](docs/images/weather_menu.png) @@ -117,14 +122,14 @@ More technical details are available in [AirSim paper (FSR 2017 Conference)](htt Please take a look at [open issues](https://github.com/microsoft/airsim/issues) if you are looking for areas to contribute to. -* [More on AirSim design](https://microsoft.github.io/AirSim/docs/design) -* [More on code structure](https://microsoft.github.io/AirSim/docs/code_structure) +* [More on AirSim design](https://microsoft.github.io/AirSim/design) +* [More on code structure](https://microsoft.github.io/AirSim/code_structure) * [Contribution Guidelines](CONTRIBUTING.md) * [Trello Board](https://trello.com/b/1t2qCeaA/wishlist-by-community-for-community) ### Who is Using AirSim? -We are maintaining a [list](https://microsoft.github.io/AirSim/docs/who_is_using) of a few projects, people and groups that we are aware of. If you would like to be featured in this list please [make a request here](https://github.com/microsoft/airsim/issues). +We are maintaining a [list](https://microsoft.github.io/AirSim/who_is_using) of a few projects, people and groups that we are aware of. If you would like to be featured in this list please [make a request here](https://github.com/microsoft/airsim/issues). ## Contact @@ -132,7 +137,7 @@ Join the AirSim group on [Facebook](https://www.facebook.com/groups/122583246753 ## FAQ -If you run into problems, check the [FAQ](https://microsoft.github.io/AirSim/docs/faq) and feel free to post issues in the [AirSim](https://github.com/Microsoft/AirSim/issues) repository. +If you run into problems, check the [FAQ](https://microsoft.github.io/AirSim/faq) and feel free to post issues in the [AirSim](https://github.com/Microsoft/AirSim/issues) repository. ## License diff --git a/SGM/src/sgmstereo/sgmstereo.cpp b/SGM/src/sgmstereo/sgmstereo.cpp index bc90b60a4..5c11bd66f 100644 --- a/SGM/src/sgmstereo/sgmstereo.cpp +++ b/SGM/src/sgmstereo/sgmstereo.cpp @@ -55,7 +55,7 @@ SGMStereo::SGMStereo(int _w, int _h, int minDisparity, int maxDisparity, int num wLUT = new float[256]; for (int i = 0; i < 256; i++) { - wLUT[i] = m_penalty1 + m_alpha * exp(-i * rec_penalty2); + wLUT[i] = (float)(m_penalty1 + m_alpha * exp(-i * rec_penalty2)); } } @@ -327,7 +327,7 @@ void SGMStereo::scanlineOptimization(DSI &dv, DSI &msgs, unsigned char* img, flo short * buffervec = (short*)_aligned_malloc(planes * sizeof(short), 16); - float dist = sqrt((float)(dx_*dx_+dy_*dy_)); + float dist = (float)sqrt((dx_*dx_+dy_*dy_)); for (int j = 0; j < (int)startx.size(); j++) { diff --git a/SGM/src/sgmstereo/sgmstereo_vc15.vcxproj b/SGM/src/sgmstereo/sgmstereo.vcxproj similarity index 95% rename from SGM/src/sgmstereo/sgmstereo_vc15.vcxproj rename to SGM/src/sgmstereo/sgmstereo.vcxproj index 1ea99c8b6..e2fd6802e 100644 --- a/SGM/src/sgmstereo/sgmstereo_vc15.vcxproj +++ b/SGM/src/sgmstereo/sgmstereo.vcxproj @@ -1,5 +1,5 @@  - + @@ -34,30 +34,30 @@ SAK SAK SAK - sgmstereo + sgmstereo StaticLibrary Unicode true - v141 + v142 StaticLibrary Unicode true - v141 + v142 StaticLibrary Unicode - v141 + v142 StaticLibrary Unicode - v141 + v142 @@ -110,7 +110,6 @@ Level4 ProgramDatabase NOMINMAX;_UNICODE;UNICODE;%(PreprocessorDefinitions) - false @@ -137,7 +136,6 @@ ProgramDatabase NOMINMAX;_UNICODE;UNICODE;%(PreprocessorDefinitions) true - true diff --git a/SGM/src/stereoPipeline/StateStereo.cpp b/SGM/src/stereoPipeline/StateStereo.cpp index 59b4596ee..5068ff92c 100644 --- a/SGM/src/stereoPipeline/StateStereo.cpp +++ b/SGM/src/stereoPipeline/StateStereo.cpp @@ -124,7 +124,7 @@ float CStateStereo::GetLeftDisparity(float x, float y) unsigned char c = confMap[off]; if (fabs(d) < ndisps && c >= confThreshold) { - return 1.0f - (fabs(d)/float(ndisps)); + return 1.0f - (float)(fabs(d)/ndisps); } else return -1.0f; diff --git a/SGM/src/stereoPipeline/stereoPipeline_vc15.vcxproj b/SGM/src/stereoPipeline/stereoPipeline.vcxproj similarity index 97% rename from SGM/src/stereoPipeline/stereoPipeline_vc15.vcxproj rename to SGM/src/stereoPipeline/stereoPipeline.vcxproj index be1589fd2..9bdca81c2 100644 --- a/SGM/src/stereoPipeline/stereoPipeline_vc15.vcxproj +++ b/SGM/src/stereoPipeline/stereoPipeline.vcxproj @@ -1,5 +1,5 @@  - + @@ -37,27 +37,27 @@ StaticLibrary true Unicode - v141 + v142 StaticLibrary true Unicode - v141 + v142 StaticLibrary false true Unicode - v141 + v142 StaticLibrary false true Unicode - v141 + v142 diff --git a/SUPPORT.md b/SUPPORT.md index ea1e7658b..4580e6049 100644 --- a/SUPPORT.md +++ b/SUPPORT.md @@ -1,6 +1,3 @@ # Support -We highly recommend to take a look at source code and contribute to the project. Due to large number of incoming feature request we may not be able to get to your request in your desired timeframe. So please [contribute](CONTRIBUTING.md) :). - -* [Join AirSim Facebook Group](https://www.facebook.com/groups/1225832467530667/) -* [File GitHub Issue](https://github.com/Microsoft/AirSim/issues) \ No newline at end of file +This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/SUPPORT.md). diff --git a/Unreal/Environments/Blocks/Blocks.uproject b/Unreal/Environments/Blocks/Blocks.uproject index 7ed3332dc..4dc1bb368 100644 --- a/Unreal/Environments/Blocks/Blocks.uproject +++ b/Unreal/Environments/Blocks/Blocks.uproject @@ -1,6 +1,6 @@ { "FileVersion": 3, - "EngineAssociation": "4.18", + "EngineAssociation": "4.22", "Category": "", "Description": "", "Modules": [ @@ -17,6 +17,14 @@ { "Name": "AirSim", "Enabled": true - } + }, + { + "Name": "SteamVR", + "Enabled": false + }, + { + "Name": "OculusVR", + "Enabled": false + }, ] -} \ No newline at end of file +} diff --git a/Unreal/Environments/Blocks/Config/DefaultEngine.ini b/Unreal/Environments/Blocks/Config/DefaultEngine.ini index 14ff4fd8d..7fe134840 100644 --- a/Unreal/Environments/Blocks/Config/DefaultEngine.ini +++ b/Unreal/Environments/Blocks/Config/DefaultEngine.ini @@ -12,7 +12,7 @@ GlobalDefaultGameMode=/Script/Blocks.BlocksGameMode GlobalDefaultServerGameMode=None [/Script/IOSRuntimeSettings.IOSRuntimeSettings] -MinimumiOSVersion=IOS_8 +MinimumiOSVersion=IOS_11 [/Script/Engine.Engine] diff --git a/Unreal/Environments/Blocks/Source/Blocks.Target.4.15.cs b/Unreal/Environments/Blocks/Source/Blocks.Target.4.15.cs deleted file mode 100644 index 3d56c3966..000000000 --- a/Unreal/Environments/Blocks/Source/Blocks.Target.4.15.cs +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 1998-2017 Epic Games, Inc. All Rights Reserved. - -using UnrealBuildTool; -using System.Collections.Generic; - -public class BlocksTarget : TargetRules -{ - public BlocksTarget(TargetInfo Target) - { - Type = TargetType.Game; - } - - // - // TargetRules interface. - // - - public override void SetupBinaries( - TargetInfo Target, - ref List OutBuildBinaryConfigurations, - ref List OutExtraModuleNames - ) - { - OutExtraModuleNames.Add("Blocks"); - } -} diff --git a/Unreal/Environments/Blocks/Source/Blocks/Blocks.Build.4.15.cs b/Unreal/Environments/Blocks/Source/Blocks/Blocks.Build.4.15.cs deleted file mode 100644 index 5852ecc55..000000000 --- a/Unreal/Environments/Blocks/Source/Blocks/Blocks.Build.4.15.cs +++ /dev/null @@ -1,11 +0,0 @@ -// Copyright 1998-2017 Epic Games, Inc. All Rights Reserved. - -using UnrealBuildTool; - -public class Blocks : ModuleRules -{ - public Blocks(TargetInfo Target) - { - PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore" }); - } -} diff --git a/Unreal/Environments/Blocks/Source/BlocksEditor.Target.4.15.cs b/Unreal/Environments/Blocks/Source/BlocksEditor.Target.4.15.cs deleted file mode 100644 index 184e12de9..000000000 --- a/Unreal/Environments/Blocks/Source/BlocksEditor.Target.4.15.cs +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 1998-2017 Epic Games, Inc. All Rights Reserved. - -using UnrealBuildTool; -using System.Collections.Generic; - -public class BlocksEditorTarget : TargetRules -{ - public BlocksEditorTarget(TargetInfo Target) - { - Type = TargetType.Editor; - } - - // - // TargetRules interface. - // - - public override void SetupBinaries( - TargetInfo Target, - ref List OutBuildBinaryConfigurations, - ref List OutExtraModuleNames - ) - { - OutExtraModuleNames.Add("Blocks"); - } -} diff --git a/Unreal/Environments/Blocks/Source/BlocksEditor.Target.cs b/Unreal/Environments/Blocks/Source/BlocksEditor.Target.cs index 4ec09ba18..187daed54 100644 --- a/Unreal/Environments/Blocks/Source/BlocksEditor.Target.cs +++ b/Unreal/Environments/Blocks/Source/BlocksEditor.Target.cs @@ -9,7 +9,7 @@ public BlocksEditorTarget(TargetInfo Target) : base(Target) { Type = TargetType.Editor; ExtraModuleNames.AddRange(new string[] { "Blocks" }); - + DefaultBuildSettings = BuildSettingsVersion.V2; //bUseUnityBuild = false; //bUsePCHFiles = false; } diff --git a/Unreal/Environments/Blocks/package.bat b/Unreal/Environments/Blocks/package.bat index 80f53ac63..7969e7662 100644 --- a/Unreal/Environments/Blocks/package.bat +++ b/Unreal/Environments/Blocks/package.bat @@ -7,7 +7,7 @@ set OutPath=%1 set ToolPath=%2 set UEVer=%3 -if "%UEVer%"=="" set "UEVer=4.18" +if "%UEVer%"=="" set "UEVer=4.24" set "_ToolPath=%PROGRAMFILES%\Epic Games\UE_%UEVer%\Engine\Build\BatchFiles" if "%ToolPath%"=="" set ToolPath=%_ToolPath% diff --git a/Unreal/Environments/Blocks/update_from_git_ci.bat b/Unreal/Environments/Blocks/update_from_git_ci.bat new file mode 100644 index 000000000..58c565c83 --- /dev/null +++ b/Unreal/Environments/Blocks/update_from_git_ci.bat @@ -0,0 +1,34 @@ +@echo off +REM //---------- set up variable ---------- +setlocal +set ROOT_DIR=%~dp0 + +set AirSimPath=%1 + +REM default path works for Blocks environment +if "%AirSimPath%"=="" set "AirSimPath=..\..\.." + +IF NOT EXIST "%AirSimPath%" ( + echo "AirSimPath %AirSimPath% was not found" + goto :failed +) + +echo Using AirSimPath = %AirSimPath% + +robocopy /MIR "%AirSimPath%\Unreal\Plugins\AirSim" Plugins\AirSim /XD temp *. /njh /njs /ndl /np +robocopy /MIR "%AirSimPath%\AirLib" Plugins\AirSim\Source\AirLib /XD temp *. /njh /njs /ndl /np +robocopy /njh /njs /ndl /np "%AirSimPath%\Unreal\Environments\Blocks" "." *.bat +robocopy /njh /njs /ndl /np "%AirSimPath%\Unreal\Environments\Blocks" "." *.sh +rem robocopy /njh /njs /ndl /np "%AirSimPath%" "." *.gitignore + +REM cmd /c clean.bat +REM cmd /c GenerateProjectFiles.bat + +goto :done + +:failed +echo Error occured while updating. +exit /b 1 + +:done +REM if "%1"=="" pause \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/AirSim.uplugin b/Unreal/Plugins/AirSim/AirSim.uplugin index a467e4187..bbbf5cc8b 100644 --- a/Unreal/Plugins/AirSim/AirSim.uplugin +++ b/Unreal/Plugins/AirSim/AirSim.uplugin @@ -1,7 +1,7 @@ { "FileVersion" : 3, - "Version" : "1.2.0", - "VersionName": "1.2.0", + "Version" : "1.3.1", + "VersionName": "1.3.1", "FriendlyName": "AirSim", "Description": "AirSim - Autonomous Aerial Vehicles Simulator Plugin", "Category" : "Science", diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG new file mode 100644 index 000000000..7cd3e4f6e Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG differ diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index e16579221..58a8ea4f5 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -9,18 +9,21 @@ #include "Components/StaticMeshComponent.h" #include "EngineUtils.h" #include "Runtime/Engine/Classes/Engine/StaticMesh.h" -#include "UObjectIterator.h" +#include "Runtime/Engine/Classes/Engine/LevelStreamingDynamic.h" +#include "UObject/UObjectIterator.h" #include "Camera/CameraComponent.h" -//#include "Runtime/Foliage/Public/FoliageType.h" -#include "MessageDialog.h" +#include "Runtime/Engine/Classes/GameFramework/PlayerStart.h" +#include "Misc/MessageDialog.h" #include "Engine/LocalPlayer.h" #include "Engine/SkeletalMesh.h" #include "Slate/SceneViewport.h" #include "IImageWrapper.h" -#include "ObjectThumbnail.h" +#include "Misc/ObjectThumbnail.h" #include "Engine/Engine.h" +#include "Engine/World.h" #include #include "common/common_utils/Utils.hpp" +#include "Modules/ModuleManager.h" /* //TODO: change naming conventions to same as other files? @@ -29,6 +32,7 @@ Methods -> CamelCase parameters -> camel_case */ +ULevelStreamingDynamic *UAirBlueprintLib::CURRENT_LEVEL = nullptr; bool UAirBlueprintLib::log_messages_hidden_ = false; msr::airlib::AirSimSettings::SegmentationSetting::MeshNamingMethodType UAirBlueprintLib::mesh_naming_method_ = msr::airlib::AirSimSettings::SegmentationSetting::MeshNamingMethodType::OwnerName; @@ -72,6 +76,45 @@ void UAirBlueprintLib::setSimulatePhysics(AActor* actor, bool simulate_physics) } } +ULevelStreamingDynamic* UAirBlueprintLib::loadLevel(UObject* context, const FString& level_name) +{ + bool success{ false }; + context->GetWorld()->SetNewWorldOrigin(FIntVector(0,0,0)); + ULevelStreamingDynamic* new_level = UAirsimLevelStreaming::LoadAirsimLevelInstance( + context->GetWorld(), level_name, FVector(0, 0, 0), FRotator(0, 0, 0), success); + if (success) + { + if(CURRENT_LEVEL != nullptr && CURRENT_LEVEL->IsValidLowLevel()) + CURRENT_LEVEL->SetShouldBeLoaded(false); + CURRENT_LEVEL = new_level; + } + return CURRENT_LEVEL; +} + +bool UAirBlueprintLib::spawnPlayer(UWorld* context) +{ + + bool success{ false }; + TArray player_start_actors; + FindAllActor(context, player_start_actors); + if (player_start_actors.Num() > 1) + { + for (auto player_start : player_start_actors) + { + if (player_start->GetName() != FString("SuperStart")) + { + //context->GetWorld()->SetNewWorldOrigin(FIntVector(0, 0, 0)); + auto location = player_start->GetActorLocation(); + context->RequestNewWorldOrigin(FIntVector(location.X, location.Y, location.Z)); + success = true; + break; + } + } + } + return success; +} + + std::vector UAirBlueprintLib::getPhysicsComponents(AActor* actor) { std::vector phys_comps; @@ -323,14 +366,33 @@ bool UAirBlueprintLib::SetMeshStencilID(const std::string& mesh_name, int object int UAirBlueprintLib::GetMeshStencilID(const std::string& mesh_name) { - FString fmesh_name(mesh_name.c_str()); - for (TObjectIterator comp; comp; ++comp) - { - // Access the subclass instance with the * or -> operators. - UMeshComponent *mesh = *comp; - if (mesh->GetName() == fmesh_name) { + // Takes a UStaticMeshComponent, USkinnedMeshComponent or ALandscapeProxy and returns their custom stencil ID if + // their meshes's name or their owner's name (depending on the naming method in mesh_naming_method_) equals mesh_name + auto getCustomStencilForMesh = [&mesh_name](auto mesh) -> int { + const std::string component_mesh_name = common_utils::Utils::toLower(GetMeshName(mesh)); + if (component_mesh_name.compare(mesh_name) == 0) { return mesh->CustomDepthStencilValue; } + return -1; + }; + + for (TObjectIterator comp; comp; ++comp) + { + int id = getCustomStencilForMesh(*comp); + if(id != -1) + return id; + } + for (TObjectIterator comp; comp; ++comp) + { + int id = getCustomStencilForMesh(*comp); + if (id != -1) + return id; + } + for (TObjectIterator comp; comp; ++comp) + { + int id = getCustomStencilForMesh(*comp); + if (id != -1) + return id; } return -1; @@ -352,6 +414,166 @@ std::vector UAirBlueprintLib::ListMatchingActors(const UObject *con return results; } +std::vector UAirBlueprintLib::GetStaticMeshComponents() +{ + std::vector meshes; + int num_meshes = 0; + for (TObjectIterator comp; comp; ++comp) + { + *comp; + + std::string name = common_utils::Utils::toLower(GetMeshName(*comp)); + //The skybox is ignored here as it is huge, and really is of no use to the end user typically. Also the associated meshes with the cameras + if (name == "" || common_utils::Utils::startsWith(name, "default_") + || common_utils::Utils::startsWith(name, "sky") + || common_utils::Utils::startsWith(name, "camera")) + { + continue; + } + + //Various checks if there is even a valid mesh + if (!comp->GetStaticMesh()) continue; + if (!comp->GetStaticMesh()->RenderData) continue; + if (comp->GetStaticMesh()->RenderData->LODResources.Num() == 0) continue; + + msr::airlib::MeshPositionVertexBuffersResponse mesh; + mesh.name = name; + + FVector pos = comp->GetComponentLocation(); + FQuat att = comp->GetComponentQuat(); + mesh.position[0] = pos.X; + mesh.position[1] = pos.Y; + mesh.position[2] = pos.Z; + mesh.orientation.w() = att.W; + mesh.orientation.x() = att.X; + mesh.orientation.y() = att.Y; + mesh.orientation.z() = att.Z; + + FPositionVertexBuffer* vertex_buffer = &comp->GetStaticMesh()->RenderData->LODResources[0].VertexBuffers.PositionVertexBuffer; + if (vertex_buffer) + { + const int32 vertex_count = vertex_buffer->VertexBufferRHI->GetSize(); + TArray vertices; + vertices.SetNum(vertex_count); + FVector* data = vertices.GetData(); + + ENQUEUE_RENDER_COMMAND(GetVertexBuffer)( + [vertex_buffer, data](FRHICommandListImmediate& RHICmdList) + { + FVector* indices = (FVector*)RHILockVertexBuffer(vertex_buffer->VertexBufferRHI, 0, vertex_buffer->VertexBufferRHI->GetSize(), RLM_ReadOnly); + memcpy(data, indices, vertex_buffer->VertexBufferRHI->GetSize()); + RHIUnlockVertexBuffer(vertex_buffer->VertexBufferRHI); + }); + + FStaticMeshLODResources& lod = comp->GetStaticMesh()->RenderData->LODResources[0]; + FRawStaticIndexBuffer* IndexBuffer = &lod.IndexBuffer; + int num_indices = IndexBuffer->IndexBufferRHI->GetSize() / IndexBuffer->IndexBufferRHI->GetStride(); + + if (IndexBuffer->IndexBufferRHI->GetStride() == 2) { + TArray indices_vec; + indices_vec.SetNum(num_indices); + + uint16_t* data_ptr = indices_vec.GetData(); + + ENQUEUE_RENDER_COMMAND(GetIndexBuffer)( + [IndexBuffer, data_ptr](FRHICommandListImmediate& RHICmdList) + { + uint16_t* indices = (uint16_t*)RHILockIndexBuffer(IndexBuffer->IndexBufferRHI, 0, IndexBuffer->IndexBufferRHI->GetSize(), RLM_ReadOnly); + memcpy(data_ptr, indices, IndexBuffer->IndexBufferRHI->GetSize()); + RHIUnlockIndexBuffer(IndexBuffer->IndexBufferRHI); + }); + + //Need to force the render command to go through cause on the next iteration the buffer no longer exists + FlushRenderingCommands(); + + mesh.indices.resize(num_indices); + for (int idx = 0; idx < num_indices; ++idx) { + mesh.indices[idx] = indices_vec[idx]; + } + } + + else { //stride ==4 + TArray indices_vec; + indices_vec.SetNum(num_indices); + + uint32_t* data_ptr = indices_vec.GetData(); + + ENQUEUE_RENDER_COMMAND(GetIndexBuffer)( + [IndexBuffer, data_ptr](FRHICommandListImmediate& RHICmdList) + { + uint32_t* indices = (uint32_t*)RHILockIndexBuffer(IndexBuffer->IndexBufferRHI, 0, IndexBuffer->IndexBufferRHI->GetSize(), RLM_ReadOnly); + memcpy(data_ptr, indices, IndexBuffer->IndexBufferRHI->GetSize()); + RHIUnlockIndexBuffer(IndexBuffer->IndexBufferRHI); + }); + + FlushRenderingCommands(); + + mesh.indices.resize(num_indices); + for (int idx = 0; idx < num_indices; ++idx) { + mesh.indices[idx] = indices_vec[idx]; + } + } + + //Unreal stores more vertices than triangles. So here we find the highest referenced vertex and ignore any after that + auto result_iter = std::max_element(mesh.indices.begin(), mesh.indices.end()); + auto max_triangle_index = std::distance(mesh.indices.begin(), result_iter); + + mesh.vertices.resize(max_triangle_index * 3); + int aligned_index = 0; + FTransform transform = comp->GetComponentTransform(); + for(int vertex_idx=0;vertex_idx UAirBlueprintLib::ListWorldsInRegistry() +{ + FARFilter Filter; + Filter.ClassNames.Add(UWorld::StaticClass()->GetFName()); + Filter.bRecursivePaths = true; + + TArray AssetData; + FAssetRegistryModule& AssetRegistryModule = FModuleManager::LoadModuleChecked("AssetRegistry"); + AssetRegistryModule.Get().GetAssets(Filter, AssetData); + + TArray WorldNames; + for (auto asset : AssetData) + WorldNames.Add(asset.AssetName); + return WorldNames; +} + +UObject* UAirBlueprintLib::GetMeshFromRegistry(const std::string& load_object) +{ + FARFilter Filter; + Filter.ClassNames.Add(UStaticMesh::StaticClass()->GetFName()); + Filter.bRecursivePaths = true; + + TArray AssetData; + FAssetRegistryModule& AssetRegistryModule = FModuleManager::LoadModuleChecked("AssetRegistry"); + AssetRegistryModule.Get().GetAssets(Filter, AssetData); + + UObject* LoadObject = NULL; + for (auto asset : AssetData) + { + UE_LOG(LogTemp, Log, TEXT("Asset path: %s"), *asset.PackagePath.ToString()); + if (asset.AssetName == FName(load_object.c_str())) + { + LoadObject = asset.GetAsset(); + break; + } + } + return LoadObject; +} bool UAirBlueprintLib::HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor, ECollisionChannel collision_channel) { diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 21fcf8b1e..bddcff4db 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -4,8 +4,10 @@ #pragma once #include "CoreMinimal.h" +#include "Runtime/AssetRegistry/Public/AssetRegistryModule.h" #include "GameFramework/Actor.h" #include "Components/InputComponent.h" +#include "EngineUtils.h" #include "GameFramework/PlayerInput.h" #include "IImageWrapperModule.h" #include "Kismet/BlueprintFunctionLibrary.h" @@ -15,13 +17,16 @@ #include "Kismet/GameplayStatics.h" #include "Kismet/KismetStringLibrary.h" #include "Engine/World.h" - #include "Runtime/Landscape/Classes/LandscapeComponent.h" +#include "Runtime/Engine/Classes/Kismet/GameplayStatics.h" +#include "AirsimLevelStreaming.h" +#include "Runtime/Core/Public/HAL/FileManager.h" #include "common/AirSimSettings.hpp" #include #include #include "AirBlueprintLib.generated.h" +class ULevelStreamingDynamic; UENUM(BlueprintType) enum class LogDebugLevel : uint8 { @@ -46,25 +51,21 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary UFUNCTION(BlueprintCallable, Category = "Utils") static void LogMessage(const FString &prefix, const FString &suffix, LogDebugLevel level, float persist_sec = 60); static float GetWorldToMetersScale(const AActor* context); - template static T* GetActorComponent(AActor* actor, FString name); template static T* FindActor(const UObject* context, FString name) { - TArray foundActors; - FindAllActor(context, foundActors); FName name_n = FName(*name); - - for (AActor* actor : foundActors) { - if (actor->ActorHasTag(name_n) || actor->GetName().Compare(name) == 0) { - return static_cast(actor); + for (TActorIterator It(context->GetWorld(), T::StaticClass()); It; ++It) + { + AActor* Actor = *It; + if (!Actor->IsPendingKill() && (Actor->ActorHasTag(name_n) || Actor->GetName().Compare(name) == 0)) + { + return static_cast(Actor); } } - - //UAirBlueprintLib::LogMessage(name + TEXT(" Actor not found!"), TEXT(""), LogDebugLevel::Failure); - return nullptr; } @@ -73,8 +74,17 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary { UGameplayStatics::GetAllActorsOfClass(context, T::StaticClass(), foundActors); } + + static ULevelStreamingDynamic *CURRENT_LEVEL; static std::vector ListMatchingActors(const UObject *context, const std::string& name_regex); + UFUNCTION(BlueprintCallable, Category = "AirSim|LevelAPI") + static ULevelStreamingDynamic* loadLevel(UObject* context, const FString& level_name); + UFUNCTION(BlueprintCallable, Category = "AirSim|LevelAPI") + static bool spawnPlayer(UWorld* context); + UFUNCTION(BlueprintPure, Category = "AirSim|LevelAPI") + static TArray ListWorldsInRegistry(); + static UObject* GetMeshFromRegistry(const std::string& load_object); static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); @@ -182,7 +192,7 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static void setUnrealClockSpeed(const AActor* context, float clock_speed); static IImageWrapperModule* getImageWrapperModule(); static void CompressImageArray(int32 width, int32 height, const TArray &src, TArray &dest); - + static std::vector GetStaticMeshComponents(); private: template static void InitializeObjectStencilID(T* mesh, bool ignore_existing = true) diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.4.15.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.4.15.cs deleted file mode 100644 index 0f6b1031c..000000000 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.4.15.cs +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -using UnrealBuildTool; -using System.IO; - -public class AirSim : ModuleRules -{ - const string readmurl = "https://github.com/Microsoft/AirSim/blob/master/docs/install_eigen.md"; - - private string ModulePath - { - get { return ModuleDirectory; } - } - - private string AirSimPath - { - get { return Path.GetFullPath(Path.Combine(ModulePath, "AirLib")); } - } - - private enum CompileMode - { - HeaderOnlyNoRpc, - HeaderOnlyWithRpc, - CppCompileNoRpc, - CppCompileWithRpc - } - - private void SetupCompileMode(CompileMode mode, TargetInfo Target) - { - LoadAirSimDependency(Target, "MavLinkCom", "MavLinkCom"); - - switch (mode) - { - case CompileMode.HeaderOnlyNoRpc: - Definitions.Add("AIRLIB_HEADER_ONLY=1"); - Definitions.Add("AIRLIB_NO_RPC=1"); - AddLibDependency("AirLib", Path.Combine(AirSimPath, "lib"), "AirLib", Target, false); - break; - case CompileMode.HeaderOnlyWithRpc: - Definitions.Add("AIRLIB_HEADER_ONLY=1"); - AddLibDependency("AirLib", Path.Combine(AirSimPath, "lib"), "AirLib", Target, false); - LoadAirSimDependency(Target, "rpclib", "rpc"); - break; - case CompileMode.CppCompileNoRpc: - LoadAirSimDependency(Target, "MavLinkCom", "MavLinkCom"); - Definitions.Add("AIRLIB_NO_RPC=1"); - break; - case CompileMode.CppCompileWithRpc: - LoadAirSimDependency(Target, "MavLinkCom", "MavLinkCom"); - LoadAirSimDependency(Target, "rpclib", "rpc"); - break; - default: - throw new System.Exception("CompileMode specified in plugin's Build.cs file is not recognized"); - } - - } - - public AirSim(TargetInfo Target) - { - bEnforceIWYU = false; - PCHUsage = PCHUsageMode.UseExplicitOrSharedPCHs; - bEnableExceptions = true; - - PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "RenderCore", "RHI" }); - PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore" }); - - //suppress VC++ proprietary warnings - Definitions.Add("_SCL_SECURE_NO_WARNINGS=1"); - Definitions.Add("CRT_SECURE_NO_WARNINGS=1"); - - PrivateIncludePaths.Add(Path.Combine(AirSimPath, "include")); - PrivateIncludePaths.Add(Path.Combine(AirSimPath, "deps", "eigen3")); - AddOSLibDependencies(Target); - - SetupCompileMode(CompileMode.HeaderOnlyWithRpc, Target); - } - - private void AddOSLibDependencies(TargetInfo Target) - { - if (Target.Platform == UnrealTargetPlatform.Win64) - { - // for SHGetFolderPath. - PublicAdditionalLibraries.Add("Shell32.lib"); - - // XInput for JoyStick, make sure to delay load this because we use generated DLL from x360ce - PublicDelayLoadDLLs.Add("xinput9_1_0.dll"); - //Lib for the xinput DLL - //this should be in path, typically at C:\Program Files (x86)\Windows Kits\8.1\Lib\winv6.3\um\x64 - //typically gets installed with Visual Studio - PublicAdditionalLibraries.Add("xinput9_1_0.lib"); - } - } - - private bool LoadAirSimDependency(TargetInfo Target, string LibName, string LibFileName) - { - string LibrariesPath = Path.Combine(AirSimPath, "deps", LibName, "lib"); - return AddLibDependency(LibName, LibrariesPath, LibFileName, Target, true); - } - - private bool AddLibDependency(string LibName, string LibPath, string LibFileName, TargetInfo Target, bool IsAddLibInclude) - { - string PlatformString = (Target.Platform == UnrealTargetPlatform.Win64 || Target.Platform == UnrealTargetPlatform.Mac) ? "x64" : "x86"; - string ConfigurationString = (Target.Configuration == UnrealTargetConfiguration.Debug) ? "Debug" : "Release"; - bool isLibrarySupported = false; - - - if (Target.Platform == UnrealTargetPlatform.Win64) - { - isLibrarySupported = true; - PublicAdditionalLibraries.Add(Path.Combine(LibPath, PlatformString, ConfigurationString, LibFileName + ".lib")); - } else if (Target.Platform == UnrealTargetPlatform.Linux || Target.Platform == UnrealTargetPlatform.Mac) { - isLibrarySupported = true; - PublicAdditionalLibraries.Add(Path.Combine(LibPath, "lib" + LibFileName + ".a")); - } - - if (isLibrarySupported && IsAddLibInclude) - { - // Include path - PrivateIncludePaths.Add(Path.Combine(AirSimPath, "deps", LibName, "include")); - } - - Definitions.Add(string.Format("WITH_" + LibName.ToUpper() + "_BINDING={0}", isLibrarySupported ? 1 : 0)); - - return isLibrarySupported; - } -} diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 836dd3a7a..11970b262 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -6,8 +6,6 @@ public class AirSim : ModuleRules { - const string readmurl = "https://github.com/Microsoft/AirSim/blob/master/docs/install_eigen.md"; - private string ModulePath { get { return ModuleDirectory; } @@ -47,35 +45,26 @@ private void SetupCompileMode(CompileMode mode, ReadOnlyTargetRules Target) switch (mode) { case CompileMode.HeaderOnlyNoRpc: -#if UE_4_19_OR_LATER PublicDefinitions.Add("AIRLIB_HEADER_ONLY=1"); PublicDefinitions.Add("AIRLIB_NO_RPC=1"); -#else - Definitions.Add("AIRLIB_HEADER_ONLY=1"); - Definitions.Add("AIRLIB_NO_RPC=1"); -#endif AddLibDependency("AirLib", Path.Combine(AirLibPath, "lib"), "AirLib", Target, false); break; + case CompileMode.HeaderOnlyWithRpc: -#if UE_4_19_OR_LATER PublicDefinitions.Add("AIRLIB_HEADER_ONLY=1"); -#else - Definitions.Add("AIRLIB_HEADER_ONLY=1"); -#endif AddLibDependency("AirLib", Path.Combine(AirLibPath, "lib"), "AirLib", Target, false); LoadAirSimDependency(Target, "rpclib", "rpc"); break; + case CompileMode.CppCompileNoRpc: LoadAirSimDependency(Target, "MavLinkCom", "MavLinkCom"); -#if UE_4_19_OR_LATER PublicDefinitions.Add("AIRLIB_NO_RPC=1"); -#else - Definitions.Add("AIRLIB_NO_RPC=1"); -#endif break; + case CompileMode.CppCompileWithRpc: LoadAirSimDependency(Target, "rpclib", "rpc"); break; + default: throw new System.Exception("CompileMode specified in plugin's Build.cs file is not recognized"); } @@ -89,19 +78,13 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) bEnableExceptions = true; - PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape" }); + PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry","PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape" }); PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore" }); //suppress VC++ proprietary warnings -#if UE_4_19_OR_LATER PublicDefinitions.Add("_SCL_SECURE_NO_WARNINGS=1"); PublicDefinitions.Add("_CRT_SECURE_NO_WARNINGS=1"); PublicDefinitions.Add("HMD_MODULE_INCLUDED=0"); -#else - Definitions.Add("_SCL_SECURE_NO_WARNINGS=1"); - Definitions.Add("_CRT_SECURE_NO_WARNINGS=1"); - Definitions.Add("HMD_MODULE_INCLUDED=0"); -#endif PublicIncludePaths.Add(Path.Combine(AirLibPath, "include")); PublicIncludePaths.Add(Path.Combine(AirLibPath, "deps", "eigen3")); @@ -169,11 +152,7 @@ private bool AddLibDependency(string LibName, string LibPath, string LibFileName // Include path PublicIncludePaths.Add(Path.Combine(AirLibPath, "deps", LibName, "include")); } -#if UE_4_19_OR_LATER PublicDefinitions.Add(string.Format("WITH_" + LibName.ToUpper() + "_BINDING={0}", isLibrarySupported ? 1 : 0)); -#else - Definitions.Add(string.Format("WITH_" + LibName.ToUpper() + "_BINDING={0}", isLibrarySupported ? 1 : 0)); -#endif return isLibrarySupported; } diff --git a/Unreal/Plugins/AirSim/Source/AirSimCharacter.cpp b/Unreal/Plugins/AirSim/Source/AirSimCharacter.cpp deleted file mode 100644 index 42198e6d2..000000000 --- a/Unreal/Plugins/AirSim/Source/AirSimCharacter.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include "AirSimCharacter.h" - - -void AAirSimCharacter::setFaceExpression(const std::string& expression_name, float value) -{ - unused(expression_name); - unused(value); - //derived class should override this -} -float AAirSimCharacter::getFaceExpression(const std::string& expression_name) const -{ - unused(expression_name); - //derived class should override this - - return common_utils::Utils::nan(); -} -std::vector AAirSimCharacter::getAvailableFaceExpressions() const -{ - //derived class should override this - return std::vector(); -} -void AAirSimCharacter::setSkinDarkness(float value) -{ - unused(value); - //derived class should override this -} -float AAirSimCharacter::getSkinDarkness() const -{ - //derived class should override this - return common_utils::Utils::nan(); -} -void AAirSimCharacter::setSkinAgeing(float value) -{ - unused(value); - //derived class should override this -} -float AAirSimCharacter::getSkinAgeing() const -{ - //derived class should override this - return common_utils::Utils::nan(); -} -void AAirSimCharacter::setHeadRotation(const msr::airlib::Quaternionr& q) -{ - unused(q); - //derived class should override this -} -msr::airlib::Quaternionr AAirSimCharacter::getHeadRotation() const -{ - //derived class should override this - return msr::airlib::Quaternionr::Identity(); -} -void AAirSimCharacter::setBonePose(const std::string& bone_name, const msr::airlib::Pose& pose) -{ - unused(bone_name); - unused(pose); - //derived class should override this -} -void AAirSimCharacter::setBonePoses(const std::unordered_map& poses) -{ - unused(poses); -} -msr::airlib::Pose AAirSimCharacter::getBonePose(const std::string& bone_name) const -{ - unused(bone_name); - //derived class should override this - return msr::airlib::Pose::nanPose(); -} -std::unordered_map AAirSimCharacter::getBonePoses(const std::vector& bone_names) const -{ - unused(bone_names); - - //derived class should override this - return std::unordered_map(); -} -void AAirSimCharacter::resetBonePose(const std::string& bone_name) -{ - unused(bone_name); - //derived class should override this -} -void AAirSimCharacter::setFacePreset(const std::string& preset_name, float value) -{ - unused(preset_name); - unused(value); - //derived class should override this -} -void AAirSimCharacter::setFacePresets(const std::unordered_map& presets) -{ - unused(presets); - //derived class should override this -} -void AAirSimCharacter::reset() -{ - //derived class should override this -} diff --git a/Unreal/Plugins/AirSim/Source/AirSimCharacter.h b/Unreal/Plugins/AirSim/Source/AirSimCharacter.h deleted file mode 100644 index 5e1bc70ad..000000000 --- a/Unreal/Plugins/AirSim/Source/AirSimCharacter.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include "CoreMinimal.h" -#include "GameFramework/Character.h" -#include "Engine/DataTable.h" - -#include "common/Common.hpp" -#include "AirSimCharacter.generated.h" - -UCLASS() -class AIRSIM_API AAirSimCharacter : public ACharacter -{ - GENERATED_BODY() - -public: - virtual void setFaceExpression(const std::string& expression_name, float value); - virtual float getFaceExpression(const std::string& expression_name) const; - virtual std::vector getAvailableFaceExpressions() const; - virtual void setSkinDarkness(float value); - virtual float getSkinDarkness() const; - virtual void setSkinAgeing(float value); - virtual float getSkinAgeing() const; - virtual void setHeadRotation(const msr::airlib::Quaternionr& q); - virtual msr::airlib::Quaternionr getHeadRotation() const; - virtual void setBonePose(const std::string& bone_name, const msr::airlib::Pose& pose); - virtual void setBonePoses(const std::unordered_map& poses); - virtual std::unordered_map getBonePoses(const std::vector& bone_names) const; - virtual msr::airlib::Pose getBonePose(const std::string& bone_name) const; - virtual void resetBonePose(const std::string& bone_name); - virtual void setFacePreset(const std::string& preset_name, float value); - virtual void setFacePresets(const std::unordered_map& presets); - virtual void reset(); -}; diff --git a/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp new file mode 100644 index 000000000..29f634c51 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp @@ -0,0 +1,51 @@ +#pragma once + +#include "AirsimLevelStreaming.h" +#include "Engine/World.h" + +int32 UAirsimLevelStreaming::LevelInstanceId = 0; + + +UAirsimLevelStreaming* UAirsimLevelStreaming::LoadAirsimLevelInstance(UWorld* WorldContextObject, FString LevelName, FVector Location, FRotator Rotation, bool& bOutSuccess) +{ + if (!WorldContextObject) + { + return nullptr; + } + + // Check whether requested map exists, this could be very slow if LevelName is a short package name + FString LongPackageName; + bool success = FPackageName::SearchForPackageOnDisk(LevelName, &LongPackageName); + if (!success) +{ + bOutSuccess = false; + return nullptr; + } + + // Create Unique Name for sub-level package + const FString ShortPackageName = FPackageName::GetShortName(LongPackageName); + const FString PackagePath = FPackageName::GetLongPackagePath(LongPackageName); + FString UniqueLevelPackageName = PackagePath + TEXT("/") + WorldContextObject->StreamingLevelsPrefix + ShortPackageName; + UniqueLevelPackageName += TEXT("_LevelInstance_") + FString::FromInt(++LevelInstanceId); + + // Setup streaming level object that will load specified map + ULevelStreamingDynamic* level_pointer = NewObject(WorldContextObject, ULevelStreamingDynamic::StaticClass(), NAME_None, RF_Transient, NULL); + level_pointer->SetWorldAssetByPackageName(FName(*UniqueLevelPackageName)); + level_pointer->LevelColor = FColor::MakeRandomColor(); + level_pointer->SetShouldBeLoaded(true); + level_pointer->SetShouldBeVisible(true); + level_pointer->bShouldBlockOnLoad = true; + level_pointer->bInitiallyLoaded = true; + level_pointer->bInitiallyVisible = true; + + // Transform + level_pointer->LevelTransform = FTransform(Rotation, Location); + // Map to Load + level_pointer->PackageNameToLoad = FName(*LongPackageName); + // Add the new level to world. + WorldContextObject->AddStreamingLevel(level_pointer); + + bOutSuccess = true; + + return dynamic_cast (level_pointer); +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h new file mode 100644 index 000000000..d45570127 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h @@ -0,0 +1,12 @@ +#pragma once +#include "Runtime/Engine/Classes/Engine/LevelStreamingDynamic.h" + + +class UAirsimLevelStreaming : public ULevelStreamingDynamic +{ +public: + static UAirsimLevelStreaming* LoadAirsimLevelInstance(UWorld* WorldContextObject, FString LevelName, FVector Location, FRotator Rotation, bool& bOutSuccess); + +private: + static int32 LevelInstanceId; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.h b/Unreal/Plugins/AirSim/Source/CameraDirector.h index e061aac80..0daade05d 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.h +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.h @@ -13,14 +13,14 @@ UENUM(BlueprintType) enum class ECameraDirectorMode : uint8 { - CAMERA_DIRECTOR_MODE_FPV = 1 UMETA(DisplayName = "FPV"), - CAMERA_DIRECTOR_MODE_GROUND_OBSERVER = 2 UMETA(DisplayName = "GroundObserver"), - CAMERA_DIRECTOR_MODE_FLY_WITH_ME = 3 UMETA(DisplayName = "FlyWithMe"), - CAMERA_DIRECTOR_MODE_MANUAL = 4 UMETA(DisplayName = "Manual"), - CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE = 5 UMETA(DisplayName = "SpringArmChase"), - CAMERA_DIRECTOR_MODE_BACKUP = 6 UMETA(DisplayName = "Backup"), - CAMERA_DIRECTOR_MODE_NODISPLAY = 7 UMETA(DisplayName = "No Display"), - CAMERA_DIRECTOR_MODE_FRONT = 8 UMETA(DisplayName = "Front") + CAMERA_DIRECTOR_MODE_FPV = 0 UMETA(DisplayName = "FPV"), + CAMERA_DIRECTOR_MODE_GROUND_OBSERVER = 1 UMETA(DisplayName = "GroundObserver"), + CAMERA_DIRECTOR_MODE_FLY_WITH_ME = 2 UMETA(DisplayName = "FlyWithMe"), + CAMERA_DIRECTOR_MODE_MANUAL = 3 UMETA(DisplayName = "Manual"), + CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE = 4 UMETA(DisplayName = "SpringArmChase"), + CAMERA_DIRECTOR_MODE_BACKUP = 5 UMETA(DisplayName = "Backup"), + CAMERA_DIRECTOR_MODE_NODISPLAY = 6 UMETA(DisplayName = "No Display"), + CAMERA_DIRECTOR_MODE_FRONT = 7 UMETA(DisplayName = "Front") }; UCLASS() diff --git a/Unreal/Plugins/AirSim/Source/ManualPoseController.cpp b/Unreal/Plugins/AirSim/Source/ManualPoseController.cpp index 7589e0fcb..b2e322e77 100644 --- a/Unreal/Plugins/AirSim/Source/ManualPoseController.cpp +++ b/Unreal/Plugins/AirSim/Source/ManualPoseController.cpp @@ -10,8 +10,9 @@ void UManualPoseController::initializeForPlay() left_mapping_ = FInputAxisKeyMapping("inputManualArrowLeft", EKeys::Left); right_mapping_ = FInputAxisKeyMapping("inputManualArrowRight", EKeys::Right); forward_mapping_= FInputAxisKeyMapping("inputManualForward", EKeys::Up); backward_mapping_ = FInputAxisKeyMapping("inputManualBackward", EKeys::Down); up_mapping_ = FInputAxisKeyMapping("inputManualArrowUp", EKeys::PageUp); down_mapping_ = FInputAxisKeyMapping("inputManualArrowDown", EKeys::PageDown); - left_yaw_mapping_ = FInputAxisKeyMapping("inputManualLeftYaw", EKeys::A); up_pitch_mapping_ = FInputAxisKeyMapping("inputManualUpPitch", EKeys::W); - right_yaw_mapping_ = FInputAxisKeyMapping("inputManualRightYaw", EKeys::D); down_pitch_mapping_ = FInputAxisKeyMapping("inputManualDownPitch", EKeys::S); + left_yaw_mapping_ = FInputAxisKeyMapping("inputManualLeftYaw", EKeys::A); right_yaw_mapping_ = FInputAxisKeyMapping("inputManualRightYaw", EKeys::D); + left_roll_mapping_ = FInputAxisKeyMapping("inputManualLefRoll", EKeys::Q); right_roll_mapping_ = FInputAxisKeyMapping("inputManualRightRoll", EKeys::E); + up_pitch_mapping_ = FInputAxisKeyMapping("inputManualUpPitch", EKeys::W); down_pitch_mapping_ = FInputAxisKeyMapping("inputManualDownPitch", EKeys::S); input_positive_ = inpute_negative_ = last_velocity_ = FVector::ZeroVector; } @@ -20,7 +21,7 @@ void UManualPoseController::clearBindings() { left_binding_ = right_binding_ = up_binding_ = down_binding_ = nullptr; forward_binding_ = backward_binding_ = left_yaw_binding_ = up_pitch_binding_ = nullptr; - right_yaw_binding_ = down_pitch_binding_ = nullptr; + right_yaw_binding_ = down_pitch_binding_ = left_roll_binding_ = right_roll_binding_ = nullptr; } void UManualPoseController::setActor(AActor* actor) @@ -86,10 +87,14 @@ void UManualPoseController::removeInputBindings() UAirBlueprintLib::RemoveAxisBinding(down_mapping_, down_binding_, actor_); if (left_yaw_binding_) UAirBlueprintLib::RemoveAxisBinding(left_yaw_mapping_, left_yaw_binding_, actor_); - if (up_pitch_binding_) - UAirBlueprintLib::RemoveAxisBinding(up_pitch_mapping_, up_pitch_binding_, actor_); if (right_yaw_binding_) UAirBlueprintLib::RemoveAxisBinding(right_yaw_mapping_, right_yaw_binding_, actor_); + if (left_roll_binding_) + UAirBlueprintLib::RemoveAxisBinding(left_roll_mapping_, left_roll_binding_, actor_); + if (right_roll_binding_) + UAirBlueprintLib::RemoveAxisBinding(right_roll_mapping_, right_roll_binding_, actor_); + if (up_pitch_binding_) + UAirBlueprintLib::RemoveAxisBinding(up_pitch_mapping_, up_pitch_binding_, actor_); if (down_pitch_binding_) UAirBlueprintLib::RemoveAxisBinding(down_pitch_mapping_, down_pitch_binding_, actor_); @@ -107,8 +112,10 @@ void UManualPoseController::setupInputBindings() up_binding_ = & UAirBlueprintLib::BindAxisToKey(up_mapping_, actor_, this, &UManualPoseController::inputManualMoveUp); down_binding_ = & UAirBlueprintLib::BindAxisToKey(down_mapping_, actor_, this, &UManualPoseController::inputManualDown); left_yaw_binding_ = & UAirBlueprintLib::BindAxisToKey(left_yaw_mapping_, actor_, this, &UManualPoseController::inputManualLeftYaw); - up_pitch_binding_ = & UAirBlueprintLib::BindAxisToKey(up_pitch_mapping_, actor_, this, &UManualPoseController::inputManualUpPitch); right_yaw_binding_ = & UAirBlueprintLib::BindAxisToKey(right_yaw_mapping_, actor_, this, &UManualPoseController::inputManualRightYaw); + left_roll_binding_ = & UAirBlueprintLib::BindAxisToKey(left_roll_mapping_, actor_, this, &UManualPoseController::inputManualLeftRoll); + right_roll_binding_ = & UAirBlueprintLib::BindAxisToKey(right_roll_mapping_, actor_, this, &UManualPoseController::inputManualRightRoll); + up_pitch_binding_ = & UAirBlueprintLib::BindAxisToKey(up_pitch_mapping_, actor_, this, &UManualPoseController::inputManualUpPitch); down_pitch_binding_ = & UAirBlueprintLib::BindAxisToKey(down_pitch_mapping_, actor_, this, &UManualPoseController::inputManualDownPitch); } @@ -155,15 +162,25 @@ void UManualPoseController::inputManualLeftYaw(float val) if (!FMath::IsNearlyEqual(val, 0.f)) delta_rotation_.Add(0, -val, 0); } -void UManualPoseController::inputManualUpPitch(float val) +void UManualPoseController::inputManualRightYaw(float val) { if (!FMath::IsNearlyEqual(val, 0.f)) - delta_rotation_.Add(val, 0, 0); + delta_rotation_.Add(0, val, 0); } -void UManualPoseController::inputManualRightYaw(float val) +void UManualPoseController::inputManualLeftRoll(float val) { if (!FMath::IsNearlyEqual(val, 0.f)) - delta_rotation_.Add(0, val, 0); + delta_rotation_.Add(0, 0, -val); +} +void UManualPoseController::inputManualRightRoll(float val) +{ + if (!FMath::IsNearlyEqual(val, 0.f)) + delta_rotation_.Add(0, 0, val); +} +void UManualPoseController::inputManualUpPitch(float val) +{ + if (!FMath::IsNearlyEqual(val, 0.f)) + delta_rotation_.Add(val, 0, 0); } void UManualPoseController::inputManualDownPitch(float val) { diff --git a/Unreal/Plugins/AirSim/Source/ManualPoseController.h b/Unreal/Plugins/AirSim/Source/ManualPoseController.h index 6d668c671..91d6e123c 100644 --- a/Unreal/Plugins/AirSim/Source/ManualPoseController.h +++ b/Unreal/Plugins/AirSim/Source/ManualPoseController.h @@ -27,8 +27,10 @@ class AIRSIM_API UManualPoseController : public UObject { void inputManualMoveUp(float val); void inputManualDown(float val); void inputManualLeftYaw(float val); - void inputManualUpPitch(float val); void inputManualRightYaw(float val); + void inputManualLeftRoll(float val); + void inputManualRightRoll(float val); + void inputManualUpPitch(float val); void inputManualDownPitch(float val); void setupInputBindings(); @@ -37,12 +39,12 @@ class AIRSIM_API UManualPoseController : public UObject { private: FInputAxisBinding *left_binding_, *right_binding_, *up_binding_, *down_binding_; - FInputAxisBinding *forward_binding_, *backward_binding_, *left_yaw_binding_, *up_pitch_binding_; - FInputAxisBinding *right_yaw_binding_, *down_pitch_binding_; + FInputAxisBinding *forward_binding_, *backward_binding_, *left_yaw_binding_, *right_yaw_binding_; + FInputAxisBinding *up_pitch_binding_, *down_pitch_binding_, *left_roll_binding_, *right_roll_binding_; FInputAxisKeyMapping left_mapping_, right_mapping_, up_mapping_, down_mapping_; - FInputAxisKeyMapping forward_mapping_, backward_mapping_, left_yaw_mapping_, up_pitch_mapping_; - FInputAxisKeyMapping right_yaw_mapping_, down_pitch_mapping_; + FInputAxisKeyMapping forward_mapping_, backward_mapping_, left_yaw_mapping_, right_yaw_mapping_; + FInputAxisKeyMapping up_pitch_mapping_, down_pitch_mapping_, left_roll_mapping_, right_roll_mapping_; FVector delta_position_; diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.cpp b/Unreal/Plugins/AirSim/Source/NedTransform.cpp index 9f30f2445..384f74a51 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.cpp +++ b/Unreal/Plugins/AirSim/Source/NedTransform.cpp @@ -28,7 +28,12 @@ NedTransform::NedTransform(const AActor* pivot, const FTransform& global_transfo NedTransform::Vector3r NedTransform::toLocalNed(const FVector& position) const { - return NedTransform::toVector3r(position - local_ned_offset_, + return NedTransform::toVector3r(position - local_ned_offset_, + 1 / world_to_meters_, true); +} +NedTransform::Vector3r NedTransform::toLocalNedVelocity(const FVector& velocity) const +{ + return NedTransform::toVector3r(velocity, 1 / world_to_meters_, true); } NedTransform::Vector3r NedTransform::toGlobalNed(const FVector& position) const @@ -61,6 +66,14 @@ FVector NedTransform::fromLocalNed(const NedTransform::Vector3r& position) const { return NedTransform::toFVector(position, world_to_meters_, true) + local_ned_offset_; } +FVector NedTransform::fromRelativeNed(const NedTransform::Vector3r& position) const +{ + return NedTransform::toFVector(position, world_to_meters_, true); +} +FTransform NedTransform::fromRelativeNed(const Pose& pose) const +{ + return FTransform(fromNed(pose.orientation), fromRelativeNed(pose.position)); +} FVector NedTransform::fromGlobalNed(const NedTransform::Vector3r& position) const { return NedTransform::toFVector(position, world_to_meters_, true) + global_transform_.GetLocation(); @@ -77,6 +90,15 @@ FTransform NedTransform::fromGlobalNed(const Pose& pose) const { return FTransform(fromNed(pose.orientation), fromGlobalNed(pose.position)); } +FQuat NedTransform::fromUUtoFLU(const FQuat& q) const +{ + return FQuat(-q.X, q.Y, -q.Z, q.W); +} +// todo unused. need to manually plots tf axes' line in right handed FLU instead of using DrawDebugCoordinateSystem +FQuat NedTransform::fromGlobalNedToFLU(const Quaternionr& q) const +{ + return fromUUtoFLU(fromNed(q)); +} FVector NedTransform::getGlobalOffset() const { diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.h b/Unreal/Plugins/AirSim/Source/NedTransform.h index 7512a2cc9..717b1aaa1 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.h +++ b/Unreal/Plugins/AirSim/Source/NedTransform.h @@ -3,7 +3,6 @@ #include "CoreMinimal.h" #include "Kismet/KismetMathLibrary.h" #include "GameFramework/Actor.h" - #include "common/Common.hpp" /* @@ -31,21 +30,28 @@ class AIRSIM_API NedTransform //UU -> local NED Vector3r toLocalNed(const FVector& position) const; + Vector3r toLocalNedVelocity(const FVector& velocity) const; Vector3r toGlobalNed(const FVector& position) const; Quaternionr toNed(const FQuat& q) const; float toNed(float length) const; Pose toLocalNed(const FTransform& pose) const; Pose toGlobalNed(const FTransform& pose) const; - //local NED -> UU FVector fromLocalNed(const Vector3r& position) const; FVector fromGlobalNed(const Vector3r& position) const; FQuat fromNed(const Quaternionr& q) const; float fromNed(float length) const; + FVector fromRelativeNed(const Vector3r& position) const; + FTransform fromRelativeNed(const Pose& pose) const; FTransform fromLocalNed(const Pose& pose) const; FTransform fromGlobalNed(const Pose& pose) const; + // UU -> ROS FLU. UU is XYZ:FrontRightUp (left handed); ROS FLU is XYZ:FrontLeftUp (right handed) + // used by simPlotApis: + FQuat fromUUtoFLU(const FQuat& q) const; + FQuat fromGlobalNedToFLU(const Quaternionr& q) const; + FVector getGlobalOffset() const; FVector getLocalOffset() const; FTransform getGlobalTransform() const; diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 7374b407f..735298788 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -1,5 +1,5 @@ #include "PIPCamera.h" -#include "ConstructorHelpers.h" +#include "UObject/ConstructorHelpers.h" #include "Components/SceneCaptureComponent2D.h" #include "Camera/CameraComponent.h" #include "Engine/TextureRenderTarget2D.h" @@ -244,8 +244,12 @@ void APIPCamera::setCameraTypeEnabled(ImageType type, bool enabled) enableCaptureComponent(type, enabled); } -void APIPCamera::setCameraOrientation(const FRotator& rotator) +void APIPCamera::setCameraPose(const FTransform& pose) { + FVector position = pose.GetLocation(); + this->SetActorRelativeLocation(pose.GetLocation()); + + FRotator rotator = pose.GetRotation().Rotator(); if (gimbal_stabilization_ > 0) { gimbald_rotator_.Pitch = rotator.Pitch; gimbald_rotator_.Roll = rotator.Roll; @@ -254,6 +258,15 @@ void APIPCamera::setCameraOrientation(const FRotator& rotator) this->SetActorRelativeRotation(rotator); } +void APIPCamera::setCameraFoV(float fov_degrees) +{ + int image_count = static_cast(Utils::toNumeric(ImageType::Count)); + for (int image_type = 0; image_type < image_count; ++image_type) { + captures_[image_type]->FOVAngle = fov_degrees; + } +} + + void APIPCamera::setupCameraFromSettings(const APIPCamera::CameraSetting& camera_setting, const NedTransform& ned_transform) { //TODO: should we be ignoring position and orientation settings here? @@ -277,12 +290,27 @@ void APIPCamera::setupCameraFromSettings(const APIPCamera::CameraSetting& camera const auto& noise_setting = camera_setting.noise_settings.at(image_type); if (image_type >= 0) { //scene capture components - if (image_type==0 || image_type==5 || image_type==6 || image_type==7) - updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], false, - image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform); - else - updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], true, - image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform); + switch (Utils::toEnum(image_type)) { + case ImageType::Scene: + case ImageType::Infrared: + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], + false, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, + false); + break; + + case ImageType::Segmentation: + case ImageType::SurfaceNormals: + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], + true, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, + true); + break; + + default: + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], + true, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, + false); + break; + } setNoiseMaterial(image_type, captures_[image_type], captures_[image_type]->PostProcessSettings, noise_setting); } @@ -295,7 +323,8 @@ void APIPCamera::setupCameraFromSettings(const APIPCamera::CameraSetting& camera } void APIPCamera::updateCaptureComponentSetting(USceneCaptureComponent2D* capture, UTextureRenderTarget2D* render_target, - bool auto_format, const EPixelFormat& pixel_format, const CaptureSetting& setting, const NedTransform& ned_transform) + bool auto_format, const EPixelFormat& pixel_format, const CaptureSetting& setting, const NedTransform& ned_transform, + bool force_linear_gamma) { if (auto_format) { @@ -303,7 +332,7 @@ void APIPCamera::updateCaptureComponentSetting(USceneCaptureComponent2D* capture } else { - render_target->InitCustomFormat(setting.width, setting.height, pixel_format, false); + render_target->InitCustomFormat(setting.width, setting.height, pixel_format, force_linear_gamma); } if (!std::isnan(setting.target_gamma)) diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index 27e7d29ee..a1465163b 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.h @@ -41,7 +41,8 @@ class AIRSIM_API APIPCamera : public ACameraActor void setCameraTypeEnabled(ImageType type, bool enabled); bool getCameraTypeEnabled(ImageType type) const; void setupCameraFromSettings(const APIPCamera::CameraSetting& camera_setting, const NedTransform& ned_transform); - void setCameraOrientation(const FRotator& rotator); + void setCameraPose(const FTransform& pose); + void setCameraFoV(float fov_degrees); msr::airlib::ProjectionMatrix getProjectionMatrix(const APIPCamera::ImageType image_type) const; @@ -74,8 +75,9 @@ class AIRSIM_API APIPCamera : public ACameraActor static unsigned int imageTypeCount(); void enableCaptureComponent(const ImageType type, bool is_enabled); - static void updateCaptureComponentSetting(USceneCaptureComponent2D* capture, UTextureRenderTarget2D* render_target, - bool auto_format, const EPixelFormat& pixel_format, const CaptureSetting& setting, const NedTransform& ned_transform); + static void updateCaptureComponentSetting(USceneCaptureComponent2D* capture, UTextureRenderTarget2D* render_target, + bool auto_format, const EPixelFormat& pixel_format, const CaptureSetting& setting, const NedTransform& ned_transform, + bool force_linear_gamma); void setNoiseMaterial(int image_type, UObject* outer, FPostProcessSettings& obj, const NoiseSetting& settings); static void updateCameraPostProcessingSetting(FPostProcessSettings& obj, const CaptureSetting& setting); static void updateCameraSetting(UCameraComponent* camera, const CaptureSetting& setting, const NedTransform& ned_transform); diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index a9fc8c6b1..9da9d1d61 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -363,6 +363,12 @@ void PawnSimApi::toggleTrace() } } +void PawnSimApi::setTraceLine(const std::vector& color_rgba, float thickness) { + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + trace_color_ = color.ToFColor(true); + trace_thickness_ = thickness; +} + void PawnSimApi::allowPassthroughToggleInput() { state_.passthrough_enabled = !state_.passthrough_enabled; @@ -403,12 +409,20 @@ msr::airlib::CameraInfo PawnSimApi::getCameraInfo(const std::string& camera_name return camera_info; } -void PawnSimApi::setCameraOrientation(const std::string& camera_name, const msr::airlib::Quaternionr& orientation) +void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose) +{ + UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, pose]() { + APIPCamera* camera = getCamera(camera_name); + FTransform pose_unreal = ned_transform_.fromRelativeNed(pose); + camera->setCameraPose(pose_unreal); + }, true); +} + +void PawnSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees) { - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, orientation]() { + UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, fov_degrees]() { APIPCamera* camera = getCamera(camera_name); - FQuat quat = ned_transform_.fromNed(orientation); - camera->setCameraOrientation(quat.Rotator()); + camera->setCameraFoV(fov_degrees); }, true); } @@ -455,7 +469,7 @@ void PawnSimApi::setPoseInternal(const Pose& pose, bool ignore_collision) params_.pawn->SetActorLocationAndRotation(position, orientation, true); if (state_.tracing_enabled && (state_.last_position - position).SizeSquared() > 0.25) { - DrawDebugLine(params_.pawn->GetWorld(), state_.last_position, position, FColor::Purple, true, -1.0F, 0, 3.0F); + DrawDebugLine(params_.pawn->GetWorld(), state_.last_position, position, trace_color_, true, -1.0F, 0, trace_thickness_); state_.last_position = position; } else if (!state_.tracing_enabled) { @@ -496,12 +510,12 @@ void PawnSimApi::updateKinematics(float dt) auto next_kinematics = kinematics_->getState(); next_kinematics.pose = getPose(); - next_kinematics.twist.linear = getNedTransform().toLocalNed(getPawn()->GetVelocity()); + next_kinematics.twist.linear = getNedTransform().toLocalNedVelocity(getPawn()->GetVelocity()); next_kinematics.twist.angular = msr::airlib::VectorMath::toAngularVelocity( kinematics_->getPose().orientation, next_kinematics.pose.orientation, dt); - next_kinematics.accelerations.linear = (next_kinematics.twist.linear - kinematics_->getTwist().linear) / dt; - next_kinematics.accelerations.angular = (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt; + next_kinematics.accelerations.linear = dt > 0 ? (next_kinematics.twist.linear - kinematics_->getTwist().linear) / dt : next_kinematics.accelerations.linear; + next_kinematics.accelerations.angular = dt > 0 ? (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt : next_kinematics.accelerations.angular; kinematics_->setState(next_kinematics); kinematics_->update(); diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index 750d86506..2148ae16c 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -77,7 +77,8 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase { virtual Pose getPose() const override; virtual void setPose(const Pose& pose, bool ignore_collision) override; virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name) const override; - virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) override; + virtual void setCameraPose(const std::string& camera_name, const Pose& pose) override; + virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) override; virtual CollisionInfo getCollisionInfo() const override; virtual int getRemoteControlID() const override; virtual msr::airlib::RCData getRCData() const override; @@ -86,6 +87,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase { return params_.vehicle_name; } virtual void toggleTrace() override; + virtual void setTraceLine(const std::vector& color_rgba, float thickness) override; virtual void updateRenderedState(float dt) override; virtual void updateRendering(float dt) override; @@ -188,4 +190,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase { std::unique_ptr kinematics_; std::unique_ptr environment_; + + FColor trace_color_ = FColor::Purple; + float trace_thickness_ = 3.0f; }; diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp b/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp index be17c2544..c9d3d61f9 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp @@ -1,6 +1,6 @@ #include "RecordingFile.h" #include "HAL/PlatformFilemanager.h" -#include "FileHelper.h" +#include "Misc/FileHelper.h" #include #include "ImageUtils.h" #include "common/ClockFactory.hpp" diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.h b/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.h index fcc7a5a5d..dac5cba6b 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.h +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.h @@ -4,7 +4,7 @@ #include #include "AirBlueprintLib.h" #include "physics/Kinematics.hpp" -#include "FileManager.h" +#include "HAL/FileManager.h" #include "PawnSimApi.h" diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp index 6fb24febf..e325386e9 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp @@ -1,5 +1,5 @@ #include "RecordingThread.h" -#include "TaskGraphInterfaces.h" +#include "Async/TaskGraphInterfaces.h" #include "HAL/RunnableThread.h" #include diff --git a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp index 3b6017a34..57f423272 100644 --- a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp +++ b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp @@ -1,7 +1,7 @@ #include "RenderRequest.h" #include "TextureResource.h" #include "Engine/TextureRenderTarget2D.h" -#include "TaskGraphInterfaces.h" +#include "Async/TaskGraphInterfaces.h" #include "ImageUtils.h" #include "AirBlueprintLib.h" @@ -74,13 +74,12 @@ void RenderRequest::getScreenshot(std::shared_ptr params[], std::v // The completion is called immeidately after GameThread sends the // rendering commands to RenderThread. Hence, our ExecuteTask will // execute *immediately* after RenderThread renders the scene! - ENQUEUE_UNIQUE_RENDER_COMMAND_ONEPARAMETER( - SceneDrawCompletion, - RenderRequest *, This, this, - { - This->ExecuteTask(); - } - ); + RenderRequest* This = this; + ENQUEUE_RENDER_COMMAND(SceneDrawCompletion)( + [This](FRHICommandListImmediate& RHICmdList) + { + This->ExecuteTask(); + }); game_viewport_->bDisableWorldRendering = saved_DisableWorldRendering_; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 4e5102c79..74f353f9e 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -1,5 +1,5 @@ #include "SimHUD.h" -#include "ConstructorHelpers.h" +#include "UObject/ConstructorHelpers.h" #include "Kismet/KismetSystemLibrary.h" #include "Misc/FileHelper.h" @@ -23,6 +23,8 @@ void ASimHUD::BeginPlay() try { UAirBlueprintLib::OnBeginPlay(); + loadLevel(); + initializeSettings(); setUnrealEngineSettings(); createSimMode(); @@ -282,6 +284,13 @@ std::string ASimHUD::getSimModeFromUser() return "Car"; } +void ASimHUD::loadLevel() +{ + if (AirSimSettings::singleton().level_name != "") + UAirBlueprintLib::RunCommandOnGameThread([&]() {UAirBlueprintLib::loadLevel(this->GetWorld(), FString(AirSimSettings::singleton().level_name.c_str()));}, true); + else + UAirBlueprintLib::RunCommandOnGameThread([&]() {UAirBlueprintLib::loadLevel(this->GetWorld(), FString("Blocks"));}, true); +} void ASimHUD::createSimMode() { std::string simmode_name = AirSimSettings::singleton().simmode_name; @@ -357,9 +366,10 @@ bool ASimHUD::getSettingsText(std::string& settingsText) readSettingsTextFromFile(FString(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); } -// Attempts to parse the settings text from the command line +// Attempts to parse the settings file path or the settings text from the command line // Looks for the flag "--settings". If it exists, settingsText will be set to the value. -// Example: AirSim.exe -s '{"foo" : "bar"}' -> settingsText will be set to {"foo": "bar"} +// Example (Path): AirSim.exe --settings "C:\path\to\settings.json" +// Example (Text): AirSim.exe -s '{"foo" : "bar"}' -> settingsText will be set to {"foo": "bar"} // Returns true if the argument is present, false otherwise. bool ASimHUD::getSettingsTextFromCommandLine(std::string& settingsText) { @@ -372,6 +382,11 @@ bool ASimHUD::getSettingsTextFromCommandLine(std::string& settingsText) FString commandLineArgsFString = FString(commandLineArgs); int idx = commandLineArgsFString.Find(TEXT("-settings")); FString settingsJsonFString = commandLineArgsFString.RightChop(idx + 10); + + if (readSettingsTextFromFile(settingsJsonFString.TrimQuotes(), settingsText)) { + return true; + } + if (FParse::QuotedString(*settingsJsonFString, settingsTextFString)) { settingsText = std::string(TCHAR_TO_UTF8(*settingsTextFString)); found = true; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h index 06bde5087..fab7fdf82 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h @@ -59,6 +59,7 @@ class AIRSIM_API ASimHUD : public AHUD void createSimMode(); void initializeSettings(); void setUnrealEngineSettings(); + void loadLevel(); void createMainWidget(); const std::vector& getSubWindowSettings() const; std::vector& getSubWindowSettings(); diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 68e8106e7..fc292d400 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -2,7 +2,7 @@ // Licensed under the MIT License. #include "DirectInputJoystick.h" -#include "UnrealMathUtility.h" +#include "Math/UnrealMathUtility.h" #if defined _WIN32 || defined _WIN64 diff --git a/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h b/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h new file mode 100644 index 000000000..93bd97e79 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h @@ -0,0 +1,15 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#pragma once + +#include "CoreMinimal.h" +#include "Blueprint/UserWidget.h" +#include "Runtime/UMG/Public/Components/Image.h" +#include "LoadingScreenWidget.generated.h" + + +UCLASS() +class AIRSIM_API ULoadingScreenWidget : public UUserWidget +{ + GENERATED_BODY() +}; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index c5f7033f9..74f923c65 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -3,7 +3,7 @@ #include "Misc/MessageDialog.h" #include "Misc/EngineVersion.h" #include "Runtime/Launch/Resources/Version.h" -#include "ConstructorHelpers.h" +#include "UObject/ConstructorHelpers.h" #include "Kismet/GameplayStatics.h" #include "Misc/OutputDeviceNull.h" #include "Engine/World.h" @@ -16,6 +16,7 @@ #include "SimJoyStick/SimJoyStick.h" #include "common/EarthCelestial.hpp" #include "sensors/lidar/LidarSimple.hpp" +#include "sensors/distance/DistanceSimple.hpp" #include "Weather/WeatherLib.h" @@ -26,9 +27,17 @@ //it to AirLib and directly implement WorldSimApiBase interface #include "WorldSimApi.h" +ASimModeBase *ASimModeBase::SIMMODE = nullptr; + +ASimModeBase* ASimModeBase::getSimMode() +{ + return SIMMODE; +} ASimModeBase::ASimModeBase() { + SIMMODE = this; + static ConstructorHelpers::FClassFinder external_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); external_camera_class_ = external_camera_class.Succeeded() ? external_camera_class.Class : nullptr; static ConstructorHelpers::FClassFinder camera_director_class(TEXT("Blueprint'/AirSim/Blueprints/BP_CameraDirector'")); @@ -47,6 +56,30 @@ ASimModeBase::ASimModeBase() static ConstructorHelpers::FClassFinder sky_sphere_class(TEXT("Blueprint'/Engine/EngineSky/BP_Sky_Sphere'")); sky_sphere_class_ = sky_sphere_class.Succeeded() ? sky_sphere_class.Class : nullptr; + + static ConstructorHelpers::FClassFinder loading_screen_class_find(TEXT("WidgetBlueprint'/AirSim/Blueprints/BP_LoadingScreenWidget'")); + if (loading_screen_class_find.Succeeded()) + { + auto loading_screen_class = loading_screen_class_find.Class; + loading_screen_widget_ = CreateWidget(this->GetWorld(), loading_screen_class); + + } + else + loading_screen_widget_ = nullptr; + +} + +void ASimModeBase::toggleLoadingScreen(bool is_visible) +{ + if (loading_screen_widget_ == nullptr) + return; + else { + + if (is_visible) + loading_screen_widget_->SetVisibility(ESlateVisibility::Visible); + else + loading_screen_widget_->SetVisibility(ESlateVisibility::Hidden); + } } void ASimModeBase::BeginPlay() @@ -90,6 +123,9 @@ void ASimModeBase::BeginPlay() UWeatherLib::initWeather(World, spawned_actors_); //UWeatherLib::showWeatherMenu(World); } + + loading_screen_widget_->AddToViewport(); + loading_screen_widget_->SetVisibility(ESlateVisibility::Hidden); } const NedTransform& ASimModeBase::getGlobalNedTransform() @@ -103,12 +139,13 @@ void ASimModeBase::checkVehicleReady() if (api) { //sim-only vehicles may have api as null std::string message; if (!api->isReady(message)) { - UAirBlueprintLib::LogMessage("Vehicle %s was not initialized: ", - "", LogDebugLevel::Failure); //TODO: add vehicle name in message + UAirBlueprintLib::LogMessage("Vehicle was not initialized", "", LogDebugLevel::Failure); + if (message.size() > 0) { + UAirBlueprintLib::LogMessage(message.c_str(), "", LogDebugLevel::Failure); + } UAirBlueprintLib::LogMessage("Tip: check connection info in settings.json", "", LogDebugLevel::Informational); } } - } } @@ -117,8 +154,7 @@ void ASimModeBase::setStencilIDs() UAirBlueprintLib::SetMeshNamingMethod(getSettings().segmentation_setting.mesh_naming_method); if (getSettings().segmentation_setting.init_method == - AirSimSettings::SegmentationSetting::InitMethodType::CommonObjectsRandomIDs) { - + AirSimSettings::SegmentationSetting::InitMethodType::CommonObjectsRandomIDs) { UAirBlueprintLib::InitializeMeshStencilIDs(!getSettings().segmentation_setting.override_existing); } //else don't init @@ -269,6 +305,8 @@ void ASimModeBase::Tick(float DeltaSeconds) drawLidarDebugPoints(); + drawDistanceSensorDebugPoints(); + Super::Tick(DeltaSeconds); } @@ -503,8 +541,8 @@ void ASimModeBase::setupVehiclesAndCamera() //compute initial pose FVector spawn_position = uu_origin.GetLocation(); - msr::airlib::Vector3r settings_position = vehicle_setting.position; - if (!msr::airlib::VectorMath::hasNan(settings_position)) + Vector3r settings_position = vehicle_setting.position; + if (!VectorMath::hasNan(settings_position)) spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); @@ -635,13 +673,12 @@ void ASimModeBase::drawLidarDebugPoints() msr::airlib::VehicleApiBase* api = getApiProvider()->getVehicleApi(vehicle_name); if (api != nullptr) { - - msr::airlib::uint count_lidars = api->getSensors().size(msr::airlib::SensorBase::SensorType::Lidar); + msr::airlib::uint count_lidars = api->getSensors().size(SensorType::Lidar); for (msr::airlib::uint i = 0; i < count_lidars; i++) { // TODO: Is it incorrect to assume LidarSimple here? const msr::airlib::LidarSimple* lidar = - static_cast(api->getSensors().getByType(msr::airlib::SensorBase::SensorType::Lidar, i)); + static_cast(api->getSensors().getByType(SensorType::Lidar, i)); if (lidar != nullptr && lidar->getParams().draw_debug_points) { lidar_draw_debug_points_ = true; @@ -651,7 +688,7 @@ void ASimModeBase::drawLidarDebugPoints() return; for (int j = 0; j < lidar_data.point_cloud.size(); j = j + 3) { - msr::airlib::Vector3r point(lidar_data.point_cloud[j], lidar_data.point_cloud[j + 1], lidar_data.point_cloud[j + 2]); + Vector3r point(lidar_data.point_cloud[j], lidar_data.point_cloud[j + 1], lidar_data.point_cloud[j + 2]); FVector uu_point; @@ -660,7 +697,7 @@ void ASimModeBase::drawLidarDebugPoints() } else if (lidar->getParams().data_frame == AirSimSettings::kSensorLocalFrame) { - msr::airlib::Vector3r point_w = msr::airlib::VectorMath::transformToWorldFrame(point, lidar_data.pose, true); + Vector3r point_w = VectorMath::transformToWorldFrame(point, lidar_data.pose, true); uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point_w); } else @@ -669,10 +706,10 @@ void ASimModeBase::drawLidarDebugPoints() DrawDebugPoint( this->GetWorld(), uu_point, - 5, //size + 5, // size FColor::Green, - true, //persistent (never goes away) - 0.1 //point leaves a trail on moving object + false, // persistent (never goes away) + 0.03 // LifeTime: point leaves a trail on moving object ); } } @@ -681,4 +718,51 @@ void ASimModeBase::drawLidarDebugPoints() } lidar_checks_done_ = true; -} \ No newline at end of file +} + +// Draw debug-point on main viewport for Distance sensor hit +void ASimModeBase::drawDistanceSensorDebugPoints() +{ + if (getApiProvider() == nullptr) + return; + + for (auto& sim_api : getApiProvider()->getVehicleSimApis()) { + PawnSimApi* pawn_sim_api = static_cast(sim_api); + std::string vehicle_name = pawn_sim_api->getVehicleName(); + + msr::airlib::VehicleApiBase* api = getApiProvider()->getVehicleApi(vehicle_name); + + if (api != nullptr) { + msr::airlib::uint count_distance_sensors = api->getSensors().size(SensorType::Distance); + Pose vehicle_pose = pawn_sim_api->getGroundTruthKinematics()->pose; + + for (msr::airlib::uint i=0; i(api->getSensors().getByType(SensorType::Distance, i)); + + if (distance_sensor != nullptr && distance_sensor->getParams().draw_debug_points) { + msr::airlib::DistanceSensorData distance_sensor_data = distance_sensor->getOutput(); + + // Find position of point hit + // Similar to UnrealDistanceSensor.cpp#L19 + // order of Pose addition is important here because it also adds quaternions which is not commutative! + Pose distance_sensor_pose = distance_sensor_data.relative_pose + vehicle_pose; + Vector3r start = distance_sensor_pose.position; + Vector3r point = start + VectorMath::rotateVector(VectorMath::front(), + distance_sensor_pose.orientation, true) * distance_sensor_data.distance; + + FVector uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point); + + DrawDebugPoint( + this->GetWorld(), + uu_point, + 10, // size + FColor::Green, + false, // persistent (never goes away) + 0.03 // LifeTime: point leaves a trail on moving object + ); + } + } + } + } +} diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index bf116989d..1b0b678ff 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -14,9 +14,11 @@ #include "api/ApiProvider.hpp" #include "PawnSimApi.h" #include "common/StateReporterWrapper.hpp" +#include "LoadingScreenWidget.h" #include "SimModeBase.generated.h" +DECLARE_DYNAMIC_MULTICAST_DELEGATE(FLevelLoaded); UCLASS() class AIRSIM_API ASimModeBase : public AActor @@ -25,6 +27,9 @@ class AIRSIM_API ASimModeBase : public AActor GENERATED_BODY() + UPROPERTY(BlueprintAssignable, BlueprintCallable) + FLevelLoaded OnLevelLoaded; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Refs") ACameraDirector* CameraDirector; @@ -34,7 +39,15 @@ class AIRSIM_API ASimModeBase : public AActor UFUNCTION(BlueprintCallable, Category = "Recording") bool toggleRecording(); -public: + UFUNCTION(BlueprintPure, Category = "Airsim | get stuff") + static ASimModeBase* getSimMode(); + + UFUNCTION(BlueprintCallable, Category = "Airsim | get stuff") + void toggleLoadingScreen(bool is_visible); + + UFUNCTION(BlueprintCallable, Category = "Airsim | get stuff") + virtual void reset(); + // Sets default values for this actor's properties ASimModeBase(); virtual void BeginPlay() override; @@ -42,7 +55,6 @@ class AIRSIM_API ASimModeBase : public AActor virtual void Tick( float DeltaSeconds ) override; //additional overridable methods - virtual void reset(); virtual std::string getDebugReport(); virtual ECameraDirectorMode getInitialViewMode() const; @@ -110,17 +122,23 @@ class AIRSIM_API ASimModeBase : public AActor UPROPERTY() UClass* pip_camera_class; UPROPERTY() UParticleSystem* collision_display_template; + private: typedef common_utils::Utils Utils; typedef msr::airlib::ClockFactory ClockFactory; typedef msr::airlib::TTimePoint TTimePoint; typedef msr::airlib::TTimeDelta TTimeDelta; + typedef msr::airlib::SensorBase::SensorType SensorType; + typedef msr::airlib::Vector3r Vector3r; + typedef msr::airlib::Pose Pose; + typedef msr::airlib::VectorMath VectorMath; private: //assets loaded in constructor UPROPERTY() UClass* external_camera_class_; UPROPERTY() UClass* camera_director_class_; UPROPERTY() UClass* sky_sphere_class_; + UPROPERTY() ULoadingScreenWidget* loading_screen_widget_; UPROPERTY() AActor* sky_sphere_; @@ -147,7 +165,7 @@ class AIRSIM_API ASimModeBase : public AActor bool lidar_checks_done_ = false; bool lidar_draw_debug_points_ = false; - + static ASimModeBase* SIMMODE; private: void setStencilIDs(); void initializeTimeOfDay(); @@ -156,4 +174,5 @@ class AIRSIM_API ASimModeBase : public AActor void setupPhysicsLoopPeriod(); void showClockStats(); void drawLidarDebugPoints(); + void drawDistanceSensorDebugPoints(); }; diff --git a/Unreal/Plugins/AirSim/Source/TextureShuffleActor.cpp b/Unreal/Plugins/AirSim/Source/TextureShuffleActor.cpp new file mode 100644 index 000000000..b7fd401dd --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/TextureShuffleActor.cpp @@ -0,0 +1,33 @@ +#include "TextureShuffleActor.h" + +void ATextureShuffleActor::SwapTexture_Implementation(int tex_id, int component_id, int material_id) +{ + if (SwappableTextures.Num() < 1) + return; + + if (!MaterialCacheInitialized) + { + TArray components; + GetComponents(components); + NumComponents = components.Num(); + DynamicMaterialInstances.Init(nullptr, components[component_id]->GetNumMaterials()); + MaterialCacheInitialized = true; + } + + if (NumComponents == 0 || DynamicMaterialInstances.Num() == 0) + return; + + tex_id %= SwappableTextures.Num(); + component_id %= NumComponents; + material_id %= DynamicMaterialInstances.Num(); + + if (DynamicMaterialInstances[material_id] == nullptr) + { + DynamicMaterialInstances[material_id] = UMaterialInstanceDynamic::Create(DynamicMaterial, this); + TArray components; + GetComponents(components); + components[component_id]->SetMaterial(material_id, DynamicMaterialInstances[material_id]); + } + + DynamicMaterialInstances[material_id]->SetTextureParameterValue("TextureParameter", SwappableTextures[tex_id]); +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/TextureShuffleActor.h b/Unreal/Plugins/AirSim/Source/TextureShuffleActor.h new file mode 100644 index 000000000..21d5cd806 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/TextureShuffleActor.h @@ -0,0 +1,35 @@ +#pragma once + +#include "CoreMinimal.h" +#include "Materials/Material.h" +#include "common/common_utils/Utils.hpp" +#include "common/AirSimSettings.hpp" +#include "Engine/StaticMeshActor.h" +#include "TextureShuffleActor.generated.h" + + +UCLASS() +class AIRSIM_API ATextureShuffleActor : public AStaticMeshActor +{ + GENERATED_BODY() + +protected: + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = TextureShuffle) + UMaterialInterface *DynamicMaterial = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = TextureShuffle) + TArray SwappableTextures; + +public: + + UFUNCTION(BlueprintNativeEvent) + void SwapTexture(int tex_id = 0, int component_id = 0, int material_id = 0); + +private: + bool MaterialCacheInitialized = false; + int NumComponents = -1; + + UPROPERTY() + TArray DynamicMaterialInstances; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp index 269e6b889..bf1554879 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp @@ -3,23 +3,14 @@ #include "PhysXVehicleManager.h" -CarPawnApi::CarPawnApi(ACarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics, const msr::airlib::GeoPoint& home_geopoint, - const msr::airlib::AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, - const msr::airlib::Kinematics::State& state, const msr::airlib::Environment& environment) - : msr::airlib::CarApiBase(vehicle_setting, sensor_factory, state, environment), - pawn_(pawn), pawn_kinematics_(pawn_kinematics), home_geopoint_(home_geopoint) +CarPawnApi::CarPawnApi(ACarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics, + msr::airlib::CarApiBase* vehicle_api) + : pawn_(pawn), pawn_kinematics_(pawn_kinematics), vehicle_api_(vehicle_api) { movement_ = pawn->GetVehicleMovement(); } -bool CarPawnApi::armDisarm(bool arm) -{ - //TODO: implement arming for car - unused(arm); - return true; -} - -void CarPawnApi::setCarControls(const CarApiBase::CarControls& controls) +void CarPawnApi::updateMovement(const msr::airlib::CarApiBase::CarControls& controls) { last_controls_ = controls; @@ -35,14 +26,9 @@ void CarPawnApi::setCarControls(const CarApiBase::CarControls& controls) movement_->SetUseAutoGears(!controls.is_manual_gear); } -const msr::airlib::CarApiBase::CarControls& CarPawnApi::getCarControls() const -{ - return last_controls_; -} - msr::airlib::CarApiBase::CarState CarPawnApi::getCarState() const { - CarApiBase::CarState state( + msr::airlib::CarApiBase::CarState state( movement_->GetForwardSpeed() / 100, //cm/s -> m/s movement_->GetCurrentGear(), movement_->GetEngineRotationSpeed(), @@ -54,11 +40,11 @@ msr::airlib::CarApiBase::CarState CarPawnApi::getCarState() const return state; } -void CarPawnApi::resetImplementation() +void CarPawnApi::reset() { - msr::airlib::CarApiBase::resetImplementation(); + vehicle_api_->reset(); - last_controls_ = CarControls(); + last_controls_ = msr::airlib::CarApiBase::CarControls(); auto phys_comps = UAirBlueprintLib::getPhysicsComponents(pawn_); UAirBlueprintLib::RunCommandOnGameThread([this, &phys_comps]() { for (auto* phys_comp : phys_comps) { @@ -69,16 +55,17 @@ void CarPawnApi::resetImplementation() movement_->ResetMoveState(); movement_->SetActive(false); movement_->SetActive(true, true); - setCarControls(CarControls()); + vehicle_api_->setCarControls(msr::airlib::CarApiBase::CarControls()); + updateMovement(msr::airlib::CarApiBase::CarControls()); - auto pv = movement_->PVehicle; - if (pv) { - pv->mWheelsDynData.setToRestState(); - } - auto pvd = movement_->PVehicleDrive; - if (pvd) { - pvd->mDriveDynData.setToRestState(); - } + auto pv = movement_->PVehicle; + if (pv) { + pv->mWheelsDynData.setToRestState(); + } + auto pvd = movement_->PVehicleDrive; + if (pvd) { + pvd->mDriveDynData.setToRestState(); + } }, true); UAirBlueprintLib::RunCommandOnGameThread([this, &phys_comps]() { @@ -89,25 +76,8 @@ void CarPawnApi::resetImplementation() void CarPawnApi::update() { - msr::airlib::CarApiBase::update(); -} - -msr::airlib::GeoPoint CarPawnApi::getHomeGeoPoint() const -{ - return home_geopoint_; -} - -void CarPawnApi::enableApiControl(bool is_enabled) -{ - if (api_control_enabled_ != is_enabled) { - last_controls_ = CarControls(); - api_control_enabled_ = is_enabled; - } -} - -bool CarPawnApi::isApiControlEnabled() const -{ - return api_control_enabled_; + vehicle_api_->updateCarState(getCarState()); + vehicle_api_->update(); } CarPawnApi::~CarPawnApi() = default; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h index b5db614c6..294378528 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h @@ -6,38 +6,26 @@ #include "CarPawn.h" -class CarPawnApi : public msr::airlib::CarApiBase { +class CarPawnApi { public: typedef msr::airlib::ImageCaptureBase ImageCaptureBase; - CarPawnApi(ACarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics, const msr::airlib::GeoPoint& home_geopoint, - const msr::airlib::AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, - const msr::airlib::Kinematics::State& state, const msr::airlib::Environment& environment); + CarPawnApi(ACarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics, + msr::airlib::CarApiBase* vehicle_api); - virtual void setCarControls(const CarApiBase::CarControls& controls) override; + void updateMovement(const msr::airlib::CarApiBase::CarControls& controls); - virtual CarApiBase::CarState getCarState() const override; + msr::airlib::CarApiBase::CarState getCarState() const; - virtual void update() override; - - virtual msr::airlib::GeoPoint getHomeGeoPoint() const override; - - virtual void enableApiControl(bool is_enabled) override; - virtual bool isApiControlEnabled() const override; - virtual bool armDisarm(bool arm) override; - - virtual const CarApiBase::CarControls& getCarControls() const override; + void reset(); + void update(); virtual ~CarPawnApi(); -protected: - virtual void resetImplementation() override; - private: UWheeledVehicleMovementComponent* movement_; - bool api_control_enabled_ = false; - CarControls last_controls_; + msr::airlib::CarApiBase::CarControls last_controls_; ACarPawn* pawn_; const msr::airlib::Kinematics::State* pawn_kinematics_; - msr::airlib::GeoPoint home_geopoint_; -}; \ No newline at end of file + msr::airlib::CarApiBase* vehicle_api_; +}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp index 7e55d99d0..bea68bc8e 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp @@ -7,7 +7,7 @@ using namespace msr::airlib; CarPawnSimApi::CarPawnSimApi(const Params& params, - const CarPawnApi::CarControls& keyboard_controls, UWheeledVehicleMovementComponent* movement) + const msr::airlib::CarApiBase::CarControls& keyboard_controls, UWheeledVehicleMovementComponent* movement) : PawnSimApi(params), params_(params), keyboard_controls_(keyboard_controls) { @@ -20,16 +20,17 @@ void CarPawnSimApi::initialize() createVehicleApi(static_cast(params_.pawn), params_.home_geopoint); //TODO: should do reset() here? - joystick_controls_ = CarPawnApi::CarControls(); + joystick_controls_ = msr::airlib::CarApiBase::CarControls(); } void CarPawnSimApi::createVehicleApi(ACarPawn* pawn, const msr::airlib::GeoPoint& home_geopoint) { //create vehicle params std::shared_ptr sensor_factory = std::make_shared(getPawn(), &getNedTransform()); - vehicle_api_ = std::unique_ptr(new CarPawnApi(pawn, getGroundTruthKinematics(), home_geopoint, - getVehicleSetting(), sensor_factory, - (*getGroundTruthKinematics()), (*getGroundTruthEnvironment()))); + + vehicle_api_ = CarApiFactory::createApi(getVehicleSetting(), sensor_factory, (*getGroundTruthKinematics()), + (*getGroundTruthEnvironment()), home_geopoint); + pawn_api_ = std::unique_ptr(new CarPawnApi(pawn, getGroundTruthKinematics(), vehicle_api_.get())); } std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const @@ -41,7 +42,7 @@ std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const } const msr::airlib::Kinematics::State* kinematics = getGroundTruthKinematics(); - const auto state = vehicle_api_->getCarState(); + const auto state = pawn_api_->getCarState(); common_line .append(std::to_string(current_controls_.throttle)).append("\t") @@ -60,7 +61,7 @@ std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const void CarPawnSimApi::updateRenderedState(float dt) { PawnSimApi::updateRenderedState(dt); - + vehicle_api_->getStatusMessages(vehicle_api_messages_); //TODO: do we need this for cars? @@ -146,10 +147,12 @@ void CarPawnSimApi::updateCarControls() if (!vehicle_api_->isApiControlEnabled()) { //all car controls from anywhere must be routed through API component vehicle_api_->setCarControls(current_controls_); + pawn_api_->updateMovement(current_controls_); } else { UAirBlueprintLib::LogMessageString("Control Mode: ", "API", LogDebugLevel::Informational); current_controls_ = vehicle_api_->getCarControls(); + pawn_api_->updateMovement(current_controls_); } UAirBlueprintLib::LogMessageString("Accel: ", std::to_string(current_controls_.throttle), LogDebugLevel::Informational); UAirBlueprintLib::LogMessageString("Break: ", std::to_string(current_controls_.brake), LogDebugLevel::Informational); @@ -163,13 +166,13 @@ void CarPawnSimApi::resetImplementation() { PawnSimApi::resetImplementation(); - vehicle_api_->reset(); + pawn_api_->reset(); } //physics tick void CarPawnSimApi::update() { - vehicle_api_->update(); + pawn_api_->update(); PawnSimApi::update(); } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h index 94be97408..2bd25d5cc 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h @@ -8,9 +8,10 @@ #include "PawnEvents.h" #include "PawnSimApi.h" #include "vehicles/car/api/CarApiBase.hpp" -#include "physics//Kinematics.hpp" +#include "physics/Kinematics.hpp" #include "common/Common.hpp" #include "common/CommonStructs.hpp" +#include "vehicles/car/CarApiFactory.hpp" class CarPawnSimApi : public PawnSimApi { @@ -19,7 +20,7 @@ class CarPawnSimApi : public PawnSimApi typedef msr::airlib::StateReporter StateReporter; typedef msr::airlib::UpdatableObject UpdatableObject; typedef msr::airlib::Pose Pose; - + public: virtual void initialize() override; virtual ~CarPawnSimApi() = default; @@ -27,9 +28,8 @@ class CarPawnSimApi : public PawnSimApi //VehicleSimApiBase interface //implements game interface to update pawn CarPawnSimApi(const Params& params, - const CarPawnApi::CarControls& keyboard_controls, UWheeledVehicleMovementComponent* movement); + const msr::airlib::CarApiBase::CarControls& keyboard_controls, UWheeledVehicleMovementComponent* movement); - virtual void resetImplementation() override; virtual void update() override; virtual std::string getRecordFileLine(bool is_header_line) const override; @@ -47,6 +47,9 @@ class CarPawnSimApi : public PawnSimApi return vehicle_api_.get(); } +protected: + virtual void resetImplementation() override; + private: void createVehicleApi(ACarPawn* pawn, const msr::airlib::GeoPoint& home_geopoint); void updateCarControls(); @@ -55,11 +58,12 @@ class CarPawnSimApi : public PawnSimApi Params params_; std::unique_ptr vehicle_api_; + std::unique_ptr pawn_api_; std::vector vehicle_api_messages_; //storing reference from pawn - const CarPawnApi::CarControls& keyboard_controls_; + const msr::airlib::CarApiBase::CarControls& keyboard_controls_; - CarPawnApi::CarControls joystick_controls_; - CarPawnApi::CarControls current_controls_; + msr::airlib::CarApiBase::CarControls joystick_controls_; + msr::airlib::CarApiBase::CarControls current_controls_; }; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp index cbaf3a64b..32c760f32 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp @@ -1,5 +1,5 @@ #include "SimModeCar.h" -#include "ConstructorHelpers.h" +#include "UObject/ConstructorHelpers.h" #include "AirBlueprintLib.h" #include "common/AirSimSettings.hpp" @@ -88,7 +88,8 @@ void ASimModeCar::getExistingVehiclePawns(TArray& pawns) const bool ASimModeCar::isVehicleTypeSupported(const std::string& vehicle_type) const { - return vehicle_type == AirSimSettings::kVehicleTypePhysXCar; + return ((vehicle_type == AirSimSettings::kVehicleTypePhysXCar) || + (vehicle_type == AirSimSettings::kVehicleTypeArduRover)); } std::string ASimModeCar::getVehiclePawnPathName(const AirSimSettings::VehicleSetting& vehicle_setting) const diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp index b87d5170c..800e9067d 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp @@ -2,7 +2,7 @@ // Licensed under the MIT License. #include "SimModeComputerVision.h" -#include "ConstructorHelpers.h" +#include "UObject/ConstructorHelpers.h" #include "Engine/World.h" #include "AirBlueprintLib.h" diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/FlyingPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/FlyingPawn.cpp index 6d7005591..af4ccb355 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/FlyingPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/FlyingPawn.cpp @@ -79,7 +79,7 @@ void AFlyingPawn::NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Oth HitNormal, NormalImpulse, Hit); } -void AFlyingPawn::setRotorSpeed(const std::vector& rotor_infos) +void AFlyingPawn::setRotorSpeed(const std::vector& rotor_infos) { for (auto rotor_index = 0; rotor_index < rotor_infos.size(); ++rotor_index) { auto comp = rotating_movements_[rotor_index]; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/FlyingPawn.h b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/FlyingPawn.h index ad657e7d5..9cf5db58f 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/FlyingPawn.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/FlyingPawn.h @@ -34,7 +34,7 @@ class AIRSIM_API AFlyingPawn : public APawn return &pawn_events_; } //called by API to set rotor speed - void setRotorSpeed(const std::vector& rotor_infos); + void setRotorSpeed(const std::vector& rotor_infos); private: //variables diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnEvents.h b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnEvents.h index 007474f62..611e3b475 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnEvents.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnEvents.h @@ -11,14 +11,14 @@ class MultirotorPawnEvents : public PawnEvents { public: //types typedef msr::airlib::real_T real_T; - struct RotorInfo { + struct RotorActuatorInfo { real_T rotor_speed = 0; int rotor_direction = 0; real_T rotor_thrust = 0; real_T rotor_control_filtered = 0; }; - typedef common_utils::Signal&> ActuatorsSignal; + typedef common_utils::Signal&> ActuatorsSignal; public: ActuatorsSignal& getActuatorSignal(); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp index 432abc39a..f10dd3fe8 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp @@ -27,10 +27,10 @@ void MultirotorPawnSimApi::initialize() vehicle_params_ = MultiRotorParamsFactory::createConfig(getVehicleSetting(), sensor_factory); vehicle_api_ = vehicle_params_->createMultirotorApi(); //setup physics vehicle - phys_vehicle_ = std::unique_ptr(new MultiRotor(vehicle_params_.get(), vehicle_api_.get(), + multirotor_physics_body_ = std::unique_ptr(new MultiRotorPhysicsBody(vehicle_params_.get(), vehicle_api_.get(), getKinematics(), getEnvironment())); - rotor_count_ = phys_vehicle_->wrenchVertexCount(); - rotor_info_.assign(rotor_count_, RotorInfo()); + rotor_count_ = multirotor_physics_body_->wrenchVertexCount(); + rotor_actuator_info_.assign(rotor_count_, RotorActuatorInfo()); vehicle_api_->setSimulatedGroundTruth(getGroundTruthKinematics(), getGroundTruthEnvironment()); @@ -60,21 +60,21 @@ void MultirotorPawnSimApi::updateRenderedState(float dt) //move collision info from rendering engine to vehicle const CollisionInfo& collision_info = getCollisionInfo(); - phys_vehicle_->setCollisionInfo(collision_info); + multirotor_physics_body_->setCollisionInfo(collision_info); if (pending_pose_status_ == PendingPoseStatus::RenderStatePending) { - phys_vehicle_->setPose(pending_phys_pose_); + multirotor_physics_body_->setPose(pending_phys_pose_); pending_pose_status_ = PendingPoseStatus::RenderPending; } - last_phys_pose_ = phys_vehicle_->getPose(); + last_phys_pose_ = multirotor_physics_body_->getPose(); - collision_response = phys_vehicle_->getCollisionResponseInfo(); + collision_response = multirotor_physics_body_->getCollisionResponseInfo(); //update rotor poses for (unsigned int i = 0; i < rotor_count_; ++i) { - const auto& rotor_output = phys_vehicle_->getRotorOutput(i); - RotorInfo* info = &rotor_info_[i]; + const auto& rotor_output = multirotor_physics_body_->getRotorOutput(i); + RotorActuatorInfo* info = &rotor_actuator_info_[i]; info->rotor_speed = rotor_output.speed; info->rotor_direction = static_cast(rotor_output.turning_direction); info->rotor_thrust = rotor_output.thrust; @@ -126,7 +126,7 @@ void MultirotorPawnSimApi::updateRendering(float dt) UAirBlueprintLib::LogMessage(FString(e.what()), TEXT(""), LogDebugLevel::Failure, 30); } - pawn_events_->getActuatorSignal().emit(rotor_info_); + pawn_events_->getActuatorSignal().emit(rotor_actuator_info_); } void MultirotorPawnSimApi::setPose(const Pose& pose, bool ignore_collision) @@ -142,7 +142,7 @@ void MultirotorPawnSimApi::resetImplementation() PawnSimApi::resetImplementation(); vehicle_api_->reset(); - phys_vehicle_->reset(); + multirotor_physics_body_->reset(); vehicle_api_messages_.clear(); } @@ -153,7 +153,7 @@ void MultirotorPawnSimApi::update() PawnSimApi::update(); //update forces on vertices - phys_vehicle_->update(); + multirotor_physics_body_->update(); //update to controller must be done after kinematics have been updated by physics engine } @@ -162,12 +162,12 @@ void MultirotorPawnSimApi::reportState(StateReporter& reporter) { PawnSimApi::reportState(reporter); - phys_vehicle_->reportState(reporter); + multirotor_physics_body_->reportState(reporter); } MultirotorPawnSimApi::UpdatableObject* MultirotorPawnSimApi::getPhysicsBody() { - return phys_vehicle_->getPhysicsBody(); + return multirotor_physics_body_->getPhysicsBody(); } //*** End: UpdatableState implementation ***// diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h index 6254c5d6c..3e3d17916 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h @@ -3,7 +3,7 @@ #include "CoreMinimal.h" #include "PawnSimApi.h" -#include "vehicles/multirotor/MultiRotor.hpp" +#include "vehicles/multirotor/MultiRotorPhysicsBody.hpp" #include "vehicles/multirotor/MultiRotorParams.hpp" #include "physics//Kinematics.hpp" #include "common/Common.hpp" @@ -18,12 +18,12 @@ class MultirotorPawnSimApi : public PawnSimApi public: typedef msr::airlib::real_T real_T; typedef msr::airlib::Utils Utils; - typedef msr::airlib::MultiRotor MultiRotor; + typedef msr::airlib::MultiRotorPhysicsBody MultiRotor; typedef msr::airlib::StateReporter StateReporter; typedef msr::airlib::UpdatableObject UpdatableObject; typedef msr::airlib::Pose Pose; - typedef MultirotorPawnEvents::RotorInfo RotorInfo; + typedef MultirotorPawnEvents::RotorActuatorInfo RotorActuatorInfo; public: virtual void initialize() override; @@ -60,9 +60,9 @@ class MultirotorPawnSimApi : public PawnSimApi std::unique_ptr vehicle_api_; std::unique_ptr vehicle_params_; - std::unique_ptr phys_vehicle_; + std::unique_ptr multirotor_physics_body_; unsigned int rotor_count_; - std::vector rotor_info_; + std::vector rotor_actuator_info_; //show info on collision response from physics engine CollisionResponse collision_response; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp index 8af796f10..801ace327 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp @@ -2,7 +2,7 @@ // Licensed under the MIT License. #include "SimModeWorldMultiRotor.h" -#include "ConstructorHelpers.h" +#include "UObject/ConstructorHelpers.h" #include "Logging/MessageLog.h" #include "Engine/World.h" #include "GameFramework/PlayerController.h" diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 1627aaca4..7180fba12 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -1,11 +1,137 @@ #include "WorldSimApi.h" +#include "common/common_utils/Utils.hpp" #include "AirBlueprintLib.h" +#include "TextureShuffleActor.h" #include "common/common_utils/Utils.hpp" #include "Weather/WeatherLib.h" +#include "DrawDebugHelpers.h" +#include +#include WorldSimApi::WorldSimApi(ASimModeBase* simmode) - : simmode_(simmode) + : simmode_(simmode) {} + +bool WorldSimApi::loadLevel(const std::string& level_name) +{ + bool success; + using namespace std::chrono_literals; + + // Add loading screen to viewport + simmode_->toggleLoadingScreen(true); + std::this_thread::sleep_for(0.1s); + UAirBlueprintLib::RunCommandOnGameThread([this, level_name]() { + + this->current_level_ = UAirBlueprintLib::loadLevel(this->simmode_->GetWorld(), FString(level_name.c_str())); + }, true); + + if (this->current_level_) + { + success = true; + std::this_thread::sleep_for(1s); + spawnPlayer(); + } + else + success = false; + + //Remove Loading screen from viewport + UAirBlueprintLib::RunCommandOnGameThread([this, level_name]() { + this->simmode_->OnLevelLoaded.Broadcast(); + }, true); + this->simmode_->toggleLoadingScreen(false); + + return success; +} + +void WorldSimApi::spawnPlayer() +{ + using namespace std::chrono_literals; + UE_LOG(LogTemp, Log, TEXT("spawning player")); + bool success{ false }; + + UAirBlueprintLib::RunCommandOnGameThread([&]() { + success = UAirBlueprintLib::spawnPlayer(this->simmode_->GetWorld()); + }, true); + + if (!success) + { + UE_LOG(LogTemp, Error, TEXT("Could not find valid PlayerStart Position")); + } + else + { + std::this_thread::sleep_for(1s); + simmode_->reset(); + } +} + +bool WorldSimApi::destroyObject(const std::string& object_name) +{ + bool result{ false }; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + if (actor) + { + actor->Destroy(); + result = actor->IsPendingKill(); + } + }, true); + return result; +} + +std::string WorldSimApi::spawnObject(std::string& object_name, const std::string& load_object, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale) +{ + // Create struct for Location and Rotation of actor in Unreal + FTransform actor_transform = simmode_->getGlobalNedTransform().fromGlobalNed(pose); + bool found_object; + UAirBlueprintLib::RunCommandOnGameThread([this, load_object, &object_name, &actor_transform, &found_object, &scale]() { + // Find mesh in /Game and /AirSim asset registry. When more plugins are added this function will have to change + UStaticMesh* LoadObject = dynamic_cast(UAirBlueprintLib::GetMeshFromRegistry(load_object)); + if (LoadObject) + { + std::vector matching_names = UAirBlueprintLib::ListMatchingActors(simmode_->GetWorld(), ".*"+object_name+".*"); + if (matching_names.size() > 0) + { + size_t greatest_num{ 0 }, result{ 0 }; + for (auto match : matching_names) + { + std::string number_extension = match.substr(match.find_last_not_of("0123456789") + 1); + if (number_extension != "") + { + result = std::stoi(number_extension); + greatest_num = greatest_num > result ? greatest_num : result; + } + } + object_name += std::to_string(greatest_num + 1); + } + FActorSpawnParameters new_actor_spawn_params; + new_actor_spawn_params.Name = FName(object_name.c_str()); + this->createNewActor(new_actor_spawn_params, actor_transform, scale, LoadObject); + found_object = true; + } + else + { + found_object = false; + } + }, true); + + if (!found_object) + { + throw std::invalid_argument( + "There were no objects with name " + load_object + " found in the Registry"); + } + return object_name; +} + +void WorldSimApi::createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh) { + AActor* NewActor = simmode_->GetWorld()->SpawnActor(AActor::StaticClass(), FVector::ZeroVector, FRotator::ZeroRotator, spawn_params); // new + UStaticMeshComponent* ObjectComponent = NewObject(NewActor); + ObjectComponent->SetStaticMesh(static_mesh); + ObjectComponent->SetRelativeLocation(FVector(0, 0, 0)); + ObjectComponent->SetWorldScale3D(FVector(scale[0], scale[1], scale[2])); + ObjectComponent->SetHiddenInGame(false, true); + ObjectComponent->RegisterComponent(); + NewActor->SetRootComponent(ObjectComponent); + NewActor->SetActorLocationAndRotation(actor_transform.GetLocation(), actor_transform.GetRotation(), false, nullptr, ETeleportType::TeleportPhysics); } bool WorldSimApi::isPaused() const @@ -16,12 +142,8 @@ bool WorldSimApi::isPaused() const void WorldSimApi::reset() { UAirBlueprintLib::RunCommandOnGameThread([this]() { - simmode_->reset(); - - //reset any chars we have - for (auto& c : chars_) - c.second->reset(); - }, true); + simmode_->reset(); + }, true); } void WorldSimApi::pause(bool is_paused) @@ -74,7 +196,6 @@ std::vector WorldSimApi::listSceneObjects(const std::string& name_r return result; } - WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) const { Pose result; @@ -83,6 +204,18 @@ WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) con result = actor ? simmode_->getGlobalNedTransform().toGlobalNed(FTransform(actor->GetActorRotation(), actor->GetActorLocation())) : Pose::nanPose(); }, true); + + return result; +} + +WorldSimApi::Vector3r WorldSimApi::getObjectScale(const std::string& object_name) const +{ + Vector3r result; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + result = actor ? Vector3r(actor->GetActorScale().X, actor->GetActorScale().Y, actor->GetActorScale().Z) + : Vector3r::Zero(); + }, true); return result; } @@ -104,10 +237,26 @@ bool WorldSimApi::setObjectPose(const std::string& object_name, const WorldSimAp return result; } +bool WorldSimApi::setObjectScale(const std::string& object_name, const Vector3r& scale) +{ + bool result; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &scale, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + if (actor) { + actor->SetActorScale3D(FVector(scale[0], scale[1], scale[2])); + result = true; + } + else + result = false; + }, true); + return result; +} + void WorldSimApi::enableWeather(bool enable) { UWeatherLib::setWeatherEnabled(simmode_->GetWorld(), enable); } + void WorldSimApi::setWeatherParameter(WeatherParameter param, float val) { unsigned char param_n = static_cast(msr::airlib::Utils::toNumeric(param)); @@ -116,138 +265,144 @@ void WorldSimApi::setWeatherParameter(WeatherParameter param, float val) UWeatherLib::setWeatherParamScalar(simmode_->GetWorld(), param_e, val); } - -//------------------------------------------------- Char APIs -----------------------------------------------------------/ - -void WorldSimApi::charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) -{ - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->setFaceExpression(expression_name, value); -} - -float WorldSimApi::charGetFaceExpression(const std::string& expression_name, const std::string& character_name) const +std::unique_ptr> WorldSimApi::swapTextures(const std::string& tag, int tex_id, int component_id, int material_id) { - const AAirSimCharacter* character = getAirSimCharacter(character_name); - return character->getFaceExpression(expression_name); + auto swappedObjectNames = std::make_unique>(); + UAirBlueprintLib::RunCommandOnGameThread([this, &tag, tex_id, component_id, material_id, &swappedObjectNames]() { + //Split the tag string into individual tags. + TArray splitTags; + FString notSplit = FString(tag.c_str()); + FString next = ""; + while (notSplit.Split(",", &next, ¬Split)) + { + next.TrimStartInline(); + splitTags.Add(next); + } + notSplit.TrimStartInline(); + splitTags.Add(notSplit); + + //Texture swap on actors that have all of those tags. + TArray shuffleables; + UAirBlueprintLib::FindAllActor(simmode_, shuffleables); + for (auto *shuffler : shuffleables) + { + bool invalidChoice = false; + for (auto required_tag : splitTags) + { + invalidChoice |= !shuffler->ActorHasTag(FName(*required_tag)); + if (invalidChoice) + break; + } + + if (invalidChoice) + continue; + dynamic_cast(shuffler)->SwapTexture(tex_id, component_id, material_id); + swappedObjectNames->push_back(TCHAR_TO_UTF8(*shuffler->GetName())); + } + }, true); + return swappedObjectNames; } -std::vector WorldSimApi::charGetAvailableFaceExpressions() +//----------- Plotting APIs ----------/ +void WorldSimApi::simFlushPersistentMarkers() { - const AAirSimCharacter* character = getAirSimCharacter(""); - return character->getAvailableFaceExpressions(); + FlushPersistentDebugLines(simmode_->GetWorld()); } -void WorldSimApi::charSetSkinDarkness(float value, const std::string& character_name) +void WorldSimApi::simPlotPoints(const std::vector& points, const std::vector& color_rgba, float size, float duration, bool is_persistent) { - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->setSkinDarkness(value); + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (const auto& point : points) + { + DrawDebugPoint(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(point), size, color.ToFColor(true), is_persistent, duration); + } } -float WorldSimApi::charGetSkinDarkness(const std::string& character_name) const +// plot line for points 0-1, 1-2, 2-3 +void WorldSimApi::simPlotLineStrip(const std::vector& points, const std::vector& color_rgba, float thickness, float duration, bool is_persistent) { - const AAirSimCharacter* character = getAirSimCharacter(character_name); - return character->getSkinDarkness(); + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (size_t idx = 0; idx != points.size()-1; idx++) + { + DrawDebugLine(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(points[idx]), simmode_->getGlobalNedTransform().fromGlobalNed(points[idx+1]), color.ToFColor(true), is_persistent, duration, 0, thickness); + } } -void WorldSimApi::charSetSkinAgeing(float value, const std::string& character_name) +// plot line for points 0-1, 2-3, 4-5... must be even number of points +void WorldSimApi::simPlotLineList(const std::vector& points, const std::vector& color_rgba, float thickness, float duration, bool is_persistent) { - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->setSkinAgeing(value); -} + if (points.size() % 2) + { -float WorldSimApi::charGetSkinAgeing(const std::string& character_name) const -{ - const AAirSimCharacter* character = getAirSimCharacter(character_name); - return character->getSkinAgeing(); -} + } -void WorldSimApi::charSetHeadRotation(const msr::airlib::Quaternionr& q, const std::string& character_name) -{ - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->setHeadRotation(q); + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (int idx = 0; idx < points.size(); idx += 2) + { + DrawDebugLine(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(points[idx]), simmode_->getGlobalNedTransform().fromGlobalNed(points[idx+1]), color.ToFColor(true), is_persistent, duration, 0, thickness); + } } -msr::airlib::Quaternionr WorldSimApi::charGetHeadRotation(const std::string& character_name) const +void WorldSimApi::simPlotArrows(const std::vector& points_start, const std::vector& points_end, const std::vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) { - const AAirSimCharacter* character = getAirSimCharacter(character_name); - return character->getHeadRotation(); + // assert points_start.size() == poinst_end.size() + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (int idx = 0; idx < points_start.size(); idx += 1) + { + DrawDebugDirectionalArrow(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(points_start[idx]), simmode_->getGlobalNedTransform().fromGlobalNed(points_end[idx]), arrow_size, color.ToFColor(true), is_persistent, duration, 0, thickness); + } } -void WorldSimApi::charSetBonePose(const std::string& bone_name, const msr::airlib::Pose& pose, const std::string& character_name) +void WorldSimApi::simPlotStrings(const std::vector& strings, const std::vector& positions, float scale, const std::vector& color_rgba, float duration) { - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->setBonePose(bone_name, pose); + // assert positions.size() == strings.size() + FLinearColor color {color_rgba[0], color_rgba[1], color_rgba[2], color_rgba[3]}; + for (int idx = 0; idx < positions.size(); idx += 1) + { + DrawDebugString(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(positions[idx]), FString(strings[idx].c_str()), NULL, color.ToFColor(true), duration, false, scale); + } } -msr::airlib::Pose WorldSimApi::charGetBonePose(const std::string& bone_name, const std::string& character_name) const +void WorldSimApi::simPlotTransforms(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) { - const AAirSimCharacter* character = getAirSimCharacter(character_name); - return character->getBonePose(bone_name); + for (const auto& pose : poses) + { + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(pose.position), simmode_->getGlobalNedTransform().fromNed(pose.orientation).Rotator(), scale, is_persistent, duration, 0, thickness); + } } -void WorldSimApi::charResetBonePose(const std::string& bone_name, const std::string& character_name) +void WorldSimApi::simPlotTransformsWithNames(const std::vector& poses, const std::vector& names, float tf_scale, float tf_thickness, float text_scale, const std::vector& text_color_rgba, float duration) { - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->resetBonePose(bone_name); + // assert poses.size() == names.size() + FLinearColor color {text_color_rgba[0], text_color_rgba[1], text_color_rgba[2], text_color_rgba[3]}; + for (int idx = 0; idx < poses.size(); idx += 1) + { + DrawDebugCoordinateSystem(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx].position), simmode_->getGlobalNedTransform().fromNed(poses[idx].orientation).Rotator(), tf_scale, false, duration, 0, tf_thickness); + DrawDebugString(simmode_->GetWorld(), simmode_->getGlobalNedTransform().fromGlobalNed(poses[idx]).GetLocation(), FString(names[idx].c_str()), NULL, color.ToFColor(true), duration, false, text_scale); + } } -void WorldSimApi::charSetFacePreset(const std::string& preset_name, float value, const std::string& character_name) +std::vector WorldSimApi::getMeshPositionVertexBuffers() const { - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->setFacePreset(preset_name, value); + std::vector responses; + UAirBlueprintLib::RunCommandOnGameThread([&responses]() { + responses = UAirBlueprintLib::GetStaticMeshComponents(); + }, true); + return responses; } -void WorldSimApi::charSetFacePresets(const std::unordered_map& presets, const std::string& character_name) -{ - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->setFacePresets(presets); -} -void WorldSimApi::charSetBonePoses(const std::unordered_map& poses, const std::string& character_name) -{ - AAirSimCharacter* character = getAirSimCharacter(character_name); - character->setBonePoses(poses); -} -std::unordered_map WorldSimApi::charGetBonePoses(const std::vector& bone_names, const std::string& character_name) const +// Recording APIs +void WorldSimApi::startRecording() { - const AAirSimCharacter* character = getAirSimCharacter(character_name); - return character->getBonePoses(bone_names); + simmode_->startRecording(); } -AAirSimCharacter* WorldSimApi::getAirSimCharacter(const std::string& character_name) +void WorldSimApi::stopRecording() { - AAirSimCharacter* character = nullptr; - UAirBlueprintLib::RunCommandOnGameThread([this, &character_name, &character]() { - if (chars_.size() == 0) { //not found in the cache - TArray characters; - UAirBlueprintLib::FindAllActor(simmode_, characters); - for (AActor* actor : characters) { - character = static_cast(actor); - chars_[std::string( - TCHAR_TO_UTF8(*character->GetName()))] = character; - } - } - - if (chars_.size() == 0) { - throw std::invalid_argument( - "There were no actors of class ACharactor found in the environment"); - } - - //choose first character if name was blank or find by name - character = character_name == "" ? chars_.begin()->second - : common_utils::Utils::findOrDefault(chars_, character_name); - - if (!character) { - throw std::invalid_argument(common_utils::Utils::stringf( - "Character with name %s was not found in the environment", character_name.c_str()).c_str()); - } - }, true); - - return character; + simmode_->stopRecording(); } -const AAirSimCharacter* WorldSimApi::getAirSimCharacter(const std::string& character_name) const +bool WorldSimApi::isRecording() const { - return const_cast(this)->getAirSimCharacter(character_name); + return simmode_->isRecording(); } -//------------------------------------------------- Char APIs -----------------------------------------------------------/ - diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 66a87c5ae..7b647af93 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -4,16 +4,25 @@ #include "common/CommonStructs.hpp" #include "api/WorldSimApiBase.hpp" #include "SimMode/SimModeBase.h" -#include "AirSimCharacter.h" +#include "Components/StaticMeshComponent.h" +#include "Runtime/Engine/Classes/Engine/StaticMesh.h" +#include "Engine/LevelStreamingDynamic.h" #include class WorldSimApi : public msr::airlib::WorldSimApiBase { public: typedef msr::airlib::Pose Pose; + typedef msr::airlib::Vector3r Vector3r; + typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; WorldSimApi(ASimModeBase* simmode); virtual ~WorldSimApi() = default; + virtual bool loadLevel(const std::string& level_name) override; + + virtual std::string spawnObject(std::string& object_name, const std::string& load_name, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale) override; + virtual bool destroyObject(const std::string& object_name) override; + virtual bool isPaused() const override; virtual void reset() override; virtual void pause(bool is_paused) override; @@ -31,35 +40,34 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; + virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override; virtual std::vector listSceneObjects(const std::string& name_regex) const override; virtual Pose getObjectPose(const std::string& object_name) const override; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; + virtual Vector3r getObjectScale(const std::string& object_name) const override; + virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) override; - //----------- APIs to control ACharacter in scene ----------/ - virtual void charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) override; - virtual float charGetFaceExpression(const std::string& expression_name, const std::string& character_name) const override; - virtual std::vector charGetAvailableFaceExpressions() override; - virtual void charSetSkinDarkness(float value, const std::string& character_name) override; - virtual float charGetSkinDarkness(const std::string& character_name) const override; - virtual void charSetSkinAgeing(float value, const std::string& character_name) override; - virtual float charGetSkinAgeing(const std::string& character_name) const override; - virtual void charSetHeadRotation(const msr::airlib::Quaternionr& q, const std::string& character_name) override; - virtual msr::airlib::Quaternionr charGetHeadRotation(const std::string& character_name) const override; - virtual void charSetBonePose(const std::string& bone_name, const msr::airlib::Pose& pose, const std::string& character_name) override; - virtual msr::airlib::Pose charGetBonePose(const std::string& bone_name, const std::string& character_name) const override; - virtual void charResetBonePose(const std::string& bone_name, const std::string& character_name) override; - virtual void charSetFacePreset(const std::string& preset_name, float value, const std::string& character_name) override; - virtual void charSetFacePresets(const std::unordered_map& presets, const std::string& character_name) override; - virtual void charSetBonePoses(const std::unordered_map& poses, const std::string& character_name) override; - virtual std::unordered_map charGetBonePoses(const std::vector& bone_names, const std::string& character_name) const override; - -private: - AAirSimCharacter* getAirSimCharacter(const std::string& character_name); - const AAirSimCharacter* getAirSimCharacter(const std::string& character_name) const; + //----------- Plotting APIs ----------/ + virtual void simFlushPersistentMarkers() override; + virtual void simPlotPoints(const std::vector& points, const std::vector& color_rgba, float size, float duration, bool is_persistent) override; + virtual void simPlotLineStrip(const std::vector& points, const std::vector& color_rgba, float thickness, float duration, bool is_persistent) override; + virtual void simPlotLineList(const std::vector& points, const std::vector& color_rgba, float thickness, float duration, bool is_persistent) override; + virtual void simPlotArrows(const std::vector& points_start, const std::vector& points_end, const std::vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) override; + virtual void simPlotStrings(const std::vector& strings, const std::vector& positions, float scale, const std::vector& color_rgba, float duration) override; + virtual void simPlotTransforms(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) override; + virtual void simPlotTransformsWithNames(const std::vector& poses, const std::vector& names, float tf_scale, float tf_thickness, float text_scale, const std::vector& text_color_rgba, float duration) override; + virtual std::vector getMeshPositionVertexBuffers() const override; + // Recording APIs + virtual void startRecording() override; + virtual void stopRecording() override; + virtual bool isRecording() const override; +private: + void createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); + void spawnPlayer(); private: ASimModeBase* simmode_; - std::map chars_; + ULevelStreamingDynamic* current_level_; }; diff --git a/UnrealPluginFiles.vcxproj b/UnrealPluginFiles.vcxproj index 9d812e2e2..d8b47d7d4 100644 --- a/UnrealPluginFiles.vcxproj +++ b/UnrealPluginFiles.vcxproj @@ -1,5 +1,5 @@  - + Debug @@ -93,6 +93,7 @@ + diff --git a/UnrealPluginFiles.vcxproj.filters b/UnrealPluginFiles.vcxproj.filters index 797cf9717..b0516486a 100644 --- a/UnrealPluginFiles.vcxproj.filters +++ b/UnrealPluginFiles.vcxproj.filters @@ -108,6 +108,9 @@ Source Files + + Source Files + diff --git a/azure/.gitignore b/azure/.gitignore new file mode 100644 index 000000000..f0f1cccf7 --- /dev/null +++ b/azure/.gitignore @@ -0,0 +1 @@ +!.vscode \ No newline at end of file diff --git a/azure/.vscode/extensions.json b/azure/.vscode/extensions.json new file mode 100644 index 000000000..88a39fc08 --- /dev/null +++ b/azure/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + "recommendations": [ + "ms-python.python", + "ms-azuretools.vscode-docker", + "ms-vscode.powershell" + ] +} diff --git a/azure/.vscode/launch.json b/azure/.vscode/launch.json new file mode 100644 index 000000000..17e15f27e --- /dev/null +++ b/azure/.vscode/launch.json @@ -0,0 +1,15 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Python: Current File", + "type": "python", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal" + } + ] +} \ No newline at end of file diff --git a/azure/.vscode/tasks.json b/azure/.vscode/tasks.json new file mode 100644 index 000000000..f4294d52d --- /dev/null +++ b/azure/.vscode/tasks.json @@ -0,0 +1,14 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "Start AirSim", + "type": "shell", + "options": {"shell": {"executable": "powershell.exe"}}, + "command": ".\\start-airsim.ps1" + + } + ] +} \ No newline at end of file diff --git a/azure/app/multirotor.py b/azure/app/multirotor.py new file mode 100644 index 000000000..8fe53c642 --- /dev/null +++ b/azure/app/multirotor.py @@ -0,0 +1,42 @@ +import airsim +import pprint + +def print_state(client): + state = client.getMultirotorState() + s = pprint.pformat(state) + print("state: %s" % s) + + imu_data = client.getImuData() + s = pprint.pformat(imu_data) + print("imu_data: %s" % s) + + barometer_data = client.getBarometerData() + s = pprint.pformat(barometer_data) + print("barometer_data: %s" % s) + + magnetometer_data = client.getMagnetometerData() + s = pprint.pformat(magnetometer_data) + print("magnetometer_data: %s" % s) + + gps_data = client.getGpsData() + s = pprint.pformat(gps_data) + print("gps_data: %s" % s) + +# connect to the AirSim simulator +client = airsim.MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +print_state(client) +print('Takeoff') +client.takeoffAsync().join() + +while True: + print_state(client) + print('Go to (-10, 10, -10) at 5 m/s') + client.moveToPositionAsync(-10, 10, -10, 5).join() + client.hoverAsync().join() + print_state(client) + print('Go to (0, 10, 0) at 5 m/s') + client.moveToPositionAsync(0, 10, 0, 5).join() diff --git a/azure/app/requirements.txt b/azure/app/requirements.txt new file mode 100644 index 000000000..ca180bf3e --- /dev/null +++ b/azure/app/requirements.txt @@ -0,0 +1 @@ +airsim \ No newline at end of file diff --git a/azure/app/settings.json b/azure/app/settings.json new file mode 100644 index 000000000..44b5a2000 --- /dev/null +++ b/azure/app/settings.json @@ -0,0 +1,19 @@ +{ + "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings_json.md", + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "ClockSpeed": 1.0, + "Vehicles": { + "SimpleFlight": { + "VehicleType": "SimpleFlight", + "DefaultVehicleState": "Armed", + "EnableCollisionPassthrogh": false, + "EnableCollisions": true, + "AllowAPIAlways": true, + "RC": { + "RemoteControlID": 0, + "AllowAPIWhenDisconnected": false + } + } + } +} \ No newline at end of file diff --git a/azure/azure-env-creation/configure-vm.ps1 b/azure/azure-env-creation/configure-vm.ps1 new file mode 100644 index 000000000..3910298c0 --- /dev/null +++ b/azure/azure-env-creation/configure-vm.ps1 @@ -0,0 +1,37 @@ +$airSimInstallPath = "C:\AirSim\" +$airSimBinaryZipUrl = "https://github.com/microsoft/AirSim/releases/download/v1.3.1-windows/Blocks.zip" +$airSimBinaryZipFilename = "Blocks.zip" +$airSimBinaryPath = $airSimInstallPath + "blocks\blocks\binaries\win64\blocks.exe" +$airSimBinaryName = "Blocks" + +$webClient = new-object System.Net.WebClient + +# Install the OpenSSH Client +Add-WindowsCapability -Online -Name OpenSSH.Client~~~~0.0.1.0 +# Install the OpenSSH Server +Add-WindowsCapability -Online -Name OpenSSH.Server~~~~0.0.1.0 +# Enable service +Start-Service sshd +Set-Service -Name sshd -StartupType 'Automatic' + +#Install Chocolatey +Set-ExecutionPolicy Bypass -Scope Process -Force; [System.Net.ServicePointManager]::SecurityProtocol = [System.Net.ServicePointManager]::SecurityProtocol -bor 3072; iwr https://chocolatey.org/install.ps1 -UseBasicParsing | iex +# Bypass confirmation in scripts. +choco feature enable --name="'allowGlobalConfirmation'" +choco install python --version=3.8.2 +choco install git +# Run time c++ +choco install vcredist-all +choco install directx + +#Create new folder & set as default directory +New-Item -ItemType directory -Path $airSimInstallPath +cd $airSimInstallPath + +# Get AirSim +$webClient.DownloadFile($airSimBinaryZipUrl, $airSimInstallPath + $airSimBinaryZipFilename) +# Unzip AirSim +Expand-Archive $airSimBinaryZipFilename + +# Firewall rule for AirSim +New-NetFirewallRule -DisplayName $airSimBinaryName -Direction Inbound -Program $airSimBinaryPath -Action Allow diff --git a/azure/azure-env-creation/vm-arm-template.json b/azure/azure-env-creation/vm-arm-template.json new file mode 100644 index 000000000..e87babba6 --- /dev/null +++ b/azure/azure-env-creation/vm-arm-template.json @@ -0,0 +1,250 @@ +{ + "$schema": "https://schema.management.azure.com/schemas/2019-04-01/deploymentTemplate.json#", + "contentVersion": "1.0.0.0", + "parameters": { + "VmName": { + "type": "string", + "metadata": { + "description": "Name for the VM can be whatever you want, but can't be changed once the VM is created." + } + }, + "AdminUsername": { + "type": "string", + "metadata": { + "description": "Admin user name for the VM." + }, + "defaultValue": "AzureUser" + }, + "AdminPassword": { + "type": "securestring", + "metadata": { + "description": "Admin user password for the VM." + } + }, + "VmSize": { + "type": "string", + "metadata": { + "description": "Desired Size of the VM (NV-series installs NVIDIA GPU drivers in WDDM mode for graphical display by default)." + }, + "defaultValue": "Standard_NV6", + "allowedValues": [ + "Standard_NV6_Promo", + "Standard_NV6", + "Standard_NV12", + "Standard_NV24" + ] + }, + "ScriptLocation": { + "type": "string", + "metadata": { + "description": "Location of the setup script" + }, + "defaultValue": "https://raw.githubusercontent.com/microsoft/airsim/master/azure/azure-env-creation" + }, + "ScriptFileName": { + "type": "string", + "metadata": { + "description": "Name of the setup script" + }, + "defaultValue": "configure-vm.ps1" + } + + }, + "variables": { + "NetworkInterfaceCardName": "[concat(parameters('VmName'),'-nic')]", + "PublicIPAddressName": "[concat(parameters('VmName'),'-ip')]", + "NetworkSecurityGroupName": "[concat(parameters('VmName'),'-nsg')]", + "VirtualNetworkName": "[concat(parameters('VmName'),'-vnet')]" + }, + "resources": [ + { + "type": "Microsoft.Network/networkSecurityGroups", + "apiVersion": "2019-12-01", + "name": "[variables('NetworkSecurityGroupName')]", + "location": "[resourceGroup().location]", + "properties": { + "securityRules": [ + { + "name": "RDP", + "properties": { + "protocol": "Tcp", + "sourcePortRange": "*", + "destinationPortRange": "3389", + "sourceAddressPrefix": "*", + "destinationAddressPrefix": "*", + "access": "Allow", + "priority": 300, + "direction": "Inbound" + } + }, + { + "name": "SSH", + "properties": { + "protocol": "Tcp", + "sourcePortRange": "*", + "destinationPortRange": "22", + "sourceAddressPrefix": "*", + "destinationAddressPrefix": "*", + "access": "Allow", + "priority": 320, + "direction": "Inbound" + } + } + + ] + } + }, + { + "type": "Microsoft.Network/virtualNetworks/subnets", + "apiVersion": "2019-12-01", + "name": "[concat(variables('VirtualNetworkName'), '/default')]", + "dependsOn": [ + "[resourceId('Microsoft.Network/virtualNetworks', variables('VirtualNetworkName'))]" + ], + "properties": { + "addressPrefix": "10.0.0.0/24" + } + }, + { + "type": "Microsoft.Network/virtualNetworks", + "apiVersion": "2019-12-01", + "name": "[variables('VirtualNetworkName')]", + "location": "[resourceGroup().location]", + "properties": { + "addressSpace": { + "addressPrefixes": [ + "10.0.0.0/24" + ] + }, + "subnets": [ + { + "name": "default", + "properties": { + "addressPrefix": "10.0.0.0/24" + } + } + ] + } + }, + { + "type": "Microsoft.Network/publicIPAddresses", + "apiVersion": "2019-12-01", + "name": "[variables('PublicIPAddressName')]", + "location": "[resourceGroup().location]", + "properties": { + "publicIPAllocationMethod": "Dynamic" + } + }, + { + "type": "Microsoft.Network/networkInterfaces", + "apiVersion": "2019-12-01", + "name": "[variables('NetworkInterfaceCardName')]", + "location": "[resourceGroup().location]", + "dependsOn": [ + "[resourceId('Microsoft.Network/publicIPAddresses', variables('PublicIPAddressName'))]", + "[resourceId('Microsoft.Network/networkSecurityGroups', variables('NetworkSecurityGroupName'))]", + "[resourceId('Microsoft.Network/virtualNetworks/subnets', variables('VirtualNetworkName'), 'default')]" + ], + "properties": { + "ipConfigurations": [ + { + "name": "ipconfig1", + "properties": { + "publicIPAddress": { + "id": "[resourceId('Microsoft.Network/publicIPAddresses', variables('PublicIPAddressName'))]" + }, + "subnet": { + "id": "[resourceId('Microsoft.Network/virtualNetworks/subnets', variables('VirtualNetworkName'), 'default')]" + } + } + } + ], + "networkSecurityGroup": { + "id": "[resourceId('Microsoft.Network/networkSecurityGroups', variables('NetworkSecurityGroupName'))]" + } + } + }, + { + "type": "Microsoft.Compute/virtualMachines", + "apiVersion": "2019-07-01", + "name": "[parameters('VmName')]", + "location": "[resourceGroup().location]", + "dependsOn": [ + "[resourceId('Microsoft.Network/networkInterfaces', variables('NetworkInterfaceCardName'))]" + ], + "properties": { + "hardwareProfile": { + "vmSize": "[parameters('vmSize')]" + }, + "storageProfile": { + "imageReference": { + "publisher": "MicrosoftWindowsDesktop", + "offer": "Windows-10", + "sku": "rs5-pro", + "version": "latest" + }, + "osDisk": { + "osType": "Windows", + "name": "[concat(parameters('VmName'), '_OsDisk')]", + "createOption": "FromImage", + "caching": "ReadWrite" + } + }, + "osProfile": { + "computerName": "[parameters('VmName')]", + "adminUsername": "[parameters('AdminUsername')]", + "adminPassword": "[parameters('AdminPassword')]" + }, + "networkProfile": { + "networkInterfaces": [ + { + "id": "[resourceId('Microsoft.Network/networkInterfaces', variables('NetworkInterfaceCardName'))]" + } + ] + } + } + }, + { + "name": "[concat(parameters('VmName'),'/GPUDrivers')]", + "type": "Microsoft.Compute/virtualMachines/extensions", + "location": "[resourceGroup().location]", + "apiVersion": "2019-07-01", + "dependsOn": [ + "[resourceId('Microsoft.Compute/virtualMachines/', parameters('VmName'))]" + ], + "properties": { + "publisher": "Microsoft.HpcCompute", + "type": "NvidiaGpuDriverWindows", + "typeHandlerVersion": "1.3", + "autoUpgradeMinorVersion": true, + "settings": { + } + } + }, + { + "name": "[concat(parameters('VmName'),'/SetupScript')]", + "apiVersion": "2019-07-01", + "type": "Microsoft.Compute/virtualMachines/extensions", + "location": "[resourceGroup().location]", + "dependsOn": [ + "[resourceId('Microsoft.Compute/virtualMachines/extensions', parameters('VmName'), 'GPUDrivers')]" + ], + "properties": { + "publisher": "Microsoft.Compute", + "type": "CustomScriptExtension", + "typeHandlerVersion": "1.10", + "autoUpgradeMinorVersion": true, + "settings": { + "fileUris": [ + "[concat(parameters('ScriptLocation'), '/' , parameters('ScriptFileName'))]" + ], + "commandToExecute": "[concat('powershell.exe -ExecutionPolicy bypass -File ./', parameters('ScriptFileName'))]" + }, + "protectedSettings": { + } + } + } + ], + "outputs": { + } +} \ No newline at end of file diff --git a/azure/docker/Dockerfile b/azure/docker/Dockerfile new file mode 100644 index 000000000..27766f87f --- /dev/null +++ b/azure/docker/Dockerfile @@ -0,0 +1,63 @@ +FROM nvidia/cudagl:9.0-devel + +RUN apt-get update +RUN apt-get install \ + sudo \ + libglu1-mesa-dev \ + xdg-user-dirs \ + pulseaudio \ + sudo \ + x11-xserver-utils \ + unzip \ + wget \ + software-properties-common \ + -y --no-install-recommends + +RUN apt-add-repository ppa:deadsnakes/ppa +RUN apt-get update +RUN apt-get install -y \ + python3.6 \ + python3-pip + +RUN python3.6 -m pip install --upgrade pip + +RUN python3.6 -m pip install setuptools wheel + +RUN adduser --force-badname --disabled-password --gecos '' --shell /bin/bash airsim_user && \ + echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers && \ + adduser airsim_user sudo && \ + adduser airsim_user audio && \ + adduser airsim_user video + +USER airsim_user +WORKDIR /home/airsim_user + +# Change the following values to use a different AirSim binary +# Also change the AIRSIM_EXECUTABLE variable in docker-entrypoint.sh +ENV AIRSIM_BINARY_ZIP_URL=https://github.com/microsoft/AirSim/releases/download/v1.3.1-linux/Blocks.zip +ENV AIRSIM_BINARY_ZIP_FILENAME=Blocks.zip + +ENV SDL_VIDEODRIVER_VALUE=offscreen +ENV SDL_HINT_CUDA_DEVICE=0 + +# Download and unzip the AirSim binary +RUN wget -c $AIRSIM_BINARY_ZIP_URL +RUN unzip $AIRSIM_BINARY_ZIP_FILENAME +RUN rm $AIRSIM_BINARY_ZIP_FILENAME + +# Add the Python app to the image +ADD ./app /home/airsim_user/app + +WORKDIR /home/airsim_user +RUN mkdir -p /home/airsim_user/Documents/AirSim +ADD ./app/settings.json /home/airsim_user/Documents/AirSim +ADD ./docker/docker-entrypoint.sh /home/airsim_user/docker-entrypoint.sh + +RUN sudo chown -R airsim_user /home/airsim_user + +WORKDIR /home/airsim_user/app + +# Install Python requirements +RUN python3.6 -m pip install -r requirements.txt + +ENTRYPOINT /home/airsim_user/docker-entrypoint.sh \ No newline at end of file diff --git a/azure/docker/docker-entrypoint.sh b/azure/docker/docker-entrypoint.sh new file mode 100644 index 000000000..436736918 --- /dev/null +++ b/azure/docker/docker-entrypoint.sh @@ -0,0 +1,11 @@ +#!/bin/bash +AIRSIM_EXECUTABLE=/home/airsim_user/Blocks/Blocks.sh + +echo Starting AirSim binary... +$AIRSIM_EXECUTABLE & + +echo Waiting 10 seconds before starting app... +sleep 10 + +echo Starting Python app +python3.6 /home/airsim_user/app/multirotor.py \ No newline at end of file diff --git a/azure/start-airsim.ps1 b/azure/start-airsim.ps1 new file mode 100644 index 000000000..6f5206150 --- /dev/null +++ b/azure/start-airsim.ps1 @@ -0,0 +1,23 @@ +# Script parameters +$airSimExecutable = "c:\AirSim\Blocks\blocks.exe" +$airSimProcessName = "Blocks" + +# Ensure proper path +$env:Path = + [System.Environment]::GetEnvironmentVariable("Path","Machine") + ";" + + [System.Environment]::GetEnvironmentVariable("Path","User") + +# Install python app requirements +pip3 install -r .\app\requirements.txt + +# Overwrite AirSim configuration +New-Item -ItemType Directory -Force -Path $env:USERPROFILE\Documents\AirSim\ +copy .\app\settings.json $env:USERPROFILE\Documents\AirSim\ + +# Kill previous AirSim instance +Stop-Process -Name $airSimProcessName -Force -ErrorAction SilentlyContinue +sleep 2 + +# Start new AirSim instance +Start-Process -NoNewWindow -FilePath $airSimExecutable -ArgumentList "-RenderOffScreen" +echo "Starting the AirSim environment has completed." diff --git a/build.cmd b/build.cmd index e808dc51e..ef65e935b 100644 --- a/build.cmd +++ b/build.cmd @@ -8,14 +8,14 @@ set "noFullPolyCar=" set "buildMode=" REM //check VS version -if "%VisualStudioVersion%"=="" ( +if "%VisualStudioVersion%" == "" ( echo( - echo oh oh... You need to run this command from x64 Native Tools Command Prompt for VS 2017. + echo oh oh... You need to run this command from x64 Native Tools Command Prompt for VS 2019. goto :buildfailed_nomsg ) -if "%VisualStudioVersion%"=="14.0" ( +if "%VisualStudioVersion%" lss "16.0" ( echo( - echo Hello there! We just upgraded AirSim to Unreal Engine 4.18 and Visual Studio 2017. + echo Hello there! We just upgraded AirSim to Unreal Engine 4.24 and Visual Studio 2019. echo Here are few easy steps for upgrade so everything is new and shiny: echo https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md goto :buildfailed_nomsg @@ -79,7 +79,7 @@ ECHO Starting cmake to build rpclib... IF NOT EXIST external\rpclib\rpclib-%RPC_VERSION%\build mkdir external\rpclib\rpclib-%RPC_VERSION%\build cd external\rpclib\rpclib-%RPC_VERSION%\build REM cmake -G"Visual Studio 14 2015 Win64" .. -cmake -G"Visual Studio 15 2017 Win64" .. +cmake -G"Visual Studio 16 2019" .. if "%buildMode%" == "--Debug" ( cmake --build . --config Debug @@ -144,7 +144,7 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 ( REM //---------- get Eigen library ---------- IF NOT EXIST AirLib\deps mkdir AirLib\deps IF NOT EXIST AirLib\deps\eigen3 ( - powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.2/eigen-3.3.2.zip -OutFile eigen3.zip }" + powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip -OutFile eigen3.zip }" powershell -command "& { Expand-Archive -Path eigen3.zip -DestinationPath AirLib\deps }" powershell -command "& { Move-Item -Path AirLib\deps\eigen* -Destination AirLib\deps\del_eigen }" REM move AirLib\deps\eigen* AirLib\deps\del_eigen @@ -158,15 +158,15 @@ IF NOT EXIST AirLib\deps\eigen3 goto :buildfailed REM //---------- now we have all dependencies to compile AirSim.sln which will also compile MavLinkCom ---------- if "%buildMode%" == "--Debug" ( -msbuild /p:Platform=x64 /p:Configuration=Debug AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Debug AirSim.sln if ERRORLEVEL 1 goto :buildfailed ) else if "%buildMode%" == "--Release" ( -msbuild /p:Platform=x64 /p:Configuration=Release AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln if ERRORLEVEL 1 goto :buildfailed ) else ( -msbuild /p:Platform=x64 /p:Configuration=Debug AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Debug AirSim.sln if ERRORLEVEL 1 goto :buildfailed -msbuild /p:Platform=x64 /p:Configuration=Release AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln if ERRORLEVEL 1 goto :buildfailed ) diff --git a/build.sh b/build.sh index 0eed49d5d..c298a6c17 100755 --- a/build.sh +++ b/build.sh @@ -7,23 +7,8 @@ pushd "$SCRIPT_DIR" >/dev/null set -e set -x -MIN_GCC_VERSION=6.0.0 -gccBuild=false function version_less_than_equal_to() { test "$(printf '%s\n' "$@" | sort -V | head -n 1)" = "$1"; } -# Parse command line arguments -while [[ $# -gt 0 ]] -do -key="$1" - -case $key in - --gcc) - gccBuild=true - shift # past argument - ;; -esac -done - # check for rpclib RPC_VERSION=c4fb37acbe67ec99e47e5187acd2a7450bde0cec if [ ! -d "./external/rpclib/rpclib-${RPC_VERSION}" ]; then @@ -43,53 +28,14 @@ else CMAKE=$(which cmake) fi -# set up paths of cc and cxx compiler -if $gccBuild; then - # variable for build output - build_dir=build_gcc_debug - # gcc tools - gcc_ver=$(gcc -dumpfullversion) - gcc_path=$(which cmake) - if [[ "$gcc_path" == "" ]] ; then - echo "ERROR: run setup.sh to install a good version of gcc." - exit 1 - fi - if version_less_than_equal_to $gcc_ver $MIN_GCC_VERSION; then - export CC="gcc-6" - export CXX="g++-6" - else - export CC="gcc" - export CXX="g++" - fi +# variable for build output +build_dir=build_debug +if [ "$(uname)" == "Darwin" ]; then + export CC=/usr/local/opt/llvm@8/bin/clang + export CXX=/usr/local/opt/llvm@8/bin/clang++ else - #check for correct verion of llvm - if [[ ! -d "llvm-source-50" ]]; then - if [[ -d "llvm-source-39" ]]; then - echo "Hello there! We just upgraded AirSim to Unreal Engine 4.18." - echo "Here are few easy steps for upgrade so everything is new and shiny :)" - echo "https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md" - exit 1 - else - echo "The llvm-souce-50 folder was not found! Mystery indeed." - fi - fi - - # check for libc++ - if [[ !(-d "./llvm-build/output/lib") ]]; then - echo "ERROR: clang++ and libc++ is necessary to compile AirSim and run it in Unreal engine" - echo "Please run setup.sh first." - exit 1 - fi - - # variable for build output - build_dir=build_debug - if [ "$(uname)" == "Darwin" ]; then - export CC=/usr/local/opt/llvm-5.0/bin/clang-5.0 - export CXX=/usr/local/opt/llvm-5.0/bin/clang++-5.0 - else - export CC="clang-5.0" - export CXX="clang++-5.0" - fi + export CC="clang-8" + export CXX="clang++-8" fi #install EIGEN library @@ -124,7 +70,6 @@ pushd $build_dir >/dev/null make -j`nproc` popd >/dev/null - mkdir -p AirLib/lib/x64/Debug mkdir -p AirLib/deps/rpclib/lib mkdir -p AirLib/deps/MavLinkCom/lib @@ -158,5 +103,4 @@ echo "For help see:" echo "https://github.com/Microsoft/AirSim/blob/master/docs/build_linux.md" echo "==================================================================" - popd >/dev/null diff --git a/build_docs.sh b/build_docs.sh new file mode 100755 index 000000000..7ba137d74 --- /dev/null +++ b/build_docs.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +cp README.md docs/ +sed -i 's/](docs\//](/g' docs/README.md +mkdocs build diff --git a/check_cmake.bat b/check_cmake.bat index e998c6cbd..13097b257 100644 --- a/check_cmake.bat +++ b/check_cmake.bat @@ -3,7 +3,7 @@ REM //---------- set up variable ---------- setlocal set ROOT_DIR=%~dp0 -set cmake_minversion_minmaj=" 3. 9" +set cmake_minversion_minmaj=" 3. 14" set "cmake_version= . " @@ -38,12 +38,12 @@ exit /b 0 :download_install set /p choice="Press any key to download and install cmake (make sure to add it in path in install options)" -IF NOT EXIST %temp%\cmake-3.10.2-win64-x64.msi ( +IF NOT EXIST %temp%\cmake-3.14.7-win64-x64.msi ( @echo on - powershell -command "& { iwr https://cmake.org/files/v3.10/cmake-3.10.2-win64-x64.msi -OutFile %temp%\cmake-3.10.2-win64-x64.msi }" + powershell -command "& { iwr https://cmake.org/files/v3.14/cmake-3.14.7-win64-x64.msi -OutFile %temp%\cmake-3.14.7-win64-x64.msi }" @echo off ) -msiexec.exe /i "%temp%\cmake-3.10.2-win64-x64.msi" +msiexec.exe /i "%temp%\cmake-3.14.7-win64-x64.msi" exit /b 1 \ No newline at end of file diff --git a/cmake/MavLinkCom/CMakeLists.txt b/cmake/MavLinkCom/CMakeLists.txt index 7743eb8b9..02a3ae64b 100644 --- a/cmake/MavLinkCom/CMakeLists.txt +++ b/cmake/MavLinkCom/CMakeLists.txt @@ -48,6 +48,8 @@ ELSE() LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/windows/WindowsFindSerialPorts.cpp") ENDIF() + + add_library(MavLinkCom STATIC ${MAVLINK_SOURCES}) CommonTargetLink() diff --git a/cmake/cmake-modules/CommonSetup.cmake b/cmake/cmake-modules/CommonSetup.cmake index 639985811..a2b029d5d 100644 --- a/cmake/cmake-modules/CommonSetup.cmake +++ b/cmake/cmake-modules/CommonSetup.cmake @@ -36,10 +36,6 @@ macro(CommonSetup) SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/output/bin) SET(LIBRARY_OUTPUT_PATH ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}) - #libcxx which we will use with specific version of clang - SET(LIBCXX_INC_PATH ${AIRSIM_ROOT}/llvm-build/output/include/c++/v1) - SET(LIBCXX_LIB_PATH ${AIRSIM_ROOT}/llvm-build/output/lib) - #setup include and lib for rpclib which will be referenced by other projects set(RPCLIB_VERSION_FOLDER rpclib-c4fb37acbe67ec99e47e5187acd2a7450bde0cec) set(RPC_LIB_INCLUDES " ${AIRSIM_ROOT}/external/rpclib/${RPCLIB_VERSION_FOLDER}/include") @@ -52,42 +48,19 @@ macro(CommonSetup) IF(UNIX) set(RPC_LIB_DEFINES "-D MSGPACK_PP_VARIADICS_MSVC=0") set(BUILD_TYPE "linux") - - if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang") - #TODO: need to check why below is needed - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D__CLANG__") + if (APPLE) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wstrict-aliasing -D__CLANG__") else () - # other flags used in Unreal: -funwind-tables -fdiagnostics-format=msvc -fno-inline -Werror -fno-omit-frame-pointer -fstack-protector -O2 - # TODO: add back -Wunused-parameter -Wno-documentation after rpclib can be compiled set(CMAKE_CXX_FLAGS "\ - -std=c++14 -ggdb -Wall -Wextra -Wstrict-aliasing -Wunreachable-code -Wcast-qual -Wctor-dtor-privacy \ - -Wdisabled-optimization -Wformat=2 -Winit-self -Wmissing-include-dirs -Wswitch-default \ - -Wold-style-cast -Woverloaded-virtual -Wredundant-decls -Wshadow -Wstrict-overflow=5 -Wswitch-default -Wundef \ - -Wno-variadic-macros -Wno-parentheses -Wno-unused-function -Wno-unused -Wno-documentation -fdiagnostics-show-option \ + -std=c++17 -ggdb -Wall -Wextra \ + -Wno-variadic-macros -Wno-parentheses -Wno-unused-function -Wno-unused \ -pthread \ ${RPC_LIB_DEFINES} ${CMAKE_CXX_FLAGS}") if (${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") - # make sure to match the compiler flags with which the Unreal - # Engine is built with - set(CMAKE_CXX_FLAGS "\ - -nostdinc++ -ferror-limit=10 -isystem ${LIBCXX_INC_PATH} \ - -D__CLANG__ ${CMAKE_CXX_FLAGS}") - - # removed -lsupc++ from below (Git issue # 678) - set(CMAKE_EXE_LINKER_FLAGS "\ - ${CMAKE_EXE_LINKER_FLAGS} -stdlib=libc++ -lc++ -lc++abi -lm -lc \ - -L ${LIBCXX_LIB_PATH} -rpath ${LIBCXX_LIB_PATH}") - - #do not use experimental as it might potentially cause ABI issues - #set(CXX_EXP_LIB "-lc++experimental") - - if("${BUILD_TYPE}" STREQUAL "debug") - # set same options that Unreal sets in debug builds - set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -funwind-tables -fdiagnostics-format=msvc -fno-inline -fno-omit-frame-pointer -fstack-protector") - endif() - + set(CMAKE_CXX_FLAGS "-stdlib=libc++ -Wno-documentation -Wno-unknown-warning-option ${CMAKE_CXX_FLAGS}") + set(CXX_EXP_LIB "-lc++fs -ferror-limit=10") else() set(CXX_EXP_LIB "-lstdc++fs -fmax-errors=10 -Wnoexcept -Wstrict-null-sentinel") endif () diff --git a/docker/Dockerfile_binary b/docker/Dockerfile_binary index 12f1d9fe8..7c633bfbf 100644 --- a/docker/Dockerfile_binary +++ b/docker/Dockerfile_binary @@ -1,4 +1,4 @@ -ARG BASE_IMAGE=nvidia/cudagl:10.0-devel-ubuntu16.04 +ARG BASE_IMAGE=nvidia/cudagl:10.0-devel-ubuntu18.04 FROM $BASE_IMAGE RUN apt-get update diff --git a/docker/build_airsim_image.py b/docker/build_airsim_image.py index d79c55714..e5130927d 100644 --- a/docker/build_airsim_image.py +++ b/docker/build_airsim_image.py @@ -22,7 +22,7 @@ def build_docker_image(args): else: dockerfile = 'Dockerfile_binary' if not args.base_image: - args.base_image = "nvidia/cudagl:10.0-devel-ubuntu16.04" + args.base_image = "nvidia/cudagl:10.0-devel-ubuntu18.04" target_image_tag = args.base_image.split(":")[1] # take tag from base image if not args.target_image: args.target_image = 'airsim_binary' + ':' + target_image_tag @@ -32,8 +32,8 @@ def build_docker_image(args): '-f', dockerfile, \ '--build-arg', 'BASE_IMAGE=' + args.base_image, \ '.'] - print " ".join(docker_command) + print(" ".join(docker_command)) subprocess.call(docker_command) if __name__=="__main__": - main() \ No newline at end of file + main() diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md new file mode 100644 index 000000000..7c5d55379 --- /dev/null +++ b/docs/CHANGELOG.md @@ -0,0 +1,157 @@ +# What's new + +Below is summarized list of important changes. This does not include minor/less important changes or bug fixes or documentation update. This list updated every few months. For complete detailed changes, please review [commit history](https://github.com/Microsoft/AirSim/commits/master). + +### April 2020 + +* [Fix issues with PX4 latest master branch](https://github.com/microsoft/AirSim/pull/2634) +* [Fix Lidar DrawDebugPoints causing crash](https://github.com/microsoft/AirSim/pull/2614) +* [Add docstrings for Python API](https://github.com/microsoft/AirSim/pull/2565) +* [Add missing noise, weather texture materials](https://github.com/microsoft/AirSim/pull/2625) +* [Update AirSim.uplugin version to 1.3.1](https://github.com/microsoft/AirSim/pull/2584) +* [Camera Roll angle control using Q,E keys in CV mode, manual camera](https://github.com/microsoft/AirSim/pull/2610) +* [Remove broken GCC build](https://github.com/microsoft/AirSim/pull/2572) +* New API - [`simSetTraceLine()`](https://github.com/microsoft/AirSim/pull/2506) +* [ROS package compilation fixes and updates](https://github.com/microsoft/AirSim/pull/2571) +* Latest release `v1.3.1` for [Windows](https://github.com/microsoft/AirSim/releases/tag/v1.3.1-windows) and [Linux](https://github.com/microsoft/AirSim/releases/tag/v1.3.1-linux) +* APIs added and fixed - [`simSetCameraFov`](https://github.com/microsoft/AirSim/pull/2534), [`rotateToYaw`](https://github.com/microsoft/AirSim/pull/2516) +* [airsim](https://pypi.org/project/airsim/) Python package update to `1.2.8` +* [NoDisplay ViewMode render state fix](https://github.com/microsoft/AirSim/pull/2518) + +### March 2020 + +* Latest release `v1.3.0` for [Windows](https://github.com/microsoft/AirSim/releases/tag/v1.3.0-Windows) and [Linux](https://github.com/microsoft/AirSim/releases/tag/v1.3.0-linux) +* Upgraded to Unreal Engine 4.24, Visual Studio 2019, Clang 8, C++ 17 standard +* Mac OSX Catalina support +* Updated [airsim](https://pypi.org/project/airsim/) Python package, with lots of new APIs +* [Removed legacy API wrappers](https://github.com/microsoft/AirSim/pull/2494) +* [Support for latest PX4 stable release](px4_setup.md) +* Support for [ArduPilot](https://ardupilot.org/ardupilot/) - [Copter, Rover vehicles](https://ardupilot.org/dev/docs/sitl-with-airsim.html) +* [Updated Unity support](Unity.md) +* [Removed simChar* APIs](https://github.com/microsoft/AirSim/pull/2493) +* [Plotting APIs for Debugging](https://github.com/microsoft/AirSim/pull/2304) +* [Low-level Multirotor APIs](https://github.com/microsoft/AirSim/pull/2297) +* [Updated Eigen version to 3.3.7](https://github.com/microsoft/AirSim/pull/2325) +* [Distance Sensor API fix](https://github.com/microsoft/AirSim/pull/2403) +* Add [`simSwapTextures`](retexturing.md) API +* Fix [`simContinueForTime`](https://github.com/microsoft/AirSim/pull/2299), [`simPause`](https://github.com/microsoft/AirSim/pull/2292) APIs +* [Lidar Sensor Trace Casting fix](https://github.com/microsoft/AirSim/pull/2143) +* [Fix rare `reset()` bug which causes Unreal crash](https://github.com/microsoft/AirSim/pull/2146) +* [Lidar sensor improvements, add `simGetLidarSegmentation` API](https://github.com/microsoft/AirSim/pull/2011) +* [Add RpcLibPort in settings](https://github.com/microsoft/AirSim/pull/2185) +* [Recording thread deadlock fix](https://github.com/microsoft/AirSim/pull/1695) +* [Prevent environment crash when Sun is not present](https://github.com/microsoft/AirSim/pull/2147) +* [Africa Tracking feautre, add `simListSceneObjects()` API, fix camera projection matrix](https://github.com/microsoft/AirSim/pull/1959) +* ROS wrapper for multirotors is available. See [airsim_ros_pkgs](airsim_ros_pkgs.md) for the ROS API, and [airsim_tutorial_pkgs](airsim_tutorial_pkgs.md) for tutorials. +* [Added sensor APIs for Barometer, IMU, GPS, Magnetometer, Distance Sensor](sensors.md) +* Added support for [docker in ubuntu](docker_ubuntu.md) + +### November, 2018 +* Added Weather Effects and [APIs](apis.md#weather-apis) +* Added [Time of Day API](apis.md#time-of-day-api) +* An experimental integration of [AirSim on Unity](https://github.com/Microsoft/AirSim/tree/master/Unity) is now available. Learn more in [Unity blog post](https://blogs.unity3d.com/2018/11/14/airsim-on-unity-experiment-with-autonomous-vehicle-simulation). +* [New environments](https://github.com/Microsoft/AirSim/releases/tag/v1.2.1): Forest, Plains (windmill farm), TalkingHeads (human head simulation), TrapCam (animal detection via camera) +* Highly efficient [NoDisplay view mode](https://github.com/Microsoft/AirSim/blob/master/docs/settings.md#viewmode) to turn off main screen rendering so you can capture images at high rate +* [Enable/disable sensors](https://github.com/Microsoft/AirSim/pull/1479) via settings +* [Lidar Sensor](lidar.md) +* [Support for Flysky FS-SM100 RC](https://github.com/Microsoft/AirSim/commit/474214364676b6631c01b3ed79d00c83ba5bccf5) USB adapter +* Case Study: [Formula Student Technion Driverless](https://github.com/Microsoft/AirSim/wiki/technion) +* [Multi-Vehicle Capability](multi_vehicle.md) +* [Custom speed units](https://github.com/Microsoft/AirSim/pull/1181) +* [ROS publisher](https://github.com/Microsoft/AirSim/pull/1135) +* [simSetObjectPose API](https://github.com/Microsoft/AirSim/pull/1161) +* [Character Control APIs](https://github.com/Microsoft/AirSim/blob/master/PythonClient/airsim/client.py#L137) (works on TalkingHeads binaries in release) +* [Arducopter Solo Support](https://github.com/Microsoft/AirSim/pull/1387) +* [Linux install without sudo access](https://github.com/Microsoft/AirSim/pull/1434) +* [Kinect like ROS publisher](https://github.com/Microsoft/AirSim/pull/1298) + + +### June, 2018 +* Development workflow doc +* Better Python 2 compatibility +* OSX setup fixes +* Almost complete rewrite of our APIs with new threading model, merging old APIs and creating few newer ones + +### April, 2018 +* Upgraded to Unreal Engine 4.18 and Visual Studio 2017 +* API framework refactoring to support world-level APIs +* Latest PX4 firmware now supported +* CarState with more information +* ThrustMaster wheel support +* pause and continueForTime APIs for drone as well as car +* Allow drone simulation run at higher clock rate without any degradation +* Forward-only mode fully functional for drone (do orbits while looking at center) +* Better PID tuning to reduce wobble for drones +* Ability to set arbitrary vehicle blueprint for drone as well as car +* gimbal stabilization via settings +* Ability to segment skinned and skeletal meshes by their name +* moveByAngleThrottle API +* Car physics tuning for better maneuverability +* Configure additional cameras via settings +* Time of day with geographically computed sun position +* Better car steering via keyboard +* Added MeshNamingMethod in segmentation setting +* gimbal API +* getCameraParameters API +* Ability turn off main rendering to save GPU resources +* Projection mode for capture settings +* getRCData, setRCData APIs +* Ability to turn off segmentation using negative IDs +* OSX build improvements +* Segmentation working for very large environments with initial IDs +* Better and extensible hash calculation for segmentation IDs +* Extensible PID controller for custom integration methods +* Sensor architecture now enables renderer specific features like ray casting +* Laser altimeter sensor + + +### Jan 2018 +* Config system rewrite, enable flexible config we are targeting in future +* Multi-Vehicle support Phase 1, core infrastructure changes +* MacOS support +* Infrared view +* 5 types of noise and interference for cameras +* WYSIWIG capture settings for cameras, preview recording settings in main view +* Azure support Phase 1, enable configurability of instances for headless mode +* Full kinematics APIs, ability to get pose, linear and angular velocities + accelerations via APIs +* Record multiple images from multiple cameras +* New segmentation APIs, ability to set configure object IDs, search via regex +* New object pose APIs, ability to get pose of objects (like animals) in environment +* Camera infrastructure enhancements, ability to add new image types like IR with just few lines +* Clock speed APIs for drone as well as car, simulation can be run with speed factor of 0 < x < infinity +* Support for Logitech G920 wheel +* Physics tuning of the car, Car doesn’t roll over, responds to steering with better curve, releasing gas paddle behavior more realistic +* Debugging APIs +* Stress tested to 24+ hours of continuous runs +* Support for Landscape and sky segmentation +* Manual navigation with accelerated controls in CV mode, user can explore environment much more easily +* Collison APIs +* Recording enhancements, log several new data points including ground truth, multiple images, controls state +* Planner and Perspective Depth views +* Disparity view +* New Image APIs supports float, png or numpy formats +* 6 config settings for image capture, ability to set auto-exposure, motion blur, gamma etc +* Full multi-camera support through out including sub-windows, recording, APIs etc +* Command line script to build all environments in one shot +* Remove submodules, use rpclib as download + +### Nov 2017 +* We now have the [car model](using_car.md). +* No need to build the code. Just download [binaries](https://github.com/Microsoft/AirSim/releases) and you are good to go! +* The [reinforcement learning example](reinforcement_learning.md) with AirSim +* New built-in flight controller called [simple_flight](simple_flight.md) that "just works" without any additional setup. It is also now *default*. +* AirSim now also generates [depth as well as disparity images](image_apis.md) that is in camera plan. +* We also have official Linux build now! + +## Sep 2017 +- We have added [car model](using_car.md)! + +## Aug 2017 +- [simple_flight](simple_flight.md) is now default flight controller for drones. If you want to use PX4, you will need to modify settings.json as per [PX4 setup doc](px4_setup.md). +- Linux build is official and currently uses Unreal 4.17 due to various bug fixes required +- ImageType enum has breaking changes with several new additions and clarifying existing ones +- SubWindows are now configurable from settings.json +- PythonClient is now complete and has parity with C++ APIs. Some of these would have breaking changes. + +## Feb 2017 +- First release! \ No newline at end of file diff --git a/docs/CONTRIBUTING.md b/docs/CONTRIBUTING.md new file mode 100644 index 000000000..66cbd63d4 --- /dev/null +++ b/docs/CONTRIBUTING.md @@ -0,0 +1,17 @@ +# Contributing + +## Quick Start +- Please read our [short and sweet coding guidelines](coding_guidelines.md). +- For big changes such as adding new feature or refactoring, [file an issue first](https://github.com/Microsoft/AirSim/issues). +- Use our [recommended development workflow](dev_workflow.md) to make changes and test it. +- Use [usual steps](https://akrabat.com/the-beginners-guide-to-contributing-to-a-github-project/) to make contributions just like other GitHub projects. If you are not familiar with Git Branch-Rebase-Merge workflow, please [read this first](http://shitalshah.com/p/git-workflow-branch-rebase-squash-merge/). + +## Checklist +- Use same style and formatting as rest of code even if it's not your preferred one. +- Change any documentation that goes with code changes. +- Do not include OS specific header files or new 3rd party dependencies. +- Keep your pull request small, ideally under 10 files. +- Make sure you don't include large binary files. +- When adding new includes, make dependency is absolutely necessary. +- Rebase your branch frequently with master (once every 2-3 days is ideal). +- Make sure your code would compile on Windows, Linux and OSX. diff --git a/docs/SUPPORT.md b/docs/SUPPORT.md new file mode 100644 index 000000000..ea1e7658b --- /dev/null +++ b/docs/SUPPORT.md @@ -0,0 +1,6 @@ +# Support + +We highly recommend to take a look at source code and contribute to the project. Due to large number of incoming feature request we may not be able to get to your request in your desired timeframe. So please [contribute](CONTRIBUTING.md) :). + +* [Join AirSim Facebook Group](https://www.facebook.com/groups/1225832467530667/) +* [File GitHub Issue](https://github.com/Microsoft/AirSim/issues) \ No newline at end of file diff --git a/docs/Unity.md b/docs/Unity.md new file mode 100644 index 000000000..05854de66 --- /dev/null +++ b/docs/Unity.md @@ -0,0 +1,125 @@ +# AirSim on Unity + +* AirSim on Unity allows you to run your simulators in the [Unity Engine](https://unity3d.com/). This project comes with some sample Unity projects and a wrapper around the AirLib library to run as a [native plugin](https://docs.unity3d.com/Manual/NativePlugins.html) in Unity. +* Included are two basic Unity Projects, one for a Car simulator and another for a Drone simulator. They are meant to be lightweight, and can be used to verify your setup is correct. +* Check out the [Unity blogpost](https://blogs.unity3d.com/2018/11/14/airsim-on-unity-experiment-with-autonomous-vehicle-simulation/) for overview on the release. + +### Warning: Experimental Release +This project is still in early development, expect some rough edges. We are working to fully support the full AirLib API and feature set, but some things may be missing. [Click here](unity_api_support.md) for the list of currently supported APIs. + +## Windows +### Building from source +#### Install Unity +* Download **Unity Hub** from [this page](https://unity3d.com/get-unity/download). +* Install **Unity 2019.3.12** using the Unity Hub from [here](https://unity3d.com/get-unity/update?_ga=2.150316848.720992218.1588269226-65412882.1588269226). [Detailed instructions here](https://docs.unity3d.com/Manual/GettingStartedInstallingHub.html). +* Note: If you are using Unity for the first time, check out [the Getting started guide](https://docs.unity3d.com/Manual/GettingStarted.html). The [Unity User Manual](https://docs.unity3d.com/Manual/UnityManual.html) has additional tips, resources, and FAQs. + +#### Build Airsim +* Install Visual Studio 2019. +**Make sure** to select **Desktop Development with C++** and **Windows 10 SDK 10.0.18362** (should be selected by default) while installing VS 2019. + +* Start `x64 Native Tools Command Prompt for VS 2019`. +* Clone the repo: `git clone https://github.com/Microsoft/AirSim.git`, and go the AirSim directory by `cd AirSim`. +* Run `build.cmd` from the command line. + +#### Build Unity Project +* Go inside the AirSim\Unity directory: `cd Unity`. +* Build the unity project: `build.cmd`. +* Additionally, there is a free environment `Windridge City` which you can download from [Unity Asset Store](https://assetstore.unity.com/packages/3d/environments/roadways/windridge-city-132222). And, of course, you can always create your own environment. + +## Linux +#### Dependencies +``` +sudo apt-get install libboost-all-dev +``` +#### Download and Install Unity for Linux +Warning: Unity Editor for Linux is still in Beta. Expect some rough edges. + +#### Install Unity +* Download **Unity Hub** from [this page](https://unity3d.com/get-unity/download). +* [Install **Unity 2019.3.12** using the Unity Hub](https://docs.unity3d.com/Manual/GettingStartedInstallingHub.html). +* Note: If you are using Unity for the first time, check out [the Getting started guide](https://docs.unity3d.com/Manual/GettingStarted.html). The [Unity User Manual](https://docs.unity3d.com/Manual/UnityManual.html) has additional tips, resources, and FAQs. + +#### Build Airsim +``` +git clone https://github.com/Microsoft/AirSim.git; +cd AirSim; +./setup.sh; +./build.sh +``` + +#### Generate AirsimWrapper Shared Library +``` +cd AirSim/Unity +./build.sh +``` + +This will generate the necessary shared library and copy it to the UnityDemo Plugins folder. + +## Usage +* Start Unity Hub, click on `Projects` on left pane, and then click on the `Add` button +* Select the folder `AirSim\Unity\UnityDemo`, and then hit the `OK` button. +* Click on the new project which showed up in the Unity Hub menu to open it in Unity. +* In the bottom pane, click on `Projects`->`Assets`->`Scenes`. Then, **Double-click** on `SimModeSelector`. This will load the SimModeSelector scene into the scene hierarchy pane. *DO NOT* add CarDemo or DroneDemo scene into the scene hierarchy pane. +* Hit the play button to start the simulation (and hit play again to stop the simulation. . +* Alternatively, you can change the SimMode in your `Settings.json` file. (You can read more about [`Settings.json` here](https://github.com/Microsoft/AirSim/blob/master/docs/settings.md)) +* Controlling the car: +Use `WASD` or the `Arrow keys` or the AirSim client. +* Controlling the drone: +Keyboard control is not currently available for drone flight. +* Changing camera views: +Keys `0`, `1`, `2`, `3` are used to toggle windows of different camera views. +* Recording simulation data: +Press *Record* button(Red button) located at the right bottom corner of the screen, to toggle recording of the simulation data. The recorded data can be found at `Documents\AirSim\(Date of recording)` on Windows and `~/Documents/AirSim/(Date of recording)` on Linux. +## Building Custom Environments For AirSim +To use environments other than `UnityDemo`, follow the instructions written [here](custom_unity_environments.md) +## Cross-Compiling to Linux +Unity Editor supports compiling projects to Linux systems. +After following the steps to build AirSim and Unity on Windows, do the following: + +#### Linux Pre-Requisites +Before being able to run Unity Binaries with the Airsim plugin, be sure have airsim and airsim unity built on your linux machine by following the Linux build steps above. + +### Package UnityDemo Binary On Windows + +#### Install Necessary Components +In order to package your project for linux, the **Linux Build Support** Unity add-on must be installed. +* Open **Unity Hub**, and click the **Add component** button in the dropdown window under **more options** to the right of your **Unity 2018.2.15f1** tab. +![Adding components](images/unity_linux_components_1.PNG) +* Make sure the **Linux Build Support** Platform is selected +![Adding components](images/unity_linux_components_2.PNG) +Once this component is successfully installed, you are ready to build Unity Projects for Linux! + +#### Build the Project +* On your Windows machine, build the Unity Demo by navigating to the build settings option in the toolbar ```File -> Build Settings``` +* Make sure the following scenes are set to be built: + 1. SimModeSelector + 2. CarDemo + 3. DroneDemo +* Set the target operating system to linux, and choose the version appropriate for your system (x86 vs x86_64) +* Click ```Build``` +* Transport the built project as well as the generated folder ```"{project_name}_Data"``` to your linux machine + +#### Copy The AirsimWrapper Library to the Project Plugins folder +* On your linux machine, navigate to your AirSim repository, and run the following commands in a terminal window: + ``` + cp Unity/linux-build/libAirsimWrapper.so path/to/your/project/{project_name}_Data/Plugins/{os_version} + ``` +This will generate the necessary shared library to allow Airsim to communicate with Unity and copy it to the plugins folder of your project binary. + +#### Run the Project Binary +* Open a terminal and navigate to your project directory +* Set your project binary as an executable file: +``` +chmod +x "{project_name}.{configuration}" +``` +* Run the binary file +``` +./{project_name}.{configuration} +``` +### Using Airsim API +* For quickstart with the Python APIs for the car or the drone, simply run the [`hello_car.py`](https://github.com/Microsoft/AirSim/blob/master/PythonClient/car/hello_car.py) or the [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/master/PythonClient/multirotor/hello_drone.py) script accordingly. +* Details of the AirSim C++ and Python APIs are [here](https://github.com/Microsoft/AirSim/blob/master/docs/apis.md). + +### Acknowledgements +* The drone object was provided by user 31415926 on [sketchfab](https://sketchfab.com/models/055841df0fb24cd4abde06a91f7d360a). It is licensed under the [CC License](https://creativecommons.org/licenses/by/4.0/). diff --git a/docs/airsim_ros_pkgs.md b/docs/airsim_ros_pkgs.md new file mode 100644 index 000000000..160b51825 --- /dev/null +++ b/docs/airsim_ros_pkgs.md @@ -0,0 +1,214 @@ +# airsim_ros_pkgs + +A ROS wrapper over the AirSim C++ client library. + +## Setup +- Install gcc >= 8.0.0 +`sudo apt-get install gcc-8 g++-8` +Verify version by `gcc --version` + +- Ubuntu 16.04 + * Install [ROS kinetic](https://wiki.ros.org/kinetic/Installation/Ubuntu) + * Install tf2 sensor and mavros packages: `sudo apt-get install ros-kinetic-tf2-sensor-msgs ros-kinetic-mavros*` + +- Ubuntu 18.04 + * Install [ROS melodic](https://wiki.ros.org/melodic/Installation/Ubuntu) + * Install tf2 sensor and mavros packages: `sudo apt-get install ros-melodic-tf2-sensor-msgs ros-melodic-mavros*` + +- Install [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/installing.html) + `sudo apt-get install python-catkin-tools` or + `pip install catkin_tools` + +## Build +- Build AirSim +``` +git clone https://github.com/Microsoft/AirSim.git; +cd AirSim; +./setup.sh; +./build.sh; +``` +- Build ROS package + +``` +cd ros; +catkin build; # or catkin_make +``` + +If your default GCC isn't 8 or greater (check using `gcc --version`), then compilation will fail. In that case, use `gcc-8` explicitly as follows- + +``` +catkin build -DCMAKE_C_COMPILER=gcc-8 -DCMAKE_CXX_COMPILER=g++-8 +``` + +## Running +``` +source devel/setup.bash; +roslaunch airsim_ros_pkgs airsim_node.launch; +roslaunch airsim_ros_pkgs rviz.launch; +``` + +# Using AirSim ROS wrapper +The ROS wrapper is composed of two ROS nodes - the first is a wrapper over AirSim's multirotor C++ client library, and the second is a simple PD position controller. +Let's look at the ROS API for both nodes: + +### AirSim ROS Wrapper Node +#### Publishers: +- `/airsim_node/origin_geo_point` [airsim_ros_pkgs/GPSYaw](https://github.com/microsoft/AirSim/tree/master/ros/src/airsim_ros_pkgs/msg/GPSYaw.msg) +GPS coordinates corresponding to global NED frame. This is set in the airsim's [settings.json](https://microsoft.github.io/AirSim/docs/settings/) file under the `OriginGeopoint` key. + +- `/airsim_node/VEHICLE_NAME/global_gps` [sensor_msgs/NavSatFix](https://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html) +This the current GPS coordinates of the drone in airsim. + +- `/airsim_node/VEHICLE_NAME/odom_local_ned` [nav_msgs/Odometry](https://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) +Odometry in NED frame (default name: odom_local_ned, launch name and frame type are configurable) wrt take-off point. + +- `/airsim_node/VEHICLE_NAME/CAMERA_NAME/IMAGE_TYPE/camera_info` [sensor_msgs/CameraInfo](https://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html) + +- `/airsim_node/VEHICLE_NAME/CAMERA_NAME/IMAGE_TYPE` [sensor_msgs/Image](https://docs.ros.org/api/sensor_msgs/html/msg/Image.html) + RGB or float image depending on image type requested in settings.json. + +- `/tf` [tf2_msgs/TFMessage](https://docs.ros.org/api/tf2_msgs/html/msg/TFMessage.html) + +- `/airsim_node/VEHICLE_NAME/altimeter/SENSOR_NAME` [airsim_ros_pkgs::Altimeter] This the current altimeter reading for altitude, pressure, and QNH (https://en.wikipedia.org/wiki/QNH) + +- `/airsim_node/VEHICLE_NAME/imu/SENSOR_NAME` [sensor_msgs::Imu] (http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html) + IMU sensor data + +- `/airsim_node/VEHICLE_NAME/magnetometer/SENSOR_NAME` [sensor_msgs::MagneticField] (http://docs.ros.org/api/sensor_msgs/html/msg/MagneticField.html) + Meausrement of magnetic field vector/compass + +- `/airsim_node/VEHICLE_NAME/distance/SENSOR_NAME` [sensor_msgs::Range] (http://docs.ros.org/api/sensor_msgs/html/msg/Range.html) + Meausrement of distance from an active ranger, such as infrared or IR + +- `/airsim_node/VEHICLE_NAME/lidar/SENSOR_NAME` [sensor_msgs::PointCloud2] (http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html) + LIDAR pointcloud + +#### Subscribers: +- `/airsim_node/vel_cmd_body_frame` [airsim_ros_pkgs/VelCmd](https://github.com/microsoft/AirSim/tree/master/ros/src/airsim_ros_pkgs/msg/VelCmd.msg) + Ignore `vehicle_name` field, leave it to blank. We will use `vehicle_name` in future for multiple drones. + +- `/airsim_node/vel_cmd_world_frame` [airsim_ros_pkgs/VelCmd](https://github.com/microsoft/AirSim/tree/master/ros/src/airsim_ros_pkgs/msg/VelCmd.msg) + Ignore `vehicle_name` field, leave it to blank. We will use `vehicle_name` in future for multiple drones. + +- `/gimbal_angle_euler_cmd` [airsim_ros_pkgs/GimbalAngleEulerCmd](https://github.com/microsoft/AirSim/tree/master/ros/src/airsim_ros_pkgs/msg/GimbalAngleEulerCmd.msg) + Gimbal set point in euler angles. + +- `/gimbal_angle_quat_cmd` [airsim_ros_pkgs/GimbalAngleQuatCmd](https://github.com/microsoft/AirSim/tree/master/ros/src/airsim_ros_pkgs/msg/GimbalAngleQuatCmd.msg) + Gimbal set point in quaternion. + +- `/airsim_node/VEHICLE_NAME/car_cmd` [airsim_ros_pkgs/CarControls] + Throttle, brake, steering and gear selections for control. Both automatic and manual transmission control possible, see the car_joy.py script for use. + +#### Services: +- `/airsim_node/VEHICLE_NAME/land` [airsim_ros_pkgs/Takeoff](https://docs.ros.org/api/std_srvs/html/srv/Empty.html) + +- `/airsim_node/takeoff` [airsim_ros_pkgs/Takeoff](https://docs.ros.org/api/std_srvs/html/srv/Empty.html) + +- `/airsim_node/reset` [airsim_ros_pkgs/Reset](https://docs.ros.org/api/std_srvs/html/srv/Empty.html) + Resets *all* drones + +#### Parameters: +- `/airsim_node/world_frame_id` [string] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: world_ned + Set to "world_enu" to switch to ENU frames automatically + +- `/airsim_node/odom_frame_id` [string] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: odom_local_ned + If you set world_frame_id to "world_enu", the default odom name will instead default to "odom_local_enu" + +- `/airsim_node/coordinate_system_enu` [boolean] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: false + If you set world_frame_id to "world_enu", this setting will instead default to true + +- `/airsim_node/update_airsim_control_every_n_sec` [double] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: 0.01 seconds. + Timer callback frequency for updating drone odom and state from airsim, and sending in control commands. + The current RPClib interface to unreal engine maxes out at 50 Hz. + Timer callbacks in ROS run at maximum rate possible, so it's best to not touch this parameter. + +- `/airsim_node/update_airsim_img_response_every_n_sec` [double] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: 0.01 seconds. + Timer callback frequency for receiving images from all cameras in airsim. + The speed will depend on number of images requested and their resolution. + Timer callbacks in ROS run at maximum rate possible, so it's best to not touch this parameter. + +- `/airsim_node/publish_clock` [double] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: false + Will publish the ros /clock topic if set to true. + +### Simple PID Position Controller Node + +#### Parameters: +- PD controller parameters: + * `/pd_position_node/kd_x` [double], + `/pd_position_node/kp_y` [double], + `/pd_position_node/kp_z` [double], + `/pd_position_node/kp_yaw` [double] + Proportional gains + + * `/pd_position_node/kd_x` [double], + `/pd_position_node/kd_y` [double], + `/pd_position_node/kd_z` [double], + `/pd_position_node/kd_yaw` [double] + Derivative gains + + * `/pd_position_node/reached_thresh_xyz` [double] + Threshold euler distance (meters) from current position to setpoint position + + * `/pd_position_node/reached_yaw_degrees` [double] + Threshold yaw distance (degrees) from current position to setpoint position + +- `/pd_position_node/update_control_every_n_sec` [double] + Default: 0.01 seconds + +#### Services: +- `/airsim_node/VEHICLE_NAME/gps_goal` [Request: [srv/SetGPSPosition](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_ros_pkgs/srv/SetGPSPosition.srv)] + Target gps position + yaw. + In **absolute** altitude. + +- `/airsim_node/VEHICLE_NAME/local_position_goal` [Request: [srv/SetLocalPosition](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_ros_pkgs/srv/SetLocalPosition.srv) + Target local position + yaw in global NED frame. + +#### Subscribers: +- `/airsim_node/origin_geo_point` [airsim_ros_pkgs/GPSYaw](https://github.com/microsoft/AirSim/tree/master/ros/src/airsim_ros_pkgs/msg/GPSYaw.msg) + Listens to home geo coordinates published by `airsim_node`. + +- `/airsim_node/VEHICLE_NAME/odom_local_ned` [nav_msgs/Odometry](https://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) + Listens to odometry published by `airsim_node` + +#### Publishers: +- `/vel_cmd_world_frame` [airsim_ros_pkgs/VelCmd](airsim_ros_pkgs/VelCmd) + Sends velocity command to `airsim_node` + +### Global params +- Dynamic constraints. These can be changed in `dynamic_constraints.launch`: + * `/max_vel_horz_abs` [double] + Maximum horizontal velocity of the drone (meters/second) + + * `/max_vel_vert_abs` [double] + Maximum vertical velocity of the drone (meters/second) + + * `/max_yaw_rate_degree` [double] + Maximum yaw rate (degrees/second) + +### Misc +#### Windows Subsytem for Linux on Windows 10 +- WSL setup: + * Get [Windows Subsystem for Linux](https://docs.microsoft.com/en-us/windows/wsl/install-win10) + * Get [Ubuntu 16.04](https://www.microsoft.com/en-us/p/ubuntu-1604-lts/9pjn388hp8c9?activetab=pivot:overviewtab) or [Ubuntu 18.04](https://www.microsoft.com/en-us/p/ubuntu-1804-lts/9n9tngvndl3q?activetab=pivot%3Aoverviewtab) + * Go to Ubuntu 16 / 18 instructions! + + +- Setup for X apps (like RViz, rqt_image_view, terminator) in Windows + WSL + * Install [Xming X Server](https://sourceforge.net/projects/xming/). + * Find and run `XLaunch` from the Windows start menu. + Select `Multiple Windows` in first popup, `Start no client` in second popup, **only** `Clipboard` in third popup. Do **not** select `Native Opengl`. + * Open Ubuntu 16.04 / 18.04 session by typing `Ubuntu 16.04` / `Ubuntu 18.04` in Windows start menu. + * Recommended: Install [terminator](http://www.ubuntugeek.com/terminator-multiple-gnome-terminals-in-one-window.html) : `$ sudo apt-get install terminator.` + - You can open terminator in a new window by entering `$ DISPLAY=:0 terminator -u`. diff --git a/docs/airsim_tutorial_pkgs.md b/docs/airsim_tutorial_pkgs.md new file mode 100644 index 000000000..cf818d8ac --- /dev/null +++ b/docs/airsim_tutorial_pkgs.md @@ -0,0 +1,72 @@ +# AirSim ROS Tutorials + +This is a set of sample AirSim `settings.json`s, roslaunch and rviz files to give a starting point for using AirSim with ROS. +See [airsim_ros_pkgs](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_ros_pkgs/README.md) for the ROS API. + + +## Setup + +Make sure that the [airsim_ros_pkgs Setup](airsim_ros_pkgs.md) has been completed and the prerequisites installed. + +```shell +$ cd PATH_TO/AirSim/ros +$ catkin build airsim_tutorial_pkgs +``` + +If your default GCC isn't 8 or greater (check using `gcc --version`), then compilation will fail. In that case, use `gcc-8` explicitly as follows- + +```shell +catkin build airsim_tutorial_pkgs -DCMAKE_C_COMPILER=gcc-8 -DCMAKE_CXX_COMPILER=g++-8 +``` + +Note: For running examples, and also whenever a new terminal is opened, sourcing the `setup.bash` file is necessary. If you're using the ROS wrapper frequently, it might be helpful to add the `source PATH_TO/AirSim/ros/devel/setup.bash` to your `~/.profile` or `~/.bashrc` to avoid the need to run the command every time a new terminal is opened + +## Examples + +### Single drone with monocular and depth cameras, and lidar + - Settings.json - [front_stereo_and_center_mono.json](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_tutorial_pkgs/settings/front_stereo_and_center_mono.json) + ```shell + $ source PATH_TO/AirSim/ros/devel/setup.bash + $ roscd airsim_tutorial_pkgs + $ cp settings/front_stereo_and_center_mono.json ~/Documents/AirSim/settings.json + + ## Start your unreal package or binary here + + $ roslaunch airsim_ros_pkgs airsim_node.launch; + + # in a new pane / terminal + $ source PATH_TO/AirSim/ros/devel/setup.bash + $ roslaunch airsim_tutorial_pkgs front_stereo_and_center_mono.launch + ``` + The above would start rviz with tf's, registered RGBD cloud using [depth_image_proc](https://wiki.ros.org/depth_image_proc) using the [`depth_to_pointcloud` launch file](https://github.com/microsoft/AirSim/master/ros/src/airsim_tutorial_pkgs/launch/front_stereo_and_center_mono/depth_to_pointcloud.launch), and the lidar point cloud. + + +### Two drones, with cameras, lidar, IMU each +- Settings.json - [two_drones_camera_lidar_imu.json](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_tutorial_pkgs/settings/two_drones_camera_lidar_imu.json) + + ```shell + $ source PATH_TO/AirSim/ros/devel/setup.bash + $ roscd airsim_tutorial_pkgs + $ cp settings/two_drones_camera_lidar_imu.json ~/Documents/AirSim/settings.json + + ## Start your unreal package or binary here + + $ roslaunch airsim_ros_pkgs airsim_node.launch; + $ roslaunch airsim_ros_pkgs rviz.launch + ``` +You can view the tfs in rviz. And do a `rostopic list` and `rosservice list` to inspect the services avaiable. + +### Twenty-five drones in a square pattern +- Settings.json - [twenty_five_drones.json](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_tutorial_pkgs/settings/twenty_five_drones.json) + + ```shell + $ source PATH_TO/AirSim/ros/devel/setup.bash + $ roscd airsim_tutorial_pkgs + $ cp settings/twenty_five_drones.json ~/Documents/AirSim/settings.json + + ## Start your unreal package or binary here + + $ roslaunch airsim_ros_pkgs airsim_node.launch; + $ roslaunch airsim_ros_pkgs rviz.launch + ``` +You can view the tfs in rviz. And do a `rostopic list` and `rosservice list` to inspect the services avaiable. diff --git a/docs/apis.md b/docs/apis.md index 866ee3637..a8807f74b 100644 --- a/docs/apis.md +++ b/docs/apis.md @@ -1,10 +1,10 @@ # AirSim APIs ## Introduction -AirSim exposes APIs so you can interact with vehicle in the simulation programmatically. You can use these APIs to retrieve images, get state, control the vehicle and so on. +AirSim exposes APIs so you can interact with vehicle in the simulation programmatically. You can use these APIs to retrieve images, get state, control the vehicle and so on. ## Python Quickstart -If you want to use Python to call AirSim APIs, we recommend using Anaconda with Python 3.5 or later versions however some code may also work with Python 2.7 ([help us](../CONTRIBUTING.md) improve compatibility!). +If you want to use Python to call AirSim APIs, we recommend using Anaconda with Python 3.5 or later versions however some code may also work with Python 2.7 ([help us](CONTRIBUTING.md) improve compatibility!). First install this package: @@ -18,7 +18,7 @@ You can either get AirSim binaries from [releases](https://github.com/Microsoft/ python hello_car.py ``` -If you are using Visual Studio 2017 then just open AirSim.sln, set PythonClient as startup project and choose `car\hello_car.py` as your startup script. +If you are using Visual Studio 2019 then just open AirSim.sln, set PythonClient as startup project and choose `car\hello_car.py` as your startup script. ### Installing AirSim Package You can also install `airsim` package simply by, @@ -27,7 +27,7 @@ You can also install `airsim` package simply by, pip install airsim ``` -You can find source code and samples for this package in `PythonClient` folder in your repo. +You can find source code and samples for this package in `PythonClient` folder in your repo. **Notes** 1. You may notice a file `setup_path.py` in our example folders. This file has simple code to detect if `airsim` package is available in parent folder and in that case we use that instead of pip installed package so you always use latest code. @@ -44,7 +44,7 @@ Here's how to use AirSim APIs using Python to control simulated car (see also [C import airsim import time -# connect to the AirSim simulator +# connect to the AirSim simulator client = airsim.CarClient() client.confirmConnection() client.enableApiControl(True) @@ -66,7 +66,7 @@ while True: # get camera images from the car responses = client.simGetImages([ airsim.ImageRequest(0, airsim.ImageType.DepthVis), - airsim.ImageRequest(1, airsim.ImageType.DepthPlanner, True)]) + airsim.ImageRequest(1, airsim.ImageType.DepthPlanner, True)]) print('Retrieved images: %d', len(responses)) # do something with images @@ -87,7 +87,7 @@ Here's how to use AirSim APIs using Python to control simulated quadrotor (see a # ready to run example: PythonClient/multirotor/hello_drone.py import airsim -# connect to the AirSim simulator +# connect to the AirSim simulator client = airsim.MultirotorClient() client.confirmConnection() client.enableApiControl(True) @@ -99,7 +99,7 @@ client.moveToPositionAsync(-10, 10, -10, 5).join() # take images responses = client.simGetImages([ - airsim.ImageRequest("0", airsim.ImageType.DepthVis), + airsim.ImageRequest("0", airsim.ImageType.DepthVis), airsim.ImageRequest("1", airsim.ImageType.DepthPlanner, True)]) print('Retrieved images: %d', len(responses)) @@ -127,6 +127,7 @@ for response in responses: AirSim offers comprehensive images APIs to retrieve synchronized images from multiple cameras along with ground truth including depth, disparity, surface normals and vision. You can set the resolution, FOV, motion blur etc parameters in [settings.json](settings.md). There is also API for detecting collision state. See also [complete code](https://github.com/Microsoft/AirSim/tree/master/Examples/DataCollection/StereoImageGenerator.hpp) that generates specified number of stereo images and ground truth depth with normalization to camera plan, computation of disparity image and saving it to [pfm format](pfm.md). More on [image APIs and Computer Vision mode](image_apis.md). +For vision problems that can benefit from domain randomization, there is also an [object retexturing API](retexturing.md), which can be used in supported scenes. ### Pause and Continue APIs AirSim allows to pause and continue the simulation through `pause(is_paused)` API. To pause the simulation call `pause(True)` and to continue the simulation call `pause(False)`. You may have scenario, especially while using reinforcement learning, to run the simulation for specified amount of time and then automatically pause. While simulation is paused, you may then do some expensive computation, send a new command and then again run the simulation for specified amount of time. This can be achieved by API `continueForTime(seconds)`. This API runs the simulation for the specified number of seconds and then pauses the simulation. For example usage, please see [pause_continue_car.py](https://github.com/Microsoft/AirSim/tree/master/PythonClient//car/pause_continue_car.py) and [pause_continue_drone.py](https://github.com/Microsoft/AirSim/tree/master/PythonClient//multirotor/pause_continue_drone.py). @@ -136,7 +137,7 @@ AirSim allows to pause and continue the simulation through `pause(is_paused)` AP The collision information can be obtained using `simGetCollisionInfo` API. This call returns a struct that has information not only whether collision occurred but also collision position, surface normal, penetration depth and so on. ### Time of Day API -AirSim assumes there exist sky sphere of class `EngineSky/BP_Sky_Sphere` in your environment with [ADirectionalLight actor](https://github.com/Microsoft/AirSim/blob/master/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp#L156). By default, the position of the sun in the scene doesn't move with time. You can use [settings](settings.md#timeofday) to set up latitude, longitude, date and time which AirSim uses to compute the position of sun in the scene. +AirSim assumes there exist sky sphere of class `EngineSky/BP_Sky_Sphere` in your environment with [ADirectionalLight actor](https://github.com/Microsoft/AirSim/blob/master/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp#L156). By default, the position of the sun in the scene doesn't move with time. You can use [settings](settings.md#timeofday) to set up latitude, longitude, date and time which AirSim uses to compute the position of sun in the scene. You can also use following API call to set the sun position according to given date time: @@ -178,8 +179,22 @@ Please note that `Roadwetness`, `RoadSnow` and `RoadLeaf` effects requires addin Please see [example code](https://github.com/Microsoft/AirSim/blob/master/PythonClient/computer_vision/weather.py) for more details. +### Recording APIs + +Recording APIs can be used to start recording data through APIs. Data to be recorded can be specified using [settings](settings.md#recording). To start recording, use - + +``` +client.startRecording() +``` + +Similarly, to stop recording, use `client.stopRecording()`. To check whether Recording is running, call `client.isRecording()`, returns a `bool`. + +This API works alongwith toggling Recording using R button, therefore if it's enabled using R key, `isRecording()` will return `True`, and recording can be stopped via API using `stopRecording()`. Similarly, recording started using API will be stopped if R key is pressed in Viewport. LogMessage will also appear in the top-left of the viewport if recording is started or stopped using API. + +Note that this will only save the data as specfied in the settings. For full freedom in storing data such as certain sensor information, or in a different format or layout, use the other APIs to fetch the data and save as desired. + ### Lidar APIs -AirSim offers API to retrieve point cloud data from Lidar sensors on vehicles. You can set the number of channels, points per second, horizontal and vertical FOV, etc parameters in [settings.json](settings.md). +AirSim offers API to retrieve point cloud data from Lidar sensors on vehicles. You can set the number of channels, points per second, horizontal and vertical FOV, etc parameters in [settings.json](settings.md). More on [lidar APIs and settings](lidar.md) and [sensor settings](sensors.md) @@ -224,17 +239,17 @@ All *Async* method returns `concurrent.futures.Future` in Python (`std::future` There are two modes you can fly vehicle: `drivetrain` parameter is set to `airsim.DrivetrainType.ForwardOnly` or `airsim.DrivetrainType.MaxDegreeOfFreedom`. When you specify ForwardOnly, you are saying that vehicle's front should always point in the direction of travel. So if you want drone to take left turn then it would first rotate so front points to left. This mode is useful when you have only front camera and you are operating vehicle using FPV view. This is more or less like travelling in car where you always have front view. The MaxDegreeOfFreedom means you don't care where the front points to. So when you take left turn, you just start going left like crab. Quadrotors can go in any direction regardless of where front points to. The MaxDegreeOfFreedom enables this mode. #### yaw_mode -`yaw_mode` is a struct `YawMode` with two fields, `yaw_or_rate` and `is_rate`. If `is_rate` field is True then `yaw_or_rate` field is interpreted as angular velocity in degrees/sec which means you want vehicle to rotate continuously around its axis at that angular velocity while moving. If `is_rate` is False then `yaw_or_rate` is interpreted as angle in degrees which means you want vehicle to rotate to specific angle (i.e. yaw) and keep that angle while moving. +`yaw_mode` is a struct `YawMode` with two fields, `yaw_or_rate` and `is_rate`. If `is_rate` field is True then `yaw_or_rate` field is interpreted as angular velocity in degrees/sec which means you want vehicle to rotate continuously around its axis at that angular velocity while moving. If `is_rate` is False then `yaw_or_rate` is interpreted as angle in degrees which means you want vehicle to rotate to specific angle (i.e. yaw) and keep that angle while moving. You can probably see that when `yaw_mode.is_rate == true`, the `drivetrain` parameter shouldn't be set to `ForwardOnly` because you are contradicting by saying that keep front pointing ahead but also rotate continuously. However if you have `yaw_mode.is_rate = false` in `ForwardOnly` mode then you can do some funky stuff. For example, you can have drone do circles and have yaw_or_rate set to 90 so camera is always pointed to center ("super cool selfie mode"). In `MaxDegreeofFreedom` also you can get some funky stuff by setting `yaw_mode.is_rate = true` and say `yaw_mode.yaw_or_rate = 20`. This will cause drone to go in its path while rotating which may allow to do 360 scanning. -In most cases, you just don't want yaw to change which you can do by setting yaw rate of 0. The shorthand for this is `airsim.YawMode.Zero()` (or in C++: `YawMode::Zero()`). +In most cases, you just don't want yaw to change which you can do by setting yaw rate of 0. The shorthand for this is `airsim.YawMode.Zero()` (or in C++: `YawMode::Zero()`). #### lookahead and adaptive_lookahead When you ask vehicle to follow a path, AirSim uses "carrot following" algorithm. This algorithm operates by looking ahead on path and adjusting its velocity vector. The parameters for this algorithm is specified by `lookahead` and `adaptive_lookahead`. For most of the time you want algorithm to auto-decide the values by simply setting `lookahead = -1` and `adaptive_lookahead = 0`. ## Using APIs on Real Vehicles -We want to be able to run *same code* that runs in simulation as on real vehicle. This allows you to test your code in simulator and deploy to real vehicle. +We want to be able to run *same code* that runs in simulation as on real vehicle. This allows you to test your code in simulator and deploy to real vehicle. Generally speaking, APIs therefore shouldn't allow you to do something that cannot be done on real vehicle (for example, getting the ground truth). But, of course, simulator has much more information and it would be useful in applications that may not care about running things on real vehicle. For this reason, we clearly delineate between sim-only APIs by attaching `sim` prefix, for example, `simGetGroundTruthKinematics`. This way you can avoid using these simulation-only APIs if you care about running your code on real vehicles. @@ -263,7 +278,7 @@ The APIs use [msgpack-rpc protocol](https://github.com/msgpack-rpc/msgpack-rpc) If you see Unreal getting slowed down dramatically when Unreal Engine window loses focus then go to 'Edit->Editor Preferences' in Unreal Editor, in the 'Search' box type 'CPU' and ensure that the 'Use Less CPU when in Background' is unchecked. #### Do I need anything else on Windows? -You should install VS2017 with VC++, Windows SDK 8.1 and Python. To use Python APIs you will need Python 3.5 or later (install it using Anaconda). +You should install VS2019 with VC++, Windows SDK 10.0 and Python. To use Python APIs you will need Python 3.5 or later (install it using Anaconda). #### Which version of Python should I use? We recommend [Anaconda](https://www.anaconda.com/download/) to get Python tools and libraries. Our code is tested with Python 3.5.3 :: Anaconda 4.4.0. This is important because older version have been known to have [problems](https://stackoverflow.com/a/45934992/207661). @@ -275,3 +290,7 @@ conda install opencv pip install opencv-python ``` +#### TypeError: unsupported operand type(s) for *: 'AsyncIOLoop' and 'float' + +This error happens if you install Jupyter, which somehow breaks the msgpackrpc library. Create a new python environment +which the minimal required packages. \ No newline at end of file diff --git a/docs/apis_cpp.md b/docs/apis_cpp.md index 3a01d5444..d8399a3e7 100644 --- a/docs/apis_cpp.md +++ b/docs/apis_cpp.md @@ -3,7 +3,7 @@ Please read [general API doc](apis.md) first if you haven't already. This document describes C++ examples and other C++ specific details. ## Quick Start -Fastest way to get started is to open AirSim.sln in Visual Studio 2017. You will see [Hello Car](https://github.com/Microsoft/AirSim/tree/master/HelloCar/) and [Hello Drone](https://github.com/Microsoft/AirSim/tree/master/HelloDrone/) examples in the solution. These examples will show you the include paths and lib paths you will need to setup in your VC++ projects. If you are using Linux then you will specify these paths either in your [cmake file](https://github.com/Microsoft/AirSim/tree/master/cmake//HelloCar/CMakeLists.txt) or on compiler command line. +Fastest way to get started is to open AirSim.sln in Visual Studio 2019. You will see [Hello Car](https://github.com/Microsoft/AirSim/tree/master/HelloCar/) and [Hello Drone](https://github.com/Microsoft/AirSim/tree/master/HelloDrone/) examples in the solution. These examples will show you the include paths and lib paths you will need to setup in your VC++ projects. If you are using Linux then you will specify these paths either in your [cmake file](https://github.com/Microsoft/AirSim/tree/master/cmake//HelloCar/CMakeLists.txt) or on compiler command line. #### Include and Lib Folders * Include folders: `$(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include` @@ -12,7 +12,7 @@ Fastest way to get started is to open AirSim.sln in Visual Studio 2017. You will ## Hello Car -Here's how to use AirSim APIs using Python to control simulated car (see also [Python example](apis.md#hello_car)): +Here's how to use AirSim APIs using C++ to control simulated car (see also [Python example](apis.md#hello_car)): ```cpp @@ -50,7 +50,7 @@ int main() ## Hello Drone -Here's how to use AirSim APIs using Python to control simulated car (see also [Python example](apis.md#hello_drone)): +Here's how to use AirSim APIs using C++ to control simulated quadrotor (see also [Python example](apis.md#hello_drone)): ```cpp diff --git a/docs/azure.md b/docs/azure.md new file mode 100644 index 000000000..4d4f0f75d --- /dev/null +++ b/docs/azure.md @@ -0,0 +1,84 @@ +# AirSim Development Environment on Azure + +This document explains how to automate the creation of a development environment on Azure and code and debug a Python application connected to AirSim using Visual Studio Code + +## Automatically Deploy Your Azure VM +Use [this](https://github.com/microsoft/AirSim/blob/master/azure/azure-env-creation/vm-arm-template.json) template to create, deploy and configure an Azure VM to work with AirSim + +*Note: the VM deployment and configuration process may take 20+ minutes to complete* + + + + + +### Regarding the deployment of the Azure VM +- When using an Azure Trial account, the default vCPU quota may not be enough to allocate the required VM to run AirSim. If that's the case, you will see an error when trying to create the VM and will have to make a support request to increase the default quota. + +- To avoid charges for the Virtual Machine usage while not in use, remember to deallocate its resources from the [Azure Portal](https://portal.azure.com) or use the following command from the Azure CLI: + ```bash + az vm deallocate --resource-group MyResourceGroup --name MyVMName + ``` + +## Code and debug from Visual Studio Code and Remote SSH +- Install Visual Studio Code +- Install the *Remote - SSH* extension +- Press `F1` and run the `Remote - SSH: Connect to host...` command +- Add the recently create VM details. For instance, `AzureUser@11.22.33.44` +- Run the `Remote - SSH: Connect to host...` command again, and now select the newly added connection. +- Once connected, click on the `Clone Repository` button in Visual Studio Code, and either clone this repository in the remote VM and open *just the `azure` folder*, or create a brand new repository, clone it and copy the contents of the `azure` folder from this repository in it. It is important to open that directory so Visual Studio Code can use the specific `.vscode` directory for the scenario and not the general AirSim `.vscode` directory. It contains the recommended extensions to install, the task to start AirSim remotely and the launch configuration for the Python application. +- Install all the recommended extensions +- Press `F1` and select the `Tasks: Run Task` option. Then, select the `Start AirSim` task from Visual Studio Code to execute the `start-airsim.ps1` script from Visual Studio Code. +- Open the `multirotor.py` file inside the `app` directory +- Start debugging with Python +- When finished, remember to stop an deallocate the Azure VM to avoid extra charges + +## Code and debug from a local Visual Studio Code and connect to AirSim via forwarded ports + +*Note: this scenario, will be using two Visual Studio Code instances. +The first one will be used as a bridge to forward ports via SSH to the Azure VM and execute remote processes, and the second one will +be used for local Python development. +To be able to reach the VM from the local Python code, it is required to keep the `Remote - SSH` instance of Visual Studio Code opened, while working with the local Python environment on the second instance* + +- Open the first Visual Studio Code instance +- Follow the steps in the previous section to connect via `Remote - SSH` +- In the *Remote Explorer*, add the port `41451` as a forwarded port to localhost +- Either run the `Start AirSim` task on the Visual Studio Code with the remote session as explained in the previous scenario or manually start the AirSim binary in the VM +- Open a second Visual Studio Code instance, without disconnecting or closing the first one +- Either clone this repository locally and open *just the `azure` folder* in Visual Studio Code, or create a brand new repository, clone it and copy the contents of the `azure` folder from this repository in it. +- Run `pip install -r requirements.txt` inside the `app` directory +- Open the `multirotor.py` file inside the `app` directory +- Start debugging with Python +- When finished, remember to stop an deallocate the Azure VM to avoid extra charges + +## Running with Docker +Once both the AirSim environment and the Python application are ready, you can package everything as a Docker image. The sample project inside the `azure` directory is already prepared to run a prebuilt AirSim binary and Python code using Docker. + +This would be a perfect scenario when you want to run the simulation at scale. For instance, you could have several different configurations for the same simulation and execute them in a parallel, unattended way using a Docker image on Azure Container Services + +Since AirSim requires access to the host GPU, it is required to use a Docker runtime that supports it. For more information about running AirSim in Docker, click [here](https://github.com/microsoft/AirSim/blob/master/docs/docker_ubuntu.md). + +When using Azure Container Services to run this image, the only extra-requirement is to add GPU support to the Container Group where it will be deployed. + +It can use either public docker images from DockerHub or images deployed to a private Azure Container Registry + +### Building the docker image + +```bash +docker build -t / -f ./docker/Dockerfile .` +``` + +## Using a different AirSim binary + +To use a different AirSim binary, first check the official documentation on [How to Build AirSim on Windows](build_windows.md) and [How to Build AirSim on Linux](build_linux.md) if you also want to run it with Docker + +Once you have a zip file with the new AirSim environment (or prefer to use one from the [Official Releases](https://github.com/microsoft/AirSim/releases)), you need to modify some of the scripts in the `azure` directory of the repository to point to the new environment: +- In [`azure/azure-env-creation/configure-vm.ps1`](https://github.com/microsoft/AirSim/blob/master/azure/azure-env-creation/configure-vm.ps1), modify all the parameters starting with `$airSimBinary` with the new values +- In [`azure/start-airsim.ps1`](https://github.com/microsoft/AirSim/blob/master/azure/start-airsim.ps1), modify `$airSimExecutable` and `$airSimProcessName` with the new values + +If you are using the docker image, you also need a linux binary zip file and modify the following Docker-related files: +- In [`azure/docker/Dockerfile`](https://github.com/microsoft/AirSim/blob/master/azure/docker/Dockerfile), modify the `AIRSIM_BINARY_ZIP_URL` and `AIRSIM_BINARY_ZIP_FILENAME` ENV declarations with the new values +- In [`azure/docker/docker-entrypoint.sh`](https://github.com/microsoft/AirSim/blob/master/azure/docker/docker-entrypoint.sh), modify `AIRSIM_EXECUTABLE` with the new value + +## Maintaining this development environment + +Several components of this development environment (ARM templates, initialization scripts and VSCode tasks) directly depend on the current directory structures file names and repository locations. When planning to modify/fork any of those, make sure to check every script and template to make any required adjustment. diff --git a/docs/build_linux.md b/docs/build_linux.md index 17feaeb98..37519a7da 100644 --- a/docs/build_linux.md +++ b/docs/build_linux.md @@ -1,65 +1,90 @@ -# Build AirSim on Linux +# Build AirSim on Linux & MacOS -The current recommended and tested environment is **Ubuntu 16.04 LTS**. Theoretically, you can build on other distros and OSX as well, but we haven't tested it. +The current recommended and tested environment is **Ubuntu 18.04 LTS**. Theoretically, you can build on other distros as well, but we haven't tested it. -We've two options - you can either build inside docker containers or your host machine. +Only macOS **Catalina (10.15)** is supported. -## Docker +We've two options - you can either build inside docker containers or your host machine. + +## Docker Please see instructions [here](https://github.com/Microsoft/AirSim/blob/master/docs/docker_ubuntu.md) ## Host machine -### Build Unreal Engine and Airsim +### Pre-build Setup + +#### Linux - Build Unreal Engine + - Make sure you are [registered with Epic Games](https://docs.unrealengine.com/latest/INT/Platforms/Linux/BeginnerLinuxDeveloper/SettingUpAnUnrealWorkflow/1/index.html). This is required to get source code access for Unreal Engine. -- Clone Unreal in your favorite folder and build it (this may take a while!). **Note**: We only support Unreal 4.18 at present. +- Clone Unreal in your favorite folder and build it (this may take a while!). **Note**: We only support Unreal >= 4.22 at present. We recommend using 4.24. + ```bash # go to the folder where you clone GitHub projects - git clone -b 4.18 https://github.com/EpicGames/UnrealEngine.git + git clone -b 4.24 https://github.com/EpicGames/UnrealEngine.git cd UnrealEngine ./Setup.sh ./GenerateProjectFiles.sh make ``` +#### macOS - Download Unreal Engine + +1. [Download](https://www.unrealengine.com/download) the Epic Games Launcher. While the Unreal Engine is open source and free to download, registration is still required. +2. Run the Epic Games Launcher, open the `Library` tab on the left pane. +Click on the `Add Versions` which should show the option to download **Unreal 4.24** as shown below. If you have multiple versions of Unreal installed then **make sure 4.24 is set to `current`** by clicking down arrow next to the Launch button for the version. + + **Note**: AirSim also works with UE >= 4.22, however, we recommend you update to 4.24. + **Note**: If you have UE 4.16 or older projects, please see the [upgrade guide](unreal_upgrade.md) to upgrade your projects. + +### Build AirSim + - Clone AirSim and build it: + ```bash # go to the folder where you clone GitHub projects git clone https://github.com/Microsoft/AirSim.git cd AirSim + ``` + + By default AirSim recommends using clang 8 to build the binaries as those will be compatible with UE 4.24. The setup script will install the right version of cmake, llvm, and eigen. - By default AirSim recommends using clang 5 to build the binaries as those will be compatible with Unreal. The setup script - will install the right version of cmake, llvm, and eigen: + ```bash ./setup.sh ./build.sh ``` - Optionally, if you need GCC binaries for some other reason, you can simply add gcc to the setup and build invocation, like this: - ```bash - ./setup.sh --gcc - ./build.sh --gcc - ``` - ### Build Unreal Environment -Finally, you will need an Unreal project that hosts the environment for your vehicles. AirSim comes with a built-in "Blocks Environment" which you can use, or you can create your own. Please see [setting up Unreal Environment](unreal_proj.md). +Finally, you will need an Unreal project that hosts the environment for your vehicles. AirSim comes with a built-in "Blocks Environment" which you can use, or you can create your own. Please see [setting up Unreal Environment](unreal_proj.md) if you'd like to setup your own environment. -### [Optional] Setup Remote Control (Multirotor Only) +## How to Use AirSim -A remote control is required if you want to fly manually. See the [remote control setup](remote_control.md) for more details. +### Linux -Alternatively, you can use [APIs](apis.md) for programmatic control or use the so-called [Computer Vision mode](image_apis.md) to move around using the keyboard. +Once AirSim is setup: -## How to Use AirSim +- Go to `UnrealEngine` installation folder and start Unreal by running `./Engine/Binaries/Linux/UE4Editor`. +- When Unreal Engine prompts for opening or creating project, select Browse and choose `AirSim/Unreal/Environments/Blocks` (or your [custom](unreal_custenv.md) Unreal project). +- Alternatively, the project file can be passed as a commandline argument. For Blocks: `./Engine/Binaries/Linux/UE4Editor /Unreal/Environments/Blocks/Blocks.uproject` +- If you get prompts to convert project, look for More Options or Convert-In-Place option. If you get prompted to build, choose Yes. If you get prompted to disable AirSim plugin, choose No. +- After Unreal Editor loads, press Play button. -Once AirSim is set up by following above steps, you can, +### Mac -- Go to `UnrealEngine` folder and start Unreal by running `UnrealEngine/Engine/Binaries/Linux/UE4Editor`. -- When Unreal Engine prompts for opening or creating project, select Browse and choose `AirSim/Unreal/Environments/Blocks` (or your [custom](unreal_custenv.md) Unreal project). -- If you get prompts to convert project, look for More Options or Convert-In-Place option. If you get prompted to build, chose Yes. If you get prompted to disable AirSim plugin, choose No. -- After Unreal Editor loads, press Play button. Tip: go to 'Edit->Editor Preferences', in the 'Search' box type 'CPU' and ensure that the 'Use Less CPU when in Background' is unchecked. +- Browse to `AirSim/Unreal/Environments/Blocks`. +- Run `./GenerateProjectFiles.sh ` from the terminal, where `UE_PATH` is the path to the Unreal installation folder. (By default, this is `/Users/Shared/Epic\ Games/UE_4.24/`) The script creates an XCode workspace by the name Blocks.xcworkspace. +- Open the XCode workspace, and press the Build and run button in the top left. +- After Unreal Editor loads, press Play button. -See [Using APIs](apis.md) and [settings.json](settings.md) for various options available. +See [Using APIs](apis.md) and [settings.json](settings.md) for various options available for AirSim usage. +Tip: go to 'Edit->Editor Preferences', in the 'Search' box type 'CPU' and ensure that the 'Use Less CPU when in Background' is unchecked. + +### [Optional] Setup Remote Control (Multirotor Only) + +A remote control is required if you want to fly manually. See the [remote control setup](remote_control.md) for more details. + +Alternatively, you can use [APIs](apis.md) for programmatic control or use the so-called [Computer Vision mode](image_apis.md) to move around using the keyboard. ## FAQs @@ -72,7 +97,7 @@ See [Using APIs](apis.md) and [settings.json](settings.md) for various options a * If you see other compile errors in console then open up those source files and see if it is due to changes you made. If not, then report it as issue on GitHub. - Unreal crashed! How do I know what went wrong? - * Go to the `MyUnrealProject/Saved/Crashes` folder and search for the file `MyProject.log` within its subdirectories. At the end of this file you will see the stack trace and messages. + * Go to the `MyUnrealProject/Saved/Crashes` folder and search for the file `MyProject.log` within its subdirectories. At the end of this file you will see the stack trace and messages. You can also take a look at the `Diagnostics.txt` file. - How do I use an IDE on Linux? @@ -82,15 +107,15 @@ See [Using APIs](apis.md) and [settings.json](settings.md) for various options a * Yes, you can, but we haven't tested it. You can find the instructions [here](https://docs.unrealengine.com/latest/INT/Platforms/Linux/GettingStarted/index.html). - What compiler and stdlib does AirSim use? - * We use the same compiler that Unreal Engine uses, **Clang 5.0**, and stdlib, **libc++**. AirSim's `setup.sh` will automatically download them both. The libc++ source code is cloned into the `llvm-source-(version)` folder and is built into the `llvm-build` folder, from where CMake uses libc++. + * We use the same compiler that Unreal Engine uses, **Clang 8**, and stdlib, **libc++**. AirSim's `setup.sh` will automatically download them. - What version of CMake does the AirSim build use? - * 3.9.0 or higher. This is *not* the default in Ubuntu 16.04 so setup.sh installs it for you. You can check your CMake version using `cmake --version`. If you have an older version, follow [these instructions](cmake_linux.md) or see the [CMake website](https://cmake.org/install/). + * 3.10.0 or higher. This is *not* the default in Ubuntu 16.04 so setup.sh installs it for you. You can check your CMake version using `cmake --version`. If you have an older version, follow [these instructions](cmake_linux.md) or see the [CMake website](https://cmake.org/install/). - Can I compile AirSim in BashOnWindows? - * Yes, however, you can't run Unreal from BashOnWindows. So this is kind of useful to check a Linux compile, but not for an end-to-end run. - See the [BashOnWindows install guide](https://msdn.microsoft.com/en-us/commandline/wsl/install_guide). - Make sure to have the latest version (Windows 10 Creators Edition) as previous versions had various issues. + * Yes, however, you can't run Unreal from BashOnWindows. So this is kind of useful to check a Linux compile, but not for an end-to-end run. + See the [BashOnWindows install guide](https://msdn.microsoft.com/en-us/commandline/wsl/install_guide). + Make sure to have the latest version (Windows 10 Creators Edition) as previous versions had various issues. Also, don't invoke `bash` from `Visual Studio Command Prompt`, otherwise CMake might find VC++ and try and use that! - Where can I find more info on running Unreal on Linux? diff --git a/docs/build_windows.md b/docs/build_windows.md index ed672bea3..a5096296e 100644 --- a/docs/build_windows.md +++ b/docs/build_windows.md @@ -3,16 +3,17 @@ ## Install Unreal Engine 1. [Download](https://www.unrealengine.com/download) the Epic Games Launcher. While the Unreal Engine is open source and free to download, registration is still required. -2. Run the Epic Games Launcher, open the `Library` tab on the left pane. -Click on the `Add Versions` which should show the option to download **Unreal 4.18** as shown below. If you have multiple versions of Unreal installed then **make sure 4.18 is set to `current`** by clicking down arrow next to the Launch button for the version. +2. Run the Epic Games Launcher, open the `Library` tab on the left pane. +Click on the `Add Versions` which should show the option to download **Unreal 4.24** as shown below. If you have multiple versions of Unreal installed then **make sure 4.24 is set to `current`** by clicking down arrow next to the Launch button for the version. + **Note**: AirSim also works with UE >= 4.22, however, we recommend you update to 4.24. **Note**: If you have UE 4.16 or older projects, please see the [upgrade guide](unreal_upgrade.md) to upgrade your projects. ## Build AirSim -* Install Visual Studio 2017. -**Make sure** to select **VC++** and **Windows SDK 8.1** while installing VS 2017. -* Start `x64 Native Tools Command Prompt for VS 2017`. -* Clone the repo: `git clone https://github.com/Microsoft/AirSim.git`, and go the AirSim directory by `cd AirSim`. +* Install Visual Studio 2019. +**Make sure** to select **Desktop Development with C++** and **Windows 10 SDK 10.0.18362** (should be selected by default) while installing VS 2019. +* Start `Developer Command Prompt for VS 2019`. +* Clone the repo: `git clone https://github.com/Microsoft/AirSim.git`, and go the AirSim directory by `cd AirSim`. * Run `build.cmd` from the command line. This will create ready to use plugin bits in the `Unreal\Plugins` folder that can be dropped into any Unreal project. ## Build Unreal Project @@ -36,23 +37,56 @@ Once AirSim is set up by following above steps, you can, See [Using APIs](apis.md) and [settings.json](settings.md) for various options available. # AirSim on Unity (Experimental) -[Unity](https://unity3d.com/) is another great game engine platform and we have an [experimental release](https://github.com/Microsoft/AirSim/tree/master/Unity) of AirSim on Unity. Please note that this is work in progress and all features may not work yet. +[Unity](https://unity3d.com/) is another great game engine platform and we have an **experimental** integration of [AirSim with Unity](https://microsoft.github.com/AirSim/Unity). Please note that this is work in progress and all features may not work yet. # FAQ -#### I get `error C100 : An internal error has occurred in the compiler` when running build.cmd + +### How to force Unreal to use Visual Studio 2019? + +If the default `update_from_git.bat` file results in VS 2017 project, then you may need to run the `C:\Program Files\Epic Games\UE_4.24\Engine\Binaries\DotNET\UnrealBuildTool.exe` tool manually, with the command line options `-projectfiles -project= -game -rocket -progress -2019`. + +If you are upgrading from 4.18 to 4.24 you may also need to add `BuildSettingsVersion.V2` to your `*.Target.cs` and `*Editor.Target.cs` build files, like this: + +```c# + public AirSimNHTestTarget(TargetInfo Target) : base(Target) + { + Type = TargetType.Game; + DefaultBuildSettings = BuildSettingsVersion.V2; + ExtraModuleNames.AddRange(new string[] { "AirSimNHTest" }); + } +``` + +You may also need to edit this file: + +``` +"%APPDATA%\Unreal Engine\UnrealBuildTool\BuildConfiguration.xml +``` + +And add this Compiler version setting: + +```xml + + + VisualStudio2019 + + +``` + + +### I get `error C100 : An internal error has occurred in the compiler` when running build.cmd We have noticed this happening with VS version `15.9.0` and have checked-in a workaround in AirSim code. If you have this VS version, please make sure to pull the latest AirSim code. #### I get error "'corecrt.h': No such file or directory" or "Windows SDK version 8.1 not found" -Very likely you don't have [Windows SDK](https://developercommunity.visualstudio.com/content/problem/3754/cant-compile-c-program-because-of-sdk-81cant-add-a.html) installed with Visual Studio. +Very likely you don't have [Windows SDK](https://developercommunity.visualstudio.com/content/problem/3754/cant-compile-c-program-because-of-sdk-81cant-add-a.html) installed with Visual Studio. -#### How do I use PX4 firmware with AirSim? +### How do I use PX4 firmware with AirSim? By default, AirSim uses its own built-in firmware called [simple_flight](simple_flight.md). There is no additional setup if you just want to go with it. If you want to switch to using PX4 instead then please see [this guide](px4_setup.md). -#### I made changes in Visual Studio but there is no effect +### I made changes in Visual Studio but there is no effect Sometimes the Unreal + VS build system doesn't recompile if you make changes to only header files. To ensure a recompile, make some Unreal based cpp file "dirty" like AirSimGameMode.cpp. -#### Unreal still uses VS2015 or I'm getting some link error +### Unreal still uses VS2015 or I'm getting some link error Running several versions of VS can lead to issues when compiling UE projects. One problem that may arise is that UE will try to compile with an older version of VS which may or may not work. There are two settings in Unreal, one for for the engine and one for the project, to adjust the version of VS to be used. 1. Edit -> Editor preferences -> General -> Source code 2. Edit -> Project Settings -> Platforms -> Windows -> Toolchain ->CompilerVersion @@ -61,5 +95,5 @@ In some cases, these settings will still not lead to the desired result and erro To resolve such issues the following procedure can be applied: 1. Uninstall all old versions of VS using the [VisualStudioUninstaller](https://github.com/Microsoft/VisualStudioUninstaller/releases) -2. Repair/Install VS2017 +2. Repair/Install VS 2019 3. Restart machine and install Epic launcher and desired version of the engine \ No newline at end of file diff --git a/docs/code_structure.md b/docs/code_structure.md index 0341fda70..59cd36567 100644 --- a/docs/code_structure.md +++ b/docs/code_structure.md @@ -26,7 +26,7 @@ DroneShell demonstrates how to connect to the simulator using UDP. The simulato ## Contributing -See [Contribution Guidelines](../CONTRIBUTING.md) +See [Contribution Guidelines](CONTRIBUTING.md) ## Unreal Framework diff --git a/docs/create_issue.md b/docs/create_issue.md index 227f81abe..be29562b0 100644 --- a/docs/create_issue.md +++ b/docs/create_issue.md @@ -18,6 +18,4 @@ AirSim is open source project and contributors like you keeps it going. It is im * Do not use "Please help" etc in the title. See above. * Do not copy and paste screen shot of error message. Copy and paste text. * Do not use "it doesn't work". Precisely state what is the error message or symptom. -* Do not ask to write code for you. [Contribute](../CONTRIBUTING.md)! - - +* Do not ask to write code for you. [Contribute](CONTRIBUTING.md)! diff --git a/docs/custom_unity_environments.md b/docs/custom_unity_environments.md index 4eb555ee0..22d1eea05 100644 --- a/docs/custom_unity_environments.md +++ b/docs/custom_unity_environments.md @@ -1,5 +1,5 @@ # Adding AirSim to Custom Unity Projects -Before completing these steps, make sure you have properly [set up AirSim for Unity](../Unity/README.md) +Before completing these steps, make sure you have properly [set up AirSim for Unity](Unity.md) 1. Open the Containing folder of your custom unity project 2. Copy and paste the following items from Unity demo into the main project folder of your custom environment: ``` diff --git a/docs/dev_workflow.md b/docs/dev_workflow.md index f10cacc22..b03f05f67 100644 --- a/docs/dev_workflow.md +++ b/docs/dev_workflow.md @@ -4,7 +4,7 @@ Below is the guide on how to perform different development activities while work ## Development Environment ### OS -We highly recommend Windows 10 and Visual Studio 2017 as your development environment. The support for other OSes and IDE is unfortunately not as mature on the Unreal Engine side and you may risk severe loss of productivity trying to do workarounds and jumping through the hoops. +We highly recommend Windows 10 and Visual Studio 2019 as your development environment. The support for other OSes and IDE is unfortunately not as mature on the Unreal Engine side and you may risk severe loss of productivity trying to do workarounds and jumping through the hoops. ### Hardware We recommend GPUs such as NVidia 1080 or NVidia Titan series with powerful desktop such as one with 64GB RAM, 6+ cores, SSDs and 2-3 displays (ideally 4K). We have found HP Z840 work quite well for our needs. The development experience on high-end laptops is generally sub-par compared to powerful desktops however they might be useful in a pinch. You generally want laptops with discrete NVidia GPU (at least M2000 or better) with 64GB RAM, SSDs and hopefully 4K display. We have found models such as Lenovo P50 work well for our needs. Laptops with only integrated graphics might not work well. @@ -20,7 +20,7 @@ The first step is accomplished by build.cmd available in AirSim root. This comma Below are the steps we use to make changes in AirSim and test them out. The best way to do development in AirSim code is to use [Blocks project](unreal_blocks.md). This is the light weight project so compile time is relatively faster. Generally the workflow is, ``` -REM //Use x64 Native Tools Command Prompt for VS 2017 +REM //Use x64 Native Tools Command Prompt for VS 2019 REM //Navigate to AirSim repo folder git pull @@ -36,7 +36,7 @@ After you are done with you code changes, you might want to push your changes ba ``` -REM //Use x64 Native Tools Command Prompt for VS 2017 +REM //Use x64 Native Tools Command Prompt for VS 2019 REM //run this from Unreal\Environments\Blocks update_to_git.bat diff --git a/docs/docker_ubuntu.md b/docs/docker_ubuntu.md index 54bc01ace..67987d729 100644 --- a/docs/docker_ubuntu.md +++ b/docs/docker_ubuntu.md @@ -7,15 +7,15 @@ We've two options for docker. You can either build an image for running [airsim #### Build the docker image - Below are the default arguments. - `--base_image`: This is image over which we'll install airsim. We've tested on Ubuntu 16.04 + CUDA 10.0. + `--base_image`: This is image over which we'll install airsim. We've tested on Ubuntu 18.04 with CUDA 10.0. You can specify any [NVIDIA cudagl](https://hub.docker.com/r/nvidia/cudagl/) at your own risk. `--target_image` is the desired name of your docker image. Defaults to `airsim_binary` with same tag as the base image ``` $ cd Airsim/docker; $ python build_airsim_image.py \ - --base_image=nvidia/cudagl:10.0-devel-ubuntu16.04 \ - --target_image=airsim_binary:10.0-devel-ubuntu16.04 + --base_image=nvidia/cudagl:10.0-devel-ubuntu18.04 \ + --target_image=airsim_binary:10.0-devel-ubuntu18.04 ``` - Verify you have an image by: @@ -35,9 +35,9 @@ You can download it by running ``` $ ./run_airsim_image_binary.sh DOCKER_IMAGE_NAME UNREAL_BINARY_SHELL_SCRIPT UNREAL_BINARY_ARGUMENTS -- headless ``` - For blocks, you can do a `$ ./run_airsim_image_binary.sh airsim_binary:10.0-devel-ubuntu16.04 Blocks/Blocks.sh -windowed -ResX=1080 -ResY=720` + For blocks, you can do a `$ ./run_airsim_image_binary.sh airsim_binary:10.0-devel-ubuntu18.04 Blocks/Blocks.sh -windowed -ResX=1080 -ResY=720` - * `DOCKER_IMAGE_NAME`: Same as `target_image` parameter in previous step. By default, enter `airsim_binary:10.0-devel-ubuntu16.04` + * `DOCKER_IMAGE_NAME`: Same as `target_image` parameter in previous step. By default, enter `airsim_binary:10.0-devel-ubuntu18.04` * `UNREAL_BINARY_SHELL_SCRIPT`: for Blocks enviroment, it will be `Blocks/Blocks.sh` * [`UNREAL_BINARY_ARGUMENTS`](https://docs.unrealengine.com/en-us/Programming/Basics/CommandLineArguments): For airsim, most relevant would be `-windowed`, `-ResX`, `-ResY`. Click on link to see all options. diff --git a/docs/drone_survey.md b/docs/drone_survey.md new file mode 100644 index 000000000..50988da1a --- /dev/null +++ b/docs/drone_survey.md @@ -0,0 +1,50 @@ +# Implementing a Drone Survey script + +Moved here from [https://github.com/microsoft/AirSim/wiki/Implementing-a-Drone-Survey-script](https://github.com/microsoft/AirSim/wiki/Implementing-a-Drone-Survey-script) + +Ever wanted to capture a bunch of top-down pictures of a certain location? Well, the Python API makes this really simple. See the [code available here](https://github.com/microsoft/AirSim/blob/master/PythonClient/multirotor/survey.py). + +![survey](images/survey.png) + +Let's assume we want the following variables: + +| Variable | Description | +| ------------- | ------------- | +| boxsize | The overall size of the square box to survey | +| stripewidth | How far apart to drive the swim lanes, this can depend on the type of camera lens, for example. | +| altitude | The height to fly the survey. | +| speed | The speed of the survey can depend on how fast your camera can snap shots. | + +So with these we can compute a square path box using this code: + +```python + path = [] + distance = 0 + while x < self.boxsize: + distance += self.boxsize + path.append(Vector3r(x, self.boxsize, z)) + x += self.stripewidth + distance += self.stripewidth + path.append(Vector3r(x, self.boxsize, z)) + distance += self.boxsize + path.append(Vector3r(x, -self.boxsize, z)) + x += self.stripewidth + distance += self.stripewidth + path.append(Vector3r(x, -self.boxsize, z)) + distance += self.boxsize +``` +Assuming we start in the corner of the box, increment x by the stripe width, then fly the full y-dimension of `-boxsize` to `+boxsize`, so in this case, `boxsize` is half the size of the actual box we will be covering. + +Once we have this list of Vector3r objects, we can fly this path very simply with the following call: +```python +result = self.client.moveOnPath(path, self.velocity, trip_time, DrivetrainType.ForwardOnly, + YawMode(False,0), lookahead, 1) +``` + +We can compute an appropriate `trip_time` timeout by dividing the distance of the path and the speed we are flying. + +The `lookahead` needed here for smooth path interpolation can be computed from the velocity using `self.velocity + (self.velocity/2)`. The more lookahead, the smoother the turns. This is why you see in the screenshot that the ends of each swimland are smooth turns rather than a square box pattern. This can result in a smoother video from your camera also. + +That's it, pretty simple, eh? + +Now of course you can add a lot more intelligence to this, make it avoid known obstacles on your map, make it climb up and down a hillside so you can survey a slope, etc. Lots of fun to be had. diff --git a/docs/flight_controller.md b/docs/flight_controller.md index 303473aaf..8b7233364 100644 --- a/docs/flight_controller.md +++ b/docs/flight_controller.md @@ -16,5 +16,6 @@ In "software-in-loop" simulation (SITL or SIL) mode the firmware runs in your co AirSim has built-in flight controller called [simple_flight](simple_flight.md) and it is used by default. You don't need to do anything to use or configure it. AirSim also supports [PX4](px4_setup.md) as another flight controller for advanced users. In the future, we also plan to support [ROSFlight](https://rosflight.org/) and [Hackflight](https://github.com/simondlevy/hackflight). - ## Using AirSim Without Flight Controller +## Using AirSim Without Flight Controller + Yes, now it's possible to use AirSim without flight controller. Please see the [instructions here](image_apis.md) for how to use so-called "Computer Vision" mode. If you don't need vehicle dynamics, we highly recommend using this mode. diff --git a/docs/image_apis.md b/docs/image_apis.md index 96c9cd133..4076ae80b 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -25,14 +25,13 @@ png_image = client.simGetImage("0", airsim.ImageType.Scene) int getOneImage() { - using namespace std; using namespace msr::airlib; - //for car use CarRpcLibClient - msr::airlib::MultirotorRpcLibClient client; + // for car use CarRpcLibClient + MultirotorRpcLibClient client; - vector png_image = client.simGetImage("0", VehicleCameraBase::ImageType::Scene); - //do something with images + std::vector png_image = client.simGetImage("0", VehicleCameraBase::ImageType::Scene); + // do something with images } ``` @@ -94,19 +93,18 @@ airsim.write_png(os.path.normpath(filename + '.png'), img_rgb) ```cpp int getStereoAndDepthImages() { - using namespace std; using namespace msr::airlib; typedef VehicleCameraBase::ImageRequest ImageRequest; typedef VehicleCameraBase::ImageResponse ImageResponse; typedef VehicleCameraBase::ImageType ImageType; - //for car use - //msr::airlib::CarRpcLibClient client; - msr::airlib::MultirotorRpcLibClient client; + // for car use + // CarRpcLibClient client; + MultirotorRpcLibClient client; - //get right, left and depth images. First two as png, second as float16. - vector request = { + // get right, left and depth images. First two as png, second as float16. + std::vector request = { //png format ImageRequest("0", ImageType::Scene), //uncompressed RGB array bytes @@ -115,8 +113,8 @@ int getStereoAndDepthImages() ImageRequest("1", ImageType::DepthPlanner, true) }; - const vector& response = client.simGetImages(request); - //do something with response which contains image data, pose, timestamp etc + const std::vector& response = client.simGetImages(request); + // do something with response which contains image data, pose, timestamp etc } ``` @@ -166,9 +164,10 @@ To move around the environment using APIs you can use `simSetVehiclePose` API. T ## Camera APIs The `simGetCameraInfo` returns the pose (in world frame, NED coordinates, SI units) and FOV (in degrees) for the specified camera. Please see [example usage](https://github.com/Microsoft/AirSim/tree/master/PythonClient//computer_vision/cv_mode.py). -The `simSetCameraOrientation` sets the orientation for the specified camera as quaternion in NED frame. The handy `airsim.to_quaternion()` function allows to convert pitch, roll, yaw to quaternion. For example, to set camera-0 to 15-degree pitch, you can use: +The `simSetCameraPose` sets the pose for the specified camera while taking an input pose as a combination of relative position and a quaternion in NED frame. The handy `airsim.to_quaternion()` function allows to convert pitch, roll, yaw to quaternion. For example, to set camera-0 to 15-degree pitch while maintaining the same position, you can use: ``` -client.simSetCameraOrientation(0, airsim.to_quaternion(0.261799, 0, 0)); #radians +camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.261799, 0, 0)) #RPY in radians +client.simSetCameraPose(0, camera_pose); ``` ### Gimbal @@ -221,7 +220,7 @@ When you specify `ImageType = DepthVis` in `ImageRequest`, you get an image that You normally want to retrieve disparity image as float (i.e. set `pixels_as_float = true` and specify `ImageType = DisparityNormalized` in `ImageRequest`) in which case each pixel is `(Xl - Xr)/Xmax`, which is thereby normalized to values between 0 to 1. ### Segmentation -When you specify `ImageType = Segmentation` in `ImageRequest`, you get an image that gives you ground truth segmentation of the scene. At the startup, AirSim assigns value 0 to 255 to each mesh available in environment. This value is than mapped to a specific color in [the pallet](https://github.com/Microsoft/AirSim/tree/master/Unreal//Plugins/AirSim/Content/HUDAssets/seg_color_pallet.png). The RGB values for each object ID can be found in [this file](seg_rgbs.txt). +When you specify `ImageType = Segmentation` in `ImageRequest`, you get an image that gives you ground truth segmentation of the scene. At the startup, AirSim assigns value 0 to 255 to each mesh available in environment. This value is then mapped to a specific color in [the pallet](https://github.com/Microsoft/AirSim/tree/master/Unreal//Plugins/AirSim/Content/HUDAssets/seg_color_pallet.png). The RGB values for each object ID can be found in [this file](seg_rgbs.txt). You can assign a specific value (limited to the range 0-255) to a specific mesh using APIs. For example, below Python code sets the object ID for the mesh called "Ground" to 20 in Blocks environment and hence changes its color in Segmentation view: @@ -258,7 +257,7 @@ A complete ready-to-run example can be found in [segmentation.py](https://github An object's ID can be set to -1 to make it not show up on the segmentation image. #### How to Find Mesh Names? -To get desired ground truth segmentation you will need to know the names of the meshes in your Unreal environment. To do this, you will need to open up Unreal Environment in Unreal Editor and then inspect the names of the meshes you are interested in using the World Outliner. For example, below we see the mesh names for he ground in Blocks environment in right panel in the editor: +To get desired ground truth segmentation you will need to know the names of the meshes in your Unreal environment. To do this, you will need to open up Unreal Environment in Unreal Editor and then inspect the names of the meshes you are interested in using the World Outliner. For example, below we see the mesh names for the ground in Blocks environment in right panel in the editor: ![record screenshot](images/unreal_editor_blocks.png) @@ -267,7 +266,7 @@ If you don't know how to open Unreal Environment in Unreal Editor then try follo Once you decide on the meshes you are interested, note down their names and use above API to set their object IDs. There are [few settings](settings.md#segmentation-settings) available to change object ID generation behavior. #### Changing Colors for Object IDs -At present the color for each object ID is fixed as in [this palate](https://github.com/Microsoft/AirSim/tree/master/Unreal//Plugins/AirSim/Content/HUDAssets/seg_color_pallet.png). We will be adding ability to change colors for object IDs to desired values shortly. In the meantime you can open the segmentation image in your favorite image editor and get the RGB values you are interested in. +At present the color for each object ID is fixed as in [this pallet](https://github.com/Microsoft/AirSim/tree/master/Unreal//Plugins/AirSim/Content/HUDAssets/seg_color_pallet.png). We will be adding ability to change colors for object IDs to desired values shortly. In the meantime you can open the segmentation image in your favorite image editor and get the RGB values you are interested in. #### Startup Object IDs At the start, AirSim assigns object ID to each object found in environment of type `UStaticMeshComponent` or `ALandscapeProxy`. It then either uses mesh name or owner name (depending on settings), lower cases it, removes any chars below ASCII 97 to remove numbers and some punctuations, sums int value of all chars and modulo 255 to generate the object ID. In other words, all object with same alphabet chars would get same object ID. This heuristic is simple and effective for many Unreal environments but may not be what you want. In that case, please use above APIs to change object IDs to your desired values. There are [few settings](settings.md#segmentation-settings) available to change this behavior. diff --git a/docs/images/depth.png b/docs/images/depth.png new file mode 100644 index 000000000..1b1612b47 Binary files /dev/null and b/docs/images/depth.png differ diff --git a/docs/images/orbit.png b/docs/images/orbit.png new file mode 100644 index 000000000..adf171d8d Binary files /dev/null and b/docs/images/orbit.png differ diff --git a/docs/images/point_cloud.png b/docs/images/point_cloud.png new file mode 100644 index 000000000..2140120e3 Binary files /dev/null and b/docs/images/point_cloud.png differ diff --git a/docs/images/survey.png b/docs/images/survey.png new file mode 100644 index 000000000..649f3c5bf Binary files /dev/null and b/docs/images/survey.png differ diff --git a/docs/images/tex_shuffle_actor.png b/docs/images/tex_shuffle_actor.png new file mode 100644 index 000000000..53e113668 Binary files /dev/null and b/docs/images/tex_shuffle_actor.png differ diff --git a/docs/images/tex_swap_demo.gif b/docs/images/tex_swap_demo.gif new file mode 100644 index 000000000..d9d18e0a0 Binary files /dev/null and b/docs/images/tex_swap_demo.gif differ diff --git a/docs/images/tex_swap_group_editing.png b/docs/images/tex_swap_group_editing.png new file mode 100644 index 000000000..650d3962a Binary files /dev/null and b/docs/images/tex_swap_group_editing.png differ diff --git a/docs/images/tex_swap_material.png b/docs/images/tex_swap_material.png new file mode 100644 index 000000000..06f0289be Binary files /dev/null and b/docs/images/tex_swap_material.png differ diff --git a/docs/images/tex_swap_subset.png b/docs/images/tex_swap_subset.png new file mode 100644 index 000000000..eec0eba38 Binary files /dev/null and b/docs/images/tex_swap_subset.png differ diff --git a/docs/mavlinkcom.md b/docs/mavlinkcom.md new file mode 100644 index 000000000..344773f74 --- /dev/null +++ b/docs/mavlinkcom.md @@ -0,0 +1,220 @@ +# Welcome to MavLinkCom + +MavLinkCom is a cross-platform C++ library that helps connect to and communicate with [MavLink](https://github.com/mavlink/mavlink) based vehicles. +Specifically this library is designed to work well with [PX4](https://github.com/PX4/Firmware) based drones. + +## Design + +You can view and edit the [Design.dgml](mavlinkcom_design/Design.dgml) diagram in Visual Studio. +![Overview](mavlinkcom_design/images/overview.png) + +The following are the most important classes in this library. + +### MavLinkNode + +This is the base class for all MavLinkNodes (subclasses include MavLinkVehicle, MavLinkVideoClient and MavLinkVideoServer). +The node connects to your mavlink enabled vehicle via a MavLinkConnection and provides methods for sending MavLinkMessages and MavLinkCommands +and for subscribing to receive messages. This base class also stores the local system id and component id your app wants to use to identify +itself to your remove vehicle. You can also call startHeartbeat to send regular heartbeat messages to keep the connection alive. + +### MavLinkMessage + +This is the encoded MavLinkMessage. For those who have used the mavlink.h C API, this is similar to mavlink_message_t. You do +not create these manually, they are encoded from a strongly typed MavLinkMessageBase subclass. + +### Strongly typed message and command classes + +The MavLinkComGenerator parses the mavlink common.xml message definitions and generates all the MavLink* MavLinkMessageBase subclasses +as well as a bunch of handy mavlink enums and a bunch of strongly typed MavLinkCommand subclasses. + +### MavLinkMessageBase + +This is the base class for a set of strongly typed message classes that are code generated by the MavLinkComGenerator project. +This replaces the C messages defined in the mavlink C API and provides a slightly more object oriented way to send and receive messages +via sendMessage on MavLinkNode. These classes have encode/decode methods that convert to and from the MavLinkMessage class. + +### MavLinkCommand + +This is the base class for a set of strongly typed command classes that are code generated by the MavLinkComGenerator project. +This replaces the C definitions defined in the mavlink C API and provides a more object oriented way to send commands via the sendCommand +method on MavLinkNode. The MavLinkNode takes care of turning these into the underlying mavlink COMMAND_LONG message. + +### MavLinkConnection + +This class provides static helper methods for creating connections to remote MavLink nodes, over serial ports, as well as UDP, or TCP sockets. +This class provides a way to subscribe to receive messages from that node in a pub/sub way so you can have multiple subscribers on the +same connection. MavLinkVehicle uses this to track various messages that define the overall vehicle state. + +### MavLinkVehicle + +MavLinkVehicle is a MavLinkNode that tracks various messages that define the overall vehicle state and provides a VehicleState struct +containing a snapshot of that state, including home position, current orientation, local position, global position, and so on. +This class also provides a bunch of helper methods that wrap commonly used commands providing simple method calls to do things like +arm, disarm, takeoff, land, go to a local coordinate, and fly under offbaord control either by position or velocity control. + +### MavLinkTcpServer + +This helper class provides a way to setup a "server" that accepts MavLinkConnections from remote nodes. You can use this class +to get a connection that you can then give to MavLinkVideoServer to serve images over MavLink. + +### MavLinkFtpClient + +This helper class takes a given MavLinkConnection and provides FTP client support for the MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL +for vehicles that support the FTP capability. This class provides simple methods to list directory contents, and the get and put +files. + +### MavLinkVideoClient + +This helper class takes a given MavLinkConnection and provides helper methods for requesting video from remote node and +packaging up the MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE and MAVLINK_MSG_ID_ENCAPSULATED_DATA messages into simple to use +MavLinkVideoFrames. + +### MavLinkVideoServer + +This helper class takes a given MavLinkConnection and provides the server side of the MavLinkVideoClient protocol, including helper methods +for notifying when there is a video request to process (hasVideoRequest) and a method to send video frames (sendFrame) which +will generate the right MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE and MAVLINK_MSG_ID_ENCAPSULATED_DATA sequence. + +## Examples + +The following code from the UnitTest project shows how to connect to a [Pixhawk](http://www.pixhawk.org/) flight controller over USB serial port, +then wait for the first heartbeat message to be received: + + auto connection = MavLinkConnection::connectSerial("drone", "/dev/ttyACM0", 115200, "sh /etc/init.d/rc.usb\n"); + MavLinkHeartbeat heartbeat; + if (!waitForHeartbeat(10000, heartbeat)) { + throw std::runtime_error("Received no heartbeat from PX4 after 10 seconds"); + } + + +The following code connects to serial port, and then forwards all messages to and from QGroundControl to that drone using another connection +that is joined to the drone stream. + + auto droneConnection = MavLinkConnection::connectSerial("drone", "/dev/ttyACM0", 115200, "sh /etc/init.d/rc.usb\n"); + auto proxyConnection = MavLinkConnection::connectRemoteUdp("qgc", "127.0.0.1", "127.0.0.1", 14550); + droneConnection->join(proxyConnection); + +The following code then takes that connection and turns on heartBeats and starts tracking vehicle information using local +system id 166 and component id 1. + + auto vehicle = std::make_shared(166, 1); + vehicle->connect(connection); + vehicle->startHeartbeat(); + + std::this_thread::sleep_for(std::chrono::seconds(5)); + + VehicleState state = vehicle->getVehicleState(); + printf("Home position is %s, %f,%f,%f\n", state.home.is_set ? "set" : "not set", + state.home.global_pos.lat, state.home.global_pos.lon, state.home.global_pos.alt); + +The following code uses the vehicle object to arm the drone and take off and wait for the takeoff altitude to be reached: + + bool rc = false; + if (!vehicle->armDisarm(true).wait(3000, &rc) || !rc) { + printf("arm command failed\n"); + return; + } + if (!vehicle->takeoff(targetAlt).wait(3000, &rc) || !rc) { + printf("takeoff command failed\n"); + return; + } + int version = vehicle->getVehicleStateVersion(); + while (true) { + int newVersion = vehicle->getVehicleStateVersion(); + if (version != newVersion) { + VehicleState state = vehicle->getVehicleState(); + float alt = state.local_est.pos.z; + if (alt >= targetAlt - delta && alt <= targetAlt + delta) + { + reached = true; + printf("Target altitude reached\n"); + break; + } + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + +The following code uses offboard control to make the drone fly in a circle with camera pointed at the center. +Here we use the subscribe method to check each new local position message to indicate so we can compute the new +velocity vector as soon as that new position is received. We request a high rate for those messages using +setMessageInterval to ensure smooth circular orbit. + + vehicle->setMessageInterval((int)MavLinkMessageIds::MAVLINK_MSG_ID_LOCAL_POSITION_NED, 30); + vehicle->requestControl(); + int subscription = vehicle->getConnection()->subscribe( + [&](std::shared_ptr connection, const MavLinkMessage& m) { + if (m.msgid == (int)MavLinkMessageIds::MAVLINK_MSG_ID_LOCAL_POSITION_NED) + { + float x = localPos.x; + float y = localPos.y; + float dx = x - cx; + float dy = y - cy; + float angle = atan2(dy, dx); + if (angle < 0) angle += M_PI * 2; + float tangent = angle + M_PI_2; + double newvx = orbitSpeed * cos(tangent); + double newvy = orbitSpeed * sin(tangent); + float heading = angle + M_PI; + vehicle->moveByLocalVelocityWithAltHold(newvx, newvy, altitude, true, heading); + } + }); + +The following code stops flying the drone in offboard mode and tells the drone to loiter at its current location. +This version of the code shows how to use the AsyncResult without blocking on a wait call. + + vehicle->releaseControl(); + if (vehicle->loiter().then([=](bool rc) { + printf("loiter command %s\n", rc ? "succeeded" : "failed"); + } + +The following code gets all configurable parameters from the drone and prints them: + + auto list = vehicle->getParamList(); + auto end = list.end(); + int count = 0; + for (auto iter = list.begin(); iter < end; iter++) + { + count++; + MavLinkParameter p = *iter; + if (p.type == MAV_PARAM_TYPE_REAL32 || p.type == MAV_PARAM_TYPE_REAL64) { + printf("%s=%f\n", p.name.c_str(), p.value); + } + else { + printf("%s=%d\n", p.name.c_str(), static_cast(p.value)); + } + } + +The following code sets a parameter on the Pixhawk to disable the USB safety check (this is handy if you are controlling +the Pixhawk over USB using another onboard computer that is part of the drone itself). You should NOT do this if you +are connecting your PC or laptop to the drone over USB. + + MavLinkParameter p; + p.name = "CBRK_USB_CHK"; + p.value = 197848; + if (!vehicle->setParameter(p).wait(3000,&rc) || !rc) { + printf("Setting the CBRK_USB_CHK failed"); + } + +MavLinkVehicle actually has a helper method for this called allowFlightControlOverUsb, so now you know how it is implemented :-) + +## Advanced Connections + +You can wire up different configurations of mavlink pipelines using the MavLinkConnection class "join" method as shown below. + +Example 1, we connect to PX4 over serial, and proxy those messages through to QGroundControl and the LogViewer who are listening on remote ports. + +![Serial to QGC](mavlinkcom_design/images/example1.png) + +Example 2: simulation can talk to jMavSim and jMavSim connects to PX4. jMavSim can also manage multiple connections, so it can talk to unreal simulator. +Another MavLinkConnection can be joined to proxy connections that jMavSim doesn't support, like the LogViewer or a remote camera node. + +![Serial to QGC](mavlinkcom_design/images/example2.png) + +Example 3: we use MavLinkConnection to connect to PX4 over serial, then join additional connections for all our remote nodes including jMavSim. + +![Serial to QGC](mavlinkcom_design/images/example3.png) + +Example 4: We can also do distributed systems to control the drone remotely: + +![Serial to QGC](mavlinkcom_design/images/example4.png) diff --git a/MavLinkCom/Design/Design.dgml b/docs/mavlinkcom_design/Design.dgml similarity index 100% rename from MavLinkCom/Design/Design.dgml rename to docs/mavlinkcom_design/Design.dgml diff --git a/MavLinkCom/Design/FtpDesign.dgml b/docs/mavlinkcom_design/FtpDesign.dgml similarity index 100% rename from MavLinkCom/Design/FtpDesign.dgml rename to docs/mavlinkcom_design/FtpDesign.dgml diff --git a/MavLinkCom/Design/OffboardControl.dgml b/docs/mavlinkcom_design/OffboardControl.dgml similarity index 100% rename from MavLinkCom/Design/OffboardControl.dgml rename to docs/mavlinkcom_design/OffboardControl.dgml diff --git a/MavLinkCom/Design/images/example1.png b/docs/mavlinkcom_design/images/example1.png similarity index 100% rename from MavLinkCom/Design/images/example1.png rename to docs/mavlinkcom_design/images/example1.png diff --git a/MavLinkCom/Design/images/example2.png b/docs/mavlinkcom_design/images/example2.png similarity index 100% rename from MavLinkCom/Design/images/example2.png rename to docs/mavlinkcom_design/images/example2.png diff --git a/MavLinkCom/Design/images/example3.png b/docs/mavlinkcom_design/images/example3.png similarity index 100% rename from MavLinkCom/Design/images/example3.png rename to docs/mavlinkcom_design/images/example3.png diff --git a/MavLinkCom/Design/images/example4.png b/docs/mavlinkcom_design/images/example4.png similarity index 100% rename from MavLinkCom/Design/images/example4.png rename to docs/mavlinkcom_design/images/example4.png diff --git a/MavLinkCom/Design/images/overview.png b/docs/mavlinkcom_design/images/overview.png similarity index 100% rename from MavLinkCom/Design/images/overview.png rename to docs/mavlinkcom_design/images/overview.png diff --git a/docs/mavlinkcom_mocap.md b/docs/mavlinkcom_mocap.md new file mode 100644 index 000000000..2d49a9baa --- /dev/null +++ b/docs/mavlinkcom_mocap.md @@ -0,0 +1,28 @@ +# Welcome to MavLinkMoCap + +This folder contains the MavLinkMoCap library which connects to a OptiTrack camera system +for accurate indoor location. + +## Dependencies: +* [OptiTrack Motive](http://www.optitrack.com/products/motive/). +* [MavLinkCom](mavlinkcom.md). + +### Setup RigidBody + +First you need to define a RigidBody named 'Quadrocopter' using Motive. +See [Rigid_Body_Tracking](http://wiki.optitrack.com/index.php?title=Rigid_Body_Tracking). + +### MavLinkTest + +Use MavLinkTest to talk to your PX4 drone, with "-server:addr:port", for example, when connected +to drone wifi use: + + MavLinkMoCap -server:10.42.0.228:14590 "-project:D:\OptiTrack\Motive Project 2016-12-19 04.09.42 PM.ttp" + +This publishes the ATT_POS_MOCAP messages and you can proxy those through to the PX4 by running +MavLinkTest on the dronebrain using: + + MavLinkTest -serial:/dev/ttyACM0,115200 -proxy:10.42.0.228:14590 + +Now the drone will get the ATT_POS_MOCAP and you should see the light turn green meaning it is +now has a home position and is ready to fly. \ No newline at end of file diff --git a/docs/meshes.md b/docs/meshes.md new file mode 100644 index 000000000..73b34c206 --- /dev/null +++ b/docs/meshes.md @@ -0,0 +1,79 @@ +# How to Access Meshes in AIRSIM + +AirSim supports the ability to access the static meshes that make up the scene + + +## Mesh structure +Each mesh is represented with the below struct. +``` +struct MeshPositionVertexBuffersResponse { + + Vector3r position; + Quaternionr orientation; + + std::vector vertices; + std::vector indices; + std::string name; +}; +``` +* The position and orientation are in the Unreal coordinate system. +* The mesh itself is a triangular mesh represented by the vertices and the indices. + * The triangular mesh type is typically called a [Face-Vertex](https://en.wikipedia.org/wiki/Polygon_mesh#Face-vertex_meshes) Mesh. This means every triplet of indices hold the indexes of the vertices that make up the triangle/face. + * The x,y,z coordinates of the vertices are all stored in a single vector. This means the vertices vertices is Nx3 where N is number of vertices. + * The position of the vertices are the global positions in the Unreal coordinate system. This means they have already been Transformed by the position and orientation. +* +## How to use +The API to get the meshes in the scene is quite simple. However, one should note that the function call is very expensive and should + very rarely be called. In general this is ok because this function only accesses the static meshes which for most applications are + not changing during the duration of your program. + +Note that you will have to use a 3rdparty library or your own custom code to actually interact with the recieved meshes. Below I utilize the +python bindings of [libigl](https://github.com/libigl/libigl) to visualize the recieved meshes. + +``` +import airsim + +AIRSIM_HOST_IP='127.0.0.1' + +client = airsim.VehicleClient(ip=AIRSIM_HOST_IP) +client.confirmConnection() + +# List of returned meshes are received via this function +meshes=client.simGetMeshPositionVertexBuffers() + + +index=0 +for m in meshes: + # Finds one of the cube meshes in the Blocks environment + if 'cube' in m.name: + + # Code from here on relies on libigl. Libigl uses pybind11 to wrap C++ code. So here the built pyigl.so + # library is in the same directory as this example code. + # This is here as code for your own mesh library should require something similar + from pyigl import * + from iglhelpers import * + + # Convert the lists to numpy arrays + vertex_list=np.array(m.vertices,dtype=np.float32) + indices=np.array(m.indices,dtype=np.uint32) + + num_vertices=int(len(vertex_list)/3) + num_indices=len(indices) + + # Libigl requires the shape to be Nx3 where N is number of vertices or indices + # It also requires the actual type to be double(float64) for vertices and int64 for the triangles/indices + vertices_reshaped=vertex_list.reshape((num_vertices,3)) + indices_reshaped=indices.reshape((int(num_indices/3),3)) + vertices_reshaped=vertices_reshaped.astype(np.float64) + indices_reshaped=indices_reshaped.astype(np.int64) + + #Libigl function to convert to internal Eigen format + v_eig=p2e(vertices_reshaped) + i_eig=p2e(indices_reshaped) + + # View the mesh + viewer = igl.glfw.Viewer() + viewer.data().set_mesh(v_eig,i_eig) + viewer.launch() + break +``` \ No newline at end of file diff --git a/docs/mkdocs.yml b/docs/mkdocs.yml deleted file mode 100644 index ba5f32156..000000000 --- a/docs/mkdocs.yml +++ /dev/null @@ -1,181 +0,0 @@ -dev_addr: 127.0.0.1:8080 -strict: true -site_name: AirSim -site_dir: build -docs_dir: doc_root -repo_url: https://github.com/microsoft/airsim -#site_url: http://microsoft.github.io/airsim -#edit_uri: edit/master/docs -#repo_name: GitHub -site_description: 'Open source simulator based on Unreal Engine for autonomous vehicles from Microsoft AI & Research' - -markdown_extensions: - # - meta - # - toc: - # permalink: true # For ¶ - # #permalink: "#" - # #separator: "_" - # - admonition - # - fenced_code - # - codehilite - # - smarty - # - tables - # - def_list - # - sane_lists - # - codehilite: - # guess_lang: true - # linenums: true - # - mdx_math: - # enable_dollar_delimiter: True - # - pymdownx.highlight: - # guess_lang: true - # extend_pygments_lang: - # - name: php - # lang: php - # options: - # startinline: true - # - pymdownx.arithmatex: - # #inline_syntax: ['dollar'] - # #block_syntax: ['dollar', 'begin'] - # #insert_as_script: True - # # - pymdownx.keys: - # # strict: false - # - pymdownx.betterem: - # smart_enable: all - # - pymdownx.caret - # #- pymdownx.critic - # - pymdownx.details - # - pymdownx.inlinehilite - # - pymdownx.tasklist: - # custom_checkbox: true - # #- pymdownx.magiclink: - # #- base_repo_url: https://github.com/abinit/abinit - # #- repo_url_shortener: true - # - pymdownx.mark - # - pymdownx.smartsymbols: - # registered: false - # copyright: false - # care_of: false - # - pymdownx.superfences - # - pymdownx.tilde - -remote_branch: gh-pages -#remote_name: origin - -theme: - #name: 'rtd-dropdown' - name: 'readthedocs' - # Disable the default top left logo until we sort out our custom one. - logo: - icon: ' ' -# extra_css: -# - css/extra.css -# - css/gallery.css -# - css/my_codehilite.css - - # # Don't include MkDocs' JavaScript - # include_search_page: false - # search_index_only: true - -extra: - version: 1.2.1 - #previous_published_versions: [1.2.1] - #logo: images/logo-abinit-2015.svg - feature: - tabs: true - palette: - #primary: orange - #accent: orange - # primary: deep orange - # accent: deep orange - #primary: indigo - #accent: indigo - #font: - #fonttext: 'Ubuntu' - #fontcode: 'Ubuntu Mono' - social: - - type: facebook - link: https://www.facebook.com/groups/1225832467530667/ - - type: github-alt - link: https://github.com/Microsoft/AirSim - # - type: twitter - # link: https://twitter.com/hashtag/abinit - # - type: youtube - # link: https://www.youtube.com/watch?v=gcbfb_Mteo4 - -copyright: Copyright © 2018 Microsoft Research - -nav: - - "Home": 'README.md' - - "Whats New": 'CHANGELOG.md' - - "Get AirSim": - - "Download Binaries": 'docs/use_precompiled.md' - - "Build on Windows": 'docs/build_windows.md' - - "Build on Linux": 'docs/build_linux.md' - - "Creating Custom Environment": 'docs/unreal_custenv.md' - - "Using AirSim": - - "Core APIs": 'docs/apis.md' - - "Image APIs": 'docs/image_apis.md' - - "C++ APIs": 'docs/apis_cpp.md' - - "Development Workflow": 'docs/dev_workflow.md' - - "Settings": 'docs/settings.md' - - "Camera Views": 'docs/camera_views.md' - - "Using Car": 'docs/using_car.md' - - "Remote Control": 'docs/remote_control.md' - - "XBox Controller": 'docs/xbox_controller.md' - - "Steering Wheel": 'docs/steering_wheel_installation.md' - - "Multiple Vehicles": 'docs/multi_vehicle.md' - - "Sensors": 'docs/sensors.md' - - "LIDAR": 'docs/lidar.md' - - "ROS": 'docs/ros.md' - - "Playing Logs": 'docs/playback.md' - - "Design": - - "Architecture": 'docs/design.md' - - "Code Structure": 'docs/code_structure.md' - - "Coding Guidelines": 'docs/coding_guidelines.md' - - "Flight Controller": 'docs/flight_controller.md' - - "Simple Flight": 'docs/simple_flight.md' - - "Hello Drone": 'docs/hello_drone.md' - - "MavLink and PX4": - - "PX4 Setup for AirSim": 'docs/px4_setup.md' - - "PX4 in SITL": 'docs/px4_sitl.md' - - "AirSim with Pixhawk": 'https://youtu.be/1oY8Qu5maQQ' - - "PX4 Setup with AirSim": 'https://youtu.be/HNWdYrtw3f0' - - "Debugging Attitude Estimation": 'https://www.youtube.com/watch?v=d_FyjKDWQfc&feature=youtu.be' - - "Intercepting MavLink Messages": 'https://github.com/Microsoft/AirSim/wiki/Intercepting-MavLink-messages' - - "Rapid Descent on PX4 Drones": 'https://github.com/Microsoft/AirSim/wiki/Rapid-Descent-on-PX4-drones' - - "Building PX4": "docs/px4_build.md" - - "PX4/MavLink Logging": 'docs/px4_logging.md' - - "MavLink LogViewer": "docs/log_viewer.md" - # - "MavLinkCom": 'MavLinkCom/README.md' - # - "MavLink MoCap": 'MavLinkCom/MavLinkMoCap/Readme.md' - - "Upgrading": - - "Upgrading Unreal": 'docs/unreal_upgrade.md' - - "Upgrading APIs": 'docs/upgrade_apis.md' - - "Upgrading Settings": 'docs/upgrade_settings.md' - - "Contributed Tutorials": - - "Reinforcement Learning": 'docs/reinforcement_learning.md' - - "Using Environments from Marketplace": 'https://www.youtube.com/watch?v=y09VbdQWvQY' - - "Simple Collision Avoidance": 'https://github.com/simondlevy/AirSimTensorFlow' - - "Autonomous Driving on Azure": 'https://aka.ms/AutonomousDrivingCookbook' - - "Building Hexacopter": 'https://github.com/Microsoft/AirSim/wiki/hexacopter' - - "Moving on Path Demo": 'https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo' - - "Building Point Clouds": 'https://github.com/Microsoft/AirSim/wiki/Point-Clouds' - - "Surveying Using Drone": 'https://github.com/Microsoft/AirSim/wiki/Implementing-a-Drone-Survey-script' - - "Misc": - - "AirSim on Real Drones": 'docs/custom_drone.md' - - "Installing cmake on Linux": 'docs/cmake_linux.md' - - "Tips for Busy HDD": 'docs/hard_drive.md' - - "pfm format": 'docs/pfm.md' - - "Setting up Unreal Environment": 'docs/unreal_proj.md' - - "Blocks Environment": 'docs/unreal_blocks.md' - - "Who is Using AirSim": 'docs/who_is_using.md' - - "Working with UE Plugin Contents": 'docs/working_with_plugin_contents.md' - - "Formula Student Technion Self-drive": 'https://github.com/Microsoft/AirSim/wiki/technion' - - "Support": - - "FAQ": 'docs/faq.md' - - "Support": 'SUPPORT.md' - - "Create Issue": 'docs/create_issue.md' - - "Contribute": 'CONTRIBUTING.md' - - diff --git a/docs/orbit.md b/docs/orbit.md new file mode 100644 index 000000000..78209fe7e --- /dev/null +++ b/docs/orbit.md @@ -0,0 +1,21 @@ +# An Orbit Trajectory + +Moved here from [https://github.com/microsoft/AirSim/wiki/An-Orbit-Trajectory](https://github.com/microsoft/AirSim/wiki/An-Orbit-Trajectory) + +Have you ever wanted to fly a nice smooth circular orbit? This can be handy for capturing 3D objects from all sides especially if you get multiple orbits at different altitudes. + +So the `PythonClient/multirotor` folder contains a script named [Orbit](https://github.com/microsoft/AirSim/blob/master/PythonClient/multirotor/orbit.py) that will do precisely that. + +See [demo video](https://youtu.be/RFG5CTQi3Us) + +The demo video was created by running this command line: + +```shell +python orbit.py --radius 10 --altitude 5 --speed 1 --center "0,1" --iterations 1 +``` + +This flies a 10 meter radius orbit around the center location at (startpos + radius * [0,1]), in other words, the center is located `radius` meters away in the direction of the provided center vector. It also keeps the front-facing camera on the drone always pointing at the center of the circle. If you watch the flight using LogViewer you will see a nice circular pattern get traced out on the GPS map: + +![image](images/orbit.png) + +The core of the algorithm is not that complicated. At each point on the circle, we look ahead by a small delta in degrees, called the `lookahead_angle`, where that angle is computed based on our desired velocity. We then find that lookahead point on the circle using sin/cosine and make that our "target point". Calculating the velocity then is easy, just subtract our current position from that point and feed that into the AirSim method `moveByVelocityZ`. diff --git a/docs/point_clouds.md b/docs/point_clouds.md new file mode 100644 index 000000000..eb9e06cc1 --- /dev/null +++ b/docs/point_clouds.md @@ -0,0 +1,17 @@ +# Point Clouds + +Moved here from [https://github.com/microsoft/AirSim/wiki/Point-Clouds](https://github.com/microsoft/AirSim/wiki/Point-Clouds) + +A Python script [point_cloud.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/multirotor/point_cloud.py) shows how to convert the depth image returned from AirSim into a point cloud. + +The following depth image was captured using the Modular Neighborhood environment: + +![depth](images/depth.png) + +And with the appropriate projection matrix, the OpenCV `reprojectImageTo3D` function can turn this into a point cloud. The following is the result, which is also available here: [https://skfb.ly/68r7y](https://skfb.ly/68r7y). + +![depth](images/point_cloud.png) + +[SketchFab](https://sketchfab.com) can upload the resulting file `cloud.asc` and render it for you. + +PS: you may notice the scene is reflected on the Y axis, so I may have a sign wrong in the projection matrix. An exercise for the reader :-) diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md index f8ebb5c0f..ae3702fdf 100644 --- a/docs/px4_sitl.md +++ b/docs/px4_sitl.md @@ -1,9 +1,12 @@ # Setting up PX4 Software-in-Loop -The [PX4](http://dev.px4.io) software provides a "software-in-loop" simulation (SITL) version of their stack that runs in Linux. Sorry it doesn't run in Windows, but if you install [BashOnWindows](https://msdn.microsoft.com/en-us/commandline/wsl/install_guide) -you can build and run it there. +The [PX4](http://dev.px4.io) software provides a "software-in-loop" simulation (SITL) version of their stack that runs in Linux. If you are on Windows then you must +use the [Cygwin Toolchain](https://dev.px4.io/master/en/setup/dev_env_windows_cygwin.html) as the [Bash On Windows](https://dev.px4.io/master/en/setup/dev_env_windows_bash_on_win.html) toolchain no longer works for SITL. -1. From your Linux bash terminal follow [these steps for Linux](http://dev.px4.io/starting-installing-linux.html) and follow **all** the instructions under `NuttX based hardware` to install prerequisites. We've also included out own copy of the [PX4 build instructions](px4_build.md) which is a bit more concise about what we need exactly. +**Note** that every time you stop the unreal app you have to restart the `px4` app. + + +1. From your bash terminal follow [these steps for Linux](http://dev.px4.io/starting-installing-linux.html) and follow **all** the instructions under `NuttX based hardware` to install prerequisites. We've also included out own copy of the [PX4 build instructions](px4_build.md) which is a bit more concise about what we need exactly. 2. Get the PX4 source code and build the posix SITL version of PX4: ``` @@ -11,14 +14,29 @@ you can build and run it there. cd PX4 git clone https://github.com/PX4/Firmware.git cd Firmware - git checkout v1.8.2 # Pick a well known "good" release tag. + git checkout v1.10.1 # recommended version ``` 3. Use following command to build and start PX4 firmware in SITL mode: ``` - make posix_sitl_ekf2 none_iris + make px4_sitl_default none_iris + ``` + If you are using older version v1.8.* use this command instead: `make posix_sitl_ekf2 none_iris`. + +4. You should see a message saying the SITL PX4 app is waiting for the simulator (AirSim) to connect. +You will also see information about which ports are configured for mavlink connection to the PX4 app. +The default ports have changed recently, so check them closely to make sure AirSim settings are correct. + ``` + INFO [simulator] Waiting for simulator to connect on TCP port 4560 + INFO [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0 + INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14570 remote port 14550 + INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540 ``` -4. You should see a message like this you `INFO [simulator] Waiting for initial data on UDP port 14560` which means the SITL PX4 app is waiting for someone to connect. -5. Now edit [AirSim settings](settings.md) file to make sure you have followings: + + Note: this is also an interactive PX4 console, type `help` to see the + list of commands you can enter here. They are mostly low level PX4 + commands, but some of them can be useful for debugging. + +5. Now edit [AirSim settings](settings.md) file to make sure you have matching UDP and TCP port settings: ```json { "SettingsVersion": 1.2, @@ -26,33 +44,74 @@ you can build and run it there. "Vehicles": { "PX4": { "VehicleType": "PX4Multirotor", - "UseSerial": false + "UseSerial": false, + "UseTcp": true, + "TcpPort": 4560, + "ControlPort": 14580, + "params": { + "NAV_RCL_ACT": 0, + "NAV_DLL_ACT": 0, + "LPE_LAT": 47.641468, + "LPE_LON": -122.140165, + "COM_OBL_ACT": 1 + } } } } -} + ``` + Notice the PX4 `[simulator]` is using TCP, which is why we need to add: `"UseTcp": true,`. +6. Now run your Unreal AirSim environment and it should connect to SITL PX4 via TCP. +You should see a bunch of messages from the SITL PX4 window. +Specifically, the following messages tell you that AirSim is connected properly and GPS fusion is stable: ``` -6. Run Unreal environment and it should connect to SITL via UDP. You should see a bunch of messages from the SITL PX4 window from things like `[mavlink]` and `[commander]` and so on. -7. You should also be able to use QGroundControl just like with flight controller hardware. Note that as we don't have physical board, RC cannot be connected directly to it. So the alternatives are either use XBox 360 Controller or connect your RC using USB (for example, in case of FrSky Taranis X9D Plus) or using trainer USB cable to PC. This makes your RC look like joystick. You will need to do extra set up in QGroundControl to use virtual joystick for RC control. + INFO [simulator] Simulator connected on UDP port 14560 + INFO [mavlink] partner IP: 127.0.0.1 + INFO [ecl/EKF] EKF GPS checks passed (WGS-84 origin set) + INFO [ecl/EKF] EKF commencing GPS fusion + ``` + + If you do not see these messages then check your port settings. + +7. You should also be able to use QGroundControl with SITL mode. Make sure +there is no Pixhawk hardware plugged in, otherwise QGroundControl will choose +to use that instead. Note that as we don't have a physical board, an RC cannot be connected directly to it. So the alternatives are either use XBox 360 Controller or connect your RC using USB (for example, in case of FrSky Taranis X9D Plus) or using trainer USB cable to your PC. This makes your RC look like a joystick. You will need to do extra set up in QGroundControl to use virtual joystick for RC control. You do not need to do this unless you plan to fly a drone manually in AirSim. Autonomous flight using the Python +API does not require RC, see `No Remote Control` below. ## Setting GPS origin -PX4 SITL mode needs to be configured to get the home location correct. Run the following in the PX4 console window so that the origin matches that which is setup in AirSim AVehiclePawnBase::HomeLatitude and HomeLongitude. +Notice the above settings are provided in the `params` section of the `settings.json` file: +``` + "LPE_LAT": 47.641468, + "LPE_LON": -122.140165, +``` + +PX4 SITL mode needs to be configured to get the home location correct. +There is a bug in AirSim that makes it such that flight does not work +unless the home location is set to the same coordinates defined in AVehiclePawnBase::HomeLatitude and HomeLongitude. + +You can also run the following in the SITL PX4 console window to check +that these values are set correctly. ```` -param set LPE_LAT 47.641468 -param set LPE_LON -122.140165 +param show LPE_LAT +param show LPE_LON ```` -You might also want to set this one so that the drone automatically hovers after each offboard control command (the default setting is to land): +## Smooth Offboard Transitions + +Notice the above setting is provided in the `params` section of the `settings.json` file: +``` + "COM_OBL_ACT": 1 +``` + +This tells the drone automatically hover after each offboard control command finishes (the default setting is to land). Hovering is a smoother transition between multiple offboard commands. You can check this setting +by running the following PX4 console command: ```` -param set COM_OBL_ACT 1 +param show COM_OBL_ACT ```` -Now close Unreal app, restart the `px4` app and re-start the unreal app. In fact, every time you stop the unreal app you have top restart the `px4` app. - ## Check the Home Position If you are using DroneShell to execute commands (arm, takeoff, etc) then you should wait until the Home position is set. You will see the PX4 SITL console output this message: @@ -78,21 +137,38 @@ If the z coordinate is large like this then takeoff might not work as expected. ## No Remote Control -If you plan to fly with no remote control, just using DroneShell commands for example, then you will need to set the following parameters to stop the PX4 from triggering "failsafe mode on" every time a move command is finished. +Notice the above setting is provided in the `params` section of the `settings.json` file: +``` + "NAV_RCL_ACT": 0, + "NAV_DLL_ACT": 0, +``` + +This is required if you plan to fly the SITL mode PX4 with no remote control, just using python scripts, for example. These parameters stop the PX4 from triggering "failsafe mode on" every time a move command is finished. You can use the following PX4 command to check these values are set correctly: ```` -param set NAV_RCL_ACT 0 -param set NAV_DLL_ACT 0 +param show NAV_RCL_ACT +param show NAV_DLL_ACT ```` NOTE: Do `NOT` do this on a real drone as it is too dangerous to fly without these failsafe measures. +## Manually set parameters + +You can also run the following in the PX4 console to set all these parameters: + +``` +param set LPE_LAT 47.641468 +param set LPE_LON -122.140165 +param set COM_OBL_ACT 1 +param set NAV_RCL_ACT 0 +param set NAV_DLL_ACT 0 +``` + ## Using VirtualBox Ubuntu If you want to run the above posix_sitl in a `VirtualBox Ubuntu` machine then it will have a different ip address from localhost. So in this case you need to edit the [settings file](settings.md) and change the UdpIp and SitlIp to the ip address of your virtual machine -set the LocalIpAddress to the address of your host machine running the Unreal engine. +set the LocalIpAddress to the address of your host machine running the Unreal engine. ## Remote Controller There are several options for flying the simulated drone using a remote control or joystick like xbox gamepad. See [remote controllers](remote_control.md#RC_Setup_for_PX4) - diff --git a/docs/python.md b/docs/python.md deleted file mode 100644 index caf1354e0..000000000 --- a/docs/python.md +++ /dev/null @@ -1,3 +0,0 @@ -# Using Python APIs for AirSim - -This document is now merged with [general APIs document](apis.md). \ No newline at end of file diff --git a/docs/remote_control.md b/docs/remote_control.md index 06136c7fb..b50632e50 100644 --- a/docs/remote_control.md +++ b/docs/remote_control.md @@ -8,12 +8,13 @@ By default AirSim uses [simple_flight](simple_flight.md) as its flight controlle You can either use XBox controller or [FrSky Taranis X9D Plus](https://hobbyking.com/en_us/frsky-2-4ghz-accst-taranis-x9d-plus-and-x8r-combo-digital-telemetry-radio-system-mode-2.html). Note that XBox 360 controller is not precise enough and is not recommended if you wanted more real world experience. See FAQ below if things are not working. - ### Other Devices - AirSim can detect large variety of devices however devices other than above *might* need extra configuration. In future we will add ability to set this config through settings.json. For now, if things are not working then you might want to try workarounds such as [x360ce](http://www.x360ce.com/) or chnage code in [SimJoystick.cpp file](/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp#L50). +### Other Devices - ### Note on FrSky Taranis X9D Plus +AirSim can detect large variety of devices however devices other than above *might* need extra configuration. In future we will add ability to set this config through settings.json. For now, if things are not working then you might want to try workarounds such as [x360ce](http://www.x360ce.com/) or change code in [SimJoystick.cpp file](/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp#L50). - [FrSky Taranis X9D Plus](https://hobbyking.com/en_us/frsky-2-4ghz-accst-taranis-x9d-plus-and-x8r-combo-digital-telemetry-radio-system-mode-2.html) is real UAV remote control with an advantage that it has USB port so it can be directly connected to PC. You can [download AirSim config file](misc/AirSim_FrSkyTaranis.bin) and [follow this tutorial](https://www.youtube.com/watch?v=qe-13Gyb0sw) to import it in your RC. You should then see "sim" model in RC with all channels configured properly. +### Note on FrSky Taranis X9D Plus + +[FrSky Taranis X9D Plus](https://hobbyking.com/en_us/frsky-2-4ghz-accst-taranis-x9d-plus-and-x8r-combo-digital-telemetry-radio-system-mode-2.html) is real UAV remote control with an advantage that it has USB port so it can be directly connected to PC. You can [download AirSim config file](misc/AirSim_FrSkyTaranis.bin) and [follow this tutorial](https://www.youtube.com/watch?v=qe-13Gyb0sw) to import it in your RC. You should then see "sim" model in RC with all channels configured properly. ### Note on Linux Currently default config on Linux is for using Xbox controller. This means other devices might not work properly. In future we will add ability to configure RC in settings.json but for now you *might* have to change code in [SimJoystick.cpp file](/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp#L340) to use other devices. @@ -24,7 +25,7 @@ AirSim supports PX4 flight controller however it requires different setup. There 1. If you are going to use Hardware-in-Loop mode, you need transmitter for your specific brand of RC and bind it. You can find this information in RC's user guide. 2. For Hardware-in-Loop mode, you connect transmitter to Pixhawk. Usually you can find online doc or YouTube video tutorial on how to do that. -2. [Calibrate your RC in QGroundControl](https://docs.qgroundcontrol.com/en/SetupView/Radio.html). +3. [Calibrate your RC in QGroundControl](https://docs.qgroundcontrol.com/en/SetupView/Radio.html). See [PX4 RC configuration](https://docs.px4.io/en/getting_started/rc_transmitter_receiver.html) and Please see [this guide](http://ardupilot.org/copter/docs/common-pixhawk-and-px4-compatible-rc-transmitter-and-receiver-systems.html) for more information. @@ -71,4 +72,4 @@ We haven't implemented it yet. This means your RC firmware will need to have a c #### My RC is not working with PX4 setup. -First you want to make sure your RC is working in [QGroundControl](https://docs.qgroundcontrol.com/en/SetupView/Radio.html). If it doesn't then it will sure not work in AirSim. The PX4 mode is suitable for folks who have at least intermediate level of experience to deal with various issues related to PX4 and we would generally refer you to get help from PX4 forums. \ No newline at end of file +First you want to make sure your RC is working in [QGroundControl](https://docs.qgroundcontrol.com/en/SetupView/Radio.html). If it doesn't then it will sure not work in AirSim. The PX4 mode is suitable for folks who have at least intermediate level of experience to deal with various issues related to PX4 and we would generally refer you to get help from PX4 forums. diff --git a/docs/retexturing.md b/docs/retexturing.md new file mode 100644 index 000000000..7c24f0239 --- /dev/null +++ b/docs/retexturing.md @@ -0,0 +1,70 @@ +# Runtime Texture Swapping + +## How to Make An Actor Retexturable + +To be made texture-swappable, an actor must derive from the parent class TextureShuffleActor. +The parent class can be set via the settings tab in the actor's blueprint. + +![Parent Class](images/tex_shuffle_actor.png) + +After setting the parent class to TextureShuffActor, the object gains the member DynamicMaterial. +DynamicMaterial needs to be set--on all actor instances in the scene--to TextureSwappableMaterial. +Warning: Statically setting the Dynamic Material in the blueprint class may cause rendering errors. It seems to work better to set it on all the actor instances in the scene, using the details panel. + +![TextureSwappableMaterial](images/tex_swap_material.png) + +## How to Define the Set(s) of Textures to Choose From + +Typically, certain subsets of actors will share a set of texture options with each other. (e.g. walls that are part of the same building) + +It's easy to set up these groupings by using Unreal Engine's group editing functionality. +Select all the instances that should have the same texture selection, and add the textures to all of them simultaneously via the Details panel. +Use the same technique to add descriptive tags to groups of actors, which will be used to address them in the API. + +![Group Editing](images/tex_swap_group_editing.png) + +It's ideal to work from larger groupings to smaller groupings, simply deselecting actors to narrow down the grouping as you go, and applying any individual actor properties last. + +![Subset Editing](images/tex_swap_subset.png) + +## How to Swap Textures from the API + +The following API is available in C++ and python. (C++ shown) + +```C++ +std::vector simSwapTextures(const std::string& tags, int tex_id); +``` + +The string of "," or ", " delimited tags identifies on which actors to perform the swap. +The tex_id indexes the array of textures assigned to each actor undergoing a swap. +The function will return the list of objects which matched the provided tags and had the texture swap perfomed. +If tex_id is out-of-bounds for some object's texture set, it will be taken modulo the number of textures that were available. + +Demo (Python): + +```Python +import airsim +import time + +c = airsim.client.MultirotorClient() +print(c.simSwapTextures("furniture", 0)) +time.sleep(2) +print(c.simSwapTextures("chair", 1)) +time.sleep(2) +print(c.simSwapTextures("table", 1)) +time.sleep(2) +print(c.simSwapTextures("chair, right", 0)) +``` + +Results: + +```bash +['RetexturableChair', 'RetexturableChair2', 'RetexturableTable'] +['RetexturableChair', 'RetexturableChair2'] +['RetexturableTable'] +['RetexturableChair2'] +``` + +![Demo](images/tex_swap_demo.gif) + +Note that in this example, different textures were chosen on each actor for the same index value. \ No newline at end of file diff --git a/docs/ros.md b/docs/ros.md deleted file mode 100644 index dfc143496..000000000 --- a/docs/ros.md +++ /dev/null @@ -1,58 +0,0 @@ -# How to use AirSim with Robot Operating System (ROS) - -AirSim and ROS can be integrated using C++ or Python. Some example ROS nodes are provided demonstrating how to publish data from AirSim as ROS topics. - -# Python - -## Prerequisites - -These instructions are for Ubuntu 16.04, ROS Kinetic, UE4 4.18 or higher, and latest AirSim release. -You should have these components installed and working before proceeding - -## Setup - - -### Create a new ROS package in your catkin workspace following these instructions. - -Create a new ROS package called airsim or whatever you like. - -[Create ROS package](http://wiki.ros.org/ROS/Tutorials/CreatingPackage) - -If you don't already have a catkin workspace, you should first work through the ROS beginner tutorials. - -### Add AirSim ROS node examples to ROS package - -In the ROS package directory you made, copy the ros examples from the AirSim/PythonClient directory to your ROS package. Change the code below to match your AirSim and catkin workspace paths. - -``` -# copy package -mkdir -p ../catkin_ws/src/airsim/scripts/airsim -cp AirSim/PythonClient/airsim/*.py ../catkin_ws/src/airsim/scripts/airsim - -# copy ROS examples -cp AirSim/PythonClient/ros/*.py ../catkin_ws/src/airsim/scripts -``` - - -### Build ROS AirSim package - -Change directory to your top level catkin workspace folder i.e. ```cd ~/catkin_ws``` and run ```catkin_make``` -This will build the airsim package. Next, run ```source devel/setup.bash``` so ROS can find the new package. -You can add this command to your ~/.bashrc to load your catkin workspace automatically. - -## How to run ROS AirSim nodes - -First make sure UE4 is running an airsim project, the car or drone should be selected, and the simulations is playing. -Examples support car or drone. Make sure to have the correct vehicle for the ros example running. - -The example airsim nodes can be run using ```rosrun airsim example_name.py``` The output of the node -can be viewed in another terminal by running ```rostopic echo /example_name``` You can view a list of the -topics currently published via tab completion after typing ```rostopic echo``` in the terminal. -Rviz is a useful visualization tool that can display the published data. - -### Troubleshooting - -In the case of ```rosrun airsim example_name.py``` returning ```Couldn't find executable named...``` you may ```chmod +x example_name.py``` to tell the system that this is executable. - - -# C++ (coming soon) diff --git a/docs/sensors.md b/docs/sensors.md index e64f4ed2b..5f81bf037 100644 --- a/docs/sensors.md +++ b/docs/sensors.md @@ -1,6 +1,6 @@ # Sensors in AirSim -AirSim currently supports the following sensors. +AirSim currently supports the following sensors. Each sensor is associated with a integer enum specifying its sensor type. * Camera @@ -8,14 +8,14 @@ Each sensor is associated with a integer enum specifying its sensor type. * Imu = 2 * Gps = 3 * Magnetometer = 4 -* Distance Sensor = 5 +* Distance Sensor = 5 * Lidar = 6 -**Note** : Cameras are configured differently than the other sensors and do not have an enum associated with them. Look at [general settings](settings.md) and [image API](image_apis.md) for camera config and API. +**Note** : Cameras are configured differently than the other sensors and do not have an enum associated with them. Look at [general settings](settings.md) and [image API](image_apis.md) for camera config and API. ## Default sensors -If no sensors are specified in the `settings json`, the the following sensors are enabled by default based on the sim mode. +If no sensors are specified in the `settings.json`, the the following sensors are enabled by default based on the sim mode. ### Multirotor * Imu @@ -29,13 +29,13 @@ If no sensors are specified in the `settings json`, the the following sensors ar ### ComputerVision * None -Behind the scenes, 'createDefaultSensorSettings' method in [AirSimSettings.hpp](https://github.com/Microsoft/AirSim/blob/master/AirLib/include/common/AirSimSettings.hpp) which sets up the above sensors with their default parameters, depending on the sim mode specified in the `settings.json` file. +Behind the scenes, `createDefaultSensorSettings` method in [AirSimSettings.hpp](https://github.com/Microsoft/AirSim/blob/master/AirLib/include/common/AirSimSettings.hpp) sets up the above sensors with their default parameters, depending on the sim mode specified in the `settings.json` file. ## Configuring the default sensor list The default sensor list can be configured in settings json: -```JSON +```json "DefaultSensors": { "Barometer": { "SensorType": 1, @@ -57,7 +57,7 @@ The default sensor list can be configured in settings json: "SensorType": 5, "Enabled" : true }, - "Lidar2": { + "Lidar2": { "SensorType": 6, "Enabled" : true, "NumberOfChannels": 4, @@ -68,11 +68,11 @@ The default sensor list can be configured in settings json: ## Configuring vehicle-specific sensor list -If a vehicle provides its sensor list, it **must** provide the whole list. Selective add/remove/update of the default sensor list is **NOT** supported. +If a vehicle provides its sensor list, it **must** provide the whole list. Selective add/remove/update of the default sensor list is **NOT** supported. A vehicle specific sensor list can be specified in the vehicle settings part of the json. e.g., -``` +```json "Vehicles": { "Drone1": { @@ -80,7 +80,7 @@ e.g., "AutoCreate": true, ... "Sensors": { - "MyLidar1": { + "MyLidar1": { "SensorType": 6, "Enabled" : true, "NumberOfChannels": 16, @@ -88,7 +88,7 @@ e.g., "X": 0, "Y": 0, "Z": -1, "DrawDebugPoints": true }, - "MyLidar2": { + "MyLidar2": { "SensorType": 6, "Enabled" : true, "NumberOfChannels": 4, @@ -102,11 +102,49 @@ e.g., ``` ### Sensor specific settings -Each sensor-type has its own set of settings as well. +Each sensor-type has its own set of settings as well. Please see [lidar](lidar.md) for example of Lidar specific settings. -## Sensor APIs -Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/master/PythonClient/multirotor/hello_drone.py) or [`hello_drone.cpp`](https://github.com/Microsoft/AirSim/blob/master/HelloDrone/main.cpp) for example usage, or see follow below for the full API. +#### Distance Sensor + +By default, Distance Sensor points to the front of the vehicle. It can be pointed in any direction by modifying the settings + +Configurable Parameters - + +Parameter | Description +-----------------|------------ +X Y Z | Position of the sensor relative to the vehicle (in NED, in meters) (Default (0,0,0)-Multirotor, (0,0,-1)-Car) +Yaw Pitch Roll | Orientation of the sensor relative to the vehicle (degrees) (Default (0,0,0)) +MinDistance | Minimum distance measured by distance sensor (metres, only used to fill Mavlink message for PX4) (Default 0.2m) +MaxDistance | Maximum distance measured by distance sensor (metres) (Default 40.0m) + +For example, to make the sensor point towards the ground (for altitude measurement similar to barometer), the orientation can be modified as follows - + +```json +"Distance": { + "SensorType": 5, + "Enabled" : true, + "Yaw": 0, "Pitch": -90, "Roll": 0 +} +``` + +**Note:** For Cars, the sensor is placed 1 meter above the vehicle center by default. This is required since otherwise the sensor gives strange data due it being inside the vehicle. This doesn't affect the sensor values say when measuring the distance between 2 cars. See [`PythonClient/car/distance_sensor_multi.py`](https://github.com/Microsoft/AirSim/blob/master/PythonClient/car/distance_sensor_multi.py) for an example usage. + +##### Server side visualization for debugging + +Be default, the points hit by distance sensor are not drawn on the viewport. To enable the drawing of hit points on the viewport, please enable setting `DrawDebugPoints` via settings json. E.g. - + +```json +"Distance": { + "SensorType": 5, + "Enabled" : true, + ... + "DrawDebugPoints": true +} +``` + +## Sensor APIs +Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/master/PythonClient/multirotor/hello_drone.py) or [`hello_drone.cpp`](https://github.com/Microsoft/AirSim/blob/master/HelloDrone/main.cpp) for example usage, or see follow below for the full API. - Barometer @@ -117,19 +155,19 @@ Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/mas Python ```python - barometer_data = getBarometerData(barometer_name = "", vehicle_name = "") + barometer_data = client.getBarometerData(barometer_name = "", vehicle_name = "") ``` - IMU -C++ + C++ ```cpp msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = ""); ``` Python ```python - imu_data = getImuData(imu_name = "", vehicle_name = "") + imu_data = client.getImuData(imu_name = "", vehicle_name = "") ``` - GPS @@ -140,7 +178,7 @@ C++ ``` Python ```python - gps_data = getGpsData(gps_name = "", vehicle_name = "") + gps_data = client.getGpsData(gps_name = "", vehicle_name = "") ``` - Magnetometer @@ -151,19 +189,19 @@ C++ ``` Python ```python - magnetometer_data = getMagnetometerData(magnetometer_name = "", vehicle_name = "") + magnetometer_data = client.getMagnetometerData(magnetometer_name = "", vehicle_name = "") ``` - Distance sensor C++ ```cpp - msr::airlib::DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = ""); + msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = ""); ``` Python ```python - distance_sensor_name = getDistanceSensorData(distance_sensor_name = "", vehicle_name = "") + distance_sensor_data = client.getDistanceSensorData(distance_sensor_name = "", vehicle_name = "") ``` -- Lidar +- Lidar See [lidar](lidar.md) for Lidar API. diff --git a/docs/settings.md b/docs/settings.md index aa6f0f18c..2d827cae4 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -1,10 +1,18 @@ # AirSim Settings ## Where are Settings Stored? -Windows: `Documents\AirSim` -Linux: `~/Documents/AirSim` +AirSim is searching for the settings definition in 4 different ways in the following order. The first match will be used: -The file is in usual [json format](https://en.wikipedia.org/wiki/JSON). On first startup AirSim would create `settings.json` file with no settings. To avoid problems, always use ASCII format to save json file. +1. Looking at the (absolute) path specified by the `--settings` command line argument. +For example, in Windows: `AirSim.exe --settings 'C:\path\to\settings.json'` +In Linux `./Blocks.sh --settings '/home/$USER/path/to/settings.json'` +1. Looking for a json document passed as a command line argument by the `--settings` argument. +For example, in Windows: `AirSim.exe --settings '{"foo" : "bar"}'` +In Linux `./Blocks.sh --settings '{"foo" : "bar"}'` +1. Looking in the folder of the executable for a file called `settings.json`. +2. Looking in the users home folder for a file called `settings.json`. The AirSim subfolder is located at `Documents\AirSim` on Windows and `~/Documents/AirSim` on Linux systems. + +The file is in usual [json format](https://en.wikipedia.org/wiki/JSON). On first startup AirSim would create `settings.json` file with no settings at the users home folder. To avoid problems, always use ASCII format to save json file. ## How to Chose Between Car and Multirotor? The default is to use multirotor. To use car simple set `"SimMode": "Car"` like this: @@ -71,15 +79,15 @@ Below are complete list of settings available along with their default values. I "RandSize": 500.0, "RandDensity": 2, - "HorzWaveContrib":0.03, + "HorzWaveContrib":0.03, "HorzWaveStrength": 0.08, "HorzWaveVertSize": 1.0, "HorzWaveScreenSize": 1.0, - + "HorzNoiseLinesContrib": 1.0, "HorzNoiseLinesDensityY": 0.01, "HorzNoiseLinesDensityXY": 0.5, - + "HorzDistortionContrib": 1.0, "HorzDistortionStrength": 0.002 } @@ -89,7 +97,7 @@ Below are complete list of settings available along with their default values. I "Pitch": NaN, "Roll": NaN, "Yaw": NaN } "X": NaN, "Y": NaN, "Z": NaN, - "Pitch": NaN, "Roll": NaN, "Yaw": NaN + "Pitch": NaN, "Roll": NaN, "Yaw": NaN }, "OriginGeopoint": { "Latitude": 47.641468, @@ -106,7 +114,7 @@ Below are complete list of settings available along with their default values. I "SubWindows": [ {"WindowID": 0, "CameraName": "0", "ImageType": 3, "Visible": false}, {"WindowID": 1, "CameraName": "0", "ImageType": 5, "Visible": false}, - {"WindowID": 2, "CameraName": "0", "ImageType": 0, "Visible": false} + {"WindowID": 2, "CameraName": "0", "ImageType": 0, "Visible": false} ], "SegmentationSettings": { "InitMethod": "", @@ -132,7 +140,7 @@ Below are complete list of settings available along with their default values. I "RemoteControlID": 0, "AllowAPIWhenDisconnected": false }, - "Cameras": { + "Cameras": { //same elements as CameraDefaults above, key as name }, "X": NaN, "Y": NaN, "Z": NaN, @@ -148,29 +156,29 @@ Below are complete list of settings available along with their default values. I "RC": { "RemoteControlID": -1 }, - "Cameras": { + "Cameras": { "MyCamera1": { //same elements as elements inside CameraDefaults above }, "MyCamera2": { //same elements as elements inside CameraDefaults above - }, + }, }, "X": NaN, "Y": NaN, "Z": NaN, - "Pitch": NaN, "Roll": NaN, "Yaw": NaN + "Pitch": NaN, "Roll": NaN, "Yaw": NaN } } } ``` ## SimMode -SimMode determines which simulation mode will be used. Below are currently supported values: +SimMode determines which simulation mode will be used. Below are currently supported values: - `""`: prompt user to select vehicle type multirotor or car - `"Multirotor"`: Use multirotor simulation - `"Car"`: Use car simulation - `"ComputerVision"`: Use only camera, no vehicle or physics -## ViewMode +## ViewMode The ViewMode determines which camera to use as default and how camera will follow the vehicle. For multirotors, the default ViewMode is `"FlyWithMe"` while for cars the default ViewMode is `"SpringArmChase"`. * `FlyWithMe`: Chase the vehicle from behind with 6 degrees of freedom @@ -212,7 +220,7 @@ The `InitMethod` determines how object IDs are initialized at startup to generat If `OverrideExisting` is false then initialization does not alter non-zero object IDs already assigned otherwise it does. - If `MeshNamingMethod` is "" or "OwnerName" then we use mesh's owner name to generate random hash as object IDs. If its "StaticMeshName" then we use static mesh's name to generate random hash as object IDs. Note that it is not possible to tell individual instances of the same static mesh apart this way, but the names are often more intuitive. + If `MeshNamingMethod` is "" or "OwnerName" then we use mesh's owner name to generate random hash as object IDs. If it is "StaticMeshName" then we use static mesh's name to generate random hash as object IDs. Note that it is not possible to tell individual instances of the same static mesh apart this way, but the names are often more intuitive. ## Camera Settings The `CameraDefaults` element at root level specifies defaults used for all cameras. These defaults can be overridden for individual camera in `Cameras` element inside `Vehicles` as described later. @@ -225,7 +233,7 @@ For example, `CaptureSettings` element is json array so you can add settings for ### CaptureSettings The `CaptureSettings` determines how different image types such as scene, depth, disparity, surface normals and segmentation views are rendered. The Width, Height and FOV settings should be self explanatory. The AutoExposureSpeed decides how fast eye adaptation works. We set to generally high value such as 100 to avoid artifacts in image capture. Similarly we set MotionBlurAmount to 0 by default to avoid artifacts in ground truth images. The `ProjectionMode` decides the projection used by the capture camera and can take value "perspective" (default) or "orthographic". If projection mode is "orthographic" then `OrthoWidth` determines width of projected area captured in meters. -For explanation of other settings, please see [this article](https://docs.unrealengine.com/latest/INT/Engine/Rendering/PostProcessEffects/AutomaticExposure/). +For explanation of other settings, please see [this article](https://docs.unrealengine.com/latest/INT/Engine/Rendering/PostProcessEffects/AutomaticExposure/). ### NoiseSettings The `NoiseSettings` allows to add noise to the specified image type with a goal of simulating camera sensor noise, interference and other artifacts. By default no noise is added, i.e., `Enabled: false`. If you set `Enabled: true` then following different types of noise and interference artifacts are enabled, each can be further tuned using setting. The noise effects are implemented as shader created as post processing material in Unreal Engine called [CameraSensorNoise](https://github.com/Microsoft/AirSim/blob/master/Unreal/Plugins/AirSim/Content/HUDAssets/CameraSensorNoise.uasset). @@ -313,6 +321,8 @@ The defaults for PX4 is to enable hardware-in-loop setup. There are various othe "PX4": { "VehicleType": "PX4Multirotor", + "ControlIp": "127.0.0.1", + "ControlPort": 14580, "LogViewerHostIp": "127.0.0.1", "LogViewerPort": 14388, "OffboardCompID": 1, @@ -323,11 +333,11 @@ The defaults for PX4 is to enable hardware-in-loop setup. There are various othe "SerialPort": "*", "SimCompID": 42, "SimSysID": 142, - "SitlIp": "127.0.0.1", - "SitlPort": 14556, + "TcpPort": 4560, "UdpIp": "127.0.0.1", "UdpPort": 14560, "UseSerial": true, + "UseTcp": false, "VehicleCompID": 1, "VehicleSysID": 135, "Model": "Generic", @@ -336,15 +346,21 @@ The defaults for PX4 is to enable hardware-in-loop setup. There are various othe } ``` -These settings define the MavLink SystemId and ComponentId for the Simulator (SimSysID, SimCompID), and for an optional external renderer (ExtRendererSysID, ExtRendererCompID) -and the node that allows remote control of the drone from another app this is called the Air Control node (AirControlSysID, AirControlCompID). +These settings define the MavLink SystemId and ComponentId for the Simulator (SimSysID, SimCompID), and for the vehicle (VehicleSysID, VehicleCompID) +and the node that allows remote control of the drone from another app this is called the offboard node (OffboardSysID, OffboardCompID). If you want the simulator to also talk to your ground control app (like QGroundControl) you can also set the UDP address for that in case you want to run -that on a different machine (QgcHostIp,QgcPort). +that on a different machine (QgcHostIp, QgcPort). The default is local host so QGroundControl should "just work" if it is running on the same machine. + +You can connect the simulator to the LogViewer app, provided in this repo, by setting the UDP address for that (LogViewerHostIp, LogViewerPort). -You can connect the simulator to the LogViewer app, provided in this repo, by setting the UDP address for that (LogViewerHostIp,LogViewerPort). +And for each flying drone added to the simulator there is a named block of additional settings. In the above you see the default name "PX4". You can change this name from the Unreal Editor when you add a new BP_FlyingPawn asset. You will see these properties grouped under the category "MavLink". The MavLink node for this pawn can be remote over UDP or it can be connected to a local serial port. If serial then set UseSerial to true, otherwise set UseSerial to false. For serial connections you also need to set the appropriate SerialBaudRate. The default of 115200 works with Pixhawk version 2 over USB. -And for each flying drone added to the simulator there is a named block of additional settings. In the above you see the default name "PX4". You can change this name from the Unreal Editor when you add a new BP_FlyingPawn asset. You will see these properties grouped under the category "MavLink". The MavLink node for this pawn can be remote over UDP or it can be connected to a local serial port. If serial then set UseSerial to true, otherwise set UseSerial to false and set the appropriate bard rate. The default of 115200 works with Pixhawk version 2 over USB. +When communicating with the PX4 drone over serial port both the HIL_* messages and vehicle control messages share the same serial port. +When communicating over UDP or TCP PX4 requires two separate channels. If UseTcp is false, then UdpIp, UdpPort are used to send HIL_* messages, +otherwise the TcpPort is used. TCP support in PX4 was added in 1.9.2 with the `lockstep` feature because the guarantee of message delivery that +TCP provides is required for the proper functioning of lockstep. AirSim becomes a TCP server in that case, and waits for a connection +from the PX4 app. The second channel for controlling the vehicle is defined by (ControlIp, ControlPort) and is always a UDP channel. ## Other Settings @@ -352,7 +368,7 @@ And for each flying drone added to the simulator there is a named block of addit To turn off the engine sound use [setting](settings.md) `"EngineSound": false`. Currently this setting applies only to car. ### PawnPaths -This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your own Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"Class'/Game/MyCar/MySedanBP.MySedanBP_C'"}`. The `XYZ.XYZ_C` is a special notation required to specify class for BP `XYZ`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then choosing "Car Pawn" for Parent Class settings in Class Options. It's also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. Please make sure your asset is included for cooking in packaging options if you are creating binary. +This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your own Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"Class'/Game/MyCar/MySedanBP.MySedanBP_C'"}`. The `XYZ.XYZ_C` is a special notation required to specify class for BP `XYZ`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then choosing "Car Pawn" for Parent Class settings in Class Options. It is also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. Please make sure your asset is included for cooking in packaging options if you are creating binary. ### PhysicsEngineName For cars, we support only PhysX for now (regardless of value in this setting). For multirotors, we support `"FastPhysicsEngine"` only. @@ -362,11 +378,11 @@ Now when connecting to remote machines you may need to pick a specific Ethernet over Ethernet or over Wi-Fi, or some other special virtual adapter or a VPN. Your PC may have multiple networks, and those networks might not be allowed to talk to each other, in which case the UDP messages from one network will not get through to the others. -So the LocalHostIp allows you to configure how you are reaching those machines. The default of 127.0.0.1 is not able to reach external machines, +So the LocalHostIp allows you to configure how you are reaching those machines. The default of 127.0.0.1 is not able to reach external machines, this default is only used when everything you are talking to is contained on a single PC. ### SpeedUnitFactor Unit conversion factor for speed related to `m/s`, default is 1. Used in conjunction with SpeedUnitLabel. This may be only used for display purposes for example on-display speed when car is being driven. For example, to get speed in `miles/hr` use factor 2.23694. ### SpeedUnitLabel -Unit label for speed, default is `m/s`. Used in conjunction with SpeedUnitFactor. \ No newline at end of file +Unit label for speed, default is `m/s`. Used in conjunction with SpeedUnitFactor. diff --git a/docs/unity_api_support.md b/docs/unity_api_support.md index 56049ef8f..9e01f25a6 100644 --- a/docs/unity_api_support.md +++ b/docs/unity_api_support.md @@ -47,22 +47,6 @@ | rotateToYaw | TODO | TODO | | setCameraOrientation | TODO | TODO | | setRCData | TODO | TODO | -| simCharGetAvailableFaceExpressions | Not Supported | Not Supported | -| simCharGetBonePose | Not Supported | Not Supported | -| simCharGetBonePoses | Not Supported | Not Supported | -| simCharGetFaceExpression | Not Supported | Not Supported | -| simCharGetHeadRotation | Not Supported | Not Supported | -| simCharGetSkinAgeing | Not Supported | Not Supported | -| simCharGetSkinDarkness | Not Supported | Not Supported | -| simCharResetBonePose | Not Supported | Not Supported | -| simCharSetBonePose | Not Supported | Not Supported | -| simCharSetBonePoses | Not Supported | Not Supported | -| simCharSetFaceExpression | Not Supported | Not Supported | -| simCharSetFacePreset | Not Supported | Not Supported | -| simCharSetFacePresets | Not Supported | Not Supported | -| simCharSetHeadRotation | Not Supported | Not Supported | -| simCharSetSkinAgeing | Not Supported | Not Supported | -| simCharSetSkinDarkness | Not Supported | Not Supported | | simContinueForTime | Supported | Supported | | simGetCameraInfo | TODO | TODO | | simGetGroundTruthEnvironment | TODO | TODO | diff --git a/docs/unreal_blocks.md b/docs/unreal_blocks.md index 32d7de482..4a4f6795b 100644 --- a/docs/unreal_blocks.md +++ b/docs/unreal_blocks.md @@ -9,7 +9,7 @@ Here are quick steps to get Blocks environment up and running: 1. Make sure you have [installed Unreal and built AirSim](build_windows.md). 2. Navigate to folder `AirSim\Unreal\Environments\Blocks` and run `update_from_git.bat`. -3. Double click on generated .sln file to open in Visual Studio 2017 or newer. +3. Double click on generated .sln file to open in Visual Studio 2019 or newer. 4. Make sure `Blocks` project is the startup project, build configuration is set to `DebugGame_Editor` and `Win64`. Hit F5 to run. 5. Press the Play button in Unreal Editor and you will see something like in below video. Also see [how to use AirSim](https://github.com/Microsoft/AirSim/#how-to-use-it). @@ -22,7 +22,7 @@ For Windows, you can just change the code in Visual Studio, press F5 and re-run. 3. On first start you might not see any projects in UE4 editor. Click on Projects tab, Browse button and then navigate to `AirSim/Unreal/Environments/Blocks/Blocks.uproject`. 4. If you get prompted for incompatible version and conversion, select In-place conversion which is usually under "More" options. If you get prompted for missing modules, make sure to select No so you don't exit. 5. Finally, when prompted with building AirSim, select Yes. Now it might take a while so go get some coffee :). -6. Press the Play button in Unreal Editor and you will see something like in below video. Also see [how to use AirSim](/#how-to-use-it). +6. Press the Play button in Unreal Editor and you will see something like in below video. Also see [how to use AirSim](https://github.com/microsoft/AirSim/#how-to-use-it). [![Blocks Demo Video](images/blocks_video.png)](https://www.youtube.com/watch?v=-r_QGaxMT4A) diff --git a/docs/unreal_custenv.md b/docs/unreal_custenv.md index 37da8cd23..d327b0b83 100644 --- a/docs/unreal_custenv.md +++ b/docs/unreal_custenv.md @@ -8,7 +8,7 @@ There is no `Epic Games Launcher` for Linux which means that if you need to crea ## Step by Step Instructions -1. Make sure AirSim is built and Unreal 4.18 is installed as described in [build instructions](build_windows.md). +1. Make sure AirSim is built and Unreal 4.24 is installed as described in [build instructions](build_windows.md). 2. In `Epic Games Launcher` click the Learn tab then scroll down and find `Landscape Mountains`. Click the `Create Project` and download this content (~2GB download). ![current version](images/landscape_mountains.png) @@ -26,7 +26,7 @@ There is no `Epic Games Launcher` for Linux which means that if you need to crea ``` { "FileVersion": 3, - "EngineAssociation": "4.18", + "EngineAssociation": "4.24", "Category": "Samples", "Description": "", "Modules": [ diff --git a/docs/unreal_upgrade.md b/docs/unreal_upgrade.md index 28599ddcf..95c002e80 100644 --- a/docs/unreal_upgrade.md +++ b/docs/unreal_upgrade.md @@ -1,4 +1,4 @@ -# Upgrading to Unreal Engine 4.18 +# Upgrading to Unreal Engine 4.24 These instructions apply if you are already using AirSim on Unreal Engine 4.16. If you have never installed AirSim, please see [How to get it](https://github.com/microsoft/airsim#how-to-get-it). @@ -7,20 +7,22 @@ These instructions apply if you are already using AirSim on Unreal Engine 4.16. ## Do this first ### For Windows Users -1. Install Visual Studio 2017 with VC++, Python and C#. -2. Install UE 4.18 through Epic Games Launcher. -3. Start `x64 Native Tools Command Prompt for VS 2017` and navigate to AirSim repo. +1. Install Visual Studio 2019 with VC++, Python and C#. +2. Install UE 4.24 through Epic Games Launcher. +3. Start `x64 Native Tools Command Prompt for VS 2019` and navigate to AirSim repo. 4. Run `clean_rebuild.bat` to remove all unchecked/extra stuff and rebuild everything. +5. See also [Build AirSim on Windows](build_windows.md) for more information. ### For Linux Users 1. From your AirSim repo folder, run 'clean_rebuild.sh`. 2. Rename or delete your existing folder for Unreal Engine. -3. Follow step 1 and 2 to [install Unreal Engine 4.18](https://github.com/Microsoft/AirSim/blob/master/docs/build_linux.md#install-and-build). +3. Follow step 1 and 2 to [install Unreal Engine 4.24](https://github.com/Microsoft/AirSim/blob/master/docs/build_linux.md#install-and-build). +4. See also [Build AirSim on Linux](build_linux.md) for more information. ## Upgrading Your Custom Unreal Project -If you have your own Unreal project created in an older version of Unreal Engine then you need to upgrade your project to Unreal 4.18. To do this, +If you have your own Unreal project created in an older version of Unreal Engine then you need to upgrade your project to Unreal 4.24. To do this, -1. Open .uproject file and look for the line `"EngineAssociation"` and make sure it reads like `"EngineAssociation": "4.18"`. +1. Open .uproject file and look for the line `"EngineAssociation"` and make sure it reads like `"EngineAssociation": "4.24"`. 2. Delete `Plugins/AirSim` folder in your Unreal project's folder. 3. Go to your AirSim repo folder and copy `Unreal\Plugins` folder to your Unreal project's folder. 4. Copy *.bat (or *.sh for Linux) from `Unreal\Environments\Blocks` to your project's folder. @@ -31,7 +33,7 @@ If you have your own Unreal project created in an older version of Unreal Engine ### I have an Unreal project that is older than 4.16. How do I upgrade it? #### Option 1: Just Recreate Project -If your project doesn't have any code or assets other than environment you downloaded then you can also simply [recreate the project in Unreal 4.18 Editor](unreal_custenv.md) and then copy Plugins folder from `AirSim/Unreal/Plugins`. +If your project doesn't have any code or assets other than environment you downloaded then you can also simply [recreate the project in Unreal 4.24 Editor](unreal_custenv.md) and then copy Plugins folder from `AirSim/Unreal/Plugins`. #### Option 2: Modify Few Files Unreal versions newer than Unreal 4.15 has breaking changes. So you need to modify your *.Build.cs and *.Target.cs which you can find in the `Source` folder of your Unreal project. So what are those changes? Below is the gist of it but you should really refer to [Unreal's official 4.16 transition post](https://forums.unrealengine.com/showthread.php?145757-C-4-16-Transition-Guide). diff --git a/docs/use_precompiled.md b/docs/use_precompiled.md index 40ad9b6ab..76694067b 100644 --- a/docs/use_precompiled.md +++ b/docs/use_precompiled.md @@ -4,15 +4,15 @@ You can simply download precompiled binaries and run to get started immediately. ### Unreal Engine -**Windows**: Download the binaries for the environment of your choice from the [latest release](https://github.com/Microsoft/AirSim/releases). +**Windows, Linux**: Download the binaries for the environment of your choice from the [latest release](https://github.com/Microsoft/AirSim/releases). -**Linux**: Binaries for Ubuntu 16.04 LTS is coming soon. For now you will need to [build it on Linux](build_linux.md) yourself. +**macOS**: You will need to [build it yourself](https://microsoft.github.io/AirSim/build_linux/) ### Unity (Experimental) -A free environment called Windridge City is available at [Unity Asset Store](https://assetstore.unity.com/) as an experimental release of AirSim on Unity. Please note that this is work in progress and all features may not work yet. +A free environment called Windridge City is available at [Unity Asset Store](https://assetstore.unity.com/) as an experimental release of AirSim on Unity. **Note**: This is an old release, and many of the features and APIs might not work. ## Controlling Vehicles -Most of our users typically use [APIs](apis.md) to control the vehicles. However if you can also control vehicles manually. You can drive the car using keyboard, gamepad or [steering wheel](steering_wheel_installation.md). To fly drone manually, you will need either XBox controller or a remote control (feel free to [contribute](../CONTRIBUTING.md) keyboard support). Please see [remote control setup](remote_control.md) for more details. Alternatively you can use [APIs](apis.md) for programmatic control or use so-called [Computer Vision mode](image_apis.md) to move around in environment using the keyboard. +Most of our users typically use [APIs](apis.md) to control the vehicles. However if you can also control vehicles manually. You can drive the car using keyboard, gamepad or [steering wheel](steering_wheel_installation.md). To fly drone manually, you will need either XBox controller or a remote control (feel free to [contribute](CONTRIBUTING.md) keyboard support). Please see [remote control setup](remote_control.md) for more details. Alternatively you can use [APIs](apis.md) for programmatic control or use so-called [Computer Vision mode](image_apis.md) to move around in environment using the keyboard. ## Don't Have Good GPU? The AirSim binaries, like CityEnviron, requires a beefy GPU to run smoothly. You can run them in low resolution mode by editing the `run.bat` file on Windows like this: @@ -20,3 +20,9 @@ The AirSim binaries, like CityEnviron, requires a beefy GPU to run smoothly. You start CityEnviron -ResX=640 -ResY=480 -windowed ``` +For Linux binaries, use the `Blocks.sh` or corresponding shell script as follows - +``` +./Blocks.sh -ResX=640 -ResY=480 -windowed +``` + +UE 4.24 uses Vulkan drivers by default, but they can consume more GPU memory. If you get memory allocation errors, then you can try switching to OpenGL using `-opengl` diff --git a/docs/xbox_controller.md b/docs/xbox_controller.md index e2016a9df..ee14a0ae7 100644 --- a/docs/xbox_controller.md +++ b/docs/xbox_controller.md @@ -6,7 +6,7 @@ To use an XBox controller with AirSim follow these steps: ![Gamecontrollers](images/game_controllers.png) -2. Launch QGroundControl and you should see a new Joystick tab under stettings: +2. Launch QGroundControl and you should see a new Joystick tab under settings: ![Gamecontrollers](images/qgc_joystick.png) @@ -14,7 +14,7 @@ Now calibrate the radio, and setup some handy button actions. For example, I se the 'A' button arms the drone, 'B' put it in manual flight mode, 'X' puts it in altitude hold mode and 'Y' puts it in position hold mode. I also prefer the feel of the controller when I check the box labelled "Use exponential curve on roll,pitch, yaw" because this gives me more sensitivity for -small movements.] +small movements. QGroundControl will find your Pixhawk via the UDP proxy port 14550 setup by MavLinkTest above. AirSim will find your Pixhawk via the other UDP server port 14570 also setup by MavLinkTest above. diff --git a/install_run_all.sh b/install_run_all.sh index 4293bf4d9..45ee0c1c8 100755 --- a/install_run_all.sh +++ b/install_run_all.sh @@ -23,7 +23,7 @@ fi #install unreal if [[ !(-d "$UnrealDir") ]]; then - git clone -b 4.18 https://github.com/EpicGames/UnrealEngine.git "$UnrealDir" + git clone -b 4.24 https://github.com/EpicGames/UnrealEngine.git "$UnrealDir" pushd "$UnrealDir" >/dev/null ./Setup.sh diff --git a/install_unreal.sh b/install_unreal.sh index 41e145486..3a765c600 100755 --- a/install_unreal.sh +++ b/install_unreal.sh @@ -23,7 +23,7 @@ fi #install unreal if [[ !(-d "$UnrealDir") ]]; then - git clone -b 4.18 https://github.com/EpicGames/UnrealEngine.git "$UnrealDir" + git clone -b 4.24 https://github.com/EpicGames/UnrealEngine.git "$UnrealDir" pushd "$UnrealDir" >/dev/null ./Setup.sh diff --git a/mkdocs.yml b/mkdocs.yml new file mode 100644 index 000000000..407e58b7a --- /dev/null +++ b/mkdocs.yml @@ -0,0 +1,113 @@ +site_name: AirSim +site_dir: build_docs +repo_url: https://github.com/microsoft/airsim +site_description: 'Open source simulator based on Unreal Engine for autonomous vehicles from Microsoft AI & Research' + +markdown_extensions: + - toc: + permalink: "#" + +remote_branch: gh-pages + +theme: + name: 'readthedocs' + logo: + icon: ' ' + +extra: + highlightjs: true + version: 1.2.1 + feature: + tabs: true + palette: + social: + - type: facebook + link: https://www.facebook.com/groups/1225832467530667/ + - type: github-alt + link: https://github.com/Microsoft/AirSim + +copyright: Copyright © 2018 Microsoft Research + +nav: + - "Home": + - "Home": 'README.md' + - "Changelog": 'CHANGELOG.md' + - "Get AirSim": + - "Download Binaries": 'use_precompiled.md' + - "Build on Windows": 'build_windows.md' + - "Build on Linux": 'build_linux.md' + - "Docker on Linux": 'docker_ubuntu.md' + - "AirSim on Azure": 'azure.md' + - "Custom Unreal Environment": 'unreal_custenv.md' + - "AirSim with Unity": "Unity.md" + - "Custom Unity Environment": "custom_unity_environments.md" + - "Unity APIs": "unity_api_support.md" + - "Using AirSim": + - "Core APIs": 'apis.md' + - "Image APIs": 'image_apis.md' + - "C++ APIs": 'apis_cpp.md' + - "Development Workflow": 'dev_workflow.md' + - "Settings": 'settings.md' + - "Camera Views": 'camera_views.md' + - "Car Mode": 'using_car.md' + - "Remote Control": 'remote_control.md' + - "XBox Controller": 'xbox_controller.md' + - "Steering Wheel": 'steering_wheel_installation.md' + - "Multiple Vehicles": 'multi_vehicle.md' + - "Sensors": 'sensors.md' + - "LIDAR": 'lidar.md' + - "Infrared Camera": "InfraredCamera.md" + - "ROS: AirSim ROS Wrapper": "airsim_ros_pkgs.md" + - "ROS: AirSim Tutorial Packages": "airsim_tutorial_pkgs.md" + - "Domain Randomization": "retexturing.md" + - "Mesh Vertex Buffers": "meshes.md" + - "Playing Logs": 'playback.md' + - "Design": + - "Architecture": 'design.md' + - "Code Structure": 'code_structure.md' + - "Coding Guidelines": 'coding_guidelines.md' + - "Flight Controller": 'flight_controller.md' + - "Simple Flight": 'simple_flight.md' + - "Hello Drone": 'hello_drone.md' + - "MavLink and PX4": + - "PX4 Setup for AirSim": 'px4_setup.md' + - "PX4 in SITL": 'px4_sitl.md' + - "AirSim with Pixhawk": 'https://youtu.be/1oY8Qu5maQQ' + - "PX4 Setup with AirSim": 'https://youtu.be/HNWdYrtw3f0' + - "Debugging Attitude Estimation": 'https://www.youtube.com/watch?v=d_FyjKDWQfc&feature=youtu.be' + - "Intercepting MavLink Messages": 'https://github.com/Microsoft/AirSim/wiki/Intercepting-MavLink-messages' + - "Rapid Descent on PX4 Drones": 'https://github.com/Microsoft/AirSim/wiki/Rapid-Descent-on-PX4-drones' + - "Building PX4": "px4_build.md" + - "PX4/MavLink Logging": 'px4_logging.md' + - "MavLink LogViewer": "log_viewer.md" + - "MavLinkCom": 'mavlinkcom.md' + - "MavLink MoCap": 'mavlinkcom_mocap.md' + - "Upgrading": + - "Upgrading Unreal": 'unreal_upgrade.md' + - "Upgrading APIs": 'upgrade_apis.md' + - "Upgrading Settings": 'upgrade_settings.md' + - "Contributed Tutorials": + - "Reinforcement Learning": 'reinforcement_learning.md' + - "Using Environments from Marketplace": 'https://www.youtube.com/watch?v=y09VbdQWvQY' + - "Simple Collision Avoidance": 'https://github.com/simondlevy/AirSimTensorFlow' + - "Autonomous Driving on Azure": 'https://aka.ms/AutonomousDrivingCookbook' + - "Building Hexacopter": 'https://github.com/Microsoft/AirSim/wiki/hexacopter' + - "Moving on Path Demo": 'https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo' + - "Building Point Clouds": 'point_clouds.md' + - "Surveying Using Drone": 'drone_survey.md' + - "Orbit Trajectory": "orbit.md" + - "Misc": + - "AirSim on Real Drones": 'custom_drone.md' + - "Installing cmake on Linux": 'cmake_linux.md' + - "Tips for Busy HDD": 'hard_drive.md' + - "pfm format": 'pfm.md' + - "Setting up Unreal Environment": 'unreal_proj.md' + - "Blocks Environment": 'unreal_blocks.md' + - "Who is Using AirSim": 'who_is_using.md' + - "Working with UE Plugin Contents": 'working_with_plugin_contents.md' + - "Formula Student Technion Self-drive": 'https://github.com/Microsoft/AirSim/wiki/technion' + - "Support": + - "FAQ": 'faq.md' + - "Support": 'SUPPORT.md' + - "Create Issue": 'create_issue.md' + - "Contribute": 'CONTRIBUTING.md' diff --git a/pipelines/ci.yml b/pipelines/ci.yml new file mode 100644 index 000000000..02d9bfa6b --- /dev/null +++ b/pipelines/ci.yml @@ -0,0 +1,128 @@ +variables: + container_linux: airsimci.azurecr.io/ue4p25p1/ubuntu18:debugeditor_fulldebugoff + ue4_root_linux: /home/ue4/ue-4.25.1-linux-debugeditor + container_win: airsimci.azurecr.io/ue4p25p1/win1809:pipe + ue4_root_win: C:\ue-4.25.1-win + +stages: + - stage: AirSimCI + jobs: + - job: Ubuntu_1804 + pool: + name: 'AirSim' + demands: + - Spec -equals Ubuntu_18.04 + container: + image: $(container_linux) + endpoint: airsimci_acr_connection + variables: + ue4_root: $(ue4_root_linux) + workspace: + clean: all + steps: + # Setup / Prereq + - checkout: self + - script: | + ./setup.sh + displayName: Install system dependencies + + # Build AirLib + - script: | + ./build.sh + displayName: Build AirLib + + # Build UE Blocks project + - script: | + ./update_from_git.sh + workingDirectory: Unreal/Environments/Blocks + displayName: Copy AirLib to Blocks (update_from_git.sh) + + # Build Blocks + - script: | + $(UE4_ROOT)/Engine/Build/BatchFiles/Linux/Build.sh Blocks Linux Development \ + -project=$(pwd)/Unreal/Environments/Blocks/Blocks.uproject + $(UE4_ROOT)/Engine/Build/BatchFiles/Linux/Build.sh BlocksEditor Linux Development \ + -project=$(pwd)/Unreal/Environments/Blocks/Blocks.uproject + displayName: Build Blocks - Development + + # Package Blocks + - script: | + $(UE4_ROOT)/Engine/Build/BatchFiles/RunUAT.sh BuildCookRun \ + -project="$(pwd)/Unreal/Environments/Blocks/Blocks.uproject" \ + -nop4 -nocompile -build -cook -compressed -pak -allmaps -stage \ + -archive -archivedirectory="$(pwd)/Unreal/Environments/Blocks/Packaged/Development" \ + -clientconfig=Development -clean -utf8output + displayName: Package Blocks - Development + + # Publish Artifact for Blocks Linux + - task: PublishPipelineArtifact@1 + inputs: + targetPath: 'Unreal/Environments/Blocks/Packaged' + artifactName: 'Blocks_Linux' + displayName: Artifact for Blocks Linux + condition: succeededOrFailed() + + - task: ArchiveFiles@2 + displayName: Blocks Linux Zip + inputs: + rootFolderOrFile: 'Unreal/Environments/Blocks/Packaged' + includeRootFolder: false + archiveType: 'zip' + archiveFile: 'Unreal/Environments/Blocks/Packaged/Blocks_Linux.zip' + replaceExistingArchive: true + + - job: Windows_VS2019 + pool: + name: 'AirSim' + demands: + - Spec -equals WinServer2019_VS2019_Datacenter + container: + image: $(container_win) + endpoint: airsimci_acr_connection + variables: + ue4_root: $(ue4_root_win) + workspace: + clean: all + + steps: + - checkout: self + + # Build AirLib + - script: | + call "C:\BuildTools\VC\Auxiliary\Build\vcvars64.bat" + call .\build.cmd + displayName: Build AirLib + + - script: | + call "C:\BuildTools\VC\Auxiliary\Build\vcvars64.bat" + call .\update_from_git_ci.bat + workingDirectory: Unreal/Environments/Blocks + displayName: Copy AirLib to Blocks (update_from_git_ci.bat) + + # Build Blocks + - script: | + call "%UE4_ROOT%\Engine\Build\BatchFiles\Build.bat" Blocks Win64 Development -project="%CD%\Unreal\Environments\Blocks\Blocks.uproject" + call "%UE4_ROOT%\Engine\Build\BatchFiles\Build.bat" BlocksEditor Win64 Development -project="%CD%\Unreal\Environments\Blocks\Blocks.uproject" + displayName: Build Blocks - Development + + # Package Blocks + - script: | + call "%UE4_ROOT%\Engine\Build\BatchFiles\RunUAT.bat" BuildCookRun -project="%CD%\Unreal\Environments\Blocks\Blocks.uproject" -nop4 -nocompile -build -cook -compressed -pak -allmaps -stage -archive -archivedirectory="%CD%\Unreal\Environments\Blocks\Packaged\Development" -clientconfig=Development -clean -utf8output + displayName: Package Blocks - Development + + # Publish Artifact for Blocks Windows + - task: PublishPipelineArtifact@1 + inputs: + targetPath: 'Unreal/Environments/Blocks/Packaged' + artifactName: 'Blocks_Windows' + displayName: Artifact for Blocks Windows + condition: succeededOrFailed() + + - task: ArchiveFiles@2 + displayName: Blocks Windows Zip + inputs: + rootFolderOrFile: 'Unreal/Environments/Blocks/Packaged' + includeRootFolder: false + archiveType: 'zip' + archiveFile: 'Unreal/Environments/Blocks/Packaged/Blocks_Windows.zip' + replaceExistingArchive: true diff --git a/ros/src/airsim_ros_pkgs/CMakeLists.txt b/ros/src/airsim_ros_pkgs/CMakeLists.txt index 65ab3dff9..4131d6c20 100644 --- a/ros/src/airsim_ros_pkgs/CMakeLists.txt +++ b/ros/src/airsim_ros_pkgs/CMakeLists.txt @@ -8,7 +8,12 @@ add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" rpclib_wrapper) add_subdirectory("${AIRSIM_ROOT}/cmake/AirLib" AirLib) add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom) -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 11) +set(CXX_EXP_LIB "-nostdinc++ -I/usr/include/c++/8 -I/usr/include/x86_64-linux-gnu/c++/8 -nodefaultlibs +-l/usr/lib/x86_64-linux-gnu/libc++.so -l/usr/lib/x86_64-linux-gnu/libc++abi.so +-lm -lc -lgcc_s -lgcc +-lstdc++fs -fmax-errors=10 -Wnoexcept -Wstrict-null-sentinel") + set(RPC_LIB_INCLUDES " ${AIRSIM_ROOT}/external/rpclib/rpclib-2.2.1/include") set(RPC_LIB rpc) # name of .a file with lib prefix message(STATUS "found RPC_LIB_INCLUDES=${RPC_LIB_INCLUDES}") @@ -29,9 +34,12 @@ find_package(catkin REQUIRED COMPONENTS rospy sensor_msgs std_msgs + geographic_msgs + geometry_msgs std_srvs tf2 tf2_ros + tf2_sensor_msgs ) add_message_files( @@ -41,6 +49,10 @@ add_message_files( GPSYaw.msg VelCmd.msg VelCmdGroup.msg + CarControls.msg + CarState.msg + Altimeter.msg + Environment.msg ) add_service_files( @@ -58,6 +70,7 @@ generate_messages( DEPENDENCIES std_msgs geometry_msgs + geographic_msgs ) catkin_package( @@ -97,6 +110,7 @@ target_link_libraries(pd_position_controller_simple_node pd_position_controller_ install(TARGETS #list of nodes + airsim_node pd_position_controller_simple_node DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) @@ -123,4 +137,4 @@ install(DIRECTORY rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # PATTERN ".git" EXCLUDE -) \ No newline at end of file +) diff --git a/ros/src/airsim_ros_pkgs/README.md b/ros/src/airsim_ros_pkgs/README.md index 5bcc9d94c..9edb21396 100644 --- a/ros/src/airsim_ros_pkgs/README.md +++ b/ros/src/airsim_ros_pkgs/README.md @@ -1,162 +1,3 @@ # airsim_ros_pkgs -A ROS wrapper over the AirSim C++ client library. - -## Setup -- Ubuntu 16.04 - * Install [ROS kinetic](https://wiki.ros.org/kinetic/Installation/Ubuntu) - -- Ubuntu 18.04 - * Install [ROS melodic](https://wiki.ros.org/melodic/Installation/Ubuntu) - -## Build -- Build AirSim -``` -git clone https://github.com/Microsoft/AirSim.git; -cd AirSim; -./setup.sh; -./build.sh; -``` -- Build ROS package - -``` -cd ros; -catkin_make; # or catkin build -``` - -## Running -``` -source devel/setup.bash; -roslaunch airsim_ros_pkgs airsim_node.launch; -roslaunch airsim_ros_pkgs rviz.launch; -``` - -# Using AirSim ROS wrapper -The ROS wrapper is composed of two ROS nodes - the first is a wrapper over AirSim's multirotor C++ client library, and the second is a simple PD position controller. -Let's look at the ROS API for both nodes: - -### AirSim ROS Wrapper Node -#### Publishers: -- `/airsim_node/origin_geo_point` [airsim_ros_pkgs/GPSYaw](msg/GPSYaw.msg) -GPS coordinates corresponding to global NED frame. This is set in the airsim's [settings.json](https://microsoft.github.io/AirSim/docs/settings/) file under the `OriginGeopoint` key. - -- `/airsim_node/VEHICLE_NAME/global_gps` [sensor_msgs/NavSatFix](https://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html) -This the current GPS coordinates of the drone in airsim. - -- `/airsim_node/VEHICLE_NAME/odom_local_ned` [nav_msgs/Odometry](https://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) -Odometry in NED frame wrt take-off point. - -- `/airsim_node/VEHICLE_NAME/CAMERA_NAME/IMAGE_TYPE/camera_info` [sensor_msgs/CameraInfo](https://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html) - -- `/airsim_node/VEHICLE_NAME/CAMERA_NAME/IMAGE_TYPE` [sensor_msgs/Image](https://docs.ros.org/api/sensor_msgs/html/msg/Image.html) - RGB or float image depending on image type requested in settings.json. - -- `/tf` [tf2_msgs/TFMessage](https://docs.ros.org/api/tf2_msgs/html/msg/TFMessage.html) - - -#### Subscribers: -- `/airsim_node/vel_cmd_body_frame` [airsim_ros_pkgs/VelCmd](msg/VelCmd.msg) - Ignore `vehicle_name` field, leave it to blank. We will use `vehicle_name` in future for multiple drones. - -- `/airsim_node/vel_cmd_world_frame` [airsim_ros_pkgs/VelCmd](msg/VelCmd.msg) - Ignore `vehicle_name` field, leave it to blank. We will use `vehicle_name` in future for multiple drones. - -- `/gimbal_angle_euler_cmd` [airsim_ros_pkgs/GimbalAngleEulerCmd](msg/GimbalAngleEulerCmd.msg) - Gimbal set point in euler angles. - -- `/gimbal_angle_quat_cmd` [airsim_ros_pkgs/GimbalAngleQuatCmd](msg/GimbalAngleQuatCmd.msg) - Gimbal set point in quaternion. - -#### Services: -- `/airsim_node/VEHICLE_NAME/land` [airsim_ros_pkgs/Takeoff](https://docs.ros.org/api/std_srvs/html/srv/Empty.html) - -- `/airsim_node/takeoff` [airsim_ros_pkgs/Takeoff](https://docs.ros.org/api/std_srvs/html/srv/Empty.html) - -- `/airsim_node/reset` [airsim_ros_pkgs/Reset](https://docs.ros.org/api/std_srvs/html/srv/Empty.html) - Resets *all* drones - -#### Parameters: -- `/airsim_node/update_airsim_control_every_n_sec` [double] - Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` - Default: 0.01 seconds. - Timer callback frequency for updating drone odom and state from airsim, and sending in control commands. - The current RPClib interface to unreal engine maxes out at 50 Hz. - Timer callbacks in ROS run at maximum rate possible, so it's best to not touch this parameter. - -- `/airsim_node/update_airsim_img_response_every_n_sec` [double] - Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` - Default: 0.01 seconds. - Timer callback frequency for receiving images from all cameras in airsim. - The speed will depend on number of images requested and their resolution. - Timer callbacks in ROS run at maximum rate possible, so it's best to not touch this parameter. - -### Simple PID Position Controller Node - -#### Parameters: -- PD controller parameters: - * `/pd_position_node/kd_x` [double], - `/pd_position_node/kp_y` [double], - `/pd_position_node/kp_z` [double], - `/pd_position_node/kp_yaw` [double] - Proportional gains - - * `/pd_position_node/kd_x` [double], - `/pd_position_node/kd_y` [double], - `/pd_position_node/kd_z` [double], - `/pd_position_node/kd_yaw` [double] - Derivative gains - - * `/pd_position_node/reached_thresh_xyz` [double] - Threshold euler distance (meters) from current position to setpoint position - - * `/pd_position_node/reached_yaw_degrees` [double] - Threshold yaw distance (degrees) from current position to setpoint position - -- `/pd_position_node/update_control_every_n_sec` [double] - Default: 0.01 seconds - -#### Services: -- `/airsim_node/VEHICLE_NAME/gps_goal` [Request: [msgs/airsim_ros_pkgs/GPSYaw](msgs/airsim_ros_pkgs/GPSYaw)] - Target gps position + yaw. - In **absolute** altitude. - -- `/airsim_node/VEHICLE_NAME/local_position_goal` [Request: [msgs/airsim_ros_pkgs/XYZYaw](msgs/airsim_ros_pkgs/XYZYaw) - Target local position + yaw in global NED frame. - -#### Subscribers: -- `/airsim_node/origin_geo_point` [airsim_ros_pkgs/GPSYaw](msg/GPSYaw.msg) - Listens to home geo coordinates published by `airsim_node`. - -- `/airsim_node/VEHICLE_NAME/odom_local_ned` [nav_msgs/Odometry](https://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) - Listens to odometry published by `airsim_node` - -#### Publishers: -- `/vel_cmd_world_frame` [airsim_ros_pkgs/VelCmd](airsim_ros_pkgs/VelCmd) - Sends velocity command to `airsim_node` - -### Global params -- Dynamic constraints. These can be changed in `dynamic_constraints.launch`: - * `/max_vel_horz_abs` [double] - Maximum horizontal velocity of the drone (meters/second) - - * `/max_vel_vert_abs` [double] - Maximum vertical velocity of the drone (meters/second) - - * `/max_yaw_rate_degree` [double] - Maximum yaw rate (degrees/second) - -### Misc -#### Windows Subsytem for Linux on Windows 10 -- WSL setup: - * Get [Windows Subsystem for Linux](https://docs.microsoft.com/en-us/windows/wsl/install-win10) - * Get [Ubuntu 16.04](https://www.microsoft.com/en-us/p/ubuntu-1604-lts/9pjn388hp8c9?activetab=pivot:overviewtab) or [Ubuntu 18.04](https://www.microsoft.com/en-us/p/ubuntu-1804-lts/9n9tngvndl3q?activetab=pivot%3Aoverviewtab) - * Go to Ubuntu 16 / 18 instructions! - - -- Setup for X apps (like RViz, rqt_image_view, terminator) in Windows + WSL - * Install [Xming X Server](https://sourceforge.net/projects/xming/). - * Find and run `XLaunch` from the Windows start menu. - Select `Multiple Windows` in first popup, `Start no client` in second popup, **only** `Clipboard` in third popup. Do **not** select `Native Opengl`. - * Open Ubuntu 16.04 / 18.04 session by typing `Ubuntu 16.04` / `Ubuntu 18.04` in Windows start menu. - * Recommended: Install [terminator](http://www.ubuntugeek.com/terminator-multiple-gnome-terminals-in-one-window.html) : `$ sudo apt-get install terminator.` - - You can open terminator in a new window by entering `$ DISPLAY=:0 terminator -u`. +This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/airsim_ros_pkgs.md). \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h index 02021355e..958ea1e70 100644 --- a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -12,6 +12,7 @@ STRICT_MODE_ON #include "ros/ros.h" #include "sensors/imu/ImuBase.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" +#include "vehicles/car/api/CarRpcLibClient.hpp" #include "yaml-cpp/yaml.h" #include #include @@ -23,6 +24,9 @@ STRICT_MODE_ON #include #include #include +#include +#include +#include #include #include #include @@ -43,7 +47,11 @@ STRICT_MODE_ON #include #include #include +#include //hector_uav_msgs defunct? +#include #include +#include +#include #include #include #include @@ -51,7 +59,9 @@ STRICT_MODE_ON #include #include #include +#include #include +#include // #include "nodelet/nodelet.h" // todo move airlib typedefs to separate header file? @@ -115,7 +125,13 @@ struct GimbalCmd class AirsimROSWrapper { public: - AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); + enum class AIRSIM_MODE : unsigned + { + DRONE, + CAR + }; + + AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string & host_ip); ~AirsimROSWrapper() {}; void initialize_airsim(); @@ -128,6 +144,76 @@ class AirsimROSWrapper bool is_used_img_timer_cb_queue_; private: + struct SensorPublisher + { + SensorBase::SensorType sensor_type; + std::string sensor_name; + ros::Publisher publisher; + }; + + // utility struct for a SINGLE robot + class VehicleROS + { + public: + virtual ~VehicleROS() {} + std::string vehicle_name; + + /// All things ROS + ros::Publisher odom_local_pub; + ros::Publisher global_gps_pub; + ros::Publisher env_pub; + airsim_ros_pkgs::Environment env_msg; + std::vector sensor_pubs; + // handle lidar seperately for max performance as data is collected on its own thread/callback + std::vector lidar_pubs; + + nav_msgs::Odometry curr_odom; + sensor_msgs::NavSatFix gps_sensor_msg; + + std::vector static_tf_msg_vec; + + ros::Time stamp; + + + std::string odom_frame_id; + /// Status + // bool is_armed_; + // std::string mode_; + }; + + class CarROS : public VehicleROS + { + public: + msr::airlib::CarApiBase::CarState curr_car_state; + + ros::Subscriber car_cmd_sub; + ros::Publisher car_state_pub; + airsim_ros_pkgs::CarState car_state_msg; + + bool has_car_cmd; + msr::airlib::CarApiBase::CarControls car_cmd; + }; + + class MultiRotorROS : public VehicleROS + { + public: + /// State + msr::airlib::MultirotorState curr_drone_state; + // bool in_air_; // todo change to "status" and keep track of this + + ros::Subscriber vel_cmd_body_frame_sub; + ros::Subscriber vel_cmd_world_frame_sub; + + ros::ServiceServer takeoff_srvr; + ros::ServiceServer land_srvr; + + bool has_vel_cmd; + VelCmd vel_cmd; + + /// Status + // bool in_air_; // todo change to "status" and keep track of this + }; + /// ROS timer callbacks void img_response_timer_cb(const ros::TimerEvent& event); // update images from airsim_client_ every nth sec void drone_state_timer_cb(const ros::TimerEvent& event); // update drone state from airsim_client_ every nth sec @@ -147,7 +233,14 @@ class AirsimROSWrapper void gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAngleQuatCmd& gimbal_angle_quat_cmd_msg); void gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAngleEulerCmd& gimbal_angle_euler_cmd_msg); - // void set_zero_vel_cmd(); + // commands + void car_cmd_cb(const airsim_ros_pkgs::CarControls::ConstPtr& msg, const std::string& vehicle_name); + void update_commands(); + + // state, returns the simulation timestamp best guess based on drone state timestamp, airsim needs to return timestap for environment + ros::Time update_state(); + void update_and_publish_static_transforms(VehicleROS* vehicle_ros); + void publish_vehicle_state(); /// ROS service callbacks bool takeoff_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response, const std::string& vehicle_name); @@ -160,20 +253,22 @@ class AirsimROSWrapper /// ROS tf broadcasters void publish_camera_tf(const ImageResponse& img_response, const ros::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id); - void publish_odom_tf(const nav_msgs::Odometry& odom_ned_msg); + void publish_odom_tf(const nav_msgs::Odometry& odom_msg); /// camera helper methods sensor_msgs::CameraInfo generate_cam_info(const std::string& camera_name, const CameraSetting& camera_setting, const CaptureSetting& capture_setting) const; cv::Mat manual_decode_depth(const ImageResponse& img_response) const; - sensor_msgs::ImagePtr get_img_msg_from_response(const ImageResponse& img_response, const ros::Time curr_ros_time, const std::string frame_id) const; - sensor_msgs::ImagePtr get_depth_img_msg_from_response(const ImageResponse& img_response, const ros::Time curr_ros_time, const std::string frame_id) const; + + sensor_msgs::ImagePtr get_img_msg_from_response(const ImageResponse& img_response, const ros::Time curr_ros_time, const std::string frame_id); + sensor_msgs::ImagePtr get_depth_img_msg_from_response(const ImageResponse& img_response, const ros::Time curr_ros_time, const std::string frame_id); + void process_and_publish_img_response(const std::vector& img_response_vec, const int img_response_idx, const std::string& vehicle_name); // methods which parse setting json ang generate ros pubsubsrv void create_ros_pubs_from_settings_json(); - void append_static_camera_tf(const std::string& vehicle_name, const std::string& camera_name, const CameraSetting& camera_setting); - void append_static_lidar_tf(const std::string& vehicle_name, const std::string& lidar_name, const LidarSetting& lidar_setting); - void append_static_vehicle_tf(const std::string& vehicle_name, const VehicleSetting& vehicle_setting); + void append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting); + void append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const LidarSetting& lidar_setting); + void append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting); void set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const; void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) const; void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, LidarSetting& lidar_setting) const; @@ -182,16 +277,28 @@ class AirsimROSWrapper tf2::Quaternion get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const; msr::airlib::Quaternionr get_airlib_quat(const geometry_msgs::Quaternion& geometry_msgs_quat) const; msr::airlib::Quaternionr get_airlib_quat(const tf2::Quaternion& tf2_quat) const; - nav_msgs::Odometry get_odom_msg_from_airsim_state(const msr::airlib::MultirotorState& drone_state) const; + nav_msgs::Odometry get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const; + nav_msgs::Odometry get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; + airsim_ros_pkgs::CarState get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; + msr::airlib::Pose get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const; airsim_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; sensor_msgs::NavSatFix get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; sensor_msgs::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const; - sensor_msgs::PointCloud2 get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data) const; + airsim_ros_pkgs::Altimeter get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const; + sensor_msgs::Range get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const; + sensor_msgs::PointCloud2 get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name) const; + sensor_msgs::NavSatFix get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const; + sensor_msgs::MagneticField get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const; + airsim_ros_pkgs::Environment get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const; // not used anymore, but can be useful in future with an unreal camera calibration environment void read_params_from_yaml_and_fill_cam_info_msg(const std::string& file_name, sensor_msgs::CameraInfo& cam_info) const; void convert_yaml_to_simple_mat(const YAML::Node& node, SimpleMatrix& m) const; // todo ugly + // simulation time utility + ros::Time airsim_timestamp_to_ros(const msr::airlib::TTimePoint& stamp) const; + ros::Time chrono_timestamp_to_ros(const std::chrono::system_clock::time_point& stamp) const; + private: // subscriber / services for ALL robots ros::Subscriber vel_cmd_all_body_frame_sub_; @@ -205,80 +312,52 @@ class AirsimROSWrapper ros::ServiceServer takeoff_group_srvr_; ros::ServiceServer land_group_srvr_; - // utility struct for a SINGLE robot - struct MultiRotorROS - { - std::string vehicle_name; - - /// All things ROS - ros::Publisher odom_local_ned_pub; - ros::Publisher global_gps_pub; - // ros::Publisher home_geo_point_pub_; // geo coord of unreal origin - - ros::Subscriber vel_cmd_body_frame_sub; - ros::Subscriber vel_cmd_world_frame_sub; - - ros::ServiceServer takeoff_srvr; - ros::ServiceServer land_srvr; - - /// State - msr::airlib::MultirotorState curr_drone_state; - // bool in_air_; // todo change to "status" and keep track of this - nav_msgs::Odometry curr_odom_ned; - sensor_msgs::NavSatFix gps_sensor_msg; - bool has_vel_cmd; - VelCmd vel_cmd; - - std::string odom_frame_id; - /// Status - // bool in_air_; // todo change to "status" and keep track of this - // bool is_armed_; - // std::string mode_; - }; + AIRSIM_MODE airsim_mode_ = AIRSIM_MODE::DRONE; ros::ServiceServer reset_srvr_; ros::Publisher origin_geo_point_pub_; // home geo coord of drones msr::airlib::GeoPoint origin_geo_point_;// gps coord of unreal origin airsim_ros_pkgs::GPSYaw origin_geo_point_msg_; // todo duplicate - std::vector multirotor_ros_vec_; - - std::vector vehicle_names_; std::vector vehicle_setting_vec_; AirSimSettingsParser airsim_settings_parser_; - std::unordered_map vehicle_name_idx_map_; + std::unordered_map< std::string, std::unique_ptr< VehicleROS > > vehicle_name_ptr_map_; static const std::unordered_map image_type_int_to_string_map_; - std::map vehicle_imu_map_; - std::map vehicle_lidar_map_; - std::vector static_tf_msg_vec_; + bool is_vulkan_; // rosparam obtained from launch file. If vulkan is being used, we BGR encoding instead of RGB - msr::airlib::MultirotorRpcLibClient airsim_client_; - msr::airlib::MultirotorRpcLibClient airsim_client_images_; - msr::airlib::MultirotorRpcLibClient airsim_client_lidar_; + std::string host_ip_; + std::unique_ptr airsim_client_ = nullptr; + // seperate busy connections to airsim, update in their own thread + msr::airlib::RpcLibClientBase airsim_client_images_; + msr::airlib::RpcLibClientBase airsim_client_lidar_; ros::NodeHandle nh_; ros::NodeHandle nh_private_; // todo not sure if async spinners shuold be inside this class, or should be instantiated in airsim_node.cpp, and cb queues should be public - // todo for multiple drones with multiple sensors, this won't scale. make it a part of MultiRotorROS? + // todo for multiple drones with multiple sensors, this won't scale. make it a part of VehicleROS? ros::CallbackQueue img_timer_cb_queue_; ros::CallbackQueue lidar_timer_cb_queue_; - // todo race condition - std::recursive_mutex drone_control_mutex_; - // std::recursive_mutex img_mutex_; - // std::recursive_mutex lidar_mutex_; + std::mutex drone_control_mutex_; // gimbal control bool has_gimbal_cmd_; GimbalCmd gimbal_cmd_; /// ROS tf - std::string world_frame_id_; + const std::string AIRSIM_FRAME_ID = "world_ned"; + std::string world_frame_id_ = AIRSIM_FRAME_ID; + const std::string AIRSIM_ODOM_FRAME_ID = "odom_local_ned"; + const std::string ENU_ODOM_FRAME_ID = "odom_local_enu"; + std::string odom_frame_id_ = AIRSIM_ODOM_FRAME_ID; tf2_ros::TransformBroadcaster tf_broadcaster_; tf2_ros::StaticTransformBroadcaster static_tf_pub_; + + bool isENU_ = false; tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; /// ROS params double vel_cmd_duration_; @@ -292,13 +371,13 @@ class AirsimROSWrapper std::vector airsim_img_request_vehicle_name_pair_vec_; std::vector image_pub_vec_; std::vector cam_info_pub_vec_; - std::vector lidar_pub_vec_; - std::vector imu_pub_vec_; std::vector camera_info_msg_vec_; /// ROS other publishers ros::Publisher clock_pub_; + rosgraph_msgs::Clock ros_clock_; + bool publish_clock_ = false; ros::Subscriber gimbal_angle_quat_cmd_sub_; ros::Subscriber gimbal_angle_euler_cmd_sub_; diff --git a/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control.launch b/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control.launch new file mode 100644 index 000000000..682558b65 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control_auto.launch b/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control_auto.launch new file mode 100644 index 000000000..621e04c34 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control_auto.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/launch/airsim_node.launch b/ros/src/airsim_ros_pkgs/launch/airsim_node.launch index e61b9f752..53acec90c 100644 --- a/ros/src/airsim_ros_pkgs/launch/airsim_node.launch +++ b/ros/src/airsim_ros_pkgs/launch/airsim_node.launch @@ -1,16 +1,19 @@ - - - - + + + + - + + - + + + - \ No newline at end of file + diff --git a/ros/src/airsim_ros_pkgs/msg/Altimeter.msg b/ros/src/airsim_ros_pkgs/msg/Altimeter.msg new file mode 100644 index 000000000..3a3cd1eed --- /dev/null +++ b/ros/src/airsim_ros_pkgs/msg/Altimeter.msg @@ -0,0 +1,4 @@ +Header header +float32 altitude +float32 pressure +float32 qnh \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/msg/CarControls.msg b/ros/src/airsim_ros_pkgs/msg/CarControls.msg new file mode 100644 index 000000000..3dffecb22 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/msg/CarControls.msg @@ -0,0 +1,8 @@ +std_msgs/Header header +float32 throttle +float32 brake +float32 steering +bool handbrake +bool manual +int8 manual_gear +bool gear_immediate \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/msg/CarState.msg b/ros/src/airsim_ros_pkgs/msg/CarState.msg new file mode 100644 index 000000000..4c780ad94 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/msg/CarState.msg @@ -0,0 +1,8 @@ +std_msgs/Header header +geometry_msgs/PoseWithCovariance pose +geometry_msgs/TwistWithCovariance twist +float32 speed +int8 gear +float32 rpm +float32 maxrpm +bool handbrake \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/msg/Environment.msg b/ros/src/airsim_ros_pkgs/msg/Environment.msg new file mode 100644 index 000000000..e0c1ea724 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/msg/Environment.msg @@ -0,0 +1,8 @@ +Header header +geometry_msgs/Vector3 position +geographic_msgs/GeoPoint geo_point +geometry_msgs/Vector3 gravity +float32 air_pressure +float32 temperature +float32 air_density + diff --git a/ros/src/airsim_ros_pkgs/package.xml b/ros/src/airsim_ros_pkgs/package.xml index 6b8c98c18..af9228a1a 100644 --- a/ros/src/airsim_ros_pkgs/package.xml +++ b/ros/src/airsim_ros_pkgs/package.xml @@ -15,11 +15,16 @@ image_transport message_generation message_runtime + mavros_msgs nav_msgs roscpp rospy sensor_msgs std_msgs + geographic_msgs + geometry_msgs + tf2_sensor_msgs + geometry_msgs @@ -31,9 +36,10 @@ rospy sensor_msgs std_msgs + joy - \ No newline at end of file + diff --git a/ros/src/airsim_ros_pkgs/rviz/default.rviz b/ros/src/airsim_ros_pkgs/rviz/default.rviz index 5aebb6763..7e19fe3b0 100644 --- a/ros/src/airsim_ros_pkgs/rviz/default.rviz +++ b/ros/src/airsim_ros_pkgs/rviz/default.rviz @@ -7,8 +7,9 @@ Panels: - /TF1 - /TF1/Frames1 - /Odometry1/Shape1 - Splitter Ratio: 0.50515461 - Tree Height: 775 + - /PointCloud21 + Splitter Ratio: 0.5051546096801758 + Tree Height: 847 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -17,7 +18,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -27,7 +28,11 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: front_left_depth_planar_registered_point_cloud + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -37,7 +42,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -54,16 +59,16 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - LidarCustom: + drone_1: Value: true - MyQuad: + drone_1/LidarCustom: Value: true - MyQuad/odom_local_ned: + drone_1/odom_local_enu: Value: true front_center_custom_body: Value: false front_center_custom_body/static: - Value: false + Value: true front_center_custom_optical: Value: true front_center_custom_optical/static: @@ -71,7 +76,7 @@ Visualization Manager: front_left_custom_body: Value: false front_left_custom_body/static: - Value: false + Value: true front_left_custom_optical: Value: true front_left_custom_optical/static: @@ -79,7 +84,7 @@ Visualization Manager: front_right_custom_body: Value: false front_right_custom_body/static: - Value: false + Value: true front_right_custom_optical: Value: true front_right_custom_optical/static: @@ -95,36 +100,35 @@ Visualization Manager: Show Names: true Tree: world_ned: - MyQuad: - MyQuad/odom_local_ned: - LidarCustom: - {} - front_center_custom_body/static: + world_enu: + drone_1: + drone_1/odom_local_enu: + drone_1/LidarCustom: + {} + front_center_custom_body/static: + {} + front_center_custom_optical/static: + {} + front_left_custom_body/static: + {} + front_left_custom_optical/static: + {} + front_right_custom_body/static: + {} + front_right_custom_optical/static: + {} + front_center_custom_body: {} - front_center_custom_optical/static: + front_center_custom_optical: {} - front_left_custom_body/static: + front_left_custom_body: {} - front_left_custom_optical/static: + front_left_custom_optical: {} - front_right_custom_body/static: + front_right_custom_body: {} - front_right_custom_optical/static: + front_right_custom_optical: {} - front_center_custom_body: - {} - front_center_custom_optical: - {} - front_left_custom_body: - {} - front_left_custom_optical: - {} - front_right_custom_body: - {} - front_right_custom_optical: - {} - world_enu: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -150,14 +154,14 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.100000001 + Size (m): 0.10000000149011612 Style: Flat Squares Topic: /airsim_node/front_left_custom/DepthPlanner/registered/points Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - - Angle Tolerance: 0.100000001 + - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: @@ -169,7 +173,7 @@ Visualization Manager: Scale: 1 Value: true Position: - Alpha: 0.300000012 + Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true @@ -177,16 +181,16 @@ Visualization Manager: Enabled: true Keep: 25 Name: Odometry - Position Tolerance: 0.100000001 + Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 - Axes Radius: 0.100000001 + Axes Radius: 0.10000000149011612 Color: 255; 25; 0 - Head Length: 0.200000003 - Head Radius: 0.0500000007 - Shaft Length: 0.200000003 - Shaft Radius: 0.0199999996 + Head Length: 0.20000000298023224 + Head Radius: 0.05000000074505806 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.019999999552965164 Value: Arrow Topic: /airsim_node/MyQuad/odom_local_ned Unreliable: false @@ -214,9 +218,9 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /airsim_node/MyQuad/lidar/LidarCustom + Topic: /airsim_node/drone_1/lidar/LidarCustom Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -236,7 +240,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -246,33 +253,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 98.1204224 + Distance: 34.531036376953125 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.0908625796 - Y: -0.00513405167 - Z: -0.783257008 + X: 0.08613172918558121 + Y: 0.2811238765716553 + Z: -0.7929707169532776 Focal Shape Fixed Size: false - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.63979739 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.34479764103889465 Target Frame: world_view Value: Orbit (rviz) - Yaw: 3.76538205 + Yaw: 3.170380115509033 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 1138 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000025500000396fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000002800000396000000d700fffffffa000000010100000002fb0000002000630061006d005f00660072006f006e0074005f00630065006e0074006500720000000000ffffffff0000000000000000fb000000100044006900730070006c0061007900730100000000000001a60000016a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000038f000000160000000000000000fb0000003600630061006d005f00660072006f006e0074005f006c006500660074005f00640065007000740068005f0070006c0061006e006100720200000af40000014e0000031c00000231fb0000001c00630061006d005f00660072006f006e0074005f006c0065006600740200000ac00000010f000002d700000196fb0000001e00630061006d005f00660072006f006e0074005f007200690067006800740200000b400000017000000286000001a7000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007530000003efc0100000002fb0000000800540069006d00650100000000000007530000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003e30000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -281,6 +288,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1875 - X: 1965 - Y: 24 + Width: 1918 + X: 1050 + Y: 278 diff --git a/ros/src/airsim_ros_pkgs/scripts/car_joy b/ros/src/airsim_ros_pkgs/scripts/car_joy new file mode 100755 index 000000000..b8fab4ef4 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/scripts/car_joy @@ -0,0 +1,150 @@ +#!/usr/bin/env python + +#capture joystick events using ROS and convert to AirSim Car API commands +#to enable: +# rosrun joy joy_node + +import rospy +import threading +import sensor_msgs +import sensor_msgs.msg +import airsim_ros_pkgs as air +import airsim_ros_pkgs.msg + +class CarCommandTranslator(object): + def __init__(self): + self.lock = threading.Lock() + + self.last_forward_btn = 0 + self.last_reverse_btn = 0 + self.last_nuetral_btn = 0 + self.last_park_btn = 0 + self.last_shift_down_btn = 0 + self.last_shift_up_btn = 0 + self.parked = True + self.last_gear = 0 + self.shift_mode_manual = True + + update_rate_hz = rospy.get_param('~update_rate_hz', 20.0) + self.max_curvature = rospy.get_param('~max_curvature', 0.75) + self.steer_sign = rospy.get_param('~steer_sign', -1) + self.throttle_brake_sign = rospy.get_param('~throttle_brake_sign', 1) + self.auto_gear_max = rospy.get_param('~auto_gear_max', 5) + self.manual_transmission = rospy.get_param('~manual_transmission', True) + self.forward_btn_index = rospy.get_param('~forward_button_index', 0) + self.reverse_btn_index = rospy.get_param('~reverse_button_index', 1) + self.nuetral_btn_index = rospy.get_param('~nuetral_button_index', 2) + self.park_btn_index = rospy.get_param('~park_button_index', 3) + self.shift_down_btn_index = rospy.get_param('~shift_down_index', 4) + self.shift_up_btn_index = rospy.get_param('~shift_up_index', 5) + car_control_topic = rospy.get_param('~car_control_topic', '/airsim_node/drone_1/car_cmd') + + self.joy_msg = None + + self.joy_sub = rospy.Subscriber( + 'joy', + sensor_msgs.msg.Joy, + self.handle_joy) + + self.command_pub = rospy.Publisher( + car_control_topic, + air.msg.CarControls, + queue_size=0 + ) + + self.update_time = rospy.Timer( + rospy.Duration(1.0/update_rate_hz), + self.handle_update_timer + ) + + def handle_joy(self, msg): + with self.lock: + self.joy_msg = msg + + def handle_update_timer(self, ignored): + joy = None + with self.lock: + joy = self.joy_msg + + if joy is None: + return + + controls = airsim_ros_pkgs.msg.CarControls() + + controls.steering = self.steer_sign * self.max_curvature * joy.axes[2] + u = joy.axes[1] * self.throttle_brake_sign + if u > 0.0: + controls.throttle = abs(u) + controls.brake = 0.0 + else: + controls.throttle = 0.0 + controls.brake = abs(u) + + forward_btn = joy.buttons[self.forward_btn_index] + reverse_btn = joy.buttons[self.reverse_btn_index] + nuetral_btn = joy.buttons[self.nuetral_btn_index] + park_btn = joy.buttons[self.park_btn_index] + shift_up_btn = joy.buttons[self.shift_up_btn_index] + shift_down_btn = joy.buttons[self.shift_down_btn_index] + + + # gearing: -1 reverse, 0 N, >= 1 drive + controls.manual = True #set to False for automatic transmission along with manual_gear > 1 + if not self.last_nuetral_btn and nuetral_btn: + self.last_gear = 0 + self.parked = False + controls.manual = True + elif not self.last_forward_btn and forward_btn: + if self.manual_transmission: + self.last_gear = 1 + self.shift_mode_manual = True + else: + self.shift_mode_manual = False + self.last_gear = self.auto_gear_max + + self.parked = False + elif not self.last_reverse_btn and reverse_btn: + self.last_gear = -1 + self.parked = False + self.shift_mode_manual = True + elif not self.last_park_btn and park_btn: + self.parked = True + elif not self.last_shift_down_btn and shift_down_btn and self.last_gear > 1 and self.manual_transmission: + self.last_gear-=1 + self.parked = False + self.shift_mode_manual = True + elif not self.last_shift_up_btn and shift_up_btn and self.last_gear >= 1 and self.manual_transmission: + self.last_gear+=1 + self.parked = False + self.shift_mode_manual = True + + if self.parked: + self.last_gear = 0 + self.shift_mode_manual = True + controls.handbrake = True + else: + controls.handbrake = False + + controls.manual_gear = self.last_gear + controls.manual = self.shift_mode_manual + + now = rospy.Time.now() + controls.header.stamp = now + controls.gear_immediate = True + + self.last_nuetral_btn = nuetral_btn + self.last_forward_btn = forward_btn + self.last_reverse_btn = reverse_btn + self.last_park_btn = park_btn + self.last_shift_down_btn = shift_down_btn + self.last_shift_up_btn = shift_up_btn + + self.command_pub.publish(controls) + + def run(self): + rospy.spin() + +if __name__ == '__main__': + rospy.init_node('car_joy') + node = CarCommandTranslator() + node.run() \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/src/airsim_node.cpp b/ros/src/airsim_ros_pkgs/src/airsim_node.cpp index 9a3a9f9ea..515ec6429 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_node.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_node.cpp @@ -8,7 +8,9 @@ int main(int argc, char ** argv) ros::NodeHandle nh; ros::NodeHandle nh_private("~"); - AirsimROSWrapper airsim_ros_wrapper(nh, nh_private); + std::string host_ip = "localhost"; + nh_private.getParam("host_ip", host_ip); + AirsimROSWrapper airsim_ros_wrapper(nh, nh_private, host_ip); if (airsim_ros_wrapper.is_used_img_timer_cb_queue_) { diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 00eee7c6c..5b775c0c5 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -3,6 +3,8 @@ // #include // PLUGINLIB_EXPORT_CLASS(AirsimROSWrapper, nodelet::Nodelet) #include "common/AirSimSettings.hpp" +#include + constexpr char AirsimROSWrapper::CAM_YML_NAME[]; constexpr char AirsimROSWrapper::WIDTH_YML_NAME[]; @@ -24,23 +26,34 @@ const std::unordered_map AirsimROSWrapper::image_type_int_to_s { 7, "Infrared" } }; -AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) : +AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip) : nh_(nh), nh_private_(nh_private), img_async_spinner_(1, &img_timer_cb_queue_), // a thread for image callbacks to be 'spun' by img_async_spinner_ - lidar_async_spinner_(1, &lidar_timer_cb_queue_) // same as above, but for lidar + lidar_async_spinner_(1, &lidar_timer_cb_queue_), // same as above, but for lidar + host_ip_(host_ip), + airsim_client_images_(host_ip), + airsim_client_lidar_(host_ip), + tf_listener_(tf_buffer_) { + ros_clock_.clock.fromSec(0); is_used_lidar_timer_cb_queue_ = false; is_used_img_timer_cb_queue_ = false; - world_frame_id_ = "world_ned"; // todo rosparam? - + if (AirSimSettings::singleton().simmode_name != "Car") + { + airsim_mode_ = AIRSIM_MODE::DRONE; + ROS_INFO("Setting ROS wrapper to DRONE mode"); + } + else + { + airsim_mode_ = AIRSIM_MODE::CAR; + ROS_INFO("Setting ROS wrapper to CAR mode"); + } + initialize_ros(); std::cout << "AirsimROSWrapper Initialized!\n"; - // intitialize placeholder control commands - // vel_cmd_ = VelCmd(); - // gimbal_cmd_ = GimbalCmd(); } void AirsimROSWrapper::initialize_airsim() @@ -48,17 +61,26 @@ void AirsimROSWrapper::initialize_airsim() // todo do not reset if already in air? try { - airsim_client_.confirmConnection(); + + if (airsim_mode_ == AIRSIM_MODE::DRONE) + { + airsim_client_ = std::unique_ptr(new msr::airlib::MultirotorRpcLibClient(host_ip_)); + } + else + { + airsim_client_ = std::move(std::unique_ptr(new msr::airlib::CarRpcLibClient(host_ip_))); + } + airsim_client_->confirmConnection(); airsim_client_images_.confirmConnection(); airsim_client_lidar_.confirmConnection(); - for (const auto& vehicle_name : vehicle_names_) + for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - airsim_client_.enableApiControl(true, vehicle_name); // todo expose as rosservice? - airsim_client_.armDisarm(true, vehicle_name); // todo exposes as rosservice? + airsim_client_->enableApiControl(true, vehicle_name_ptr_pair.first); // todo expose as rosservice? + airsim_client_->armDisarm(true, vehicle_name_ptr_pair.first); // todo exposes as rosservice? } - origin_geo_point_ = airsim_client_.getHomeGeoPoint(""); + origin_geo_point_ = airsim_client_->getHomeGeoPoint(""); // todo there's only one global origin geopoint for environment. but airsim API accept a parameter vehicle_name? inside carsimpawnapi.cpp, there's a geopoint being assigned in the constructor. by? origin_geo_point_msg_ = get_gps_msg_from_airsim_geo_point(origin_geo_point_); } @@ -76,6 +98,12 @@ void AirsimROSWrapper::initialize_ros() double update_airsim_control_every_n_sec; nh_private_.getParam("is_vulkan", is_vulkan_); nh_private_.getParam("update_airsim_control_every_n_sec", update_airsim_control_every_n_sec); + nh_private_.getParam("publish_clock", publish_clock_); + nh_private_.param("world_frame_id", world_frame_id_, world_frame_id_); + odom_frame_id_ = world_frame_id_ == AIRSIM_FRAME_ID ? AIRSIM_ODOM_FRAME_ID : ENU_ODOM_FRAME_ID; + nh_private_.param("odom_frame_id", odom_frame_id_, odom_frame_id_); + isENU_ = !(odom_frame_id_ == AIRSIM_ODOM_FRAME_ID); + nh_private_.param("coordinate_system_enu", isENU_, isENU_); vel_cmd_duration_ = 0.05; // todo rosparam // todo enforce dynamics constraints in this node as well? // nh_.getParam("max_vert_vel_", max_vert_vel_); @@ -97,60 +125,73 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() image_pub_vec_.clear(); cam_info_pub_vec_.clear(); camera_info_msg_vec_.clear(); - static_tf_msg_vec_.clear(); - imu_pub_vec_.clear(); - lidar_pub_vec_.clear(); - vehicle_names_.clear(); // todo should eventually support different types of vehicles in a single instance - // vehicle_setting_vec_.clear(); - // vehicle_imu_map_; - multirotor_ros_vec_.clear(); - // callback_queues_.clear(); + vehicle_name_ptr_map_.clear(); + size_t lidar_cnt = 0; image_transport::ImageTransport image_transporter(nh_private_); - int idx = 0; // iterate over std::map> vehicles; for (const auto& curr_vehicle_elem : AirSimSettings::singleton().vehicles) { auto& vehicle_setting = curr_vehicle_elem.second; auto curr_vehicle_name = curr_vehicle_elem.first; - vehicle_names_.push_back(curr_vehicle_name); set_nans_to_zeros_in_pose(*vehicle_setting); - // auto vehicle_setting_local = vehicle_setting.get(); - - append_static_vehicle_tf(curr_vehicle_name, *vehicle_setting); - vehicle_name_idx_map_[curr_vehicle_name] = idx; // allows fast lookup in command callbacks in case of a lot of drones - - MultiRotorROS multirotor_ros; - multirotor_ros.odom_frame_id = curr_vehicle_name + "/odom_local_ned"; - multirotor_ros.vehicle_name = curr_vehicle_name; - multirotor_ros.odom_local_ned_pub = nh_private_.advertise(curr_vehicle_name + "/odom_local_ned", 10); - multirotor_ros.global_gps_pub = nh_private_.advertise(curr_vehicle_name + "/global_gps", 10); - - // bind to a single callback. todo optimal subs queue length - // bind multiple topics to a single callback, but keep track of which vehicle name it was by passing curr_vehicle_name as the 2nd argument - multirotor_ros.vel_cmd_body_frame_sub = nh_private_.subscribe(curr_vehicle_name + "/vel_cmd_body_frame", 1, - boost::bind(&AirsimROSWrapper::vel_cmd_body_frame_cb, this, _1, multirotor_ros.vehicle_name)); // todo ros::TransportHints().tcpNoDelay(); - multirotor_ros.vel_cmd_world_frame_sub = nh_private_.subscribe(curr_vehicle_name + "/vel_cmd_world_frame", 1, - boost::bind(&AirsimROSWrapper::vel_cmd_world_frame_cb, this, _1, multirotor_ros.vehicle_name)); - - multirotor_ros.takeoff_srvr = nh_private_.advertiseService(curr_vehicle_name + "/takeoff", - boost::bind(&AirsimROSWrapper::takeoff_srv_cb, this, _1, _2, multirotor_ros.vehicle_name) ); - multirotor_ros.land_srvr = nh_private_.advertiseService(curr_vehicle_name + "/land", - boost::bind(&AirsimROSWrapper::land_srv_cb, this, _1, _2, multirotor_ros.vehicle_name) ); - // multirotor_ros.reset_srvr = nh_private_.advertiseService(curr_vehicle_name + "/reset",&AirsimROSWrapper::reset_srv_cb, this); - - multirotor_ros_vec_.push_back(multirotor_ros); - idx++; - - // iterate over camera map std::map cameras; + + std::unique_ptr vehicle_ros = nullptr; + + if (airsim_mode_ == AIRSIM_MODE::DRONE) + { + vehicle_ros = std::unique_ptr(new MultiRotorROS()); + } + else + { + vehicle_ros = std::unique_ptr(new CarROS()); + } + + vehicle_ros->odom_frame_id = curr_vehicle_name + "/" + odom_frame_id_; + vehicle_ros->vehicle_name = curr_vehicle_name; + + append_static_vehicle_tf(vehicle_ros.get(), *vehicle_setting); + + vehicle_ros->odom_local_pub = nh_private_.advertise(curr_vehicle_name + "/" + odom_frame_id_, 10); + + vehicle_ros->env_pub = nh_private_.advertise(curr_vehicle_name + "/environment", 10); + + vehicle_ros->global_gps_pub = nh_private_.advertise(curr_vehicle_name + "/global_gps", 10); + + if (airsim_mode_ == AIRSIM_MODE::DRONE) + { + auto drone = static_cast(vehicle_ros.get()); + + // bind to a single callback. todo optimal subs queue length + // bind multiple topics to a single callback, but keep track of which vehicle name it was by passing curr_vehicle_name as the 2nd argument + drone->vel_cmd_body_frame_sub = nh_private_.subscribe(curr_vehicle_name + "/vel_cmd_body_frame", 1, + boost::bind(&AirsimROSWrapper::vel_cmd_body_frame_cb, this, _1, vehicle_ros->vehicle_name)); // todo ros::TransportHints().tcpNoDelay(); + drone->vel_cmd_world_frame_sub = nh_private_.subscribe(curr_vehicle_name + "/vel_cmd_world_frame", 1, + boost::bind(&AirsimROSWrapper::vel_cmd_world_frame_cb, this, _1, vehicle_ros->vehicle_name)); + + drone->takeoff_srvr = nh_private_.advertiseService(curr_vehicle_name + "/takeoff", + boost::bind(&AirsimROSWrapper::takeoff_srv_cb, this, _1, _2, vehicle_ros->vehicle_name) ); + drone->land_srvr = nh_private_.advertiseService(curr_vehicle_name + "/land", + boost::bind(&AirsimROSWrapper::land_srv_cb, this, _1, _2, vehicle_ros->vehicle_name) ); + // vehicle_ros.reset_srvr = nh_private_.advertiseService(curr_vehicle_name + "/reset",&AirsimROSWrapper::reset_srv_cb, this); + } + else + { + auto car = static_cast(vehicle_ros.get()); + car->car_cmd_sub = nh_private_.subscribe(curr_vehicle_name + "/car_cmd", 1, + boost::bind(&AirsimROSWrapper::car_cmd_cb, this, _1, vehicle_ros->vehicle_name)); + car->car_state_pub = nh_private_.advertise(curr_vehicle_name + "/car_state", 10); + } + + // iterate over camera map std::map .cameras; for (auto& curr_camera_elem : vehicle_setting->cameras) { auto& camera_setting = curr_camera_elem.second; auto& curr_camera_name = curr_camera_elem.first; // vehicle_setting_vec_.push_back(*vehicle_setting.get()); set_nans_to_zeros_in_pose(*vehicle_setting, camera_setting); - append_static_camera_tf(curr_vehicle_name, curr_camera_name, camera_setting); + append_static_camera_tf(vehicle_ros.get(), curr_camera_name, camera_setting); // camera_setting.gimbal std::vector current_image_request_vec; current_image_request_vec.clear(); @@ -186,62 +227,84 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() } - // iterate over sensors std::map> sensors; + // iterate over sensors + std::vector sensors; for (auto& curr_sensor_map : vehicle_setting->sensors) { auto& sensor_name = curr_sensor_map.first; auto& sensor_setting = curr_sensor_map.second; - switch (sensor_setting->sensor_type) - { - case SensorBase::SensorType::Barometer: - { - std::cout << "Barometer" << std::endl; - break; - } - case SensorBase::SensorType::Imu: - { - vehicle_imu_map_[curr_vehicle_name] = sensor_name; - // todo this is pretty non scalable, refactor airsim and ros api and maintain a vehicle <-> sensor (setting) map - std::cout << "Imu" << std::endl; - imu_pub_vec_.push_back(nh_private_.advertise (curr_vehicle_name + "/imu/" + sensor_name, 10)); - break; - } - case SensorBase::SensorType::Gps: - { - std::cout << "Gps" << std::endl; - break; - } - case SensorBase::SensorType::Magnetometer: - { - std::cout << "Magnetometer" << std::endl; - break; - } - case SensorBase::SensorType::Distance: - { - std::cout << "Distance" << std::endl; - break; - } - case SensorBase::SensorType::Lidar: + if (sensor_setting->enabled) + { + SensorPublisher sensor_publisher; + sensor_publisher.sensor_name = sensor_setting->sensor_name; + sensor_publisher.sensor_type = sensor_setting->sensor_type; + switch (sensor_setting->sensor_type) { - std::cout << "Lidar" << std::endl; - auto lidar_setting = *static_cast(sensor_setting.get()); - set_nans_to_zeros_in_pose(*vehicle_setting, lidar_setting); - append_static_lidar_tf(curr_vehicle_name, sensor_name, lidar_setting); // todo is there a more readable way to down-cast? - vehicle_lidar_map_[curr_vehicle_name] = sensor_name; // non scalable - lidar_pub_vec_.push_back(nh_private_.advertise (curr_vehicle_name + "/lidar/" + sensor_name, 10)); - break; - } - default: - { - throw std::invalid_argument("Unexpected sensor type"); + case SensorBase::SensorType::Barometer: + { + std::cout << "Barometer" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/altimeter/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Imu: + { + std::cout << "Imu" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/imu/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Gps: + { + std::cout << "Gps" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/gps/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Magnetometer: + { + std::cout << "Magnetometer" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/magnetometer/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Distance: + { + std::cout << "Distance" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/distance/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Lidar: + { + std::cout << "Lidar" << std::endl; + auto lidar_setting = *static_cast(sensor_setting.get()); + set_nans_to_zeros_in_pose(*vehicle_setting, lidar_setting); + append_static_lidar_tf(vehicle_ros.get(), sensor_name, lidar_setting); + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/lidar/" + sensor_name, 10); + break; + } + default: + { + throw std::invalid_argument("Unexpected sensor type"); + } } + sensors.emplace_back(sensor_publisher); } } + + // we want fast access to the lidar sensors for callback handling, sort them out now + auto isLidar = std::function([](const SensorPublisher& pub) + { + return pub.sensor_type == SensorBase::SensorType::Lidar; + }); + size_t cnt = std::count_if( sensors.begin(), sensors.end(), isLidar); + lidar_cnt+=cnt; + vehicle_ros->lidar_pubs.resize(cnt); + vehicle_ros->sensor_pubs.resize(sensors.size() - cnt); + std::partition_copy (sensors.begin(), sensors.end(), vehicle_ros->lidar_pubs.begin(), vehicle_ros->sensor_pubs.begin(), isLidar); + + vehicle_name_ptr_map_.emplace(curr_vehicle_name, std::move(vehicle_ros)); // allows fast lookup in command callbacks in case of a lot of drones } // add takeoff and land all services if more than 2 drones - if (multirotor_ros_vec_.size() > 1) + if (vehicle_name_ptr_map_.size() > 1 && airsim_mode_ == AIRSIM_MODE::DRONE) { takeoff_all_srvr_ = nh_private_.advertiseService("all_robots/takeoff", &AirsimROSWrapper::takeoff_all_srv_cb, this); land_all_srvr_ = nh_private_.advertiseService("all_robots/land", &AirsimROSWrapper::land_all_srv_cb, this); @@ -261,43 +324,31 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() // todo add per vehicle reset in AirLib API reset_srvr_ = nh_private_.advertiseService("reset",&AirsimROSWrapper::reset_srv_cb, this); - // todo mimic gazebo's /use_sim_time feature which publishes airsim's clock time..via an rpc call?! - // clock_pub_ = nh_private_.advertise("clock", 10); + if (publish_clock_) + { + clock_pub_ = nh_private_.advertise("clock", 1); + } // if >0 cameras, add one more thread for img_request_timer_cb - if(airsim_img_request_vehicle_name_pair_vec_.size() > 0) + if(!airsim_img_request_vehicle_name_pair_vec_.empty()) { double update_airsim_img_response_every_n_sec; nh_private_.getParam("update_airsim_img_response_every_n_sec", update_airsim_img_response_every_n_sec); - bool separate_spinner = true; // todo debugging race condition - if(separate_spinner) - { - ros::TimerOptions timer_options(ros::Duration(update_airsim_img_response_every_n_sec), boost::bind(&AirsimROSWrapper::img_response_timer_cb, this, _1), &img_timer_cb_queue_); - airsim_img_response_timer_ = nh_private_.createTimer(timer_options); - is_used_img_timer_cb_queue_ = true; - } - else - { - airsim_img_response_timer_ = nh_private_.createTimer(ros::Duration(update_airsim_img_response_every_n_sec), &AirsimROSWrapper::img_response_timer_cb, this); - } + + ros::TimerOptions timer_options(ros::Duration(update_airsim_img_response_every_n_sec), boost::bind(&AirsimROSWrapper::img_response_timer_cb, this, _1), &img_timer_cb_queue_); + airsim_img_response_timer_ = nh_private_.createTimer(timer_options); + is_used_img_timer_cb_queue_ = true; } - if (lidar_pub_vec_.size() > 0) + // lidars update on their own callback/thread at a given rate + if (lidar_cnt > 0) { double update_lidar_every_n_sec; nh_private_.getParam("update_lidar_every_n_sec", update_lidar_every_n_sec); // nh_private_.setCallbackQueue(&lidar_timer_cb_queue_); - bool separate_spinner = true; // todo debugging race condition - if(separate_spinner) - { - ros::TimerOptions timer_options(ros::Duration(update_lidar_every_n_sec), boost::bind(&AirsimROSWrapper::lidar_timer_cb, this, _1), &lidar_timer_cb_queue_); - airsim_lidar_update_timer_ = nh_private_.createTimer(timer_options); - is_used_lidar_timer_cb_queue_ = true; - } - else - { - airsim_lidar_update_timer_ = nh_private_.createTimer(ros::Duration(update_lidar_every_n_sec), &AirsimROSWrapper::lidar_timer_cb, this); - } + ros::TimerOptions timer_options(ros::Duration(update_lidar_every_n_sec), boost::bind(&AirsimROSWrapper::lidar_timer_cb, this, _1), &lidar_timer_cb_queue_); + airsim_lidar_update_timer_ = nh_private_.createTimer(timer_options); + is_used_lidar_timer_cb_queue_ = true; } initialize_airsim(); @@ -306,81 +357,87 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() // todo: error check. if state is not landed, return error. bool AirsimROSWrapper::takeoff_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response, const std::string& vehicle_name) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - airsim_client_.takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? // response.success = else - airsim_client_.takeoffAsync(20, vehicle_name); + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); // response.success = + return true; } bool AirsimROSWrapper::takeoff_group_srv_cb(airsim_ros_pkgs::TakeoffGroup::Request& request, airsim_ros_pkgs::TakeoffGroup::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) for(const auto& vehicle_name : request.vehicle_names) - airsim_client_.takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? // response.success = else for(const auto& vehicle_name : request.vehicle_names) - airsim_client_.takeoffAsync(20, vehicle_name); + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); // response.success = + return true; } bool AirsimROSWrapper::takeoff_all_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - for(const auto& vehicle_name : vehicle_names_) - airsim_client_.takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + for(const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first)->waitOnLastTask(); // todo value for timeout_sec? // response.success = else - for(const auto& vehicle_name : vehicle_names_) - airsim_client_.takeoffAsync(20, vehicle_name); + for(const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first); // response.success = + return true; } bool AirsimROSWrapper::land_srv_cb(airsim_ros_pkgs::Land::Request& request, airsim_ros_pkgs::Land::Response& response, const std::string& vehicle_name) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - airsim_client_.landAsync(60, vehicle_name)->waitOnLastTask(); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); else - airsim_client_.landAsync(60, vehicle_name); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name); + return true; //todo } bool AirsimROSWrapper::land_group_srv_cb(airsim_ros_pkgs::LandGroup::Request& request, airsim_ros_pkgs::LandGroup::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) for(const auto& vehicle_name : request.vehicle_names) - airsim_client_.landAsync(60, vehicle_name)->waitOnLastTask(); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); else for(const auto& vehicle_name : request.vehicle_names) - airsim_client_.landAsync(60, vehicle_name); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name); + return true; //todo } bool AirsimROSWrapper::land_all_srv_cb(airsim_ros_pkgs::Land::Request& request, airsim_ros_pkgs::Land::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - for(const auto& vehicle_name : vehicle_names_) - airsim_client_.landAsync(60, vehicle_name)->waitOnLastTask(); + for(const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + static_cast(airsim_client_.get())->landAsync(60, vehicle_name_ptr_pair.first)->waitOnLastTask(); else - for(const auto& vehicle_name : vehicle_names_) - airsim_client_.landAsync(60, vehicle_name); + for(const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + static_cast(airsim_client_.get())->landAsync(60, vehicle_name_ptr_pair.first); + return true; //todo } @@ -388,7 +445,7 @@ bool AirsimROSWrapper::land_all_srv_cb(airsim_ros_pkgs::Land::Request& request, // todo not async remove waitonlasttask bool AirsimROSWrapper::reset_srv_cb(airsim_ros_pkgs::Reset::Request& request, airsim_ros_pkgs::Reset::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); airsim_client_.reset(); return true; //todo @@ -409,123 +466,146 @@ msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const tf2::Quaternion return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(), tf2_quat.z()); } +void AirsimROSWrapper::car_cmd_cb(const airsim_ros_pkgs::CarControls::ConstPtr& msg, const std::string& vehicle_name) +{ + std::lock_guard guard(drone_control_mutex_); + + auto car = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + car->car_cmd.throttle = msg->throttle; + car->car_cmd.steering = msg->steering; + car->car_cmd.brake = msg->brake; + car->car_cmd.handbrake = msg->handbrake; + car->car_cmd.is_manual_gear = msg->manual; + car->car_cmd.manual_gear = msg->manual_gear; + car->car_cmd.gear_immediate = msg->gear_immediate; + + car->has_car_cmd = true; +} + +msr::airlib::Pose AirsimROSWrapper::get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const +{ + return msr::airlib::Pose(msr::airlib::Vector3r(x, y, z), airlib_quat); +} + // void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd& msg, const std::string& vehicle_name) void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name) { - std::lock_guard guard(drone_control_mutex_); - - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; + std::lock_guard guard(drone_control_mutex_); + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + double roll, pitch, yaw; - tf2::Matrix3x3(get_tf2_quat(multirotor_ros_vec_[vehicle_idx].curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw + tf2::Matrix3x3(get_tf2_quat(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw // todo do actual body frame? - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = (msg->twist.linear.x * cos(yaw)) - (msg->twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = (msg->twist.linear.x * sin(yaw)) + (msg->twist.linear.y * cos(yaw)); //body frame - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg->twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.x = (msg->twist.linear.x * cos(yaw)) - (msg->twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll + drone->vel_cmd.y = (msg->twist.linear.x * sin(yaw)) + (msg->twist.linear.y * cos(yaw)); //body frame + drone->vel_cmd.z = msg->twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; // airsim uses degrees - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg->twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg->twist.angular.z); + drone->has_vel_cmd = true; } void AirsimROSWrapper::vel_cmd_group_body_frame_cb(const airsim_ros_pkgs::VelCmdGroup& msg) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); for(const auto& vehicle_name : msg.vehicle_names) { - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; - double roll, pitch, yaw; - tf2::Matrix3x3(get_tf2_quat(multirotor_ros_vec_[vehicle_idx].curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + double roll, pitch, yaw; + tf2::Matrix3x3(get_tf2_quat(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw + // todo do actual body frame? - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg.twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll + drone->vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame + drone->vel_cmd.z = msg.twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; // airsim uses degrees - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + drone->has_vel_cmd = true; } } // void AirsimROSWrapper::vel_cmd_all_body_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg) void AirsimROSWrapper::vel_cmd_all_body_frame_cb(const airsim_ros_pkgs::VelCmd& msg) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); // todo expose waitOnLastTask or nah? - for(const auto& vehicle_name : vehicle_names_) + for(auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; + auto drone = static_cast(vehicle_name_ptr_pair.second.get()); + double roll, pitch, yaw; - tf2::Matrix3x3(get_tf2_quat(multirotor_ros_vec_[vehicle_idx].curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw + tf2::Matrix3x3(get_tf2_quat(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw // todo do actual body frame? - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg.twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll + drone->vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame + drone->vel_cmd.z = msg.twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; // airsim uses degrees - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + drone->has_vel_cmd = true; } } void AirsimROSWrapper::vel_cmd_world_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = msg->twist.linear.x; - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = msg->twist.linear.y; - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg->twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg->twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + drone->vel_cmd.x = msg->twist.linear.x; + drone->vel_cmd.y = msg->twist.linear.y; + drone->vel_cmd.z = msg->twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg->twist.angular.z); + drone->has_vel_cmd = true; } // this is kinda unnecessary but maybe it makes life easier for the end user. void AirsimROSWrapper::vel_cmd_group_world_frame_cb(const airsim_ros_pkgs::VelCmdGroup& msg) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); for(const auto& vehicle_name : msg.vehicle_names) { - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; - - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = msg.twist.linear.x; - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = msg.twist.linear.y; - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg.twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + + drone->vel_cmd.x = msg.twist.linear.x; + drone->vel_cmd.y = msg.twist.linear.y; + drone->vel_cmd.z = msg.twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + drone->has_vel_cmd = true; } } void AirsimROSWrapper::vel_cmd_all_world_frame_cb(const airsim_ros_pkgs::VelCmd& msg) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); // todo expose waitOnLastTask or nah? - for(const auto& vehicle_name : vehicle_names_) + for(auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; - - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = msg.twist.linear.x; - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = msg.twist.linear.y; - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg.twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + auto drone = static_cast(vehicle_name_ptr_pair.second.get()); + + drone->vel_cmd.x = msg.twist.linear.x; + drone->vel_cmd.y = msg.twist.linear.y; + drone->vel_cmd.z = msg.twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + drone->has_vel_cmd = true; } } @@ -551,7 +631,7 @@ void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAng // todo support multiple gimbal commands // 1. find quaternion of default gimbal pose // 2. forward multiply with quaternion equivalent to desired euler commands (in degrees) -// 3. call airsim client's setcameraorientation which sets camera orientation wrt world (or takeoff?) ned frame. todo +// 3. call airsim client's setCameraPose which sets camera pose wrt world (or takeoff?) ned frame. todo void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAngleEulerCmd& gimbal_angle_euler_cmd_msg) { try @@ -570,37 +650,99 @@ void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAn } } -nav_msgs::Odometry AirsimROSWrapper::get_odom_msg_from_airsim_state(const msr::airlib::MultirotorState& drone_state) const +airsim_ros_pkgs::CarState AirsimROSWrapper::get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const +{ + airsim_ros_pkgs::CarState state_msg; + const auto odo = get_odom_msg_from_car_state(car_state); + + state_msg.pose = odo.pose; + state_msg.twist = odo.twist; + state_msg.speed = car_state.speed; + state_msg.gear = car_state.gear; + state_msg.rpm = car_state.rpm; + state_msg.maxrpm = car_state.maxrpm; + state_msg.handbrake = car_state.handbrake; + state_msg.header.stamp = airsim_timestamp_to_ros(car_state.timestamp); + + return state_msg; +} + +nav_msgs::Odometry AirsimROSWrapper::get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const +{ + nav_msgs::Odometry odom_msg; + + odom_msg.pose.pose.position.x = car_state.getPosition().x(); + odom_msg.pose.pose.position.y = car_state.getPosition().y(); + odom_msg.pose.pose.position.z = car_state.getPosition().z(); + odom_msg.pose.pose.orientation.x = car_state.getOrientation().x(); + odom_msg.pose.pose.orientation.y = car_state.getOrientation().y(); + odom_msg.pose.pose.orientation.z = car_state.getOrientation().z(); + odom_msg.pose.pose.orientation.w = car_state.getOrientation().w(); + + odom_msg.twist.twist.linear.x = car_state.kinematics_estimated.twist.linear.x(); + odom_msg.twist.twist.linear.y = car_state.kinematics_estimated.twist.linear.y(); + odom_msg.twist.twist.linear.z = car_state.kinematics_estimated.twist.linear.z(); + odom_msg.twist.twist.angular.x = car_state.kinematics_estimated.twist.angular.x(); + odom_msg.twist.twist.angular.y = car_state.kinematics_estimated.twist.angular.y(); + odom_msg.twist.twist.angular.z = car_state.kinematics_estimated.twist.angular.z(); + + if (isENU_) + { + std::swap(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y); + odom_msg.pose.pose.position.z = -odom_msg.pose.pose.position.z; + std::swap(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y); + odom_msg.pose.pose.orientation.z = -odom_msg.pose.pose.orientation.z; + std::swap(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y); + odom_msg.twist.twist.linear.z = -odom_msg.twist.twist.linear.z; + std::swap(odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y); + odom_msg.twist.twist.angular.z = -odom_msg.twist.twist.angular.z; + } + + return odom_msg; +} + +nav_msgs::Odometry AirsimROSWrapper::get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const { - nav_msgs::Odometry odom_ned_msg; - // odom_ned_msg.header.frame_id = world_frame_id_; - // odom_ned_msg.child_frame_id = "/airsim/odom_local_ned"; // todo make param - - odom_ned_msg.pose.pose.position.x = drone_state.getPosition().x(); - odom_ned_msg.pose.pose.position.y = drone_state.getPosition().y(); - odom_ned_msg.pose.pose.position.z = drone_state.getPosition().z(); - odom_ned_msg.pose.pose.orientation.x = drone_state.getOrientation().x(); - odom_ned_msg.pose.pose.orientation.y = drone_state.getOrientation().y(); - odom_ned_msg.pose.pose.orientation.z = drone_state.getOrientation().z(); - odom_ned_msg.pose.pose.orientation.w = drone_state.getOrientation().w(); - - odom_ned_msg.twist.twist.linear.x = drone_state.kinematics_estimated.twist.linear.x(); - odom_ned_msg.twist.twist.linear.y = drone_state.kinematics_estimated.twist.linear.y(); - odom_ned_msg.twist.twist.linear.z = drone_state.kinematics_estimated.twist.linear.z(); - odom_ned_msg.twist.twist.angular.x = drone_state.kinematics_estimated.twist.angular.x(); - odom_ned_msg.twist.twist.angular.y = drone_state.kinematics_estimated.twist.angular.y(); - odom_ned_msg.twist.twist.angular.z = drone_state.kinematics_estimated.twist.angular.z(); - - return odom_ned_msg; + nav_msgs::Odometry odom_msg; + + odom_msg.pose.pose.position.x = drone_state.getPosition().x(); + odom_msg.pose.pose.position.y = drone_state.getPosition().y(); + odom_msg.pose.pose.position.z = drone_state.getPosition().z(); + odom_msg.pose.pose.orientation.x = drone_state.getOrientation().x(); + odom_msg.pose.pose.orientation.y = drone_state.getOrientation().y(); + odom_msg.pose.pose.orientation.z = drone_state.getOrientation().z(); + odom_msg.pose.pose.orientation.w = drone_state.getOrientation().w(); + + odom_msg.twist.twist.linear.x = drone_state.kinematics_estimated.twist.linear.x(); + odom_msg.twist.twist.linear.y = drone_state.kinematics_estimated.twist.linear.y(); + odom_msg.twist.twist.linear.z = drone_state.kinematics_estimated.twist.linear.z(); + odom_msg.twist.twist.angular.x = drone_state.kinematics_estimated.twist.angular.x(); + odom_msg.twist.twist.angular.y = drone_state.kinematics_estimated.twist.angular.y(); + odom_msg.twist.twist.angular.z = drone_state.kinematics_estimated.twist.angular.z(); + + if (isENU_) + { + std::swap(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y); + odom_msg.pose.pose.position.z = -odom_msg.pose.pose.position.z; + std::swap(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y); + odom_msg.pose.pose.orientation.z = -odom_msg.pose.pose.orientation.z; + std::swap(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y); + odom_msg.twist.twist.linear.z = -odom_msg.twist.twist.linear.z; + std::swap(odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y); + odom_msg.twist.twist.angular.z = -odom_msg.twist.twist.angular.z; + } + + return odom_msg; } // https://docs.ros.org/jade/api/sensor_msgs/html/point__cloud__conversion_8h_source.html#l00066 // look at UnrealLidarSensor.cpp UnrealLidarSensor::getPointCloud() for math // read this carefully https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/PointCloud2.html -sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data) const +sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name) const { sensor_msgs::PointCloud2 lidar_msg; - lidar_msg.header.frame_id = world_frame_id_; // todo + lidar_msg.header.stamp = ros::Time::now(); + lidar_msg.header.frame_id = vehicle_name; if (lidar_data.point_cloud.size() > 3) { @@ -611,6 +753,7 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr:: lidar_msg.fields[0].name = "x"; lidar_msg.fields[1].name = "y"; lidar_msg.fields[2].name = "z"; + int offset = 0; for (size_t d = 0; d < lidar_msg.fields.size(); ++d, offset += 4) @@ -627,7 +770,7 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr:: lidar_msg.is_dense = true; // todo std::vector data_std = lidar_data.point_cloud; - const unsigned char* bytes = reinterpret_cast(&data_std[0]); + const unsigned char* bytes = reinterpret_cast(data_std.data()); vector lidar_msg_data(bytes, bytes + sizeof(float) * data_std.size()); lidar_msg.data = std::move(lidar_msg_data); } @@ -635,23 +778,116 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr:: { // msg = [] } + + if (isENU_) + { + try + { + sensor_msgs::PointCloud2 lidar_msg_enu; + auto transformStampedENU = tf_buffer_.lookupTransform(AIRSIM_FRAME_ID, vehicle_name, ros::Time(0), ros::Duration(1)); + tf2::doTransform(lidar_msg, lidar_msg_enu, transformStampedENU); + + lidar_msg_enu.header.stamp = lidar_msg.header.stamp; + lidar_msg_enu.header.frame_id = lidar_msg.header.frame_id; + + lidar_msg = std::move(lidar_msg_enu); + } + catch (tf2::TransformException &ex) + { + ROS_WARN("%s", ex.what()); + ros::Duration(1.0).sleep(); + } + } + return lidar_msg; } +airsim_ros_pkgs::Environment AirsimROSWrapper::get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const +{ + airsim_ros_pkgs::Environment env_msg; + env_msg.position.x = env_data.position.x(); + env_msg.position.y = env_data.position.y(); + env_msg.position.z = env_data.position.z(); + env_msg.geo_point.latitude = env_data.geo_point.latitude; + env_msg.geo_point.longitude = env_data.geo_point.longitude; + env_msg.geo_point.altitude = env_data.geo_point.altitude; + env_msg.gravity.x = env_data.gravity.x(); + env_msg.gravity.y = env_data.gravity.y(); + env_msg.gravity.z = env_data.gravity.z(); + env_msg.air_pressure = env_data.air_pressure; + env_msg.temperature = env_data.temperature; + env_msg.air_density = env_data.temperature; + + return env_msg; +} + +sensor_msgs::MagneticField AirsimROSWrapper::get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const +{ + sensor_msgs::MagneticField mag_msg; + mag_msg.magnetic_field.x = mag_data.magnetic_field_body.x(); + mag_msg.magnetic_field.y = mag_data.magnetic_field_body.y(); + mag_msg.magnetic_field.z = mag_data.magnetic_field_body.z(); + std::copy(std::begin(mag_data.magnetic_field_covariance), + std::end(mag_data.magnetic_field_covariance), + std::begin(mag_msg.magnetic_field_covariance)); + mag_msg.header.stamp = airsim_timestamp_to_ros(mag_data.time_stamp); + + return mag_msg; +} + +// todo covariances +sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const +{ + sensor_msgs::NavSatFix gps_msg; + gps_msg.header.stamp = airsim_timestamp_to_ros(gps_data.time_stamp); + gps_msg.latitude = gps_data.gnss.geo_point.latitude; + gps_msg.longitude = gps_data.gnss.geo_point.longitude; + gps_msg.altitude = gps_data.gnss.geo_point.altitude; + gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GLONASS; + gps_msg.status.status = gps_data.gnss.fix_type; + // gps_msg.position_covariance_type = + // gps_msg.position_covariance = + + return gps_msg; +} + +sensor_msgs::Range AirsimROSWrapper::get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const +{ + sensor_msgs::Range dist_msg; + dist_msg.header.stamp = airsim_timestamp_to_ros(dist_data.time_stamp); + dist_msg.range = dist_data.distance; + dist_msg.min_range = dist_data.min_distance; + dist_msg.max_range = dist_data.min_distance; + + return dist_msg; +} + +airsim_ros_pkgs::Altimeter AirsimROSWrapper::get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const +{ + airsim_ros_pkgs::Altimeter alt_msg; + alt_msg.header.stamp = airsim_timestamp_to_ros(alt_data.time_stamp); + alt_msg.altitude = alt_data.altitude; + alt_msg.pressure = alt_data.pressure; + alt_msg.qnh = alt_data.qnh; + + return alt_msg; +} + // todo covariances sensor_msgs::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const { sensor_msgs::Imu imu_msg; // imu_msg.header.frame_id = "/airsim/odom_local_ned";// todo multiple drones + imu_msg.header.stamp = airsim_timestamp_to_ros(imu_data.time_stamp); imu_msg.orientation.x = imu_data.orientation.x(); imu_msg.orientation.y = imu_data.orientation.y(); imu_msg.orientation.z = imu_data.orientation.z(); imu_msg.orientation.w = imu_data.orientation.w(); // todo radians per second - imu_msg.angular_velocity.x = math_common::deg2rad(imu_data.angular_velocity.x()); - imu_msg.angular_velocity.y = math_common::deg2rad(imu_data.angular_velocity.y()); - imu_msg.angular_velocity.z = math_common::deg2rad(imu_data.angular_velocity.z()); + imu_msg.angular_velocity.x = imu_data.angular_velocity.x(); + imu_msg.angular_velocity.y = imu_data.angular_velocity.y(); + imu_msg.angular_velocity.z = imu_data.angular_velocity.z(); // meters/s2^m imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x(); @@ -665,18 +901,18 @@ sensor_msgs::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::Im return imu_msg; } -void AirsimROSWrapper::publish_odom_tf(const nav_msgs::Odometry& odom_ned_msg) +void AirsimROSWrapper::publish_odom_tf(const nav_msgs::Odometry& odom_msg) { geometry_msgs::TransformStamped odom_tf; - odom_tf.header = odom_ned_msg.header; - odom_tf.child_frame_id = odom_ned_msg.child_frame_id; - odom_tf.transform.translation.x = odom_ned_msg.pose.pose.position.x; - odom_tf.transform.translation.y = odom_ned_msg.pose.pose.position.y; - odom_tf.transform.translation.z = odom_ned_msg.pose.pose.position.z; - odom_tf.transform.rotation.x = odom_ned_msg.pose.pose.orientation.x; - odom_tf.transform.rotation.y = odom_ned_msg.pose.pose.orientation.y; - odom_tf.transform.rotation.z = odom_ned_msg.pose.pose.orientation.z; - odom_tf.transform.rotation.w = odom_ned_msg.pose.pose.orientation.w; + odom_tf.header = odom_msg.header; + odom_tf.child_frame_id = odom_msg.child_frame_id; + odom_tf.transform.translation.x = odom_msg.pose.pose.position.x; + odom_tf.transform.translation.y = odom_msg.pose.pose.position.y; + odom_tf.transform.translation.z = odom_msg.pose.pose.position.z; + odom_tf.transform.rotation.x = odom_msg.pose.pose.orientation.x; + odom_tf.transform.rotation.y = odom_msg.pose.pose.orientation.y; + odom_tf.transform.rotation.z = odom_msg.pose.pose.orientation.z; + odom_tf.transform.rotation.w = odom_msg.pose.pose.orientation.w; tf_broadcaster_.sendTransform(odom_tf); } @@ -698,109 +934,266 @@ sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_sensor_msg_from_airsim_geo_poin return gps_msg; } -// todo unused -// void AirsimROSWrapper::set_zero_vel_cmd() -// { -// vel_cmd_.x = 0.0; -// vel_cmd_.y = 0.0; -// vel_cmd_.z = 0.0; - -// vel_cmd_.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; -// vel_cmd_.yaw_mode.is_rate = false; +ros::Time AirsimROSWrapper::chrono_timestamp_to_ros(const std::chrono::system_clock::time_point& stamp) const +{ + auto dur = std::chrono::duration(stamp.time_since_epoch()); + ros::Time cur_time; + cur_time.fromSec(dur.count()); + return cur_time; +} -// // todo make class member or a fucntion -// double roll, pitch, yaw; -// tf2::Matrix3x3(get_tf2_quat(curr_drone_state_.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw -// vel_cmd_.yaw_mode.yaw_or_rate = yaw; -// } +ros::Time AirsimROSWrapper::airsim_timestamp_to_ros(const msr::airlib::TTimePoint& stamp) const +{ + // airsim appears to use chrono::system_clock with nanosecond precision + std::chrono::nanoseconds dur(stamp); + std::chrono::time_point tp(dur); + ros::Time cur_time = chrono_timestamp_to_ros(tp); + return cur_time; +} void AirsimROSWrapper::drone_state_timer_cb(const ros::TimerEvent& event) { try - { - std::lock_guard guard(drone_control_mutex_); - + { // todo this is global origin origin_geo_point_pub_.publish(origin_geo_point_msg_); - // iterate over drones - for (auto& multirotor_ros: multirotor_ros_vec_) + + // get the basic vehicle pose and environmental state + const auto now = update_state(); + + // on init, will publish 0 to /clock as expected for use_sim_time compatibility + if (!airsim_client_->simIsPaused()) + { + // airsim_client needs to provide the simulation time in a future version of the API + ros_clock_.clock = now; + } + // publish the simulation clock + if (publish_clock_) { - // get drone state from airsim - std::unique_lock lck(drone_control_mutex_); - multirotor_ros.curr_drone_state = airsim_client_.getMultirotorState(multirotor_ros.vehicle_name); - lck.unlock(); - ros::Time curr_ros_time = ros::Time::now(); - - // convert airsim drone state to ROS msgs - multirotor_ros.curr_odom_ned = get_odom_msg_from_airsim_state(multirotor_ros.curr_drone_state); - multirotor_ros.curr_odom_ned.header.frame_id = multirotor_ros.vehicle_name; - multirotor_ros.curr_odom_ned.child_frame_id = multirotor_ros.odom_frame_id; - multirotor_ros.curr_odom_ned.header.stamp = curr_ros_time; - - multirotor_ros.gps_sensor_msg = get_gps_sensor_msg_from_airsim_geo_point(multirotor_ros.curr_drone_state.gps_location); - multirotor_ros.gps_sensor_msg.header.stamp = curr_ros_time; - - // publish to ROS! - multirotor_ros.odom_local_ned_pub.publish(multirotor_ros.curr_odom_ned); - publish_odom_tf(multirotor_ros.curr_odom_ned); - multirotor_ros.global_gps_pub.publish(multirotor_ros.gps_sensor_msg); + clock_pub_.publish(ros_clock_); + } - // send control commands from the last callback to airsim - if (multirotor_ros.has_vel_cmd) + // publish vehicle state, odom, and all basic sensor types + publish_vehicle_state(); + + // send any commands out to the vehicles + update_commands(); + } + catch (rpc::rpc_error& e) + { + std::cout << "error" << std::endl; + std::string msg = e.get_error().as(); + std::cout << "Exception raised by the API:" << std::endl << msg << std::endl; + } +} + +void AirsimROSWrapper::update_and_publish_static_transforms(VehicleROS* vehicle_ros) +{ + if (vehicle_ros && !vehicle_ros->static_tf_msg_vec.empty()) + { + for (auto& static_tf_msg : vehicle_ros->static_tf_msg_vec) + { + static_tf_msg.header.stamp = vehicle_ros->stamp; + static_tf_pub_.sendTransform(static_tf_msg); + } + } +} + +ros::Time AirsimROSWrapper::update_state() +{ + bool got_sim_time = false; + ros::Time curr_ros_time = ros::Time::now(); + + //should be easier way to get the sim time through API, something like: + //msr::airlib::Environment::State env = airsim_client_->simGetGroundTruthEnvironment(""); + //curr_ros_time = airsim_timestamp_to_ros(env.clock().nowNanos()); + + // iterate over drones + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + { + ros::Time vehicle_time; + // get drone state from airsim + auto& vehicle_ros = vehicle_name_ptr_pair.second; + + // vehicle environment, we can get ambient temperature here and other truths + auto env_data = airsim_client_->simGetGroundTruthEnvironment(vehicle_ros->vehicle_name); + + if (airsim_mode_ == AIRSIM_MODE::DRONE) + { + auto drone = static_cast(vehicle_ros.get()); + auto rpc = static_cast(airsim_client_.get()); + drone->curr_drone_state = rpc->getMultirotorState(vehicle_ros->vehicle_name); + + vehicle_time = airsim_timestamp_to_ros(drone->curr_drone_state.timestamp); + if (!got_sim_time) { - std::unique_lock lck(drone_control_mutex_); - airsim_client_.moveByVelocityAsync(multirotor_ros.vel_cmd.x, multirotor_ros.vel_cmd.y, multirotor_ros.vel_cmd.z, vel_cmd_duration_, - msr::airlib::DrivetrainType::MaxDegreeOfFreedom, multirotor_ros.vel_cmd.yaw_mode, multirotor_ros.vehicle_name); - lck.unlock(); + curr_ros_time = vehicle_time; + got_sim_time = true; } - // "clear" control cmds - multirotor_ros.has_vel_cmd = false; - } + vehicle_ros->gps_sensor_msg = get_gps_sensor_msg_from_airsim_geo_point(drone->curr_drone_state.gps_location); + vehicle_ros->gps_sensor_msg.header.stamp = vehicle_time; - // IMUS - if (imu_pub_vec_.size() > 0) + vehicle_ros->curr_odom = get_odom_msg_from_multirotor_state(drone->curr_drone_state); + } + else { - int ctr = 0; - for (const auto& vehicle_imu_pair: vehicle_imu_map_) + auto car = static_cast(vehicle_ros.get()); + auto rpc = static_cast(airsim_client_.get()); + car->curr_car_state = rpc->getCarState(vehicle_ros->vehicle_name); + + vehicle_time = airsim_timestamp_to_ros(car->curr_car_state.timestamp); + if (!got_sim_time) { - std::unique_lock lck(drone_control_mutex_); - auto imu_data = airsim_client_.getImuData(vehicle_imu_pair.second, vehicle_imu_pair.first); - lck.unlock(); - sensor_msgs::Imu imu_msg = get_imu_msg_from_airsim(imu_data); - imu_msg.header.frame_id = vehicle_imu_pair.first; - imu_msg.header.stamp = ros::Time::now(); - imu_pub_vec_[ctr].publish(imu_msg); - ctr++; - } + curr_ros_time = vehicle_time; + got_sim_time = true; + } + + vehicle_ros->gps_sensor_msg = get_gps_sensor_msg_from_airsim_geo_point(env_data.geo_point); + vehicle_ros->gps_sensor_msg.header.stamp = vehicle_time; + + vehicle_ros->curr_odom = get_odom_msg_from_car_state(car->curr_car_state); + + airsim_ros_pkgs::CarState state_msg = get_roscarstate_msg_from_car_state(car->curr_car_state); + state_msg.header.frame_id = vehicle_ros->vehicle_name; + car->car_state_msg = state_msg; } - if (static_tf_msg_vec_.size() > 0) + vehicle_ros->stamp = vehicle_time; + + airsim_ros_pkgs::Environment env_msg = get_environment_msg_from_airsim(env_data); + env_msg.header.frame_id = vehicle_ros->vehicle_name; + env_msg.header.stamp = vehicle_time; + vehicle_ros->env_msg = env_msg; + + // convert airsim drone state to ROS msgs + vehicle_ros->curr_odom.header.frame_id = vehicle_ros->vehicle_name; + vehicle_ros->curr_odom.child_frame_id = vehicle_ros->odom_frame_id; + vehicle_ros->curr_odom.header.stamp = vehicle_time; + } + + return curr_ros_time; +} + +void AirsimROSWrapper::publish_vehicle_state() +{ + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + { + auto& vehicle_ros = vehicle_name_ptr_pair.second; + + // simulation environment truth + vehicle_ros->env_pub.publish(vehicle_ros->env_msg); + + if (airsim_mode_ == AIRSIM_MODE::CAR) { - for (auto& static_tf_msg : static_tf_msg_vec_) + // dashboard reading from car, RPM, gear, etc + auto car = static_cast(vehicle_ros.get()); + car->car_state_pub.publish(car->car_state_msg); + } + + // odom and transforms + vehicle_ros->odom_local_pub.publish(vehicle_ros->curr_odom); + publish_odom_tf(vehicle_ros->curr_odom); + + // ground truth GPS position from sim/HITL + vehicle_ros->global_gps_pub.publish(vehicle_ros->gps_sensor_msg); + + for (auto& sensor_publisher : vehicle_ros->sensor_pubs) + { + switch (sensor_publisher.sensor_type) { - static_tf_msg.header.stamp = ros::Time::now(); - static_tf_pub_.sendTransform(static_tf_msg); + case SensorBase::SensorType::Barometer: + { + auto baro_data = airsim_client_->getBarometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + airsim_ros_pkgs::Altimeter alt_msg = get_altimeter_msg_from_airsim(baro_data); + alt_msg.header.frame_id = vehicle_ros->vehicle_name; + sensor_publisher.publisher.publish(alt_msg); + break; + } + case SensorBase::SensorType::Imu: + { + auto imu_data = airsim_client_->getImuData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + sensor_msgs::Imu imu_msg = get_imu_msg_from_airsim(imu_data); + imu_msg.header.frame_id = vehicle_ros->vehicle_name; + sensor_publisher.publisher.publish(imu_msg); + break; + } + case SensorBase::SensorType::Distance: + { + auto distance_data = airsim_client_->getDistanceSensorData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + sensor_msgs::Range dist_msg = get_range_from_airsim(distance_data); + dist_msg.header.frame_id = vehicle_ros->vehicle_name; + sensor_publisher.publisher.publish(dist_msg); + break; + } + case SensorBase::SensorType::Gps: + { + auto gps_data = airsim_client_->getGpsData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + sensor_msgs::NavSatFix gps_msg = get_gps_msg_from_airsim(gps_data); + gps_msg.header.frame_id = vehicle_ros->vehicle_name; + sensor_publisher.publisher.publish(gps_msg); + break; + } + case SensorBase::SensorType::Lidar: + { + // handled via callback + break; + } + case SensorBase::SensorType::Magnetometer: + { + auto mag_data = airsim_client_->getMagnetometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + sensor_msgs::MagneticField mag_msg = get_mag_msg_from_airsim(mag_data); + mag_msg.header.frame_id = vehicle_ros->vehicle_name; + sensor_publisher.publisher.publish(mag_msg); + break; + } } } - // todo add and expose a gimbal angular velocity to airlib - if (has_gimbal_cmd_) + update_and_publish_static_transforms(vehicle_ros.get()); + } +} + +void AirsimROSWrapper::update_commands() +{ + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + { + auto& vehicle_ros = vehicle_name_ptr_pair.second; + + if (airsim_mode_ == AIRSIM_MODE::DRONE) { - std::unique_lock lck(drone_control_mutex_); - airsim_client_.simSetCameraOrientation(gimbal_cmd_.camera_name, gimbal_cmd_.target_quat, gimbal_cmd_.vehicle_name); - lck.unlock(); - } + auto drone = static_cast(vehicle_ros.get()); - has_gimbal_cmd_ = false; + // send control commands from the last callback to airsim + if (drone->has_vel_cmd) + { + std::lock_guard guard(drone_control_mutex_); + static_cast(airsim_client_.get())->moveByVelocityAsync(drone->vel_cmd.x, drone->vel_cmd.y, drone->vel_cmd.z, vel_cmd_duration_, + msr::airlib::DrivetrainType::MaxDegreeOfFreedom, drone->vel_cmd.yaw_mode, drone->vehicle_name); + } + drone->has_vel_cmd = false; + } + else + { + // send control commands from the last callback to airsim + auto car = static_cast(vehicle_ros.get()); + if (car->has_car_cmd) + { + std::lock_guard guard(drone_control_mutex_); + static_cast(airsim_client_.get())->setCarControls(car->car_cmd, vehicle_ros->vehicle_name); + } + car->has_car_cmd = false; + } } - catch (rpc::rpc_error& e) + // Only camera rotation, no translation movement of camera + if (has_gimbal_cmd_) { - std::cout << "error" << std::endl; - std::string msg = e.get_error().as(); - std::cout << "Exception raised by the API:" << std::endl << msg << std::endl; + std::lock_guard guard(drone_control_mutex_); + airsim_client_->simSetCameraPose(gimbal_cmd_.camera_name, get_airlib_pose(0, 0, 0, gimbal_cmd_.target_quat), gimbal_cmd_.vehicle_name); } + + has_gimbal_cmd_ = false; } // airsim uses nans for zeros in settings.json. we set them to zeros here for handling tfs in ROS @@ -869,15 +1262,15 @@ void AirsimROSWrapper::set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_s lidar_setting.rotation.roll = vehicle_setting.rotation.roll; } -void AirsimROSWrapper::append_static_vehicle_tf(const std::string& vehicle_name, const VehicleSetting& vehicle_setting) +void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting) { geometry_msgs::TransformStamped vehicle_tf_msg; vehicle_tf_msg.header.frame_id = world_frame_id_; vehicle_tf_msg.header.stamp = ros::Time::now(); - vehicle_tf_msg.child_frame_id = vehicle_name; + vehicle_tf_msg.child_frame_id = vehicle_ros->vehicle_name; vehicle_tf_msg.transform.translation.x = vehicle_setting.position.x(); vehicle_tf_msg.transform.translation.y = vehicle_setting.position.y(); - vehicle_tf_msg.transform.translation.z = vehicle_setting.position.z(); + vehicle_tf_msg.transform.translation.z = vehicle_setting.position.z(); tf2::Quaternion quat; quat.setRPY(vehicle_setting.rotation.roll, vehicle_setting.rotation.pitch, vehicle_setting.rotation.yaw); vehicle_tf_msg.transform.rotation.x = quat.x(); @@ -885,15 +1278,22 @@ void AirsimROSWrapper::append_static_vehicle_tf(const std::string& vehicle_name, vehicle_tf_msg.transform.rotation.z = quat.z(); vehicle_tf_msg.transform.rotation.w = quat.w(); - static_tf_msg_vec_.push_back(vehicle_tf_msg); + if (isENU_) + { + std::swap(vehicle_tf_msg.transform.translation.x, vehicle_tf_msg.transform.translation.y); + std::swap(vehicle_tf_msg.transform.rotation.x, vehicle_tf_msg.transform.rotation.y); + vehicle_tf_msg.transform.translation.z = -vehicle_tf_msg.transform.translation.z; + vehicle_tf_msg.transform.rotation.z = -vehicle_tf_msg.transform.rotation.z; + } + + vehicle_ros->static_tf_msg_vec.emplace_back(vehicle_tf_msg); } -void AirsimROSWrapper::append_static_lidar_tf(const std::string& vehicle_name, const std::string& lidar_name, const LidarSetting& lidar_setting) +void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const LidarSetting& lidar_setting) { - geometry_msgs::TransformStamped lidar_tf_msg; - lidar_tf_msg.header.frame_id = vehicle_name + "/odom_local_ned"; // todo multiple drones - lidar_tf_msg.child_frame_id = lidar_name; + lidar_tf_msg.header.frame_id = vehicle_ros->vehicle_name + "/" + odom_frame_id_; + lidar_tf_msg.child_frame_id = vehicle_ros->vehicle_name + "/" + lidar_name; lidar_tf_msg.transform.translation.x = lidar_setting.position.x(); lidar_tf_msg.transform.translation.y = lidar_setting.position.y(); lidar_tf_msg.transform.translation.z = lidar_setting.position.z(); @@ -904,13 +1304,21 @@ void AirsimROSWrapper::append_static_lidar_tf(const std::string& vehicle_name, c lidar_tf_msg.transform.rotation.z = quat.z(); lidar_tf_msg.transform.rotation.w = quat.w(); - static_tf_msg_vec_.push_back(lidar_tf_msg); + if (isENU_) + { + std::swap(lidar_tf_msg.transform.translation.x, lidar_tf_msg.transform.translation.y); + std::swap(lidar_tf_msg.transform.rotation.x, lidar_tf_msg.transform.rotation.y); + lidar_tf_msg.transform.translation.z = -lidar_tf_msg.transform.translation.z; + lidar_tf_msg.transform.rotation.z = -lidar_tf_msg.transform.rotation.z; + } + + vehicle_ros->static_tf_msg_vec.emplace_back(lidar_tf_msg); } -void AirsimROSWrapper::append_static_camera_tf(const std::string& vehicle_name, const std::string& camera_name, const CameraSetting& camera_setting) +void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting) { geometry_msgs::TransformStamped static_cam_tf_body_msg; - static_cam_tf_body_msg.header.frame_id = vehicle_name + "/odom_local_ned"; + static_cam_tf_body_msg.header.frame_id = vehicle_ros->vehicle_name + "/" + odom_frame_id_; static_cam_tf_body_msg.child_frame_id = camera_name + "_body/static"; static_cam_tf_body_msg.transform.translation.x = camera_setting.position.x(); static_cam_tf_body_msg.transform.translation.y = camera_setting.position.y(); @@ -922,6 +1330,14 @@ void AirsimROSWrapper::append_static_camera_tf(const std::string& vehicle_name, static_cam_tf_body_msg.transform.rotation.z = quat.z(); static_cam_tf_body_msg.transform.rotation.w = quat.w(); + if (isENU_) + { + std::swap(static_cam_tf_body_msg.transform.translation.x, static_cam_tf_body_msg.transform.translation.y); + std::swap(static_cam_tf_body_msg.transform.rotation.x, static_cam_tf_body_msg.transform.rotation.y); + static_cam_tf_body_msg.transform.translation.z = -static_cam_tf_body_msg.transform.translation.z; + static_cam_tf_body_msg.transform.rotation.z = -static_cam_tf_body_msg.transform.rotation.z; + } + geometry_msgs::TransformStamped static_cam_tf_optical_msg = static_cam_tf_body_msg; static_cam_tf_optical_msg.child_frame_id = camera_name + "_optical/static"; @@ -937,8 +1353,8 @@ void AirsimROSWrapper::append_static_camera_tf(const std::string& vehicle_name, quat_cam_optical.normalize(); tf2::convert(quat_cam_optical, static_cam_tf_optical_msg.transform.rotation); - static_tf_msg_vec_.push_back(static_cam_tf_body_msg); - static_tf_msg_vec_.push_back(static_cam_tf_optical_msg); + vehicle_ros->static_tf_msg_vec.emplace_back(static_cam_tf_body_msg); + vehicle_ros->static_tf_msg_vec.emplace_back(static_cam_tf_optical_msg); } void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event) @@ -948,9 +1364,7 @@ void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event) int image_response_idx = 0; for (const auto& airsim_img_request_vehicle_name_pair : airsim_img_request_vehicle_name_pair_vec_) { - std::unique_lock lck(drone_control_mutex_); const std::vector& img_response = airsim_client_images_.simGetImages(airsim_img_request_vehicle_name_pair.first, airsim_img_request_vehicle_name_pair.second); - lck.unlock(); if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size()) { @@ -972,26 +1386,19 @@ void AirsimROSWrapper::lidar_timer_cb(const ros::TimerEvent& event) { try { - // std::lock_guard guard(drone_control_mutex_); - if (lidar_pub_vec_.size() > 0) + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - // std::lock_guard guard(lidar_mutex_); - int ctr = 0; - for (const auto& vehicle_lidar_pair: vehicle_lidar_map_) + if (!vehicle_name_ptr_pair.second->lidar_pubs.empty()) { - std::unique_lock lck(drone_control_mutex_); - auto lidar_data = airsim_client_lidar_.getLidarData(vehicle_lidar_pair.second, vehicle_lidar_pair.first); // airsim api is imu_name, vehicle_name - lck.unlock(); - sensor_msgs::PointCloud2 lidar_msg = get_lidar_msg_from_airsim(lidar_data); // todo make const ptr msg to avoid copy - lidar_msg.header.frame_id = vehicle_lidar_pair.second; // sensor frame name. todo add to doc - lidar_msg.header.stamp = ros::Time::now(); - lidar_pub_vec_[ctr].publish(lidar_msg); - ctr++; - } + for (auto& lidar_publisher : vehicle_name_ptr_pair.second->lidar_pubs) + { + auto lidar_data = airsim_client_lidar_.getLidarData(lidar_publisher.sensor_name, vehicle_name_ptr_pair.first); + sensor_msgs::PointCloud2 lidar_msg = get_lidar_msg_from_airsim(lidar_data, vehicle_name_ptr_pair.first); + lidar_publisher.publisher.publish(lidar_msg); + } + } } - } - catch (rpc::rpc_error& e) { std::string msg = e.get_error().as(); @@ -1013,12 +1420,12 @@ cv::Mat AirsimROSWrapper::manual_decode_depth(const ImageResponse& img_response) sensor_msgs::ImagePtr AirsimROSWrapper::get_img_msg_from_response(const ImageResponse& img_response, const ros::Time curr_ros_time, - const std::string frame_id) const + const std::string frame_id) { sensor_msgs::ImagePtr img_msg_ptr = boost::make_shared(); img_msg_ptr->data = img_response.image_data_uint8; img_msg_ptr->step = img_response.width * 3; // todo un-hardcode. image_width*num_bytes - img_msg_ptr->header.stamp = curr_ros_time; + img_msg_ptr->header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); img_msg_ptr->header.frame_id = frame_id; img_msg_ptr->height = img_response.height; img_msg_ptr->width = img_response.width; @@ -1031,13 +1438,13 @@ sensor_msgs::ImagePtr AirsimROSWrapper::get_img_msg_from_response(const ImageRes sensor_msgs::ImagePtr AirsimROSWrapper::get_depth_img_msg_from_response(const ImageResponse& img_response, const ros::Time curr_ros_time, - const std::string frame_id) const + const std::string frame_id) { // todo using img_response.image_data_float direclty as done get_img_msg_from_response() throws an error, // hence the dependency on opencv and cv_bridge. however, this is an extremely fast op, so no big deal. cv::Mat depth_img = manual_decode_depth(img_response); sensor_msgs::ImagePtr depth_img_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_img).toImageMsg(); - depth_img_msg->header.stamp = curr_ros_time; + depth_img_msg->header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); depth_img_msg->header.frame_id = frame_id; return depth_img_msg; } @@ -1118,6 +1525,14 @@ void AirsimROSWrapper::publish_camera_tf(const ImageResponse& img_response, cons cam_tf_body_msg.transform.rotation.z = img_response.camera_orientation.z(); cam_tf_body_msg.transform.rotation.w = img_response.camera_orientation.w(); + if (isENU_) + { + std::swap(cam_tf_body_msg.transform.translation.x, cam_tf_body_msg.transform.translation.y); + std::swap(cam_tf_body_msg.transform.rotation.x, cam_tf_body_msg.transform.rotation.y); + cam_tf_body_msg.transform.translation.z = -cam_tf_body_msg.transform.translation.z; + cam_tf_body_msg.transform.rotation.z = -cam_tf_body_msg.transform.rotation.z; + } + geometry_msgs::TransformStamped cam_tf_optical_msg; cam_tf_optical_msg.header.stamp = ros_time; cam_tf_optical_msg.header.frame_id = frame_id; diff --git a/ros/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp b/ros/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp index 26f0638e9..e0be99330 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp @@ -51,6 +51,8 @@ bool AirSimSettingsParser::initializeSettings() std::cout << "simmode_name: " << simmode_name << std::endl; AirSimSettings::singleton().load(std::bind(&AirSimSettingsParser::getSimMode, this)); + + return true; } else { diff --git a/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp b/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp index 9ed9b5fd1..a50d0957b 100644 --- a/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp +++ b/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp @@ -127,6 +127,10 @@ bool PIDPositionController::local_position_goal_srv_cb(airsim_ros_pkgs::SetLocal reset_errors(); // todo return true; } + + // Already have goal, and have reached it + ROS_INFO_STREAM("[PIDPositionController] Already have goal and have reached it"); + return false; } bool PIDPositionController::local_position_goal_srv_override_cb(airsim_ros_pkgs::SetLocalPosition::Request& request, airsim_ros_pkgs::SetLocalPosition::Response& response) @@ -206,6 +210,10 @@ bool PIDPositionController::gps_goal_srv_cb(airsim_ros_pkgs::SetGPSPosition::Req reset_errors(); // todo return true; } + + // Already have goal, this shouldn't happen + ROS_INFO_STREAM("[PIDPositionController] Goal already received, ignoring!"); + return false; } // todo do relative altitude, or add an option for the same? diff --git a/ros/src/airsim_tutorial_pkgs/CMakeLists.txt b/ros/src/airsim_tutorial_pkgs/CMakeLists.txt index 1ded5756c..db27f90aa 100644 --- a/ros/src/airsim_tutorial_pkgs/CMakeLists.txt +++ b/ros/src/airsim_tutorial_pkgs/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros ) - catkin_package( INCLUDE_DIRS CATKIN_DEPENDS message_runtime roscpp std_msgs airsim_ros_pkgs diff --git a/ros/src/airsim_tutorial_pkgs/README.md b/ros/src/airsim_tutorial_pkgs/README.md index 340983e0e..e7a07ea8d 100644 --- a/ros/src/airsim_tutorial_pkgs/README.md +++ b/ros/src/airsim_tutorial_pkgs/README.md @@ -1,60 +1,3 @@ # AirSim ROS Tutorials -This is a set of sample AirSim `settings.json`s, roslaunch and rviz files to give a starting point for using AirSim with ROS. -See [airsim_ros_pkgs](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_ros_pkgs/README.md) for the ROS API. - - -## Setup -```shell -$ cd PATH_TO/AirSim/ros -$ catkin build airsim_tutorial_pkgs -``` - -## Examples - -### Single drone with monocular and depth cameras, and lidar - - Settings.json - [front_stereo_and_center_mono.json](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_tutorial_pkgs/settings/front_stereo_and_center_mono.json) - ```shell - $ source PATH_TO/AirSim/ros/devel/setup.bash - $ roscd airsim_tutorial_pkgs - $ cp settings/front_stereo_and_center_mono.json ~/Documents/AirSim/settings.json - - ## Start your unreal package or binary here - - $ roslaunch airsim_ros_pkgs airsim_node.launch; - - # in a new pane / terminal - $ roslaunch airsim_tutorial_pkgs front_stereo_and_center_mono.launch - ``` - The above would start rviz with tf's, registered RGBD cloud using [depth_image_proc](https://wiki.ros.org/depth_image_proc) using the [`depth_to_pointcloud` launch file](https://github.com/microsoft/AirSim/master/ros/src/airsim_tutorial_pkgs/launch/front_stereo_and_center_mono/depth_to_pointcloud.launch), and the lidar point cloud. - - -### Two drones, with cameras, lidar, IMU each -- Settings.json - [two_drones_camera_lidar_imu.json](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_tutorial_pkgs/settings/two_drones_camera_lidar_imu.json) - - ```shell - $ source PATH_TO/AirSim/ros/devel/setup.bash - $ roscd airsim_tutorial_pkgs - $ cp settings/two_drones_camera_lidar_imu.json ~/Documents/AirSim/settings.json - - ## Start your unreal package or binary here - - $ roslaunch airsim_ros_pkgs airsim_node.launch; - $ roslaunch airsim_ros_pkgs rviz.launch - ``` -You can view the tfs in rviz. And do a `rostopic list` and `rosservice list` to inspect the services avaiable. - -### Twenty-five drones in a square pattern -- Settings.json - [twenty_five_drones.json](https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_tutorial_pkgs/settings/twenty_five_drones.json) - - ```shell - $ source PATH_TO/AirSim/ros/devel/setup.bash - $ roscd airsim_tutorial_pkgs - $ cp settings/twenty_five_drones.json ~/Documents/AirSim/settings.json - - ## Start your unreal package or binary here - - $ roslaunch airsim_ros_pkgs airsim_node.launch; - $ roslaunch airsim_ros_pkgs rviz.launch - ``` -You can view the tfs in rviz. And do a `rostopic list` and `rosservice list` to inspect the services avaiable. +This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/airsim_tutorial_pkgs.md). \ No newline at end of file diff --git a/ros/src/airsim_tutorial_pkgs/settings/car.json b/ros/src/airsim_tutorial_pkgs/settings/car.json new file mode 100644 index 000000000..5969b9dc9 --- /dev/null +++ b/ros/src/airsim_tutorial_pkgs/settings/car.json @@ -0,0 +1,108 @@ +{ + "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md", + "SettingsVersion": 1.2, + "SimMode": "Car", + "ViewMode": "SpringArmChase", + "ClockSpeed": 1.0, + "Vehicles": { + "drone_1": { + "VehicleType": "PhysXCar", + "DefaultVehicleState": "Armed", + "EnableCollisionPassthrogh": false, + "EnableCollisions": true, + "AllowAPIAlways": true, + "Sensors": { + "Gps": { + "SensorType": 3, + "Enabled" : true + }, + "Barometer": { + "SensorType": 1, + "Enabled" : true + }, + "Magnetometer": { + "SensorType": 4, + "Enabled" : true + }, + "Imu" : { + "SensorType": 2, + "Enabled": true + }, + "LidarCustom": { + "SensorType": 6, + "Enabled": true, + "NumberOfChannels": 16, + "PointsPerSecond": 10000, + "X": 0, + "Y": 0, + "Z": -1.5, + "DrawDebugPoints": true, + "DataFrame": "SensorLocalFrame" + } + }, + "Cameras": { + "front_center_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "Width": 640, + "Height": 480, + "FOV_Degrees": 27, + "DepthOfFieldFstop": 2.8, + "DepthOfFieldFocalDistance": 200.0, + "DepthOfFieldFocalRegion": 200.0, + "TargetGamma": 1.5 + } + ], + "X": 1.75, "Y": 0, "Z": -1.25, + "Pitch": 0, "Roll": 0, "Yaw": 0 + }, + "front_left_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "Width": 672, + "Height": 376, + "FOV_Degrees": 90, + "TargetGamma": 1.5 + }, + { + "PublishToRos": 1, + "ImageType": 1, + "Width": 672, + "Height": 376, + "FOV_Degrees": 90, + "TargetGamma": 1.5 + } + ], + "X": 1.75, "Y": -0.06, "Z": -1.25, + "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 + }, + "front_right_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "Width": 672, + "Height": 376, + "FOV_Degrees": 90, + "TargetGamma": 1.5 + } + ], + "X": 1.75, "Y": 0.06, "Z": -1.25, + "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 + } + }, + "X": 0, "Y": 0, "Z": 0, + "Pitch": 0, "Roll": 0, "Yaw": 0 + } + }, + "SubWindows": [ + {"WindowID": 0, "ImageType": 0, "CameraName": "front_left_custom", "Visible": true}, + {"WindowID": 1, "ImageType": 0, "CameraName": "front_center_custom", "Visible": false}, + {"WindowID": 2, "ImageType": 0, "CameraName": "front_right_custom", "Visible": true} + ] +} + diff --git a/setup.sh b/setup.sh index 73cae4ffa..a9dccecdf 100755 --- a/setup.sh +++ b/setup.sh @@ -1,22 +1,12 @@ #! /bin/bash - -if [[ -d "llvm-source-39" ]]; then - echo "Hello there! We just upgraded AirSim to Unreal Engine 4.18." - echo "Here are few easy steps for upgrade so everything is new and shiny :)" - echo "https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md" - exit 1 -fi - set -x -# set -e +set -e SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" pushd "$SCRIPT_DIR" >/dev/null downloadHighPolySuv=true -gccBuild=false MIN_CMAKE_VERSION=3.10.0 -MIN_GCC_VERSION=6.0.0 function version_less_than_equal_to() { test "$(printf '%s\n' "$@" | sort -V | head -n 1)" = "$1"; } # Parse command line arguments @@ -29,69 +19,57 @@ case $key in downloadHighPolySuv=false shift # past value ;; - --gcc) - gccBuild=true - shift # past argument - ;; esac done -if $gccBuild; then - # gcc tools - gcc_ver=$(gcc -dumpfullversion) - gcc_path=$(which cmake) - if [[ "$gcc_path" == "" ]] ; then - gcc_ver=0 - fi - if version_less_than_equal_to $gcc_ver $MIN_GCC_VERSION; then - if [ "$(uname)" == "Darwin" ]; then # osx - brew update - brew install gcc-6 g++-6 - else - sudo add-apt-repository ppa:ubuntu-toolchain-r/test - sudo apt-get -y update - sudo apt-get install -y gcc-6 g++-6 - fi - else - echo "Already have good version of gcc: $gcc_ver" +# llvm tools +if [ "$(uname)" == "Darwin" ]; then # osx + brew update + brew tap llvm-hs/homebrew-llvm + brew install llvm@8 +else #linux + sudo apt-get update + sudo apt-get -y install --no-install-recommends \ + lsb-release \ + rsync \ + software-properties-common \ + wget \ + libvulkan1 \ + vulkan-utils + + #install clang and build tools + VERSION=$(lsb_release -rs | cut -d. -f1) + # Since Ubuntu 17 clang is part of the core repository + # See https://packages.ubuntu.com/search?keywords=clang-8 + if [ "$VERSION" -lt "17" ]; then + wget -O - http://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add - + sudo apt-get update fi + sudo apt-get install -y clang-8 clang++-8 libc++-8-dev libc++abi-8-dev +fi + +if ! which cmake; then + # CMake not installed + cmake_ver=0 else - # llvm tools - if [ "$(uname)" == "Darwin" ]; then # osx - brew update - - # brew install llvm@3.9 - brew tap llvm-hs/homebrew-llvm - brew install llvm-5.0 - export C_COMPILER=/usr/local/opt/llvm-5.0/bin/clang-5.0 - export COMPILER=/usr/local/opt/llvm-5.0/bin/clang++-5.0 - - else #linux - #install clang and build tools - - VERSION=$(lsb_release -rs | cut -d. -f1) - # Since Ubuntu 17 clang-5.0 is part of the core repository - # See https://packages.ubuntu.com/search?keywords=clang-5.0 - if [ "$VERSION" -lt "17" ]; then - wget -O - http://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - - sudo apt-get update - fi - sudo apt-get install -y clang-5.0 clang++-5.0 - export C_COMPILER=clang-5.0 - export COMPILER=clang++-5.0 - fi + cmake_ver=$(cmake --version 2>&1 | head -n1 | cut -d ' ' -f3 | awk '{print $NF}') fi #give user perms to access USB port - this is not needed if not using PX4 HIL #TODO: figure out how to do below in travis +# Install additional tools, CMake if required if [ "$(uname)" == "Darwin" ]; then # osx if [[ ! -z "${whoami}" ]]; then #this happens when running in travis sudo dseditgroup -o edit -a `whoami` -t user dialout fi - brew install wget - brew install coreutils - brew install cmake # should get cmake 3.8 + brew install wget coreutils + + if version_less_than_equal_to $cmake_ver $MIN_CMAKE_VERSION; then + brew install cmake # should get cmake 3.8 + else + echo "Already have good version of cmake: $cmake_ver" + fi else #linux if [[ ! -z "${whoami}" ]]; then #this happens when running in travis @@ -99,41 +77,46 @@ else #linux sudo usermod -a -G dialout $USER fi - #install additional tools - sudo apt-get install -y build-essential - sudo apt-get install -y unzip + # install additional tools + sudo apt-get install -y build-essential unzip + + if version_less_than_equal_to $cmake_ver $MIN_CMAKE_VERSION; then + # in ubuntu 18 docker CI, avoid building cmake from scratch to save time + # ref: https://apt.kitware.com/ + if [ "$(lsb_release -rs)" == "18.04" ]; then + sudo apt-get -y install \ + apt-transport-https \ + ca-certificates \ + gnupg + wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null + sudo apt-add-repository 'deb https://apt.kitware.com/ubuntu/ bionic main' + sudo apt-get -y install --no-install-recommends \ + make \ + cmake - cmake_ver=$(cmake --version 2>&1 | head -n1 | cut -d ' ' -f3 | awk '{print $NF}') - cmake_path=$(which cmake) - if [[ "$cmake_path" == "" ]] ; then - cmake_ver=0 - fi - - #download cmake - v3.10.2 is not out of box in Ubuntu 16.04 - if version_less_than_equal_to $gcc_ver $MIN_GCC_VERSION; then - if [[ ! -d "cmake_build/bin" ]]; then - echo "Downloading cmake..." - wget https://cmake.org/files/v3.10/cmake-3.10.2.tar.gz \ - -O cmake.tar.gz - tar -xzf cmake.tar.gz - rm cmake.tar.gz - rm -rf ./cmake_build - mv ./cmake-3.10.2 ./cmake_build - pushd cmake_build - ./bootstrap - make - popd - fi - if [ "$(uname)" == "Darwin" ]; then - CMAKE="$(greadlink -f cmake_build/bin/cmake)" else - CMAKE="$(readlink -f cmake_build/bin/cmake)" + # For Ubuntu 16.04, or anything else, build CMake 3.10.2 from source + if [[ ! -d "cmake_build/bin" ]]; then + echo "Downloading cmake..." + wget https://cmake.org/files/v3.10/cmake-3.10.2.tar.gz \ + -O cmake.tar.gz + tar -xzf cmake.tar.gz + rm cmake.tar.gz + rm -rf ./cmake_build + mv ./cmake-3.10.2 ./cmake_build + pushd cmake_build + ./bootstrap + make + popd + fi fi + else echo "Already have good version of cmake: $cmake_ver" - CMAKE=$(which cmake) fi -fi + +fi # End USB setup, CMake install + # Download rpclib RPC_VERSION=c4fb37acbe67ec99e47e5187acd2a7450bde0cec @@ -148,8 +131,8 @@ if [ ! -d "external/rpclib/rpclib-${RPC_VERSION}" ]; then rm -rf "external/rpclib" mkdir -p "external/rpclib" - unzip rpclib.zip -d external/rpclib - rm rpclib.zip + unzip -q v2.2.1.zip -d external/rpclib + rm v2.2.1.zip fi # Download high-polycount SUV model @@ -172,7 +155,7 @@ if $downloadHighPolySuv; then if [ -d "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" ]; then rm -rf "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" fi - unzip car_assets.zip -d ../Unreal/Plugins/AirSim/Content/VehicleAdv + unzip -q car_assets.zip -d ../Unreal/Plugins/AirSim/Content/VehicleAdv cd .. rm -rf "suv_download_tmp" fi @@ -180,66 +163,19 @@ else echo "### Not downloading high-poly car asset (--no-full-poly-car). The default unreal vehicle will be used." fi -# Below is alternative way to get clang by downloading binaries -# get clang, libc++ -# sudo rm -rf llvm-build -# mkdir -p llvm-build/output -# wget "http://releases.llvm.org/4.0.1/clang+llvm-4.0.1-x86_64-linux-gnu-debian8.tar.xz" -# tar -xf "clang+llvm-4.0.1-x86_64-linux-gnu-debian8.tar.xz" -C llvm-build/output - -# #other packages - not need for now -# #sudo apt-get install -y clang-3.9-doc libclang-common-3.9-dev libclang-3.9-dev libclang1-3.9 libclang1-3.9-dbg libllvm-3.9-ocaml-dev libllvm3.9 libllvm3.9-dbg lldb-3.9 llvm-3.9 llvm-3.9-dev llvm-3.9-doc llvm-3.9-examples llvm-3.9-runtime clang-format-3.9 python-clang-3.9 libfuzzer-3.9-dev - -#get libc++ source -if ! $gccBuild; then - echo "### Installing llvm 5 libc++ library..." - if [[ ! -d "llvm-source-50" ]]; then - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/llvm.git llvm-source-50 - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxx.git llvm-source-50/projects/libcxx - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxxabi.git llvm-source-50/projects/libcxxabi - else - echo "folder llvm-source-50 already exists, skipping git clone..." - fi - #build libc++ - if [ "$(uname)" == "Darwin" ]; then - rm -rf llvm-build - else - sudo rm -rf llvm-build - fi - mkdir -p llvm-build - pushd llvm-build >/dev/null - - "$CMAKE" -DCMAKE_C_COMPILER=${C_COMPILER} -DCMAKE_CXX_COMPILER=${COMPILER} \ - -LIBCXX_ENABLE_EXPERIMENTAL_LIBRARY=OFF -DLIBCXX_INSTALL_EXPERIMENTAL_LIBRARY=OFF \ - -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=./output \ - ../llvm-source-50 - - make cxx -j`nproc` - - #install libc++ locally in output folder - if [ "$(uname)" == "Darwin" ]; then - make install-libcxx install-libcxxabi - else - sudo make install-libcxx install-libcxxabi - fi - - popd >/dev/null -fi - -echo "Installing EIGEN library..." +echo "Installing Eigen library..." -if [ "$(uname)" == "Darwin" ]; then - rm -rf ./AirLib/deps/eigen3/Eigen +if [ ! -d "AirLib/deps/eigen3" ]; then + echo "Downloading Eigen..." + wget -O eigen3.zip https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip + unzip -q eigen3.zip -d temp_eigen + mkdir -p AirLib/deps/eigen3 + mv temp_eigen/eigen*/Eigen AirLib/deps/eigen3 + rm -rf temp_eigen + rm eigen3.zip else - sudo rm -rf ./AirLib/deps/eigen3/Eigen + echo "Eigen is already installed." fi -echo "downloading eigen..." -wget https://gitlab.com/libeigen/eigen/-/archive/3.3.2/eigen-3.3.2.zip -unzip eigen-3.3.2.zip -d temp_eigen -mkdir -p AirLib/deps/eigen3 -mv temp_eigen/eigen*/Eigen AirLib/deps/eigen3 -rm -rf temp_eigen -rm eigen-3.3.2.zip popd >/dev/null diff --git a/tools/install_ros_deps.sh b/tools/install_ros_deps.sh new file mode 100755 index 000000000..4480dfbe9 --- /dev/null +++ b/tools/install_ros_deps.sh @@ -0,0 +1,34 @@ +#!/usr/bin/env bash + +set -x + +DISTRO="$(lsb_release -sc)" +if [[ "$DISTRO" == "bionic" ]]; then + ROS_DISTRO="melodic" +elif [[ "$DISTRO" == "xenial" ]]; then + ROS_DISTRO="kinetic" +fi + +sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + +sudo apt update +sudo apt install ros-$ROS_DISTRO-desktop-full + +echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc + +sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential +sudo rosdep init +rosdep update + +# AirSim ROS Wrapper dependencies + +if [[ "$DISTRO" == "xenial" ]]; then + sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test + sudo apt-get update +fi + +sudo apt-get install gcc-8 g++-8 +sudo apt-get install ros-$ROS_DISTRO-mavros* +sudo apt-get install ros-$ROS_DISTRO-tf2-sensor-msgs +sudo apt-get install python-catkin-tools