diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj index dfc5a90477..7d466390ef 100644 --- a/AirLib/AirLib.vcxproj +++ b/AirLib/AirLib.vcxproj @@ -172,7 +172,6 @@ - diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters index 84d8165819..f2d972b15c 100644 --- a/AirLib/AirLib.vcxproj.filters +++ b/AirLib/AirLib.vcxproj.filters @@ -485,11 +485,5 @@ Source Files - - Source Files - - - Source Files - \ No newline at end of file diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 58491b9d1f..bd35b1c4ff 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -100,11 +100,10 @@ class FastPhysicsEngine : public PhysicsEngineBase { body.kinematicsUpdated(); - ////// (FIXME this is from PhysicsBody, where it's commented out - it appears that we need it here to ensure that GPS sensing functions properly) //TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence - body.getEnvironment().setPosition(next.pose.position); - body.getEnvironment().update(); - ////// (End FIXME) + //with below commented out - Arducopter GPS may not work. + //body.getEnvironment().setPosition(next.pose.position); + //body.getEnvironment().update(); } diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp index 00287bf0d5..e4d3d319ba 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp @@ -4,26 +4,41 @@ #ifndef msr_airlib_vehicles_MultiRotorParamsFactory_hpp #define msr_airlib_vehicles_MultiRotorParamsFactory_hpp -#include "vehicles/multirotor/MultiRotorParams.hpp" -#include "common/AirSimSettings.hpp" -#include "sensors/SensorFactory.hpp" +#include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp" +#include "vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp" +#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp" +#include "vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp" namespace msr { namespace airlib { class MultiRotorParamsFactory { public: - - static void reset(); - - static std::unique_ptr createConfig(const AirSimSettings::VehicleSetting* vehicle_setting, - std::shared_ptr sensor_factory); - -private: - - // Simple zero-based ID for ArduCopterSolo vehicles - static int next_solo_id_; - + static std::unique_ptr createConfig(const AirSimSettings::VehicleSetting* vehicle_setting, + std::shared_ptr sensor_factory) + { + std::unique_ptr config; + + if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePX4) { + config.reset(new Px4MultiRotorParams(*static_cast(vehicle_setting), + sensor_factory)); + } + else if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduCopterSolo) { + config.reset(new ArduCopterSoloParams(*static_cast(vehicle_setting), sensor_factory)); + } + else if (vehicle_setting->vehicle_type == "" || //default config + vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight) { + config.reset(new SimpleFlightQuadXParams(vehicle_setting, sensor_factory)); + } + else + throw std::runtime_error(Utils::stringf( + "Cannot create vehicle config because vehicle name '%s' is not recognized", + vehicle_setting->vehicle_name.c_str())); + + config->initialize(); + + return config; + } }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp index 32980d0f6e..6cec6a25fa 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp @@ -12,10 +12,10 @@ namespace msr { namespace airlib { class ArduCopterSoloParams : public MultiRotorParams { public: - ArduCopterSoloParams(const AirSimSettings::MavLinkVehicleSetting& vehicle_settings, std::shared_ptr sensor_factory, int instance_id) + ArduCopterSoloParams(const AirSimSettings::MavLinkVehicleSetting& vehicle_settings, std::shared_ptr sensor_factory) : sensor_factory_(sensor_factory) { - connection_info_ = getConnectionInfo(vehicle_settings, instance_id); + connection_info_ = getConnectionInfo(vehicle_settings); } virtual ~ArduCopterSoloParams() = default; @@ -74,13 +74,9 @@ namespace msr { namespace airlib { computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); } - static const AirSimSettings::MavLinkConnectionInfo getConnectionInfo(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting, int instance_id) + static const AirSimSettings::MavLinkConnectionInfo getConnectionInfo(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting) { AirSimSettings::MavLinkConnectionInfo result = vehicle_setting.connection_info; - - result.sitl_ip_port += 10 * instance_id; - result.ip_port += 10 * instance_id; - return result; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index a286cd949c..c105672210 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -1178,6 +1178,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase 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_; diff --git a/AirLib/src/vehicles/multirotor/MultiRotorParamsFactory.cpp b/AirLib/src/vehicles/multirotor/MultiRotorParamsFactory.cpp deleted file mode 100644 index 004cb4494d..0000000000 --- a/AirLib/src/vehicles/multirotor/MultiRotorParamsFactory.cpp +++ /dev/null @@ -1,47 +0,0 @@ -//in header only mode, control library is not available -#ifndef AIRLIB_HEADER_ONLY - -#include "vehicles/multirotor/MultiRotorParamsFactory.hpp" -#include "vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp" -#include "vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp" -#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp" - -namespace msr { namespace airlib { - - int MultiRotorParamsFactory::next_solo_id_ = 0; - - void MultiRotorParamsFactory::reset() - { - next_solo_id_ = 0; - } - - std::unique_ptr MultiRotorParamsFactory::createConfig(const AirSimSettings::VehicleSetting* vehicle_setting, - std::shared_ptr sensor_factory) - { - std::unique_ptr config; - - if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePX4) { - config.reset(new Px4MultiRotorParams(*static_cast(vehicle_setting), - sensor_factory)); - } - else if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduCopterSolo) { - config.reset(new ArduCopterSoloParams(*static_cast(vehicle_setting), sensor_factory, next_solo_id_)); - next_solo_id_++; - } - else if (vehicle_setting->vehicle_type == "" || //default config - vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight) { - config.reset(new SimpleFlightQuadXParams(vehicle_setting, sensor_factory)); - } - else - throw std::runtime_error(Utils::stringf( - "Cannot create vehicle config because vehicle name '%s' is not recognized", - vehicle_setting->vehicle_name.c_str())); - - config->initialize(); - - return config; - } - } -} - -#endif /* AIRLIB_HEADER_ONLY */ diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index a3634bcc30..fde22ff6b0 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -7,7 +7,6 @@ #include "Kismet/GameplayStatics.h" #include "Misc/OutputDeviceNull.h" #include "Engine/World.h" -#include "vehicles/multirotor/MultiRotorParamsFactory.hpp" #include #include "AirBlueprintLib.h" @@ -438,8 +437,6 @@ void ASimModeBase::setupVehiclesAndCamera() camera_director_position_uu); initializeCameraDirector(camera_transform, camera_director_setting.follow_distance); - MultiRotorParamsFactory::reset(); - //find all vehicle pawns { TArray pawns;