diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000000..9f7e399e1d --- /dev/null +++ b/.clang-format @@ -0,0 +1,56 @@ +--- +# clang-format documentation +# http://clang.llvm.org/docs/ClangFormat.html +# http://clang.llvm.org/docs/ClangFormatStyleOptions.html + +# Preexisting formats: +# LLVM +# Google +# Chromium +# Mozilla +# WebKit + +Language: Cpp +BasedOnStyle: Mozilla + +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignTrailingComments: false +AllowShortBlocksOnASingleLine: false +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +BinPackArguments: false +BinPackParameters: true +BreakBeforeBraces: Custom +BraceWrapping: + AfterClass: true + AfterControlStatement: false + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterObjCDeclaration: false + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false +ContinuationIndentWidth: 4 +ConstructorInitializerIndentWidth: 4 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ColumnLimit: 0 +Cpp11BracedListStyle: false +IndentWidth: 4 +IndentCaseLabels: false +# KeepEmptyLinesAtTheStartOfBlocks: false +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: Inner +ReflowComments: false +PenaltyBreakBeforeFirstCallParameter: 100000 +PenaltyBreakComment: 100000 +SortIncludes: true +SpaceAfterTemplateKeyword: true +# Standard: Cpp11 # Broken +UseTab: Never +... diff --git a/.gitignore b/.gitignore index 628c35995d..ec5b0897c7 100644 --- a/.gitignore +++ b/.gitignore @@ -386,4 +386,3 @@ build_docs/ # api docs PythonClient/docs/_build -/LogViewer/LogViewer/publish.cmd diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index e124c386d0..fd9126248d 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -4,1383 +4,1313 @@ #ifndef airsim_core_AirSimSettings_hpp #define airsim_core_AirSimSettings_hpp -#include -#include -#include -#include -#include -#include "Settings.hpp" #include "CommonStructs.hpp" -#include "common_utils/Utils.hpp" #include "ImageCaptureBase.hpp" +#include "Settings.hpp" +#include "common_utils/Utils.hpp" #include "sensors/SensorBase.hpp" +#include +#include +#include +#include +#include -namespace msr { namespace airlib { - -struct AirSimSettings { -private: - typedef common_utils::Utils Utils; - typedef ImageCaptureBase::ImageType ImageType; - -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 * 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"; - static constexpr char const * kSensorLocalFrame = "SensorLocalFrame"; - - static constexpr char const * kSimModeTypeMultirotor = "Multirotor"; - static constexpr char const * kSimModeTypeCar = "Car"; - static constexpr char const * kSimModeTypeComputerVision = "ComputerVision"; - - struct SubwindowSetting { - int window_index; - ImageType image_type; - bool visible; - std::string camera_name; - std::string vehicle_name; - - SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, - bool visible_val = false, const std::string& camera_name_val = "", const std::string& vehicle_name_val = "") - : window_index(window_index_val), image_type(image_type_val), - visible(visible_val), camera_name(camera_name_val), vehicle_name(vehicle_name_val) - { - } - }; - - struct RecordingSetting { - bool record_on_move = false; - float record_interval = 0.05f; - std::string folder = ""; - bool enabled = false; - - std::map > requests; +namespace msr +{ +namespace airlib +{ - RecordingSetting() + struct AirSimSettings + { + private: + typedef common_utils::Utils Utils; + typedef ImageCaptureBase::ImageType ImageType; + + 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* 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"; + static constexpr char const* kSensorLocalFrame = "SensorLocalFrame"; + + static constexpr char const* kSimModeTypeMultirotor = "Multirotor"; + static constexpr char const* kSimModeTypeCar = "Car"; + static constexpr char const* kSimModeTypeComputerVision = "ComputerVision"; + + struct SubwindowSetting { - } + int window_index; + ImageType image_type; + bool visible; + std::string camera_name; + std::string vehicle_name; + + SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, + bool visible_val = false, const std::string& camera_name_val = "", const std::string& vehicle_name_val = "") + : window_index(window_index_val), image_type(image_type_val), visible(visible_val), camera_name(camera_name_val), vehicle_name(vehicle_name_val) + { + } + }; - RecordingSetting(bool record_on_move_val, float record_interval_val, const std::string& folder_val, bool enabled_val) - : record_on_move(record_on_move_val), record_interval(record_interval_val), folder(folder_val), - enabled(enabled_val) + struct RecordingSetting { - } - }; + bool record_on_move = false; + float record_interval = 0.05f; + std::string folder = ""; + bool enabled = false; - struct PawnPath { - std::string pawn_bp; - std::string slippery_mat; - std::string non_slippery_mat; + std::map> requests; - 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") - : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val) - { - } - }; - - struct RCSettings { - int remote_control_id = -1; - bool allow_api_when_disconnected = false; - }; + RecordingSetting() + { + } - struct Rotation { - float yaw = 0; - float pitch = 0; - float roll = 0; + RecordingSetting(bool record_on_move_val, float record_interval_val, const std::string& folder_val, bool enabled_val) + : record_on_move(record_on_move_val), record_interval(record_interval_val), folder(folder_val), enabled(enabled_val) + { + } + }; - Rotation() + struct PawnPath { - } + std::string pawn_bp; + std::string slippery_mat; + std::string non_slippery_mat; + + 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") + : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val) + { + } + }; - Rotation(float yaw_val, float pitch_val, float roll_val) - : yaw(yaw_val), pitch(pitch_val), roll(roll_val) + struct RCSettings { - } + int remote_control_id = -1; + bool allow_api_when_disconnected = false; + }; - static Rotation nanRotation() + struct Rotation { - static const Rotation val(Utils::nan(), Utils::nan(), Utils::nan()); - return val; - } - }; - - - struct GimbalSetting { - float stabilization = 0; - //bool is_world_frame = false; - Rotation rotation = Rotation::nanRotation(); - }; - - struct CaptureSetting { - //below settings_json are obtained by using Unreal console command (press ~): - // ShowFlag.VisualizeHDR 1. - //to replicate camera settings_json to SceneCapture2D - //TODO: should we use UAirBlueprintLib::GetDisplayGamma()? - static constexpr float kSceneTargetGamma = 1.4f; - - int image_type = 0; - - unsigned int width = 256, height = 144; //960 X 540 - float fov_degrees = Utils::nan(); //90.0f - int auto_exposure_method = -1; //histogram - float auto_exposure_speed = Utils::nan(); // 100.0f; - float auto_exposure_bias = Utils::nan(); // 0; - float auto_exposure_max_brightness = Utils::nan(); // 0.64f; - float auto_exposure_min_brightness = Utils::nan(); // 0.03f; - float auto_exposure_low_percent = Utils::nan(); // 80.0f; - float auto_exposure_high_percent = Utils::nan(); // 98.3f; - float auto_exposure_histogram_log_min = Utils::nan(); // -8; - float auto_exposure_histogram_log_max = Utils::nan(); // 4; - float motion_blur_amount = Utils::nan(); - float target_gamma = Utils::nan(); //1.0f; //This would be reset to kSceneTargetGamma for scene as default - int projection_mode = 0; // ECameraProjectionMode::Perspective - float ortho_width = Utils::nan(); - }; - - struct NoiseSetting { - int ImageType = 0; - - bool Enabled = false; - - float RandContrib = 0.2f; - float RandSpeed = 100000.0f; - float RandSize = 500.0f; - float RandDensity = 2.0f; - - float HorzWaveContrib = 0.03f; - float HorzWaveStrength = 0.08f; - float HorzWaveVertSize = 1.0f; - float HorzWaveScreenSize = 1.0f; - - float HorzNoiseLinesContrib = 1.0f; - float HorzNoiseLinesDensityY = 0.01f; - float HorzNoiseLinesDensityXY = 0.5f; + float yaw = 0; + float pitch = 0; + float roll = 0; - float HorzDistortionContrib = 1.0f; - float HorzDistortionStrength = 0.002f; - }; + Rotation() + { + } - struct CameraSetting { - //nan means keep the default values set in components - Vector3r position = VectorMath::nanVector(); - Rotation rotation = Rotation::nanRotation(); + Rotation(float yaw_val, float pitch_val, float roll_val) + : yaw(yaw_val), pitch(pitch_val), roll(roll_val) + { + } - GimbalSetting gimbal; - std::map capture_settings; - std::map noise_settings; + static Rotation nanRotation() + { + static const Rotation val(Utils::nan(), Utils::nan(), Utils::nan()); + return val; + } + }; - CameraSetting() + struct GimbalSetting { - initializeCaptureSettings(capture_settings); - initializeNoiseSettings(noise_settings); - } - }; - - struct CameraDirectorSetting { - Vector3r position = VectorMath::nanVector(); - Rotation rotation = Rotation::nanRotation(); - float follow_distance = Utils::nan(); - }; - - struct SensorSetting { - SensorBase::SensorType sensor_type; - std::string sensor_name; - bool enabled; - }; - - struct BarometerSetting : SensorSetting { - }; + float stabilization = 0; + //bool is_world_frame = false; + Rotation rotation = Rotation::nanRotation(); + }; - struct ImuSetting : SensorSetting { - }; + struct CaptureSetting + { + //below settings_json are obtained by using Unreal console command (press ~): + // ShowFlag.VisualizeHDR 1. + //to replicate camera settings_json to SceneCapture2D + //TODO: should we use UAirBlueprintLib::GetDisplayGamma()? + static constexpr float kSceneTargetGamma = 1.4f; + + int image_type = 0; + + unsigned int width = 256, height = 144; //960 X 540 + float fov_degrees = Utils::nan(); //90.0f + int auto_exposure_method = -1; //histogram + float auto_exposure_speed = Utils::nan(); // 100.0f; + float auto_exposure_bias = Utils::nan(); // 0; + float auto_exposure_max_brightness = Utils::nan(); // 0.64f; + float auto_exposure_min_brightness = Utils::nan(); // 0.03f; + float auto_exposure_low_percent = Utils::nan(); // 80.0f; + float auto_exposure_high_percent = Utils::nan(); // 98.3f; + float auto_exposure_histogram_log_min = Utils::nan(); // -8; + float auto_exposure_histogram_log_max = Utils::nan(); // 4; + float motion_blur_amount = Utils::nan(); + float target_gamma = Utils::nan(); //1.0f; //This would be reset to kSceneTargetGamma for scene as default + int projection_mode = 0; // ECameraProjectionMode::Perspective + float ortho_width = Utils::nan(); + }; - struct GpsSetting : SensorSetting { - }; + struct NoiseSetting + { + int ImageType = 0; - struct MagnetometerSetting : SensorSetting { - }; + bool Enabled = false; - 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; - bool external_controller = true; - }; + float RandContrib = 0.2f; + float RandSpeed = 100000.0f; + float RandSize = 500.0f; + float RandDensity = 2.0f; - struct LidarSetting : SensorSetting { + float HorzWaveContrib = 0.03f; + float HorzWaveStrength = 0.08f; + float HorzWaveVertSize = 1.0f; + float HorzWaveScreenSize = 1.0f; - // shared defaults - uint number_of_channels = 16; - real_T range = 10000.0f / 100; // meters - uint points_per_second = 100000; - uint horizontal_rotation_frequency = 10; // rotations/sec - float horizontal_FOV_start = 0; // degrees - float horizontal_FOV_end = 359; // degrees + float HorzNoiseLinesContrib = 1.0f; + float HorzNoiseLinesDensityY = 0.01f; + float HorzNoiseLinesDensityXY = 0.5f; - // 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(); - Rotation rotation = Rotation::nanRotation(); + float HorzDistortionContrib = 1.0f; + float HorzDistortionStrength = 0.002f; + }; - bool draw_debug_points = false; - std::string data_frame = AirSimSettings::kVehicleInertialFrame; + struct CameraSetting + { + //nan means keep the default values set in components + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); - bool external_controller = true; - }; + GimbalSetting gimbal; + std::map capture_settings; + std::map noise_settings; - struct VehicleSetting { - //required - std::string vehicle_name; - std::string vehicle_type; - - //optional - std::string default_vehicle_state; - std::string pawn_path; - bool allow_api_always = true; - bool auto_create = true; - bool enable_collision_passthrough = false; - 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(); - - std::map cameras; - std::map> sensors; - - RCSettings rc; - }; + CameraSetting() + { + initializeCaptureSettings(capture_settings); + initializeNoiseSettings(noise_settings); + } + }; - struct MavLinkConnectionInfo { - /* Default values are requires so uninitialized instance doesn't have random values */ + struct CameraDirectorSetting + { + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + float follow_distance = Utils::nan(); + }; - bool use_serial = true; // false means use UDP or TCP instead + struct SensorSetting + { + SensorBase::SensorType sensor_type; + std::string sensor_name; + bool enabled = true; + Settings settings; // imported json data that needs to be parsed by specific sensors. + }; - //Used to connect via HITL: needed only if use_serial = true - std::string serial_port = "*"; - int baud_rate = 115200; + struct BarometerSetting : SensorSetting + { + }; - // 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; + struct ImuSetting : SensorSetting + { + }; - // Used to accept connections from drone over TCP: needed only if use_tcp = true - bool use_tcp = false; - int tcp_port = 4560; + struct GpsSetting : SensorSetting + { + }; - // 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 control_ip_address = "127.0.0.1"; - int control_port = 14580; + struct MagnetometerSetting : SensorSetting + { + }; - // 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 = ""; + struct DistanceSetting : SensorSetting + { + }; - // 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 = ""; + struct LidarSetting : SensorSetting + { + }; - // mavlink vehicle identifiers - uint8_t sim_sysid = 142; - int sim_compid = 42; - uint8_t offboard_sysid = 134; - int offboard_compid = 1; - uint8_t vehicle_sysid = 135; - int vehicle_compid = 1; + struct VehicleSetting + { + //required + std::string vehicle_name; + std::string vehicle_type; + + //optional + std::string default_vehicle_state; + std::string pawn_path; + bool allow_api_always = true; + bool auto_create = true; + bool enable_collision_passthrough = false; + 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(); + + std::map cameras; + std::map> sensors; + + RCSettings rc; + }; - // 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"; + struct MavLinkConnectionInfo + { + /* Default values are requires so uninitialized instance doesn't have random values */ + + 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 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 lock_step = true; + bool use_tcp = false; + int tcp_port = 4560; + + // The PX4 SITL app requires receiving drone commands over a different mavlink channel called + // the "ground control station" (or GCS) channel. + std::string control_ip_address = "127.0.0.1"; + int control_port_local = 14540; + int control_port_remote = 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 = ""; + + // 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; + int sim_compid = 42; + uint8_t offboard_sysid = 134; + int offboard_compid = 1; + 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) + // 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; + std::string logs; + }; - std::string model = "Generic"; + struct MavLinkVehicleSetting : public VehicleSetting + { + MavLinkConnectionInfo connection_info; + }; - std::map params; - }; + struct SegmentationSetting + { + enum class InitMethodType + { + None, + CommonObjectsRandomIDs + }; - struct MavLinkVehicleSetting : public VehicleSetting { - MavLinkConnectionInfo connection_info; - }; + enum class MeshNamingMethodType + { + OwnerName, + StaticMeshName + }; - struct SegmentationSetting { - enum class InitMethodType { - None, CommonObjectsRandomIDs + InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs; + bool override_existing = false; + MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName; }; - enum class MeshNamingMethodType { - OwnerName, StaticMeshName + struct TimeOfDaySetting + { + bool enabled = false; + std::string start_datetime = ""; //format: %Y-%m-%d %H:%M:%S + bool is_start_datetime_dst = false; + float celestial_clock_speed = 1; + float update_interval_secs = 60; + bool move_sun = true; }; - InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs; - bool override_existing = false; - MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName; - }; + private: //fields + float settings_version_actual; + float settings_version_minimum = 1.2f; + + public: //fields + std::string simmode_name = ""; + std::string level_name = ""; + + std::vector subwindow_settings; + RecordingSetting recording_setting; + SegmentationSetting segmentation_setting; + TimeOfDaySetting tod_setting; + + std::vector warning_messages; + std::vector error_messages; + + bool is_record_ui_visible = false; + 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; + std::string physics_engine_name = ""; + + std::string clock_type = ""; + float clock_speed = 1.0f; + bool engine_sound = false; + bool log_messages_visible = true; + HomeGeoPoint origin_geopoint{ GeoPoint(47.641468, -122.140165, 122) }; //The geo-coordinate assigned to Unreal coordinate 0,0,0 + std::map pawn_paths; //path for pawn blueprint + std::map> vehicles; + CameraSetting camera_defaults; + CameraDirectorSetting camera_director; + float speed_unit_factor = 1.0f; + std::string speed_unit_label = "m\\s"; + std::map> sensor_defaults; + Vector3r wind = Vector3r::Zero(); + + std::string settings_text_ = ""; + + public: //methods + static AirSimSettings& singleton() + { + static AirSimSettings instance; + return instance; + } - struct TimeOfDaySetting { - bool enabled = false; - std::string start_datetime = ""; //format: %Y-%m-%d %H:%M:%S - bool is_start_datetime_dst = false; - float celestial_clock_speed = 1; - float update_interval_secs = 60; - bool move_sun = true; - }; + AirSimSettings() + { + initializeSubwindowSettings(subwindow_settings); + initializePawnPaths(pawn_paths); + } -private: //fields - float settings_version_actual; - float settings_version_minimum = 1.2f; - -public: //fields - std::string simmode_name = ""; - std::string level_name = ""; - - std::vector subwindow_settings; - RecordingSetting recording_setting; - SegmentationSetting segmentation_setting; - TimeOfDaySetting tod_setting; - - std::vector warning_messages; - std::vector error_messages; - - bool is_record_ui_visible = false; - 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; - std::string physics_engine_name = ""; - - std::string clock_type = ""; - float clock_speed = 1.0f; - bool engine_sound = false; - bool log_messages_visible = true; - HomeGeoPoint origin_geopoint{ GeoPoint(47.641468, -122.140165, 122) }; //The geo-coordinate assigned to Unreal coordinate 0,0,0 - std::map pawn_paths; //path for pawn blueprint - std::map> vehicles; - CameraSetting camera_defaults; - CameraDirectorSetting camera_director; - float speed_unit_factor = 1.0f; - std::string speed_unit_label = "m\\s"; - std::map> sensor_defaults; - Vector3r wind = Vector3r::Zero(); - - std::string settings_text_ = ""; - -public: //methods - static AirSimSettings& singleton() - { - static AirSimSettings instance; - return instance; - } + //returns number of warnings + void load(std::function simmode_getter) + { + warning_messages.clear(); + error_messages.clear(); + const Settings& settings_json = Settings::singleton(); + 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); + loadViewModeSettings(settings_json); + loadSegmentationSetting(settings_json, segmentation_setting); + loadPawnPaths(settings_json, pawn_paths); + loadOtherSettings(settings_json); + loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults); + loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults); + + //this should be done last because it depends on vehicles (and/or their type) we have + loadRecordingSetting(settings_json); + loadClockSettings(settings_json); + } - AirSimSettings() - { - initializeSubwindowSettings(subwindow_settings); - initializePawnPaths(pawn_paths); - } + static void initializeSettings(const std::string& json_settings_text) + { + singleton().settings_text_ = json_settings_text; + Settings& settings_json = Settings::loadJSonString(json_settings_text); + if (!settings_json.isLoadSuccess()) + throw std::invalid_argument("Cannot parse JSON settings_json string."); + } - //returns number of warnings - void load(std::function simmode_getter) - { - warning_messages.clear(); - error_messages.clear(); - const Settings& settings_json = Settings::singleton(); - 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); - loadViewModeSettings(settings_json); - loadSegmentationSetting(settings_json, segmentation_setting); - loadPawnPaths(settings_json, pawn_paths); - loadOtherSettings(settings_json); - loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults); - loadVehicleSettings(simmode_name, settings_json, vehicles); - - //this should be done last because it depends on vehicles (and/or their type) we have - loadRecordingSetting(settings_json); - loadClockSettings(settings_json); - } - - static void initializeSettings(const std::string& json_settings_text) - { - singleton().settings_text_ = json_settings_text; - Settings& settings_json = Settings::loadJSonString(json_settings_text); - if (! settings_json.isLoadSuccess()) - throw std::invalid_argument("Cannot parse JSON settings_json string."); - } + static void createDefaultSettingsFile() + { + initializeSettings("{}"); - static void createDefaultSettingsFile() - { - initializeSettings("{}"); + Settings& settings_json = Settings::singleton(); + //write some settings_json in new file otherwise the string "null" is written if all settings_json are empty + settings_json.setString("SeeDocsAt", "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md"); + settings_json.setDouble("SettingsVersion", 1.2); - Settings& settings_json = Settings::singleton(); - //write some settings_json in new file otherwise the string "null" is written if all settings_json are empty - settings_json.setString("SeeDocsAt", "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md"); - settings_json.setDouble("SettingsVersion", 1.2); + std::string settings_filename = Settings::getUserDirectoryFullPath("settings.json"); + //TODO: there is a crash in Linux due to settings_json.saveJSonString(). Remove this workaround after we only support Unreal 4.17 + //https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html + settings_json.saveJSonFile(settings_filename); + } - std::string settings_filename = Settings::getUserDirectoryFullPath("settings.json"); - //TODO: there is a crash in Linux due to settings_json.saveJSonString(). Remove this workaround after we only support Unreal 4.17 - //https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html - settings_json.saveJSonFile(settings_filename); - } + // This is for the case when a new vehicle is made on the fly, at runtime + void addVehicleSetting(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") + { + auto vehicle_setting = std::unique_ptr(new VehicleSetting()); - // This is for the case when a new vehicle is made on the fly, at runtime - void addVehicleSetting(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path="") - { - auto vehicle_setting = std::unique_ptr(new VehicleSetting()); + vehicle_setting->vehicle_name = vehicle_name; + vehicle_setting->vehicle_type = vehicle_type; + vehicle_setting->position = pose.position; + vehicle_setting->pawn_path = pawn_path; - vehicle_setting->vehicle_name = vehicle_name; - vehicle_setting->vehicle_type = vehicle_type; - vehicle_setting->position = pose.position; - vehicle_setting->pawn_path = pawn_path; + VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, vehicle_setting->rotation.roll, vehicle_setting->rotation.yaw); - VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, - vehicle_setting->rotation.roll, vehicle_setting->rotation.yaw); + vehicles[vehicle_name] = std::move(vehicle_setting); + } - vehicles[vehicle_name] = std::move(vehicle_setting); - } + const VehicleSetting* getVehicleSetting(const std::string& vehicle_name) const + { + auto it = vehicles.find(vehicle_name); + if (it == vehicles.end()) + // pre-existing flying pawns in Unreal Engine don't have name 'SimpleFlight' + it = vehicles.find("SimpleFlight"); + return it->second.get(); + } - const VehicleSetting* getVehicleSetting(const std::string& vehicle_name) const - { - auto it = vehicles.find(vehicle_name); - if (it == vehicles.end()) - // pre-existing flying pawns in Unreal Engine don't have name 'SimpleFlight' - it = vehicles.find("SimpleFlight"); - return it->second.get(); - } - -private: - void checkSettingsVersion(const Settings& settings_json) - { - bool has_default_settings = hasDefaultSettings(settings_json, settings_version_actual); - bool upgrade_required = settings_version_actual < settings_version_minimum; - if (upgrade_required) { - bool auto_upgrade = false; - - //if we have default setting file not modified by user then we will - //just auto-upgrade it - if (has_default_settings) { - auto_upgrade = true; - } - else { - //check if auto-upgrade is possible - if (settings_version_actual == 1) { - const std::vector all_changed_keys = { - "AdditionalCameras", "CaptureSettings", "NoiseSettings", - "UsageScenario", "SimpleFlight", "PX4" - }; - std::stringstream detected_keys_ss; - for (const auto& changed_key : all_changed_keys) { - if (settings_json.hasKey(changed_key)) - detected_keys_ss << changed_key << ","; - } - std::string detected_keys = detected_keys_ss.str(); - if (detected_keys.length()) { - std::string error_message = - "You are using newer version of AirSim with older version of settings.json. " - "You can either delete your settings.json and restart AirSim or use the upgrade " - "instructions at https://git.io/vjefh. \n\n" - "Following keys in your settings.json needs updating: " - ; - - error_messages.push_back(error_message + detected_keys); + 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()), + settings_json.getFloat("Z", default_vec.z())); + } + static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot) + { + return Rotation(settings_json.getFloat("Yaw", default_rot.yaw), + settings_json.getFloat("Pitch", default_rot.pitch), + settings_json.getFloat("Roll", default_rot.roll)); + } + + private: + void checkSettingsVersion(const Settings& settings_json) + { + bool has_default_settings = hasDefaultSettings(settings_json, settings_version_actual); + bool upgrade_required = settings_version_actual < settings_version_minimum; + if (upgrade_required) { + bool auto_upgrade = false; + + //if we have default setting file not modified by user then we will + //just auto-upgrade it + if (has_default_settings) { + auto_upgrade = true; + } + else { + //check if auto-upgrade is possible + if (settings_version_actual == 1) { + const std::vector all_changed_keys = { + "AdditionalCameras", "CaptureSettings", "NoiseSettings", "UsageScenario", "SimpleFlight", "PX4" + }; + std::stringstream detected_keys_ss; + for (const auto& changed_key : all_changed_keys) { + if (settings_json.hasKey(changed_key)) + detected_keys_ss << changed_key << ","; + } + std::string detected_keys = detected_keys_ss.str(); + if (detected_keys.length()) { + std::string error_message = + "You are using newer version of AirSim with older version of settings.json. " + "You can either delete your settings.json and restart AirSim or use the upgrade " + "instructions at https://git.io/vjefh. \n\n" + "Following keys in your settings.json needs updating: "; + + error_messages.push_back(error_message + detected_keys); + } + else + auto_upgrade = true; } else auto_upgrade = true; } - else - auto_upgrade = true; - } - if (auto_upgrade) { - warning_messages.push_back( - "You are using newer version of AirSim with older version of settings.json. " - "You should delete your settings.json file and restart AirSim."); + if (auto_upgrade) { + warning_messages.push_back( + "You are using newer version of AirSim with older version of settings.json. " + "You should delete your settings.json file and restart AirSim."); + } } + //else no action necessary } - //else no action necessary - } - bool hasDefaultSettings(const Settings& settings_json, float& version) - { - //if empty settings file - bool has_default = settings_json.size() == 0; + bool hasDefaultSettings(const Settings& settings_json, float& version) + { + //if empty settings file + bool has_default = settings_json.size() == 0; - bool has_docs = settings_json.getString("SeeDocsAt", "") != "" - || settings_json.getString("see_docs_at", "") != ""; - //we had spelling mistake so we are currently supporting SettingsVersion or SettingdVersion :( - version = settings_json.getFloat("SettingsVersion", settings_json.getFloat("SettingdVersion", 0)); + bool has_docs = settings_json.getString("SeeDocsAt", "") != "" || settings_json.getString("see_docs_at", "") != ""; + //we had spelling mistake so we are currently supporting SettingsVersion or SettingdVersion :( + version = settings_json.getFloat("SettingsVersion", settings_json.getFloat("SettingdVersion", 0)); - //If we have pre-V1 settings and only element is docs link - has_default |= settings_json.size() == 1 && has_docs; + //If we have pre-V1 settings and only element is docs link + has_default |= settings_json.size() == 1 && has_docs; - //if we have V1 settings and only elements are docs link and version - has_default |= settings_json.size() == 2 && has_docs && version > 0; + //if we have V1 settings and only elements are docs link and version + has_default |= settings_json.size() == 2 && has_docs && version > 0; - return has_default; - } + return has_default; + } - void loadCoreSimModeSettings(const Settings& settings_json, std::function simmode_getter) - { - //get the simmode from user if not specified - simmode_name = settings_json.getString("SimMode", ""); - if (simmode_name == "") { - if (simmode_getter) - simmode_name = simmode_getter(); - else - throw std::invalid_argument("simmode_name is not expected empty in SimModeBase"); + void loadCoreSimModeSettings(const Settings& settings_json, std::function simmode_getter) + { + //get the simmode from user if not specified + simmode_name = settings_json.getString("SimMode", ""); + if (simmode_name == "") { + if (simmode_getter) + simmode_name = simmode_getter(); + else + throw std::invalid_argument("simmode_name is not expected empty in SimModeBase"); + } + + physics_engine_name = settings_json.getString("PhysicsEngineName", ""); + if (physics_engine_name == "") { + if (simmode_name == kSimModeTypeMultirotor) + physics_engine_name = "FastPhysicsEngine"; + else + physics_engine_name = "PhysX"; //this value is only informational for now + } } - physics_engine_name = settings_json.getString("PhysicsEngineName", ""); - if (physics_engine_name == "") { - if (simmode_name == kSimModeTypeMultirotor) - physics_engine_name = "FastPhysicsEngine"; - else - 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 loadLevelSettings(const Settings& settings_json) - { - level_name = settings_json.getString("Default Environment", ""); - } - void loadViewModeSettings(const Settings& settings_json) - { - std::string view_mode_string = settings_json.getString("ViewMode", ""); + void loadViewModeSettings(const Settings& settings_json) + { + std::string view_mode_string = settings_json.getString("ViewMode", ""); - if (view_mode_string == "") { - if (simmode_name == kSimModeTypeMultirotor) - view_mode_string = "FlyWithMe"; - else if (simmode_name == kSimModeTypeComputerVision) - view_mode_string = "Fpv"; + if (view_mode_string == "") { + if (simmode_name == kSimModeTypeMultirotor) + view_mode_string = "FlyWithMe"; + else if (simmode_name == kSimModeTypeComputerVision) + view_mode_string = "Fpv"; + else + view_mode_string = "SpringArmChase"; + } + + if (view_mode_string == "Fpv") + initial_view_mode = 0; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV; + else if (view_mode_string == "GroundObserver") + initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER; + else if (view_mode_string == "FlyWithMe") + initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME; + else if (view_mode_string == "Manual") + initial_view_mode = 3; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL; + else if (view_mode_string == "SpringArmChase") + initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE; + else if (view_mode_string == "Backup") + initial_view_mode = 5; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP; + else if (view_mode_string == "NoDisplay") + initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; + else if (view_mode_string == "Front") + initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; else - view_mode_string = "SpringArmChase"; + error_messages.push_back("ViewMode setting is not recognized: " + view_mode_string); } - if (view_mode_string == "Fpv") - initial_view_mode = 0; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV; - else if (view_mode_string == "GroundObserver") - initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER; - else if (view_mode_string == "FlyWithMe") - initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME; - else if (view_mode_string == "Manual") - initial_view_mode = 3; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL; - else if (view_mode_string == "SpringArmChase") - initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE; - else if (view_mode_string == "Backup") - initial_view_mode = 5; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP; - else if (view_mode_string == "NoDisplay") - initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; - else if (view_mode_string == "Front") - initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; - else - error_messages.push_back("ViewMode setting is not recognized: " + view_mode_string); - } - - static void loadRCSetting(const std::string& simmode_name, const Settings& settings_json, RCSettings& rc_setting) - { - Settings rc_json; - if (settings_json.getChild("RC", rc_json)) { - rc_setting.remote_control_id = rc_json.getInt("RemoteControlID", - simmode_name == kSimModeTypeMultirotor ? 0 : -1); - rc_setting.allow_api_when_disconnected = rc_json.getBool("AllowAPIWhenDisconnected", - rc_setting.allow_api_when_disconnected); + static void loadRCSetting(const std::string& simmode_name, const Settings& settings_json, RCSettings& rc_setting) + { + Settings rc_json; + if (settings_json.getChild("RC", rc_json)) { + rc_setting.remote_control_id = rc_json.getInt("RemoteControlID", + simmode_name == kSimModeTypeMultirotor ? 0 : -1); + rc_setting.allow_api_when_disconnected = rc_json.getBool("AllowAPIWhenDisconnected", + rc_setting.allow_api_when_disconnected); + } } - } - static std::string getCameraName(const Settings& settings_json) - { - 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))); - } + static std::string getCameraName(const Settings& settings_json) + { + 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))); + } - void loadDefaultRecordingSettings() - { - recording_setting.requests.clear(); - // Add Scene image for each vehicle - for (const auto& vehicle : vehicles) { - recording_setting.requests[vehicle.first].push_back(ImageCaptureBase::ImageRequest( - "", ImageType::Scene, false, true)); + void loadDefaultRecordingSettings() + { + recording_setting.requests.clear(); + // Add Scene image for each vehicle + for (const auto& vehicle : vehicles) { + recording_setting.requests[vehicle.first].push_back(ImageCaptureBase::ImageRequest( + "", ImageType::Scene, false, true)); + } } - } - void loadRecordingSetting(const Settings& settings_json) - { - loadDefaultRecordingSettings(); - - Settings recording_json; - if (settings_json.getChild("Recording", recording_json)) { - recording_setting.record_on_move = recording_json.getBool("RecordOnMove", recording_setting.record_on_move); - recording_setting.record_interval = recording_json.getFloat("RecordInterval", recording_setting.record_interval); - recording_setting.folder = recording_json.getString("Folder", recording_setting.folder); - recording_setting.enabled = recording_json.getBool("Enabled", recording_setting.enabled); - - Settings req_cameras_settings; - if (recording_json.getChild("Cameras", req_cameras_settings)) { - // If 'Cameras' field is present, clear defaults - recording_setting.requests.clear(); - // Get name of the default vehicle to be used if "VehicleName" isn't specified - // Map contains a default vehicle if vehicles haven't been specified - std::string default_vehicle_name = vehicles.begin()->first; - - for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) { - Settings req_camera_settings; - - if (req_cameras_settings.getChild(child_index, req_camera_settings)) { - std::string camera_name = getCameraName(req_camera_settings); - ImageType image_type = Utils::toEnum( - req_camera_settings.getInt("ImageType", 0)); - bool compress = req_camera_settings.getBool("Compress", true); - bool pixels_as_float = req_camera_settings.getBool("PixelsAsFloat", false); - std::string vehicle_name = req_camera_settings.getString("VehicleName", default_vehicle_name); - - recording_setting.requests[vehicle_name].push_back(ImageCaptureBase::ImageRequest( - camera_name, image_type, pixels_as_float, compress)); + void loadRecordingSetting(const Settings& settings_json) + { + loadDefaultRecordingSettings(); + + Settings recording_json; + if (settings_json.getChild("Recording", recording_json)) { + recording_setting.record_on_move = recording_json.getBool("RecordOnMove", recording_setting.record_on_move); + recording_setting.record_interval = recording_json.getFloat("RecordInterval", recording_setting.record_interval); + recording_setting.folder = recording_json.getString("Folder", recording_setting.folder); + recording_setting.enabled = recording_json.getBool("Enabled", recording_setting.enabled); + + Settings req_cameras_settings; + if (recording_json.getChild("Cameras", req_cameras_settings)) { + // If 'Cameras' field is present, clear defaults + recording_setting.requests.clear(); + // Get name of the default vehicle to be used if "VehicleName" isn't specified + // Map contains a default vehicle if vehicles haven't been specified + std::string default_vehicle_name = vehicles.begin()->first; + + for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) { + Settings req_camera_settings; + + if (req_cameras_settings.getChild(child_index, req_camera_settings)) { + std::string camera_name = getCameraName(req_camera_settings); + ImageType image_type = Utils::toEnum( + req_camera_settings.getInt("ImageType", 0)); + bool compress = req_camera_settings.getBool("Compress", true); + bool pixels_as_float = req_camera_settings.getBool("PixelsAsFloat", false); + std::string vehicle_name = req_camera_settings.getString("VehicleName", default_vehicle_name); + + recording_setting.requests[vehicle_name].push_back(ImageCaptureBase::ImageRequest( + camera_name, image_type, pixels_as_float, compress)); + } } } } } - } - static void initializeCaptureSettings(std::map& capture_settings) - { - capture_settings.clear(); - for (int i = -1; i < Utils::toNumeric(ImageType::Count); ++i) { - capture_settings[i] = CaptureSetting(); + static void initializeCaptureSettings(std::map& capture_settings) + { + capture_settings.clear(); + for (int i = -1; i < Utils::toNumeric(ImageType::Count); ++i) { + capture_settings[i] = CaptureSetting(); + } + capture_settings.at(Utils::toNumeric(ImageType::Scene)).target_gamma = CaptureSetting::kSceneTargetGamma; } - capture_settings.at(Utils::toNumeric(ImageType::Scene)).target_gamma = CaptureSetting::kSceneTargetGamma; - } - static void loadCaptureSettings(const Settings& settings_json, std::map& capture_settings) - { - initializeCaptureSettings(capture_settings); - - 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; - if (json_parent.getChild(child_index, json_settings_child)) { - CaptureSetting capture_setting; - createCaptureSettings(json_settings_child, capture_setting); - capture_settings[capture_setting.image_type] = capture_setting; + static void loadCaptureSettings(const Settings& settings_json, std::map& capture_settings) + { + initializeCaptureSettings(capture_settings); + + 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; + if (json_parent.getChild(child_index, json_settings_child)) { + CaptureSetting capture_setting; + createCaptureSettings(json_settings_child, capture_setting); + capture_settings[capture_setting.image_type] = capture_setting; + } } } } - } - 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()); + 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()); - //TODO: we should be selecting remote if available else keyboard - //currently keyboard is not supported so use rc as default - vehicle_setting->rc.remote_control_id = 0; + //TODO: we should be selecting remote if available else keyboard + //currently keyboard is not supported so use rc as default + vehicle_setting->rc.remote_control_id = 0; - MavLinkConnectionInfo &connection_info = vehicle_setting->connection_info; - connection_info.sim_sysid = static_cast(settings_json.getInt("SimSysID", connection_info.sim_sysid)); - connection_info.sim_compid = settings_json.getInt("SimCompID", connection_info.sim_compid); + MavLinkConnectionInfo& connection_info = vehicle_setting->connection_info; + connection_info.sim_sysid = static_cast(settings_json.getInt("SimSysID", connection_info.sim_sysid)); + connection_info.sim_compid = settings_json.getInt("SimCompID", connection_info.sim_compid); - connection_info.vehicle_sysid = static_cast(settings_json.getInt("VehicleSysID", connection_info.vehicle_sysid)); - connection_info.vehicle_compid = settings_json.getInt("VehicleCompID", connection_info.vehicle_compid); + connection_info.vehicle_sysid = static_cast(settings_json.getInt("VehicleSysID", connection_info.vehicle_sysid)); + connection_info.vehicle_compid = settings_json.getInt("VehicleCompID", connection_info.vehicle_compid); - connection_info.offboard_sysid = static_cast(settings_json.getInt("OffboardSysID", connection_info.offboard_sysid)); - connection_info.offboard_compid = settings_json.getInt("OffboardCompID", connection_info.offboard_compid); + connection_info.offboard_sysid = static_cast(settings_json.getInt("OffboardSysID", connection_info.offboard_sysid)); + connection_info.offboard_compid = settings_json.getInt("OffboardCompID", connection_info.offboard_compid); - connection_info.logviewer_ip_address = settings_json.getString("LogViewerHostIp", connection_info.logviewer_ip_address); - connection_info.logviewer_ip_port = settings_json.getInt("LogViewerPort", connection_info.logviewer_ip_port); - connection_info.logviewer_ip_sport = settings_json.getInt("LogViewerSendPort", connection_info.logviewer_ip_sport); + connection_info.logviewer_ip_address = settings_json.getString("LogViewerHostIp", connection_info.logviewer_ip_address); + connection_info.logviewer_ip_port = settings_json.getInt("LogViewerPort", connection_info.logviewer_ip_port); + connection_info.logviewer_ip_sport = settings_json.getInt("LogViewerSendPort", connection_info.logviewer_ip_sport); - 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.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.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.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address); + connection_info.control_port_local = settings_json.getInt("ControlPort", connection_info.control_port_local); // legacy + connection_info.control_port_local = settings_json.getInt("ControlPortLocal", connection_info.control_port_local); + connection_info.control_port_remote = settings_json.getInt("ControlPortRemote", connection_info.control_port_remote); - 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; + 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_local = settings_json.getInt("SitlPort", connection_info.control_port_local); + } + + 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.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.lock_step = settings_json.getBool("LockStep", connection_info.lock_step); + 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); + connection_info.logs = settings_json.getString("Logs", connection_info.logs); + + 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; } - if (settings_json.hasKey("SitlPort")) + + static std::unique_ptr createVehicleSetting(const std::string& simmode_name, const Settings& settings_json, + const std::string vehicle_name, + std::map>& sensor_defaults) { - // backwards compat - connection_info.control_port = settings_json.getInt("SitlPort", connection_info.control_port); - } + auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", "")); - 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.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); + std::unique_ptr vehicle_setting; + 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 { + vehicle_setting = std::unique_ptr(new VehicleSetting()); + if (vehicle_type == kVehicleTypeSimpleFlight) { + //TODO: we should be selecting remote if available else keyboard + //currently keyboard is not supported so use rc as default + vehicle_setting->rc.remote_control_id = 0; + } } + vehicle_setting->vehicle_name = vehicle_name; + + //required settings_json + vehicle_setting->vehicle_type = vehicle_type; + + //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); + 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); + vehicle_setting->enable_trace = settings_json.getBool("EnableTrace", + vehicle_setting->enable_trace); + vehicle_setting->enable_collisions = settings_json.getBool("EnableCollisions", + vehicle_setting->enable_collisions); + vehicle_setting->is_fpv_vehicle = settings_json.getBool("IsFpvVehicle", + vehicle_setting->is_fpv_vehicle); + + loadRCSetting(simmode_name, settings_json, vehicle_setting->rc); + + vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position); + vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation); + + loadCameraSettings(settings_json, vehicle_setting->cameras); + loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults); + + return vehicle_setting; } - 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()), - settings_json.getFloat("Z", default_vec.z())); - } - static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot) - { - return Rotation(settings_json.getFloat("Yaw", default_rot.yaw), - settings_json.getFloat("Pitch", default_rot.pitch), - settings_json.getFloat("Roll", default_rot.roll)); - } + static void initializeVehicleSettings(const std::string& simmode_name, std::map>& vehicles) + { + vehicles.clear(); - static std::unique_ptr createVehicleSetting(const std::string& simmode_name, const Settings& settings_json, - const std::string vehicle_name) - { - auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", "")); - - std::unique_ptr vehicle_setting; - 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 { - vehicle_setting = std::unique_ptr(new VehicleSetting()); - if (vehicle_type == kVehicleTypeSimpleFlight) { - //TODO: we should be selecting remote if available else keyboard - //currently keyboard is not supported so use rc as default - vehicle_setting->rc.remote_control_id = 0; + //NOTE: Do not set defaults for vehicle type here. If you do then make sure + //to sync code in createVehicleSetting() as well. + if (simmode_name == kSimModeTypeMultirotor) { + // create simple flight as default multirotor + auto simple_flight_setting = std::unique_ptr(new VehicleSetting()); + simple_flight_setting->vehicle_name = "SimpleFlight"; + simple_flight_setting->vehicle_type = kVehicleTypeSimpleFlight; + // TODO: we should be selecting remote if available else keyboard + // currently keyboard is not supported so use rc as default + simple_flight_setting->rc.remote_control_id = 0; + vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting); + } + else if (simmode_name == kSimModeTypeCar) { + // create PhysX as default car vehicle + auto physx_car_setting = std::unique_ptr(new VehicleSetting()); + physx_car_setting->vehicle_name = "PhysXCar"; + physx_car_setting->vehicle_type = kVehicleTypePhysXCar; + vehicles[physx_car_setting->vehicle_name] = std::move(physx_car_setting); + } + else if (simmode_name == kSimModeTypeComputerVision) { + // create default computer vision vehicle + auto cv_setting = std::unique_ptr(new VehicleSetting()); + cv_setting->vehicle_name = "ComputerVision"; + cv_setting->vehicle_type = kVehicleTypeComputerVision; + vehicles[cv_setting->vehicle_name] = std::move(cv_setting); + } + else { + throw std::invalid_argument(Utils::stringf( + "Unknown SimMode: %s, failed to set default vehicle settings", simmode_name.c_str()) + .c_str()); } } - vehicle_setting->vehicle_name = vehicle_name; - - //required settings_json - vehicle_setting->vehicle_type = vehicle_type; - - //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); - 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); - vehicle_setting->enable_trace = settings_json.getBool("EnableTrace", - vehicle_setting->enable_trace); - vehicle_setting->enable_collisions = settings_json.getBool("EnableCollisions", - vehicle_setting->enable_collisions); - vehicle_setting->is_fpv_vehicle = settings_json.getBool("IsFpvVehicle", - vehicle_setting->is_fpv_vehicle); - - loadRCSetting(simmode_name, settings_json, vehicle_setting->rc); - - vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position); - vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation); - - loadCameraSettings(settings_json, vehicle_setting->cameras); - loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors); - - return vehicle_setting; - } - - static void initializeVehicleSettings(const std::string &simmode_name, std::map>& vehicles) - { - vehicles.clear(); - - //NOTE: Do not set defaults for vehicle type here. If you do then make sure - //to sync code in createVehicleSetting() as well. - if (simmode_name == kSimModeTypeMultirotor) { - // create simple flight as default multirotor - auto simple_flight_setting = std::unique_ptr(new VehicleSetting()); - simple_flight_setting->vehicle_name = "SimpleFlight"; - simple_flight_setting->vehicle_type = kVehicleTypeSimpleFlight; - // TODO: we should be selecting remote if available else keyboard - // currently keyboard is not supported so use rc as default - simple_flight_setting->rc.remote_control_id = 0; - vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting); - } - else if (simmode_name == kSimModeTypeCar) { - // create PhysX as default car vehicle - auto physx_car_setting = std::unique_ptr(new VehicleSetting()); - physx_car_setting->vehicle_name = "PhysXCar"; - physx_car_setting->vehicle_type = kVehicleTypePhysXCar; - vehicles[physx_car_setting->vehicle_name] = std::move(physx_car_setting); - } - else if (simmode_name == kSimModeTypeComputerVision) { - // create default computer vision vehicle - auto cv_setting = std::unique_ptr(new VehicleSetting()); - cv_setting->vehicle_name = "ComputerVision"; - cv_setting->vehicle_type = kVehicleTypeComputerVision; - vehicles[cv_setting->vehicle_name] = std::move(cv_setting); - } - else { - throw std::invalid_argument(Utils::stringf( - "Unknown SimMode: %s, failed to set default vehicle settings", simmode_name.c_str()).c_str()); - } - } - static void loadVehicleSettings(const std::string& simmode_name, const Settings& settings_json, - std::map>& vehicles) - { - initializeVehicleSettings(simmode_name, vehicles); + static void loadVehicleSettings(const std::string& simmode_name, const Settings& settings_json, + std::map>& vehicles, + std::map>& sensor_defaults) + { + initializeVehicleSettings(simmode_name, vehicles); - msr::airlib::Settings vehicles_child; - if (settings_json.getChild("Vehicles", vehicles_child)) { - std::vector keys; - vehicles_child.getChildNames(keys); + msr::airlib::Settings vehicles_child; + if (settings_json.getChild("Vehicles", vehicles_child)) { + std::vector keys; + vehicles_child.getChildNames(keys); - //remove default vehicles, if values are specified in settings - if (keys.size()) - vehicles.clear(); + //remove default vehicles, if values are specified in settings + if (keys.size()) + vehicles.clear(); - for (const auto& key : keys) { - msr::airlib::Settings child; - vehicles_child.getChild(key, child); - vehicles[key] = createVehicleSetting(simmode_name, child, key); + for (const auto& key : keys) { + msr::airlib::Settings child; + vehicles_child.getChild(key, child); + vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults); + } } } - } - static void initializePawnPaths(std::map& pawn_paths) - { - pawn_paths.clear(); - pawn_paths.emplace("BareboneCar", - PawnPath("Class'/AirSim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'")); - pawn_paths.emplace("DefaultCar", - PawnPath("Class'/AirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'")); - pawn_paths.emplace("DefaultQuadrotor", - 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) - { - initializePawnPaths(pawn_paths); + static void initializePawnPaths(std::map& pawn_paths) + { + pawn_paths.clear(); + pawn_paths.emplace("BareboneCar", + PawnPath("Class'/AirSim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'")); + pawn_paths.emplace("DefaultCar", + PawnPath("Class'/AirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'")); + pawn_paths.emplace("DefaultQuadrotor", + 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) + { + initializePawnPaths(pawn_paths); - msr::airlib::Settings pawn_paths_child; - if (settings_json.getChild("PawnPaths", pawn_paths_child)) { - std::vector keys; - pawn_paths_child.getChildNames(keys); + msr::airlib::Settings pawn_paths_child; + if (settings_json.getChild("PawnPaths", pawn_paths_child)) { + std::vector keys; + pawn_paths_child.getChildNames(keys); - for (const auto& key : keys) { - msr::airlib::Settings child; - pawn_paths_child.getChild(key, child); - pawn_paths[key] = createPathPawn(child); + for (const auto& key : keys) { + msr::airlib::Settings child; + pawn_paths_child.getChild(key, child); + pawn_paths[key] = createPathPawn(child); + } } } - } - static PawnPath createPathPawn(const Settings& settings_json) - { - auto paths = PawnPath(); - paths.pawn_bp = settings_json.getString("PawnBP", ""); - auto slippery_mat = settings_json.getString("SlipperyMat", ""); - auto non_slippery_mat = settings_json.getString("NonSlipperyMat", ""); + static PawnPath createPathPawn(const Settings& settings_json) + { + auto paths = PawnPath(); + paths.pawn_bp = settings_json.getString("PawnBP", ""); + auto slippery_mat = settings_json.getString("SlipperyMat", ""); + auto non_slippery_mat = settings_json.getString("NonSlipperyMat", ""); - if (slippery_mat != "") - paths.slippery_mat = slippery_mat; - if (non_slippery_mat != "") - paths.non_slippery_mat = non_slippery_mat; + if (slippery_mat != "") + paths.slippery_mat = slippery_mat; + if (non_slippery_mat != "") + paths.non_slippery_mat = non_slippery_mat; - return paths; - } + return paths; + } - static void loadSegmentationSetting(const Settings& settings_json, SegmentationSetting& segmentation_setting) - { - Settings json_parent; - if (settings_json.getChild("SegmentationSettings", json_parent)) { - std::string init_method = Utils::toLower(json_parent.getString("InitMethod", "")); - if (init_method == "" || init_method == "commonobjectsrandomids") - segmentation_setting.init_method = SegmentationSetting::InitMethodType::CommonObjectsRandomIDs; - else if (init_method == "none") - segmentation_setting.init_method = SegmentationSetting::InitMethodType::None; - else - //TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine? - throw std::invalid_argument(std::string("SegmentationSetting init_method has invalid value in settings_json ") + init_method); + static void loadSegmentationSetting(const Settings& settings_json, SegmentationSetting& segmentation_setting) + { + Settings json_parent; + if (settings_json.getChild("SegmentationSettings", json_parent)) { + std::string init_method = Utils::toLower(json_parent.getString("InitMethod", "")); + if (init_method == "" || init_method == "commonobjectsrandomids") + segmentation_setting.init_method = SegmentationSetting::InitMethodType::CommonObjectsRandomIDs; + else if (init_method == "none") + segmentation_setting.init_method = SegmentationSetting::InitMethodType::None; + else + //TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine? + throw std::invalid_argument(std::string("SegmentationSetting init_method has invalid value in settings_json ") + init_method); - segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", false); + segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", false); - std::string mesh_naming_method = Utils::toLower(json_parent.getString("MeshNamingMethod", "")); - if (mesh_naming_method == "" || mesh_naming_method == "ownername") - segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::OwnerName; - else if (mesh_naming_method == "staticmeshname") - segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::StaticMeshName; - else - throw std::invalid_argument(std::string("SegmentationSetting MeshNamingMethod has invalid value in settings_json ") + mesh_naming_method); + std::string mesh_naming_method = Utils::toLower(json_parent.getString("MeshNamingMethod", "")); + if (mesh_naming_method == "" || mesh_naming_method == "ownername") + segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::OwnerName; + else if (mesh_naming_method == "staticmeshname") + segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::StaticMeshName; + else + throw std::invalid_argument(std::string("SegmentationSetting MeshNamingMethod has invalid value in settings_json ") + mesh_naming_method); + } } - } - static void initializeNoiseSettings(std::map& noise_settings) - { - int image_count = Utils::toNumeric(ImageType::Count); - noise_settings.clear(); - for (int i = -1; i < image_count; ++i) - noise_settings[i] = NoiseSetting(); - } + static void initializeNoiseSettings(std::map& noise_settings) + { + const int image_count = Utils::toNumeric(ImageType::Count); + noise_settings.clear(); + for (int i = -1; i < image_count; ++i) + noise_settings[i] = NoiseSetting(); + } - static void loadNoiseSettings(const Settings& settings_json, std::map& noise_settings) - { - initializeNoiseSettings(noise_settings); - - 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; - if (json_parent.getChild(child_index, json_settings_child)) { - NoiseSetting noise_setting; - loadNoiseSetting(json_settings_child, noise_setting); - noise_settings[noise_setting.ImageType] = noise_setting; + static void loadNoiseSettings(const Settings& settings_json, std::map& noise_settings) + { + initializeNoiseSettings(noise_settings); + + 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; + if (json_parent.getChild(child_index, json_settings_child)) { + NoiseSetting noise_setting; + loadNoiseSetting(json_settings_child, noise_setting); + noise_settings[noise_setting.ImageType] = noise_setting; + } } } } - } - static void loadNoiseSetting(const msr::airlib::Settings& settings_json, NoiseSetting& noise_setting) - { - noise_setting.Enabled = settings_json.getBool("Enabled", noise_setting.Enabled); - noise_setting.ImageType = settings_json.getInt("ImageType", noise_setting.ImageType); - - noise_setting.HorzWaveStrength = settings_json.getFloat("HorzWaveStrength", noise_setting.HorzWaveStrength); - noise_setting.RandSpeed = settings_json.getFloat("RandSpeed", noise_setting.RandSpeed); - noise_setting.RandSize = settings_json.getFloat("RandSize", noise_setting.RandSize); - noise_setting.RandDensity = settings_json.getFloat("RandDensity", noise_setting.RandDensity); - noise_setting.RandContrib = settings_json.getFloat("RandContrib", noise_setting.RandContrib); - noise_setting.HorzWaveContrib = settings_json.getFloat("HorzWaveContrib", noise_setting.HorzWaveContrib); - noise_setting.HorzWaveVertSize = settings_json.getFloat("HorzWaveVertSize", noise_setting.HorzWaveVertSize); - noise_setting.HorzWaveScreenSize = settings_json.getFloat("HorzWaveScreenSize", noise_setting.HorzWaveScreenSize); - noise_setting.HorzNoiseLinesContrib = settings_json.getFloat("HorzNoiseLinesContrib", noise_setting.HorzNoiseLinesContrib); - noise_setting.HorzNoiseLinesDensityY = settings_json.getFloat("HorzNoiseLinesDensityY", noise_setting.HorzNoiseLinesDensityY); - noise_setting.HorzNoiseLinesDensityXY = settings_json.getFloat("HorzNoiseLinesDensityXY", noise_setting.HorzNoiseLinesDensityXY); - noise_setting.HorzDistortionStrength = settings_json.getFloat("HorzDistortionStrength", noise_setting.HorzDistortionStrength); - noise_setting.HorzDistortionContrib = settings_json.getFloat("HorzDistortionContrib", noise_setting.HorzDistortionContrib); - } - - static GimbalSetting createGimbalSetting(const Settings& settings_json) - { - GimbalSetting gimbal; - //capture_setting.gimbal.is_world_frame = settings_json.getBool("IsWorldFrame", false); - gimbal.stabilization = settings_json.getFloat("Stabilization", false); - gimbal.rotation = createRotationSetting(settings_json, gimbal.rotation); - return gimbal; - } - - static CameraSetting createCameraSetting(const Settings& settings_json) - { - CameraSetting setting; + static void loadNoiseSetting(const msr::airlib::Settings& settings_json, NoiseSetting& noise_setting) + { + noise_setting.Enabled = settings_json.getBool("Enabled", noise_setting.Enabled); + noise_setting.ImageType = settings_json.getInt("ImageType", noise_setting.ImageType); + + noise_setting.HorzWaveStrength = settings_json.getFloat("HorzWaveStrength", noise_setting.HorzWaveStrength); + noise_setting.RandSpeed = settings_json.getFloat("RandSpeed", noise_setting.RandSpeed); + noise_setting.RandSize = settings_json.getFloat("RandSize", noise_setting.RandSize); + noise_setting.RandDensity = settings_json.getFloat("RandDensity", noise_setting.RandDensity); + noise_setting.RandContrib = settings_json.getFloat("RandContrib", noise_setting.RandContrib); + noise_setting.HorzWaveContrib = settings_json.getFloat("HorzWaveContrib", noise_setting.HorzWaveContrib); + noise_setting.HorzWaveVertSize = settings_json.getFloat("HorzWaveVertSize", noise_setting.HorzWaveVertSize); + noise_setting.HorzWaveScreenSize = settings_json.getFloat("HorzWaveScreenSize", noise_setting.HorzWaveScreenSize); + noise_setting.HorzNoiseLinesContrib = settings_json.getFloat("HorzNoiseLinesContrib", noise_setting.HorzNoiseLinesContrib); + noise_setting.HorzNoiseLinesDensityY = settings_json.getFloat("HorzNoiseLinesDensityY", noise_setting.HorzNoiseLinesDensityY); + noise_setting.HorzNoiseLinesDensityXY = settings_json.getFloat("HorzNoiseLinesDensityXY", noise_setting.HorzNoiseLinesDensityXY); + noise_setting.HorzDistortionStrength = settings_json.getFloat("HorzDistortionStrength", noise_setting.HorzDistortionStrength); + noise_setting.HorzDistortionContrib = settings_json.getFloat("HorzDistortionContrib", noise_setting.HorzDistortionContrib); + } - setting.position = createVectorSetting(settings_json, setting.position); - setting.rotation = createRotationSetting(settings_json, setting.rotation); + static GimbalSetting createGimbalSetting(const Settings& settings_json) + { + GimbalSetting gimbal; + //capture_setting.gimbal.is_world_frame = settings_json.getBool("IsWorldFrame", false); + gimbal.stabilization = settings_json.getFloat("Stabilization", false); + gimbal.rotation = createRotationSetting(settings_json, gimbal.rotation); + return gimbal; + } - loadCaptureSettings(settings_json, setting.capture_settings); - loadNoiseSettings(settings_json, setting.noise_settings); - Settings json_gimbal; - if (settings_json.getChild("Gimbal", json_gimbal)) - setting.gimbal = createGimbalSetting(json_gimbal); + static CameraSetting createCameraSetting(const Settings& settings_json) + { + CameraSetting setting; - return setting; - } + setting.position = createVectorSetting(settings_json, setting.position); + setting.rotation = createRotationSetting(settings_json, setting.rotation); - static void loadCameraSettings(const Settings& settings_json, std::map& cameras) - { - cameras.clear(); + loadCaptureSettings(settings_json, setting.capture_settings); + loadNoiseSettings(settings_json, setting.noise_settings); + Settings json_gimbal; + if (settings_json.getChild("Gimbal", json_gimbal)) + setting.gimbal = createGimbalSetting(json_gimbal); - Settings json_parent; - if (settings_json.getChild("Cameras", json_parent)) { - std::vector keys; - json_parent.getChildNames(keys); - - for (const auto& key : keys) { - msr::airlib::Settings child; - json_parent.getChild(key, child); - cameras[key] = createCameraSetting(child); - } + return setting; } - } - static void createCaptureSettings(const msr::airlib::Settings& settings_json, CaptureSetting& capture_setting) - { - capture_setting.width = settings_json.getInt("Width", capture_setting.width); - capture_setting.height = settings_json.getInt("Height", capture_setting.height); - capture_setting.fov_degrees = settings_json.getFloat("FOV_Degrees", capture_setting.fov_degrees); - capture_setting.auto_exposure_speed = settings_json.getFloat("AutoExposureSpeed", capture_setting.auto_exposure_speed); - capture_setting.auto_exposure_bias = settings_json.getFloat("AutoExposureBias", capture_setting.auto_exposure_bias); - capture_setting.auto_exposure_max_brightness = settings_json.getFloat("AutoExposureMaxBrightness", capture_setting.auto_exposure_max_brightness); - 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.image_type == 0 ? CaptureSetting::kSceneTargetGamma : Utils::nan()); - - std::string projection_mode = Utils::toLower(settings_json.getString("ProjectionMode", "")); - if (projection_mode == "" || projection_mode == "perspective") - capture_setting.projection_mode = 0; // Perspective - else if (projection_mode == "orthographic") - capture_setting.projection_mode = 1; // Orthographic - else - throw std::invalid_argument(std::string("CaptureSettings projection_mode has invalid value in settings_json ") + projection_mode); - - capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width); - } - - static void loadSubWindowsSettings(const Settings& settings_json, std::vector& subwindow_settings) - { - //load default subwindows - initializeSubwindowSettings(subwindow_settings); - - Settings json_parent; - if (settings_json.getChild("SubWindows", json_parent)) { - for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { - Settings json_settings_child; - if (json_parent.getChild(child_index, json_settings_child)) { - int window_index = json_settings_child.getInt("WindowID", 0); - SubwindowSetting& subwindow_setting = subwindow_settings.at(window_index); - subwindow_setting.window_index = window_index; - subwindow_setting.image_type = Utils::toEnum( - json_settings_child.getInt("ImageType", 0)); - subwindow_setting.visible = json_settings_child.getBool("Visible", false); - subwindow_setting.camera_name = getCameraName(json_settings_child); - subwindow_setting.vehicle_name = json_settings_child.getString("VehicleName", ""); + static void loadCameraSettings(const Settings& settings_json, std::map& cameras) + { + cameras.clear(); + + Settings json_parent; + if (settings_json.getChild("Cameras", json_parent)) { + std::vector keys; + json_parent.getChildNames(keys); + + for (const auto& key : keys) { + msr::airlib::Settings child; + json_parent.getChild(key, child); + cameras[key] = createCameraSetting(child); } } } - } - static void initializeSubwindowSettings(std::vector& subwindow_settings) - { - subwindow_settings.clear(); - subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "")); //depth - subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "")); //seg - subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "")); //vis - } + static void createCaptureSettings(const msr::airlib::Settings& settings_json, CaptureSetting& capture_setting) + { + capture_setting.width = settings_json.getInt("Width", capture_setting.width); + capture_setting.height = settings_json.getInt("Height", capture_setting.height); + capture_setting.fov_degrees = settings_json.getFloat("FOV_Degrees", capture_setting.fov_degrees); + capture_setting.auto_exposure_speed = settings_json.getFloat("AutoExposureSpeed", capture_setting.auto_exposure_speed); + capture_setting.auto_exposure_bias = settings_json.getFloat("AutoExposureBias", capture_setting.auto_exposure_bias); + capture_setting.auto_exposure_max_brightness = settings_json.getFloat("AutoExposureMaxBrightness", capture_setting.auto_exposure_max_brightness); + 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.image_type == 0 ? CaptureSetting::kSceneTargetGamma : Utils::nan()); + + std::string projection_mode = Utils::toLower(settings_json.getString("ProjectionMode", "")); + if (projection_mode == "" || projection_mode == "perspective") + capture_setting.projection_mode = 0; // Perspective + else if (projection_mode == "orthographic") + capture_setting.projection_mode = 1; // Orthographic + else + throw std::invalid_argument(std::string("CaptureSettings projection_mode has invalid value in settings_json ") + projection_mode); - void loadOtherSettings(const Settings& settings_json) - { - //by default we spawn server at local endpoint. Do not use 127.0.0.1 as default below - //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); - is_record_ui_visible = settings_json.getBool("RecordUIVisible", true); - engine_sound = settings_json.getBool("EngineSound", false); - enable_rpc = settings_json.getBool("EnableRpc", enable_rpc); - speed_unit_factor = settings_json.getFloat("SpeedUnitFactor", 1.0f); - speed_unit_label = settings_json.getString("SpeedUnitLabel", "m\\s"); - log_messages_visible = settings_json.getBool("LogMessagesVisible", true); - - { //load origin geopoint - Settings origin_geopoint_json; - if (settings_json.getChild("OriginGeopoint", origin_geopoint_json)) { - GeoPoint origin = origin_geopoint.home_geo_point; - origin.latitude = origin_geopoint_json.getDouble("Latitude", origin.latitude); - origin.longitude = origin_geopoint_json.getDouble("Longitude", origin.longitude); - origin.altitude = origin_geopoint_json.getFloat("Altitude", origin.altitude); - origin_geopoint.initialize(origin); - } + capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width); } - { //time of day settings_json - Settings tod_settings_json; - if (settings_json.getChild("TimeOfDay", tod_settings_json)) { - tod_setting.enabled = tod_settings_json.getBool("Enabled", tod_setting.enabled); - tod_setting.start_datetime = tod_settings_json.getString("StartDateTime", tod_setting.start_datetime); - tod_setting.celestial_clock_speed = tod_settings_json.getFloat("CelestialClockSpeed", tod_setting.celestial_clock_speed); - tod_setting.is_start_datetime_dst = tod_settings_json.getBool("StartDateTimeDst", tod_setting.is_start_datetime_dst); - tod_setting.update_interval_secs = tod_settings_json.getFloat("UpdateIntervalSecs", tod_setting.update_interval_secs); - tod_setting.move_sun = tod_settings_json.getBool("MoveSun", tod_setting.move_sun); + static void loadSubWindowsSettings(const Settings& settings_json, std::vector& subwindow_settings) + { + //load default subwindows + initializeSubwindowSettings(subwindow_settings); + + Settings json_parent; + if (settings_json.getChild("SubWindows", json_parent)) { + for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { + Settings json_settings_child; + if (json_parent.getChild(child_index, json_settings_child)) { + int window_index = json_settings_child.getInt("WindowID", 0); + SubwindowSetting& subwindow_setting = subwindow_settings.at(window_index); + subwindow_setting.window_index = window_index; + subwindow_setting.image_type = Utils::toEnum( + json_settings_child.getInt("ImageType", 0)); + subwindow_setting.visible = json_settings_child.getBool("Visible", false); + subwindow_setting.camera_name = getCameraName(json_settings_child); + subwindow_setting.vehicle_name = json_settings_child.getString("VehicleName", ""); + } + } } } + static void initializeSubwindowSettings(std::vector& subwindow_settings) { - // Wind Settings - Settings child_json; - if (settings_json.getChild("Wind", child_json)) { - wind = createVectorSetting(child_json, wind); - } + subwindow_settings.clear(); + subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "")); //depth + subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "")); //seg + subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "")); //vis } - } - static void loadDefaultCameraSetting(const Settings& settings_json, CameraSetting& camera_defaults) - { - Settings child_json; - if (settings_json.getChild("CameraDefaults", child_json)) { - camera_defaults = createCameraSetting(child_json); - } - } + void loadOtherSettings(const Settings& settings_json) + { + //by default we spawn server at local endpoint. Do not use 127.0.0.1 as default below + //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); + is_record_ui_visible = settings_json.getBool("RecordUIVisible", true); + engine_sound = settings_json.getBool("EngineSound", false); + enable_rpc = settings_json.getBool("EnableRpc", enable_rpc); + speed_unit_factor = settings_json.getFloat("SpeedUnitFactor", 1.0f); + speed_unit_label = settings_json.getString("SpeedUnitLabel", "m\\s"); + log_messages_visible = settings_json.getBool("LogMessagesVisible", true); + + { //load origin geopoint + Settings origin_geopoint_json; + if (settings_json.getChild("OriginGeopoint", origin_geopoint_json)) { + GeoPoint origin = origin_geopoint.home_geo_point; + origin.latitude = origin_geopoint_json.getDouble("Latitude", origin.latitude); + origin.longitude = origin_geopoint_json.getDouble("Longitude", origin.longitude); + origin.altitude = origin_geopoint_json.getFloat("Altitude", origin.altitude); + origin_geopoint.initialize(origin); + } + } - static void loadCameraDirectorSetting(const Settings& settings_json, - CameraDirectorSetting& camera_director, const std::string& simmode_name) - { - camera_director = CameraDirectorSetting(); + { //time of day settings_json + Settings tod_settings_json; + if (settings_json.getChild("TimeOfDay", tod_settings_json)) { + tod_setting.enabled = tod_settings_json.getBool("Enabled", tod_setting.enabled); + tod_setting.start_datetime = tod_settings_json.getString("StartDateTime", tod_setting.start_datetime); + tod_setting.celestial_clock_speed = tod_settings_json.getFloat("CelestialClockSpeed", tod_setting.celestial_clock_speed); + tod_setting.is_start_datetime_dst = tod_settings_json.getBool("StartDateTimeDst", tod_setting.is_start_datetime_dst); + tod_setting.update_interval_secs = tod_settings_json.getFloat("UpdateIntervalSecs", tod_setting.update_interval_secs); + tod_setting.move_sun = tod_settings_json.getBool("MoveSun", tod_setting.move_sun); + } + } - Settings child_json; - if (settings_json.getChild("CameraDirector", child_json)) { - camera_director.position = createVectorSetting(settings_json, camera_director.position); - camera_director.rotation = createRotationSetting(settings_json, camera_director.rotation); - camera_director.follow_distance = child_json.getFloat("FollowDistance", camera_director.follow_distance); + { + // Wind Settings + Settings child_json; + if (settings_json.getChild("Wind", child_json)) { + wind = createVectorSetting(child_json, wind); + } + } } - if (std::isnan(camera_director.follow_distance)) { - if (simmode_name == kSimModeTypeCar) - camera_director.follow_distance = -8; - else - camera_director.follow_distance = -3; - } - if (std::isnan(camera_director.position.x())) - camera_director.position.x() = camera_director.follow_distance; - if (std::isnan(camera_director.position.y())) - camera_director.position.y() = 0; - if (std::isnan(camera_director.position.z())) { - if (simmode_name == kSimModeTypeCar) - camera_director.position.z() = -4; - else - camera_director.position.z() = -2; + static void loadDefaultCameraSetting(const Settings& settings_json, CameraSetting& camera_defaults) + { + Settings child_json; + if (settings_json.getChild("CameraDefaults", child_json)) { + camera_defaults = createCameraSetting(child_json); + } } - } - void loadClockSettings(const Settings& settings_json) - { - clock_type = settings_json.getString("ClockType", ""); + static void loadCameraDirectorSetting(const Settings& settings_json, + CameraDirectorSetting& camera_director, const std::string& simmode_name) + { + camera_director = CameraDirectorSetting(); - if (clock_type == "") { - //default value - clock_type = "ScalableClock"; + Settings child_json; + if (settings_json.getChild("CameraDirector", child_json)) { + camera_director.position = createVectorSetting(settings_json, camera_director.position); + camera_director.rotation = createRotationSetting(settings_json, camera_director.rotation); + camera_director.follow_distance = child_json.getFloat("FollowDistance", camera_director.follow_distance); + } - //override if multirotor simmode with simple_flight - if (simmode_name == kSimModeTypeMultirotor) { - //TODO: this won't work if simple_flight and PX4 is combined together! - - //for multirotors we select steppable fixed interval clock unless we have - //PX4 enabled vehicle - clock_type = "SteppableClock"; - for (auto const& vehicle : vehicles) - { - if (vehicle.second->auto_create && - vehicle.second->vehicle_type == kVehicleTypePX4) { - clock_type = "ScalableClock"; - break; - } - } + if (std::isnan(camera_director.follow_distance)) { + if (simmode_name == kSimModeTypeCar) + camera_director.follow_distance = -8; + else + camera_director.follow_distance = -3; + } + if (std::isnan(camera_director.position.x())) + camera_director.position.x() = camera_director.follow_distance; + if (std::isnan(camera_director.position.y())) + camera_director.position.y() = 0; + if (std::isnan(camera_director.position.z())) { + if (simmode_name == kSimModeTypeCar) + camera_director.position.z() = -4; + else + camera_director.position.z() = -2; } } - clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); - } - - static void initializeBarometerSetting(BarometerSetting& barometer_setting, const Settings& settings_json) - { - unused(barometer_setting); - unused(settings_json); - - //TODO: set from json as needed - } - - static void initializeImuSetting(ImuSetting& imu_setting, const Settings& settings_json) - { - unused(imu_setting); - unused(settings_json); - - //TODO: set from json as needed - } - - static void initializeGpsSetting(GpsSetting& gps_setting, const Settings& settings_json) - { - unused(gps_setting); - unused(settings_json); + void loadClockSettings(const Settings& settings_json) + { + clock_type = settings_json.getString("ClockType", ""); + + if (clock_type == "") { + //default value + clock_type = "ScalableClock"; + + //override if multirotor simmode with simple_flight + if (simmode_name == kSimModeTypeMultirotor) { + //TODO: this won't work if simple_flight and PX4 is combined together! + + //for multirotors we select steppable fixed interval clock unless we have + //PX4 enabled vehicle + clock_type = "SteppableClock"; + for (auto const& vehicle : vehicles) { + if (vehicle.second->auto_create && + vehicle.second->vehicle_type == kVehicleTypePX4) { + clock_type = "ScalableClock"; + break; + } + } + } + } - //TODO: set from json as needed - } + clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); + } - static void initializeMagnetometerSetting(MagnetometerSetting& magnetometer_setting, const Settings& settings_json) - { - unused(magnetometer_setting); - unused(settings_json); + static std::shared_ptr createSensorSetting( + SensorBase::SensorType sensor_type, const std::string& sensor_name, + bool enabled) + { + std::shared_ptr sensor_setting; + + switch (sensor_type) { + case SensorBase::SensorType::Barometer: + sensor_setting = std::shared_ptr(new BarometerSetting()); + break; + case SensorBase::SensorType::Imu: + sensor_setting = std::shared_ptr(new ImuSetting()); + break; + case SensorBase::SensorType::Gps: + sensor_setting = std::shared_ptr(new GpsSetting()); + break; + case SensorBase::SensorType::Magnetometer: + sensor_setting = std::shared_ptr(new MagnetometerSetting()); + break; + case SensorBase::SensorType::Distance: + sensor_setting = std::shared_ptr(new DistanceSetting()); + break; + case SensorBase::SensorType::Lidar: + sensor_setting = std::shared_ptr(new LidarSetting()); + break; + default: + throw std::invalid_argument("Unexpected sensor type"); + } - //TODO: set from json as needed - } + sensor_setting->sensor_type = sensor_type; + sensor_setting->sensor_name = sensor_name; + sensor_setting->enabled = enabled; - static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& 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); - distance_setting.external_controller = settings_json.getBool("ExternalController", distance_setting.external_controller); + return sensor_setting; + } - distance_setting.position = createVectorSetting(settings_json, distance_setting.position); - distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation); - } + static void initializeSensorSetting(SensorSetting* sensor_setting, const Settings& settings_json) + { + sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled); - static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json) - { - lidar_setting.number_of_channels = settings_json.getInt("NumberOfChannels", lidar_setting.number_of_channels); - lidar_setting.range = settings_json.getFloat("Range", lidar_setting.range); - lidar_setting.points_per_second = settings_json.getInt("PointsPerSecond", lidar_setting.points_per_second); - lidar_setting.horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", lidar_setting.horizontal_rotation_frequency); - lidar_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", lidar_setting.draw_debug_points); - lidar_setting.data_frame = settings_json.getString("DataFrame", lidar_setting.data_frame); - lidar_setting.external_controller = settings_json.getBool("ExternalController", lidar_setting.external_controller); - - lidar_setting.vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", lidar_setting.vertical_FOV_upper); - lidar_setting.vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", lidar_setting.vertical_FOV_lower); - lidar_setting.horizontal_FOV_start = settings_json.getFloat("HorizontalFOVStart", lidar_setting.horizontal_FOV_start); - lidar_setting.horizontal_FOV_end = settings_json.getFloat("HorizontalFOVEnd", lidar_setting.horizontal_FOV_end); - - lidar_setting.position = createVectorSetting(settings_json, lidar_setting.position); - lidar_setting.rotation = createRotationSetting(settings_json, lidar_setting.rotation); - } - - static std::unique_ptr createSensorSetting( - SensorBase::SensorType sensor_type, const std::string& sensor_name, - bool enabled) - { - std::unique_ptr sensor_setting; - - switch (sensor_type) { - case SensorBase::SensorType::Barometer: - sensor_setting = std::unique_ptr(new BarometerSetting()); - break; - case SensorBase::SensorType::Imu: - sensor_setting = std::unique_ptr(new ImuSetting()); - break; - case SensorBase::SensorType::Gps: - sensor_setting = std::unique_ptr(new GpsSetting()); - break; - case SensorBase::SensorType::Magnetometer: - sensor_setting = std::unique_ptr(new MagnetometerSetting()); - break; - case SensorBase::SensorType::Distance: - sensor_setting = std::unique_ptr(new DistanceSetting()); - break; - case SensorBase::SensorType::Lidar: - sensor_setting = std::unique_ptr(new LidarSetting()); - break; - default: - throw std::invalid_argument("Unexpected sensor type"); + // pass the json Settings property bag through to the specific sensor params object where it is + // extracted there. This way default values can be kept in one place. For example, see the + // BarometerSimpleParams::initializeFromSettings method. + sensor_setting->settings = settings_json; } - sensor_setting->sensor_type = sensor_type; - sensor_setting->sensor_name = sensor_name; - sensor_setting->enabled = enabled; + // creates and intializes sensor settings from json + static void loadSensorSettings(const Settings& settings_json, const std::string& collectionName, + std::map>& sensors, + std::map>& sensor_defaults) - return sensor_setting; - } + { + // start with defaults. + for (auto ptr = sensor_defaults.begin(); ptr != sensor_defaults.end(); ptr++) { + auto key = ptr->first; + sensors[key] = sensor_defaults[key]; + } - static void initializeSensorSetting(SensorSetting* sensor_setting, const Settings& settings_json) - { - sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled); - - switch (sensor_setting->sensor_type) { - case SensorBase::SensorType::Barometer: - initializeBarometerSetting(*static_cast(sensor_setting), settings_json); - break; - case SensorBase::SensorType::Imu: - initializeImuSetting(*static_cast(sensor_setting), settings_json); - break; - case SensorBase::SensorType::Gps: - initializeGpsSetting(*static_cast(sensor_setting), settings_json); - break; - case SensorBase::SensorType::Magnetometer: - initializeMagnetometerSetting(*static_cast(sensor_setting), settings_json); - break; - case SensorBase::SensorType::Distance: - initializeDistanceSetting(*static_cast(sensor_setting), settings_json); - break; - case SensorBase::SensorType::Lidar: - initializeLidarSetting(*static_cast(sensor_setting), settings_json); - break; - default: - throw std::invalid_argument("Unexpected sensor type"); - } - } + msr::airlib::Settings sensors_child; + if (settings_json.getChild(collectionName, sensors_child)) { + std::vector keys; + sensors_child.getChildNames(keys); - // creates and intializes sensor settings from json - static void loadSensorSettings( const Settings& settings_json, const std::string& collectionName, - std::map>& sensors) - { - msr::airlib::Settings sensors_child; - if (settings_json.getChild(collectionName, sensors_child)) { - std::vector keys; - sensors_child.getChildNames(keys); + for (const auto& key : keys) { + msr::airlib::Settings child; + sensors_child.getChild(key, child); - for (const auto& key : keys) { - msr::airlib::Settings child; - sensors_child.getChild(key, child); + auto sensor_type = Utils::toEnum(child.getInt("SensorType", 0)); + auto enabled = child.getBool("Enabled", false); - 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); + sensors[key] = createSensorSetting(sensor_type, key, enabled); + initializeSensorSetting(sensors[key].get(), child); + } } } - } - // creates default sensor list when none specified in json - static void createDefaultSensorSettings(const std::string& simmode_name, - std::map>& sensors) - { - if (simmode_name == kSimModeTypeMultirotor) { - sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true); - sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true); - sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); - sensors["barometer"] = createSensorSetting(SensorBase::SensorType::Barometer, "barometer", true); - } - else if (simmode_name == kSimModeTypeCar) { - sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); + // creates default sensor list when none specified in json + static void createDefaultSensorSettings(const std::string& simmode_name, + std::map>& sensors) + { + if (simmode_name == kSimModeTypeMultirotor) { + sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true); + sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true); + sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); + sensors["barometer"] = createSensorSetting(SensorBase::SensorType::Barometer, "barometer", true); + } + else if (simmode_name == kSimModeTypeCar) { + sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); + } + else { + // no sensors added for other modes + } } - else { - // no sensors added for other modes + + // loads or creates default sensor list + static void loadDefaultSensorSettings(const std::string& simmode_name, + const Settings& settings_json, + std::map>& sensors) + { + msr::airlib::Settings sensors_child; + if (settings_json.getChild("DefaultSensors", sensors_child)) + loadSensorSettings(settings_json, "DefaultSensors", sensors, sensors); + else + createDefaultSensorSettings(simmode_name, sensors); } - } + }; - // loads or creates default sensor list - static void loadDefaultSensorSettings(const std::string& simmode_name, - const Settings& settings_json, - std::map>& sensors) - { - msr::airlib::Settings sensors_child; - if (settings_json.getChild("DefaultSensors", sensors_child)) - loadSensorSettings(settings_json, "DefaultSensors", sensors); - else - createDefaultSensorSettings(simmode_name, sensors); - } -}; - -}} //namespace +} +} //namespace #endif diff --git a/AirLib/include/common/UpdatableContainer.hpp b/AirLib/include/common/UpdatableContainer.hpp index b841d8f44b..2b9d476930 100644 --- a/AirLib/include/common/UpdatableContainer.hpp +++ b/AirLib/include/common/UpdatableContainer.hpp @@ -10,7 +10,8 @@ namespace msr { namespace airlib { template -class UpdatableContainer : public UpdatableObject { +class UpdatableContainer : public UpdatableObject +{ public: //limited container interface typedef vector MembersContainer; typedef typename MembersContainer::iterator iterator; @@ -24,10 +25,24 @@ class UpdatableContainer : public UpdatableObject { const TUpdatableObjectPtr& at(uint index) const { members_.at(index); } TUpdatableObjectPtr& at(uint index) { return members_.at(index); } //allow to override membership modifications - virtual void clear() { members_.clear(); } - virtual void insert(TUpdatableObjectPtr member) { members_.push_back(member); } - virtual void erase_remove(TUpdatableObjectPtr obj) { - members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); } + virtual void clear() + { + for (auto m : members_) { + m->setParent(nullptr); + } + members_.clear(); + } + virtual void insert(TUpdatableObjectPtr member) + { + member->setParent(this); + members_.push_back(member); + } + virtual void erase_remove(TUpdatableObjectPtr member) + { + member->setParent(nullptr); + members_.erase(std::remove(members_.begin(), members_.end(), member), members_.end()); + } + public: //*** Start: UpdatableState implementation ***// diff --git a/AirLib/include/common/UpdatableObject.hpp b/AirLib/include/common/UpdatableObject.hpp index 579f2350fb..9f40e3bafb 100644 --- a/AirLib/include/common/UpdatableObject.hpp +++ b/AirLib/include/common/UpdatableObject.hpp @@ -24,8 +24,8 @@ Do not call reset() from constructor or initialization because that will produce init->reset calls for base-derived class that would be incorrect. */ - -class UpdatableObject { +class UpdatableObject +{ public: void reset() { @@ -73,6 +73,26 @@ class UpdatableObject { return ClockFactory::get(); } + UpdatableObject* getParent() + { + return parent_; + } + + void setParent(UpdatableObject* container) + { + parent_ = container; + } + + std::string getName() + { + return name_; + } + + void setName(const std::string& name) + { + this->name_ = name; + } + protected: virtual void resetImplementation() = 0; virtual void failResetUpdateOrdering(std::string err) @@ -84,6 +104,8 @@ class UpdatableObject { bool reset_called = false; bool update_called = false; bool reset_in_progress = false; + UpdatableObject* parent_ = nullptr; + std::string name_; }; }} //namespace diff --git a/AirLib/include/common/common_utils/FileSystem.hpp b/AirLib/include/common/common_utils/FileSystem.hpp index d6e7fba778..5b7849d627 100644 --- a/AirLib/include/common/common_utils/FileSystem.hpp +++ b/AirLib/include/common/common_utils/FileSystem.hpp @@ -50,23 +50,27 @@ class FileSystem static std::string getUserDocumentsFolder(); - static std::string getExecutableFolder(); + static std::string getExecutableFolder(); - static std::string getAppDataFolder() { + static std::string getAppDataFolder() + { return ensureFolder(combine(getUserDocumentsFolder(), ProductFolderName)); } - static std::string ensureFolder(const std::string& fullpath) { + static std::string ensureFolder(const std::string& fullpath) + { // make sure this directory exists. return createDirectory(fullpath); } - static std::string ensureFolder(const std::string& parentFolder, const std::string& child) { + static std::string ensureFolder(const std::string& parentFolder, const std::string& child) + { // make sure this directory exists. return createDirectory(combine(parentFolder, child)); } - static std::string combine(const std::string& parentFolder, const std::string& child) { + static std::string combine(const std::string& parentFolder, const std::string& child) + { if (child.size() == 0) return parentFolder; @@ -83,7 +87,8 @@ class FileSystem return parentFolder + kPathSeparator + child; } - static void removeLeaf(std::string& path) { + static void removeLeaf(std::string& path) + { size_t size = path.size(); size_t pos = path.find_last_of('/'); if (pos != std::string::npos) { @@ -98,14 +103,12 @@ class FileSystem int len = static_cast(str.size()); const char* ptr = str.c_str(); int i = 0; - for (i = len - 1; i >= 0; i--) - { + for (i = len - 1; i >= 0; i--) { if (ptr[i] == '.') break; } if (i < 0) return ""; - auto ui = static_cast(i); - return str.substr(ui, len - ui); + return str.substr(i, len - i); } static std::string getLogFolderPath(bool folder_timestamp, const std::string& parent = "") @@ -139,8 +142,8 @@ class FileSystem //return filename_ss.str(); } - static void openTextFile(const std::string& filepath, std::ifstream& file){ - + static void openTextFile(const std::string& filepath, std::ifstream& file) + { #ifdef _WIN32 // WIN32 will create the wrong file names if we don't first convert them to UTF-16. std::wstring_convert, wchar_t> converter; @@ -151,8 +154,8 @@ class FileSystem #endif } - static void createBinaryFile(const std::string& filepath, std::ofstream& file){ - + static void createBinaryFile(const std::string& filepath, std::ofstream& file) + { #ifdef _WIN32 // WIN32 will create the wrong file names if we don't first convert them to UTF-16. std::wstring_convert, wchar_t> converter; @@ -163,8 +166,8 @@ class FileSystem #endif } - static void createTextFile(const std::string& filepath, std::ofstream& file){ - + static void createTextFile(const std::string& filepath, std::ofstream& file) + { #ifdef _WIN32 // WIN32 will create the wrong file names if we don't first convert them to UTF-16. std::wstring_convert, wchar_t> converter; diff --git a/AirLib/include/common/common_utils/ScheduledExecutor.hpp b/AirLib/include/common/common_utils/ScheduledExecutor.hpp index d62cea1be4..8ab23b9e1f 100644 --- a/AirLib/include/common/common_utils/ScheduledExecutor.hpp +++ b/AirLib/include/common/common_utils/ScheduledExecutor.hpp @@ -14,18 +14,22 @@ namespace common_utils { -class ScheduledExecutor { +class ScheduledExecutor +{ public: ScheduledExecutor() {} + ScheduledExecutor(const std::function& callback, uint64_t period_nanos) { initialize(callback, period_nanos); } + ~ScheduledExecutor() { stop(); } + void initialize(const std::function& callback, uint64_t period_nanos) { callback_ = callback; @@ -49,6 +53,7 @@ class ScheduledExecutor { void pause(bool is_paused) { paused_ = is_paused; + pause_period_start_ = 0; // cancel any pause period. } bool isPaused() const @@ -56,6 +61,13 @@ class ScheduledExecutor { return paused_; } + void pauseForTime(double seconds) + { + pause_period_start_ = nanos(); + pause_period_ = static_cast(1E9 * seconds); + paused_ = true; + } + void continueForTime(double seconds) { pause_period_start_ = nanos(); @@ -65,6 +77,7 @@ class ScheduledExecutor { void continueForFrames(uint32_t frames) { + pause_period_start_ = 0; // cancel any pause period. frame_countdown_enabled_ = true; targetFrameNumber_ = frames + currentFrameNumber_; paused_ = false; @@ -86,8 +99,8 @@ class ScheduledExecutor { th_.join(); } } - catch(const std::system_error& /* e */) - { } + catch(const std::system_error& /* e */) { + } } } @@ -174,9 +187,7 @@ class ScheduledExecutor { if (pause_period_start_ > 0) { if (nanos() - pause_period_start_ >= pause_period_) { - if (! isPaused()) - pause(true); - + pause(!isPaused()); pause_period_start_ = 0; } } diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index ad962a00ab..ff024462bb 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -16,11 +16,13 @@ namespace msr { namespace airlib { -class FastPhysicsEngine : public PhysicsEngineBase { +class FastPhysicsEngine : public PhysicsEngineBase +{ public: FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero()) : enable_ground_lock_(enable_ground_lock), wind_(wind) - { + { + setName("FastPhysicsEngine"); } //*** Start: UpdatableState implementation ***// @@ -50,8 +52,8 @@ class FastPhysicsEngine : public PhysicsEngineBase { { for (PhysicsBody* body_ptr : *this) { reporter.writeValue("Phys", debug_string_.str()); - reporter.writeValue("Is Grounded", body_ptr->isGrounded()); - reporter.writeValue("Force (world)", body_ptr->getWrench().force); + reporter.writeValue("Is Grounded", body_ptr->isGrounded()); + reporter.writeValue("Force (world)", body_ptr->getWrench().force); reporter.writeValue("Torque (body)", body_ptr->getWrench().torque); } //call base @@ -105,12 +107,12 @@ class FastPhysicsEngine : public PhysicsEngineBase { body.unlock(); - //TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence + //TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence //with below commented out - Arducopter GPS may not work. - //body.getEnvironment().setPosition(next.pose.position); - //body.getEnvironment().update(); - - } + //body.getEnvironment().setPosition(next.pose.position); + //body.getEnvironment().update(); + + } static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, bool is_collision_response, CollisionResponse& collision_response) @@ -231,7 +233,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { // also eliminate any linear velocity due to twist - since we are sitting on the ground there shouldn't be any. next.twist.linear = Vector3r::Zero(); next.pose.position = collision_info.position; - body.setGrounded(true); + body.setGrounded(true); // but we do want to "feel" the ground when we hit it (we should see a small z-acc bump) // equal and opposite our downward velocity. @@ -239,8 +241,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { //throttledLogOutput("*** Triggering ground lock", 0.1); } - else - { + else { //else keep the orientation next.pose.position = collision_info.position + (collision_info.normal * collision_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles); } @@ -255,8 +256,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { { TTimeDelta dt = clock()->elapsedSince(last_message_time); const real_T dt_real = static_cast(dt); - if (dt_real > seconds) - { + if (dt_real > seconds) { Utils::log(msg); last_message_time = clock()->nowNanos(); } @@ -340,10 +340,9 @@ class FastPhysicsEngine : public PhysicsEngineBase { 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) - { + if (external_force_magnitude >= weight_magnitude) { //throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1); - body.setGrounded(false); + body.setGrounded(false); } next_wrench.force = Vector3r::Zero(); next_wrench.torque = Vector3r::Zero(); diff --git a/AirLib/include/physics/PhysicsBody.hpp b/AirLib/include/physics/PhysicsBody.hpp index 7165e68948..a0ea640da0 100644 --- a/AirLib/include/physics/PhysicsBody.hpp +++ b/AirLib/include/physics/PhysicsBody.hpp @@ -15,7 +15,8 @@ namespace msr { namespace airlib { -class PhysicsBody : public UpdatableObject { +class PhysicsBody : public UpdatableObject +{ public: //interface virtual real_T getRestitution() const = 0; virtual real_T getFriction() const = 0; @@ -91,7 +92,9 @@ class PhysicsBody : public UpdatableObject { inertia_ = inertia; inertia_inv_ = inertia_.inverse(); environment_ = environment; + environment_->setParent(this); kinematics_ = kinematics; + kinematics_->setParent(this); } //enable physics body detection @@ -109,7 +112,7 @@ class PhysicsBody : public UpdatableObject { wrench_ = Wrench::zero(); collision_info_ = CollisionInfo(); collision_response_ = CollisionResponse(); - grounded_ = false; + grounded_ = false; //update individual vertices for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) { @@ -223,24 +226,24 @@ class PhysicsBody : public UpdatableObject { return collision_response_; } - bool isGrounded() const - { - return grounded_; - } - void setGrounded(bool grounded) - { - grounded_ = grounded; - } + bool isGrounded() const + { + return grounded_; + } + void setGrounded(bool grounded) + { + grounded_ = grounded; + } - void lock() - { - mutex_.lock(); - } + void lock() + { + mutex_.lock(); + } - void unlock() - { - mutex_.unlock(); - } + void unlock() + { + mutex_.unlock(); + } public: //for use in physics engine: //TODO: use getter/setter or friend method? @@ -259,8 +262,8 @@ class PhysicsBody : public UpdatableObject { CollisionInfo collision_info_; CollisionResponse collision_response_; - bool grounded_ = false; - std::mutex mutex_; + bool grounded_ = false; + std::mutex mutex_; }; }} //namespace diff --git a/AirLib/include/physics/PhysicsEngineBase.hpp b/AirLib/include/physics/PhysicsEngineBase.hpp index 2dff630ab4..0d41967f49 100644 --- a/AirLib/include/physics/PhysicsEngineBase.hpp +++ b/AirLib/include/physics/PhysicsEngineBase.hpp @@ -10,7 +10,8 @@ namespace msr { namespace airlib { -class PhysicsEngineBase : public UpdatableObject { +class PhysicsEngineBase : public UpdatableContainer +{ public: virtual void update() override { @@ -23,31 +24,7 @@ class PhysicsEngineBase : public UpdatableObject { //default nothing to report for physics engine } - //TODO: reduce copy-past from UpdatableContainer which has same code - /********************** Container interface **********************/ - typedef PhysicsBody* TUpdatableObjectPtr; - typedef vector MembersContainer; - typedef typename MembersContainer::iterator iterator; - typedef typename MembersContainer::const_iterator const_iterator; - typedef typename MembersContainer::value_type value_type; - - iterator begin() { return members_.begin(); } - iterator end() { return members_.end(); } - const_iterator begin() const { return members_.begin(); } - const_iterator end() const { return members_.end(); } - uint size() const { return static_cast(members_.size()); } - const TUpdatableObjectPtr &at(uint index) const { return members_.at(index); } - TUpdatableObjectPtr &at(uint index) { return members_.at(index); } - //allow to override membership modifications - virtual void clear() { members_.clear(); } - virtual void insert(TUpdatableObjectPtr member) { members_.push_back(member); } - virtual void erase_remove(TUpdatableObjectPtr obj) { - members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); } - - virtual void setWind(const Vector3r& wind) {}; - -private: - MembersContainer members_; + virtual void setWind(const Vector3r& wind) { unused(wind); }; }; diff --git a/AirLib/include/physics/PhysicsWorld.hpp b/AirLib/include/physics/PhysicsWorld.hpp index dbf0ec8302..7dbcc8ae26 100644 --- a/AirLib/include/physics/PhysicsWorld.hpp +++ b/AirLib/include/physics/PhysicsWorld.hpp @@ -13,7 +13,8 @@ namespace msr { namespace airlib { -class PhysicsWorld { +class PhysicsWorld : UpdatableObject +{ public: PhysicsWorld(std::unique_ptr physics_engine, const std::vector& bodies, uint64_t update_period_nanos = 3000000LL, bool state_reporter_enabled = false, @@ -21,6 +22,7 @@ class PhysicsWorld { ) : world_(std::move(physics_engine)) { + setName("PhysicsWorld"); enableStateReport(state_reporter_enabled); update_period_nanos_ = update_period_nanos; initializeWorld(bodies, start_async_updator); @@ -108,6 +110,8 @@ class PhysicsWorld { world_.setFrameNumber(frameNumber); } + void resetImplementation() override {} + private: void initializeWorld(const std::vector& bodies, bool start_async_updator) { diff --git a/AirLib/include/physics/World.hpp b/AirLib/include/physics/World.hpp index bf88c188b6..94f738e84e 100644 --- a/AirLib/include/physics/World.hpp +++ b/AirLib/include/physics/World.hpp @@ -14,13 +14,15 @@ namespace msr { namespace airlib { -class World : public UpdatableContainer { +class World : public UpdatableContainer +{ public: World(std::unique_ptr physics_engine) : physics_engine_(std::move(physics_engine)) { World::clear(); - + setName("World"); + physics_engine_->setParent(this); if (physics_engine) physics_engine_->clear(); } @@ -118,6 +120,11 @@ class World : public UpdatableContainer { return executor_.isPaused(); } + void pauseForTime(double seconds) + { + executor_.pauseForTime(seconds); + } + void continueForTime(double seconds) { executor_.continueForTime(seconds); diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index 765cfabcf1..d56f46b3de 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/AirLib/include/sensors/SensorBase.hpp @@ -18,7 +18,8 @@ namespace msr { namespace airlib { After construction of the derived class an initialize(...) must be made which would set the sensor in good-to-use state by call to reset. */ -class SensorBase : public UpdatableObject { +class SensorBase : public UpdatableObject +{ public: enum class SensorType : uint { Barometer = 1, @@ -59,7 +60,7 @@ class SensorBase : public UpdatableObject { private: //ground truth can be shared between many sensors - GroundTruth ground_truth_; + GroundTruth ground_truth_ = { nullptr, nullptr }; std::string name_ = ""; }; diff --git a/AirLib/include/sensors/SensorFactory.hpp b/AirLib/include/sensors/SensorFactory.hpp index ed69b4abb7..1f25312a13 100644 --- a/AirLib/include/sensors/SensorFactory.hpp +++ b/AirLib/include/sensors/SensorFactory.hpp @@ -15,22 +15,23 @@ namespace msr { namespace airlib { -class SensorFactory { +class SensorFactory +{ public: // creates one sensor from settings - virtual std::unique_ptr createSensorFromSettings( + virtual std::shared_ptr createSensorFromSettings( const AirSimSettings::SensorSetting* sensor_setting) const { switch (sensor_setting->sensor_type) { case SensorBase::SensorType::Imu: - return std::unique_ptr(new ImuSimple(*static_cast(sensor_setting))); + return std::shared_ptr(new ImuSimple(*static_cast(sensor_setting))); case SensorBase::SensorType::Magnetometer: - return std::unique_ptr(new MagnetometerSimple(*static_cast(sensor_setting))); + return std::shared_ptr(new MagnetometerSimple(*static_cast(sensor_setting))); case SensorBase::SensorType::Gps: - return std::unique_ptr(new GpsSimple(*static_cast(sensor_setting))); + return std::shared_ptr(new GpsSimple(*static_cast(sensor_setting))); case SensorBase::SensorType::Barometer: - return std::unique_ptr(new BarometerSimple(*static_cast(sensor_setting))); + return std::shared_ptr(new BarometerSimple(*static_cast(sensor_setting))); default: throw new std::invalid_argument("Unexpected sensor type"); } @@ -38,9 +39,9 @@ class SensorFactory { // creates sensor-collection virtual void createSensorsFromSettings( - const std::map>& sensors_settings, + const std::map>& sensors_settings, SensorCollection& sensors, - vector>& sensor_storage) const + vector>& sensor_storage) const { for (const auto& sensor_setting_pair : sensors_settings) { const AirSimSettings::SensorSetting* sensor_setting = sensor_setting_pair.second.get(); @@ -49,11 +50,10 @@ class SensorFactory { if (sensor_setting == nullptr || !sensor_setting->enabled) continue; - std::unique_ptr sensor = createSensorFromSettings(sensor_setting); + std::shared_ptr sensor = createSensorFromSettings(sensor_setting); if (sensor) { - SensorBase* sensor_temp = sensor.get(); - sensor_storage.push_back(std::move(sensor)); - sensors.insert(sensor_temp, sensor_setting->sensor_type); + sensor_storage.push_back(sensor); + sensors.insert(sensor.get(), sensor_setting->sensor_type); } } } diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index 399eb86cd7..eda67bf092 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp @@ -16,7 +16,8 @@ namespace msr { namespace airlib { -class BarometerSimple : public BarometerBase { +class BarometerSimple : public BarometerBase +{ public: BarometerSimple(const AirSimSettings::BarometerSetting& setting = AirSimSettings::BarometerSetting()) : BarometerBase(setting.sensor_name) @@ -27,7 +28,7 @@ class BarometerSimple : public BarometerBase { //GM process that would do random walk for pressure factor pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0); - uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma); + uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma); //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f); //initialize frequency limiter @@ -76,7 +77,7 @@ class BarometerSimple : public BarometerBase { auto altitude = ground_truth.environment->getState().geo_point.altitude; auto pressure = EarthUtils::getStandardPressure(altitude); - //add drift in pressure, about 10m change per hour + //add drift in pressure, about 10m change per hour using default settings. pressure_factor_.update(); pressure += pressure * pressure_factor_.getOutput(); diff --git a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp index 62082342dd..ef128e78a1 100644 --- a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp @@ -5,12 +5,14 @@ #define msr_airlib_BarometerSimpleParams_hpp #include "common/Common.hpp" +#include "common/AirSimSettings.hpp" namespace msr { namespace airlib { -struct BarometerSimpleParams { +struct BarometerSimpleParams +{ //user specified sea level pressure is specified in hPa units real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa @@ -33,14 +35,14 @@ struct BarometerSimpleParams { real_T correlated_noise_sigma = 0.27f; real_T correlated_noise_tau = 0.87f; - real_T unnorrelated_noise_sigma = 0.24f; + real_T uncorrelated_noise_sigma = 0.24f; */ //Experiments for MEAS MS56112 sensor shows 0.021mbar, datasheet has resoultion of 0.027mbar @ 1024 //http://www.te.com/commerce/DocumentDelivery/DDEController?Action=srchrtrv&DocNm=MS5611-01BA03&DocType=Data+Sheet&DocLang=English - real_T unnorrelated_noise_sigma = 0.027f * 100; + real_T uncorrelated_noise_sigma = 0.027f * 100; //jMavSim uses below - //real_T unnorrelated_noise_sigma = 0.1f; + //real_T uncorrelated_noise_sigma = 0.1f; //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html real_T update_latency = 0.0f; //sec @@ -49,7 +51,13 @@ struct BarometerSimpleParams { void initializeFromSettings(const AirSimSettings::BarometerSetting& settings) { - unused(settings); + const auto& json = settings.settings; + pressure_factor_sigma = json.getFloat("PressureFactorSigma", pressure_factor_sigma); + pressure_factor_tau = json.getFloat("PressureFactorTau", pressure_factor_tau); + uncorrelated_noise_sigma = json.getFloat("UncorrelatedNoiseSigma", uncorrelated_noise_sigma); + update_latency = json.getFloat("UpdateLatency", update_latency); + update_frequency = json.getFloat("UpdateFrequency", update_frequency); + startup_delay = json.getFloat("StartupDelay", startup_delay); } }; diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp index 45adcdddc3..81f7a4858e 100644 --- a/AirLib/include/sensors/distance/DistanceSimple.hpp +++ b/AirLib/include/sensors/distance/DistanceSimple.hpp @@ -14,7 +14,8 @@ namespace msr { namespace airlib { -class DistanceSimple : public DistanceBase { +class DistanceSimple : public DistanceBase +{ public: DistanceSimple(const AirSimSettings::DistanceSetting& setting = AirSimSettings::DistanceSetting()) : DistanceBase(setting.sensor_name) @@ -22,7 +23,7 @@ class DistanceSimple : public DistanceBase { // initialize params params_.initializeFromSettings(setting); - uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma); + uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma); //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f); diff --git a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp index acd461e9b7..3afadd29c1 100644 --- a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp +++ b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp @@ -11,7 +11,8 @@ namespace msr { namespace airlib { -struct DistanceSimpleParams { +struct DistanceSimpleParams +{ real_T min_distance = 20.0f / 100; //m real_T max_distance = 4000.0f / 100; //m @@ -32,13 +33,13 @@ struct DistanceSimpleParams { real_T correlated_noise_sigma = 0.27f; real_T correlated_noise_tau = 0.87f; - real_T unnorrelated_noise_sigma = 0.24f; + real_T uncorrelated_noise_sigma = 0.24f; */ //TODO: update sigma based on documentation, maybe as a function increasing with measured distance - real_T unnorrelated_noise_sigma = 0.002f * 100; + real_T uncorrelated_noise_sigma = 0.002f * 100; //jMavSim uses below - //real_T unnorrelated_noise_sigma = 0.1f; + //real_T uncorrelated_noise_sigma = 0.1f; //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html real_T update_latency = 0.0f; //sec @@ -47,15 +48,18 @@ struct DistanceSimpleParams { void initializeFromSettings(const AirSimSettings::DistanceSetting& settings) { - std::string simmode_name = AirSimSettings::singleton().simmode_name; + const auto& settings_json = settings.settings; + min_distance = settings_json.getFloat("MinDistance", min_distance); + max_distance = settings_json.getFloat("MaxDistance", max_distance); + draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points); + external_controller = settings_json.getBool("ExternalController", external_controller); - min_distance = settings.min_distance; - max_distance = settings.max_distance; + auto position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector()); + auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation()); - draw_debug_points = settings.draw_debug_points; - external_controller = settings.external_controller; + std::string simmode_name = AirSimSettings::singleton().simmode_name; - relative_pose.position = settings.position; + relative_pose.position = position; if (std::isnan(relative_pose.position.x())) relative_pose.position.x() = 0; if (std::isnan(relative_pose.position.y())) @@ -68,9 +72,9 @@ struct DistanceSimpleParams { } 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; + pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0; + roll = !std::isnan(rotation.roll) ? rotation.roll : 0; + yaw = !std::isnan(rotation.yaw) ? rotation.yaw : 0; relative_pose.orientation = VectorMath::toQuaternion( Utils::degreesToRadians(pitch), //pitch - rotation around Y axis Utils::degreesToRadians(roll), //roll - rotation around X axis diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index dd32cfd22b..8645a3b609 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -15,7 +15,8 @@ namespace msr { namespace airlib { -class GpsSimple : public GpsBase { +class GpsSimple : public GpsBase +{ public: //methods GpsSimple(const AirSimSettings::GpsSetting& setting = AirSimSettings::GpsSetting()) : GpsBase(setting.sensor_name) diff --git a/AirLib/include/sensors/gps/GpsSimpleParams.hpp b/AirLib/include/sensors/gps/GpsSimpleParams.hpp index 52d8cf88c8..b8715fd16e 100644 --- a/AirLib/include/sensors/gps/GpsSimpleParams.hpp +++ b/AirLib/include/sensors/gps/GpsSimpleParams.hpp @@ -9,10 +9,11 @@ namespace msr { namespace airlib { -struct GpsSimpleParams { +struct GpsSimpleParams +{ real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f; real_T eph_initial = 100.0f, epv_initial = 100.0f; //initially fully diluted positions - real_T eph_final = 0.3f, epv_final = 0.4f; + real_T eph_final = 0.1f, epv_final = 0.1f; // PX4 won't converge GPS sensor fusion until we get to 10% accuracty. real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f; real_T update_latency = 0.2f; //sec @@ -21,7 +22,18 @@ struct GpsSimpleParams { void initializeFromSettings(const AirSimSettings::GpsSetting& settings) { - unused(settings); + const auto& json = settings.settings; + eph_time_constant = json.getFloat("EPH_TimeConstant", eph_time_constant); + epv_time_constant = json.getFloat("EPV_TimeConstant", epv_time_constant); + eph_initial = json.getFloat("EphInitial", eph_initial); + epv_initial = json.getFloat("EpvInitial", epv_initial); + eph_final = json.getFloat("EphFinal", eph_final); + epv_final = json.getFloat("EpvFinal", epv_final); + eph_min_3d = json.getFloat("EphMin3d", eph_min_3d); + eph_min_2d = json.getFloat("EphMin2d", eph_min_2d); + update_latency = json.getFloat("UpdateLatency", update_latency); + update_frequency = json.getFloat("UpdateFrequency", update_frequency); + startup_delay = json.getFloat("StartupDelay", startup_delay); } }; diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp index fbf38c730c..38bac02795 100644 --- a/AirLib/include/sensors/imu/ImuSimple.hpp +++ b/AirLib/include/sensors/imu/ImuSimple.hpp @@ -11,7 +11,8 @@ namespace msr { namespace airlib { -class ImuSimple : public ImuBase { +class ImuSimple : public ImuBase +{ public: //constructors ImuSimple(const AirSimSettings::ImuSetting& setting = AirSimSettings::ImuSetting()) diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp index 6cbe166a4e..115f65294e 100644 --- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp +++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp @@ -15,7 +15,8 @@ namespace msr { namespace airlib { // A description of the parameters: // https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics -struct ImuSimpleParams { +struct ImuSimpleParams +{ /* ref: Parameter values are for MPU 6000 IMU from InvenSense Design and Characterization of a Low Cost MEMS IMU Cluster for Precision Navigation Daniel R. Greenheck, 2009, sec 2.2, pp 17 @@ -24,8 +25,9 @@ struct ImuSimpleParams { https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf For Allan Variance/Deviation plots see http://www.invensense.com/wp-content/uploads/2015/02/MPU-3300-Datasheet.pdf */ - struct Gyroscope { - //angule random walk (ARW) + struct Gyroscope + { + //angular random walk (ARW) real_T arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec) //Bias Stability (tau = 500s) real_T tau = 500; @@ -33,12 +35,13 @@ struct ImuSimpleParams { Vector3r turn_on_bias = Vector3r::Zero(); //assume calibration is done } gyro; - struct Accelerometer { + struct Accelerometer + { //velocity random walk (ARW) real_T vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2 //Bias Stability (tau = 800s) real_T tau = 800; - real_T bias_stability = 36.0f * 1E-6f * 9.80665f; //ug converted to m/s^2 + real_T bias_stability = 36.0f * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2 Vector3r turn_on_bias = Vector3r::Zero(); //assume calibration is done } accel; @@ -46,7 +49,25 @@ struct ImuSimpleParams { void initializeFromSettings(const AirSimSettings::ImuSetting& settings) { - unused(settings); + const auto& json = settings.settings; + float arw = json.getFloat("AngularRandomWalk", Utils::nan()); + if (!std::isnan(arw)) { + gyro.arw = arw / sqrt(3600.0f) * M_PIf / 180; // //deg/sqrt(hour) converted to rad/sqrt(sec) + } + gyro.tau = json.getFloat("GyroBiasStabilityTau", gyro.tau); + float bias_stability = json.getFloat("GyroBiasStability", Utils::nan()); + if (!std::isnan(bias_stability)) { + gyro.bias_stability = bias_stability / 3600 * M_PIf / 180; //deg/hr converted to rad/sec + } + auto vrw = json.getFloat("VelocityRandomWalk", Utils::nan()); + if (!std::isnan(vrw)) { + accel.vrw = vrw * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2 + } + accel.tau = json.getFloat("AccelBiasStabilityTau", accel.tau); + bias_stability = json.getFloat("AccelBiasStability", Utils::nan()); + if (!std::isnan(bias_stability)) { + accel.bias_stability = bias_stability * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2 + } } }; diff --git a/AirLib/include/sensors/lidar/LidarSimple.hpp b/AirLib/include/sensors/lidar/LidarSimple.hpp index db6f4b05a7..f05f21b127 100644 --- a/AirLib/include/sensors/lidar/LidarSimple.hpp +++ b/AirLib/include/sensors/lidar/LidarSimple.hpp @@ -13,7 +13,8 @@ namespace msr { namespace airlib { -class LidarSimple : public LidarBase { +class LidarSimple : public LidarBase +{ public: LidarSimple(const AirSimSettings::LidarSetting& setting = AirSimSettings::LidarSetting()) : LidarBase(setting.sensor_name) diff --git a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp index 2cb5c1a9b8..e53922f817 100644 --- a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp +++ b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp @@ -9,7 +9,8 @@ namespace msr { namespace airlib { -struct LidarSimpleParams { +struct LidarSimpleParams +{ // Velodyne VLP-16 Puck config // https://velodynelidar.com/vlp-16.html @@ -43,17 +44,19 @@ struct LidarSimpleParams { { std::string simmode_name = AirSimSettings::singleton().simmode_name; - number_of_channels = settings.number_of_channels; - range = settings.range; - points_per_second = settings.points_per_second; - horizontal_rotation_frequency = settings.horizontal_rotation_frequency; + const auto& settings_json = settings.settings; + number_of_channels = settings_json.getInt("NumberOfChannels", number_of_channels); + range = settings_json.getFloat("Range", range); + points_per_second = settings_json.getInt("PointsPerSecond", points_per_second); + horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", horizontal_rotation_frequency); + draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points); + data_frame = settings_json.getString("DataFrame", data_frame); + external_controller = settings_json.getBool("ExternalController", external_controller); - horizontal_FOV_start = settings.horizontal_FOV_start; - horizontal_FOV_end = settings.horizontal_FOV_end; + vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", Utils::nan()); // By default, for multirotors the lidars FOV point downwards; // for cars, the lidars FOV is more forward facing. - vertical_FOV_upper = settings.vertical_FOV_upper; if (std::isnan(vertical_FOV_upper)) { if (simmode_name == AirSimSettings::kSimModeTypeMultirotor) vertical_FOV_upper = -15; @@ -61,7 +64,8 @@ struct LidarSimpleParams { vertical_FOV_upper = +10; } - vertical_FOV_lower = settings.vertical_FOV_lower; + + vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", Utils::nan()); if (std::isnan(vertical_FOV_lower)) { if (simmode_name == AirSimSettings::kSimModeTypeMultirotor) vertical_FOV_lower = -45; @@ -69,7 +73,13 @@ struct LidarSimpleParams { vertical_FOV_lower = -10; } - relative_pose.position = settings.position; + + horizontal_FOV_start = settings_json.getFloat("HorizontalFOVStart", horizontal_FOV_start); + horizontal_FOV_end = settings_json.getFloat("HorizontalFOVEnd", horizontal_FOV_end); + + relative_pose.position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector()); + auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation()); + if (std::isnan(relative_pose.position.x())) relative_pose.position.x() = 0; if (std::isnan(relative_pose.position.y())) @@ -82,17 +92,13 @@ struct LidarSimpleParams { } 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; + pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0; + roll = !std::isnan(rotation.roll) ? rotation.roll : 0; + yaw = !std::isnan(rotation.yaw) ? 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 - - draw_debug_points = settings.draw_debug_points; - data_frame = settings.data_frame; - external_controller = settings.external_controller; + 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/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index b2a1556611..c77eee9829 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -15,7 +15,8 @@ namespace msr { namespace airlib { -class MagnetometerSimple : public MagnetometerBase { +class MagnetometerSimple : public MagnetometerBase +{ public: MagnetometerSimple(const AirSimSettings::MagnetometerSetting& setting = AirSimSettings::MagnetometerSetting()) : MagnetometerBase(setting.sensor_name) @@ -66,8 +67,7 @@ class MagnetometerSimple : public MagnetometerBase { private: //methods void updateReference(const GroundTruth& ground_truth) { - switch (params_.ref_source) - { + switch (params_.ref_source) { case MagnetometerSimpleParams::ReferenceSource::ReferenceSource_Constant: // Constant magnetic field for Seattle magnetic_field_true_ = Vector3r(0.34252f, 0.09805f, 0.93438f); diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp index 652cb3257c..884cef5a1a 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp @@ -10,7 +10,8 @@ namespace msr { namespace airlib { -struct MagnetometerSimpleParams { +struct MagnetometerSimpleParams +{ enum ReferenceSource { ReferenceSource_Constant, ReferenceSource_DipoleModel @@ -34,7 +35,15 @@ struct MagnetometerSimpleParams { void initializeFromSettings(const AirSimSettings::MagnetometerSetting& settings) { - unused(settings); + const auto& json = settings.settings; + float noise = json.getFloat("NoiseSigma", noise_sigma.x()); + noise_sigma = Vector3r(noise, noise, noise); + scale_factor = json.getFloat("ScaleFactor", scale_factor); + float bias = json.getFloat("NoiseBias", noise_bias.x()); + noise_bias = Vector3r(bias, bias, bias); + update_latency = json.getFloat("UpdateLatency", update_latency); + update_frequency = json.getFloat("UpdateFrequency", update_frequency); + startup_delay = json.getFloat("StartupDelay", startup_delay); } }; diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 3f012b51e7..7d69420d43 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -14,7 +14,8 @@ namespace msr { namespace airlib { -class CarApiBase : public VehicleApiBase { +class CarApiBase : public VehicleApiBase +{ public: struct CarControls { float throttle = 0; /* 1 to -1 */ @@ -83,7 +84,7 @@ class CarApiBase : public VehicleApiBase { public: // TODO: Temporary constructor for the Unity implementation which does not use the new Sensor Configuration Settings implementation. - //CarApiBase() {} + //CarApiBase() {} CarApiBase(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, @@ -131,11 +132,7 @@ class CarApiBase : public VehicleApiBase { void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting) { - // use sensors from vehicle settings; if empty list, use default sensors. - // note that the vehicle settings completely override the default sensor "list"; - // there is no piecemeal add/remove/update per sensor. - const std::map>& sensor_settings - = vehicle_setting->sensors.size() > 0 ? vehicle_setting->sensors : AirSimSettings::AirSimSettings::singleton().sensor_defaults; + const auto& sensor_settings = vehicle_setting->sensors; sensor_factory_->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_); } @@ -149,7 +146,7 @@ class CarApiBase : public VehicleApiBase { std::shared_ptr sensor_factory_; SensorCollection sensors_; //maintains sensor type indexed collection of sensors - vector> sensor_storage_; //RAII for created sensors + vector> sensor_storage_; //RAII for created sensors protected: virtual void resetImplementation() override diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp index 9fb2dcf46a..ae61993aad 100644 --- a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp +++ b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp @@ -28,7 +28,8 @@ namespace msr { namespace airlib { -class ArduRoverApi : public CarApiBase { +class ArduRoverApi : public CarApiBase +{ public: ArduRoverApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, @@ -143,10 +144,10 @@ class ArduRoverApi : public CarApiBase { } 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); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); udp_socket_ = std::make_unique(); - udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port); + udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port_local); } private: diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index f04635e44a..bb730c8540 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -12,7 +12,8 @@ namespace msr { namespace airlib { -class MultiRotorParams { +class MultiRotorParams +{ //All units are SI public: //types struct RotorPose { @@ -85,11 +86,7 @@ class MultiRotorParams { void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting) { - // use sensors from vehicle settings; if empty list, use default sensors. - // note that the vehicle settings completely override the default sensor "list"; - // there is no piecemeal add/remove/update per sensor. - const std::map>& sensor_settings - = vehicle_setting->sensors.size() > 0 ? vehicle_setting->sensors : AirSimSettings::AirSimSettings::singleton().sensor_defaults; + const auto& sensor_settings = vehicle_setting->sensors; getSensorFactory()->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_); } @@ -412,7 +409,7 @@ class MultiRotorParams { private: Params params_; SensorCollection sensors_; //maintains sensor type indexed collection of sensors - vector> sensor_storage_; //RAII for created sensors + vector> sensor_storage_; //RAII for created sensors }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp index 81ceb42493..08265ae8bd 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp @@ -16,12 +16,15 @@ namespace msr { namespace airlib { -class MultiRotorPhysicsBody : public PhysicsBody { +class MultiRotorPhysicsBody : public PhysicsBody +{ public: MultiRotorPhysicsBody(MultiRotorParams* params, VehicleApiBase* vehicle_api, Kinematics* kinematics, Environment* environment) : params_(params), vehicle_api_(vehicle_api) { + setName("MultiRotorPhysicsBody"); + vehicle_api_->setParent(this); initialize(kinematics, environment); } diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp index a2993dfeb6..9bd5e3014d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -31,7 +31,8 @@ namespace msr { namespace airlib { -class ArduCopterApi : public MultirotorApiBase { +class ArduCopterApi : public MultirotorApiBase +{ public: ArduCopterApi(const MultiRotorParams* vehicle_params, const AirSimSettings::MavLinkConnectionInfo& connection_info) @@ -319,10 +320,10 @@ class ArduCopterApi : public MultirotorApiBase { } 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); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo); udp_socket_ = std::make_unique(); - udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port); + udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port_local); } private: diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp index 6190c86ef0..bbeeeb43b5 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp @@ -71,8 +71,7 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi packet.magic = 0x4c56414f; - if (udpSocket_ != nullptr) - { + if (udpSocket_ != nullptr) { std::vector msg(sizeof(packet)); memcpy(msg.data(), &packet, sizeof(packet)); udpSocket_->sendMessage(msg); @@ -113,9 +112,9 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi } 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); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); - udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port); + udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port_local); mavlinkcom::AdHocMessageHandler handler = [this](std::shared_ptr connection, const std::vector &msg) { this->rotorPowerMessageHandler(connection, msg); }; @@ -170,8 +169,7 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi void rotorPowerMessageHandler(std::shared_ptr connection, const std::vector &msg) { - if (msg.size() != sizeof(RotorControlMessage)) - { + 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; } @@ -201,8 +199,7 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi 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) - { + if (fabs(cos(theta)) < 1.0e-20) { theta += 1.0e-10f; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index 8167b08c5f..36bf96e088 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -4,1653 +4,2033 @@ #ifndef msr_airlib_MavLinkDroneController_hpp #define msr_airlib_MavLinkDroneController_hpp -#include "MavLinkVehicle.hpp" #include "MavLinkConnection.hpp" #include "MavLinkMessages.hpp" #include "MavLinkNode.hpp" +#include "MavLinkVehicle.hpp" #include "MavLinkVideoStream.hpp" -#include +#include +#include #include +#include #include -#include -#include -#include #include #include +#include +#include "common/AirSimSettings.hpp" #include "common/Common.hpp" -#include "common/common_utils/SmoothingFilter.hpp" -#include "common/common_utils/Timer.hpp" #include "common/CommonStructs.hpp" -#include "common/VectorMath.hpp" -#include "common/AirSimSettings.hpp" -#include "vehicles/multirotor/api/MultirotorApiBase.hpp" #include "common/PidController.hpp" +#include "common/VectorMath.hpp" +#include "common/common_utils/FileSystem.hpp" +#include "common/common_utils/SmoothingFilter.hpp" +#include "common/common_utils/Timer.hpp" +#include "physics/World.hpp" #include "sensors/SensorCollection.hpp" +#include "vehicles/multirotor/api/MultirotorApiBase.hpp" //sensors #include "sensors/barometer/BarometerBase.hpp" -#include "sensors/imu/ImuBase.hpp" +#include "sensors/distance/DistanceSimple.hpp" #include "sensors/gps/GpsBase.hpp" +#include "sensors/imu/ImuBase.hpp" #include "sensors/magnetometer/MagnetometerBase.hpp" -#include "sensors/distance/DistanceSimple.hpp" -namespace msr { namespace airlib { - -class MavLinkMultirotorApi : public MultirotorApiBase +namespace msr +{ +namespace airlib { -public: //methods - virtual ~MavLinkMultirotorApi() + + class MavLinkMultirotorApi : public MultirotorApiBase { - closeAllConnection(); - if (this->connect_thread_.joinable()) + public: //methods + virtual ~MavLinkMultirotorApi() { - this->connect_thread_.join(); + closeAllConnection(); + if (this->connect_thread_.joinable()) { + this->connect_thread_.join(); + } + if (this->telemetry_thread_.joinable()) { + this->telemetry_thread_.join(); + } } - } - - //non-base interface specific to MavLinKDroneController - void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation) - { - connection_info_ = connection_info; - sensors_ = sensors; - is_simulation_mode_ = is_simulation; - try { - openAllConnections(); - is_ready_ = true; + //non-base interface specific to MavLinKDroneController + void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation) + { + connection_info_ = connection_info; + sensors_ = sensors; + is_simulation_mode_ = is_simulation; + lock_step_enabled_ = connection_info.lock_step; + try { + openAllConnections(); + is_ready_ = true; + } + catch (std::exception& ex) { + is_ready_ = false; + is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what()); + } } - catch (std::exception& ex) { - is_ready_ = false; - is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what()); + + Pose getMocapPose() + { + std::lock_guard guard(mocap_pose_mutex_); + return mocap_pose_; } - } - Pose getMocapPose() - { - std::lock_guard guard(mocap_pose_mutex_); - return mocap_pose_; - } + virtual const SensorCollection& getSensors() const override + { + return *sensors_; + } - virtual const SensorCollection& getSensors() const override - { - return *sensors_; - } + //reset PX4 stack + virtual void resetImplementation() override + { + // note this is called AFTER "initialize" when we've connected to the drone + // so this method cannot reset any of that connection state. + world_ = nullptr; + + for (UpdatableObject* container = this->getParent(); container != nullptr; container = container->getParent()) { + if (container->getName() == "World") { + // cool beans! + // note: cannot use dynamic_cast because Unreal builds with /GR- for some unknown reason... + world_ = static_cast(container); + } + } + MultirotorApiBase::resetImplementation(); + was_reset_ = true; + } - //reset PX4 stack - virtual void resetImplementation() override - { - MultirotorApiBase::resetImplementation(); + unsigned long long getSimTime() + { + // This ensures HIL_SENSOR and HIL_GPS have matching clocks. + if (lock_step_active_) { + if (sim_time_us_ == 0) { + sim_time_us_ = clock()->nowNanos() / 1000; + } + return sim_time_us_; + } + else { + return clock()->nowNanos() / 1000; + } + } - resetState(); - was_reset_ = true; - setNormalMode(); - } + void advanceTime() + { + sim_time_us_ = clock()->nowNanos() / 1000; + } - //update sensors in PX4 stack - virtual void update() override - { - try { - MultirotorApiBase::update(); + //update sensors in PX4 stack + virtual void update() override + { + try { + auto now = clock()->nowNanos() / 1000; + MultirotorApiBase::update(); - if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_) - return; + if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_) + return; - if (send_params_) { - send_params_ = false; - sendParams(); - } + { + std::lock_guard guard(telemetry_mutex_); + update_count_++; + } - //send sensor updates - const auto& imu_output = getImuData(""); - const auto& mag_output = getMagnetometerData(""); - const auto& baro_output = getBarometerData(""); + if (lock_step_active_) { + if (last_update_time_ + 1000000 < now) { + // if 1 second passes then something is terribly wrong, reset lockstep mode + lock_step_active_ = false; + { + std::lock_guard guard(telemetry_mutex_); + lock_step_resets_++; + } + addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode"); + } + else if (!received_actuator_controls_) { + // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage. + return; + } + } - 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); + last_update_time_ = now; + { + std::lock_guard guard(telemetry_mutex_); + hil_sensor_count_++; + } + advanceTime(); + + //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); + + sendSystemTime(); + + const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); + if (count_distance_sensors != 0) { + const auto* distance_sensor = static_cast( + sensors_->getByType(SensorBase::SensorType::Distance)); + // Don't send the data if sending to external controller is disabled in settings + if (distance_sensor && distance_sensor->getParams().external_controller) { + const auto& distance_output = distance_sensor->getOutput(); + + sendDistanceSensor(distance_output.min_distance * 100, //m -> cm + distance_output.max_distance * 100, //m -> cm + distance_output.distance * 100, //m-> cm + 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_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); - if (count_distance_sensors != 0) { - const auto* distance_sensor = static_cast( - sensors_->getByType(SensorBase::SensorType::Distance)); - // Don't send the data if sending to external controller is disabled in settings - if (distance_sensor && distance_sensor->getParams().external_controller) { - const auto& distance_output = distance_sensor->getOutput(); + 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); + } + } - sendDistanceSensor(distance_output.min_distance * 100, //m -> cm - distance_output.max_distance * 100, //m -> cm - distance_output.distance * 100, //m-> cm - 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? + auto end = clock()->nowNanos() / 1000; + { + std::lock_guard guard(telemetry_mutex_); + update_time_ += (end - now); } } + 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. + } - 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; + //must be done at the end + if (was_reset_) + was_reset_ = false; + } - 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); - } + virtual bool isReady(std::string& message) const override + { + if (!is_ready_ && is_ready_message_.size() > 0) { + message = is_ready_message_; } - } - 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. + return is_ready_; } - //must be done at the end - if (was_reset_) - was_reset_ = false; - } - - virtual bool isReady(std::string& message) const override - { - if (!is_ready_ && is_ready_message_.size() > 0) { - message = is_ready_message_; + virtual bool canArm() const override + { + return is_ready_ && has_gps_lock_; } - 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 + { + updateState(); - //TODO: this method can't be const yet because it clears previous messages - virtual void getStatusMessages(std::vector& messages) override - { - updateState(); + //clear param + messages.clear(); - //clear param - messages.clear(); + //move messages from private vars to param + std::lock_guard guard(status_text_mutex_); + while (!status_messages_.empty()) { + messages.push_back(status_messages_.front()); + status_messages_.pop(); + } + } - //move messages from private vars to param - std::lock_guard guard(status_text_mutex_); - while (!status_messages_.empty()) { - messages.push_back(status_messages_.front()); - status_messages_.pop(); + virtual Kinematics::State getKinematicsEstimated() const override + { + updateState(); + Kinematics::State state; + //TODO: reduce code duplication in getPosition() etc methods? + state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z); + state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw); + state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z); + state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate); + state.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; + } + + virtual bool isApiControlEnabled() const override + { + return is_api_control_enabled_; } - } - virtual Kinematics::State getKinematicsEstimated() const override - { - updateState(); - Kinematics::State state; - //TODO: reduce code duplication in getPosition() etc methods? - state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z); - state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw); - state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z); - state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate); - state.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; - } - - virtual bool isApiControlEnabled() const override - { - return is_api_control_enabled_; - } + virtual void enableApiControl(bool is_enabled) override + { + checkValidVehicle(); + if (is_enabled) { + mav_vehicle_->requestControl(); + is_api_control_enabled_ = true; + } + else { + mav_vehicle_->releaseControl(); + is_api_control_enabled_ = false; + } + } - virtual void enableApiControl(bool is_enabled) override - { - checkValidVehicle(); - if (is_enabled) { - mav_vehicle_->requestControl(); - is_api_control_enabled_ = true; + virtual Vector3r getPosition() const override + { + updateState(); + return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z); } - else { - mav_vehicle_->releaseControl(); - is_api_control_enabled_ = false; + + virtual Vector3r getVelocity() const override + { + updateState(); + return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z); } - } - virtual Vector3r getPosition() const override - { - 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(); - return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z); - } + virtual Quaternionr getOrientation() const override + { + updateState(); + return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw); + } - virtual Quaternionr getOrientation() const override - { - updateState(); - return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw); - } + virtual LandedState getLandedState() const override + { + updateState(); + return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying; + } - virtual LandedState getLandedState() const override - { - updateState(); - return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying; - } + virtual real_T getActuation(unsigned int rotor_index) const override + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to read motor controls while not in simulation mode"); - virtual real_T getActuation(unsigned int rotor_index) const override - { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to read motor controls while not in simulation mode"); + std::lock_guard guard(hil_controls_mutex_); + return rotor_controls_[rotor_index]; + } - std::lock_guard guard(hil_controls_mutex_); - return rotor_controls_[rotor_index]; - } - virtual size_t getActuatorCount() const override - { - return RotorControlsCount; - } + virtual size_t getActuatorCount() const override + { + return RotorControlsCount; + } - virtual bool armDisarm(bool arm) override - { - SingleCall lock(this); + virtual bool armDisarm(bool arm) override + { + SingleCall lock(this); - checkValidVehicle(); - bool rc = false; - if (arm) { - float timeout_sec = 10; - waitForHomeLocation(timeout_sec); - waitForStableGroundPosition(timeout_sec); + 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; } - mav_vehicle_->armDisarm(arm).wait(10000, &rc); - return rc; - } + void onArmed() + { + if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) { + auto con = mav_vehicle_->getConnection(); + if (con != nullptr) { + if (log_ != nullptr) { + log_->close(); + log_ = nullptr; + } - 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"); + try { + std::time_t t = std::time(0); // get time now + std::tm* now = std::localtime(&t); + auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday); + auto path = common_utils::FileSystem::ensureFolder(connection_info_.logs, folder); + auto filename = Utils::stringf("%02d-%02d-%02d.mavlink", now->tm_hour, now->tm_min, now->tm_sec); + auto fullpath = common_utils::FileSystem::combine(path, filename); + addStatusMessage(Utils::stringf("Opening log file: %s", fullpath.c_str())); + log_file_name_ = fullpath; + log_ = std::make_shared(); + log_->openForWriting(fullpath, false); + con->startLoggingSendMessage(log_); + con->startLoggingReceiveMessage(log_); + if (con != connection_) { + // log the SITL channel also + connection_->startLoggingSendMessage(log_); + connection_->startLoggingReceiveMessage(log_); + } + start_telemtry_thread(); + } + catch (std::exception& ex) { + addStatusMessage(std::string("Opening log file failed: ") + ex.what()); + } + } } } - } - 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); + void onDisarmed() + { + if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) { + auto con = mav_vehicle_->getConnection(); + if (con != nullptr) { + con->stopLoggingSendMessage(); + con->stopLoggingReceiveMessage(); + } + if (connection_ != nullptr) { + connection_->stopLoggingSendMessage(); + connection_->stopLoggingReceiveMessage(); + } + } + if (log_ != nullptr) { + addStatusMessage(Utils::stringf("Closing log file: %s", log_file_name_.c_str())); + log_->close(); + log_ = nullptr; } } - } - - 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 */, yaw).wait(static_cast(timeout_sec * 1000), &rc)) { - throw VehicleMoveException("TakeOff command - timeout waiting for response"); + 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"); + } + } } - if (!rc) { - throw VehicleMoveException("TakeOff command rejected by drone"); + + 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); + } + } } - if (timeout_sec <= 0) - return true; // client doesn't want to wait. - return waitForZ(timeout_sec, z, getDistanceAccuracy()); - } + virtual bool takeoff(float timeout_sec) override + { + SingleCall lock(this); + + checkValidVehicle(); - virtual bool land(float timeout_sec) override - { - SingleCall lock(this); + waitForHomeLocation(timeout_sec); + waitForStableGroundPosition(timeout_sec); - //TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now - //we assume the ground is relatively flat an we are landing roughly at the home altitude. - updateState(); - checkValidVehicle(); - if (current_state_.home.is_set) { bool rc = false; - if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc)) - { - throw VehicleMoveException("Landing command - timeout waiting for response from drone"); + auto vec = getPosition(); + auto yaw = current_state_.attitude.yaw; + float z = vec.z() + getTakeoffZ(); + if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast(timeout_sec * 1000), &rc)) { + throw VehicleMoveException("TakeOff command - timeout waiting for response"); } - else if (!rc) { - throw VehicleMoveException("Landing command rejected by drone"); + if (!rc) { + throw VehicleMoveException("TakeOff command rejected by drone"); } + if (timeout_sec <= 0) + return true; // client doesn't want to wait. + + return waitForZ(timeout_sec, z, getDistanceAccuracy()); } - 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([&]() { + virtual bool land(float timeout_sec) override + { + SingleCall lock(this); + + //TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now + //we assume the ground is relatively flat an we are landing roughly at the home altitude. updateState(); - return current_state_.controls.landed; - }, timeout_sec); + checkValidVehicle(); + if (current_state_.home.is_set) { + bool rc = false; + if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc)) { + throw VehicleMoveException("Landing command - timeout waiting for response from drone"); + } + else if (!rc) { + throw VehicleMoveException("Landing command rejected by drone"); + } + } + else { + throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude. Could fix this if we hook up a distance to ground sensor..."); + } - // Wait for landed state (or user cancellation) - if (!waiter.isComplete()) - { - throw VehicleMoveException("Drone hasn't reported a landing state"); + const auto& waiter = waitForFunction([&]() { + updateState(); + return current_state_.controls.landed; + }, + timeout_sec); + + // Wait for landed state (or user cancellation) + if (!waiter.isComplete()) { + throw VehicleMoveException("Drone hasn't reported a landing state"); + } + return waiter.isComplete(); } - return waiter.isComplete(); - } - virtual bool goHome(float timeout_sec) override - { - SingleCall lock(this); + virtual bool goHome(float timeout_sec) override + { + SingleCall lock(this); - checkValidVehicle(); - bool rc = false; - if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait( - static_cast(timeout_sec) * 1000, &rc)) { - throw VehicleMoveException("goHome - timeout waiting for response from drone"); + checkValidVehicle(); + bool rc = false; + if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait( + static_cast(timeout_sec) * 1000, &rc)) { + throw VehicleMoveException("goHome - timeout waiting for response from drone"); + } + return rc; } - return rc; - } - virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override - { - SingleTaskCall lock(this); - - unused(adaptive_lookahead); - unused(lookahead); - unused(drivetrain); - - // save current manual, cruise, and max velocity parameters - bool result = false; - mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter; - result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter); - result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter); - result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter); - - if (result) { - // set max velocity parameter - mavlinkcom::MavLinkParameter p; - p.name = "MPC_XY_VEL_MAX"; - p.value = velocity; - mav_vehicle_->setParameter(p).wait(1000, &result); + virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override + { + SingleTaskCall lock(this); - if (result) { - const Vector3r& goal_pos = Vector3r(x, y, z); - Vector3r goal_dist_vect; - float goal_dist; + unused(adaptive_lookahead); + unused(lookahead); + unused(drivetrain); - Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); + // save current manual, cruise, and max velocity parameters + bool result = false; + mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter; + result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter); + result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter); + result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter); - while (!waiter.isComplete()) { - goal_dist_vect = getPosition() - goal_pos; - const Vector3r& goal_normalized = goal_dist_vect.normalized(); - goal_dist = goal_dist_vect.dot(goal_normalized); + if (result) { + // set max velocity parameter + mavlinkcom::MavLinkParameter p; + p.name = "MPC_XY_VEL_MAX"; + p.value = velocity; + mav_vehicle_->setParameter(p).wait(1000, &result); - if (goal_dist > getDistanceAccuracy()) { - moveToPositionInternal(goal_pos, yaw_mode); + if (result) { + const Vector3r& goal_pos = Vector3r(x, y, z); + Vector3r goal_dist_vect; + float goal_dist; - //sleep for rest of the cycle - if (!waiter.sleep()) - return false; - } - else { - waiter.complete(); - } - } + Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); - // reset manual, cruise, and max velocity parameters - bool result_temp = false; - mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result); - mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp); - result = result && result_temp; - mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp); - result = result && result_temp; + while (!waiter.isComplete()) { + goal_dist_vect = getPosition() - goal_pos; + const Vector3r& goal_normalized = goal_dist_vect.normalized(); + goal_dist = goal_dist_vect.dot(goal_normalized); - return result && waiter.isComplete(); - } - } + if (goal_dist > getDistanceAccuracy()) { + moveToPositionInternal(goal_pos, yaw_mode); - return result; - } + //sleep for rest of the cycle + if (!waiter.sleep()) + return false; + } + else { + waiter.complete(); + } + } - virtual bool hover() override - { - SingleCall lock(this); + // reset manual, cruise, and max velocity parameters + bool result_temp = false; + mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result); + mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp); + result = result && result_temp; + mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp); + result = result && result_temp; - bool rc = false; - checkValidVehicle(); - mavlinkcom::AsyncResult result = mav_vehicle_->loiter(); - //auto start_time = std::chrono::system_clock::now(); - while (!getCancelToken().isCancelled()) - { - if (result.wait(100, &rc)) - { - break; + return result && waiter.isComplete(); + } } + + return result; } - return rc; - } - virtual GeoPoint getHomeGeoPoint() const override - { - updateState(); - if (current_state_.home.is_set) - return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt); - else - return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); - } - - virtual GeoPoint getGpsLocation() const override - { - updateState(); - return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt); - } + virtual bool hover() override + { + SingleCall lock(this); - virtual void sendTelemetry(float last_interval = -1) override - { - if (logviewer_proxy_ == nullptr || connection_ == nullptr || mav_vehicle_ == nullptr) { - return; - } - mavlinkcom::MavLinkTelemetry data; - connection_->getTelemetry(data); - if (data.messagesReceived == 0) { - if (!hil_message_timer_.started()) { - hil_message_timer_.start(); - } - else if (hil_message_timer_.seconds() > messageReceivedTimeout) { - addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again"); + bool rc = false; + checkValidVehicle(); + mavlinkcom::AsyncResult result = mav_vehicle_->loiter(); + //auto start_time = std::chrono::system_clock::now(); + while (!getCancelToken().isCancelled()) { + if (result.wait(100, &rc)) { + break; + } } + return rc; } - else { - hil_message_timer_.stop(); + + virtual GeoPoint getHomeGeoPoint() const override + { + updateState(); + if (current_state_.home.is_set) + return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt); + else + return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); } - // listen to the other mavlink connection also - auto mavcon = mav_vehicle_->getConnection(); - if (mavcon != connection_) { - mavlinkcom::MavLinkTelemetry gcs; - mavcon->getTelemetry(gcs); + virtual GeoPoint getGpsLocation() const override + { + updateState(); + return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt); + } - data.handlerMicroseconds += gcs.handlerMicroseconds; - data.messagesHandled += gcs.messagesHandled; - data.messagesReceived += gcs.messagesReceived; - data.messagesSent += gcs.messagesSent; + virtual void sendTelemetry(float last_interval = -1) override + { + if (connection_ == nullptr || mav_vehicle_ == nullptr) { + return; + } - if (gcs.messagesReceived == 0) - { - if (!gcs_message_timer_.started()) { - gcs_message_timer_.start(); + // This method is called at high frequence from MultirotorPawnSimApi::updateRendering. + mavlinkcom::MavLinkTelemetry data; + connection_->getTelemetry(data); + if (data.messages_received == 0) { + if (!hil_message_timer_.started()) { + hil_message_timer_.start(); } - else if (gcs_message_timer_.seconds() > messageReceivedTimeout) { - addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again"); + else if (hil_message_timer_.seconds() > messageReceivedTimeout) { + addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again"); + hil_message_timer_.stop(); } } else { - gcs_message_timer_.stop(); + hil_message_timer_.stop(); } } - data.renderTime = static_cast(last_interval * 1000000);// microseconds - logviewer_proxy_->sendMessage(data); - } + void writeTelemetry(float last_interval = -1) + { + auto proxy = logviewer_proxy_; + auto log = log_; - virtual float getCommandPeriod() const override - { - return 1.0f / 50; //1 period of 50hz - } - virtual float getTakeoffZ() const override - { - // 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; - } - virtual float getDistanceAccuracy() const override - { - return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled - } + if ((logviewer_proxy_ == nullptr && log_ == nullptr)) { + return; + } -protected: //methods - 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 - // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not - // been tested on a real drone outside jMavSim, so it may need recalibrating... - thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); - target_height_ = -z; - } - checkValidVehicle(); - auto state = mav_vehicle_->getVehicleState(); - float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); - mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust); - } - 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(); - 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); - } + mavlinkcom::MavLinkTelemetry data; + connection_->getTelemetry(data); - virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override - { - checkValidVehicle(); - float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; - mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw); - } - virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override - { - checkValidVehicle(); - float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; - mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw); - } - virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override - { - checkValidVehicle(); - float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; - mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw); - } + // listen to the other mavlink connection also + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != connection_) { + mavlinkcom::MavLinkTelemetry gcs; + mavcon->getTelemetry(gcs); - //TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval - virtual const MultirotorApiParams& getMultirotorApiParams() const override - { - //defaults are good for PX4 generic quadcopter. - static const MultirotorApiParams vehicle_params_; - return vehicle_params_; - } + data.handler_microseconds += gcs.handler_microseconds; + data.messages_handled += gcs.messages_handled; + data.messages_received += gcs.messages_received; + data.messages_sent += gcs.messages_sent; - virtual void beforeTask() override - { - startOffboardMode(); - } - virtual void afterTask() override - { - endOffboardMode(); - } + if (gcs.messages_received == 0) { + if (!gcs_message_timer_.started()) { + gcs_message_timer_.start(); + } + else if (gcs_message_timer_.seconds() > messageReceivedTimeout) { + addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again"); + } + } + else { + gcs_message_timer_.stop(); + } + } -public: + data.render_time = static_cast(last_interval * 1000000); // microseconds - class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog - { - public: - MavLinkLogViewerLog(std::shared_ptr proxy) { - proxy_ = proxy; - } - void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override { - unused(timestamp); - mavlinkcom::MavLinkMessage copy; - ::memcpy(©, &msg, sizeof(mavlinkcom::MavLinkMessage)); - proxy_->sendMessage(copy); - } + { + std::lock_guard guard(telemetry_mutex_); + uint32_t average_delay = 0; + uint32_t average_update = 0; + if (hil_sensor_count_ != 0) { + average_delay = actuator_delay_ / hil_sensor_count_; + average_update = static_cast(update_time_ / hil_sensor_count_); + } - private: - std::shared_ptr proxy_; - }; + data.update_rate = update_count_; + data.sensor_rate = hil_sensor_count_; + data.actuation_delay = average_delay; + data.lock_step_resets = lock_step_resets_; + data.update_time = average_update; + // reset the counters we just captured. + update_count_ = 0; + hil_sensor_count_ = 0; + actuator_delay_ = 0; + update_time_ = 0; + } + if (proxy != nullptr) { + proxy->sendMessage(data); + } -protected: //methods - virtual void connect() - { - if (!connecting_) { - connecting_ = true; - if (this->connect_thread_.joinable()) - { - this->connect_thread_.join(); + if (log != nullptr) { + mavlinkcom::MavLinkMessage msg; + msg.magic = MAVLINK_STX_MAVLINK1; + data.encode(msg); + msg.update_checksum(); + // disk I/O is unpredictable, so we have to get it out of the update loop + // which is why this thread exists. + log->write(msg); } - this->connect_thread_ = std::thread(&MavLinkMultirotorApi::connect_thread, this); } - } - virtual void disconnect() { - addStatusMessage("Disconnecting mavlink vehicle"); - connected_ = false; - connecting_ = false; - if (connection_ != nullptr) { - if (is_hil_mode_set_ && mav_vehicle_ != nullptr) { - setNormalMode(); + void start_telemtry_thread() + { + if (this->telemetry_thread_.joinable()) { + this->telemetry_thread_.join(); } - connection_->close(); - } + // reset the counters we use for telemetry. + update_count_ = 0; + hil_sensor_count_ = 0; + actuator_delay_ = 0; - if (hil_node_ != nullptr) { - hil_node_->close(); + this->telemetry_thread_ = std::thread(&MavLinkMultirotorApi::telemtry_thread, this); } - if (mav_vehicle_ != nullptr) { - auto c = mav_vehicle_->getConnection(); - if (c != nullptr) { - c->stopLoggingSendMessage(); + void telemtry_thread() + { + while (log_ != nullptr) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + writeTelemetry(1); } - mav_vehicle_->close(); - mav_vehicle_ = nullptr; } - if (video_server_ != nullptr) - video_server_->close(); - - if (logviewer_proxy_ != nullptr) { - logviewer_proxy_->close(); - logviewer_proxy_ = nullptr; + virtual float getCommandPeriod() const override + { + return 1.0f / 50; //1 period of 50hz } - if (logviewer_out_proxy_ != nullptr) { - logviewer_out_proxy_->close(); - logviewer_out_proxy_ = nullptr; + virtual float getTakeoffZ() const override + { + // 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; } - if (qgc_proxy_ != nullptr) { - qgc_proxy_->close(); - qgc_proxy_ = nullptr; + virtual float getDistanceAccuracy() const override + { + return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled } - } - void connect_thread() - { - addStatusMessage("Waiting for mavlink vehicle..."); - connecting_ = true; - createMavConnection(connection_info_); - if (mav_vehicle_ != nullptr) { - connectToLogViewer(); - connectToQGC(); - } - connecting_ = false; - connected_ = true; - } - - virtual void close() - { - disconnect(); - } + protected: //methods + 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); + } - void closeAllConnection() - { - close(); - } + 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 + // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not + // been tested on a real drone outside jMavSim, so it may need recalibrating... + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkValidVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust); + } + 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(); + 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); + } -private: //methods + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + checkValidVehicle(); + mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle); + } - void openAllConnections() - { - close(); //just in case if connections were open - resetState(); //reset all variables we might have changed during last session + 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); + } - connect(); - } + 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); + } - void getMocapPose(Vector3r& position, Quaternionr& orientation) const - { - position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z; - orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1]; - orientation.y() = MocapPoseMessage.q[2]; orientation.z() = MocapPoseMessage.q[3]; - } + 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); + } - //TODO: this method used to send collision to external sim. Do we still need this? - void sendCollision(float normalX, float normalY, float normalZ) - { - checkValidVehicle(); - - mavlinkcom::MavLinkCollision collision{}; - collision.src = 1; //provider of data is MavLink system in id field - collision.id = mav_vehicle_->getLocalSystemId(); - collision.action = static_cast(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT); - collision.threat_level = static_cast(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE); - // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off. - collision.time_to_minimum_delta = normalX; - collision.altitude_minimum_delta = normalY; - collision.horizontal_minimum_delta = normalZ; - mav_vehicle_->sendMessage(collision); - } - - //TODO: do we still need this method? - bool hasVideoRequest() - { - mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req; - return video_server_->hasVideoRequest(image_req); - } + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override + { + checkValidVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw); + } - //TODO: do we still need this method? - void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height) - { - const int MAVLINK_DATA_STREAM_IMG_PNG = 6; - video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0); - } + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override + { + checkValidVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw); + } - //put PX4 in normal mode (i.e. non-simulation mode) - void setNormalMode() - { - if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) { + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override + { + checkValidVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw); + } - // 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); + //TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval + virtual const MultirotorApiParams& getMultirotorApiParams() const override + { + //defaults are good for PX4 generic quadcopter. + static const MultirotorApiParams vehicle_params_; + return vehicle_params_; + } - mavlinkcom::MavCmdDoSetMode cmd; - cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); - cmd.Mode = static_cast(mode); - mav_vehicle_->sendCommand(cmd); + virtual void beforeTask() override + { + startOffboardMode(); + } - is_hil_mode_set_ = false; + virtual void afterTask() override + { + endOffboardMode(); } - } - //put PX4 in simulation mode - void setHILMode() - { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode"); + public: + class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog + { + public: + MavLinkLogViewerLog(std::shared_ptr proxy) + { + proxy_ = proxy; + } + ~MavLinkLogViewerLog() + { + proxy_ = nullptr; + } - checkValidVehicle(); + void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override + { + if (proxy_ != nullptr) { + unused(timestamp); + mavlinkcom::MavLinkMessage copy; + ::memcpy(©, &msg, sizeof(mavlinkcom::MavLinkMessage)); + try { + proxy_->sendMessage(copy); + } + catch (std::exception&) { + failures++; + if (failures == 100) { + // hmmm, doesn't like the proxy, bad socket perhaps, so stop trying... + proxy_ = nullptr; + } + } + } + } - // 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); + private: + std::shared_ptr proxy_; + int failures = 0; + }; - mavlinkcom::MavCmdDoSetMode cmd; - cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); - cmd.Mode = static_cast(mode); - mav_vehicle_->sendCommand(cmd); + protected: //methods + 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); + } + } - is_hil_mode_set_ = true; - } + virtual void disconnect() + { + addStatusMessage("Disconnecting mavlink vehicle"); + connected_ = false; + connecting_ = false; + + if (is_armed_) { + // close the telemetry log. + is_armed_ = false; + onDisarmed(); + } - bool startOffboardMode() - { - checkValidVehicle(); - try { - mav_vehicle_->requestControl(); - } - catch (std::exception& ex) { - ensureSafeMode(); - addStatusMessage(std::string("Request control failed: ") + ex.what()); - return false; - } - return true; - } + if (connection_ != nullptr) { + if (is_hil_mode_set_ && mav_vehicle_ != nullptr) { + setNormalMode(); + } - void endOffboardMode() - { - // bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth. - // The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout - // when you stop sending move commands and the behavior on timeout is then determined by the drone itself. - // mav_vehicle_->releaseControl(); - ensureSafeMode(); - } - - void ensureSafeMode() - { - if (mav_vehicle_ != nullptr) { - const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState(); - if (state.controls.landed || !state.controls.armed) { - return; + connection_->stopLoggingSendMessage(); + connection_->stopLoggingReceiveMessage(); + connection_->close(); } - } - } - void checkValidVehicle() { - 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"); - } - } + if (hil_node_ != nullptr) { + hil_node_->close(); + } - //status update methods should call this first! - void updateState() const - { - StatusLock lock(this); - if (mav_vehicle_ != nullptr) { - int version = mav_vehicle_->getVehicleStateVersion(); - if (version != state_version_) - { - current_state_ = mav_vehicle_->getVehicleState(); - state_version_ = version; + if (mav_vehicle_ != nullptr) { + auto c = mav_vehicle_->getConnection(); + if (c != nullptr) { + c->stopLoggingSendMessage(); + c->stopLoggingReceiveMessage(); + } + mav_vehicle_->close(); + mav_vehicle_ = nullptr; } - } - } - virtual void normalizeRotorControls() - { - //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case - //we normalize them to 0 to 1 for PX4 - if (!is_controls_0_1_) { - // change -1 to 1 to 0 to 1. - for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { - rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f; - } - } - else { - //this applies to PX4 and may work differently on other firmwares. - //We use 0.2 as idle rotors which leaves out range of 0.8 - for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { - rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f); - } - } - } - - bool sendTestMessage(std::shared_ptr node) { - try { - // try and send a test message. - mavlinkcom::MavLinkHeartbeat test; - test.autopilot = static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4); - test.type = static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS); - test.base_mode = 0; - test.custom_mode = 0; - test.mavlink_version = 3; - node->sendMessage(test); - test.system_status = 0; - return true; - } - catch (std::exception&) { - return false; - } - } + if (video_server_ != nullptr) + video_server_->close(); - bool connectToLogViewer() - { - //set up logviewer proxy - if (connection_info_.logviewer_ip_address.size() > 0) { - std::shared_ptr connection; - createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip, - logviewer_proxy_, connection); - if (!sendTestMessage(logviewer_proxy_)) { - // error talking to log viewer, so don't keep trying, and close the connection also. - logviewer_proxy_->getConnection()->close(); + if (logviewer_proxy_ != nullptr) { + logviewer_proxy_->close(); logviewer_proxy_ = nullptr; } - std::shared_ptr out_connection; - createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip, - logviewer_out_proxy_, out_connection); - if (!sendTestMessage(logviewer_out_proxy_)) { - // error talking to log viewer, so don't keep trying, and close the connection also. - logviewer_out_proxy_->getConnection()->close(); + if (logviewer_out_proxy_ != nullptr) { + logviewer_out_proxy_->close(); logviewer_out_proxy_ = nullptr; } - else if (mav_vehicle_ != nullptr) { - mav_vehicle_->getConnection()->startLoggingSendMessage(std::make_shared(logviewer_out_proxy_)); + + if (qgc_proxy_ != nullptr) { + qgc_proxy_->close(); + qgc_proxy_ = nullptr; } + + resetState(); } - return logviewer_proxy_ != nullptr; - } - bool connectToQGC() - { - if (connection_info_.qgc_ip_address.size() > 0) { - std::shared_ptr connection; - createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection); - if (!sendTestMessage(qgc_proxy_)) { - // error talking to QGC, so don't keep trying, and close the connection also. - qgc_proxy_->getConnection()->close(); - qgc_proxy_ = nullptr; + void connect_thread() + { + addStatusMessage("Waiting for mavlink vehicle..."); + connecting_ = true; + createMavConnection(connection_info_); + if (mav_vehicle_ != nullptr) { + connectToLogViewer(); + connectToQGC(); } - else { - connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) { - unused(connection_val); - processQgcMessages(msg); - }); + + if (connecting_) { + // only set connected if we haven't already been told to disconnect. + connecting_ = false; + connected_ = true; } } - return qgc_proxy_ != nullptr; - } + virtual void close() + { + disconnect(); + } - void createProxy(std::string name, std::string ip, int port, string local_host_ip, - std::shared_ptr& node, std::shared_ptr& connection) - { - if (connection_ == nullptr) - throw std::domain_error("MavLinkMultirotorApi requires connection object to be set before createProxy call"); + void closeAllConnection() + { + close(); + } - connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port); + private: //methods + void openAllConnections() + { + Utils::log("Opening mavlink connection"); + close(); //just in case if connections were open + resetState(); //reset all variables we might have changed during last session + connect(); + } - // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint - // and all other messages are funneled through from PX4 via the Join method below. - node = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); - node->connect(connection); + void getMocapPose(Vector3r& position, Quaternionr& orientation) const + { + position.x() = MocapPoseMessage.x; + position.y() = MocapPoseMessage.y; + position.z() = MocapPoseMessage.z; + orientation.w() = MocapPoseMessage.q[0]; + orientation.x() = MocapPoseMessage.q[1]; + orientation.y() = MocapPoseMessage.q[2]; + orientation.z() = MocapPoseMessage.q[3]; + } - // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be - // send directly to the PX4 (using whatever sysid/compid comes from that remote node). - connection_->join(connection); + //TODO: this method used to send collision to external sim. Do we still need this? + void sendCollision(float normalX, float normalY, float normalZ) + { + checkValidVehicle(); + + mavlinkcom::MavLinkCollision collision{}; + collision.src = 1; //provider of data is MavLink system in id field + collision.id = mav_vehicle_->getLocalSystemId(); + collision.action = static_cast(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT); + collision.threat_level = static_cast(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE); + // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off. + collision.time_to_minimum_delta = normalX; + collision.altitude_minimum_delta = normalY; + collision.horizontal_minimum_delta = normalZ; + mav_vehicle_->sendMessage(collision); + } + + //TODO: do we still need this method? + bool hasVideoRequest() + { + mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req; + return video_server_->hasVideoRequest(image_req); + } - auto mavcon = mav_vehicle_->getConnection(); - if (mavcon != connection_) { - mavcon->join(connection); + //TODO: do we still need this method? + void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height) + { + const int MAVLINK_DATA_STREAM_IMG_PNG = 6; + video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0); } - } - static std::string findPX4() - { - auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0); - for (auto iter = result.begin(); iter != result.end(); iter++) { - mavlinkcom::SerialPortInfo info = *iter; - if (( - (info.vid == pixhawkVendorId) && - (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId || info.pid == pixhawkFMUV5ProductId) - ) || - ( - (info.displayName.find(L"PX4_") != std::string::npos) - )) { - // printf("Auto Selecting COM port: %S\n", info.displayName.c_str()); - - std::string portName_str; - - for (wchar_t ch : info.portName) - { - portName_str.push_back(static_cast(ch)); - } - return portName_str; + //put PX4 in normal mode (i.e. non-simulation mode) + void setNormalMode() + { + if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) { + + // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode + std::lock_guard guard(set_mode_mutex_); + int mode = mav_vehicle_->getVehicleState().mode; + mode &= ~static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); + + mavlinkcom::MavCmdDoSetMode cmd; + cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); + cmd.Mode = static_cast(mode); + mav_vehicle_->sendCommand(cmd); + + is_hil_mode_set_ = false; } } - return ""; - } - void createMavConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info) - { - if (connection_info.use_serial) { - createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate); + //put PX4 in simulation mode + void setHILMode() + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode"); + + checkValidVehicle(); + + // 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); + + mavlinkcom::MavCmdDoSetMode cmd; + cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); + cmd.Mode = static_cast(mode); + mav_vehicle_->sendCommand(cmd); + + is_hil_mode_set_ = true; } - else { - createMavEthernetConnection(connection_info); + + bool startOffboardMode() + { + checkValidVehicle(); + try { + mav_vehicle_->requestControl(); + } + catch (std::exception& ex) { + ensureSafeMode(); + addStatusMessage(std::string("Request control failed: ") + ex.what()); + return false; + } + return true; } - //Uncomment below for sending images over MavLink - //connectToVideoServer(); - } + void endOffboardMode() + { + // bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth. + // The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout + // when you stop sending move commands and the behavior on timeout is then determined by the drone itself. + // mav_vehicle_->releaseControl(); + ensureSafeMode(); + } - void createMavEthernetConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info) - { - close(); + void ensureSafeMode() + { + if (mav_vehicle_ != nullptr) { + const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState(); + if (state.controls.landed || !state.controls.armed) { + return; + } + } + } - connecting_ = true; - 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); + void checkValidVehicle() + { + 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"); + } + } - if (connection_info.use_tcp) { - if (connection_info.tcp_port == 0) { - throw std::invalid_argument("TcpPort setting has an invalid value."); + //status update methods should call this first! + void updateState() const + { + StatusLock lock(this); + if (mav_vehicle_ != nullptr) { + int version = mav_vehicle_->getVehicleStateVersion(); + if (version != state_version_) { + current_state_ = mav_vehicle_->getVehicleState(); + state_version_ = version; + } } + } - 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); + virtual void normalizeRotorControls() + { + //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case + //we normalize them to 0 to 1 for PX4 + if (!is_controls_0_1_) { + // change -1 to 1 to 0 to 1. + for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { + rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f; + } } - catch (std::exception& e) { - addStatusMessage("Accepting TCP socket failed, is another instance running?"); - addStatusMessage(e.what()); - return; + else { + //this applies to PX4 and may work differently on other firmwares. + //We use 0.2 as idle rotors which leaves out range of 0.8 + for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { + rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f); + } } } - else if (connection_info.udp_address.size() > 0) { - if (connection_info.udp_port == 0) { - throw std::invalid_argument("UdpPort setting has an invalid value."); + + bool sendTestMessage(std::shared_ptr node) + { + try { + // try and send a test message. + mavlinkcom::MavLinkHeartbeat test; + test.autopilot = static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4); + test.type = static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS); + test.base_mode = 0; + test.custom_mode = 0; + test.mavlink_version = 3; + node->sendMessage(test); + test.system_status = 0; + return true; + } + catch (std::exception&) { + return false; } + } + + bool connectToLogViewer() + { + //set up logviewer proxy + if (connection_info_.logviewer_ip_address.size() > 0) { + std::shared_ptr connection; + createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip, logviewer_proxy_, connection); + if (!sendTestMessage(logviewer_proxy_)) { + // error talking to log viewer, so don't keep trying, and close the connection also. + logviewer_proxy_->getConnection()->close(); + logviewer_proxy_ = nullptr; + } - connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, connection_info.udp_address, connection_info.udp_port); + std::shared_ptr out_connection; + createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip, logviewer_out_proxy_, out_connection); + if (!sendTestMessage(logviewer_out_proxy_)) { + // error talking to log viewer, so don't keep trying, and close the connection also. + logviewer_out_proxy_->getConnection()->close(); + logviewer_out_proxy_ = nullptr; + } + else if (mav_vehicle_ != nullptr) { + auto proxylog = std::make_shared(logviewer_out_proxy_); + mav_vehicle_->getConnection()->startLoggingSendMessage(proxylog); + mav_vehicle_->getConnection()->startLoggingReceiveMessage(proxylog); + if (connection_ != nullptr) { + connection_->startLoggingSendMessage(proxylog); + connection_->startLoggingReceiveMessage(proxylog); + } + } + } + return logviewer_proxy_ != nullptr; } - else { - throw std::invalid_argument("Please provide valid connection info for your drone."); + + bool connectToQGC() + { + if (connection_info_.qgc_ip_address.size() > 0) { + std::shared_ptr connection; + createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection); + if (!sendTestMessage(qgc_proxy_)) { + // error talking to QGC, so don't keep trying, and close the connection also. + qgc_proxy_->getConnection()->close(); + qgc_proxy_ = nullptr; + } + else { + connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) { + unused(connection_val); + processQgcMessages(msg); + }); + } + } + return qgc_proxy_ != nullptr; } - // start listening to the SITL connection. - connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { - unused(connection); - processMavMessages(msg); - }); + void createProxy(std::string name, std::string ip, int port, string local_host_ip, + std::shared_ptr& node, std::shared_ptr& connection) + { + if (connection_ == nullptr) + throw std::domain_error("MavLinkMultirotorApi requires connection object to be set before createProxy call"); - hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); - hil_node_->connect(connection_); + connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port); - if (connection_info.use_tcp) { - addStatusMessage(std::string("Connected to SITL over TCP.")); - } - else { - addStatusMessage(std::string("Connected to SITL over UDP.")); + // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint + // and all other messages are funneled through from PX4 via the Join method below. + node = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + node->connect(connection); + + // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be + // send directly to the PX4 (using whatever sysid/compid comes from that remote node). + connection_->join(connection); + + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != connection_) { + mavcon->join(connection); + } } - mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); + static std::string findPX4() + { + auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0); + for (auto iter = result.begin(); iter != result.end(); iter++) { + mavlinkcom::SerialPortInfo info = *iter; + if (( + (info.vid == pixhawkVendorId) && + (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId || info.pid == pixhawkFMUV5ProductId)) || + ((info.displayName.find(L"PX4_") != std::string::npos))) { + // printf("Auto Selecting COM port: %S\n", info.displayName.c_str()); + + std::string portName_str; + + for (wchar_t ch : info.portName) { + portName_str.push_back(static_cast(ch)); + } + return portName_str; + } + } + return ""; + } - if (connection_info_.control_ip_address != "") { - if (connection_info_.control_port == 0) { - throw std::invalid_argument("ControlPort setting has an invalid value."); + void createMavConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info) + { + if (connection_info.use_serial) { + createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate); } + else { + createMavEthernetConnection(connection_info); + } + + //Uncomment below for sending images over MavLink + //connectToVideoServer(); + } - // 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())); + void createMavEthernetConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info) + { + close(); - // if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ? - for (int retries = 60; retries >= 0 && connecting_; retries--) { + connecting_ = true; + is_controls_0_1_ = true; + std::string remoteIpAddr; + 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 { - auto gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs", - connection_info_.local_host_ip, connection_info_.control_ip_address, connection_info_.control_port); - mav_vehicle_->connect(gcsConnection); + connection_ = std::make_shared(); + remoteIpAddr = connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port); } - catch (std::exception&) { - std::this_thread::sleep_for(std::chrono::seconds(1)); + 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 (mav_vehicle_->getConnection() != nullptr) { - addStatusMessage(std::string("Ground control connected over UDP.")); + connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, connection_info.udp_address, connection_info.udp_port); } else { - addStatusMessage(std::string("Timeout trying to connect ground control over UDP.")); - return; + throw std::invalid_argument("Please provide valid connection info for your drone."); } - } - - 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) { + // start listening to the SITL connection. + connection_->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(); + hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + hil_node_->connect(connection_); - // Also request home position messages - mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1); - } + if (connection_info.use_tcp) { + addStatusMessage(std::string("Connected to SITL over TCP.")); + } + else { + addStatusMessage(std::string("Connected to SITL over UDP.")); + } - void createMavSerialConnection(const std::string& port_name, int baud_rate) - { - close(); + mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); - connecting_ = true; - bool reported = false; - std::string port_name_auto = port_name; - while (port_name_auto == "" || port_name_auto == "*") { - port_name_auto = findPX4(); - if (port_name_auto == "") { - 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."); + if (connection_info_.control_ip_address != "") { + if (connection_info_.control_port_local == 0) { + throw std::invalid_argument("LocalControlPort setting has an invalid value."); + } + if (!connection_info.use_tcp || connection_info_.control_ip_address != "remote") { + remoteIpAddr = connection_info_.control_ip_address; + } + if (remoteIpAddr == "local" || remoteIpAddr == "localhost") { + remoteIpAddr = "127.0.0.1"; + } + + // 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 %s ...", + connection_info_.control_port_local, + connection_info_.local_host_ip.c_str(), + remoteIpAddr.c_str())); + + // if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ? + for (int retries = 60; retries >= 0 && connecting_; retries--) { + try { + std::shared_ptr gcsConnection; + if (remoteIpAddr == "127.0.0.1") { + gcsConnection = mavlinkcom::MavLinkConnection::connectLocalUdp("gcs", + connection_info_.local_host_ip, + connection_info_.control_port_local); + } + else { + gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs", + connection_info_.local_host_ip, + remoteIpAddr, + connection_info_.control_port_remote); + } + mav_vehicle_->connect(gcsConnection); + // need to try and send something to make sure the connection is good. + mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1); + break; + } + catch (std::exception&) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } + + if (mav_vehicle_ == nullptr) { + // play was stopped! + return; + } + + if (mav_vehicle_->getConnection() != nullptr) { + addStatusMessage(std::string("Ground control connected over UDP.")); + } + else { + addStatusMessage(std::string("Timeout trying to connect ground control over UDP.")); + return; } - std::this_thread::sleep_for(std::chrono::seconds(1)); + } + try { + connectVehicle(); + } + catch (std::exception& e) { + addStatusMessage("Error connecting vehicle:"); + addStatusMessage(e.what()); } } - if (port_name_auto == "") { - addStatusMessage("USB port for PX4 flight controller is empty. Please set it in settings.json."); - return; + void processControlMessages(const mavlinkcom::MavLinkMessage& msg) + { + // Utils::log(Utils::stringf("Control msg %d", msg.msgid)); + // PX4 usually sends the following on the control channel. + // If nothing is arriving here it means our control channel UDP connection isn't working. + // MAVLINK_MSG_ID_HIGHRES_IMU + // MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET + // MAVLINK_MSG_ID_SERVO_OUTPUT_RAW + // MAVLINK_MSG_ID_GPS_RAW_INT + // MAVLINK_MSG_ID_TIMESYNC + // MAVLINK_MSG_ID_ALTITUDE + // MAVLINK_MSG_ID_VFR_HUD + // MAVLINK_MSG_ID_ESTIMATOR_STATUS + // MAVLINK_MSG_ID_EXTENDED_SYS_STATE + // MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN + // MAVLINK_MSG_ID_PING + // MAVLINK_MSG_ID_SYS_STATUS + // MAVLINK_MSG_ID_SYSTEM_TIME + // MAVLINK_MSG_ID_LINK_NODE_STATUS + // MAVLINK_MSG_ID_AUTOPILOT_VERSION + // MAVLINK_MSG_ID_COMMAND_ACK + // MAVLINK_MSG_ID_STATUSTEXT + // MAVLINK_MSG_ID_PARAM_VALUE + processMavMessages(msg); } - if (baud_rate == 0) { - addStatusMessage("Baud rate specified in settings.json is 0 which is invalid"); - return; + void connectVehicle() + { + // listen to this UDP mavlink connection also + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != nullptr && mavcon != connection_) { + mavcon->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processControlMessages(msg); + }); + } + else { + mav_vehicle_->connect(connection_); + } + + connected_ = true; + + // Also request home position messages + mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1); + + // now we can start our heartbeats. + mav_vehicle_->startHeartbeat(); + + // wait for px4 to connect so we can safely send any configured parameters + while (!send_params_ && connected_) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + if (connected_) { + sendParams(); + send_params_ = false; + } } - addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate)); - reported = false; + void createMavSerialConnection(const std::string& port_name, int baud_rate) + { + close(); - while (connecting_) { - 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); - }); + connecting_ = true; + bool reported = false; + std::string port_name_auto = port_name; + while (port_name_auto == "" || port_name_auto == "*") { + port_name_auto = findPX4(); + if (port_name_auto == "") { + 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 == "") { + addStatusMessage("USB port for PX4 flight controller is empty. Please set it in settings.json."); 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)); + + if (baud_rate == 0) { + addStatusMessage("Baud rate specified in settings.json is 0 which is invalid"); + return; } - } - } - mavlinkcom::MavLinkHilSensor getLastSensorMessage() - { - std::lock_guard guard(last_message_mutex_); - return last_sensor_message_; - } + addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate)); + reported = false; - mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage() - { - std::lock_guard guard(last_message_mutex_); - return last_distance_message_; - } + while (connecting_) { + 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())); + + // start listening to the HITL connection. + connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processMavMessages(msg); + }); - mavlinkcom::MavLinkHilGps getLastGpsMessage() - { - std::lock_guard guard(last_message_mutex_); - return last_gps_message_; - } + mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); - 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())); + connectVehicle(); + 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)); } } } - } - void setArmed(bool armed) - { - is_armed_ = armed; - if (!armed) { - //reset motor controls - for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { - rotor_controls_[i] = 0; - } + mavlinkcom::MavLinkHilSensor getLastSensorMessage() + { + std::lock_guard guard(last_message_mutex_); + return last_sensor_message_; } - } - void processQgcMessages(const mavlinkcom::MavLinkMessage& msg) - { - if (msg.msgid == MocapPoseMessage.msgid) { - std::lock_guard guard(mocap_pose_mutex_); - MocapPoseMessage.decode(msg); - getMocapPose(mocap_pose_.position, mocap_pose_.orientation); + mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage() + { + std::lock_guard guard(last_message_mutex_); + return last_distance_message_; } - //else ignore message - } - void addStatusMessage(const std::string& 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); + mavlinkcom::MavLinkHilGps getLastGpsMessage() + { + std::lock_guard guard(last_message_mutex_); + return last_gps_message_; } - } - - void processMavMessages(const mavlinkcom::MavLinkMessage& msg) - { - if (msg.msgid == HeartbeatMessage.msgid) { - std::lock_guard guard_heartbeat(heartbeat_mutex_); - HeartbeatMessage.decode(msg); - - bool armed = (HeartbeatMessage.base_mode & static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0; - setArmed(armed); - 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; + void sendParams() + { + // send any mavlink parameters from settings.json through to the connected vehicle. + if (mav_vehicle_ != nullptr && connection_info_.params.size() > 0) { + try { + 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())); + } + } + } + catch (std::exception& ex) { + addStatusMessage("Exception sending parameters to vehicle"); + addStatusMessage(ex.what()); } + } + } - send_params_ = true; + void setArmed(bool armed) + { + if (is_armed_) { + if (!armed) { + onDisarmed(); + } } - else if (is_simulation_mode_ && !is_hil_mode_set_) { - setHILMode(); + else { + if (armed) { + onArmed(); + } } - } - else if (msg.msgid == StatusTextMessage.msgid) { - StatusTextMessage.decode(msg); - //lock is established by below method - addStatusMessage(std::string(StatusTextMessage.text)); - } - else if (msg.msgid == CommandLongMessage.msgid) { - CommandLongMessage.decode(msg); - if (CommandLongMessage.command == static_cast(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) { - int msg_id = static_cast(CommandLongMessage.param1 + 0.5); - if (msg_id == 115) { //HIL_STATE_QUATERNION - hil_state_freq_ = static_cast(CommandLongMessage.param2 + 0.5); + + is_armed_ = armed; + if (!armed) { + //reset motor controls + for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { + rotor_controls_[i] = 0; } } } - else if (msg.msgid == HilControlsMessage.msgid) { - if (!actuators_message_supported_) { - std::lock_guard guard_controls(hil_controls_mutex_); - - HilControlsMessage.decode(msg); - rotor_controls_[0] = HilControlsMessage.roll_ailerons; - rotor_controls_[1] = HilControlsMessage.pitch_elevator; - rotor_controls_[2] = HilControlsMessage.yaw_rudder; - rotor_controls_[3] = HilControlsMessage.throttle; - rotor_controls_[4] = HilControlsMessage.aux1; - rotor_controls_[5] = HilControlsMessage.aux2; - rotor_controls_[6] = HilControlsMessage.aux3; - rotor_controls_[7] = HilControlsMessage.aux4; - normalizeRotorControls(); - received_actuator_controls_ = true; + void processQgcMessages(const mavlinkcom::MavLinkMessage& msg) + { + if (msg.msgid == MocapPoseMessage.msgid) { + std::lock_guard guard(mocap_pose_mutex_); + MocapPoseMessage.decode(msg); + getMocapPose(mocap_pose_.position, mocap_pose_.orientation); } + //else ignore message } - else if (msg.msgid == HilActuatorControlsMessage.msgid) { - actuators_message_supported_ = true; - std::lock_guard guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl - - HilActuatorControlsMessage.decode(msg); - bool isarmed = (HilActuatorControlsMessage.mode & 128) != 0; - for (auto i = 0; i < 8; ++i) { - if (isarmed) { - rotor_controls_[i] = HilActuatorControlsMessage.controls[i]; - } - else { - rotor_controls_[i] = 0; - } - } - if (isarmed) { - normalizeRotorControls(); + void addStatusMessage(const std::string& 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 handleLockStep() + { received_actuator_controls_ = true; // if the timestamps match then it means we are in lockstep mode. - if (!lock_step_enabled_) { + if (!lock_step_active_ && lock_step_enabled_) { // && (HilActuatorControlsMessage.flags & 0x1)) // todo: enable this check when this flag is widely available... - if (hil_sensor_clock_ == HilActuatorControlsMessage.time_usec) { + if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) { addStatusMessage("Enabling lockstep mode"); - lock_step_enabled_ = true; + lock_step_active_ = 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; + else { + auto now = clock()->nowNanos() / 1000; + auto delay = static_cast(now - last_update_time_); + + std::lock_guard guard(telemetry_mutex_); + actuator_delay_ += delay; } - if (!has_home_ && current_state_.home.is_set) { - addStatusMessage("Got GPS Home Location"); - has_home_ = true; + if (world_ != nullptr) { + world_->pause(false); } - } - 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(); + + void processMavMessages(const mavlinkcom::MavLinkMessage& msg) + { + if (msg.msgid == HeartbeatMessage.msgid) { + std::lock_guard guard_heartbeat(heartbeat_mutex_); + + HeartbeatMessage.decode(msg); + + bool armed = (HeartbeatMessage.base_mode & static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0; + setArmed(armed); + 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; + } + } + else if (is_simulation_mode_ && !is_hil_mode_set_) { + setHILMode(); + } } - } - 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 - } + else if (msg.msgid == StatusTextMessage.msgid) { + StatusTextMessage.decode(msg); + //lock is established by below method + addStatusMessage(std::string(StatusTextMessage.text)); + } + else if (msg.msgid == CommandLongMessage.msgid) { + CommandLongMessage.decode(msg); + if (CommandLongMessage.command == static_cast(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) { + int msg_id = static_cast(CommandLongMessage.param1 + 0.5); + if (msg_id == 115) { //HIL_STATE_QUATERNION + hil_state_freq_ = static_cast(CommandLongMessage.param2 + 0.5); + } + } + } + else if (msg.msgid == HilControlsMessage.msgid) { + if (!actuators_message_supported_) { + std::lock_guard guard_controls(hil_controls_mutex_); + + HilControlsMessage.decode(msg); + rotor_controls_[0] = HilControlsMessage.roll_ailerons; + rotor_controls_[1] = HilControlsMessage.pitch_elevator; + rotor_controls_[2] = HilControlsMessage.yaw_rudder; + rotor_controls_[3] = HilControlsMessage.throttle; + rotor_controls_[4] = HilControlsMessage.aux1; + rotor_controls_[5] = HilControlsMessage.aux2; + rotor_controls_[6] = HilControlsMessage.aux3; + rotor_controls_[7] = HilControlsMessage.aux4; + + normalizeRotorControls(); + handleLockStep(); + } + } + else if (msg.msgid == HilActuatorControlsMessage.msgid) { + actuators_message_supported_ = true; - void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt) - { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode"); + std::lock_guard guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl - auto now = clock()->nowNanos() / 1000; - 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"); - } + HilActuatorControlsMessage.decode(msg); + bool isarmed = (HilActuatorControlsMessage.mode & 128) != 0; + for (auto i = 0; i < 8; ++i) { + if (isarmed) { + rotor_controls_[i] = HilActuatorControlsMessage.controls[i]; + } + else { + rotor_controls_[i] = 0; + } + } + if (isarmed) { + normalizeRotorControls(); + } - if (!received_actuator_controls_) { - // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage. - return; + handleLockStep(); + } + 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; + } + send_params_ = 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(); + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkHomePosition::kMessageId) { + mavlinkcom::MavLinkHomePosition home; + home.decode(msg); + // this is a good time to send the params + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkSysStatus::kMessageId) { + // this is a good time to send the params + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkAutopilotVersion::kMessageId) { + // this is a good time to send the params + send_params_ = true; + } + else { + // creates too much log output, only do this when debugging this issue specifically. + // Utils::log(Utils::stringf("Ignoring msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid)); } } - hil_sensor_clock_ = now; - - mavlinkcom::MavLinkHilSensor hil_sensor; - last_hil_sensor_time_ = now; - hil_sensor.time_usec = hil_sensor_clock_; + void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt) + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode"); - hil_sensor.xacc = acceleration.x(); - hil_sensor.yacc = acceleration.y(); - hil_sensor.zacc = acceleration.z(); - hil_sensor.fields_updated = 0b111; // Set accel bit fields + mavlinkcom::MavLinkHilSensor hil_sensor; + hil_sensor.time_usec = last_hil_sensor_time_ = getSimTime(); - hil_sensor.xgyro = gyro.x(); - hil_sensor.ygyro = gyro.y(); - hil_sensor.zgyro = gyro.z(); + 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.fields_updated |= 0b111000; // Set gyro bit fields + hil_sensor.xgyro = gyro.x(); + hil_sensor.ygyro = gyro.y(); + hil_sensor.zgyro = gyro.z(); - hil_sensor.xmag = mag.x(); - hil_sensor.ymag = mag.y(); - hil_sensor.zmag = mag.z(); + hil_sensor.fields_updated |= 0b111000; // Set gyro bit fields - hil_sensor.fields_updated |= 0b111000000; // Set mag bit fields + hil_sensor.xmag = mag.x(); + hil_sensor.ymag = mag.y(); + hil_sensor.zmag = mag.z(); - hil_sensor.abs_pressure = abs_pressure; - hil_sensor.pressure_alt = pressure_alt; + hil_sensor.fields_updated |= 0b111000000; // Set mag bit fields - hil_sensor.fields_updated |= 0b1101000000000; // Set baro bit fields + hil_sensor.abs_pressure = abs_pressure; + hil_sensor.pressure_alt = pressure_alt; - //TODO: enable temperature? diff_pressure - if (was_reset_) { - hil_sensor.fields_updated = static_cast(1 << 31); - } + hil_sensor.fields_updated |= 0b1101000000000; // Set baro bit fields - if (hil_node_ != nullptr) { - hil_node_->sendMessage(hil_sensor); - received_actuator_controls_ = false; - } + //TODO: enable temperature? diff_pressure + if (was_reset_) { + hil_sensor.fields_updated = static_cast(1 << 31); + } - std::lock_guard guard(last_message_mutex_); - last_sensor_message_ = hil_sensor; - } + if (hil_node_ != nullptr) { + hil_node_->sendMessage(hil_sensor); + received_actuator_controls_ = false; + if (lock_step_active_ && world_ != nullptr) { + world_->pauseForTime(1); // 1 second delay max waiting for actuator controls. + } + } - void sendDistanceSensor(float min_distance, float max_distance, float current_distance, - uint8_t sensor_type, uint8_t sensor_id, const Quaternionr& orientation) - { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); + std::lock_guard guard(last_message_mutex_); + last_sensor_message_ = hil_sensor; + } - mavlinkcom::MavLinkDistanceSensor distance_sensor; + void sendSystemTime() + { + // SYSTEM TIME from host + auto tu = getSimTime(); + uint32_t ms = (uint32_t)(tu / 1000); + if (ms != last_sys_time_) { + last_sys_time_ = ms; + mavlinkcom::MavLinkSystemTime msg_system_time; + msg_system_time.time_unix_usec = tu; + msg_system_time.time_boot_ms = last_sys_time_; + if (hil_node_ != nullptr) { + hil_node_->sendMessage(msg_system_time); + } + } + } - distance_sensor.time_boot_ms = static_cast(clock()->nowNanos() / 1000000); - distance_sensor.min_distance = static_cast(min_distance); - distance_sensor.max_distance = static_cast(max_distance); - distance_sensor.current_distance = static_cast(current_distance); - distance_sensor.type = sensor_type; - distance_sensor.id = sensor_id; + void sendDistanceSensor(float min_distance, float max_distance, float current_distance, + uint8_t sensor_type, uint8_t sensor_id, const Quaternionr& orientation) + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); + + mavlinkcom::MavLinkDistanceSensor distance_sensor; + distance_sensor.time_boot_ms = static_cast(getSimTime() / 1000); + distance_sensor.min_distance = static_cast(min_distance); + distance_sensor.max_distance = static_cast(max_distance); + distance_sensor.current_distance = static_cast(current_distance); + distance_sensor.type = sensor_type; + distance_sensor.id = sensor_id; + + // 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? + // PX4 doesn't support receiving simulated distance sensor messages this way. + // It results in lots of error messages from PX4. This code is still useful in that + // it sets last_distance_message_ and that is returned via Python API. + // + // if (hil_node_ != nullptr) { + // hil_node_->sendMessage(distance_sensor); + // } + + std::lock_guard guard(last_message_mutex_); + last_distance_message_ = distance_sensor; + } + + void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog, + float eph, float epv, int fix_type, unsigned int satellites_visible) + { + unused(satellites_visible); + + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode"); + + mavlinkcom::MavLinkHilGps hil_gps; + hil_gps.time_usec = getSimTime(); + hil_gps.lat = static_cast(geo_point.latitude * 1E7); + hil_gps.lon = static_cast(geo_point.longitude * 1E7); + hil_gps.alt = static_cast(geo_point.altitude * 1000); + hil_gps.vn = static_cast(velocity.x() * 100); + hil_gps.ve = static_cast(velocity.y() * 100); + hil_gps.vd = static_cast(velocity.z() * 100); + hil_gps.eph = static_cast(eph * 100); + hil_gps.epv = static_cast(epv * 100); + hil_gps.fix_type = static_cast(fix_type); + hil_gps.vel = static_cast(velocity_xy * 100); + hil_gps.cog = static_cast(cog * 100); + hil_gps.satellites_visible = static_cast(15); + + if (hil_node_ != nullptr) { + hil_node_->sendMessage(hil_gps); + } - // 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(); + if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) { + //Utils::DebugBreak(); + Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError); + } - //TODO: use covariance parameter? + std::lock_guard guard(last_message_mutex_); + last_gps_message_ = hil_gps; + } - if (hil_node_ != nullptr) { - hil_node_->sendMessage(distance_sensor); + void resetState() + { + //reset state + is_hil_mode_set_ = false; + is_controls_0_1_ = true; + hil_state_freq_ = -1; + actuators_message_supported_ = false; + state_version_ = 0; + current_state_ = mavlinkcom::VehicleState(); + target_height_ = 0; + got_first_heartbeat_ = false; + is_armed_ = false; + has_home_ = false; + sim_time_us_ = 0; + last_sys_time_ = 0; + last_gps_time_ = 0; + last_update_time_ = 0; + last_hil_sensor_time_ = 0; + update_count_ = 0; + hil_sensor_count_ = 0; + lock_step_resets_ = 0; + actuator_delay_ = 0; + is_api_control_enabled_ = false; + thrust_controller_ = PidController(); + Utils::setValue(rotor_controls_, 0.0f); + was_reset_ = false; + received_actuator_controls_ = false; + lock_step_active_ = 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(); } - std::lock_guard guard(last_message_mutex_); - last_distance_message_ = distance_sensor; - } + 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; + } + } - void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog, - float eph, float epv, int fix_type, unsigned int satellites_visible) - { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode"); - - mavlinkcom::MavLinkHilGps hil_gps; - hil_gps.time_usec = hil_sensor_clock_; - hil_gps.lat = static_cast(geo_point.latitude * 1E7); - hil_gps.lon = static_cast(geo_point.longitude * 1E7); - hil_gps.alt = static_cast(geo_point.altitude * 1000); - hil_gps.vn = static_cast(velocity.x() * 100); - hil_gps.ve = static_cast(velocity.y() * 100); - hil_gps.vd = static_cast(velocity.z() * 100); - hil_gps.eph = static_cast(eph * 100); - hil_gps.epv = static_cast(epv * 100); - hil_gps.fix_type = static_cast(fix_type); - hil_gps.vel = static_cast(velocity_xy * 100); - hil_gps.cog = static_cast(cog * 100); - hil_gps.satellites_visible = static_cast(15); - - if (hil_node_ != nullptr) { - hil_node_->sendMessage(hil_gps); - } - - if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) { - //Utils::DebugBreak(); - Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError); - } - - std::lock_guard guard(last_message_mutex_); - last_gps_message_ = hil_gps; - } - - void resetState() - { - //reset state - is_hil_mode_set_ = false; - is_controls_0_1_ = true; - hil_state_freq_ = -1; - actuators_message_supported_ = false; - last_gps_time_ = 0; - state_version_ = 0; - current_state_ = mavlinkcom::VehicleState(); - target_height_ = 0; - is_api_control_enabled_ = false; - thrust_controller_ = PidController(); - Utils::setValue(rotor_controls_, 0.0f); - was_reset_ = false; - 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; - } - } - - -protected: //variables - - //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; - - const SensorCollection* sensors_; - mutable std::mutex hil_controls_mutex_; - AirSimSettings::MavLinkConnectionInfo connection_info_; - float rotor_controls_[RotorControlsCount]; - bool is_simulation_mode_; - - -private: //variables - static const int pixhawkVendorId = 9900; ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow - static const int pixhawkFMUV4ProductId = 18; ///< Product ID for Pixhawk V2 board - static const int pixhawkFMUV2ProductId = 17; ///< Product ID for Pixhawk V2 board - static const int pixhawkFMUV5ProductId = 50; ///< Product ID for Pixhawk V5 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 - - std::shared_ptr logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_; - - size_t status_messages_MaxSize = 5000; - - std::shared_ptr hil_node_; - std::shared_ptr connection_; - std::shared_ptr video_server_; - std::shared_ptr mav_vehicle_control_; - - mavlinkcom::MavLinkAttPosMocap MocapPoseMessage; - mavlinkcom::MavLinkHeartbeat HeartbeatMessage; - mavlinkcom::MavLinkSetMode SetModeMessage; - mavlinkcom::MavLinkStatustext StatusTextMessage; - mavlinkcom::MavLinkHilControls HilControlsMessage; - mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage; - mavlinkcom::MavLinkGpsRawInt MavLinkGpsRawInt; - mavlinkcom::MavLinkCommandLong CommandLongMessage; - mavlinkcom::MavLinkLocalPositionNed MavLinkLocalPositionNed; - - mavlinkcom::MavLinkHilSensor last_sensor_message_; - mavlinkcom::MavLinkDistanceSensor last_distance_message_; - mavlinkcom::MavLinkHilGps last_gps_message_; - - std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_; - - //variables required for VehicleApiBase implementation - 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_ = 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 - std::shared_ptr mav_vehicle_; - float target_height_; - bool is_api_control_enabled_; - PidController thrust_controller_; - common_utils::Timer hil_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_; -}; + protected: //variables + //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; + + const SensorCollection* sensors_; + mutable std::mutex hil_controls_mutex_; + AirSimSettings::MavLinkConnectionInfo connection_info_; + float rotor_controls_[RotorControlsCount]; + bool is_simulation_mode_; + + private: //variables + static const int pixhawkVendorId = 9900; ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow + static const int pixhawkFMUV4ProductId = 18; ///< Product ID for Pixhawk V2 board + static const int pixhawkFMUV2ProductId = 17; ///< Product ID for Pixhawk V2 board + static const int pixhawkFMUV5ProductId = 50; ///< Product ID for Pixhawk V5 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 + + std::shared_ptr logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_; + + size_t status_messages_MaxSize = 5000; + + std::shared_ptr hil_node_; + std::shared_ptr connection_; + std::shared_ptr video_server_; + std::shared_ptr mav_vehicle_control_; + + mavlinkcom::MavLinkAttPosMocap MocapPoseMessage; + mavlinkcom::MavLinkHeartbeat HeartbeatMessage; + mavlinkcom::MavLinkSetMode SetModeMessage; + mavlinkcom::MavLinkStatustext StatusTextMessage; + mavlinkcom::MavLinkHilControls HilControlsMessage; + mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage; + mavlinkcom::MavLinkGpsRawInt MavLinkGpsRawInt; + mavlinkcom::MavLinkCommandLong CommandLongMessage; + mavlinkcom::MavLinkLocalPositionNed MavLinkLocalPositionNed; + + mavlinkcom::MavLinkHilSensor last_sensor_message_; + mavlinkcom::MavLinkDistanceSensor last_distance_message_; + mavlinkcom::MavLinkHilGps last_gps_message_; + + std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_, telemetry_mutex_; + + //variables required for VehicleApiBase implementation + bool got_first_heartbeat_ = false, is_hil_mode_set_ = false, is_armed_ = false; + 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_ = false; + bool was_reset_ = false; + bool has_home_ = false; + bool is_ready_ = false; + bool has_gps_lock_ = false; + bool lock_step_active_ = 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; + + // variables for throttling HIL_SENSOR and SYSTEM_TIME messages + uint32_t last_sys_time_ = 0; + unsigned long long sim_time_us_ = 0; + uint64_t last_gps_time_ = 0; + uint64_t last_update_time_ = 0; + uint64_t last_hil_sensor_time_ = 0; + + // variables accumulated for MavLinkTelemetry messages. + uint64_t update_time_ = 0; + uint32_t update_count_ = 0; + uint32_t hil_sensor_count_ = 0; + uint32_t lock_step_resets_ = 0; + uint32_t actuator_delay_ = 0; + std::thread telemetry_thread_; + + //additional variables required for MultirotorApiBase implementation + //this is optional for methods that might not use vehicle commands + std::shared_ptr mav_vehicle_; + float target_height_; + bool is_api_control_enabled_; + PidController thrust_controller_; + common_utils::Timer hil_message_timer_; + common_utils::Timer gcs_message_timer_; + std::shared_ptr log_; + std::string log_file_name_; + World* world_; + + //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 #endif diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index b64000cc43..79da40fd8d 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -38,7 +38,14 @@ int main(int argc, const char* argv[]) Settings& settings = Settings::singleton().loadJSonFile(settings_full_filepath); Settings child; if (settings.isLoadSuccess()) { - settings.getChild("PX4", child); + Settings vehicles; + if (settings.hasKey("Vehicles")) { + settings.getChild("Vehicles", vehicles); + vehicles.getChild("PX4", child); + } + else { + settings.getChild("PX4", child); + } // allow json overrides on a per-vehicle basis. connection_info.sim_sysid = static_cast(child.getInt("SimSysID", connection_info.sim_sysid)); @@ -58,7 +65,9 @@ int main(int argc, const char* argv[]) connection_info.qgc_ip_port = child.getInt("QgcPort", connection_info.qgc_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.control_port_local = child.getInt("ControlPort", connection_info.control_port_local); + connection_info.control_port_local = child.getInt("ControlPortLocal", connection_info.control_port_local); + connection_info.control_port_remote = child.getInt("ControlPortRemote", connection_info.control_port_remote); connection_info.local_host_ip = child.getString("LocalHostIp", connection_info.local_host_ip); @@ -69,6 +78,8 @@ int main(int argc, const char* argv[]) 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); + connection_info.model = child.getString("Model", connection_info.model); + connection_info.logs = child.getString("Logs", connection_info.logs); } else { diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj index eb1037a5f9..94f159a800 100644 --- a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj +++ b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj @@ -155,7 +155,7 @@ - Use + NotUsing Level3 Disabled true @@ -169,7 +169,7 @@ - Use + NotUsing Level3 MaxSpeed true diff --git a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml index ef8824b346..f4043fe64e 100644 --- a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml +++ b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml @@ -33,14 +33,6 @@ - - - - - - - @@ -54,5 +46,16 @@ + + + + + + + + + + diff --git a/LogViewer/LogViewer/LogViewer.csproj b/LogViewer/LogViewer/LogViewer.csproj index 3dd09bb285..a0a5065def 100644 --- a/LogViewer/LogViewer/LogViewer.csproj +++ b/LogViewer/LogViewer/LogViewer.csproj @@ -33,7 +33,7 @@ PX4 Log Viewer true publish.htm - 62 + 63 1.0.0.%2a false true diff --git a/LogViewer/LogViewer/Model/MavlinkLog.cs b/LogViewer/LogViewer/Model/MavlinkLog.cs index 7b92b7dfde..bb0572f0f8 100644 --- a/LogViewer/LogViewer/Model/MavlinkLog.cs +++ b/LogViewer/LogViewer/Model/MavlinkLog.cs @@ -584,7 +584,8 @@ public async Task Load(string file, ProgressUtility progress) this.duration = TimeSpan.Zero; bool first = true; // QT time is milliseconds from the following epoch. - byte[] msgBuf = new byte[256]; + const int BUFFER_SIZE = 8000; + byte[] msgBuf = new byte[BUFFER_SIZE]; GCHandle handle = GCHandle.Alloc(msgBuf, GCHandleType.Pinned); IntPtr ptr = handle.AddrOfPinnedObject(); @@ -605,14 +606,17 @@ await Task.Run(() => { MavLinkMessage header = new MavLinkMessage(); header.ReadHeader(reader); + + Array.Clear(msgBuf, 0, BUFFER_SIZE); int read = s.Read(msgBuf, 0, header.Length); if (read == header.Length) { + int id = (int)header.MsgId; header.Crc = reader.ReadUInt16(); bool checkCrc = true; - if ((int)header.MsgId == MAVLink.mavlink_telemetry.MessageId) + if (id == MAVLink.mavlink_telemetry.MessageId) { if (header.Crc == 0) // telemetry has no crc. { diff --git a/LogViewer/LogViewer/publish.cmd b/LogViewer/LogViewer/publish.cmd new file mode 100644 index 0000000000..7c55bbcb06 --- /dev/null +++ b/LogViewer/LogViewer/publish.cmd @@ -0,0 +1,6 @@ +@echo off +cd %~dp0 + +echo Uploading ClickOnce installer for Px4LogViewer +AzurePublishClickOnce %~dp0publish downloads/Px4LogViewer "%LOVETTSOFTWARE_STORAGE_CONNECTION_STRING%" +if ERRORLEVEL 1 goto :eof diff --git a/LogViewer/Networking/Mavlink/MavlinkTypes.cs b/LogViewer/Networking/Mavlink/MavlinkTypes.cs index 549f056b50..ceecab6229 100644 --- a/LogViewer/Networking/Mavlink/MavlinkTypes.cs +++ b/LogViewer/Networking/Mavlink/MavlinkTypes.cs @@ -2350,7 +2350,7 @@ public enum ADSB_FLAGS }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 42)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_sensor_offsets_t { /// magnetic declination (radians) @@ -2381,7 +2381,7 @@ public struct mavlink_sensor_offsets_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_set_mag_offsets_t { /// magnetometer X offset @@ -2398,7 +2398,7 @@ public struct mavlink_set_mag_offsets_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_meminfo_t { /// heap top @@ -2409,7 +2409,7 @@ public struct mavlink_meminfo_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_ap_adc_t { /// ADC output 1 @@ -2428,7 +2428,7 @@ public struct mavlink_ap_adc_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 15)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_digicam_configure_t { /// Correspondent value to given extra_param @@ -2457,7 +2457,7 @@ public struct mavlink_digicam_configure_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 13)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_digicam_control_t { /// Correspondent value to given extra_param @@ -2484,7 +2484,7 @@ public struct mavlink_digicam_control_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mount_configure_t { /// System ID @@ -2503,7 +2503,7 @@ public struct mavlink_mount_configure_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 15)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mount_control_t { /// pitch(deg*100) or lat, depending on mount mode @@ -2522,7 +2522,7 @@ public struct mavlink_mount_control_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mount_status_t { /// pitch(deg*100) @@ -2539,7 +2539,7 @@ public struct mavlink_mount_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_fence_point_t { /// Latitude of point @@ -2558,7 +2558,7 @@ public struct mavlink_fence_point_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_fence_fetch_point_t { /// System ID @@ -2571,7 +2571,7 @@ public struct mavlink_fence_fetch_point_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_fence_status_t { /// time of last breach in milliseconds since boot @@ -2586,7 +2586,7 @@ public struct mavlink_fence_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_ahrs_t { /// X gyro drift estimate rad/s @@ -2607,7 +2607,7 @@ public struct mavlink_ahrs_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 44)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_simstate_t { /// Roll angle (rad) @@ -2636,7 +2636,7 @@ public struct mavlink_simstate_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hwstatus_t { /// board voltage (mV) @@ -2647,7 +2647,7 @@ public struct mavlink_hwstatus_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 9)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_radio_t { /// receive errors @@ -2668,7 +2668,7 @@ public struct mavlink_radio_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_limits_status_t { /// time of last breach in milliseconds since boot @@ -2693,7 +2693,7 @@ public struct mavlink_limits_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_wind_t { /// wind direction that wind is coming from (degrees) @@ -2706,7 +2706,7 @@ public struct mavlink_wind_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_data16_t { /// data type @@ -2720,7 +2720,7 @@ public struct mavlink_data16_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 34)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_data32_t { /// data type @@ -2734,7 +2734,7 @@ public struct mavlink_data32_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 66)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_data64_t { /// data type @@ -2748,7 +2748,7 @@ public struct mavlink_data64_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 98)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_data96_t { /// data type @@ -2762,7 +2762,7 @@ public struct mavlink_data96_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_rangefinder_t { /// distance in meters @@ -2773,7 +2773,7 @@ public struct mavlink_rangefinder_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 48)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_airspeed_autocal_t { /// GPS velocity north m/s @@ -2804,7 +2804,7 @@ public struct mavlink_airspeed_autocal_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 19)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_rally_point_t { /// Latitude of point in degrees * 1E7 @@ -2831,7 +2831,7 @@ public struct mavlink_rally_point_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_rally_fetch_point_t { /// System ID @@ -2844,7 +2844,7 @@ public struct mavlink_rally_fetch_point_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 20)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_compassmot_status_t { /// current (amps) @@ -2863,7 +2863,7 @@ public struct mavlink_compassmot_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 24)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_ahrs2_t { /// Roll angle (rad) @@ -2882,7 +2882,7 @@ public struct mavlink_ahrs2_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 29)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_camera_status_t { /// Image timestamp (microseconds since UNIX epoch, according to camera clock) @@ -2907,7 +2907,7 @@ public struct mavlink_camera_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 45)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_camera_feedback_t { /// Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) @@ -2940,7 +2940,7 @@ public struct mavlink_camera_feedback_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_battery2_t { /// voltage in millivolts @@ -2951,7 +2951,7 @@ public struct mavlink_battery2_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 40)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_ahrs3_t { /// Roll angle (rad) @@ -2978,7 +2978,7 @@ public struct mavlink_ahrs3_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_autopilot_version_request_t { /// System ID @@ -2989,7 +2989,7 @@ public struct mavlink_autopilot_version_request_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 206)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_remote_log_data_block_t { /// log data block sequence number @@ -3005,7 +3005,7 @@ public struct mavlink_remote_log_data_block_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 7)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_remote_log_block_status_t { /// log data block sequence number @@ -3020,7 +3020,7 @@ public struct mavlink_remote_log_block_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 29)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_led_control_t { /// System ID @@ -3040,7 +3040,7 @@ public struct mavlink_led_control_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 27)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mag_cal_progress_t { /// Body frame direction vector for display @@ -3066,7 +3066,7 @@ public struct mavlink_mag_cal_progress_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 44)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mag_cal_report_t { /// RMS milligauss residuals @@ -3101,7 +3101,7 @@ public struct mavlink_mag_cal_report_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_ekf_status_report_t { /// Velocity variance @@ -3120,7 +3120,7 @@ public struct mavlink_ekf_status_report_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 25)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_pid_tuning_t { /// desired rate (degrees/s) @@ -3141,7 +3141,7 @@ public struct mavlink_pid_tuning_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 42)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gimbal_report_t { /// Time since last update (seconds) @@ -3172,7 +3172,7 @@ public struct mavlink_gimbal_report_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gimbal_control_t { /// Demanded angular rate X (rad/s) @@ -3189,7 +3189,7 @@ public struct mavlink_gimbal_control_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gimbal_torque_cmd_report_t { /// Roll Torque Command @@ -3206,7 +3206,7 @@ public struct mavlink_gimbal_torque_cmd_report_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gopro_heartbeat_t { /// Status @@ -3219,7 +3219,7 @@ public struct mavlink_gopro_heartbeat_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gopro_get_request_t { /// System ID @@ -3232,7 +3232,7 @@ public struct mavlink_gopro_get_request_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gopro_get_response_t { /// Command ID @@ -3246,7 +3246,7 @@ public struct mavlink_gopro_get_response_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 7)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gopro_set_request_t { /// System ID @@ -3262,7 +3262,7 @@ public struct mavlink_gopro_set_request_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gopro_set_response_t { /// Command ID @@ -3273,7 +3273,7 @@ public struct mavlink_gopro_set_response_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_rpm_t { /// RPM Sensor1 @@ -3284,7 +3284,7 @@ public struct mavlink_rpm_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 9)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_heartbeat_t { /// A bitfield for use for autopilot-specific flags. @@ -3303,7 +3303,7 @@ public struct mavlink_heartbeat_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 31)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_sys_status_t { /// 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 @@ -3336,7 +3336,7 @@ public struct mavlink_sys_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_system_time_t { /// Timestamp of the master clock in microseconds since UNIX epoch. @@ -3347,7 +3347,7 @@ public struct mavlink_system_time_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_ping_t { /// Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) @@ -3362,7 +3362,7 @@ public struct mavlink_ping_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_change_operator_control_t { /// System the GCS requests control for @@ -3378,7 +3378,7 @@ public struct mavlink_change_operator_control_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_change_operator_control_ack_t { /// ID of the GCS this message @@ -3391,7 +3391,7 @@ public struct mavlink_change_operator_control_ack_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_auth_key_t { /// key @@ -3401,7 +3401,7 @@ public struct mavlink_auth_key_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_set_mode_t { /// The new autopilot-specific mode. This field can be ignored by an autopilot. @@ -3414,7 +3414,7 @@ public struct mavlink_set_mode_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 20)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_param_request_read_t { /// Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) @@ -3430,7 +3430,7 @@ public struct mavlink_param_request_read_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_param_request_list_t { /// System ID @@ -3441,7 +3441,7 @@ public struct mavlink_param_request_list_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 25)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_param_value_t { /// Onboard parameter value @@ -3459,7 +3459,7 @@ public struct mavlink_param_value_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 23)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_param_set_t { /// Onboard parameter value @@ -3477,7 +3477,7 @@ public struct mavlink_param_set_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 30)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gps_raw_int_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -3504,7 +3504,7 @@ public struct mavlink_gps_raw_int_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 101)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gps_status_t { /// Number of satellites visible @@ -3528,7 +3528,7 @@ public struct mavlink_gps_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_scaled_imu_t { /// Timestamp (milliseconds since system boot) @@ -3555,7 +3555,7 @@ public struct mavlink_scaled_imu_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 26)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_raw_imu_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -3582,7 +3582,7 @@ public struct mavlink_raw_imu_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 16)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_raw_pressure_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -3599,7 +3599,7 @@ public struct mavlink_raw_pressure_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_scaled_pressure_t { /// Timestamp (milliseconds since system boot) @@ -3614,7 +3614,7 @@ public struct mavlink_scaled_pressure_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_attitude_t { /// Timestamp (milliseconds since system boot) @@ -3635,7 +3635,7 @@ public struct mavlink_attitude_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_attitude_quaternion_t { /// Timestamp (milliseconds since system boot) @@ -3658,7 +3658,7 @@ public struct mavlink_attitude_quaternion_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_local_position_ned_t { /// Timestamp (milliseconds since system boot) @@ -3679,7 +3679,7 @@ public struct mavlink_local_position_ned_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_global_position_int_t { /// Timestamp (milliseconds since system boot) @@ -3704,7 +3704,7 @@ public struct mavlink_global_position_int_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_rc_channels_scaled_t { /// Timestamp (milliseconds since system boot) @@ -3733,7 +3733,7 @@ public struct mavlink_rc_channels_scaled_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_rc_channels_raw_t { /// Timestamp (milliseconds since system boot) @@ -3762,7 +3762,7 @@ public struct mavlink_rc_channels_raw_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 21)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_servo_output_raw_t { /// Timestamp (microseconds since system boot) @@ -3789,7 +3789,7 @@ public struct mavlink_servo_output_raw_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_request_partial_list_t { /// Start index, 0 by default @@ -3804,7 +3804,7 @@ public struct mavlink_mission_request_partial_list_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_write_partial_list_t { /// Start index, 0 by default and smaller / equal to the largest index of the current onboard list. @@ -3819,7 +3819,7 @@ public struct mavlink_mission_write_partial_list_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 37)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_item_t { /// PARAM1, see MAV_CMD enum @@ -3854,7 +3854,7 @@ public struct mavlink_mission_item_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_request_t { /// Sequence @@ -3867,7 +3867,7 @@ public struct mavlink_mission_request_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_set_current_t { /// Sequence @@ -3880,7 +3880,7 @@ public struct mavlink_mission_set_current_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_current_t { /// Sequence @@ -3889,7 +3889,7 @@ public struct mavlink_mission_current_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_request_list_t { /// System ID @@ -3900,7 +3900,7 @@ public struct mavlink_mission_request_list_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_count_t { /// Number of mission items in the sequence @@ -3913,7 +3913,7 @@ public struct mavlink_mission_count_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_clear_all_t { /// System ID @@ -3924,7 +3924,7 @@ public struct mavlink_mission_clear_all_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_item_reached_t { /// Sequence @@ -3933,7 +3933,7 @@ public struct mavlink_mission_item_reached_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_ack_t { /// System ID @@ -3946,7 +3946,7 @@ public struct mavlink_mission_ack_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 13)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_set_gps_global_origin_t { /// Latitude (WGS84), in degrees * 1E7 @@ -3961,7 +3961,7 @@ public struct mavlink_set_gps_global_origin_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gps_global_origin_t { /// Latitude (WGS84), in degrees * 1E7 @@ -3974,7 +3974,7 @@ public struct mavlink_gps_global_origin_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 37)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_param_map_rc_t { /// Initial parameter value @@ -4000,7 +4000,7 @@ public struct mavlink_param_map_rc_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_request_int_t { /// Sequence @@ -4013,7 +4013,7 @@ public struct mavlink_mission_request_int_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 27)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_safety_set_allowed_area_t { /// x position 1 / Latitude 1 @@ -4038,7 +4038,7 @@ public struct mavlink_safety_set_allowed_area_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 25)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_safety_allowed_area_t { /// x position 1 / Latitude 1 @@ -4059,7 +4059,7 @@ public struct mavlink_safety_allowed_area_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 68)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_attitude_quaternion_cov_t { /// Timestamp (milliseconds since system boot) @@ -4080,7 +4080,7 @@ public struct mavlink_attitude_quaternion_cov_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 26)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_nav_controller_output_t { /// Current desired roll in degrees @@ -4103,7 +4103,7 @@ public struct mavlink_nav_controller_output_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 185)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_global_position_int_cov_t { /// Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. @@ -4133,7 +4133,7 @@ public struct mavlink_global_position_int_cov_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 229)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_local_position_ned_cov_t { /// Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. @@ -4167,7 +4167,7 @@ public struct mavlink_local_position_ned_cov_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 42)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_rc_channels_t { /// Timestamp (milliseconds since system boot) @@ -4216,7 +4216,7 @@ public struct mavlink_rc_channels_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_request_data_stream_t { /// The requested message rate @@ -4233,7 +4233,7 @@ public struct mavlink_request_data_stream_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_data_stream_t { /// The message rate @@ -4246,7 +4246,7 @@ public struct mavlink_data_stream_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 11)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_manual_control_t { /// 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. @@ -4265,7 +4265,7 @@ public struct mavlink_manual_control_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_rc_channels_override_t { /// RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. @@ -4292,7 +4292,7 @@ public struct mavlink_rc_channels_override_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 37)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_mission_item_int_t { /// PARAM1, see MAV_CMD enum @@ -4327,7 +4327,7 @@ public struct mavlink_mission_item_int_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 20)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_vfr_hud_t { /// Current airspeed in m/s @@ -4346,7 +4346,7 @@ public struct mavlink_vfr_hud_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 35)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_command_int_t { /// PARAM1, see MAV_CMD enum @@ -4379,7 +4379,7 @@ public struct mavlink_command_int_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 33)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_command_long_t { /// Parameter 1, as defined by MAV_CMD enum. @@ -4408,7 +4408,7 @@ public struct mavlink_command_long_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_command_ack_t { /// Command ID, as defined by MAV_CMD enum. @@ -4419,7 +4419,7 @@ public struct mavlink_command_ack_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_manual_setpoint_t { /// Timestamp in milliseconds since system boot @@ -4440,7 +4440,7 @@ public struct mavlink_manual_setpoint_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 39)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_set_attitude_target_t { /// Timestamp in milliseconds since system boot @@ -4466,7 +4466,7 @@ public struct mavlink_set_attitude_target_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 37)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_attitude_target_t { /// Timestamp in milliseconds since system boot @@ -4489,7 +4489,7 @@ public struct mavlink_attitude_target_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 53)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_set_position_target_local_ned_t { /// Timestamp in milliseconds since system boot @@ -4528,7 +4528,7 @@ public struct mavlink_set_position_target_local_ned_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 51)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_position_target_local_ned_t { /// Timestamp in milliseconds since system boot @@ -4563,7 +4563,7 @@ public struct mavlink_position_target_local_ned_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 53)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_set_position_target_global_int_t { /// 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. @@ -4602,7 +4602,7 @@ public struct mavlink_set_position_target_global_int_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 51)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_position_target_global_int_t { /// 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. @@ -4637,7 +4637,7 @@ public struct mavlink_position_target_global_int_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_local_position_ned_system_global_offset_t { /// Timestamp (milliseconds since system boot) @@ -4658,7 +4658,7 @@ public struct mavlink_local_position_ned_system_global_offset_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 56)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hil_state_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -4697,7 +4697,7 @@ public struct mavlink_hil_state_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 42)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hil_controls_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -4727,7 +4727,7 @@ public struct mavlink_hil_controls_t - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 81)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hil_actuator_controls_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -4742,7 +4742,7 @@ public struct mavlink_hil_actuator_controls_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 33)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hil_rc_inputs_raw_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -4777,7 +4777,7 @@ public struct mavlink_hil_rc_inputs_raw_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 26)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_optical_flow_t { /// Timestamp (UNIX) @@ -4800,7 +4800,7 @@ public struct mavlink_optical_flow_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_global_vision_position_estimate_t { /// Timestamp (microseconds, synced to UNIX time or since system boot) @@ -4821,7 +4821,7 @@ public struct mavlink_global_vision_position_estimate_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_vision_position_estimate_t { /// Timestamp (microseconds, synced to UNIX time or since system boot) @@ -4842,7 +4842,7 @@ public struct mavlink_vision_position_estimate_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 20)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_vision_speed_estimate_t { /// Timestamp (microseconds, synced to UNIX time or since system boot) @@ -4857,7 +4857,7 @@ public struct mavlink_vision_speed_estimate_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_vicon_position_estimate_t { /// Timestamp (microseconds, synced to UNIX time or since system boot) @@ -4878,7 +4878,7 @@ public struct mavlink_vicon_position_estimate_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 62)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_highres_imu_t { /// Timestamp (microseconds, synced to UNIX time or since system boot) @@ -4915,7 +4915,7 @@ public struct mavlink_highres_imu_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 44)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_optical_flow_rad_t { /// Timestamp (microseconds, synced to UNIX time or since system boot) @@ -4946,7 +4946,7 @@ public struct mavlink_optical_flow_rad_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 64)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hil_sensor_t { /// Timestamp (microseconds, synced to UNIX time or since system boot) @@ -4983,7 +4983,7 @@ public struct mavlink_hil_sensor_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 84)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_sim_state_t { /// True attitude quaternion component 1, w (1 in null-rotation) @@ -5032,7 +5032,7 @@ public struct mavlink_sim_state_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 9)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_radio_status_t { /// Receive errors @@ -5053,7 +5053,7 @@ public struct mavlink_radio_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 254)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_file_transfer_protocol_t { /// Network ID (0 for broadcast) @@ -5069,7 +5069,7 @@ public struct mavlink_file_transfer_protocol_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 16)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_timesync_t { /// Time sync timestamp 1 @@ -5080,7 +5080,7 @@ public struct mavlink_timesync_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_camera_trigger_t { /// Timestamp for the image frame in microseconds @@ -5091,7 +5091,7 @@ public struct mavlink_camera_trigger_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hil_gps_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -5124,7 +5124,7 @@ public struct mavlink_hil_gps_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 44)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hil_optical_flow_t { /// Timestamp (microseconds, synced to UNIX time or since system boot) @@ -5155,7 +5155,7 @@ public struct mavlink_hil_optical_flow_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 64)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_hil_state_quaternion_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -5195,7 +5195,7 @@ public struct mavlink_hil_state_quaternion_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_scaled_imu2_t { /// Timestamp (milliseconds since system boot) @@ -5222,7 +5222,7 @@ public struct mavlink_scaled_imu2_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_log_request_list_t { /// First log id (0 for first available) @@ -5237,7 +5237,7 @@ public struct mavlink_log_request_list_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_log_entry_t { /// UTC timestamp of log in seconds since 1970, or 0 if not available @@ -5254,7 +5254,7 @@ public struct mavlink_log_entry_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_log_request_data_t { /// Offset into the log @@ -5271,7 +5271,7 @@ public struct mavlink_log_request_data_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 97)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_log_data_t { /// Offset into the log @@ -5287,7 +5287,7 @@ public struct mavlink_log_data_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_log_erase_t { /// System ID @@ -5298,7 +5298,7 @@ public struct mavlink_log_erase_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_log_request_end_t { /// System ID @@ -5309,7 +5309,7 @@ public struct mavlink_log_request_end_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 113)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gps_inject_data_t { /// System ID @@ -5325,7 +5325,7 @@ public struct mavlink_gps_inject_data_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 35)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gps2_raw_t { /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -5356,7 +5356,7 @@ public struct mavlink_gps2_raw_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_power_status_t { /// 5V rail voltage in millivolts @@ -5369,7 +5369,7 @@ public struct mavlink_power_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 79)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_serial_control_t { /// Baudrate of transfer. Zero means no change. @@ -5389,7 +5389,7 @@ public struct mavlink_serial_control_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 35)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gps_rtk_t { /// Time since boot of last baseline message received in ms. @@ -5422,7 +5422,7 @@ public struct mavlink_gps_rtk_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 35)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_gps2_rtk_t { /// Time since boot of last baseline message received in ms. @@ -5455,7 +5455,7 @@ public struct mavlink_gps2_rtk_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_scaled_imu3_t { /// Timestamp (milliseconds since system boot) @@ -5482,7 +5482,7 @@ public struct mavlink_scaled_imu3_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 13)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_data_transmission_handshake_t { /// total data size in bytes (set on ACK only) @@ -5503,7 +5503,7 @@ public struct mavlink_data_transmission_handshake_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 255)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_encapsulated_data_t { /// sequence number (starting with 0 on every transmission) @@ -5515,7 +5515,7 @@ public struct mavlink_encapsulated_data_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_distance_sensor_t { /// Time since system boot @@ -5538,7 +5538,7 @@ public struct mavlink_distance_sensor_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_terrain_request_t { /// Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) @@ -5553,7 +5553,7 @@ public struct mavlink_terrain_request_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 43)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_terrain_data_t { /// Latitude of SW corner of first grid (degrees *10^7) @@ -5571,7 +5571,7 @@ public struct mavlink_terrain_data_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_terrain_check_t { /// Latitude (degrees *10^7) @@ -5582,7 +5582,7 @@ public struct mavlink_terrain_check_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_terrain_report_t { /// Latitude (degrees *10^7) @@ -5603,7 +5603,7 @@ public struct mavlink_terrain_report_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_scaled_pressure2_t { /// Timestamp (milliseconds since system boot) @@ -5618,7 +5618,7 @@ public struct mavlink_scaled_pressure2_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_att_pos_mocap_t { /// Timestamp (micros since boot or Unix epoch) @@ -5636,7 +5636,7 @@ public struct mavlink_att_pos_mocap_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 43)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_set_actuator_control_target_t { /// Timestamp (micros since boot or Unix epoch) @@ -5654,7 +5654,7 @@ public struct mavlink_set_actuator_control_target_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 41)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_actuator_control_target_t { /// Timestamp (micros since boot or Unix epoch) @@ -5668,7 +5668,7 @@ public struct mavlink_actuator_control_target_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_altitude_t { /// Timestamp (milliseconds since system boot) @@ -5689,7 +5689,7 @@ public struct mavlink_altitude_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 243)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_resource_request_t { /// Request ID. This ID should be re-used when sending back URI contents @@ -5708,7 +5708,7 @@ public struct mavlink_resource_request_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_scaled_pressure3_t { /// Timestamp (milliseconds since system boot) @@ -5723,7 +5723,7 @@ public struct mavlink_scaled_pressure3_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 100)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_control_system_state_t { /// Timestamp (micros since boot or Unix epoch) @@ -5767,7 +5767,7 @@ public struct mavlink_control_system_state_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_battery_status_t { /// Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate @@ -5793,7 +5793,7 @@ public struct mavlink_battery_status_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 60)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_autopilot_version_t { /// bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) @@ -5825,7 +5825,7 @@ public struct mavlink_autopilot_version_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 30)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_landing_target_t { /// Timestamp (micros since boot or Unix epoch) @@ -5848,7 +5848,7 @@ public struct mavlink_landing_target_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_vibration_t { /// Timestamp (micros since boot or Unix epoch) @@ -5869,7 +5869,7 @@ public struct mavlink_vibration_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 52)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_home_position_t { /// Latitude (WGS84), in degrees * 1E7 @@ -5898,7 +5898,7 @@ public struct mavlink_home_position_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 53)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_set_home_position_t { /// Latitude (WGS84), in degrees * 1E7 @@ -5929,7 +5929,7 @@ public struct mavlink_set_home_position_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_message_interval_t { /// 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. @@ -5940,7 +5940,7 @@ public struct mavlink_message_interval_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_extended_sys_state_t { /// The VTOL state if applicable. Is set to MAV_VTOL_STATE_UNDEFINED if UAV is not in VTOL configuration. @@ -5951,7 +5951,7 @@ public struct mavlink_extended_sys_state_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 38)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_adsb_vehicle_t { /// ICAO address @@ -6006,7 +6006,7 @@ public struct mavlink_collision_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 254)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_v2_extension_t { /// 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. @@ -6024,7 +6024,7 @@ public struct mavlink_v2_extension_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_memory_vect_t { /// Starting address of the debug variables @@ -6040,7 +6040,7 @@ public struct mavlink_memory_vect_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 30)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_debug_vect_t { /// Timestamp @@ -6058,7 +6058,7 @@ public struct mavlink_debug_vect_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_named_value_float_t { /// Timestamp (milliseconds since system boot) @@ -6072,7 +6072,7 @@ public struct mavlink_named_value_float_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_named_value_int_t { /// Timestamp (milliseconds since system boot) @@ -6086,7 +6086,7 @@ public struct mavlink_named_value_int_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 51)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_statustext_t { /// Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. @@ -6098,7 +6098,7 @@ public struct mavlink_statustext_t }; - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 9)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_debug_t { /// Timestamp (milliseconds since system boot) @@ -6111,7 +6111,7 @@ public struct mavlink_debug_t }; // custom message from the simulator - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12*4)] + [StructLayout(LayoutKind.Sequential)] public struct mavlink_telemetry { public const int MessageId = 204; diff --git a/MavLinkCom/MavLinkCom.vcxproj b/MavLinkCom/MavLinkCom.vcxproj index a1006ce469..58303aa002 100644 --- a/MavLinkCom/MavLinkCom.vcxproj +++ b/MavLinkCom/MavLinkCom.vcxproj @@ -546,6 +546,7 @@ + diff --git a/MavLinkCom/MavLinkCom.vcxproj.filters b/MavLinkCom/MavLinkCom.vcxproj.filters index 0cc6ffdbac..fcaa6c57de 100644 --- a/MavLinkCom/MavLinkCom.vcxproj.filters +++ b/MavLinkCom/MavLinkCom.vcxproj.filters @@ -218,6 +218,7 @@ + diff --git a/MavLinkCom/common_utils/Utils.hpp b/MavLinkCom/common_utils/Utils.hpp index 66f27b6d3d..40146937d5 100644 --- a/MavLinkCom/common_utils/Utils.hpp +++ b/MavLinkCom/common_utils/Utils.hpp @@ -22,6 +22,7 @@ #include #include #include "type_utils.hpp" +#include "MavLinkDebugLog.hpp" #ifndef _WIN32 #include // needed for CHAR_BIT used below @@ -85,7 +86,8 @@ void unused(T const & result) { static_cast(result); } namespace mavlink_utils { -class Utils { +class Utils +{ private: typedef std::chrono::system_clock system_clock; typedef std::chrono::steady_clock steady_clock; @@ -98,18 +100,8 @@ class Utils { public: - class Logger { - public: - virtual void log(int level, const std::string& message) - { - if (level >= 0) - std::cout << message; - else - std::cerr << message; - } - }; - - static void enableImmediateConsoleFlush() { + static void enableImmediateConsoleFlush() + { //disable buffering setbuf(stdout, NULL); } @@ -123,30 +115,21 @@ class Utils { return gaussian_dist(random_gen) * stddev + mean; } - static constexpr double degreesToRadians(double degrees) { + static constexpr double degreesToRadians(double degrees) + { return static_cast(M_PIl * degrees / 180.0); } - static constexpr float degreesToRadians(float degrees) { + static constexpr float degreesToRadians(float degrees) + { return static_cast(M_PIf * degrees / 180.0f); } - static constexpr double radiansToDegrees(double radians) { + static constexpr double radiansToDegrees(double radians) + { return static_cast(radians * 180.0 / M_PIl); } - static constexpr float radiansToDegrees(float radians) { - return static_cast(radians * 180.0f / M_PIf); - } - - static Logger* getSetLogger(Logger* logger = nullptr) + static constexpr float radiansToDegrees(float radians) { - static Logger logger_default_; - static Logger* logger_; - - if (logger != nullptr) - logger_ = logger; - else if (logger_ == nullptr) - logger_ = &logger_default_; - - return logger_; + return static_cast(radians * 180.0f / M_PIf); } static constexpr int kLogLevelInfo = 0; @@ -154,17 +137,19 @@ class Utils { static constexpr int kLogLevelError = -2; static void log(std::string message, int level = kLogLevelInfo) { - getSetLogger()->log(level, message); + mavlinkcom::MavLinkDebugLog::getSetLogger()->log(level, message); } template - static int sign(T val) { + static int sign(T val) + { return T(0) < val ? 1 : (T(0) > val ? -1 : 0); } /// Limits absolute value whole preserving sign template - static T limitAbsValue(T val, T min_value, T max_value) { + static T limitAbsValue(T val, T min_value, T max_value) + { T val_abs = std::abs(val); T val_limited = std::max(val_abs, min_value); val_limited = std::min(val_limited, max_value); @@ -173,7 +158,8 @@ class Utils { /// Limits absolute value whole preserving sign template - static T clip(T val, T min_value, T max_value) { + static T clip(T val, T min_value, T max_value) + { return std::max(min_value, std::min(val, max_value)); } @@ -190,8 +176,7 @@ class Utils { stringstream ss; ss << prefix; - for (Iterator i = start; i != last; ++i) - { + for (Iterator i = start; i != last; ++i) { if (i != start) ss << delim; ss << *i; @@ -206,8 +191,7 @@ class Utils { int len = static_cast(str.size()); const char* ptr = str.c_str(); int i = 0; - for (i = len - 1; i >= 0; i--) - { + for (i = len - 1; i >= 0; i--) { if (ptr[i] == '.') break; } @@ -246,14 +230,12 @@ class Utils { int len = static_cast(str.size()); const char* ptr = str.c_str(); int i = 0; - for (i = 0; i < len; i++) - { + for (i = 0; i < len; i++) { if (ptr[i] != ch) break; } int j = 0; - for (j = len - 1; j >= i; j--) - { + for (j = len - 1; j >= i; j--) { if (ptr[j] != ch) break; } @@ -264,29 +246,24 @@ class Utils { { auto start = s.begin(); std::vector result; - for (auto it = s.begin(); it != s.end(); it++) - { + for (auto it = s.begin(); it != s.end(); it++) { char ch = *it; bool split = false; - for (int i = 0; i < numSplitChars; i++) - { + for (int i = 0; i < numSplitChars; i++) { if (ch == splitChars[i]) { split = true; break; } } - if (split) - { - if (start < it) - { + if (split) { + if (start < it) { result.push_back(string(start, it)); } start = it; start++; } } - if (start < s.end()) - { + if (start < s.end()) { result.push_back(string(start, s.end())); } return result; @@ -300,14 +277,12 @@ class Utils { auto start = line.begin(); std::vector result; auto end = line.end(); - for (auto it = line.begin(); it != end; ) - { + for (auto it = line.begin(); it != end; ) { bool split = false; char ch = *it; if (ch == '\'' || ch == '"') { // skip quoted literal - if (start < it) - { + if (start < it) { result.push_back(string(start, it)); } it++; @@ -326,10 +301,8 @@ class Utils { } } } - if (split) - { - if (start < it) - { + if (split) { + if (start < it) { result.push_back(string(start, it)); } start = it; @@ -339,8 +312,7 @@ class Utils { it++; } } - if (start < end) - { + if (start < end) { result.push_back(string(start, end)); } return result; @@ -356,8 +328,7 @@ class Utils { _strlwr_s(buf.get(), len + 1U); #else char* p = buf.get(); - for (int i = len; i > 0; i--) - { + for (int i = len; i > 0; i--) { *p = tolower(*p); p++; } @@ -391,15 +362,18 @@ class Utils { } template - static constexpr T nan() { + static constexpr T nan() + { return std::numeric_limits::quiet_NaN(); } template - static constexpr T max() { + static constexpr T max() + { return std::numeric_limits::max(); } template - static constexpr T min() { + static constexpr T min() + { return std::numeric_limits::min(); } diff --git a/MavLinkCom/include/AsyncResult.hpp b/MavLinkCom/include/AsyncResult.hpp index 0bb6df72e6..f778c5556c 100644 --- a/MavLinkCom/include/AsyncResult.hpp +++ b/MavLinkCom/include/AsyncResult.hpp @@ -13,140 +13,166 @@ namespace mavlinkcom { - template - class AsyncResultState - { - public: - typedef std::function CompletionHandler; - mavlink_utils::Semaphore resultReceived_; - T result_; - CompletionHandler owner_; - int state_ = 0; - bool completed_ = false; - - AsyncResultState(CompletionHandler owner) { - owner_ = owner; - } +template +class AsyncResultState +{ +public: + typedef std::function CompletionHandler; + mavlink_utils::Semaphore resultReceived_; + T result_; + CompletionHandler owner_; + int state_ = 0; + bool completed_ = false; + + AsyncResultState(CompletionHandler owner) + { + owner_ = owner; + } - ~AsyncResultState() { - complete(); - } - int getState() { - return state_; - } - void setState(int s) { - state_ = s; - } - T getResult() { - resultReceived_.wait(); - return result_; - } - void setResult(T result) { - result_ = result; - resultReceived_.post(); + ~AsyncResultState() + { + complete(); + } + + int getState() + { + return state_; + } + void setState(int s) + { + state_ = s; + } + T getResult() + { + resultReceived_.wait(); + return result_; + } + void setResult(T result) + { + result_ = result; + resultReceived_.post(); + complete(); + } + bool getResult(int millisecondTimeout, T* r) + { + if (!resultReceived_.timed_wait(millisecondTimeout)) { + // timeout on wait, so complete the task in this case too. complete(); + return false; } - bool getResult(int millisecondTimeout, T* r) { - if (!resultReceived_.timed_wait(millisecondTimeout)) { - // timeout on wait, so complete the task in this case too. - complete(); - return false; - } - *r = result_; - return true; - } - void complete() { - CompletionHandler rh = owner_; - owner_ = nullptr; - - completed_ = true; - if (rh != nullptr) { - rh(state_); - } + *r = result_; + return true; + } + void complete() + { + CompletionHandler rh = owner_; + owner_ = nullptr; + + completed_ = true; + if (rh != nullptr) { + rh(state_); } - bool isCompleted() { return completed_; } - }; + } + bool isCompleted() { return completed_; } +}; - template - class AsyncResult +template +class AsyncResult +{ +public: + typedef std::function ResultHandler; + typedef std::function CompletionHandler; + + AsyncResult(CompletionHandler owner) + { + state_ = std::make_shared>(owner); + } + ~AsyncResult() { - public: - typedef std::function ResultHandler; - typedef std::function CompletionHandler; + state_ = nullptr; + } - AsyncResult(CompletionHandler owner) - { - state_ = std::make_shared>(owner); - } - ~AsyncResult() - { - state_ = nullptr; - } + static AsyncResult Completed(T state) + { + AsyncResult r(nullptr); + r.setResult(state); + return r; + } - void then(ResultHandler handler) - { - // keep state alive while we wait for async result. - std::shared_ptr> safe(state_); - T result = std::async(std::launch::async, getResult, safe).get(); - handler(result); - } + void then(ResultHandler handler) + { + // keep state alive while we wait for async result. + std::shared_ptr> safe(state_); + T result = std::async(std::launch::async, getResult, safe).get(); + handler(result); + } - int getState() { - return state_->getState(); - } + int getState() + { + return state_->getState(); + } - void setState(int s) const { - state_->setState(s); - } - void setResult(T result) const { - if (state_ != nullptr) { - state_->setResult(result); - } + void setState(int s) const + { + state_->setState(s); + } + void setResult(T result) const + { + if (state_ != nullptr) { + state_->setResult(result); } + } + + AsyncResult(const AsyncResult& other) + { + this->state_ = other.state_; + } - AsyncResult(const AsyncResult& other) { + AsyncResult& operator=(const AsyncResult& other) + { + if (this != &other) { this->state_ = other.state_; } + return *this; + } - AsyncResult& operator=(const AsyncResult& other) { - if (this != &other) { - this->state_ = other.state_; - } - return *this; - } + AsyncResult(AsyncResult&& other) + { + this->state_ = other.state_; + other.state_ = nullptr; + } - AsyncResult(AsyncResult&& other) { + AsyncResult& operator=(AsyncResult&& other) + { + if (this != &other) { this->state_ = other.state_; other.state_ = nullptr; } + return *this; + } - AsyncResult& operator=(AsyncResult&& other) { - if (this != &other) { - this->state_ = other.state_; - other.state_ = nullptr; - } - return *this; - } - - bool wait(int millisecondTimeout, T* r) { - // keep the state alive while we wait. - std::shared_ptr> safe(state_); - return state_->getResult(millisecondTimeout, r); - } + bool wait(int millisecondTimeout, T* r) + { + // keep the state alive while we wait. + std::shared_ptr> safe(state_); + return state_->getResult(millisecondTimeout, r); + } - bool isCompleted() { - return state_->isCompleted(); - } + bool isCompleted() + { + return state_->isCompleted(); + } - private: - static T getResult(std::shared_ptr> state) { - return state->getResult(); - } +private: + static T getResult(std::shared_ptr> state) + { + return state->getResult(); + } - std::shared_ptr> state_; - }; + std::shared_ptr> state_; +}; } #endif diff --git a/MavLinkCom/include/MavLinkConnection.hpp b/MavLinkCom/include/MavLinkConnection.hpp index 6dc7cd3e07..3760072db0 100644 --- a/MavLinkCom/include/MavLinkConnection.hpp +++ b/MavLinkCom/include/MavLinkConnection.hpp @@ -21,137 +21,142 @@ class Port; namespace mavlinkcom_impl { - class MavLinkConnectionImpl; - class MavLinkTcpServerImpl; - class MavLinkNodeImpl; +class MavLinkConnectionImpl; +class MavLinkTcpServerImpl; +class MavLinkNodeImpl; } namespace mavlinkcom { - class MavLinkConnection; - class MavLinkNode; - - // This callback is invoked when a MavLink message is read from the connection. - typedef std::function connection, const MavLinkMessage& msg)> MessageHandler; - - // This callback is invoked when a new TCP connection is accepted via acceptTcp(). - typedef std::function port)> MavLinkConnectionHandler; - - struct SerialPortInfo { - std::wstring displayName; - std::wstring portName; - int vid; - int pid; - }; - - // This class represents a single connection to a remote mavlink node connected either over UDP, TCP or Serial port. - // You can use this connection in a MavLinkNode to send a message directly to that node, and start listening to messages - // from that remote node. You can handle those messages directly using subscribe. - class MavLinkConnection : public std::enable_shared_from_this - { - public: - MavLinkConnection(); - - // Find available serial ports for the given vendor id/product id pair. If a matching port is found it returns the - // SerialPortInfo of that port, the portName can then be used connectSerial. Pass 0 for vid and pid to find all - // available serial ports. - static std::vector findSerialPorts(int vid, int pid); - - // create connection over serial port (e.g. /dev/ttyACM0 or on windows "com5"). - // pass initial string to write to the port, which can be used to configure the port. - // For example, on PX4 you can send "sh /etc/init.d/rc.usb\n" to turn on lots of mavlink streams. - static std::shared_ptr connectSerial(const std::string& nodeName, const std::string& portName, int baudrate = 115200, const std::string& initString = ""); - - // Start listening on a specific local port for packets from any remote computer. Once a packet is received - // it will remember the remote address of the sender so that subsequend sendMessage calls will go back to that sender. - // This is useful if the remote sender already knows which local port you plan to listen on. - // The localAddr can also a specific local ip address if you need to specify which - // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets - // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet - // adapter rather than 127.0.0.1. - static std::shared_ptr connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort); - - // Connect to a specific remote machine that is already listening on a specific port for messages from any computer. - // This will use any free local port that is available. - // The localAddr can also a specific local ip address if you need to specify which - // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets - // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet - // adapter rather than 127.0.0.1. - static std::shared_ptr connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort); - - // This method sets up a tcp connection to the specified remote host and port. The remote host - // must already be listening and accepting TCP socket connections for this to succeed. - // 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. - 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(); - int getTargetSystemId(); - bool isOpen(); - void close(); - - // provide a callback function that will be called for every message "received" from the remote mavlink node. - int subscribe(MessageHandler handler); - void unsubscribe(int id); - - // log every message that is "sent" using sendMessage. - void startLoggingSendMessage(std::shared_ptr log); - void stopLoggingSendMessage(); - - uint8_t getNextSequence(); - - // Advanced method that create a bridge between two connections. For example, if you use connectRemoteUdp to connect to - // QGroundControl port 14550, and connectSerial to connect to PX4, then you can call this method to join the two so that - // all messages from PX4 are sent to QGroundControl and vice versa. - void join(std::shared_ptr remote, bool subscribeToLeft = true, bool subscribeToRight = true); - - // Pack and send the given message, assuming the compid and sysid have been set by the caller. - void sendMessage(const MavLinkMessageBase& msg); - - // Send the given already encoded message, assuming the compid and sysid have been set by the caller. - void sendMessage(const MavLinkMessage& msg); - - // get the next telemetry snapshot, then clear the internal counters and start over. This way each snapshot - // gives you a picture of what happened in whatever timeslice you decide to call this method. This is packaged - // in a mavlink message so you can easily send it to the LogViewer. - void getTelemetry(MavLinkTelemetry& result); - - //add the message in to list of ignored messages. These messages will not be sent in the sendMessage() call. - //this does not effect reception of message, however. This is typically useful in scenario where many connections - //are bridged and you don't want certain connection to read ceratin messages. - void ignoreMessage(uint8_t message_id); - - // Compute crc checksums, and pack according to mavlink1 or mavlink2 (depending on what target node supports) and do optional - // 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); - - public: - //needed for piml pattern - ~MavLinkConnection(); - //MavLinkConnection(MavLinkConnection&&); - //MavLinkConnection& operator=(MavLinkConnection&&); - private: - std::unique_ptr pImpl; - friend class MavLinkNode; - friend class mavlinkcom_impl::MavLinkNodeImpl; - friend class mavlinkcom_impl::MavLinkConnectionImpl; - friend class mavlinkcom_impl::MavLinkTcpServerImpl; - }; +class MavLinkConnection; +class MavLinkNode; + +// This callback is invoked when a MavLink message is read from the connection. +typedef std::function connection, const MavLinkMessage& msg)> MessageHandler; + +// This callback is invoked when a new TCP connection is accepted via acceptTcp(). +typedef std::function port)> MavLinkConnectionHandler; + +struct SerialPortInfo { + std::wstring displayName; + std::wstring portName; + int vid; + int pid; +}; + +// This class represents a single connection to a remote mavlink node connected either over UDP, TCP or Serial port. +// You can use this connection in a MavLinkNode to send a message directly to that node, and start listening to messages +// from that remote node. You can handle those messages directly using subscribe. +class MavLinkConnection : public std::enable_shared_from_this +{ +public: + MavLinkConnection(); + + // Find available serial ports for the given vendor id/product id pair. If a matching port is found it returns the + // SerialPortInfo of that port, the portName can then be used connectSerial. Pass 0 for vid and pid to find all + // available serial ports. + static std::vector findSerialPorts(int vid, int pid); + + // create connection over serial port (e.g. /dev/ttyACM0 or on windows "com5"). + // pass initial string to write to the port, which can be used to configure the port. + // For example, on PX4 you can send "sh /etc/init.d/rc.usb\n" to turn on lots of mavlink streams. + static std::shared_ptr connectSerial(const std::string& nodeName, const std::string& portName, int baudrate = 115200, const std::string& initString = ""); + + // Start listening on a specific local port for packets from any remote computer. Once a packet is received + // it will remember the remote address of the sender so that subsequend sendMessage calls will go back to that sender. + // This is useful if the remote sender already knows which local port you plan to listen on. + // The localAddr can also a specific local ip address if you need to specify which + // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets + // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet + // adapter rather than 127.0.0.1. + static std::shared_ptr connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort); + + // Connect to a specific remote machine that is already listening on a specific port for messages from any computer. + // This will use any free local port that is available. + // The localAddr can also a specific local ip address if you need to specify which + // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets + // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet + // adapter rather than 127.0.0.1. + static std::shared_ptr connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort); + + // This method sets up a tcp connection to the specified remote host and port. The remote host + // must already be listening and accepting TCP socket connections for this to succeed. + // 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. + 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. + // It returns the address of the remote machine that connected. + std::string acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort); + + // instance methods + std::string getName(); + int getTargetComponentId(); + int getTargetSystemId(); + bool isOpen(); + void close(); + + // provide a callback function that will be called for every message "received" from the remote mavlink node. + int subscribe(MessageHandler handler); + void unsubscribe(int id); + + // log every message that is "sent" using sendMessage. + void startLoggingSendMessage(std::shared_ptr log); + void stopLoggingSendMessage(); + + // log every message that is "received" by a subscriber. + void startLoggingReceiveMessage(std::shared_ptr log); + void stopLoggingReceiveMessage(); + + uint8_t getNextSequence(); + + // Advanced method that create a bridge between two connections. For example, if you use connectRemoteUdp to connect to + // QGroundControl port 14550, and connectSerial to connect to PX4, then you can call this method to join the two so that + // all messages from PX4 are sent to QGroundControl and vice versa. + void join(std::shared_ptr remote, bool subscribeToLeft = true, bool subscribeToRight = true); + + // Pack and send the given message, assuming the compid and sysid have been set by the caller. + void sendMessage(const MavLinkMessageBase& msg); + + // Send the given already encoded message, assuming the compid and sysid have been set by the caller. + void sendMessage(const MavLinkMessage& msg); + + // get the next telemetry snapshot, then clear the internal counters and start over. This way each snapshot + // gives you a picture of what happened in whatever timeslice you decide to call this method. This is packaged + // in a mavlink message so you can easily send it to the LogViewer. + void getTelemetry(MavLinkTelemetry& result); + + //add the message in to list of ignored messages. These messages will not be sent in the sendMessage() call. + //this does not effect reception of message, however. This is typically useful in scenario where many connections + //are bridged and you don't want certain connection to read ceratin messages. + void ignoreMessage(uint8_t message_id); + + // Compute crc checksums, and pack according to mavlink1 or mavlink2 (depending on what target node supports) and do optional + // 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); + +public: + //needed for piml pattern + ~MavLinkConnection(); + //MavLinkConnection(MavLinkConnection&&); + //MavLinkConnection& operator=(MavLinkConnection&&); +private: + std::unique_ptr pImpl; + friend class MavLinkNode; + friend class mavlinkcom_impl::MavLinkNodeImpl; + friend class mavlinkcom_impl::MavLinkConnectionImpl; + friend class mavlinkcom_impl::MavLinkTcpServerImpl; +}; } #endif diff --git a/MavLinkCom/include/MavLinkDebugLog.hpp b/MavLinkCom/include/MavLinkDebugLog.hpp new file mode 100644 index 0000000000..14e2cceb7b --- /dev/null +++ b/MavLinkCom/include/MavLinkDebugLog.hpp @@ -0,0 +1,38 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef mavlink_Logger_hpp +#define mavlink_Logger_hpp + +#include +#include + +namespace mavlinkcom +{ +class MavLinkDebugLog +{ +public: + virtual void log(int level, const std::string& message) + { + if (level >= 0) + std::cout << message; + else + std::cerr << message; + } + + static MavLinkDebugLog* getSetLogger(MavLinkDebugLog* logger = nullptr) + { + static MavLinkDebugLog logger_default_; + static MavLinkDebugLog* logger_; + + if (logger != nullptr) + logger_ = logger; + else if (logger_ == nullptr) + logger_ = &logger_default_; + + return logger_; + } +}; +} + +#endif \ No newline at end of file diff --git a/MavLinkCom/include/MavLinkLog.hpp b/MavLinkCom/include/MavLinkLog.hpp index 63a51f54b2..922dfeaa47 100644 --- a/MavLinkCom/include/MavLinkLog.hpp +++ b/MavLinkCom/include/MavLinkLog.hpp @@ -7,37 +7,41 @@ #include #include #include +#include #include "MavLinkMessageBase.hpp" +#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol + namespace mavlinkcom { - // This abstract class defines the interface for logging MavLinkMessages. - class MavLinkLog - { - public: - virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) = 0; - virtual ~MavLinkLog() = default; - }; - - // This implementation of MavLinkLog reads/writes MavLinkMessages to a local file. - class MavLinkFileLog : public MavLinkLog - { - std::string file_name_; - FILE* ptr_; - bool reading_; - bool writing_; - bool json_; - public: - MavLinkFileLog(); - virtual ~MavLinkFileLog(); - bool isOpen(); - void openForReading(const std::string& filename); - void openForWriting(const std::string& filename, bool json = false); - void close(); - virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override; - bool read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp); - static uint64_t getTimeStamp(); - }; +// This abstract class defines the interface for logging MavLinkMessages. +class MavLinkLog +{ +public: + virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) = 0; + virtual ~MavLinkLog() = default; +}; + +// This implementation of MavLinkLog reads/writes MavLinkMessages to a local file. +class MavLinkFileLog : public MavLinkLog +{ + std::string file_name_; + FILE* ptr_; + bool reading_; + bool writing_; + bool json_; + std::mutex log_lock_; +public: + MavLinkFileLog(); + virtual ~MavLinkFileLog(); + bool isOpen(); + void openForReading(const std::string& filename); + void openForWriting(const std::string& filename, bool json = false); + void close(); + virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override; + bool read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp); + static uint64_t getTimeStamp(); +}; } diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp index e17c09aef5..071228fbd7 100644 --- a/MavLinkCom/include/MavLinkMessageBase.hpp +++ b/MavLinkCom/include/MavLinkMessageBase.hpp @@ -14,151 +14,170 @@ namespace mavlinkcom_impl { } namespace mavlinkcom { - class MavLinkConnection; +class MavLinkConnection; #define PayloadSize ((255 + 2 + 7) / 8) - // This is the raw undecoded message, use the msgid field to figure out which strongly typed - // MavLinkMessageBase subclass can be used to decode this message. For example, a heartbeat: - // MavLinkMessage msg; - // if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { - // MavLinkHeartbeat heartbeat; - // heartbeat.decode(msg); - // ... - // } - class MavLinkMessage { - public: - 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[PayloadSize]; - uint8_t ck[2]; ///< incoming checksum bytes - uint8_t signature[13]; - uint8_t protocol_version; - }; - - // This is the base class for all the strongly typed messages define in MavLinkMessages.hpp - class MavLinkMessageBase - { - public: - uint32_t msgid = 0; - uint8_t sysid = 0; ///< ID of message sender system/aircraft - uint8_t compid = 0; ///< ID of the message sender component - uint64_t timestamp = 0; - uint8_t protocol_version = 0; - - // unpack the given message - void decode(const MavLinkMessage& msg); - // pack this message into given message buffer - void encode(MavLinkMessage& msg) const; - - // find what type of message this is and decode it on the heap (call delete when you are done with it). - static MavLinkMessageBase* lookup(const MavLinkMessage& msg); - virtual std::string toJSon() = 0; - virtual ~MavLinkMessageBase() {} - protected: - virtual int pack(char* buffer) const = 0; - virtual int unpack(const char* buffer) = 0; - - void pack_uint8_t(char* buffer, const uint8_t* field, int offset) const; - void pack_int8_t(char* buffer, const int8_t* field, int offset) const; - void pack_int16_t(char* buffer, const int16_t* field, int offset) const; - void pack_uint16_t(char* buffer, const uint16_t* field, int offset) const; - void pack_uint32_t(char* buffer, const uint32_t* field, int offset) const; - void pack_int32_t(char* buffer, const int32_t* field, int offset) const; - void pack_uint64_t(char* buffer, const uint64_t* field, int offset) const; - void pack_int64_t(char* buffer, const int64_t* field, int offset) const; - void pack_float(char* buffer, const float* field, int offset) const; - void pack_char_array(int len, char* buffer, const char* field, int offset) const; - void pack_uint8_t_array(int len, char* buffer, const uint8_t* field, int offset) const; - void pack_int8_t_array(int len, char* buffer, const int8_t* field, int offset) const; - void pack_uint16_t_array(int len, char* buffer, const uint16_t* field, int offset) const; - void pack_int16_t_array(int len, char* buffer, const int16_t* field, int offset) const; - void pack_float_array(int len, char* buffer, const float* field, int offset) const; +// This is the raw undecoded message, use the msgid field to figure out which strongly typed +// MavLinkMessageBase subclass can be used to decode this message. For example, a heartbeat: +// MavLinkMessage msg; +// if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { +// MavLinkHeartbeat heartbeat; +// heartbeat.decode(msg); +// ... +// } +class MavLinkMessage +{ +public: + 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[PayloadSize]; + uint8_t ck[2]; ///< incoming checksum bytes + uint8_t signature[13]; + uint8_t protocol_version; + + // initialize checksum + int update_checksum(); +}; + +// This is the base class for all the strongly typed messages define in MavLinkMessages.hpp +class MavLinkMessageBase +{ +public: + uint32_t msgid = 0; + uint8_t sysid = 0; ///< ID of message sender system/aircraft + uint8_t compid = 0; ///< ID of the message sender component + uint64_t timestamp = 0; + uint8_t protocol_version = 0; + + // unpack the given message + void decode(const MavLinkMessage& msg); + // pack this message into given message buffer + void encode(MavLinkMessage& msg) const; + + // find what type of message this is and decode it on the heap (call delete when you are done with it). + static MavLinkMessageBase* lookup(const MavLinkMessage& msg); + virtual std::string toJSon() = 0; + virtual ~MavLinkMessageBase() {} +protected: + virtual int pack(char* buffer) const = 0; + virtual int unpack(const char* buffer) = 0; + + void pack_uint8_t(char* buffer, const uint8_t* field, int offset) const; + void pack_int8_t(char* buffer, const int8_t* field, int offset) const; + void pack_int16_t(char* buffer, const int16_t* field, int offset) const; + void pack_uint16_t(char* buffer, const uint16_t* field, int offset) const; + void pack_uint32_t(char* buffer, const uint32_t* field, int offset) const; + void pack_int32_t(char* buffer, const int32_t* field, int offset) const; + void pack_uint64_t(char* buffer, const uint64_t* field, int offset) const; + void pack_int64_t(char* buffer, const int64_t* field, int offset) const; + void pack_float(char* buffer, const float* field, int offset) const; + void pack_char_array(int len, char* buffer, const char* field, int offset) const; + void pack_uint8_t_array(int len, char* buffer, const uint8_t* field, int offset) const; + void pack_int8_t_array(int len, char* buffer, const int8_t* field, int offset) const; + void pack_uint16_t_array(int len, char* buffer, const uint16_t* field, int offset) const; + void pack_int16_t_array(int len, char* buffer, const int16_t* field, int offset) const; + void pack_float_array(int len, char* buffer, const float* field, int offset) const; - void unpack_uint8_t(const char* buffer, uint8_t* field, int offset); - void unpack_int8_t(const char* buffer, int8_t* field, int offset); - void unpack_int16_t(const char* buffer, int16_t* field, int offset); - void unpack_uint16_t(const char* buffer, uint16_t* field, int offset); - void unpack_uint32_t(const char* buffer, uint32_t* field, int offset); - void unpack_int32_t(const char* buffer, int32_t* field, int offset); - void unpack_uint64_t(const char* buffer, uint64_t* field, int offset); - void unpack_int64_t(const char* buffer, int64_t* field, int offset); - void unpack_float(const char* buffer, float* field, int offset); - void unpack_char_array(int len, const char* buffer, char* field, int offset); - void unpack_uint8_t_array(int len, const char* buffer, uint8_t* field, int offset); - void unpack_int8_t_array(int len, const char* buffer, int8_t* field, int offset); - void unpack_uint16_t_array(int len, const char* buffer, uint16_t* field, int offset); - void unpack_int16_t_array(int len, const char* buffer, int16_t* field, int offset); - void unpack_float_array(int len, const char* buffer, float* field, int offset); - - std::string char_array_tostring(int len, const char* field); - std::string uint8_t_array_tostring(int len, const uint8_t* field); - std::string int8_t_array_tostring(int len, const int8_t* field); - std::string int16_t_array_tostring(int len, const int16_t* field); - std::string uint16_t_array_tostring(int len, const uint16_t* field); - std::string float_array_tostring(int len, const float* field); - std::string float_tostring(float value); - }; - - // Base class for all strongly typed MavLinkCommand classes defined in MavLinkMessages.hpp - class MavLinkCommand { - public: - uint16_t command = 0; - protected: - virtual void pack() = 0; - virtual void unpack() = 0; - - float param1 = 0; - float param2 = 0; - float param3 = 0; - float param4 = 0; - float param5 = 0; - float param6 = 0; - float param7 = 0; - - friend class mavlinkcom_impl::MavLinkNodeImpl; - friend class mavlinkcom_impl::MavLinkConnectionImpl; - }; - - - // The location of a landing area captured from a downward facing camera - class MavLinkTelemetry : public MavLinkMessageBase { - public: - const static uint8_t kMessageId = 204; // in the user range 180-229. - MavLinkTelemetry() : wifiInterfaceName(nullptr) { msgid = kMessageId; } - uint32_t messagesSent; // number of messages sent since the last telemetry message - uint32_t messagesReceived; // number of messages received since the last telemetry message - uint32_t messagesHandled; // number of messages handled since the last telemetry message - uint32_t crcErrors; // # crc errors detected in mavlink stream since the last telemetry message - uint32_t handlerMicroseconds; // total time spent in the handlers in microseconds since the last telemetry message - uint32_t renderTime; // total time spent rendering frames since the last telemetry message - const char* wifiInterfaceName; // the name of the wifi interface we are measuring RSSI on. - int32_t wifiRssi; // if this device is communicating over wifi this is the signal strength. - virtual std::string toJSon() { - - std::ostringstream result; - result << "\"MavLinkTelemetry\"" << " : { "; - result << "\"messagesSent\":" << this->messagesSent << ","; - result << "\"messagesReceived\":" << this->messagesReceived << ","; - result << "\"messagesHandled\":" << this->messagesHandled << ","; - result << "\"crcErrors\":" << this->crcErrors << ","; - result << "\"handlerMicroseconds\":" << this->handlerMicroseconds << ","; - result << "\"renderTime\":" << this->renderTime; - result << "\"wifiRssi\":" << this->wifiRssi; - result << "}"; - return result.str(); - } - protected: - virtual int pack(char* buffer) const; - virtual int unpack(const char* buffer); - }; + void unpack_uint8_t(const char* buffer, uint8_t* field, int offset); + void unpack_int8_t(const char* buffer, int8_t* field, int offset); + void unpack_int16_t(const char* buffer, int16_t* field, int offset); + void unpack_uint16_t(const char* buffer, uint16_t* field, int offset); + void unpack_uint32_t(const char* buffer, uint32_t* field, int offset); + void unpack_int32_t(const char* buffer, int32_t* field, int offset); + void unpack_uint64_t(const char* buffer, uint64_t* field, int offset); + void unpack_int64_t(const char* buffer, int64_t* field, int offset); + void unpack_float(const char* buffer, float* field, int offset); + void unpack_char_array(int len, const char* buffer, char* field, int offset); + void unpack_uint8_t_array(int len, const char* buffer, uint8_t* field, int offset); + void unpack_int8_t_array(int len, const char* buffer, int8_t* field, int offset); + void unpack_uint16_t_array(int len, const char* buffer, uint16_t* field, int offset); + void unpack_int16_t_array(int len, const char* buffer, int16_t* field, int offset); + void unpack_float_array(int len, const char* buffer, float* field, int offset); + + std::string char_array_tostring(int len, const char* field); + std::string uint8_t_array_tostring(int len, const uint8_t* field); + std::string int8_t_array_tostring(int len, const int8_t* field); + std::string int16_t_array_tostring(int len, const int16_t* field); + std::string uint16_t_array_tostring(int len, const uint16_t* field); + std::string float_array_tostring(int len, const float* field); + std::string float_tostring(float value); +}; + +// Base class for all strongly typed MavLinkCommand classes defined in MavLinkMessages.hpp +class MavLinkCommand +{ +public: + uint16_t command = 0; +protected: + virtual void pack() = 0; + virtual void unpack() = 0; + + float param1 = 0; + float param2 = 0; + float param3 = 0; + float param4 = 0; + float param5 = 0; + float param6 = 0; + float param7 = 0; + + friend class mavlinkcom_impl::MavLinkNodeImpl; + friend class mavlinkcom_impl::MavLinkConnectionImpl; +}; + + +// The location of a landing area captured from a downward facing camera +class MavLinkTelemetry : public MavLinkMessageBase +{ +public: + const static uint8_t kMessageId = 204; // in the user range 180-229. + const static int MessageLength = 12 * 4; + MavLinkTelemetry() { msgid = kMessageId; } + uint32_t messages_sent = 0; // number of messages sent since the last telemetry message + uint32_t messages_received = 0; // number of messages received since the last telemetry message + uint32_t messages_handled = 0; // number of messages handled since the last telemetry message + uint32_t crc_errors = 0; // # crc errors detected in mavlink stream since the last telemetry message + uint32_t handler_microseconds = 0; // total time spent in the handlers in microseconds since the last telemetry message + uint32_t render_time = 0; // total time spent rendering frames since the last telemetry message + int32_t wifi_rssi = 0; // if this device is communicating over wifi this is the signal strength. + uint32_t update_rate = 0; // rate at which update() is being called on MavLinkMultiRotorApi + uint32_t actuation_delay = 0; // delay from HIL_SENSOR to HIL_ACTUATORCONTROLS response + uint32_t sensor_rate = 0; // rate we are sending HIL_SENSOR messages + uint32_t lock_step_resets = 0; // total number of lock_step resets + uint32_t update_time = 0; // time inside MavLinkMultiRotorApi::update() method + + // not serialized + const char* wifiInterfaceName = nullptr; // the name of the wifi interface we are measuring RSSI on. + virtual std::string toJSon() { + + std::ostringstream result; + result << "\"MavLinkTelemetry\"" << " : { "; + result << "\"messages_sent\":" << this->messages_sent << ","; + result << "\"messages_received\":" << this->messages_received << ","; + result << "\"messages_handled\":" << this->messages_handled << ","; + result << "\"crc_errors\":" << this->crc_errors << ","; + result << "\"handler_microseconds\":" << this->handler_microseconds << ","; + result << "\"render_time\":" << this->render_time; + result << "\"wifi_rssi\":" << this->wifi_rssi; + result << "\"update_rate\":" << this->update_rate; + result << "\"actuation_delay\":" << this->actuation_delay; + result << "\"sensor_rate\":" << this->sensor_rate; + result << "\"lock_step_resets\":" << this->lock_step_resets; + result << "\"udpate_time\":" << this->update_time; + result << "}"; + return result.str(); + } +protected: + virtual int pack(char* buffer) const; + virtual int unpack(const char* buffer); +}; } diff --git a/MavLinkCom/include/MavLinkNode.hpp b/MavLinkCom/include/MavLinkNode.hpp index 54ee613d0c..0483dbbcc7 100644 --- a/MavLinkCom/include/MavLinkNode.hpp +++ b/MavLinkCom/include/MavLinkNode.hpp @@ -15,94 +15,95 @@ namespace mavlinkcom_impl { } namespace mavlinkcom { - struct MavLinkParameter { - public: - std::string name; - int index = -1; - float value = 0; - uint8_t type = 0; - }; - - class MavLinkCommand; - - // This is the base class for all MavLink objects that represent a single - // mavlink node. This class provides high level state that applies to any - // kind of mavlink node whether it is a vehicle, ground control, simulator - // or log viewer. This base class also implements the heartbeat protocol - // which pings heart beats back to the remote node to ensure it knows our - // connection is still alive. - class MavLinkNode - { - public: - MavLinkNode(int localSystemId, int localComponentId); - ~MavLinkNode(); - - // start listening to this connection - void connect(std::shared_ptr connection); - - // stop listening to the connection. - void close(); - - // Send heartbeat to drone. You should not do this if some other node is - // already doing it. - void startHeartbeat(); - - // get the list of configurable parameters supported by this node. - std::vector getParamList(); +struct MavLinkParameter +{ +public: + std::string name; + int index = -1; + float value = 0; + uint8_t type = 0; +}; + +class MavLinkCommand; + +// This is the base class for all MavLink objects that represent a single +// mavlink node. This class provides high level state that applies to any +// kind of mavlink node whether it is a vehicle, ground control, simulator +// or log viewer. This base class also implements the heartbeat protocol +// which pings heart beats back to the remote node to ensure it knows our +// connection is still alive. +class MavLinkNode +{ +public: + MavLinkNode(int localSystemId, int localComponentId); + ~MavLinkNode(); + + // start listening to this connection + void connect(std::shared_ptr connection); + + // stop listening to the connection. + void close(); + + // Send heartbeat to drone. You should not do this if some other node is + // already doing it. + void startHeartbeat(); + + // get the list of configurable parameters supported by this node. + std::vector getParamList(); - // get the parameter from last getParamList download. - MavLinkParameter getCachedParameter(const std::string& name); + // get the parameter from last getParamList download. + MavLinkParameter getCachedParameter(const std::string& name); - // get a single parameter by name. - AsyncResult getParameter(const std::string& name); + // get a single parameter by name. + AsyncResult getParameter(const std::string& name); - // set a new value on a given parameter. - // it is best if you use getParameter to get the current value, see if it - // needs changing, change the value, then call setParameter with the same parameter object. - AsyncResult setParameter(MavLinkParameter p); + // set a new value on a given parameter. + // it is best if you use getParameter to get the current value, see if it + // needs changing, change the value, then call setParameter with the same parameter object. + AsyncResult setParameter(MavLinkParameter p); - // get the connection - std::shared_ptr getConnection(); + // get the connection + std::shared_ptr getConnection(); - // get the capabilities of the drone - AsyncResult getCapabilities(); + // get the capabilities of the drone + AsyncResult getCapabilities(); - // request that target node sends this message stream at given frequency (specify messages per second that you want). - // where msgId is a MAVLINK_MSG_ID_* enum value. - void setMessageInterval(int msgId, int frequency); + // request that target node sends this message stream at given frequency (specify messages per second that you want). + // where msgId is a MAVLINK_MSG_ID_* enum value. + void setMessageInterval(int msgId, int frequency); - // wait a given amount of time for a heart beat, and return the decoded message. - AsyncResult waitForHeartbeat(); + // wait a given amount of time for a heart beat, and return the decoded message. + AsyncResult waitForHeartbeat(); - // send a single heartbeat - void sendOneHeartbeat(); + // send a single heartbeat + void sendOneHeartbeat(); - // Get the local system and component id - int getLocalSystemId(); - int getLocalComponentId(); + // Get the local system and component id + int getLocalSystemId(); + int getLocalComponentId(); - // Get the target system and component id on the other end of the connection. - int getTargetSystemId(); - int getTargetComponentId(); + // Get the target system and component id on the other end of the connection. + int getTargetSystemId(); + int getTargetComponentId(); - // Send a message to the remote node - void sendMessage(MavLinkMessageBase& msg); + // Send a message to the remote node + void sendMessage(MavLinkMessageBase& msg); - // Send raw undecoded messge to remote node (this is handy in proxy scenarios where you are simply - // passing a message along). - void sendMessage(MavLinkMessage& msg); + // Send raw undecoded messge to remote node (this is handy in proxy scenarios where you are simply + // passing a message along). + void sendMessage(MavLinkMessage& msg); - // send a command to the remote node - void sendCommand(MavLinkCommand& cmd); + // send a command to the remote node + void sendCommand(MavLinkCommand& cmd); - // send a command to the remote node and get async result that tells you whether it succeeded or not. - AsyncResult sendCommandAndWaitForAck(MavLinkCommand& cmd); - public: - MavLinkNode(); - //MavLinkNode(MavLinkNode&&); - protected: - std::unique_ptr pImpl; - }; + // send a command to the remote node and get async result that tells you whether it succeeded or not. + AsyncResult sendCommandAndWaitForAck(MavLinkCommand& cmd); +public: + MavLinkNode(); + //MavLinkNode(MavLinkNode&&); +protected: + std::unique_ptr pImpl; +}; } #endif diff --git a/MavLinkCom/include/MavLinkVehicle.hpp b/MavLinkCom/include/MavLinkVehicle.hpp index 81902f83e4..c29ae4d4b2 100644 --- a/MavLinkCom/include/MavLinkVehicle.hpp +++ b/MavLinkCom/include/MavLinkVehicle.hpp @@ -17,64 +17,65 @@ namespace mavlinkcom_impl { namespace mavlinkcom { - // This class represents a MavLinkNode that can be controlled, hence a "vehicle" of some sort. - // It also keeps certain state about the vehicle position so you can query it any time. - // All x,y,z coordinates are in the NED coordinate system. - class MavLinkVehicle : public MavLinkNode { - public: - MavLinkVehicle(int localSystemId, int localComponentId); +// This class represents a MavLinkNode that can be controlled, hence a "vehicle" of some sort. +// It also keeps certain state about the vehicle position so you can query it any time. +// All x,y,z coordinates are in the NED coordinate system. +class MavLinkVehicle : public MavLinkNode +{ +public: + MavLinkVehicle(int localSystemId, int localComponentId); - // Send command to arm or disarm the drone. Drone will not fly until it is armed successfully. - // It returns false if the command is rejected. - AsyncResult armDisarm(bool arm); - AsyncResult takeoff(float z = -2.5, float pitch = 0, float yaw = 0); - AsyncResult land(float yaw, float lat = 0, float lon = 0, float altitude = 0); - AsyncResult returnToHome(); - AsyncResult loiter(); - AsyncResult setPositionHoldMode(); - AsyncResult setStabilizedFlightMode(); - AsyncResult setHomePosition(float lat = 0, float lon = 0, float alt = 0); - AsyncResult setMissionMode(); - AsyncResult waitForHomePosition(); - AsyncResult allowFlightControlOverUsb(); - AsyncResult setMode(int modeFlags, int customMode = 0, int customSubMode = 0); + // Send command to arm or disarm the drone. Drone will not fly until it is armed successfully. + // It returns false if the command is rejected. + AsyncResult armDisarm(bool arm); + AsyncResult takeoff(float z = -2.5, float pitch = 0, float yaw = 0); + AsyncResult land(float yaw, float lat = 0, float lon = 0, float altitude = 0); + AsyncResult returnToHome(); + AsyncResult loiter(); + AsyncResult setPositionHoldMode(); + AsyncResult setStabilizedFlightMode(); + AsyncResult setHomePosition(float lat = 0, float lon = 0, float alt = 0); + AsyncResult setMissionMode(); + AsyncResult waitForHomePosition(); + AsyncResult allowFlightControlOverUsb(); + AsyncResult setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true); - // wait for drone to reach specified local z, ensure it is not just blowing past the z location, - // wait for it to settle down with dz delta, and dvz delta velocity. - AsyncResult waitForAltitude(float z, float dz, float dvz); + // wait for drone to reach specified local z, ensure it is not just blowing past the z location, + // wait for it to settle down with dz delta, and dvz delta velocity. + AsyncResult waitForAltitude(float z, float dz, float dvz); - // request OFFBOARD control. - void requestControl(); - // release OFFBOARD control - void releaseControl(); - // return true if we still have offboard control (can lose this if user flips the switch). - bool hasOffboardControl(); + // request OFFBOARD control. + void requestControl(); + // release OFFBOARD control + void releaseControl(); + // return true if we still have offboard control (can lose this if user flips the switch). + bool hasOffboardControl(); - // offboard control methods. - bool isLocalControlSupported(); - void moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate); - void moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate); - void moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate); - void moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate); - void writeMessage(MavLinkMessageBase& message, bool update_stats = true); + // offboard control methods. + bool isLocalControlSupported(); + void moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate); + void moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate); + void moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate); + void moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate); + void writeMessage(MavLinkMessageBase& message, bool update_stats = true); - // low level control, only use this one if you really know what you are doing!! - bool isAttitudeControlSupported(); + // low level control, only use this one if you really know what you are doing!! + bool isAttitudeControlSupported(); - // Move drone by directly controlling the attitude of the drone (units are degrees). - // If the rollRate, pitchRate and yawRate are all zero then you will get the default rates provided by the drone. - void moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust); + // Move drone by directly controlling the attitude of the drone (units are degrees). + // If the rollRate, pitchRate and yawRate are all zero then you will get the default rates provided by the drone. + void moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust); - uint32_t getTimeStamp(); - int getVehicleStateVersion(); - const VehicleState& getVehicleState(); + uint32_t getTimeStamp(); + int getVehicleStateVersion(); + const VehicleState& getVehicleState(); - public: - //needed for piml pattern - MavLinkVehicle(); - ~MavLinkVehicle(); - //MavLinkVehicle(MavLinkVehicle&&); - }; +public: + //needed for piml pattern + MavLinkVehicle(); + ~MavLinkVehicle(); + //MavLinkVehicle(MavLinkVehicle&&); +}; } #endif diff --git a/MavLinkCom/src/MavLinkConnection.cpp b/MavLinkCom/src/MavLinkConnection.cpp index 0d1b93ee98..27070f494a 100644 --- a/MavLinkCom/src/MavLinkConnection.cpp +++ b/MavLinkCom/src/MavLinkConnection.cpp @@ -32,9 +32,9 @@ 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) +std::string MavLinkConnection::acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort) { - pImpl->acceptTcp(shared_from_this(), nodeName, localAddr, listeningPort); + return pImpl->acceptTcp(shared_from_this(), nodeName, localAddr, listeningPort); } void MavLinkConnection::startListening(const std::string& nodeName, std::shared_ptr connectedPort) @@ -53,6 +53,17 @@ void MavLinkConnection::stopLoggingSendMessage() pImpl->stopLoggingSendMessage(); } +// log every message that is "received" +void MavLinkConnection::startLoggingReceiveMessage(std::shared_ptr log) +{ + pImpl->startLoggingReceiveMessage(log); +} + +void MavLinkConnection::stopLoggingReceiveMessage() +{ + pImpl->stopLoggingReceiveMessage(); +} + void MavLinkConnection::close() { pImpl->close(); diff --git a/MavLinkCom/src/MavLinkLog.cpp b/MavLinkCom/src/MavLinkLog.cpp index 9ee7d6dc01..838aa0c448 100644 --- a/MavLinkCom/src/MavLinkLog.cpp +++ b/MavLinkCom/src/MavLinkLog.cpp @@ -8,6 +8,9 @@ using namespace mavlinkcom; using namespace mavlink_utils; +#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol +#define MAVLINK_STX 253 // marker for mavlink 2 + uint64_t MavLinkFileLog::getTimeStamp() { return std::chrono::duration_cast(std::chrono::high_resolution_clock::now().time_since_epoch()).count(); @@ -16,167 +19,204 @@ uint64_t MavLinkFileLog::getTimeStamp() MavLinkFileLog::MavLinkFileLog() { - ptr_ = nullptr; - reading_ = false; - writing_ = false; - json_ = false; + ptr_ = nullptr; + reading_ = false; + writing_ = false; + json_ = false; } MavLinkFileLog::~MavLinkFileLog() { - close(); + close(); } bool MavLinkFileLog::isOpen() { - return reading_ || writing_; + return reading_ || writing_; } void MavLinkFileLog::openForReading(const std::string& filename) { - close(); - file_name_ = filename; - ptr_ = fopen(filename.c_str(), "rb"); - if (ptr_ == nullptr) { - throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno)); - } + close(); + file_name_ = filename; + ptr_ = fopen(filename.c_str(), "rb"); + if (ptr_ == nullptr) { + throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno)); + } fseek(ptr_, 0, SEEK_SET); - reading_ = true; - writing_ = false; + reading_ = true; + writing_ = false; } void MavLinkFileLog::openForWriting(const std::string& filename, bool json) { - close(); - json_ = json; - file_name_ = filename; - ptr_ = fopen(filename.c_str(), "wb"); - if (ptr_ == nullptr) { - throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno)); - } - if (json) { - fprintf(ptr_, "{ \"rows\": [\n"); - } - reading_ = false; - writing_ = true; + close(); + json_ = json; + file_name_ = filename; + ptr_ = fopen(filename.c_str(), "wb"); + if (ptr_ == nullptr) { + throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno)); + } + if (json) { + fprintf(ptr_, "{ \"rows\": [\n"); + } + reading_ = false; + writing_ = true; } void MavLinkFileLog::close() { - FILE* temp = ptr_; - if (json_ && ptr_ != nullptr) { + FILE* temp = ptr_; + if (json_ && ptr_ != nullptr) { fprintf(ptr_, " {}\n"); // so that trailing comma on last row isn't a problem. - fprintf(ptr_, "]}\n"); - } - ptr_ = nullptr; - if (temp != nullptr) { - fclose(temp); - } - reading_ = false; - writing_ = false; + fprintf(ptr_, "]}\n"); + } + ptr_ = nullptr; + if (temp != nullptr) { + fclose(temp); + } + reading_ = false; + writing_ = false; } uint64_t FlipEndianness(uint64_t v) { - uint64_t result = 0; - uint64_t shift = v; - const int size = sizeof(uint64_t); - for (int i = 0; i < size; i++) - { - uint64_t low = (shift & 0xff); - result = (result << 8) + low; - shift >>= 8; - } - return result; + uint64_t result = 0; + uint64_t shift = v; + const int size = sizeof(uint64_t); + for (int i = 0; i < size; i++) { + uint64_t low = (shift & 0xff); + result = (result << 8) + low; + shift >>= 8; + } + return result; } void MavLinkFileLog::write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp) { - if (ptr_ != nullptr) { - if (reading_) { - throw std::runtime_error("Log file was opened for reading"); - } - if (json_) { - MavLinkMessageBase* strongTypedMsg = MavLinkMessageBase::lookup(msg); - if (strongTypedMsg != nullptr) { + if (ptr_ != nullptr) { + if (reading_) { + throw std::runtime_error("Log file was opened for reading"); + } + if (json_) { + MavLinkMessageBase* strongTypedMsg = MavLinkMessageBase::lookup(msg); + if (strongTypedMsg != nullptr) { strongTypedMsg->timestamp = timestamp; - std::string line = strongTypedMsg->toJSon(); - fprintf(ptr_, " %s\n", line.c_str()); + std::string line = strongTypedMsg->toJSon(); + { + std::lock_guard lock(log_lock_); + fprintf(ptr_, " %s\n", line.c_str()); + } delete strongTypedMsg; - } - } - else { + } + } + else { if (timestamp == 0) { timestamp = getTimeStamp(); } - // for compatibility with QGroundControl we have to save the time field in big endian. + // for compatibility with QGroundControl we have to save the time field in big endian. // todo: mavlink2 support? timestamp = FlipEndianness(timestamp); - fwrite(×tamp, sizeof(uint64_t), 1, ptr_); - fwrite(&msg.magic, 1, 1, ptr_); - fwrite(&msg.len, 1, 1, ptr_); - fwrite(&msg.seq, 1, 1, ptr_); - fwrite(&msg.sysid, 1, 1, ptr_); - fwrite(&msg.compid, 1, 1, ptr_); - uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink1 msgid - fwrite(&msgid, 1, 1, ptr_); - fwrite(&msg.payload64, 1, msg.len, ptr_); - fwrite(&msg.checksum, sizeof(uint16_t), 1, ptr_); - } - } + + uint8_t magic = msg.magic; + if (magic != MAVLINK_STX_MAVLINK1) { + // has to be one or the other! + magic = MAVLINK_STX; + } + + std::lock_guard lock(log_lock_); + fwrite(×tamp, sizeof(uint64_t), 1, ptr_); + fwrite(&magic, 1, 1, ptr_); + fwrite(&msg.len, 1, 1, ptr_); + fwrite(&msg.seq, 1, 1, ptr_); + fwrite(&msg.sysid, 1, 1, ptr_); + fwrite(&msg.compid, 1, 1, ptr_); + + if (magic == MAVLINK_STX_MAVLINK1) { + uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink 2 msgid + fwrite(&msgid, 1, 1, ptr_); + } + else { + // 24 bits. + uint8_t msgid = msg.msgid & 0xFF; + fwrite(&msgid, 1, 1, ptr_); + msgid = (msg.msgid >> 8) & 0xFF; + fwrite(&msgid, 1, 1, ptr_); + msgid = (msg.msgid >> 16) & 0xFF; + fwrite(&msgid, 1, 1, ptr_); + } + + fwrite(&msg.payload64, 1, msg.len, ptr_); + fwrite(&msg.checksum, sizeof(uint16_t), 1, ptr_); + } + } } bool MavLinkFileLog::read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp) { - if (ptr_ != nullptr) { - if (writing_) { - throw std::runtime_error("Log file was opened for writing"); - } - uint64_t time; - - size_t s = fread(&time, 1, sizeof(uint64_t), ptr_); - if (s < sizeof(uint64_t)) { + if (ptr_ != nullptr) { + if (writing_) { + throw std::runtime_error("Log file was opened for writing"); + } + uint64_t time; + + size_t s = fread(&time, 1, sizeof(uint64_t), ptr_); + if (s < sizeof(uint64_t)) { int hr = errno; - return false; - } + return false; + } timestamp = FlipEndianness(time); - s = fread(&msg.magic, 1, 1, ptr_); - if (s == 0) { - return false; - } - s = fread(&msg.len, 1, 1, ptr_); - if (s == 0) { - return false; - } - s = fread(&msg.seq, 1, 1, ptr_); - if (s == 0) { - return false; - } - s = fread(&msg.sysid, 1, 1, ptr_); - if (s == 0) { - return false; - } - s = fread(&msg.compid, 1, 1, ptr_); - if (s == 0) { - return false; - } - uint8_t msgid = 0; - s = fread(&msgid, 1, 1, ptr_); - msg.msgid = msgid; - if (s < 1) { - return false; - } - s = fread(&msg.payload64, 1, msg.len, ptr_); - if (s < msg.len) { - return false; - } - s = fread(&msg.checksum, 1, sizeof(uint16_t), ptr_); - if (s < sizeof(uint16_t)) { - return false; - } - return true; - } - return false; + s = fread(&msg.magic, 1, 1, ptr_); + if (s == 0) { + return false; + } + s = fread(&msg.len, 1, 1, ptr_); + if (s == 0) { + return false; + } + s = fread(&msg.seq, 1, 1, ptr_); + if (s == 0) { + return false; + } + s = fread(&msg.sysid, 1, 1, ptr_); + if (s == 0) { + return false; + } + s = fread(&msg.compid, 1, 1, ptr_); + if (s == 0) { + return false; + } + + if (msg.magic == MAVLINK_STX_MAVLINK1) { + uint8_t msgid = 0; + s = fread(&msgid, 1, 1, ptr_); + msg.msgid = msgid; + } + else { + // 24 bits. + uint8_t msgid = 0; + s = fread(&msgid, 1, 1, ptr_); + msg.msgid = msgid; + s = fread(&msgid, 1, 1, ptr_); + msg.msgid |= (msgid << 8); + s = fread(&msgid, 1, 1, ptr_); + msg.msgid |= (msgid << 16); + } + + if (s < 1) { + return false; + } + s = fread(&msg.payload64, 1, msg.len, ptr_); + if (s < msg.len) { + return false; + } + s = fread(&msg.checksum, 1, sizeof(uint16_t), ptr_); + if (s < sizeof(uint16_t)) { + return false; + } + return true; + } + return false; } diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp index 5601d5e522..0dbbc143c0 100644 --- a/MavLinkCom/src/MavLinkMessageBase.cpp +++ b/MavLinkCom/src/MavLinkMessageBase.cpp @@ -20,6 +20,74 @@ STRICT_MODE_ON using namespace mavlink_utils; using namespace mavlinkcom; +int MavLinkMessage::update_checksum() +{ + bool mavlink1 = protocol_version != 2; + uint8_t header_len = MAVLINK_CORE_HEADER_LEN + 1; + uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1]; + if (mavlink1) { + magic = MAVLINK_STX_MAVLINK1; + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1; + } + else { + magic = MAVLINK_STX; + } + + incompat_flags = 0; + compat_flags = 0; + + // pack the payload buffer. + char* payload = reinterpret_cast(&payload64[0]); + int len = this->len; + + // calculate checksum + const mavlink_msg_entry_t* entry = mavlink_get_msg_entry(msgid); + uint8_t crc_extra = 0; + int msglen = 0; + if (entry != nullptr) { + crc_extra = entry->crc_extra; + msglen = entry->min_msg_len; + } + if (msgid == MavLinkTelemetry::kMessageId) { + msglen = MavLinkTelemetry::MessageLength; // mavlink doesn't know about our custom telemetry message. + } + + if (len != msglen) { + // mavlink2 supports trimming the payload of trailing zeros so the messages + // are variable length as a result. + if (mavlink1) { + throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen)); + } + } + len = mavlink1 ? msglen : _mav_trim_payload(payload, msglen); + this->len = len; + + // form the header as a byte array for the crc + buf[0] = this->magic; + buf[1] = this->len; + if (mavlink1) { + buf[2] = this->seq; + buf[3] = this->sysid; + buf[4] = this->compid; + buf[5] = this->msgid & 0xFF; + } + else { + buf[2] = this->incompat_flags; + buf[3] = this->compat_flags; + buf[4] = this->seq; + buf[5] = this->sysid; + buf[6] = this->compid; + buf[7] = this->msgid & 0xFF; + buf[8] = (this->msgid >> 8) & 0xFF; + buf[9] = (this->msgid >> 16) & 0xFF; + } + + this->checksum = crc_calculate(&buf[1], header_len - 1); + crc_accumulate_buffer(&this->checksum, payload, len); + crc_accumulate(crc_extra, &this->checksum); + + return len + header_len + 2; +} void MavLinkMessageBase::decode(const MavLinkMessage& msg) { // unpack the message... @@ -208,8 +276,7 @@ void MavLinkMessageBase::unpack_int8_t_array(int len, const char* buffer, int8_t } void MavLinkMessageBase::unpack_uint16_t_array(int len, const char* buffer, uint16_t* field, int offset) { - for (int i = 0; i < len; i++) - { + for (int i = 0; i < len; i++) { _mav_get_uint16_t(buffer, offset, field); offset += sizeof(uint16_t); field++; @@ -217,8 +284,7 @@ void MavLinkMessageBase::unpack_uint16_t_array(int len, const char* buffer, uint } void MavLinkMessageBase::unpack_int16_t_array(int len, const char* buffer, int16_t* field, int offset) { - for (int i = 0; i < len; i++) - { + for (int i = 0; i < len; i++) { _mav_get_int16_t(buffer, offset, field); offset += sizeof(int16_t); field++; @@ -226,8 +292,7 @@ void MavLinkMessageBase::unpack_int16_t_array(int len, const char* buffer, int16 } void MavLinkMessageBase::unpack_float_array(int len, const char* buffer, float* field, int offset) { - for (int i = 0; i < len; i++) - { + for (int i = 0; i < len; i++) { _mav_get_float(buffer, offset, field); offset += sizeof(float); field++; @@ -235,33 +300,41 @@ void MavLinkMessageBase::unpack_float_array(int len, const char* buffer, float* } int MavLinkTelemetry::pack(char* buffer) const { - pack_int32_t(buffer, reinterpret_cast(&this->messagesSent), 0); - pack_int32_t(buffer, reinterpret_cast(&this->messagesReceived), 4); - pack_int32_t(buffer, reinterpret_cast(&this->messagesHandled), 8); - pack_int32_t(buffer, reinterpret_cast(&this->crcErrors), 12); - pack_int32_t(buffer, reinterpret_cast(&this->handlerMicroseconds), 16); - pack_int32_t(buffer, reinterpret_cast(&this->renderTime), 20); - pack_int32_t(buffer, reinterpret_cast(&this->wifiRssi), 24); - return 28; + pack_int32_t(buffer, reinterpret_cast(&this->messages_sent), 0); + pack_int32_t(buffer, reinterpret_cast(&this->messages_received), 4); + pack_int32_t(buffer, reinterpret_cast(&this->messages_handled), 8); + pack_int32_t(buffer, reinterpret_cast(&this->crc_errors), 12); + pack_int32_t(buffer, reinterpret_cast(&this->handler_microseconds), 16); + pack_int32_t(buffer, reinterpret_cast(&this->render_time), 20); + pack_int32_t(buffer, reinterpret_cast(&this->wifi_rssi), 24); + pack_int32_t(buffer, reinterpret_cast(&this->update_rate), 28); + pack_int32_t(buffer, reinterpret_cast(&this->actuation_delay), 32); + pack_int32_t(buffer, reinterpret_cast(&this->sensor_rate), 36); + pack_int32_t(buffer, reinterpret_cast(&this->lock_step_resets), 40); + pack_int32_t(buffer, reinterpret_cast(&this->update_time), 44); + return MavLinkTelemetry::MessageLength; } int MavLinkTelemetry::unpack(const char* buffer) { - unpack_int32_t(buffer, reinterpret_cast(&this->messagesSent), 0); - unpack_int32_t(buffer, reinterpret_cast(&this->messagesReceived), 4); - unpack_int32_t(buffer, reinterpret_cast(&this->messagesHandled), 8); - unpack_int32_t(buffer, reinterpret_cast(&this->crcErrors), 12); - unpack_int32_t(buffer, reinterpret_cast(&this->handlerMicroseconds), 16); - unpack_int32_t(buffer, reinterpret_cast(&this->renderTime), 20); - unpack_int32_t(buffer, reinterpret_cast(&this->wifiRssi), 24); - return 28; + unpack_int32_t(buffer, reinterpret_cast(&this->messages_sent), 0); + unpack_int32_t(buffer, reinterpret_cast(&this->messages_received), 4); + unpack_int32_t(buffer, reinterpret_cast(&this->messages_handled), 8); + unpack_int32_t(buffer, reinterpret_cast(&this->crc_errors), 12); + unpack_int32_t(buffer, reinterpret_cast(&this->handler_microseconds), 16); + unpack_int32_t(buffer, reinterpret_cast(&this->render_time), 20); + unpack_int32_t(buffer, reinterpret_cast(&this->wifi_rssi), 24); + unpack_int32_t(buffer, reinterpret_cast(&this->update_rate), 28); + unpack_int32_t(buffer, reinterpret_cast(&this->actuation_delay), 32); + unpack_int32_t(buffer, reinterpret_cast(&this->sensor_rate), 36); + unpack_int32_t(buffer, reinterpret_cast(&this->lock_step_resets), 40); + unpack_int32_t(buffer, reinterpret_cast(&this->update_time), 44); + return MavLinkTelemetry::MessageLength; } - std::string MavLinkMessageBase::char_array_tostring(int len, const char* field) { int i = 0; - for (i= 0; i < len; i++) - { + for (i= 0; i < len; i++) { if (field[i] == '\0') break; } diff --git a/MavLinkCom/src/MavLinkVehicle.cpp b/MavLinkCom/src/MavLinkVehicle.cpp index 3d4a183407..ebdc95b60b 100644 --- a/MavLinkCom/src/MavLinkVehicle.cpp +++ b/MavLinkCom/src/MavLinkVehicle.cpp @@ -13,10 +13,12 @@ MavLinkVehicle::MavLinkVehicle(int localSystemId, int localComponentId) pImpl.reset(new MavLinkVehicleImpl(localSystemId, localComponentId)); } -MavLinkVehicle::MavLinkVehicle(){ +MavLinkVehicle::MavLinkVehicle() +{ } -MavLinkVehicle::~MavLinkVehicle() { +MavLinkVehicle::~MavLinkVehicle() +{ pImpl = nullptr; } @@ -50,10 +52,10 @@ AsyncResult MavLinkVehicle::returnToHome() return ptr->returnToHome(); } -AsyncResult MavLinkVehicle::setMode(int modeFlags, int customMode, int customSubMode) +AsyncResult MavLinkVehicle::setMode(int modeFlags, int customMode, int customSubMode, bool waitForAck) { auto ptr = static_cast(pImpl.get()); - return ptr->setMode(modeFlags, customMode, customSubMode); + return ptr->setMode(modeFlags, customMode, customSubMode, waitForAck); } bool MavLinkVehicle::isLocalControlSupported() diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index 2e4655b7bc..9b2970e057 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -16,12 +16,11 @@ using namespace mavlinkcom_impl; MavLinkConnectionImpl::MavLinkConnectionImpl() { // add our custom telemetry message length. - telemetry_.crcErrors = 0; - telemetry_.handlerMicroseconds = 0; - telemetry_.messagesHandled = 0; - telemetry_.messagesReceived = 0; - telemetry_.messagesSent = 0; - telemetry_.renderTime = 0; + telemetry_.crc_errors = 0; + telemetry_.handler_microseconds = 0; + telemetry_.messages_handled = 0; + telemetry_.messages_received = 0; + telemetry_.messages_sent = 0; closed = true; ::memset(&mavlink_intermediate_status_, 0, sizeof(mavlink_status_t)); ::memset(&mavlink_status_, 0, sizeof(mavlink_status_t)); @@ -86,7 +85,7 @@ 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 MavLinkConnectionImpl::acceptTcp(std::shared_ptr parent, const std::string& nodeName, const std::string& localAddr, int listeningPort) { std::string local = localAddr; close(); @@ -95,10 +94,13 @@ void MavLinkConnectionImpl::acceptTcp(std::shared_ptr parent, port = socket; // this is so that a call to close() can cancel this blocking accept call. socket->accept(localAddr, listeningPort); + std::string remote = socket->remoteAddress(); + socket->setNonBlocking(); socket->setNoDelay(); parent->startListening(nodeName, socket); + return remote; } std::shared_ptr MavLinkConnectionImpl::connectSerial(const std::string& nodeName, const std::string& portName, int baudRate, const std::string& initString) @@ -144,6 +146,17 @@ void MavLinkConnectionImpl::stopLoggingSendMessage() sendLog_ = nullptr; } +// log every message that is "sent" using sendMessage. +void MavLinkConnectionImpl::startLoggingReceiveMessage(std::shared_ptr log) +{ + receiveLog_ = log; +} + +void MavLinkConnectionImpl::stopLoggingReceiveMessage() +{ + receiveLog_ = nullptr; +} + void MavLinkConnectionImpl::close() { closed = true; @@ -159,7 +172,8 @@ void MavLinkConnectionImpl::close() msg_available_.post(); publish_thread_.join(); } - sendLog_ = nullptr; + sendLog_ = nullptr; + receiveLog_ = nullptr; } bool MavLinkConnectionImpl::isOpen() @@ -201,8 +215,7 @@ void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& m) ::memcpy(&msg, &m, sizeof(MavLinkMessage)); prepareForSending(msg); - if (sendLog_ != nullptr) - { + if (sendLog_ != nullptr) { sendLog_->write(msg); } @@ -231,7 +244,7 @@ void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& m) } { std::lock_guard guard(telemetry_mutex_); - telemetry_.messagesSent++; + telemetry_.messages_sent++; } } @@ -257,7 +270,7 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg) msg.seq = seqno; msg.incompat_flags = 0; - if (signing_) { + if (signing) { msg.incompat_flags |= MAVLINK_IFLAG_SIGNED; } msg.compat_flags = 0; @@ -275,7 +288,7 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg) msglen = entry->min_msg_len; } if (msg.msgid == MavLinkTelemetry::kMessageId) { - msglen = 28; // mavlink doesn't know about our custom telemetry message. + msglen = MavLinkTelemetry::MessageLength; // mavlink doesn't know about our custom telemetry message. } if (len != msglen) { @@ -286,7 +299,7 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg) // mavlink2 supports trimming the payload of trailing zeros so the messages // are variable length as a result. } - } + } len = mavlink1 ? msglen : _mav_trim_payload(payload, msglen); msg.len = len; @@ -320,12 +333,12 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg) mavlink_ck_b(&msg) = (uint8_t)(msg.checksum >> 8); STRICT_MODE_ON - if (signing_) { + if (signing) { mavlink_sign_packet(mavlink_status_.signing, - reinterpret_cast(msg.signature), - reinterpret_cast(message_buf), header_len, - reinterpret_cast(payload), msg.len, - reinterpret_cast(payload) + msg.len); + reinterpret_cast(msg.signature), + reinterpret_cast(message_buf), header_len, + reinterpret_cast(payload), msg.len, + reinterpret_cast(payload) + msg.len); } return msg.len + header_len + 2 + signature_len; @@ -350,10 +363,8 @@ int MavLinkConnectionImpl::subscribe(MessageHandler handler) void MavLinkConnectionImpl::unsubscribe(int id) { std::lock_guard guard(listener_mutex); - for (auto ptr = listeners.begin(), end = listeners.end(); ptr != end; ptr++) - { - if ((*ptr).id == id) - { + for (auto ptr = listeners.begin(), end = listeners.end(); ptr != end; ptr++) { + if ((*ptr).id == id) { listeners.erase(ptr); snapshot_stale = true; break; @@ -365,8 +376,7 @@ void MavLinkConnectionImpl::joinLeftSubscriber(std::shared_ptrpImpl->supports_mavlink2_ = true; } @@ -401,11 +411,9 @@ void MavLinkConnectionImpl::readPackets() mavlink_intermediate_status_.parse_state = MAVLINK_PARSE_STATE_IDLE; int channel = 0; int hr = 0; - while (hr == 0 && con_ != nullptr && !closed) - { + while (hr == 0 && con_ != nullptr && !closed) { int read = 0; - if (safePort->isClosed()) - { + if (safePort->isClosed()) { // hmmm, wait till it is opened? std::this_thread::sleep_for(std::chrono::milliseconds(10)); continue; @@ -417,8 +425,7 @@ void MavLinkConnectionImpl::readPackets() std::this_thread::sleep_for(std::chrono::milliseconds(1)); continue; } - for (int i = 0; i < count; i++) - { + for (int i = 0; i < count; i++) { uint8_t frame_state = mavlink_frame_char_buffer(&msgBuffer, &mavlink_intermediate_status_, buffer[i], &msg, &mavlink_status_); if (frame_state == MAVLINK_FRAMING_INCOMPLETE) { @@ -426,29 +433,26 @@ void MavLinkConnectionImpl::readPackets() } else if (frame_state == MAVLINK_FRAMING_BAD_CRC) { std::lock_guard guard(telemetry_mutex_); - telemetry_.crcErrors++; + telemetry_.crc_errors++; } - else if (frame_state == MAVLINK_FRAMING_OK) - { + else if (frame_state == MAVLINK_FRAMING_OK) { // pick up the sysid/compid of the remote node we are connected to. if (other_system_id == -1) { other_system_id = msg.sysid; other_component_id = msg.compid; } - if (mavlink_intermediate_status_.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) - { + if (mavlink_intermediate_status_.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { // then this is a mavlink 1 message } else if (!supports_mavlink2_) { // then this mavlink sender supports mavlink 2 supports_mavlink2_ = true; } - if (con_ != nullptr && !closed) - { + if (con_ != nullptr && !closed) { { std::lock_guard guard(telemetry_mutex_); - telemetry_.messagesReceived++; + telemetry_.messages_received++; } // queue event for publishing. { @@ -475,7 +479,7 @@ void MavLinkConnectionImpl::readPackets() } else { std::lock_guard guard(telemetry_mutex_); - telemetry_.crcErrors++; + telemetry_.crc_errors++; } } @@ -499,10 +503,14 @@ void MavLinkConnectionImpl::drainQueue() hasMsg = true; } } - if (!hasMsg) - { + if (!hasMsg) { return; + } + + if (receiveLog_ != nullptr) { + receiveLog_->write(message); } + // publish the message from this thread, this is safer than publishing from the readPackets thread // as it ensures we don't lose messages if the listener is slow. if (snapshot_stale) { @@ -517,20 +525,17 @@ void MavLinkConnectionImpl::drainQueue() } auto end = snapshot.end(); - if (message.msgid == static_cast(MavLinkMessageIds::MAVLINK_MSG_ID_AUTOPILOT_VERSION)) - { + if (message.msgid == static_cast(MavLinkMessageIds::MAVLINK_MSG_ID_AUTOPILOT_VERSION)) { MavLinkAutopilotVersion cap; cap.decode(message); - if ((cap.capabilities & MAV_PROTOCOL_CAPABILITY_MAVLINK2) != 0) - { + if ((cap.capabilities & MAV_PROTOCOL_CAPABILITY_MAVLINK2) != 0) { this->supports_mavlink2_ = true; } } auto startTime = std::chrono::system_clock::now(); std::shared_ptr sharedPtr = std::shared_ptr(this->con_); - for (auto ptr = snapshot.begin(); ptr != end; ptr++) - { + for (auto ptr = snapshot.begin(); ptr != end; ptr++) { try { (*ptr).handler(sharedPtr, message); } @@ -545,8 +550,8 @@ void MavLinkConnectionImpl::drainQueue() auto diff = endTime - startTime; long microseconds = static_cast(std::chrono::duration_cast(diff).count()); std::lock_guard guard(telemetry_mutex_); - telemetry_.messagesHandled++; - telemetry_.handlerMicroseconds += microseconds; + telemetry_.messages_handled++; + telemetry_.handler_microseconds += microseconds; } } } @@ -576,13 +581,12 @@ void MavLinkConnectionImpl::getTelemetry(MavLinkTelemetry& result) std::lock_guard guard(telemetry_mutex_); result = telemetry_; // reset counters - telemetry_.crcErrors = 0; - telemetry_.handlerMicroseconds = 0; - telemetry_.messagesHandled = 0; - telemetry_.messagesReceived = 0; - telemetry_.messagesSent = 0; - telemetry_.renderTime = 0; + telemetry_.crc_errors = 0; + telemetry_.handler_microseconds = 0; + telemetry_.messages_handled = 0; + telemetry_.messages_received = 0; + telemetry_.messages_sent = 0; if (telemetry_.wifiInterfaceName != nullptr) { - telemetry_.wifiRssi = port->getRssi(telemetry_.wifiInterfaceName); + telemetry_.wifi_rssi = port->getRssi(telemetry_.wifiInterfaceName); } } diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp index 3f43251674..2a63f190cb 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp @@ -36,7 +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 acceptTcp(std::shared_ptr parent, const std::string& nodeName, const std::string& localAddr, int listeningPort); std::string getName(); int getTargetComponentId(); @@ -45,6 +45,8 @@ namespace mavlinkcom_impl { void startListening(std::shared_ptr parent, const std::string& nodeName, std::shared_ptr connectedPort); void startLoggingSendMessage(std::shared_ptr log); void stopLoggingSendMessage(); + void startLoggingReceiveMessage(std::shared_ptr log); + void stopLoggingReceiveMessage(); void close(); bool isOpen(); void sendMessage(const MavLinkMessageBase& msg); @@ -74,6 +76,7 @@ namespace mavlinkcom_impl { std::string accept_node_name_; std::shared_ptr server_; std::shared_ptr sendLog_; + std::shared_ptr receiveLog_; struct MessageHandlerEntry { public: @@ -94,7 +97,6 @@ namespace mavlinkcom_impl { 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_; std::mutex telemetry_mutex_; diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp index 43cf5097f3..f76e2502f2 100644 --- a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp @@ -435,8 +435,7 @@ void MavLinkVehicleImpl::handleMessage(std::shared_ptr connec // but we do want to know when we get the ack. So this is async ACK processing! MavLinkCommandAck ack; ack.decode(msg); - if (ack.command == MavCmdNavGuidedEnable::kCommandId) - { + if (ack.command == MavCmdNavGuidedEnable::kCommandId) { MAV_RESULT ackResult = static_cast(ack.result); if (ackResult == MAV_RESULT::MAV_RESULT_TEMPORARILY_REJECTED) { Utils::log("### command MavCmdNavGuidedEnable result: MAV_RESULT_TEMPORARILY_REJECTED"); @@ -585,20 +584,17 @@ void MavLinkVehicleImpl::releaseControl() control_requested_ = false; control_request_sent_ = false; vehicle_state_.controls.offboard = false; - MavCmdNavGuidedEnable cmd{}; - cmd.Enable = 0; - sendCommand(cmd); + setMode(static_cast(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED), + static_cast(PX4_CUSTOM_MAIN_MODE_POSCTL), 0, false); } void MavLinkVehicleImpl::checkOffboard() { - if (!control_requested_) - { + if (!control_requested_) { throw std::runtime_error("You must call requestControl first."); } - if (control_requested_ && !vehicle_state_.controls.offboard) - { + if (control_requested_ && !vehicle_state_.controls.offboard) { // Ok, now's the time to actually request it since the caller is about to send MavLinkSetPositionTargetGlobalInt, but // PX4 will reject this thinking 'offboard_control_loss_timeout' because we haven't actually sent any offboard messages // yet. I know the PX4 protocol is kind of weird. So we prime the pump here with some dummy messages that tell the @@ -609,15 +605,14 @@ void MavLinkVehicleImpl::checkOffboard() offboardIdle(); } - Utils::log("MavLinkVehicleImpl::checkOffboard: sending MavCmdNavGuidedEnable \n"); - // now the command should succeed. - bool r = false; - MavCmdNavGuidedEnable cmd{}; - cmd.Enable = 1; + Utils::log("MavLinkVehicleImpl::checkOffboard: sending MavCmdNavGuidedEnable \n"); // 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); - control_request_sent_ = true; + setMode(static_cast(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED), + static_cast(PX4_CUSTOM_MAIN_MODE_OFFBOARD), 0, false); + control_request_sent_ = true; + // assume this was successful, we'll find out if so in the next heartbeat. + vehicle_state_.controls.offboard = true; } } @@ -658,14 +653,12 @@ void MavLinkVehicleImpl::moveToGlobalPosition(float lat, float lon, float alt, b msg.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_VELOCITY | MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_ACCELERATION; - if (isYaw) - { + if (isYaw) { msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_RATE; msg.yaw = yawOrRate; msg.yaw_rate = 0; } - else - { + else { msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE; msg.yaw = 0; msg.yaw_rate = yawOrRate; @@ -679,11 +672,13 @@ void MavLinkVehicleImpl::moveToGlobalPosition(float lat, float lon, float alt, b writeMessage(msg); } -AsyncResult MavLinkVehicleImpl::setMode(int mode, int customMode, int customSubMode) +AsyncResult MavLinkVehicleImpl::setMode(int mode, int customMode, int customSubMode, bool waitForAck) { // this mode change take precedence over offboard mode. - control_requested_ = false; - control_request_sent_ = false; + if (customMode != static_cast(PX4_CUSTOM_MAIN_MODE_OFFBOARD)) { + control_requested_ = false; + control_request_sent_ = false; + } if ((vehicle_state_.mode & static_cast(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED)) != 0) { mode |= static_cast(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); // must preserve this flag. @@ -696,7 +691,13 @@ AsyncResult MavLinkVehicleImpl::setMode(int mode, int customMode, int cust cmd.Mode = static_cast(mode); cmd.CustomMode = static_cast(customMode); cmd.CustomSubmode = static_cast(customSubMode); - return sendCommandAndWaitForAck(cmd); + if (waitForAck) { + return sendCommandAndWaitForAck(cmd); + } + else { + sendCommand(cmd); + return AsyncResult::Completed(true); + } } AsyncResult MavLinkVehicleImpl::setPositionHoldMode() @@ -797,8 +798,7 @@ void MavLinkVehicleImpl::moveByLocalVelocityWithAltHold(float vx, float vy, floa msg.yaw = yawOrRate; msg.yaw_rate = 0; } - else - { + else { msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE; msg.yaw = 0; msg.yaw_rate = yawOrRate; @@ -827,8 +827,7 @@ void MavLinkVehicleImpl::moveToLocalPosition(float x, float y, float z, bool isY msg.yaw = yawOrRate; msg.yaw_rate = 0; } - else - { + else { msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE; msg.yaw = 0; msg.yaw_rate = yawOrRate; diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp index 57ab49a435..377b91dd18 100644 --- a/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp @@ -22,73 +22,74 @@ using namespace mavlinkcom; namespace mavlinkcom_impl { - class MavLinkVehicleImpl : public MavLinkNodeImpl { - public: - MavLinkVehicleImpl(int localSystemId, int localComponentId); - ~MavLinkVehicleImpl(); - public: - AsyncResult armDisarm(bool arm); - AsyncResult takeoff(float z = -2.5, float pitch = 0, float yaw = 0); - AsyncResult land(float yaw, float lat = 0, float lon = 0, float altitude = 0); - AsyncResult returnToHome(); - AsyncResult loiter(); - AsyncResult setPositionHoldMode(); - AsyncResult setStabilizedFlightMode(); - AsyncResult setHomePosition(float lat = 0, float lon = 0, float alt = 0); - AsyncResult setMissionMode(); - AsyncResult waitForHomePosition(); - AsyncResult allowFlightControlOverUsb(); - AsyncResult waitForAltitude(float z, float dz, float dvz); - AsyncResult setMode(int modeFlags, int customMode = 0, int customSubMode = 0); +class MavLinkVehicleImpl : public MavLinkNodeImpl +{ +public: + MavLinkVehicleImpl(int localSystemId, int localComponentId); + ~MavLinkVehicleImpl(); +public: + AsyncResult armDisarm(bool arm); + AsyncResult takeoff(float z = -2.5, float pitch = 0, float yaw = 0); + AsyncResult land(float yaw, float lat = 0, float lon = 0, float altitude = 0); + AsyncResult returnToHome(); + AsyncResult loiter(); + AsyncResult setPositionHoldMode(); + AsyncResult setStabilizedFlightMode(); + AsyncResult setHomePosition(float lat = 0, float lon = 0, float alt = 0); + AsyncResult setMissionMode(); + AsyncResult waitForHomePosition(); + AsyncResult allowFlightControlOverUsb(); + AsyncResult waitForAltitude(float z, float dz, float dvz); + AsyncResult setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true); - // request OFFBOARD control. - void requestControl(); - // release OFFBOARD control - void releaseControl(); + // request OFFBOARD control. + void requestControl(); + // release OFFBOARD control + void releaseControl(); - // return true if we still have offboard control (can lose this if user flips the switch). - bool hasOffboardControl(); - // send this to keep offboard control but do no movement. - void offboardIdle(); + // return true if we still have offboard control (can lose this if user flips the switch). + bool hasOffboardControl(); + // send this to keep offboard control but do no movement. + void offboardIdle(); - // offboard control methods. - bool isLocalControlSupported(); - void moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate); - void moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate); - void moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate); - void moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate); + // offboard control methods. + bool isLocalControlSupported(); + void moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate); + void moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate); + void moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate); + void moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate); - // low level control, only use this one if you really know what you are doing!! - bool isAttitudeControlSupported(); + // low level control, only use this one if you really know what you are doing!! + bool isAttitudeControlSupported(); - // Move drone by directly controlling the attitude of the drone (units are degrees). - // If the rollRate, pitchRate and yawRate are all zero then you will get the default rates provided by the drone. - void moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust); - void writeMessage(MavLinkMessageBase& message, bool update_stats = true); + // Move drone by directly controlling the attitude of the drone (units are degrees). + // If the rollRate, pitchRate and yawRate are all zero then you will get the default rates provided by the drone. + void moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust); + void writeMessage(MavLinkMessageBase& message, bool update_stats = true); - int getVehicleStateVersion(); - const VehicleState& getVehicleState(); + int getVehicleStateVersion(); + const VehicleState& getVehicleState(); - uint32_t getTimeStamp(); - private: - virtual void handleMessage(std::shared_ptr connection, const MavLinkMessage& message); - void resetCommandParams(MavLinkCommandLong& cmd); - void updateReadStats(const MavLinkMessage& msg); - void checkOffboard(); - bool getRcSwitch(int channel, float threshold); + uint32_t getTimeStamp(); +private: + virtual void handleMessage(std::shared_ptr connection, const MavLinkMessage& message); + void resetCommandParams(MavLinkCommandLong& cmd); + void updateReadStats(const MavLinkMessage& msg); + void checkOffboard(); + bool getRcSwitch(int channel, float threshold); - private: - std::mutex state_mutex_; - int state_version_ = 0; - bool control_requested_ = false; - bool control_request_sent_ = false; - int requested_mode_ = 0; - int previous_mode_ = 0; - // this latch is reset even time we receive a heartbeat, this is useful for operations that we - // want to throttle to the heartbeat rate. - bool heartbeat_throttle_ = false; - VehicleState vehicle_state_; - }; +private: + std::mutex state_mutex_; + int state_version_ = 0; + bool control_requested_ = false; + bool control_request_sent_ = false; + int requested_mode_ = 0; + int previous_mode_ = 0; + // this latch is reset even time we receive a heartbeat, this is useful for operations that we + // want to throttle to the heartbeat rate. + bool heartbeat_throttle_ = false; + VehicleState vehicle_state_; +}; } #endif diff --git a/PythonClient/multirotor/Error.wav b/PythonClient/multirotor/Error.wav new file mode 100644 index 0000000000..c4a01cb2b4 Binary files /dev/null and b/PythonClient/multirotor/Error.wav differ diff --git a/PythonClient/multirotor/hello_drone.py b/PythonClient/multirotor/hello_drone.py index e51556e1c3..a9b4a4fa9c 100644 --- a/PythonClient/multirotor/hello_drone.py +++ b/PythonClient/multirotor/hello_drone.py @@ -1,4 +1,4 @@ -import setup_path +import setup_path import airsim import numpy as np @@ -11,7 +11,6 @@ client = airsim.MultirotorClient() client.confirmConnection() client.enableApiControl(True) -client.armDisarm(True) state = client.getMultirotorState() s = pprint.pformat(state) @@ -34,6 +33,8 @@ print("gps_data: %s" % s) airsim.wait_key('Press any key to takeoff') +print("Taking off...") +client.armDisarm(True) client.takeoffAsync().join() state = client.getMultirotorState() @@ -82,8 +83,8 @@ airsim.wait_key('Press any key to reset to original state') -client.armDisarm(False) client.reset() +client.armDisarm(False) # that's enough fun for now. let's quit cleanly client.enableApiControl(False) diff --git a/PythonClient/multirotor/path.py b/PythonClient/multirotor/path.py index d36fc6a900..6d34f13488 100644 --- a/PythonClient/multirotor/path.py +++ b/PythonClient/multirotor/path.py @@ -29,9 +29,9 @@ 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...") +# z of -5 is 5 meters above the original launch point. +z = -5 +print("make sure we are hovering at {} meters...".format(-z)) client.moveToZAsync(z, 1).join() # see https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo diff --git a/PythonClient/multirotor/speaker.py b/PythonClient/multirotor/speaker.py new file mode 100644 index 0000000000..acee778942 --- /dev/null +++ b/PythonClient/multirotor/speaker.py @@ -0,0 +1,36 @@ +################################################################################################### +# +# Project: Embedded Learning Library (ELL) +# File: speaker.py +# Authors: Chris Lovett +# +# Requires: Python 3.x +# +################################################################################################### + +import pyaudio + + +class Speaker: + def __init__(self): + self.output_stream = None + self.audio = pyaudio.PyAudio() + + def open(self, audio_format, num_channels, rate): + # open speakers so we can hear what it is processing... + self.output_stream = self.audio.open(format=audio_format, + channels=num_channels, + rate=rate, + output=True) + + def write(self, data): + if self.output_stream: + self.output_stream.write(data) + + def close(self): + if self.output_stream: + self.output_stream.close() + self.output_stream = None + + def is_closed(self): + return self.output_stream is None diff --git a/PythonClient/multirotor/stability_test.py b/PythonClient/multirotor/stability_test.py new file mode 100644 index 0000000000..98d80a0e26 --- /dev/null +++ b/PythonClient/multirotor/stability_test.py @@ -0,0 +1,90 @@ +import os +import setup_path +import airsim +import time +import numpy as np +import sys + +script_dir = os.path.dirname(__file__) + +client = airsim.MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) + +def play_sound(wavfile): + import speaker + import wav_reader + reader = wav_reader.WavReader() + reader.open(wavfile, 512, speaker.Speaker()) + while True: + buffer = reader.read() + if buffer is None: + break + +class Numbers: + def __init__(self, name): + self.data = [] + self.name = name + + def add(self, x): + self.data += [x] + + def is_unstable(self, amount): + a = np.array(self.data) + minimum = a.min() + maximum = a.max() + mean = np.mean(a) + stddev = np.std(a) + print("{}: min={}, max={}, mean={}, stddev={}".format(self.name, minimum, maximum, mean, stddev)) + return (maximum - minimum) > amount + +print("### TEST STARTED ###") +print("This test takes 20 minutes.") + +iteration = 0 +while iteration < 10: + iteration += 1 + x = Numbers("x") + y = Numbers("y") + z = Numbers("z") + + print("arming the drone...") + client.armDisarm(True) + + while client.getMultirotorState().landed_state == airsim.LandedState.Landed: + print("taking off...") + client.takeoffAsync().join() + time.sleep(1) + + # fly for 2 minutes + start = time.time() + while time.time() < start + 120: + state = client.getMultirotorState() + x_val = state.kinematics_estimated.position.x_val + y_val = state.kinematics_estimated.position.y_val + z_val = state.kinematics_estimated.position.z_val + x.add(x_val) + y.add(y_val) + z.add(z_val) + print("x: {}, y: {}, z: {}".format(x_val, y_val, z_val)) + time.sleep(1) + + print("landing...") + client.landAsync().join() + + print("disarming the drone...") + client.armDisarm(False) + + # more than 50 centimeter drift is unacceptable. + print("Results for iteration {}".format(iteration)) + a = x.is_unstable(0.5) + b = y.is_unstable(0.5) + c = z.is_unstable(0.5) + + if a or b or c: + play_sound(os.path.join(script_dir, "Error.wav")) + break + + time.sleep(5) + +print("### Test Passed ###") diff --git a/PythonClient/multirotor/takeoff.py b/PythonClient/multirotor/takeoff.py index 035f065442..3d16063a4a 100644 --- a/PythonClient/multirotor/takeoff.py +++ b/PythonClient/multirotor/takeoff.py @@ -1,5 +1,15 @@ -import setup_path +import setup_path import airsim +import sys +import time + +# For high speed ascent and descent on PX4 you may need to set these properties: +# param set MPC_Z_VEL_MAX_UP 5 +# param set MPC_Z_VEL_MAX_DN 5 + +z = 5 +if len(sys.argv) > 1: + z = float(sys.argv[1]) client = airsim.MultirotorClient() client.confirmConnection() @@ -14,3 +24,25 @@ else: print("already flying...") client.hoverAsync().join() + +print("make sure we are hovering at {} meters...".format(z)) + +if z > 5: + # AirSim uses NED coordinates so negative axis is up. + # z of -50 is 50 meters above the original launch point. + client.moveToZAsync(-z, 5).join() + client.hoverAsync().join() + time.sleep(5) + +if z > 10: + print("come down quickly to 10 meters...") + z = 10 + client.moveToZAsync(-z, 3).join() + client.hoverAsync().join() + +print("landing...") +client.landAsync().join() +print("disarming...") +client.armDisarm(False) +client.enableApiControl(False) +print("done.") diff --git a/PythonClient/multirotor/wav_reader.py b/PythonClient/multirotor/wav_reader.py new file mode 100644 index 0000000000..74e3bce185 --- /dev/null +++ b/PythonClient/multirotor/wav_reader.py @@ -0,0 +1,158 @@ +################################################################################################### +# +# Project: Embedded Learning Library (ELL) +# File: wav_reader.py +# Authors: Chris Lovett +# +# Requires: Python 3.x +# +################################################################################################### +import audioop +import math +import wave + +import numpy as np +import pyaudio + + +class WavReader: + def __init__(self, sample_rate=16000, channels=1, auto_scale=True): + """ Initialize the wav reader with the type of audio you want returned. + sample_rate Rate you want audio converted to (default 16 kHz) + channels Number of channels you want output (default 1) + auto_scale Whether to scale numbers to the range -1 to 1. + """ + self.input_stream = None + self.audio = pyaudio.PyAudio() + self.wav_file = None + self.requested_channels = int(channels) + self.requested_rate = int(sample_rate) + self.buffer_size = 0 + self.sample_width = 0 + self.read_size = None + self.dtype = None + self.auto_scale = auto_scale + self.audio_scale_factor = 1 + self.tail = None + + def open(self, filename, buffer_size, speaker=None): + """ open a wav file for reading + buffersize Number of audio samples to return on each read() call + speaker Optional output speaker to send converted audio to so you can hear it. + """ + self.speaker = speaker + # open a stream on the audio input file. + self.wav_file = wave.open(filename, "rb") + self.cvstate = None + self.read_size = int(buffer_size) + self.actual_channels = self.wav_file.getnchannels() + self.actual_rate = self.wav_file.getframerate() + self.sample_width = self.wav_file.getsampwidth() + # assumes signed integer used in raw audio, so for example, the max for 16bit is 2^15 (32768) + if self.auto_scale: + self.audio_scale_factor = 1 / pow(2, (8 * self.sample_width) - 1) + + if self.requested_rate == 0: + raise Exception("Requested rate cannot be zero") + self.buffer_size = int(math.ceil((self.read_size * self.actual_rate) / self.requested_rate)) + + # convert int16 data to scaled floats + if self.sample_width == 1: + self.dtype = np.int8 + elif self.sample_width == 2: + self.dtype = np.int16 + elif self.sample_width == 4: + self.dtype = np.int32 + else: + msg = "Unexpected sample width {}, can only handle 1, 2 or 4 byte audio" + raise Exception(msg.format(self.sample_width)) + + if speaker: + # configure output stream to match what we are resampling to... + audio_format = self.audio.get_format_from_width(self.sample_width) + speaker.open(audio_format, self.requested_channels, self.requested_rate) + + def read_raw(self): + """ Reads the next chunk of audio (returns buffer_size provided to open) + It returns the raw data buffers converted to the target rate without any scaling. + """ + if self.wav_file is None: + return None + + data = self.wav_file.readframes(self.buffer_size) + if len(data) == 0: + return None + + if self.actual_rate != self.requested_rate: + # convert the audio to the desired recording rate + data, self.cvstate = audioop.ratecv(data, self.sample_width, self.actual_channels, self.actual_rate, + self.requested_rate, self.cvstate) + + return self.get_requested_channels(data) + + def get_requested_channels(self, data): + if self.requested_channels > self.actual_channels: + raise Exception("Cannot add channels, actual is {}, requested is {}".format( + self.actual_channels, self.requested_channels)) + + if self.requested_channels < self.actual_channels: + data = np.frombuffer(data, dtype=np.int16) + channels = [] + # split into separate channels + for i in range(self.actual_channels): + channels += [data[i::self.actual_channels]] + # drop the channels we don't want + channels = channels[0:self.requested_channels] + # zip the resulting channels back up. + data = np.array(list(zip(*channels))).flatten() + # convert back to packed bytes in PCM 16 format + data = bytes(np.array(data, dtype=np.int16)) + + return data + + def read(self): + """ Reads the next chunk of audio (returns buffer_size provided to open) + It returns the data converted to floating point numbers between -1 and 1, scaled by the range of + values possible for the given audio format. + """ + + # deal with any accumulation of tails, if the tail grows to a full + # buffer then return it! + if self.tail is not None and len(self.tail) >= self.read_size: + data = self.tail[0:self.read_size] + self.tail = self.tail[self.read_size:] + return data + + data = self.read_raw() + if data is None: + return None + + if self.speaker: + self.speaker.write(data) + + data = np.frombuffer(data, dtype=self.dtype).astype(float) + if self.tail is not None: + # we have a tail from previous frame, so prepend it + data = np.concatenate((self.tail, data)) + + # now the caller needs us to stick to our sample_size contract, but when + # rate conversion happens we can't be sure that 'data' is exactly that size. + if len(data) > self.read_size: + # usually one byte extra so add this to our accumulating tail + self.tail = data[self.read_size:] + data = data[0:self.read_size] + + if len(data) < self.read_size: + # might have reached the end of a file, so pad with zeros. + zeros = np.zeros(self.read_size - len(data)) + data = np.concatenate((data, zeros)) + + return data * self.audio_scale_factor + + def close(self): + if self.wav_file: + self.wav_file.close() + self.wav_file = None + + def is_closed(self): + return self.wav_file is None diff --git a/PythonClient/requirements.txt b/PythonClient/requirements.txt new file mode 100644 index 0000000000..1713d945ac --- /dev/null +++ b/PythonClient/requirements.txt @@ -0,0 +1,4 @@ +msgpack-rpc-python +numpy +pyaudio +opencv-contrib-python diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp index 85dea1a5e4..d25a174b68 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp @@ -3,27 +3,26 @@ UnitySensorFactory::UnitySensorFactory(std::string vehicle_name, const NedTransform* ned_transform) { - setActor(vehicle_name, ned_transform); + setActor(vehicle_name, ned_transform); } void UnitySensorFactory::setActor(std::string vehicle_name, const NedTransform* ned_transform) { - vehicle_name_ = vehicle_name; - ned_transform_ = ned_transform; + vehicle_name_ = vehicle_name; + ned_transform_ = ned_transform; } -std::unique_ptr UnitySensorFactory::createSensorFromSettings(const AirSimSettings::SensorSetting * sensor_setting) const +std::shared_ptr UnitySensorFactory::createSensorFromSettings(const AirSimSettings::SensorSetting * sensor_setting) const { - - using SensorBase = msr::airlib::SensorBase; + + using SensorBase = msr::airlib::SensorBase; - switch (sensor_setting->sensor_type) - { - case SensorBase::SensorType::Distance: - return std::unique_ptr(new UnityDistanceSensor(vehicle_name_, ned_transform_)); - default: - return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting); - } + switch (sensor_setting->sensor_type) { + case SensorBase::SensorType::Distance: + return std::make_shared(vehicle_name_, ned_transform_); + default: + return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting); + } - return std::unique_ptr(); + return std::shared_ptr(); } diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h index 08600898e2..1a50ed3e51 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h @@ -8,11 +8,11 @@ using namespace msr::airlib; class UnitySensorFactory : public SensorFactory { public: - UnitySensorFactory(std::string vehicle_name, const NedTransform* ned_transform); - void setActor(std::string vehicle_name, const NedTransform* ned_transform); - virtual std::unique_ptr createSensorFromSettings(const AirSimSettings::SensorSetting* sensor_setting) const override; + UnitySensorFactory(std::string vehicle_name, const NedTransform* ned_transform); + void setActor(std::string vehicle_name, const NedTransform* ned_transform); + virtual std::shared_ptr createSensorFromSettings(const AirSimSettings::SensorSetting* sensor_setting) const override; private: - std::string vehicle_name_; - const NedTransform* ned_transform_; + std::string vehicle_name_; + const NedTransform* ned_transform_; }; diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp index 4686633210..ed87adfdd3 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp @@ -9,17 +9,17 @@ UnrealSensorFactory::UnrealSensorFactory(AActor* actor, const NedTransform* ned_ setActor(actor, ned_transform); } -std::unique_ptr UnrealSensorFactory::createSensorFromSettings( +std::shared_ptr UnrealSensorFactory::createSensorFromSettings( const AirSimSettings::SensorSetting* sensor_setting) const { using SensorBase = msr::airlib::SensorBase; switch (sensor_setting->sensor_type) { case SensorBase::SensorType::Distance: - return std::unique_ptr(new UnrealDistanceSensor( + return std::shared_ptr(new UnrealDistanceSensor( *static_cast(sensor_setting), actor_, ned_transform_)); case SensorBase::SensorType::Lidar: - return std::unique_ptr(new UnrealLidarSensor( + return std::shared_ptr(new UnrealLidarSensor( *static_cast(sensor_setting), actor_, ned_transform_)); default: return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting); diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h index 7034679a76..c49ba7da17 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h @@ -17,7 +17,7 @@ class UnrealSensorFactory : public msr::airlib::SensorFactory { UnrealSensorFactory(AActor* actor, const NedTransform* ned_transform); virtual ~UnrealSensorFactory() {} void setActor(AActor* actor, const NedTransform* ned_transform); - virtual std::unique_ptr createSensorFromSettings( + virtual std::shared_ptr createSensorFromSettings( const AirSimSettings::SensorSetting* sensor_setting) const override; private: diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 164778352f..c715090080 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -180,8 +180,8 @@ bool WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c for (float k = 0; k < ncells_z; k++) { for (float j = 0; j < ncells_y; j++) { int idx = i + ncells_x * (k + ncells_z * j); - FVector position = FVector((i - ncells_x /2) * scale_cm, (j - ncells_y /2) * scale_cm, (k - ncells_z /2) * scale_cm) + position_in_UE_frame; - voxel_grid_[idx] = simmode_->GetWorld()->OverlapBlockingTestByChannel(position, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params); + FVector vposition = FVector((i - ncells_x /2) * scale_cm, (j - ncells_y /2) * scale_cm, (k - ncells_z /2) * scale_cm) + position_in_UE_frame; + voxel_grid_[idx] = simmode_->GetWorld()->OverlapBlockingTestByChannel(vposition, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params); } } } diff --git a/build.cmd b/build.cmd index 61f798ce7f..efc7395e7a 100644 --- a/build.cmd +++ b/build.cmd @@ -42,6 +42,7 @@ echo found Powershell && goto start set powershell=pwsh where pwsh > nul 2>&1 if ERRORLEVEL 1 goto :nopwsh +set PWSHV7=1 echo found pwsh && goto start :nopwsh echo Powershell or pwsh not found, please install it. @@ -64,29 +65,33 @@ if ERRORLEVEL 1 ( REM //---------- get rpclib ---------- IF NOT EXIST external\rpclib mkdir external\rpclib IF NOT EXIST external\rpclib\rpclib-2.2.1 ( - REM //leave some blank lines because %powershell% shows download banner at top of console - ECHO( - ECHO( - ECHO( - ECHO ***************************************************************************************** - ECHO Downloading rpclib - ECHO ***************************************************************************************** - @echo on - %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }" - @echo off - - REM //remove any previous versions - rmdir external\rpclib /q /s - - %powershell% -command "& { Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib }" - del external\rpclib.zip /q - - REM //Don't fail the build if the high-poly car is unable to be downloaded - REM //Instead, just notify users that the gokart will be used. - IF NOT EXIST external\rpclib\rpclib-2.2.1 ( - ECHO Unable to download high-polycount SUV. Your AirSim build will use the default vehicle. - goto :buildfailed - ) + REM //leave some blank lines because %powershell% shows download banner at top of console + ECHO( + ECHO( + ECHO( + ECHO ***************************************************************************************** + ECHO Downloading rpclib + ECHO ***************************************************************************************** + @echo on + if "%PWSHV7%" == "" ( + %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }" + ) else ( + %powershell% -command "iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip" + ) + @echo off + + REM //remove any previous versions + rmdir external\rpclib /q /s + + %powershell% -command "Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib" + del external\rpclib.zip /q + + REM //Don't fail the build if the high-poly car is unable to be downloaded + REM //Instead, just notify users that the gokart will be used. + IF NOT EXIST external\rpclib\rpclib-2.2.1 ( + ECHO Unable to download high-polycount SUV. Your AirSim build will use the default vehicle. + goto :buildfailed + ) ) REM //---------- Build rpclib ------------ @@ -137,10 +142,14 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 ( @echo on REM %powershell% -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" REM %powershell% -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" - %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" + if "%PWSHV7%" == "" ( + %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" + ) else ( + %powershell% -command "iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip" + ) @echo off - rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV - %powershell% -command "& { Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv }" + rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV + %powershell% -command "Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv" rmdir suv_download_tmp /q /s REM //Don't fail the build if the high-poly car is unable to be downloaded @@ -154,9 +163,13 @@ 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.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 }" + if "%PWSHV7%" == "" ( + %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 }" + ) else ( + %powershell% -command "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 mkdir AirLib\deps\eigen3 move AirLib\deps\del_eigen\Eigen AirLib\deps\eigen3\Eigen diff --git a/cmake/cmake-modules/CommonSetup.cmake b/cmake/cmake-modules/CommonSetup.cmake index 22241de7d7..f2da040fc8 100644 --- a/cmake/cmake-modules/CommonSetup.cmake +++ b/cmake/cmake-modules/CommonSetup.cmake @@ -24,12 +24,8 @@ macro(SetupConsoleBuild) endmacro(SetupConsoleBuild) macro(CommonSetup) - message(STATUS "Running CommonSetup...") - find_package(Threads REQUIRED) - - find_path(AIRSIM_ROOT NAMES AirSim.sln PATHS ".." "../.." "../../.." "../../../.." "../../../../.." "../../../../../..") - message(STATUS "found AIRSIM_ROOT=${AIRSIM_ROOT}") + find_path(AIRSIM_ROOT NAMES AirSim.sln PATHS ".." "../.." "../../.." "../../../.." "../../../../.." "../../../../../.." REQUIRED) #setup output paths set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/output/lib) diff --git a/docs/images/px4_debugging.png b/docs/images/px4_debugging.png new file mode 100644 index 0000000000..d4309ce5c6 Binary files /dev/null and b/docs/images/px4_debugging.png differ diff --git a/docs/images/px4_nice.png b/docs/images/px4_nice.png new file mode 100644 index 0000000000..91a1bae612 Binary files /dev/null and b/docs/images/px4_nice.png differ diff --git a/docs/log_viewer.md b/docs/log_viewer.md index 65a98d2556..2934413f71 100644 --- a/docs/log_viewer.md +++ b/docs/log_viewer.md @@ -20,7 +20,7 @@ log files so you can compare the data from each. ### Realtime -You can also get a realtime view if you connect the LogViewer `before` you run the simulation. +You can also get a realtime view if you connect the LogViewer `before` you run the simulation. ![connect](images/log_viewer_connect.png) @@ -38,9 +38,13 @@ For this to work you need to configure the `settings.json` with the following se } } ``` + +Note: do not use the "Logs" setting when you want realtime LogViewer logging. Logging to +a file using "Logs" is mutually exclusive with LogViewer logging. + Simply press the blue connector button on the top right corner of the window, select the Socket -`tab`, enter the port number `14388`, and your `localhost` network. If you are using WSL 2 on -Windows then select `vEthernet (WSL)`. +`tab`, enter the port number `14388`, and your `localhost` network. If you are using WSL 2 on +Windows then select `vEthernet (WSL)`. If you do choose `vEthernet (WSL)` then make sure you also set `LocalHostIp` and `LogViewerHostIp` to the matching WSL ethernet address, something like `172.31.64.1`. @@ -65,3 +69,6 @@ The magic port number 14388 can be configured in the simulator by editing the [s file](settings.md). If you change the port number in LogViewer connection dialog then be sure to make the matching changes in your `settings.json` file. +### Debugging + +See [PX4 Logging](px4_logging.md) for more information on how to use the LogViewer to debug situations you are setting. diff --git a/docs/px4_lockstep.md b/docs/px4_lockstep.md new file mode 100644 index 0000000000..c12d8b554c --- /dev/null +++ b/docs/px4_lockstep.md @@ -0,0 +1,40 @@ +# LockStep + +The latest version of PX4 supports a new [lockstep +feature](https://docs.px4.io/master/en/simulation/#lockstep-simulation) when communicating with the +simulator over TCP. Lockstep is an important feature because it synchronizes PX4 and the simulator +so they essentially use the same clock time. This makes PX4 behave normally even during unusually +long delays in Simulator performance. + +It is recommended that when you are running a lockstep enabled version of PX4 in SITL mode that you +tell AirSim to use a `SteppableClock`, and set `UseTcp` to `true` and `LockStep` to `true`. + +``` + { + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "ClockType": "SteppableClock", + "Vehicles": { + "PX4": { + "VehicleType": "PX4Multirotor", + "UseTcp": true, + "LockStep": true, + ... +``` + +This causes AirSim to not use a "realtime" clock, but instead it advances the clock in step which +each sensor update sent to PX4. This way PX4 thinks time is progressing smoothly no matter how long +it takes AirSim to really process that update loop. + +This has the following advantages: + +- AirSim can be used on slow machines that cannot process updates quickly. +- You can debug AirSim and hit a breakpoint, and when you resume PX4 will behave normally. +- You can enable very slow sensors like the Lidar with large number of simulated points, and PX4 + will still behave normally. + +There will be some side effects to `lockstep`, namely, slower update loops caused by running AirSim +on an underpowered machine or from expensive sensors (like Lidar) will create some visible jerkiness +in the simulated flight if you look at the updates on screen in realtime. + + diff --git a/docs/px4_logging.md b/docs/px4_logging.md index 9c91c1f55d..68a81e7c3e 100644 --- a/docs/px4_logging.md +++ b/docs/px4_logging.md @@ -4,27 +4,33 @@ Thanks to [Chris Lovett](https://github.com/clovett) for developing various tool ## Logging MavLink Messages -The following command will connect MavLinkTest app to the Simulator and enable logging -of all mavlink commands to and from the PX4. -``` -MavLinkTest -server:127.0.0.1:14550 -logdir:d:\temp -``` +AirSim can capture mavlink log files if you add the following to the PX4 section of your `settings.json` file: -Sometimes you will also want to log the "output" from the Simulator that is being sent to the PX4. -Specifically this will capture the "hilgps" and "hilsensor" messages that are generated by the -Simulator. To do that run this as well: -``` -MavLinkTest -server:127.0.0.1:14389 -logdir:d:\temp\output +```json +{ + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "Vehicles": { + "PX4": { + ..., + "Logs": "c:/temp/mavlink" + } + } +} ``` +AirSim will create a timestamped log file in this folder for each "armed/disarmed" flight session. + You will then see log files organized by date in d:\temp\logs, specifically *input.mavlink and *output.mavlink files. ## MavLink LogViewer For MavLink enabled drones, you can also use our [Log Viewer](log_viewer.md) to visualize the streams of data. +If you enable this form of realtime logging you should not use the "Logs" setting above, these two forms of logging +are mutually exclusive. ## PX4 Log in SITL Mode -In SITL mode, please a log file is produced when drone is armed. The SITL terminal will contain the path to the log file, it should look something like this +In SITL mode, please a log file is produced when drone is armed. The SITL terminal will contain the path to the log file, it should look something like this ``` INFO [logger] Opened log file: rootfs/fs/microsd/log/2017-03-27/20_02_49.ulg ``` @@ -33,3 +39,42 @@ INFO [logger] Opened log file: rootfs/fs/microsd/log/2017-03-27/20_02_49.ulg If you are using Pixhawk hardware in HIL mode, then set parameter `SYS_LOGGER=1` using QGroundControl. PX4 will write log file on device which you can download at later date using QGroundControl. + +## Debugging a bad flight + +You can use these *.mavlink log files to debug a bad flight using the [LogViewer](log_viewer.md). +For example, AirSim/PX4 flight may misbehave if you run it on an under powered computer. +The following shows what might happen in that situation. + +![screenshot](images/px4_debugging.png) + +In this flight we ran a simple `commander takeoff` test as performed +by `PythonClient/multirotor/stability_test.py` and the flight started off fine, but then went crazy at the end and the drone crashed. So why is that? What can the log file show? + +Here we've plotted the following 5 metrics: +- `hil_gps.alt` - the simulated altitude sent from AirSim to PX4 +- `telemetry.update_rate` - the rate AirSim is performing the critical drone update loop in updates per second. +- `telemetry.update_time` - the average time taken inside AirSim performing the critical drone update loop. +- `telemetry.actuation_delay` - this is a very interesting metric measuring how long it takes PX4 to send back updated actuator controls message (motor power) +- `actuator_controls.0` - the actuator controls signal from PX4 for the first rotor. + +What we see then with these metrics is that things started off nicely, with nice flat altitude, high update rate in the 275 to 300 fps range, and a nice low update time inside AirSim around 113 microseconds, and a nice low actuation delay in the round trip from PX4. The actuator controls also stabilize quickly to a nice flat line. + +But then the update_time starts to climb, at the same time the actuation_delay climbs and we see a little tip in actuator_controls. +This dip should not happen, the PX4 is panicking over loss of update rate but it recovers. + +But then we see actuator controls go crazy, a huge spike in actuation delay, and around this time we see a message from AirSim saying `lockstep disabled`. A delay over 100 millisecond triggers AirSim into jumping out of lockstep mode and the PX4 goes nuts and the drone crashes. + +The button line is that if a simple `takeoff` cannot maintain steady smooth flight and you see these kinds of spikes and uneven update rates +then it means you are running AirSim on a computer that does not have enough horsepower. + +This is what a simple takeoff and hover and land should look like: + +![nice](images/px4_nice.png) + +Here you see the `update_rate` sticking the target of 333 updates per second. +You also see the `update_time` a nice flat 39 microseconds and the `actuator_delay` +somewhere between 1.1 and 1.7 milliseconds, and the resulting `actuator_controls` +a lovely flat line. + + diff --git a/docs/px4_setup.md b/docs/px4_setup.md index 4d26a23668..b1ba3e454d 100644 --- a/docs/px4_setup.md +++ b/docs/px4_setup.md @@ -2,7 +2,7 @@ The [PX4 software stack](http://github.com/px4/firmware) is an open source very popular flight controller with support for wide variety of boards and sensors as well as built-in capability for higher level tasks such as mission planning. Please visit [px4.io](http://px4.io) for more information. -**Warning**: While all releases of AirSim are always tested with PX4 to ensure the support, setting up PX4 is not a trivial task. Unless you have at least intermediate level of experience with PX4 stack, we recommend you use [simple_flight](simple_flight.md), which is now a default in AirSim. +**Warning**: While all releases of AirSim are always tested with PX4 to ensure the support, setting up PX4 is not a trivial task. Unless you have at least intermediate level of experience with PX4 stack, we recommend you use [simple_flight](simple_flight.md), which is now a default in AirSim. ## Supported Hardware @@ -26,7 +26,7 @@ For this you will need one of the supported device listed above. For manual flig 3. Use QGroundControl to flash the latest PX4 Flight Stack. See also [initial firmware setup video](https://docs.px4.io/master/en/config/). 4. In QGroundControl, configure your Pixhawk for HIL simulation by selecting the HIL Quadrocopter X airframe. After PX4 reboots, check that "HIL Quadrocopter X" is indeed selected. -5. In QGroundControl, go to Radio tab and calibrate (make sure the remote control is on and the receiver is showing the indicator for the binding). +5. In QGroundControl, go to Radio tab and calibrate (make sure the remote control is on and the receiver is showing the indicator for the binding). 6. Go to the Flight Mode tab and chose one of the remote control switches as "Mode Channel". Then set (for example) Stabilized and Attitude flight modes for two positions of the switch. 7. Go to the Tuning section of QGroundControl and set appropriate values. For example, for Fly Sky's FS-TH9X remote control, the following settings give a more realistic feel: Hover Throttle = mid+1 mark, Roll and pitch sensitivity = mid-3 mark, Altitude and position control sensitivity = mid-2 mark. 8. In [AirSim settings](settings.md) file, specify PX4 for your vehicle config like this: @@ -34,10 +34,19 @@ See also [initial firmware setup video](https://docs.px4.io/master/en/config/). { "SettingsVersion": 1.2, "SimMode": "Multirotor", + "ClockType": "SteppableClock", "Vehicles": { "PX4": { "VehicleType": "PX4Multirotor", "UseSerial": true, + "LockStep": true, + "Sensors":{ + "Barometer":{ + "SensorType": 1, + "Enabled": true, + "PressureFactorSigma": 0.0001825 + } + }, "Parameters": { "NAV_RCL_ACT": 0, "NAV_DLL_ACT": 0, @@ -50,6 +59,11 @@ See also [initial firmware setup video](https://docs.px4.io/master/en/config/). } ``` +See [PX4 LockStep](px4_lockstep.md) for more information. + +The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much +noise generation. This setting clamps that down a bit. + After above setup you should be able to use RC to fly with AirSim. You can usually arm the vehicle by lowering and bringing two sticks of RC together in-wards. You don't need QGroundControl after the initial setup. Typically the Stabilized (instead of Manual) mode gives better experience for beginners. See [PX4 Basic Flying Guide](https://docs.px4.io/master/en/flying/basic_flying.html). You can also control the drone from [Python APIs](apis.md). @@ -63,7 +77,7 @@ The PX4 SITL mode doesn't require you to have separate device such as a Pixhawk #### Drone doesn't fly properly, it just goes "crazy". -There are a few reasons that can cause this. First, make sure your drone doesn't fall down large distance when starting the simulator. This might happen if you have created a custom Unreal environment and Player Start is placed too high above the ground. It seems that when this happens internal calibration in PX4 gets confused. +There are a few reasons that can cause this. First, make sure your drone doesn't fall down large distance when starting the simulator. This might happen if you have created a custom Unreal environment and Player Start is placed too high above the ground. It seems that when this happens internal calibration in PX4 gets confused. You should [also use QGroundControl](#setting-up-px4-hardware-in-loop) and make sure you can arm and takeoff in QGroundControl properly. @@ -76,15 +90,15 @@ PX4 custom modes in the MAV_CMD_DO_SET_MODE messages (like PX4_CUSTOM_MAIN_MODE_ #### It is not finding my Pixhawk hardware -Check your settings.json file for this line "SerialPort":"*,115200". The asterisk here means "find any +Check your settings.json file for this line "SerialPort":"*,115200". The asterisk here means "find any serial port that looks like a Pixhawk device, but this doesn't always work for all types of Pixhawk hardware. -So on Windows you can find the actual COM port using Device Manager, look under "Ports (COM & LPT), plug the -device in and see what new COM port shows up. Let's say you see a new port named "USB Serial Port (COM5)". -Well, then change the SerialPort setting to this: "SerialPort":"COM5,115200". +So on Windows you can find the actual COM port using Device Manager, look under "Ports (COM & LPT), plug the +device in and see what new COM port shows up. Let's say you see a new port named "USB Serial Port (COM5)". +Well, then change the SerialPort setting to this: "SerialPort":"COM5,115200". On Linux, the device can be found by running "ls /dev/serial/by-id" if you see a device name listed that looks like this `usb-3D_Robotics_PX4_FMU_v2.x_0-if00` then you can use that name to connect, like this: -"SerialPort":"/dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00". Note this long name is actually a symbolic link to the real +"SerialPort":"/dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00". Note this long name is actually a symbolic link to the real name, if you use "ls -l ..." you can find that symbolic link, it is usually something like "/dev/ttyACM0", so this will also work "SerialPort":"/dev/ttyACM0,115200". But that mapping is similar to windows, it is automatically assigned and can change, whereas the long name will work even if the actual TTY serial device diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md index 903b1af45c..0fa2b8ce40 100644 --- a/docs/px4_sitl.md +++ b/docs/px4_sitl.md @@ -4,7 +4,10 @@ The [PX4](http://dev.px4.io) software provides a "software-in-loop" simulation ( their stack that runs in Linux. If you are on Windows then you can use the [Cygwin Toolchain](https://dev.px4.io/master/en/setup/dev_env_windows_cygwin.html) or you can use the [Windows subsystem for Linux](https://docs.microsoft.com/en-us/windows/wsl/install-win10) and follow -the PX4 Linux toolchain setup. +the PX4 Linux toolchain setup. + +If you are using WSL2 please read these [additional +instructions](px4_sitl_wsl2.md). **Note** that every time you stop the unreal app you have to restart the `px4` app. @@ -19,7 +22,7 @@ the PX4 Linux toolchain setup. mkdir -p PX4 cd PX4 git clone https://github.com/PX4/PX4-Autopilot.git --recursive - bash ./PX4-Autopilot/Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools + bash ./PX4-Autopilot/Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools cd PX4-Autopilot ``` And find the latest stable release from [https://github.com/PX4/PX4-Autopilot/releases](https://github.com/PX4/PX4-Autopilot/releases) @@ -53,13 +56,22 @@ The default ports have changed recently, so check them closely to make sure AirS { "SettingsVersion": 1.2, "SimMode": "Multirotor", + "ClockType": "SteppableClock", "Vehicles": { "PX4": { "VehicleType": "PX4Multirotor", "UseSerial": false, + "LockStep": true, "UseTcp": true, "TcpPort": 4560, "ControlPort": 14580, + "Sensors":{ + "Barometer":{ + "SensorType": 1, + "Enabled": true, + "PressureFactorSigma": 0.0001825 + } + }, "Parameters": { "NAV_RCL_ACT": 0, "NAV_DLL_ACT": 0, @@ -72,6 +84,9 @@ The default ports have changed recently, so check them closely to make sure AirS } ``` Notice the PX4 `[simulator]` is using TCP, which is why we need to add: `"UseTcp": true,`. + See [PX4 LockStep](px4_lockstep.md) for more information. + The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much + noise generation. This setting clamps that down a bit. 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 @@ -154,6 +169,11 @@ Local position: x=-0.0326988, y=0.00656854, z=5.48506 If the z coordinate is large like this then takeoff might not work as expected. Resetting the SITL and simulation should fix that problem. +## WSL 2 + +Windows Subsystem for Linux version 2 operates in a Virtual Machine. This requires +additional setup - see [additional instructions](px4_sitl_wsl2.md). + ## No Remote Control Notice the above setting is provided in the `params` section of the `settings.json` file: diff --git a/docs/px4_sitl_wsl2.md b/docs/px4_sitl_wsl2.md new file mode 100644 index 0000000000..4627caae28 --- /dev/null +++ b/docs/px4_sitl_wsl2.md @@ -0,0 +1,108 @@ +# PX4 Software-in-Loop with WSL 2 + +The [Windows subsystem for Linux version +2](https://docs.microsoft.com/en-us/windows/wsl/install-win10) uses a Virtual Machine which has a +separate IP address from your Windows host machine. This means PX4 cannot find AirSim using +"localhost" which is the default behavior for PX4. + +You will notice that on Windows `ipconfig` returns a new ethernet adapter for WSL like this (notice +the vEthernet has `(WSL)` in the name: + +```plain +Ethernet adapter vEthernet (WSL): + + Connection-specific DNS Suffix . : + Link-local IPv6 Address . . . . . : fe80::1192:f9a5:df88:53ba%44 + IPv4 Address. . . . . . . . . . . : 172.31.64.1 + Subnet Mask . . . . . . . . . . . : 255.255.240.0 + Default Gateway . . . . . . . . . : +``` + +This address `172.31.64.1` is the address that WSL 2 can use to reach your Windows host machine. + +Starting with this [PX4 Change +Request](https://github.com/PX4/PX4-Autopilot/commit/1719ff9892f3c3d034f2b44e94d15527ab09cec6) PX4 +in SITL mode can now connect to AirSim on a different (remote) IP address. To enable this make sure +you have a version of PX4 containing this fix and set the following environment variable in linux: + +```shell +export PX4_SIM_HOST_ADDR=172.31.64.1 +``` + +**Note:** Be sure to update the above address `172.31.64.1` to match what you see from your +`ipconfig` command. + +Open incoming port 4560 using your Windows Firewall settings. + +Now on the linux side run `ip address show` and copy the `eth0 inet` address, it should be something +like `172.31.66.156`. This is the address Windows needs to know in order to find PX4. + +Edit your [AirSim settings](settings.md) file and add `LocalHostIp` to tell AirSim to use the WSL +ethernet adapter address instead of the default `localhost`. This will cause AirSim to open the TCP +port on that adapter which is the address that the PX4 app will be looking for. Also tell AirSim +to connect the `ControlIp` UDP channel by setting `ControlIp` to the magic string `remote`. +This resolves to the WSL 2 remote ip address found in the TCP socket. + +```json +{ + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "ClockType": "SteppableClock", + "Vehicles": { + "PX4": { + "VehicleType": "PX4Multirotor", + "UseSerial": false, + "LockStep": true, + "UseTcp": true, + "TcpPort": 4560, + "ControlIp": "remote", + "ControlPort": 14580, + "LocalHostIp": "172.31.64.1", + "Sensors":{ + "Barometer":{ + "SensorType": 1, + "Enabled": true, + "PressureFactorSigma": 0.0001825 + } + }, + "Parameters": { + "NAV_RCL_ACT": 0, + "NAV_DLL_ACT": 0, + "COM_OBL_ACT": 1, + "LPE_LAT": 47.641468, + "LPE_LON": -122.140165 + } + } + } +} +``` +See [PX4 LockStep](px4_lockstep.md) for more information. +The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much +noise generation. This setting clamps that down a bit. + +Lastly, please edit the Linux file in `ROMFS/px4fmu_common/init.d-posix/rcS` and make sure +it is looking for the `PX4_SIM_HOST_ADDR` environment variable and is passing that through to the +PX4 simulator like this: + +```shell +# If PX4_SIM_HOST_ADDR environment variable is empty use localhost. +if [ -z "${PX4_SIM_HOST_ADDR}" ]; then + echo "PX4 SIM HOST: localhost" + simulator start -c $simulator_tcp_port +else + echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" + simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port +fi +``` + +**Note:** this code might already be there depending on the version of PX4 you are using. + +**Note:** please be patient when waiting for the message: + +``` +INFO [simulator] Simulator connected on TCP port 4560. +``` + +It can take a little longer to establish the remote connection than it does with `localhost`. + +Now you can proceed with the steps shown in [Setting up PX4 Software-in-Loop](px4_sitl.md). diff --git a/docs/sensors.md b/docs/sensors.md index ffbab3053b..e9b3d2c9e0 100644 --- a/docs/sensors.md +++ b/docs/sensors.md @@ -38,39 +38,84 @@ The default sensor list can be configured in settings json: ```json "DefaultSensors": { "Barometer": { - "SensorType": 1, - "Enabled" : true + "SensorType": 1, + "Enabled" : true, + "PressureFactorSigma": 0.001825, + "PressureFactorTau": 3600, + "UncorrelatedNoiseSigma": 2.7, + "UpdateLatency": 0, + "UpdateFrequency": 50, + "StartupDelay": 0 + }, "Imu": { - "SensorType": 2, - "Enabled" : true + "SensorType": 2, + "Enabled" : true, + "AngularRandomWalk": 0.3, + "GyroBiasStabilityTau": 500, + "GyroBiasStability": 4.6, + "VelocityRandomWalk": 0.24, + "AccelBiasStabilityTau": 800, + "AccelBiasStability": 36 }, "Gps": { - "SensorType": 3, - "Enabled" : true + "SensorType": 3, + "Enabled" : true, + "EphTimeConstant": 0.9, + "EpvTimeConstant": 0.9, + "EphInitial": 25, + "EpvInitial": 25, + "EphFinal": 0.1, + "EpvFinal": 0.1, + "EphMin3d": 3, + "EphMin2d": 4, + "UpdateLatency": 0.2, + "UpdateFrequency": 50, + "StartupDelay": 1 }, "Magnetometer": { - "SensorType": 4, - "Enabled" : true + "SensorType": 4, + "Enabled" : true, + "NoiseSigma": 0.005, + "ScaleFactor": 1, + "NoiseBias": 0, + "UpdateLatency": 0, + "UpdateFrequency": 50, + "StartupDelay": 0 }, "Distance": { - "SensorType": 5, - "Enabled" : true + "SensorType": 5, + "Enabled" : true, + "MinDistance": 0.2, + "MaxDistance": 40, + "X": 0, "Y": 0, "Z": -1, + "Yaw": 0, "Pitch": 0, "Roll": 0, + "DrawDebugPoints": false }, "Lidar2": { - "SensorType": 6, - "Enabled" : true, - "NumberOfChannels": 4, - "PointsPerSecond": 10000 + "SensorType": 6, + "Enabled" : true, + "NumberOfChannels": 16, + "RotationsPerSecond": 10, + "PointsPerSecond": 100000, + "X": 0, "Y": 0, "Z": -1, + "Roll": 0, "Pitch": 0, "Yaw" : 0, + "VerticalFOVUpper": -15, + "VerticalFOVLower": -25, + "HorizontalFOVStart": -20, + "HorizontalFOVEnd": 20, + "DrawDebugPoints": true, + "DataFrame": "SensorLocalFrame" } }, ``` ## 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. -A vehicle specific sensor list can be specified in the vehicle settings part of the json. -e.g., +A vehicle can override a subset of the default sensors listed above. A Lidar and Distance sensor are +not added to a vehicle by default, so those you need to add this way. Each sensor must have a valid +"SensorType" and a subset of the properties can be defined that override the default values shown +above and you can set Enabled to false to disable a specific type of sensor. ```json "Vehicles": { @@ -80,6 +125,11 @@ e.g., "AutoCreate": true, ... "Sensors": { + "Barometer":{ + "SensorType": 1, + "Enabled": true, + "PressureFactorSigma": 0.0001825 + }, "MyLidar1": { "SensorType": 6, "Enabled" : true, @@ -125,7 +175,7 @@ Be default, the points hit by distance sensor are not drawn on the viewport. To ## 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 +### Barometer ```cpp msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name, const std::string& vehicle_name); ``` @@ -134,7 +184,7 @@ msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer barometer_data = client.getBarometerData(barometer_name = "", vehicle_name = "") ``` -##### IMU +### IMU ```cpp msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = ""); ``` @@ -143,7 +193,7 @@ msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const imu_data = client.getImuData(imu_name = "", vehicle_name = "") ``` -##### GPS +### GPS ```cpp msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = ""); ``` @@ -151,7 +201,7 @@ msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const gps_data = client.getGpsData(gps_name = "", vehicle_name = "") ``` -##### Magnetometer +### Magnetometer ```cpp msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = ""); ``` @@ -159,7 +209,7 @@ msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& mag magnetometer_data = client.getMagnetometerData(magnetometer_name = "", vehicle_name = "") ``` -##### Distance sensor +### Distance sensor ```cpp msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = ""); ``` @@ -167,5 +217,5 @@ msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distanc distance_sensor_data = client.getDistanceSensorData(distance_sensor_name = "", vehicle_name = "") ``` -##### Lidar +### Lidar See the [lidar page](lidar.md) for Lidar API. diff --git a/mkdocs.yml b/mkdocs.yml index 1b0ddb1b75..37f096a79f 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -86,6 +86,8 @@ nav: - "MavLink and PX4": - "PX4 Setup for AirSim": 'px4_setup.md' - "PX4 in SITL": 'px4_sitl.md' + - "PX4 SITL with WSL 2": 'px4_sitl_wsl2.md' + - "PX4 Lockstep": 'px4_lockstep.md' - "PX4 Multi-vehicle in SITL": 'px4_multi_vehicle.md' - "AirSim with Pixhawk": 'https://youtu.be/1oY8Qu5maQQ' - "PX4 Setup with AirSim": 'https://youtu.be/HNWdYrtw3f0' 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 9d51cae4b1..58aa4c2573 100644 --- a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -9,6 +9,7 @@ STRICT_MODE_ON #include "airsim_settings_parser.h" #include "common/AirSimSettings.hpp" #include "common/common_utils/FileSystem.hpp" +#include "sensors/lidar/LidarSimpleParams.hpp" #include "ros/ros.h" #include "sensors/imu/ImuBase.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" @@ -93,18 +94,18 @@ struct VelCmd msr::airlib::YawMode yaw_mode; std::string vehicle_name; - // VelCmd() : - // x(0), y(0), z(0), + // VelCmd() : + // x(0), y(0), z(0), // vehicle_name("") {drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; // yaw_mode = msr::airlib::YawMode();}; - // VelCmd(const double& x, const double& y, const double& z, - // msr::airlib::DrivetrainType drivetrain, + // VelCmd(const double& x, const double& y, const double& z, + // msr::airlib::DrivetrainType drivetrain, // const msr::airlib::YawMode& yaw_mode, - // const std::string& vehicle_name) : - // x(x), y(y), z(z), - // drivetrain(drivetrain), - // yaw_mode(yaw_mode), + // const std::string& vehicle_name) : + // x(x), y(y), z(z), + // drivetrain(drivetrain), + // yaw_mode(yaw_mode), // vehicle_name(vehicle_name) {}; }; @@ -116,9 +117,9 @@ struct GimbalCmd // GimbalCmd() : vehicle_name(vehicle_name), camera_name(camera_name), target_quat(msr::airlib::Quaternionr(1,0,0,0)) {} - // GimbalCmd(const std::string& vehicle_name, - // const std::string& camera_name, - // const msr::airlib::Quaternionr& target_quat) : + // GimbalCmd(const std::string& vehicle_name, + // const std::string& camera_name, + // const msr::airlib::Quaternionr& target_quat) : // vehicle_name(vehicle_name), camera_name(camera_name), target_quat(target_quat) {}; }; @@ -132,7 +133,7 @@ class AirsimROSWrapper }; AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string & host_ip); - ~AirsimROSWrapper() {}; + ~AirsimROSWrapper() {}; void initialize_airsim(); void initialize_ros(); @@ -163,17 +164,17 @@ class AirsimROSWrapper ros::Publisher global_gps_pub; ros::Publisher env_pub; airsim_ros_pkgs::Environment env_msg; - std::vector sensor_pubs; + 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 @@ -261,13 +262,13 @@ class AirsimROSWrapper 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(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_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& 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; @@ -316,7 +317,7 @@ class AirsimROSWrapper 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 + msr::airlib::GeoPoint origin_geo_point_;// gps coord of unreal origin airsim_ros_pkgs::GPSYaw origin_geo_point_msg_; // todo duplicate AirSimSettingsParser airsim_settings_parser_; @@ -343,7 +344,7 @@ class AirsimROSWrapper // gimbal control bool has_gimbal_cmd_; - GimbalCmd gimbal_cmd_; + GimbalCmd gimbal_cmd_; /// ROS tf const std::string AIRSIM_FRAME_ID = "world_ned"; @@ -353,7 +354,7 @@ class AirsimROSWrapper 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_; @@ -368,7 +369,7 @@ class AirsimROSWrapper typedef std::pair, std::string> airsim_img_request_vehicle_name_pair; std::vector airsim_img_request_vehicle_name_pair_vec_; - std::vector image_pub_vec_; + std::vector image_pub_vec_; std::vector cam_info_pub_vec_; std::vector camera_info_msg_vec_; 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 855dae597e..94aca385cc 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -26,10 +26,10 @@ const std::unordered_map AirsimROSWrapper::image_type_int_to_s { 7, "Infrared" } }; -AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip) : - nh_(nh), +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_ + 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 host_ip_(host_ip), airsim_client_images_(host_ip), @@ -51,7 +51,7 @@ AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHan airsim_mode_ = AIRSIM_MODE::CAR; ROS_INFO("Setting ROS wrapper to CAR mode"); } - + initialize_ros(); std::cout << "AirsimROSWrapper Initialized!\n"; @@ -62,7 +62,7 @@ void AirsimROSWrapper::initialize_airsim() // todo do not reset if already in air? try { - + if (airsim_mode_ == AIRSIM_MODE::DRONE) { airsim_client_ = std::unique_ptr(new msr::airlib::MultirotorRpcLibClient(host_ip_)); @@ -82,7 +82,7 @@ void AirsimROSWrapper::initialize_airsim() } 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? + // 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_); } catch (rpc::rpc_error& e) @@ -120,7 +120,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() // subscribe to control commands on global nodehandle gimbal_angle_quat_cmd_sub_ = nh_private_.subscribe("gimbal_angle_quat_cmd", 50, &AirsimROSWrapper::gimbal_angle_quat_cmd_cb, this); gimbal_angle_euler_cmd_sub_ = nh_private_.subscribe("gimbal_angle_euler_cmd", 50, &AirsimROSWrapper::gimbal_angle_euler_cmd_cb, this); - origin_geo_point_pub_ = nh_private_.advertise("origin_geo_point", 10); + origin_geo_point_pub_ = nh_private_.advertise("origin_geo_point", 10); airsim_img_request_vehicle_name_pair_vec_.clear(); image_pub_vec_.clear(); @@ -137,9 +137,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() auto& vehicle_setting = curr_vehicle_elem.second; auto curr_vehicle_name = curr_vehicle_elem.first; set_nans_to_zeros_in_pose(*vehicle_setting); - + std::unique_ptr vehicle_ros = nullptr; - + if (airsim_mode_ == AIRSIM_MODE::DRONE) { vehicle_ros = std::unique_ptr(new MultiRotorROS()); @@ -153,7 +153,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() 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); @@ -163,17 +163,17 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() 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, + // 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, + 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", + 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", + 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); } @@ -223,7 +223,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() camera_info_msg_vec_.push_back(generate_cam_info(curr_camera_name, camera_setting, capture_setting)); } } - // push back pair (vector of image captures, current vehicle name) + // push back pair (vector of image captures, current vehicle name) airsim_img_request_vehicle_name_pair_vec_.push_back(std::make_pair(current_image_request_vec, curr_vehicle_name)); } @@ -236,7 +236,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() auto& sensor_setting = curr_sensor_map.second; if (sensor_setting->enabled) - { + { SensorPublisher sensor_publisher; sensor_publisher.sensor_name = sensor_setting->sensor_name; sensor_publisher.sensor_type = sensor_setting->sensor_type; @@ -244,7 +244,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() { case SensorBase::SensorType::Barometer: { - std::cout << "Barometer" << std::endl; + std::cout << "Barometer" << std::endl; sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/altimeter/" + sensor_name, 10); break; } @@ -256,19 +256,19 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() } case SensorBase::SensorType::Gps: { - std::cout << "Gps" << std::endl; + 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; + 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; + std::cout << "Distance" << std::endl; sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/distance/" + sensor_name, 10); break; } @@ -276,8 +276,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() { 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); + msr::airlib::LidarSimpleParams params; + params.initializeFromSettings(lidar_setting); + append_static_lidar_tf(vehicle_ros.get(), sensor_name, params); sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/lidar/" + sensor_name, 10); break; } @@ -338,34 +339,34 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() 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; + is_used_img_timer_cb_queue_ = true; } // 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_); 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); + airsim_lidar_update_timer_ = nh_private_.createTimer(timer_options); is_used_lidar_timer_cb_queue_ = true; } initialize_airsim(); } -// todo: error check. if state is not landed, return error. +// 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_); if (request.waitOnLastTask) - static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? - // response.success = + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + // response.success = else static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); - // response.success = + // response.success = return true; } @@ -376,13 +377,13 @@ bool AirsimROSWrapper::takeoff_group_srv_cb(airsim_ros_pkgs::TakeoffGroup::Reque if (request.waitOnLastTask) for(const auto& vehicle_name : request.vehicle_names) - static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? - // response.success = + 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) static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); - // response.success = - + // response.success = + return true; } @@ -392,12 +393,12 @@ bool AirsimROSWrapper::takeoff_all_srv_cb(airsim_ros_pkgs::Takeoff::Request& req if (request.waitOnLastTask) 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 = + 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_ptr_pair : vehicle_name_ptr_map_) static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first); - // response.success = + // response.success = return true; } @@ -459,12 +460,12 @@ tf2::Quaternion AirsimROSWrapper::get_tf2_quat(const msr::airlib::Quaternionr& a msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const geometry_msgs::Quaternion& geometry_msgs_quat) const { - return msr::airlib::Quaternionr(geometry_msgs_quat.w, geometry_msgs_quat.x, geometry_msgs_quat.y, geometry_msgs_quat.z); + return msr::airlib::Quaternionr(geometry_msgs_quat.w, geometry_msgs_quat.x, geometry_msgs_quat.y, geometry_msgs_quat.z); } msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const tf2::Quaternion& tf2_quat) const { - return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(), tf2_quat.z()); + 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) @@ -494,7 +495,7 @@ void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd::Cons 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(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw @@ -519,7 +520,7 @@ void AirsimROSWrapper::vel_cmd_group_body_frame_cb(const airsim_ros_pkgs::VelCmd 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? 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 @@ -572,7 +573,7 @@ void AirsimROSWrapper::vel_cmd_world_frame_cb(const airsim_ros_pkgs::VelCmd::Con drone->has_vel_cmd = true; } -// this is kinda unnecessary but maybe it makes life easier for the end user. +// 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_); @@ -621,7 +622,7 @@ void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAng gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd); // airsim uses wxyz gimbal_cmd_.camera_name = gimbal_angle_quat_cmd_msg.camera_name; gimbal_cmd_.vehicle_name = gimbal_angle_quat_cmd_msg.vehicle_name; - has_gimbal_cmd_ = true; + has_gimbal_cmd_ = true; } catch (tf2::TransformException& ex) { @@ -632,7 +633,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 setCameraPose which sets camera pose 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 @@ -643,7 +644,7 @@ void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAn gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd); gimbal_cmd_.camera_name = gimbal_angle_euler_cmd_msg.camera_name; gimbal_cmd_.vehicle_name = gimbal_angle_euler_cmd_msg.vehicle_name; - has_gimbal_cmd_ = true; + has_gimbal_cmd_ = true; } catch (tf2::TransformException& ex) { @@ -691,7 +692,7 @@ nav_msgs::Odometry AirsimROSWrapper::get_odom_msg_from_car_state(const msr::airl { 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); + 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; @@ -725,7 +726,7 @@ nav_msgs::Odometry AirsimROSWrapper::get_odom_msg_from_multirotor_state(const ms { 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); + 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; @@ -751,8 +752,8 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr:: lidar_msg.width = lidar_data.point_cloud.size() / 3; lidar_msg.fields.resize(3); - lidar_msg.fields[0].name = "x"; - lidar_msg.fields[1].name = "y"; + lidar_msg.fields[0].name = "x"; + lidar_msg.fields[1].name = "y"; lidar_msg.fields[2].name = "z"; int offset = 0; @@ -828,8 +829,8 @@ sensor_msgs::MagneticField AirsimROSWrapper::get_mag_msg_from_airsim(const msr:: 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::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); @@ -842,12 +843,12 @@ sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_msg_from_airsim(const msr::airl 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.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 = + // gps_msg.position_covariance_type = + // gps_msg.position_covariance = return gps_msg; } @@ -890,7 +891,7 @@ sensor_msgs::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::Im imu_msg.angular_velocity.y = imu_data.angular_velocity.y(); imu_msg.angular_velocity.z = imu_data.angular_velocity.z(); - // meters/s2^m + // meters/s2^m imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x(); imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y(); imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z(); @@ -906,7 +907,7 @@ void AirsimROSWrapper::publish_odom_tf(const nav_msgs::Odometry& odom_msg) { geometry_msgs::TransformStamped odom_tf; odom_tf.header = odom_msg.header; - odom_tf.child_frame_id = odom_msg.child_frame_id; + 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; @@ -921,7 +922,7 @@ airsim_ros_pkgs::GPSYaw AirsimROSWrapper::get_gps_msg_from_airsim_geo_point(cons { airsim_ros_pkgs::GPSYaw gps_msg; gps_msg.latitude = geo_point.latitude; - gps_msg.longitude = geo_point.longitude; + gps_msg.longitude = geo_point.longitude; gps_msg.altitude = geo_point.altitude; return gps_msg; } @@ -930,7 +931,7 @@ sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_sensor_msg_from_airsim_geo_poin { sensor_msgs::NavSatFix gps_msg; gps_msg.latitude = geo_point.latitude; - gps_msg.longitude = geo_point.longitude; + gps_msg.longitude = geo_point.longitude; gps_msg.altitude = geo_point.altitude; return gps_msg; } @@ -955,7 +956,7 @@ ros::Time AirsimROSWrapper::airsim_timestamp_to_ros(const msr::airlib::TTimePoin void AirsimROSWrapper::drone_state_timer_cb(const ros::TimerEvent& event) { try - { + { // todo this is global origin origin_geo_point_pub_.publish(origin_geo_point_msg_); @@ -1013,7 +1014,7 @@ ros::Time AirsimROSWrapper::update_state() for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { ros::Time vehicle_time; - // get drone state from airsim + // get drone state from airsim auto& vehicle_ros = vehicle_name_ptr_pair.second; // vehicle environment, we can get ambient temperature here and other truths @@ -1054,20 +1055,20 @@ ros::Time AirsimROSWrapper::update_state() 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; } 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 + // 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; @@ -1108,7 +1109,7 @@ void AirsimROSWrapper::publish_vehicle_state() 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); + sensor_publisher.publisher.publish(alt_msg); break; } case SensorBase::SensorType::Imu: @@ -1169,7 +1170,7 @@ void AirsimROSWrapper::update_commands() 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_, + 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; @@ -1187,7 +1188,7 @@ void AirsimROSWrapper::update_commands() } } - // Only camera rotation, no translation movement of camera + // Only camera rotation, no translation movement of camera if (has_gimbal_cmd_) { std::lock_guard guard(drone_control_mutex_); @@ -1197,7 +1198,7 @@ void AirsimROSWrapper::update_commands() has_gimbal_cmd_ = false; } -// airsim uses nans for zeros in settings.json. we set them to zeros here for handling tfs in ROS +// airsim uses nans for zeros in settings.json. we set them to zeros here for handling tfs in ROS void AirsimROSWrapper::set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const { if (std::isnan(vehicle_setting.position.x())) @@ -1242,27 +1243,6 @@ void AirsimROSWrapper::set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_s } -void AirsimROSWrapper::set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, LidarSetting& lidar_setting) const -{ - if (std::isnan(lidar_setting.position.x())) - lidar_setting.position.x() = vehicle_setting.position.x(); - - if (std::isnan(lidar_setting.position.y())) - lidar_setting.position.y() = vehicle_setting.position.y(); - - if (std::isnan(lidar_setting.position.z())) - lidar_setting.position.z() = vehicle_setting.position.z(); - - if (std::isnan(lidar_setting.rotation.yaw)) - lidar_setting.rotation.yaw = vehicle_setting.rotation.yaw; - - if (std::isnan(lidar_setting.rotation.pitch)) - lidar_setting.rotation.pitch = vehicle_setting.rotation.pitch; - - if (std::isnan(lidar_setting.rotation.roll)) - lidar_setting.rotation.roll = vehicle_setting.rotation.roll; -} - void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting) { geometry_msgs::TransformStamped vehicle_tf_msg; @@ -1271,7 +1251,7 @@ void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const V 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(); @@ -1290,20 +1270,18 @@ void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const V vehicle_ros->static_tf_msg_vec.emplace_back(vehicle_tf_msg); } -void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const LidarSetting& lidar_setting) +void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& lidar_setting) { geometry_msgs::TransformStamped lidar_tf_msg; 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(); - tf2::Quaternion quat; - quat.setRPY(lidar_setting.rotation.roll, lidar_setting.rotation.pitch, lidar_setting.rotation.yaw); - lidar_tf_msg.transform.rotation.x = quat.x(); - lidar_tf_msg.transform.rotation.y = quat.y(); - lidar_tf_msg.transform.rotation.z = quat.z(); - lidar_tf_msg.transform.rotation.w = quat.w(); + lidar_tf_msg.transform.translation.x = lidar_setting.relative_pose.position.x(); + lidar_tf_msg.transform.translation.y = lidar_setting.relative_pose.position.y(); + lidar_tf_msg.transform.translation.z = lidar_setting.relative_pose.position.z(); + lidar_tf_msg.transform.rotation.x = lidar_setting.relative_pose.orientation.x(); + lidar_tf_msg.transform.rotation.y = lidar_setting.relative_pose.orientation.y(); + lidar_tf_msg.transform.rotation.z = lidar_setting.relative_pose.orientation.z(); + lidar_tf_msg.transform.rotation.w = lidar_setting.relative_pose.orientation.w(); if (isENU_) { @@ -1345,11 +1323,11 @@ void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const st tf2::Quaternion quat_cam_body; tf2::Quaternion quat_cam_optical; tf2::convert(static_cam_tf_body_msg.transform.rotation, quat_cam_body); - tf2::Matrix3x3 mat_cam_body(quat_cam_body); + tf2::Matrix3x3 mat_cam_body(quat_cam_body); tf2::Matrix3x3 mat_cam_optical; - mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), + mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(), - mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ()); + mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ()); mat_cam_optical.getRotation(quat_cam_optical); quat_cam_optical.normalize(); tf2::convert(quat_cam_optical, static_cam_tf_optical_msg.transform.rotation); @@ -1359,7 +1337,7 @@ void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const st } void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event) -{ +{ try { int image_response_idx = 0; @@ -1367,11 +1345,11 @@ void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event) { const std::vector& img_response = airsim_client_images_.simGetImages(airsim_img_request_vehicle_name_pair.first, airsim_img_request_vehicle_name_pair.second); - if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size()) + if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size()) { process_and_publish_img_response(img_response, image_response_idx, airsim_img_request_vehicle_name_pair.second); image_response_idx += img_response.size(); - } + } } } @@ -1384,7 +1362,7 @@ void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event) } void AirsimROSWrapper::lidar_timer_cb(const ros::TimerEvent& event) -{ +{ try { for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) @@ -1420,7 +1398,7 @@ 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 ros::Time curr_ros_time, const std::string frame_id) { sensor_msgs::ImagePtr img_msg_ptr = boost::make_shared(); @@ -1441,7 +1419,7 @@ sensor_msgs::ImagePtr AirsimROSWrapper::get_depth_img_msg_from_response(const Im const ros::Time curr_ros_time, const std::string frame_id) { - // todo using img_response.image_data_float direclty as done get_img_msg_from_response() throws an error, + // 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(); @@ -1462,28 +1440,28 @@ sensor_msgs::CameraInfo AirsimROSWrapper::generate_cam_info(const std::string& c float f_x = (capture_setting.width / 2.0) / tan(math_common::deg2rad(capture_setting.fov_degrees / 2.0)); // todo focal length in Y direction should be same as X it seems. this can change in future a scene capture component which exactly correponds to a cine camera // float f_y = (capture_setting.height / 2.0) / tan(math_common::deg2rad(fov_degrees / 2.0)); - cam_info_msg.K = {f_x, 0.0, capture_setting.width / 2.0, - 0.0, f_x, capture_setting.height / 2.0, + cam_info_msg.K = {f_x, 0.0, capture_setting.width / 2.0, + 0.0, f_x, capture_setting.height / 2.0, 0.0, 0.0, 1.0}; cam_info_msg.P = {f_x, 0.0, capture_setting.width / 2.0, 0.0, - 0.0, f_x, capture_setting.height / 2.0, 0.0, + 0.0, f_x, capture_setting.height / 2.0, 0.0, 0.0, 0.0, 1.0, 0.0}; return cam_info_msg; } void AirsimROSWrapper::process_and_publish_img_response(const std::vector& img_response_vec, const int img_response_idx, const std::string& vehicle_name) -{ +{ // todo add option to use airsim time (image_response.TTimePoint) like Gazebo /use_sim_time param - ros::Time curr_ros_time = ros::Time::now(); + ros::Time curr_ros_time = ros::Time::now(); int img_response_idx_internal = img_response_idx; for (const auto& curr_img_response : img_response_vec) { - // todo publishing a tf for each capture type seems stupid. but it foolproofs us against render thread's async stuff, I hope. - // Ideally, we should loop over cameras and then captures, and publish only one tf. + // todo publishing a tf for each capture type seems stupid. but it foolproofs us against render thread's async stuff, I hope. + // Ideally, we should loop over cameras and then captures, and publish only one tf. publish_camera_tf(curr_img_response, curr_ros_time, vehicle_name, curr_img_response.camera_name); - // todo simGetCameraInfo is wrong + also it's only for image type -1. + // todo simGetCameraInfo is wrong + also it's only for image type -1. // msr::airlib::CameraInfo camera_info = airsim_client_.simGetCameraInfo(curr_img_response.camera_name); // update timestamp of saved cam info msgs @@ -1493,15 +1471,15 @@ void AirsimROSWrapper::process_and_publish_img_response(const std::vector