From e5494b12d6b007b8436afa2b6fb021969f8ef326 Mon Sep 17 00:00:00 2001 From: Seth Nielsen Date: Wed, 29 Sep 2021 17:30:17 -0600 Subject: [PATCH 1/7] Markdown TOC improvements --- FORK.md | 12 ++++++++++-- README.md | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/FORK.md b/FORK.md index f747857..30004cf 100644 --- a/FORK.md +++ b/FORK.md @@ -1,5 +1,13 @@ # Maintaining the Fork: [byu-magicc/AirSim](https://github.com/byu-magicc/AirSim) +### Contents + +- [Rationale](#rationale) +- [Updating the AirSim Fork With Changes to vtol-AirSim](#updating-the-airsim-fork-with-changes-to-vtol-airsim) +- [Merging Upstream Commits](#merging-upstream-commits) + - [After the Merge](#after-the-merge-copying-from-the-airsim-fork-to-vtol-airsim) + +## Rationale ### Why have 2 repositories instead of just one fork of AirSim? The layout of the AirSim repository is cumbersome for development. It is a large monorepo where interdependent code is scattered around in places that do not reflect where the code needs to be at compile time, and this makes the reading, writing, and testing of the AirSim source code awkward. @@ -26,8 +34,8 @@ When you run this script, the source code files inside the following directories - `vtol-AirSim/Content/VTOL/Tiltrotor --> $AIRSIMPATH/Unreal/Plugins/AirSim/Content/VTOL/Tiltrotor` - `vtol-AirSim/Source/AirLib --> $AIRSIMPATH/AirLib` -## Merging Upstream Commits From microsoft/AirSim to byu-magicc/AirSim -If new commits have been pushed to the base AirSim repo and you want to merge those commits into our fork of AirSim, then here is a guide on how to do that. +## Merging Upstream Commits +If there are new upstream commits - i.e., the AirSim developers have pushed new commits to the base repo [microsoft/AirSim](https://github.com/microsoft/AirSim) - and you want to merge those commits into our fork of AirSim, then here is a guide on how to do that. You'll need to make some commits, so make sure your code is in a "presentable" state, i.e. you've removed the 55 debugging print statements that you sprinkled all over the place, it builds, and (preferably) it has been tested and works. 1. Recommended: make a new temporary branch on the AirSim fork. diff --git a/README.md b/README.md index 67d3c05..4edfe29 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ A copy-fork of [AirSim](https://github.com/microsoft/AirSim)'s Unreal Engine plu ### Quick links -* [Setup](#setup) (this page) +* [Setup](#setup-instructions) (this page) * [Development](DEVELOPMENT.md) * [Maintaining the byu-magicc/AirSim Fork](FORK.md) From 6070a8ea8c44ee554ba958cb2c6f81edd67f16bd Mon Sep 17 00:00:00 2001 From: Seth Nielsen Date: Tue, 12 Oct 2021 15:19:26 -0600 Subject: [PATCH 2/7] Rename Tiltrotor to Vtol everywhere --- .../AirLib/include/common/AirSimSettings.hpp | 38 ++--- .../sensors/lidar/LidarSimpleParams.hpp | 6 +- .../tiltrotor/api/TiltrotorRpcLibClient.hpp | 83 ----------- .../tiltrotor/api/TiltrotorRpcLibServer.hpp | 31 ---- .../TiltrotorSimpleEstimator.hpp | 107 -------------- .../vehicles/{tiltrotor => vtol}/AeroBody.hpp | 0 .../{tiltrotor => vtol}/AeroBodyParams.hpp | 6 +- .../AeroBodyParamsFactory.hpp | 12 +- .../{tiltrotor => vtol}/AeroParams.hpp | 0 .../{tiltrotor => vtol}/AeroVertex.hpp | 2 +- .../{tiltrotor => vtol}/RotorTiltable.hpp | 0 .../RotorTiltableParams.hpp | 0 .../api/VtolApiBase.hpp} | 22 +-- .../api/VtolCommon.hpp} | 10 +- .../api/VtolRpcLibAdaptors.hpp} | 20 +-- .../vehicles/vtol/api/VtolRpcLibClient.hpp | 83 +++++++++++ .../vehicles/vtol/api/VtolRpcLibServer.hpp | 31 ++++ .../mavlink/MavLinkOutputMappings.hpp | 0 .../firmwares/mavlink/MavLinkVtolApi.hpp} | 32 ++--- .../firmwares/mavlink/Px4VtolParams.hpp} | 16 +-- .../firmwares/vtol_simple/VtolSimpleApi.hpp} | 136 +++++++++--------- .../vtol_simple/VtolSimpleBoard.hpp} | 10 +- .../vtol_simple/VtolSimpleCommLink.hpp} | 10 +- .../vtol_simple/VtolSimpleCommon.hpp} | 26 ++-- .../vtol_simple/VtolSimpleEstimator.hpp | 107 ++++++++++++++ .../vtol_simple/VtolSimpleParams.hpp} | 18 +-- .../firmware/AdaptiveController.hpp | 6 +- .../firmware/AngleLevelController.hpp | 2 +- .../firmware/AngleRateController.hpp | 2 +- .../firmware/CascadeController.hpp | 2 +- .../firmware/ConstantOutputController.hpp | 2 +- .../firmware/DoNothingController.hpp | 2 +- .../vtol_simple}/firmware/Firmware.hpp | 2 +- .../firmwares/vtol_simple}/firmware/Mixer.hpp | 2 +- .../vtol_simple}/firmware/OffboardApi.hpp | 2 +- .../vtol_simple}/firmware/Params.hpp | 2 +- .../firmware/PassthroughController.hpp | 2 +- .../vtol_simple}/firmware/PidController.hpp | 2 +- .../firmware/PositionController.hpp | 2 +- .../vtol_simple}/firmware/RemoteControl.hpp | 2 +- .../firmware/RungKuttaPidIntegrator.hpp | 2 +- .../firmware/StdPidIntegrator.hpp | 2 +- .../firmware/VelocityController.hpp | 2 +- .../firmware/interfaces/CommonStructs.hpp | 2 +- .../firmware/interfaces/IAxisController.hpp | 2 +- .../firmware/interfaces/IBoard.hpp | 2 +- .../firmware/interfaces/IBoardClock.hpp | 2 +- .../firmware/interfaces/IBoardInputPins.hpp | 2 +- .../firmware/interfaces/IBoardOutputPins.hpp | 2 +- .../firmware/interfaces/IBoardSensors.hpp | 2 +- .../firmware/interfaces/ICommLink.hpp | 2 +- .../firmware/interfaces/IController.hpp | 2 +- .../firmware/interfaces/IFirmware.hpp | 2 +- .../firmware/interfaces/IGoal.hpp | 2 +- .../firmware/interfaces/IOffboardApi.hpp | 2 +- .../firmware/interfaces/IPidIntegrator.hpp | 2 +- .../firmware/interfaces/IStateEstimator.hpp | 2 +- .../firmware/interfaces/IUpdatable.hpp | 2 +- .../api/VtolApiBase.cpp} | 122 ++++++++-------- .../api/VtolRpcLibClient.cpp} | 86 +++++------ .../api/VtolRpcLibServer.cpp} | 30 ++-- Source/SimHUD/SimHUD.cpp | 2 +- .../Tiltrotor/SimModeWorldTiltrotor.cpp | 16 +-- .../Tiltrotor/TiltrotorPawnSimApi.cpp | 4 +- .../Vehicles/Tiltrotor/TiltrotorPawnSimApi.h | 8 +- 65 files changed, 571 insertions(+), 571 deletions(-) delete mode 100644 Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibClient.hpp delete mode 100644 Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibServer.hpp delete mode 100644 Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleEstimator.hpp rename Source/AirLib/include/vehicles/{tiltrotor => vtol}/AeroBody.hpp (100%) rename Source/AirLib/include/vehicles/{tiltrotor => vtol}/AeroBodyParams.hpp (97%) rename Source/AirLib/include/vehicles/{tiltrotor => vtol}/AeroBodyParamsFactory.hpp (73%) rename Source/AirLib/include/vehicles/{tiltrotor => vtol}/AeroParams.hpp (100%) rename Source/AirLib/include/vehicles/{tiltrotor => vtol}/AeroVertex.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor => vtol}/RotorTiltable.hpp (100%) rename Source/AirLib/include/vehicles/{tiltrotor => vtol}/RotorTiltableParams.hpp (100%) rename Source/AirLib/include/vehicles/{tiltrotor/api/TiltrotorApiBase.hpp => vtol/api/VtolApiBase.hpp} (97%) rename Source/AirLib/include/vehicles/{tiltrotor/api/TiltrotorCommon.hpp => vtol/api/VtolCommon.hpp} (94%) rename Source/AirLib/include/vehicles/{tiltrotor/api/TiltrotorRpcLibAdaptors.hpp => vtol/api/VtolRpcLibAdaptors.hpp} (84%) create mode 100644 Source/AirLib/include/vehicles/vtol/api/VtolRpcLibClient.hpp create mode 100644 Source/AirLib/include/vehicles/vtol/api/VtolRpcLibServer.hpp rename Source/AirLib/include/vehicles/{tiltrotor => vtol}/firmwares/mavlink/MavLinkOutputMappings.hpp (100%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/mavlink/MavLinkTiltrotorApi.hpp => vtol/firmwares/mavlink/MavLinkVtolApi.hpp} (98%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/mavlink/Px4TiltrotorParams.hpp => vtol/firmwares/mavlink/Px4VtolParams.hpp} (74%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleApi.hpp => vtol/firmwares/vtol_simple/VtolSimpleApi.hpp} (74%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleBoard.hpp => vtol/firmwares/vtol_simple/VtolSimpleBoard.hpp} (94%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleCommLink.hpp => vtol/firmwares/vtol_simple/VtolSimpleCommLink.hpp} (80%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleCommon.hpp => vtol/firmwares/vtol_simple/VtolSimpleCommon.hpp} (72%) create mode 100644 Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleEstimator.hpp rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleParams.hpp => vtol/firmwares/vtol_simple/VtolSimpleParams.hpp} (55%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/AdaptiveController.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/AngleLevelController.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/AngleRateController.hpp (98%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/CascadeController.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/ConstantOutputController.hpp (97%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/DoNothingController.hpp (97%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/Firmware.hpp (96%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/Mixer.hpp (98%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/OffboardApi.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/Params.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/PassthroughController.hpp (97%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/PidController.hpp (95%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/PositionController.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/RemoteControl.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/RungKuttaPidIntegrator.hpp (98%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/StdPidIntegrator.hpp (97%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/VelocityController.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/CommonStructs.hpp (99%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IAxisController.hpp (95%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IBoard.hpp (92%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IBoardClock.hpp (84%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IBoardInputPins.hpp (89%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IBoardOutputPins.hpp (90%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IBoardSensors.hpp (88%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/ICommLink.hpp (93%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IController.hpp (93%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IFirmware.hpp (91%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IGoal.hpp (88%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IOffboardApi.hpp (96%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IPidIntegrator.hpp (92%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IStateEstimator.hpp (95%) rename Source/AirLib/include/vehicles/{tiltrotor/firmwares/tiltrotor_simple => vtol/firmwares/vtol_simple}/firmware/interfaces/IUpdatable.hpp (96%) rename Source/AirLib/src/vehicles/{tiltrotor/api/TiltrotorApiBase.cpp => vtol/api/VtolApiBase.cpp} (83%) rename Source/AirLib/src/vehicles/{tiltrotor/api/TiltrotorRpcLibClient.cpp => vtol/api/VtolRpcLibClient.cpp} (63%) rename Source/AirLib/src/vehicles/{tiltrotor/api/TiltrotorRpcLibServer.cpp => vtol/api/VtolRpcLibServer.cpp} (85%) diff --git a/Source/AirLib/include/common/AirSimSettings.hpp b/Source/AirLib/include/common/AirSimSettings.hpp index aa886a3..7bae9dd 100644 --- a/Source/AirLib/include/common/AirSimSettings.hpp +++ b/Source/AirLib/include/common/AirSimSettings.hpp @@ -29,10 +29,10 @@ namespace airlib public: //types static constexpr int kSubwindowCount = 3; //must be >= 3 for now static constexpr char const* kVehicleTypePX4 = "px4multirotor"; - static constexpr char const* kVehicleTypePX4Tiltrotor = "px4tiltrotor"; + static constexpr char const* kVehicleTypePX4Vtol = "px4vtol"; static constexpr char const* kVehicleTypeArduCopterSolo = "arducoptersolo"; static constexpr char const* kVehicleTypeSimpleFlight = "simpleflight"; - static constexpr char const* kVehicleTypeTiltrotorSimple = "tiltrotorsimple"; + static constexpr char const* kVehicleTypeVtolSimple = "vtolsimple"; static constexpr char const* kVehicleTypeArduCopter = "arducopter"; static constexpr char const* kVehicleTypePhysXCar = "physxcar"; static constexpr char const* kVehicleTypeArduRover = "ardurover"; @@ -42,7 +42,7 @@ namespace airlib static constexpr char const* kSensorLocalFrame = "SensorLocalFrame"; static constexpr char const* kSimModeTypeMultirotor = "Multirotor"; - static constexpr char const* kSimModeTypeTiltrotor = "Tiltrotor"; + static constexpr char const* kSimModeTypeVtol = "Vtol"; static constexpr char const* kSimModeTypeCar = "Car"; static constexpr char const* kSimModeTypeComputerVision = "ComputerVision"; @@ -582,7 +582,7 @@ namespace airlib physics_engine_name = settings_json.getString("PhysicsEngineName", ""); if (physics_engine_name == "") { - if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeTiltrotor) + if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeVtol) physics_engine_name = "FastPhysicsEngine"; else physics_engine_name = "PhysX"; //this value is only informational for now @@ -599,7 +599,7 @@ namespace airlib std::string view_mode_string = settings_json.getString("ViewMode", ""); if (view_mode_string == "") { - if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeTiltrotor) + if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeVtol) view_mode_string = "FlyWithMe"; else if (simmode_name == kSimModeTypeComputerVision) view_mode_string = "Fpv"; @@ -793,12 +793,12 @@ namespace airlib auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", "")); std::unique_ptr vehicle_setting; - if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypePX4Tiltrotor || vehicle_type == kVehicleTypeArduCopterSolo || vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover) + if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypePX4Vtol || 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 || vehicle_type == kVehicleTypeTiltrotorSimple) { + if (vehicle_type == kVehicleTypeSimpleFlight || vehicle_type == kVehicleTypeVtolSimple) { //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; @@ -853,15 +853,15 @@ namespace airlib simple_flight_setting->sensors = sensor_defaults; vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting); } - else if (simmode_name == kSimModeTypeTiltrotor) { - // create tiltrotor simple flight as default tiltrotor - auto tiltrotor_simple_setting = std::unique_ptr(new VehicleSetting("TiltrotorSimple", - kVehicleTypeTiltrotorSimple)); + else if (simmode_name == kSimModeTypeVtol) { + // create vtol simple flight as default vtol + auto vtol_simple_setting = std::unique_ptr(new VehicleSetting("VtolSimple", + kVehicleTypeVtolSimple)); // TODO: we should be selecting remote if available else keyboard // currently keyboard is not supported so use rc as default - tiltrotor_simple_setting->rc.remote_control_id = 0; - tiltrotor_simple_setting->sensors = sensor_defaults; - vehicles[tiltrotor_simple_setting->vehicle_name] = std::move(tiltrotor_simple_setting); + vtol_simple_setting->rc.remote_control_id = 0; + vtol_simple_setting->sensors = sensor_defaults; + vehicles[vtol_simple_setting->vehicle_name] = std::move(vtol_simple_setting); } else if (simmode_name == kSimModeTypeCar) { // create PhysX as default car vehicle @@ -916,7 +916,7 @@ namespace airlib PawnPath("Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'")); pawn_paths.emplace("DefaultComputerVision", PawnPath("Class'/AirSim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'")); - pawn_paths.emplace("DefaultTiltrotor", + pawn_paths.emplace("DefaultVtol", PawnPath("Class'/AirSim/VTOL/Tiltrotor/Blueprints/BP_TiltrotorPawn.BP_TiltrotorPawn_C'")); } @@ -1214,7 +1214,7 @@ namespace airlib clock_type = "ScalableClock"; //override if multirotor simmode with simple_flight - if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeTiltrotor) { + if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeVtol) { //TODO: this won't work if simple_flight and PX4 is combined together! //for multirotors we select steppable fixed interval clock unless we have @@ -1223,7 +1223,7 @@ namespace airlib for (auto const& vehicle : vehicles) { if (vehicle.second->auto_create && (vehicle.second->vehicle_type == kVehicleTypePX4 || - vehicle.second->vehicle_type == kVehicleTypePX4Tiltrotor)) { + vehicle.second->vehicle_type == kVehicleTypePX4Vtol)) { clock_type = "ScalableClock"; break; } @@ -1325,12 +1325,12 @@ namespace airlib static void createDefaultSensorSettings(const std::string& simmode_name, std::map>& sensors) { - if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeTiltrotor) { + if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeVtol) { 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); - if (simmode_name == kSimModeTypeTiltrotor) { + if (simmode_name == kSimModeTypeVtol) { sensors["airspeed"] = createSensorSetting(SensorBase::SensorType::Airspeed, "airspeed", true); } } diff --git a/Source/AirLib/include/sensors/lidar/LidarSimpleParams.hpp b/Source/AirLib/include/sensors/lidar/LidarSimpleParams.hpp index a9f2b50..53d4c3c 100644 --- a/Source/AirLib/include/sensors/lidar/LidarSimpleParams.hpp +++ b/Source/AirLib/include/sensors/lidar/LidarSimpleParams.hpp @@ -61,7 +61,7 @@ namespace airlib // By default, for multirotors the lidars FOV point downwards; // for cars, the lidars FOV is more forward facing. if (std::isnan(vertical_FOV_upper)) { - if (simmode_name == AirSimSettings::kSimModeTypeMultirotor || simmode_name == AirSimSettings::kSimModeTypeTiltrotor) + if (simmode_name == AirSimSettings::kSimModeTypeMultirotor || simmode_name == AirSimSettings::kSimModeTypeVtol) vertical_FOV_upper = -15; else vertical_FOV_upper = +10; @@ -69,7 +69,7 @@ namespace airlib vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", Utils::nan()); if (std::isnan(vertical_FOV_lower)) { - if (simmode_name == AirSimSettings::kSimModeTypeMultirotor || simmode_name == AirSimSettings::kSimModeTypeTiltrotor) + if (simmode_name == AirSimSettings::kSimModeTypeMultirotor || simmode_name == AirSimSettings::kSimModeTypeVtol) vertical_FOV_lower = -45; else vertical_FOV_lower = -10; @@ -86,7 +86,7 @@ namespace airlib if (std::isnan(relative_pose.position.y())) relative_pose.position.y() = 0; if (std::isnan(relative_pose.position.z())) { - if (simmode_name == AirSimSettings::kSimModeTypeMultirotor || simmode_name == AirSimSettings::kSimModeTypeTiltrotor) + if (simmode_name == AirSimSettings::kSimModeTypeMultirotor || simmode_name == AirSimSettings::kSimModeTypeVtol) relative_pose.position.z() = 0; else relative_pose.position.z() = -1; // a little bit above for cars diff --git a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibClient.hpp b/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibClient.hpp deleted file mode 100644 index 7d8db43..0000000 --- a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibClient.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef air_TiltrotorRpcLibClient_hpp -#define air_TiltrotorRpcLibClient_hpp - -#include "common/Common.hpp" -#include -#include "common/CommonStructs.hpp" -#include "common/ImageCaptureBase.hpp" -#include "vehicles/tiltrotor/api/TiltrotorApiBase.hpp" -#include "api/RpcLibClientBase.hpp" -#include "vehicles/tiltrotor/api/TiltrotorCommon.hpp" -#include "vehicles/tiltrotor/api/TiltrotorCommon.hpp" - -namespace msr -{ -namespace airlib -{ - - class TiltrotorRpcLibClient : public RpcLibClientBase - { - public: - TiltrotorRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); - - TiltrotorRpcLibClient* takeoffAsync(float timeout_sec = 20, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); - - TiltrotorRpcLibClient* moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByVelocityZBodyFrameAsync(float vx, float vy, float z, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveOnPathAsync(const vector& path, float velocity, float timeout_sec = Utils::max(), - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), - float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec = Utils::max(), - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), - float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveToZAsync(float z, float velocity, float timeout_sec = Utils::max(), - const YawMode& yaw_mode = YawMode(), float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* rotateToYawAsync(float yaw, float timeout_sec = Utils::max(), float margin = 5, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* hoverAsync(const std::string& vehicle_name = ""); - - TiltrotorRpcLibClient* setTiltrotorPose(Pose pose, const vector& tilt_angles, bool ignore_collision, const std::string& vehicle_name = ""); - TiltrotorRpcLibClient* moveByMotorPWMsAsync(const vector& pwm_values, float duration, const std::string& vehicle_name = ""); - - void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); - void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); - void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); - void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); - void moveByRC(const RCData& rc_data, const std::string& vehicle_name = ""); - - TiltrotorState getTiltrotorState(const std::string& vehicle_name = ""); - RotorTiltableStates getRotorStates(const std::string& vehicle_name = ""); - - bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, - float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name = ""); - - virtual TiltrotorRpcLibClient* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan()) override; - - virtual ~TiltrotorRpcLibClient(); //required for pimpl - - private: - struct impl; - std::unique_ptr pimpl_; - }; -} -} //namespace -#endif diff --git a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibServer.hpp b/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibServer.hpp deleted file mode 100644 index d8ee2f3..0000000 --- a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibServer.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef air_TiltrotorRpcLibServer_hpp -#define air_TiltrotorRpcLibServer_hpp - -#include "common/Common.hpp" -#include -#include "api/RpcLibServerBase.hpp" -#include "vehicles/tiltrotor/api/TiltrotorApiBase.hpp" - -namespace msr -{ -namespace airlib -{ - - class TiltrotorRpcLibServer : public RpcLibServerBase - { - public: - TiltrotorRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = RpcLibPort); - virtual ~TiltrotorRpcLibServer(); - - protected: - virtual TiltrotorApiBase* getVehicleApi(const std::string& vehicle_name) override - { - return static_cast(RpcLibServerBase::getVehicleApi(vehicle_name)); - } - }; -} -} //namespace -#endif diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleEstimator.hpp b/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleEstimator.hpp deleted file mode 100644 index cc6d25f..0000000 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleEstimator.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef msr_airlib_TiltrotorSimpleEstimator_hpp -#define msr_airlib_TiltrotorSimpleEstimator_hpp - -#include "firmware/interfaces/CommonStructs.hpp" -#include "TiltrotorSimpleCommon.hpp" -#include "physics/Kinematics.hpp" -#include "physics/Environment.hpp" -#include "common/Common.hpp" - -namespace msr -{ -namespace airlib -{ - - class TiltrotorSimpleEstimator : public tiltrotor_simple::IStateEstimator - { - public: - virtual ~TiltrotorSimpleEstimator() {} - - //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors) - void setGroundTruthKinematics(const Kinematics::State* kinematics, const Environment* environment) - { - kinematics_ = kinematics; - environment_ = environment; - } - - virtual tiltrotor_simple::Axis3r getAngles() const override - { - tiltrotor_simple::Axis3r angles; - VectorMath::toEulerianAngle(kinematics_->pose.orientation, - angles.pitch(), - angles.roll(), - angles.yaw()); - - //Utils::log(Utils::stringf("Ang Est:\t(%f, %f, %f)", angles.pitch(), angles.roll(), angles.yaw())); - - return angles; - } - - virtual tiltrotor_simple::Axis3r getAngularVelocity() const override - { - const auto& anguler = kinematics_->twist.angular; - - tiltrotor_simple::Axis3r conv; - conv.x() = anguler.x(); - conv.y() = anguler.y(); - conv.z() = anguler.z(); - - return conv; - } - - virtual tiltrotor_simple::Axis3r getPosition() const override - { - return TiltrotorSimpleCommon::toAxis3r(kinematics_->pose.position); - } - - virtual tiltrotor_simple::Axis3r transformToBodyFrame(const tiltrotor_simple::Axis3r& world_frame_val) const override - { - const Vector3r& vec = TiltrotorSimpleCommon::toVector3r(world_frame_val); - const Vector3r& trans = VectorMath::transformToBodyFrame(vec, kinematics_->pose.orientation); - return TiltrotorSimpleCommon::toAxis3r(trans); - } - - virtual tiltrotor_simple::Axis3r getLinearVelocity() const override - { - return TiltrotorSimpleCommon::toAxis3r(kinematics_->twist.linear); - } - - virtual tiltrotor_simple::Axis4r getOrientation() const override - { - return TiltrotorSimpleCommon::toAxis4r(kinematics_->pose.orientation); - } - - virtual tiltrotor_simple::GeoPoint getGeoPoint() const override - { - return TiltrotorSimpleCommon::toSimpleFlightGeoPoint(environment_->getState().geo_point); - } - - virtual tiltrotor_simple::GeoPoint getHomeGeoPoint() const override - { - return TiltrotorSimpleCommon::toSimpleFlightGeoPoint(environment_->getHomeGeoPoint()); - } - - virtual tiltrotor_simple::KinematicsState getKinematicsEstimated() const override - { - tiltrotor_simple::KinematicsState state; - state.position = getPosition(); - state.orientation = getOrientation(); - state.linear_velocity = getLinearVelocity(); - state.angular_velocity = getAngularVelocity(); - state.linear_acceleration = TiltrotorSimpleCommon::toAxis3r(kinematics_->accelerations.linear); - state.angular_acceleration = TiltrotorSimpleCommon::toAxis3r(kinematics_->accelerations.angular); - - return state; - } - - private: - const Kinematics::State* kinematics_; - const Environment* environment_; - }; - -} -} //namespace -#endif diff --git a/Source/AirLib/include/vehicles/tiltrotor/AeroBody.hpp b/Source/AirLib/include/vehicles/vtol/AeroBody.hpp similarity index 100% rename from Source/AirLib/include/vehicles/tiltrotor/AeroBody.hpp rename to Source/AirLib/include/vehicles/vtol/AeroBody.hpp diff --git a/Source/AirLib/include/vehicles/tiltrotor/AeroBodyParams.hpp b/Source/AirLib/include/vehicles/vtol/AeroBodyParams.hpp similarity index 97% rename from Source/AirLib/include/vehicles/tiltrotor/AeroBodyParams.hpp rename to Source/AirLib/include/vehicles/vtol/AeroBodyParams.hpp index 5fe2f8f..298788d 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/AeroBodyParams.hpp +++ b/Source/AirLib/include/vehicles/vtol/AeroBodyParams.hpp @@ -9,7 +9,7 @@ #include "AeroParams.hpp" #include "sensors/SensorCollection.hpp" #include "sensors/SensorFactory.hpp" -#include "vehicles/tiltrotor/api/TiltrotorApiBase.hpp" +#include "vehicles/vtol/api/VtolApiBase.hpp" namespace msr { @@ -57,7 +57,7 @@ namespace airlib virtual const SensorFactory* getSensorFactory() const = 0; public: //interface - virtual std::unique_ptr createTiltrotorApi() = 0; + virtual std::unique_ptr createVtolApi() = 0; virtual ~AeroBodyParams() = default; virtual void initialize(const AirSimSettings::VehicleSetting* vehicle_setting) @@ -121,7 +121,7 @@ namespace airlib void setupTriTiltrotor(Params& params) { - //the rotor setup of the tritiltrotor is + //the rotor setup of the trivtol is // (0) (1) // \ / // \ / diff --git a/Source/AirLib/include/vehicles/tiltrotor/AeroBodyParamsFactory.hpp b/Source/AirLib/include/vehicles/vtol/AeroBodyParamsFactory.hpp similarity index 73% rename from Source/AirLib/include/vehicles/tiltrotor/AeroBodyParamsFactory.hpp rename to Source/AirLib/include/vehicles/vtol/AeroBodyParamsFactory.hpp index 683ea60..c7ba94d 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/AeroBodyParamsFactory.hpp +++ b/Source/AirLib/include/vehicles/vtol/AeroBodyParamsFactory.hpp @@ -4,8 +4,8 @@ #ifndef msr_airlib_vehicles_AeroBodyParamsFactory_hpp #define msr_airlib_vehicles_AeroBodyParamsFactory_hpp -#include "vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleParams.hpp" -#include "vehicles/tiltrotor/firmwares/mavlink/Px4TiltrotorParams.hpp" +#include "vehicles/vtol/firmwares/vtol_simple/VtolSimpleParams.hpp" +#include "vehicles/vtol/firmwares/mavlink/Px4VtolParams.hpp" namespace msr { @@ -20,13 +20,13 @@ namespace airlib { std::unique_ptr config; - if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePX4Tiltrotor) { - config.reset(new Px4TiltrotorParams(*static_cast(vehicle_setting), + if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePX4Vtol) { + config.reset(new Px4VtolParams(*static_cast(vehicle_setting), sensor_factory)); } else if (vehicle_setting->vehicle_type == "" || //default config - vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeTiltrotorSimple) { - config.reset(new TiltrotorSimpleParams(vehicle_setting, sensor_factory)); + vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeVtolSimple) { + config.reset(new VtolSimpleParams(vehicle_setting, sensor_factory)); } else throw std::runtime_error(Utils::stringf( diff --git a/Source/AirLib/include/vehicles/tiltrotor/AeroParams.hpp b/Source/AirLib/include/vehicles/vtol/AeroParams.hpp similarity index 100% rename from Source/AirLib/include/vehicles/tiltrotor/AeroParams.hpp rename to Source/AirLib/include/vehicles/vtol/AeroParams.hpp diff --git a/Source/AirLib/include/vehicles/tiltrotor/AeroVertex.hpp b/Source/AirLib/include/vehicles/vtol/AeroVertex.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/AeroVertex.hpp rename to Source/AirLib/include/vehicles/vtol/AeroVertex.hpp index 5fcdd6f..5055734 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/AeroVertex.hpp +++ b/Source/AirLib/include/vehicles/vtol/AeroVertex.hpp @@ -9,7 +9,7 @@ #include "physics/Environment.hpp" #include "physics/Kinematics.hpp" #include "physics/PhysicsBodyVertex.hpp" -#include "vehicles/tiltrotor/AeroParams.hpp" +#include "vehicles/vtol/AeroParams.hpp" namespace msr { diff --git a/Source/AirLib/include/vehicles/tiltrotor/RotorTiltable.hpp b/Source/AirLib/include/vehicles/vtol/RotorTiltable.hpp similarity index 100% rename from Source/AirLib/include/vehicles/tiltrotor/RotorTiltable.hpp rename to Source/AirLib/include/vehicles/vtol/RotorTiltable.hpp diff --git a/Source/AirLib/include/vehicles/tiltrotor/RotorTiltableParams.hpp b/Source/AirLib/include/vehicles/vtol/RotorTiltableParams.hpp similarity index 100% rename from Source/AirLib/include/vehicles/tiltrotor/RotorTiltableParams.hpp rename to Source/AirLib/include/vehicles/vtol/RotorTiltableParams.hpp diff --git a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorApiBase.hpp b/Source/AirLib/include/vehicles/vtol/api/VtolApiBase.hpp similarity index 97% rename from Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorApiBase.hpp rename to Source/AirLib/include/vehicles/vtol/api/VtolApiBase.hpp index edbb5ac..3e74f38 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorApiBase.hpp +++ b/Source/AirLib/include/vehicles/vtol/api/VtolApiBase.hpp @@ -5,7 +5,7 @@ #define air_DroneVTOLControlServer_hpp #include "common/Common.hpp" -#include "TiltrotorCommon.hpp" +#include "VtolCommon.hpp" #include "safety/SafetyEval.hpp" #include "physics/Kinematics.hpp" #include "physics/Environment.hpp" @@ -22,7 +22,7 @@ namespace msr namespace airlib { - class TiltrotorApiBase : public VehicleApiBase + class VtolApiBase : public VehicleApiBase { protected: //must be implemented @@ -45,7 +45,7 @@ namespace airlib virtual Kinematics::State getKinematicsEstimated() const = 0; virtual LandedState getLandedState() const = 0; virtual GeoPoint getGpsLocation() const = 0; - virtual const TiltrotorApiParams& getTiltrotorApiParams() const = 0; + virtual const VtolApiParams& getVtolApiParams() const = 0; /************************* basic config APIs *********************************/ virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds @@ -88,7 +88,7 @@ namespace airlib virtual void resetImplementation() override; public: //these APIs uses above low level APIs - virtual ~TiltrotorApiBase() = default; + virtual ~VtolApiBase() = default; /************************* high level move APIs *********************************/ //return value of these function is true if command was completed without interruption or timeouts @@ -146,9 +146,9 @@ namespace airlib return state; } - TiltrotorState getTiltrotorState() const + VtolState getVtolState() const { - TiltrotorState state; + VtolState state; state.kinematics_estimated = getKinematicsEstimated(); state.kinematics_true = getKinematicsGroundTruth(); //TODO: add GPS health, accuracy in API @@ -236,7 +236,7 @@ namespace airlib class SingleCall { public: - SingleCall(TiltrotorApiBase* api) + SingleCall(VtolApiBase* api) : api_(api) { auto& token = api->getCancelToken(); @@ -265,7 +265,7 @@ namespace airlib } protected: - TiltrotorApiBase* getVehicleApi() + VtolApiBase* getVehicleApi() { return api_; } @@ -276,13 +276,13 @@ namespace airlib } private: - TiltrotorApiBase* api_; + VtolApiBase* api_; }; class SingleTaskCall : public SingleCall { public: - SingleTaskCall(TiltrotorApiBase* api) + SingleTaskCall(VtolApiBase* api) : SingleCall(api) { if (isRootCall()) @@ -301,7 +301,7 @@ namespace airlib { //this const correctness gymnastic is required because most //status update APIs are const - StatusLock(const TiltrotorApiBase* api) + StatusLock(const VtolApiBase* api) : lock_( *const_cast(&api->status_mutex_)) { diff --git a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorCommon.hpp b/Source/AirLib/include/vehicles/vtol/api/VtolCommon.hpp similarity index 94% rename from Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorCommon.hpp rename to Source/AirLib/include/vehicles/vtol/api/VtolCommon.hpp index afe8afb..fffc170 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorCommon.hpp +++ b/Source/AirLib/include/vehicles/vtol/api/VtolCommon.hpp @@ -53,9 +53,9 @@ namespace airlib }; //properties of vehicle - struct TiltrotorApiParams + struct VtolApiParams { - TiltrotorApiParams(){}; + VtolApiParams(){}; //what is the breaking distance for given velocity? //Below is just proportionality constant to convert from velocity to breaking distance float vel_to_breaking_dist = 0.5f; //ideally this should be 2X for very high speed but for testing we are keeping it 0.5 @@ -77,7 +77,7 @@ namespace airlib int obs_window = 0; }; - struct TiltrotorState + struct VtolState { CollisionInfo collision; Kinematics::State kinematics_estimated; @@ -90,10 +90,10 @@ namespace airlib std::string ready_message; // can show error message if drone is not reachable over the network or is not responding bool can_arm; // indicates drone is ready to be armed - TiltrotorState() + VtolState() { } - TiltrotorState(const CollisionInfo& collision_val, const Kinematics::State& kinematics_estimated_val, + VtolState(const CollisionInfo& collision_val, const Kinematics::State& kinematics_estimated_val, const Kinematics::State& kinematics_true_val, const GeoPoint& gps_location_val, uint64_t timestamp_val, LandedState landed_state_val, const RCData& rc_data_val, bool ready_val, const std::string& message, bool can_arm_val) : collision(collision_val), kinematics_estimated(kinematics_estimated_val), kinematics_true(kinematics_true_val), gps_location(gps_location_val), timestamp(timestamp_val), landed_state(landed_state_val), rc_data(rc_data_val), ready(ready_val), ready_message(message), can_arm(can_arm_val) diff --git a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibAdaptors.hpp b/Source/AirLib/include/vehicles/vtol/api/VtolRpcLibAdaptors.hpp similarity index 84% rename from Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibAdaptors.hpp rename to Source/AirLib/include/vehicles/vtol/api/VtolRpcLibAdaptors.hpp index c8a34cd..536c99e 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/api/TiltrotorRpcLibAdaptors.hpp +++ b/Source/AirLib/include/vehicles/vtol/api/VtolRpcLibAdaptors.hpp @@ -1,14 +1,14 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. -#ifndef air_TiltrotorRpcLibAdaptors_hpp -#define air_TiltrotorRpcLibAdaptors_hpp +#ifndef air_VtolRpcLibAdaptors_hpp +#define air_VtolRpcLibAdaptors_hpp #include "common/Common.hpp" #include "common/CommonStructs.hpp" #include "api/RpcLibAdaptorsBase.hpp" -#include "vehicles/tiltrotor/api/TiltrotorCommon.hpp" -#include "vehicles/tiltrotor/api/TiltrotorApiBase.hpp" +#include "vehicles/vtol/api/VtolCommon.hpp" +#include "vehicles/vtol/api/VtolApiBase.hpp" #include "vehicles/multirotor/api/MultirotorCommon.hpp" #include "common/ImageCaptureBase.hpp" #include "safety/SafetyEval.hpp" @@ -22,7 +22,7 @@ namespace msr namespace airlib_rpclib { - class TiltrotorRpcLibAdaptors : public RpcLibAdaptorsBase + class VtolRpcLibAdaptors : public RpcLibAdaptorsBase { public: struct RotorTiltableParameters @@ -81,7 +81,7 @@ namespace airlib_rpclib } }; - struct TiltrotorState + struct VtolState { CollisionInfo collision; KinematicsState kinematics_estimated; @@ -98,11 +98,11 @@ namespace airlib_rpclib MSGPACK_DEFINE_MAP(collision, kinematics_estimated, kinematics_true, gps_location, timestamp, landed_state, rc_data, ready, ready_message, can_arm); - TiltrotorState() + VtolState() { } - TiltrotorState(const msr::airlib::TiltrotorState& s) + VtolState(const msr::airlib::VtolState& s) { collision = s.collision; kinematics_estimated = s.kinematics_estimated; @@ -116,9 +116,9 @@ namespace airlib_rpclib can_arm = s.can_arm; } - msr::airlib::TiltrotorState to() const + msr::airlib::VtolState to() const { - return msr::airlib::TiltrotorState(collision.to(), kinematics_estimated.to(), kinematics_true.to(), gps_location.to(), timestamp, landed_state, rc_data.to(), ready, ready_message, can_arm); + return msr::airlib::VtolState(collision.to(), kinematics_estimated.to(), kinematics_true.to(), gps_location.to(), timestamp, landed_state, rc_data.to(), ready, ready_message, can_arm); } }; }; diff --git a/Source/AirLib/include/vehicles/vtol/api/VtolRpcLibClient.hpp b/Source/AirLib/include/vehicles/vtol/api/VtolRpcLibClient.hpp new file mode 100644 index 0000000..dfa9504 --- /dev/null +++ b/Source/AirLib/include/vehicles/vtol/api/VtolRpcLibClient.hpp @@ -0,0 +1,83 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef air_VtolRpcLibClient_hpp +#define air_VtolRpcLibClient_hpp + +#include "common/Common.hpp" +#include +#include "common/CommonStructs.hpp" +#include "common/ImageCaptureBase.hpp" +#include "vehicles/vtol/api/VtolApiBase.hpp" +#include "api/RpcLibClientBase.hpp" +#include "vehicles/vtol/api/VtolCommon.hpp" +#include "vehicles/vtol/api/VtolCommon.hpp" + +namespace msr +{ +namespace airlib +{ + + class VtolRpcLibClient : public RpcLibClientBase + { + public: + VtolRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); + + VtolRpcLibClient* takeoffAsync(float timeout_sec = 20, const std::string& vehicle_name = ""); + VtolRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); + VtolRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); + + VtolRpcLibClient* moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByVelocityZBodyFrameAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + VtolRpcLibClient* moveOnPathAsync(const vector& path, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveToZAsync(float z, float velocity, float timeout_sec = Utils::max(), + const YawMode& yaw_mode = YawMode(), float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + VtolRpcLibClient* rotateToYawAsync(float yaw, float timeout_sec = Utils::max(), float margin = 5, const std::string& vehicle_name = ""); + VtolRpcLibClient* rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name = ""); + VtolRpcLibClient* hoverAsync(const std::string& vehicle_name = ""); + + VtolRpcLibClient* setVtolPose(Pose pose, const vector& tilt_angles, bool ignore_collision, const std::string& vehicle_name = ""); + VtolRpcLibClient* moveByMotorPWMsAsync(const vector& pwm_values, float duration, const std::string& vehicle_name = ""); + + void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void moveByRC(const RCData& rc_data, const std::string& vehicle_name = ""); + + VtolState getVtolState(const std::string& vehicle_name = ""); + RotorTiltableStates getRotorStates(const std::string& vehicle_name = ""); + + bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name = ""); + + virtual VtolRpcLibClient* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan()) override; + + virtual ~VtolRpcLibClient(); //required for pimpl + + private: + struct impl; + std::unique_ptr pimpl_; + }; +} +} //namespace +#endif diff --git a/Source/AirLib/include/vehicles/vtol/api/VtolRpcLibServer.hpp b/Source/AirLib/include/vehicles/vtol/api/VtolRpcLibServer.hpp new file mode 100644 index 0000000..2b4cd31 --- /dev/null +++ b/Source/AirLib/include/vehicles/vtol/api/VtolRpcLibServer.hpp @@ -0,0 +1,31 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef air_VtolRpcLibServer_hpp +#define air_VtolRpcLibServer_hpp + +#include "common/Common.hpp" +#include +#include "api/RpcLibServerBase.hpp" +#include "vehicles/vtol/api/VtolApiBase.hpp" + +namespace msr +{ +namespace airlib +{ + + class VtolRpcLibServer : public RpcLibServerBase + { + public: + VtolRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = RpcLibPort); + virtual ~VtolRpcLibServer(); + + protected: + virtual VtolApiBase* getVehicleApi(const std::string& vehicle_name) override + { + return static_cast(RpcLibServerBase::getVehicleApi(vehicle_name)); + } + }; +} +} //namespace +#endif diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/mavlink/MavLinkOutputMappings.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/mavlink/MavLinkOutputMappings.hpp similarity index 100% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/mavlink/MavLinkOutputMappings.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/mavlink/MavLinkOutputMappings.hpp diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/mavlink/MavLinkTiltrotorApi.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/mavlink/MavLinkVtolApi.hpp similarity index 98% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/mavlink/MavLinkTiltrotorApi.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/mavlink/MavLinkVtolApi.hpp index 767af48..12fb137 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/mavlink/MavLinkTiltrotorApi.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/mavlink/MavLinkVtolApi.hpp @@ -29,8 +29,8 @@ #include "common/common_utils/Timer.hpp" #include "physics/World.hpp" #include "sensors/SensorCollection.hpp" -#include "vehicles/tiltrotor/api/TiltrotorApiBase.hpp" -#include "vehicles/tiltrotor/firmwares/mavlink/MavLinkOutputMappings.hpp" +#include "vehicles/vtol/api/VtolApiBase.hpp" +#include "vehicles/vtol/firmwares/mavlink/MavLinkOutputMappings.hpp" //sensors #include "sensors/barometer/BarometerBase.hpp" @@ -45,10 +45,10 @@ namespace msr namespace airlib { - class MavLinkTiltrotorApi : public TiltrotorApiBase + class MavLinkVtolApi : public VtolApiBase { public: //methods - virtual ~MavLinkTiltrotorApi() + virtual ~MavLinkVtolApi() { closeAllConnection(); if (this->connect_thread_.joinable()) { @@ -112,7 +112,7 @@ namespace airlib world_ = static_cast(container); } } - TiltrotorApiBase::resetImplementation(); + VtolApiBase::resetImplementation(); was_reset_ = true; } @@ -140,7 +140,7 @@ namespace airlib { try { auto now = clock()->nowNanos() / 1000; - TiltrotorApiBase::update(); + VtolApiBase::update(); if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_) return; @@ -621,7 +621,7 @@ namespace airlib return; } - // This method is called at high frequence from TiltrotorPawnSimApi::updateRendering. + // This method is called at high frequency from PawnSimApi::updateRendering. mavlinkcom::MavLinkTelemetry data; connection_->getTelemetry(data); if (data.messages_received == 0) { @@ -723,7 +723,7 @@ namespace airlib hil_sensor_count_ = 0; actuator_delay_ = 0; - this->telemetry_thread_ = std::thread(&MavLinkTiltrotorApi::telemtry_thread, this); + this->telemetry_thread_ = std::thread(&MavLinkVtolApi::telemtry_thread, this); } void telemtry_thread() @@ -844,11 +844,11 @@ namespace airlib mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw); } - //TODO: decouple TiltrotorApiBase, VehicalParams and SafetyEval - virtual const TiltrotorApiParams& getTiltrotorApiParams() const override + //TODO: decouple VtolApiBase, VehicalParams and SafetyEval + virtual const VtolApiParams& getVtolApiParams() const override { //defaults are good for PX4 generic quadcopter. - static const TiltrotorApiParams vehicle_params_; + static const VtolApiParams vehicle_params_; return vehicle_params_; } @@ -908,7 +908,7 @@ namespace airlib if (this->connect_thread_.joinable()) { this->connect_thread_.join(); } - this->connect_thread_ = std::thread(&MavLinkTiltrotorApi::connect_thread, this); + this->connect_thread_ = std::thread(&MavLinkVtolApi::connect_thread, this); } } @@ -1234,7 +1234,7 @@ namespace airlib std::shared_ptr& node, std::shared_ptr& connection) { if (connection_ == nullptr) - throw std::domain_error("MavLinkTiltrotorApi requires connection object to be set before createProxy call"); + throw std::domain_error("MavLinkVtolApi requires connection object to be set before createProxy call"); connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port); @@ -1680,7 +1680,7 @@ namespace airlib } } else if (msg.msgid == HilControlsMessage.msgid) { - throw std::invalid_argument("HilControlsMessage not supported in MavLinkTiltrotorApi"); + throw std::invalid_argument("HilControlsMessage not supported in MavLinkVtolApi"); } else if (msg.msgid == HilActuatorControlsMessage.msgid) { actuators_message_supported_ = true; @@ -1959,7 +1959,7 @@ namespace airlib std::shared_ptr hil_node_; std::shared_ptr connection_; std::shared_ptr video_server_; - std::shared_ptr mav_vehicle_control_; + std::shared_ptr mav_vehicle_control_; mavlinkcom::MavLinkAttPosMocap MocapPoseMessage; mavlinkcom::MavLinkHeartbeat HeartbeatMessage; @@ -2015,7 +2015,7 @@ namespace airlib uint32_t actuator_delay_ = 0; std::thread telemetry_thread_; - //additional variables required for TiltrotorApiBase implementation + //additional variables required for VtolApiBase implementation //this is optional for methods that might not use vehicle commands std::shared_ptr mav_vehicle_; float target_height_; diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/mavlink/Px4TiltrotorParams.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/mavlink/Px4VtolParams.hpp similarity index 74% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/mavlink/Px4TiltrotorParams.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/mavlink/Px4VtolParams.hpp index 2590461..8b0bc2d 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/mavlink/Px4TiltrotorParams.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/mavlink/Px4VtolParams.hpp @@ -4,31 +4,31 @@ #ifndef msr_airlib_vehicles_Px4TiltRotor_hpp #define msr_airlib_vehicles_Px4TiltRotor_hpp -#include "vehicles/tiltrotor/firmwares/mavlink/MavLinkTiltrotorApi.hpp" +#include "vehicles/vtol/firmwares/mavlink/MavLinkVtolApi.hpp" #include "common/AirSimSettings.hpp" #include "sensors/SensorFactory.hpp" -#include "vehicles/tiltrotor/AeroBodyParams.hpp" +#include "vehicles/vtol/AeroBodyParams.hpp" namespace msr { namespace airlib { - class Px4TiltrotorParams : public AeroBodyParams + class Px4VtolParams : public AeroBodyParams { public: - Px4TiltrotorParams(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting, std::shared_ptr sensor_factory) + Px4VtolParams(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting, std::shared_ptr sensor_factory) : sensor_factory_(sensor_factory) { connection_info_ = getConnectionInfo(vehicle_setting); } - virtual ~Px4TiltrotorParams() = default; + virtual ~Px4VtolParams() = default; - virtual std::unique_ptr createTiltrotorApi() override + virtual std::unique_ptr createVtolApi() override { - unique_ptr api(new MavLinkTiltrotorApi()); - auto api_ptr = static_cast(api.get()); + unique_ptr api(new MavLinkVtolApi()); + auto api_ptr = static_cast(api.get()); api_ptr->initialize(connection_info_, &getSensors(), true); return api; diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleApi.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleApi.hpp similarity index 74% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleApi.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleApi.hpp index bed5db0..b62c74c 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleApi.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleApi.hpp @@ -1,20 +1,20 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. -#ifndef msr_airlib_TiltrotorSimpleController_hpp -#define msr_airlib_TiltrotorSimpleController_hpp +#ifndef msr_airlib_VtolSimpleController_hpp +#define msr_airlib_VtolSimpleController_hpp -#include "vehicles/tiltrotor/api/TiltrotorApiBase.hpp" +#include "vehicles/vtol/api/VtolApiBase.hpp" #include "sensors/SensorCollection.hpp" #include "physics/Environment.hpp" #include "physics/Kinematics.hpp" -#include "vehicles/tiltrotor/AeroBodyParams.hpp" +#include "vehicles/vtol/AeroBodyParams.hpp" #include "common/Common.hpp" #include "firmware/Firmware.hpp" -#include "TiltrotorSimpleBoard.hpp" -#include "TiltrotorSimpleCommLink.hpp" -#include "TiltrotorSimpleEstimator.hpp" -#include "TiltrotorSimpleCommon.hpp" +#include "VtolSimpleBoard.hpp" +#include "VtolSimpleCommLink.hpp" +#include "VtolSimpleEstimator.hpp" +#include "VtolSimpleCommon.hpp" #include "physics/PhysicsBody.hpp" #include "common/AirSimSettings.hpp" @@ -25,11 +25,11 @@ namespace msr namespace airlib { - class TiltrotorSimpleApi : public TiltrotorApiBase + class VtolSimpleApi : public VtolApiBase { public: - TiltrotorSimpleApi(const AeroBodyParams* vehicle_params, const AirSimSettings::VehicleSetting* vehicle_setting) + VtolSimpleApi(const AeroBodyParams* vehicle_params, const AirSimSettings::VehicleSetting* vehicle_setting) : vehicle_params_(vehicle_params) { readSettings(*vehicle_setting); @@ -38,24 +38,24 @@ namespace airlib safety_params_.vel_to_breaking_dist = safety_params_.min_breaking_dist = 0; //create sim implementations of board and commlink - board_.reset(new TiltrotorSimpleBoard(¶ms_)); - comm_link_.reset(new TiltrotorSimpleCommLink()); - estimator_.reset(new TiltrotorSimpleEstimator()); + board_.reset(new VtolSimpleBoard(¶ms_)); + comm_link_.reset(new VtolSimpleCommLink()); + estimator_.reset(new VtolSimpleEstimator()); //create firmware - firmware_.reset(new tiltrotor_simple::Firmware(¶ms_, board_.get(), comm_link_.get(), estimator_.get())); + firmware_.reset(new vtol_simple::Firmware(¶ms_, board_.get(), comm_link_.get(), estimator_.get())); } public: //VehicleApiBase implementation virtual void resetImplementation() override { - TiltrotorApiBase::resetImplementation(); + VtolApiBase::resetImplementation(); firmware_->reset(); } virtual void update() override { - TiltrotorApiBase::update(); + VtolApiBase::update(); //update controller which will update actuator control signal firmware_->update(); @@ -84,7 +84,7 @@ namespace airlib } virtual GeoPoint getHomeGeoPoint() const override { - return TiltrotorSimpleCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint()); + return VtolSimpleCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint()); } virtual void getStatusMessages(std::vector& messages) override { @@ -96,7 +96,7 @@ namespace airlib return vehicle_params_->getSensors(); } - public: //TiltrotorApiBase implementation + public: //VtolApiBase implementation virtual real_T getActuation(unsigned int actuator_index) const override { auto control_signal = board_->getActuatorControlSignal(actuator_index); @@ -112,7 +112,7 @@ namespace airlib } virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) override { - TiltrotorApiBase::setSimulatedGroundTruth(kinematics, environment); + VtolApiBase::setSimulatedGroundTruth(kinematics, environment); board_->setGroundTruthKinematics(kinematics); estimator_->setGroundTruthKinematics(kinematics, environment); @@ -145,25 +145,25 @@ namespace airlib protected: virtual Kinematics::State getKinematicsEstimated() const override { - return TiltrotorSimpleCommon::toKinematicsState3r(firmware_->offboardApi().getStateEstimator().getKinematicsEstimated()); + return VtolSimpleCommon::toKinematicsState3r(firmware_->offboardApi().getStateEstimator().getKinematicsEstimated()); } virtual Vector3r getPosition() const override { const auto& val = firmware_->offboardApi().getStateEstimator().getPosition(); - return TiltrotorSimpleCommon::toVector3r(val); + return VtolSimpleCommon::toVector3r(val); } virtual Vector3r getVelocity() const override { const auto& val = firmware_->offboardApi().getStateEstimator().getLinearVelocity(); - return TiltrotorSimpleCommon::toVector3r(val); + return VtolSimpleCommon::toVector3r(val); } virtual Quaternionr getOrientation() const override { const auto& val = firmware_->offboardApi().getStateEstimator().getOrientation(); - return TiltrotorSimpleCommon::toQuaternion(val); + return VtolSimpleCommon::toQuaternion(val); } virtual LandedState getLandedState() const override @@ -179,7 +179,7 @@ namespace airlib virtual GeoPoint getGpsLocation() const override { - return TiltrotorSimpleCommon::toGeoPoint(firmware_->offboardApi().getGeoPoint()); + return VtolSimpleCommon::toGeoPoint(firmware_->offboardApi().getGeoPoint()); } virtual float getCommandPeriod() const override @@ -203,10 +203,10 @@ namespace airlib { //Utils::log(Utils::stringf("commandMotorPWMs %f, %f, %f, %f", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough); + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough); - // tiltrotor_simple::Axis4r goal(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); + // vtol_simple::Axis4r goal(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -219,10 +219,10 @@ namespace airlib { //Utils::log(Utils::stringf("commandRollPitchYawZ %f, %f, %f, %f", pitch, roll, z, yaw)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld); + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld); - // tiltrotor_simple::Axis4r goal(roll, pitch, yaw, z); + // vtol_simple::Axis4r goal(roll, pitch, yaw, z); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -237,10 +237,10 @@ namespace airlib { //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::Passthrough); + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::Passthrough); - // tiltrotor_simple::Axis4r goal(roll, pitch, yaw, throttle); + // vtol_simple::Axis4r goal(roll, pitch, yaw, throttle); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -255,10 +255,10 @@ namespace airlib { //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); - // tiltrotor_simple::Axis4r goal(roll, pitch, yaw_rate, throttle); + // vtol_simple::Axis4r goal(roll, pitch, yaw_rate, throttle); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -273,10 +273,10 @@ namespace airlib { //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::PositionWorld); + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::PositionWorld); - // tiltrotor_simple::Axis4r goal(roll, pitch, yaw_rate, z); + // vtol_simple::Axis4r goal(roll, pitch, yaw_rate, z); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -291,10 +291,10 @@ namespace airlib { //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::PositionWorld); + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::PositionWorld); - // tiltrotor_simple::Axis4r goal(roll_rate, pitch_rate, yaw_rate, z); + // vtol_simple::Axis4r goal(roll_rate, pitch_rate, yaw_rate, z); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -309,10 +309,10 @@ namespace airlib { //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough); + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough); - // tiltrotor_simple::Axis4r goal(roll_rate, pitch_rate, yaw_rate, throttle); + // vtol_simple::Axis4r goal(roll_rate, pitch_rate, yaw_rate, throttle); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -327,12 +327,12 @@ namespace airlib { //Utils::log(Utils::stringf("commandVelocity %f, %f, %f, %f", vx, vy, vz, yaw_mode.yaw_or_rate)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, // yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, // GoalModeType::VelocityWorld); - // tiltrotor_simple::Axis4r goal(vy, vx, Utils::degreesToRadians(yaw_mode.yaw_or_rate), vz); + // vtol_simple::Axis4r goal(vy, vx, Utils::degreesToRadians(yaw_mode.yaw_or_rate), vz); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -347,12 +347,12 @@ namespace airlib { //Utils::log(Utils::stringf("commandVelocityZ %f, %f, %f, %f", vx, vy, z, yaw_mode.yaw_or_rate)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, // yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, // GoalModeType::PositionWorld); - // tiltrotor_simple::Axis4r goal(vy, vx, Utils::degreesToRadians(yaw_mode.yaw_or_rate), z); + // vtol_simple::Axis4r goal(vy, vx, Utils::degreesToRadians(yaw_mode.yaw_or_rate), z); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -365,7 +365,7 @@ namespace airlib virtual void setControllerGains(uint8_t controller_type, const vector& kp, const vector& ki, const vector& kd) override { - // tiltrotor_simple::GoalModeType controller_type_enum = static_cast(controller_type); + // vtol_simple::GoalModeType controller_type_enum = static_cast(controller_type); // vector kp_axis4(4); // vector ki_axis4(4); @@ -373,7 +373,7 @@ namespace airlib // switch(controller_type_enum) { // // roll gain, pitch gain, yaw gain, and no gains in throttle / z axis - // case tiltrotor_simple::GoalModeType::AngleRate: + // case vtol_simple::GoalModeType::AngleRate: // kp_axis4 = {kp[0], kp[1], kp[2], 1.0}; // ki_axis4 ={ki[0], ki[1], ki[2], 0.0}; // kd_axis4 = {kd[0], kd[1], kd[2], 0.0}; @@ -382,7 +382,7 @@ namespace airlib // params_.angle_rate_pid.d.setValues(kd_axis4); // params_.gains_changed = true; // break; - // case tiltrotor_simple::GoalModeType::AngleLevel: + // case vtol_simple::GoalModeType::AngleLevel: // kp_axis4 = {kp[0], kp[1], kp[2], 1.0}; // ki_axis4 = {ki[0], ki[1], ki[2], 0.0}; // kd_axis4 = {kd[0], kd[1], kd[2], 0.0}; @@ -391,7 +391,7 @@ namespace airlib // params_.angle_level_pid.d.setValues(kd_axis4); // params_.gains_changed = true; // break; - // case tiltrotor_simple::GoalModeType::VelocityWorld: + // case vtol_simple::GoalModeType::VelocityWorld: // kp_axis4 = {kp[1], kp[0], 0.0, kp[2]}; // ki_axis4 = {ki[1], ki[0], 0.0, ki[2]}; // kd_axis4 = {kd[1], kd[0], 0.0, kd[2]}; @@ -400,7 +400,7 @@ namespace airlib // params_.velocity_pid.d.setValues(kd_axis4); // params_.gains_changed = true; // break; - // case tiltrotor_simple::GoalModeType::PositionWorld: + // case vtol_simple::GoalModeType::PositionWorld: // kp_axis4 = {kp[1], kp[0], 0.0, kp[2]}; // ki_axis4 = {ki[1], ki[0], 0.0, ki[2]}; // kd_axis4 = {kd[1], kd[0], 0.0, kd[2]}; @@ -424,12 +424,12 @@ namespace airlib { //Utils::log(Utils::stringf("commandPosition %f, %f, %f, %f", x, y, z, yaw_mode.yaw_or_rate)); - // typedef tiltrotor_simple::GoalModeType GoalModeType; - // tiltrotor_simple::GoalMode mode(GoalModeType::PositionWorld, GoalModeType::PositionWorld, + // typedef vtol_simple::GoalModeType GoalModeType; + // vtol_simple::GoalMode mode(GoalModeType::PositionWorld, GoalModeType::PositionWorld, // yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, // GoalModeType::PositionWorld); - // tiltrotor_simple::Axis4r goal(y, x, Utils::degreesToRadians(yaw_mode.yaw_or_rate), z); + // vtol_simple::Axis4r goal(y, x, Utils::degreesToRadians(yaw_mode.yaw_or_rate), z); // std::string message; // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); @@ -440,12 +440,12 @@ namespace airlib Utils::log("Not Implemented: commandPosition", Utils::kLogLevelInfo); } - virtual const TiltrotorApiParams& getTiltrotorApiParams() const override + virtual const VtolApiParams& getVtolApiParams() const override { return safety_params_; } - //*** End: TiltrotorApiBase implementation ***// + //*** End: VtolApiBase implementation ***// private: //convert pitch, roll, yaw from -1 to 1 to PWM @@ -464,7 +464,7 @@ namespace airlib void readSettings(const AirSimSettings::VehicleSetting& vehicle_setting) { - params_.default_vehicle_state = tiltrotor_simple::VehicleState::fromString( + params_.default_vehicle_state = vtol_simple::VehicleState::fromString( vehicle_setting.default_vehicle_state == "" ? "Armed" : vehicle_setting.default_vehicle_state); remote_control_id_ = vehicle_setting.rc.remote_control_id; @@ -478,14 +478,14 @@ namespace airlib const AeroBodyParams* vehicle_params_; int remote_control_id_ = 0; - tiltrotor_simple::Params params_; + vtol_simple::Params params_; - unique_ptr board_; - unique_ptr comm_link_; - unique_ptr estimator_; - unique_ptr firmware_; + unique_ptr board_; + unique_ptr comm_link_; + unique_ptr estimator_; + unique_ptr firmware_; - TiltrotorApiParams safety_params_; + VtolApiParams safety_params_; RCData last_rcData_; }; diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleBoard.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleBoard.hpp similarity index 94% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleBoard.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleBoard.hpp index 26b9da5..cf214e7 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleBoard.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleBoard.hpp @@ -1,8 +1,8 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. -#ifndef msr_airlib_TiltrotorSimpleBoard_hpp -#define msr_airlib_TiltrotorSimpleBoard_hpp +#ifndef msr_airlib_VtolSimpleBoard_hpp +#define msr_airlib_VtolSimpleBoard_hpp #include #include @@ -17,10 +17,10 @@ namespace msr namespace airlib { - class TiltrotorSimpleBoard : public tiltrotor_simple::IBoard + class VtolSimpleBoard : public vtol_simple::IBoard { public: - TiltrotorSimpleBoard(const tiltrotor_simple::Params* params) + VtolSimpleBoard(const vtol_simple::Params* params) : params_(params) { } @@ -153,7 +153,7 @@ namespace airlib std::vector input_channels_; bool is_connected_; - const tiltrotor_simple::Params* params_; + const vtol_simple::Params* params_; const Kinematics::State* kinematics_; }; diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleCommLink.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleCommLink.hpp similarity index 80% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleCommLink.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleCommLink.hpp index 24e65b2..1f78d25 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleCommLink.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleCommLink.hpp @@ -1,8 +1,8 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. -#ifndef msr_airlib_TiltrotorSimpleCommLink_hpp -#define msr_airlib_TiltrotorSimpleCommLink_hpp +#ifndef msr_airlib_VtolSimpleCommLink_hpp +#define msr_airlib_VtolSimpleCommLink_hpp #include #include "firmware/interfaces/ICommLink.hpp" @@ -13,7 +13,7 @@ namespace msr namespace airlib { - class TiltrotorSimpleCommLink : public tiltrotor_simple::ICommLink + class VtolSimpleCommLink : public vtol_simple::ICommLink { public: // derived class specific methods void getStatusMessages(std::vector& messages) @@ -27,14 +27,14 @@ namespace airlib public: // implement CommLink interface virtual void reset() { - tiltrotor_simple::ICommLink::reset(); + vtol_simple::ICommLink::reset(); messages_.clear(); } virtual void update() { - tiltrotor_simple::ICommLink::update(); + vtol_simple::ICommLink::update(); } virtual void log(const std::string& message, int32_t log_level = ICommLink::kLogLevelInfo) diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleCommon.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleCommon.hpp similarity index 72% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleCommon.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleCommon.hpp index 165bd23..b4bd5f4 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleCommon.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleCommon.hpp @@ -1,8 +1,8 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. -#ifndef msr_airlib_TiltrotorSimpleCommon_hpp -#define msr_airlib_TiltrotorSimpleCommon_hpp +#ifndef msr_airlib_VtolSimpleCommon_hpp +#define msr_airlib_VtolSimpleCommon_hpp #include "physics/Kinematics.hpp" #include "common/Common.hpp" @@ -12,12 +12,12 @@ namespace msr namespace airlib { - class TiltrotorSimpleCommon + class VtolSimpleCommon { public: - static tiltrotor_simple::Axis3r toAxis3r(const Vector3r& vec) + static vtol_simple::Axis3r toAxis3r(const Vector3r& vec) { - tiltrotor_simple::Axis3r conv; + vtol_simple::Axis3r conv; conv.x() = vec.x(); conv.y() = vec.y(); conv.z() = vec.z(); @@ -25,7 +25,7 @@ namespace airlib return conv; } - static Vector3r toVector3r(const tiltrotor_simple::Axis3r& vec) + static Vector3r toVector3r(const vtol_simple::Axis3r& vec) { Vector3r conv; conv.x() = vec.x(); @@ -34,7 +34,7 @@ namespace airlib return conv; } - static Kinematics::State toKinematicsState3r(const tiltrotor_simple::KinematicsState& state) + static Kinematics::State toKinematicsState3r(const vtol_simple::KinematicsState& state) { Kinematics::State state3r; state3r.pose.position = toVector3r(state.position); @@ -47,9 +47,9 @@ namespace airlib return state3r; } - static tiltrotor_simple::Axis4r toAxis4r(const Quaternionr& q) + static vtol_simple::Axis4r toAxis4r(const Quaternionr& q) { - tiltrotor_simple::Axis4r conv; + vtol_simple::Axis4r conv; conv.x() = q.x(); conv.y() = q.y(); conv.z() = q.z(); @@ -58,7 +58,7 @@ namespace airlib return conv; } - static Quaternionr toQuaternion(const tiltrotor_simple::Axis4r& q) + static Quaternionr toQuaternion(const vtol_simple::Axis4r& q) { Quaternionr conv; conv.x() = q.x(); @@ -68,9 +68,9 @@ namespace airlib return conv; } - static tiltrotor_simple::GeoPoint toSimpleFlightGeoPoint(const GeoPoint& geo_point) + static vtol_simple::GeoPoint toSimpleFlightGeoPoint(const GeoPoint& geo_point) { - tiltrotor_simple::GeoPoint conv; + vtol_simple::GeoPoint conv; conv.latitude = geo_point.latitude; conv.longitude = geo_point.longitude; conv.altiude = geo_point.altitude; @@ -78,7 +78,7 @@ namespace airlib return conv; } - static GeoPoint toGeoPoint(const tiltrotor_simple::GeoPoint& geo_point) + static GeoPoint toGeoPoint(const vtol_simple::GeoPoint& geo_point) { GeoPoint conv; conv.latitude = geo_point.latitude; diff --git a/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleEstimator.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleEstimator.hpp new file mode 100644 index 0000000..8375b2e --- /dev/null +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleEstimator.hpp @@ -0,0 +1,107 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_VtolSimpleEstimator_hpp +#define msr_airlib_VtolSimpleEstimator_hpp + +#include "firmware/interfaces/CommonStructs.hpp" +#include "VtolSimpleCommon.hpp" +#include "physics/Kinematics.hpp" +#include "physics/Environment.hpp" +#include "common/Common.hpp" + +namespace msr +{ +namespace airlib +{ + + class VtolSimpleEstimator : public vtol_simple::IStateEstimator + { + public: + virtual ~VtolSimpleEstimator() {} + + //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors) + void setGroundTruthKinematics(const Kinematics::State* kinematics, const Environment* environment) + { + kinematics_ = kinematics; + environment_ = environment; + } + + virtual vtol_simple::Axis3r getAngles() const override + { + vtol_simple::Axis3r angles; + VectorMath::toEulerianAngle(kinematics_->pose.orientation, + angles.pitch(), + angles.roll(), + angles.yaw()); + + //Utils::log(Utils::stringf("Ang Est:\t(%f, %f, %f)", angles.pitch(), angles.roll(), angles.yaw())); + + return angles; + } + + virtual vtol_simple::Axis3r getAngularVelocity() const override + { + const auto& anguler = kinematics_->twist.angular; + + vtol_simple::Axis3r conv; + conv.x() = anguler.x(); + conv.y() = anguler.y(); + conv.z() = anguler.z(); + + return conv; + } + + virtual vtol_simple::Axis3r getPosition() const override + { + return VtolSimpleCommon::toAxis3r(kinematics_->pose.position); + } + + virtual vtol_simple::Axis3r transformToBodyFrame(const vtol_simple::Axis3r& world_frame_val) const override + { + const Vector3r& vec = VtolSimpleCommon::toVector3r(world_frame_val); + const Vector3r& trans = VectorMath::transformToBodyFrame(vec, kinematics_->pose.orientation); + return VtolSimpleCommon::toAxis3r(trans); + } + + virtual vtol_simple::Axis3r getLinearVelocity() const override + { + return VtolSimpleCommon::toAxis3r(kinematics_->twist.linear); + } + + virtual vtol_simple::Axis4r getOrientation() const override + { + return VtolSimpleCommon::toAxis4r(kinematics_->pose.orientation); + } + + virtual vtol_simple::GeoPoint getGeoPoint() const override + { + return VtolSimpleCommon::toSimpleFlightGeoPoint(environment_->getState().geo_point); + } + + virtual vtol_simple::GeoPoint getHomeGeoPoint() const override + { + return VtolSimpleCommon::toSimpleFlightGeoPoint(environment_->getHomeGeoPoint()); + } + + virtual vtol_simple::KinematicsState getKinematicsEstimated() const override + { + vtol_simple::KinematicsState state; + state.position = getPosition(); + state.orientation = getOrientation(); + state.linear_velocity = getLinearVelocity(); + state.angular_velocity = getAngularVelocity(); + state.linear_acceleration = VtolSimpleCommon::toAxis3r(kinematics_->accelerations.linear); + state.angular_acceleration = VtolSimpleCommon::toAxis3r(kinematics_->accelerations.angular); + + return state; + } + + private: + const Kinematics::State* kinematics_; + const Environment* environment_; + }; + +} +} //namespace +#endif diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleParams.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleParams.hpp similarity index 55% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleParams.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleParams.hpp index a2bd8ca..5963368 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleParams.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/VtolSimpleParams.hpp @@ -1,8 +1,8 @@ -#ifndef msr_airlib_vehicles_TiltrotorSimple_hpp -#define msr_airlib_vehicles_TiltrotorSimple_hpp +#ifndef msr_airlib_vehicles_VtolSimple_hpp +#define msr_airlib_vehicles_VtolSimple_hpp -#include "vehicles/tiltrotor/firmwares/tiltrotor_simple/TiltrotorSimpleApi.hpp" -#include "vehicles/tiltrotor/AeroBodyParams.hpp" +#include "vehicles/vtol/firmwares/vtol_simple/VtolSimpleApi.hpp" +#include "vehicles/vtol/AeroBodyParams.hpp" #include "common/AirSimSettings.hpp" #include "sensors/SensorFactory.hpp" @@ -11,19 +11,19 @@ namespace msr namespace airlib { - class TiltrotorSimpleParams : public AeroBodyParams + class VtolSimpleParams : public AeroBodyParams { public: - TiltrotorSimpleParams(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory) + VtolSimpleParams(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory) : vehicle_setting_(vehicle_setting), sensor_factory_(sensor_factory) { } - virtual ~TiltrotorSimpleParams() = default; + virtual ~VtolSimpleParams() = default; - virtual std::unique_ptr createTiltrotorApi() override + virtual std::unique_ptr createVtolApi() override { - return std::unique_ptr(new TiltrotorSimpleApi(this, vehicle_setting_)); + return std::unique_ptr(new VtolSimpleApi(this, vehicle_setting_)); } protected: diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AdaptiveController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AdaptiveController.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AdaptiveController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AdaptiveController.hpp index 94e2416..26df598 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AdaptiveController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AdaptiveController.hpp @@ -10,10 +10,10 @@ // DEFINES (drone based defines for physical characteristics) // -------------------------------------------------------------------------- -#ifndef msr_airsim_TiltrotorAdaptiveController_hpp -#define msr_airsim_TiltrotorAdaptiveController_hpp +#ifndef msr_airsim_VtolAdaptiveController_hpp +#define msr_airsim_VtolAdaptiveController_hpp -namespace tiltrotor_simple +namespace vtol_simple { class AdaptiveController : public IController diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AngleLevelController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AngleLevelController.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AngleLevelController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AngleLevelController.hpp index ea35c45..d43f770 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AngleLevelController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AngleLevelController.hpp @@ -13,7 +13,7 @@ #include #include -namespace tiltrotor_simple +namespace vtol_simple { class AngleLevelController : public IAxisController diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AngleRateController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AngleRateController.hpp similarity index 98% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AngleRateController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AngleRateController.hpp index 84e47bb..00a64e1 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/AngleRateController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/AngleRateController.hpp @@ -11,7 +11,7 @@ #include #include -namespace tiltrotor_simple +namespace vtol_simple { class AngleRateController : public IAxisController diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/CascadeController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/CascadeController.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/CascadeController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/CascadeController.hpp index 47ef0df..d891413 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/CascadeController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/CascadeController.hpp @@ -14,7 +14,7 @@ #include "PositionController.hpp" #include "common/common_utils/Utils.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class CascadeController : public IController diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/ConstantOutputController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/ConstantOutputController.hpp similarity index 97% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/ConstantOutputController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/ConstantOutputController.hpp index bb6a009..a5c8af0 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/ConstantOutputController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/ConstantOutputController.hpp @@ -8,7 +8,7 @@ #include "Params.hpp" #include -namespace tiltrotor_simple +namespace vtol_simple { class ConstantOutputController : public IAxisController diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/DoNothingController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/DoNothingController.hpp similarity index 97% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/DoNothingController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/DoNothingController.hpp index 27aac25..4898650 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/DoNothingController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/DoNothingController.hpp @@ -5,7 +5,7 @@ #include "interfaces/ICommLink.hpp" #include "interfaces/IGoal.hpp" -namespace tiltrotor_simple +namespace vtol_simple { //This controller literally does nothing. It just implements the pure virtual IController functions. diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Firmware.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Firmware.hpp similarity index 96% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Firmware.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Firmware.hpp index 0171ece..aa2ead5 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Firmware.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Firmware.hpp @@ -14,7 +14,7 @@ // #include "AdaptiveController.hpp" #include "DoNothingController.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class Firmware : public IFirmware diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Mixer.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Mixer.hpp similarity index 98% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Mixer.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Mixer.hpp index 48b3cd0..9842863 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Mixer.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Mixer.hpp @@ -5,7 +5,7 @@ #include "Params.hpp" #include "interfaces/CommonStructs.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class Mixer diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/OffboardApi.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/OffboardApi.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/OffboardApi.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/OffboardApi.hpp index 8b9c63e..9d5c8e0 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/OffboardApi.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/OffboardApi.hpp @@ -9,7 +9,7 @@ #include "RemoteControl.hpp" #include "Params.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class OffboardApi : public IUpdatable diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Params.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Params.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Params.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Params.hpp index f357326..3566224 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/Params.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/Params.hpp @@ -2,7 +2,7 @@ #include "interfaces/CommonStructs.hpp" -namespace tiltrotor_simple +namespace vtol_simple { struct Params diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PassthroughController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PassthroughController.hpp similarity index 97% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PassthroughController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PassthroughController.hpp index 148d59b..4f31ad2 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PassthroughController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PassthroughController.hpp @@ -8,7 +8,7 @@ #include "Params.hpp" #include -namespace tiltrotor_simple +namespace vtol_simple { class PassthroughController : public IAxisController diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PidController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PidController.hpp similarity index 95% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PidController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PidController.hpp index 2049d4f..635065a 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PidController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PidController.hpp @@ -7,7 +7,7 @@ #include "StdPidIntegrator.hpp" #include "RungKuttaPidIntegrator.hpp" -namespace tiltrotor_simple +namespace vtol_simple { template diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PositionController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PositionController.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PositionController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PositionController.hpp index 65164b5..d2bc73c 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/PositionController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/PositionController.hpp @@ -11,7 +11,7 @@ #include "PidController.hpp" #include "common/common_utils/Utils.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class PositionController : public IAxisController diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/RemoteControl.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/RemoteControl.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/RemoteControl.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/RemoteControl.hpp index 223fcb3..2a848bf 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/RemoteControl.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/RemoteControl.hpp @@ -7,7 +7,7 @@ #include "interfaces/IGoal.hpp" #include "interfaces/CommonStructs.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class RemoteControl : public IGoal diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/RungKuttaPidIntegrator.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/RungKuttaPidIntegrator.hpp similarity index 98% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/RungKuttaPidIntegrator.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/RungKuttaPidIntegrator.hpp index 268a835..303683e 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/RungKuttaPidIntegrator.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/RungKuttaPidIntegrator.hpp @@ -2,7 +2,7 @@ #include "interfaces/CommonStructs.hpp" -namespace tiltrotor_simple +namespace vtol_simple { template diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/StdPidIntegrator.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/StdPidIntegrator.hpp similarity index 97% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/StdPidIntegrator.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/StdPidIntegrator.hpp index edbeb59..98bb941 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/StdPidIntegrator.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/StdPidIntegrator.hpp @@ -2,7 +2,7 @@ #include "interfaces/CommonStructs.hpp" -namespace tiltrotor_simple +namespace vtol_simple { template diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/VelocityController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/VelocityController.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/VelocityController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/VelocityController.hpp index 8537264..fa6854c 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/VelocityController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/VelocityController.hpp @@ -11,7 +11,7 @@ #include "PidController.hpp" #include "common/common_utils/Utils.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class VelocityController : public IAxisController diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/CommonStructs.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/CommonStructs.hpp similarity index 99% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/CommonStructs.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/CommonStructs.hpp index 56785dd..515046e 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/CommonStructs.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/CommonStructs.hpp @@ -3,7 +3,7 @@ #include #include -namespace tiltrotor_simple +namespace vtol_simple { typedef float TReal; diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IAxisController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IAxisController.hpp similarity index 95% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IAxisController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IAxisController.hpp index 1b535a5..b76e543 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IAxisController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IAxisController.hpp @@ -6,7 +6,7 @@ #include "IStateEstimator.hpp" #include "IBoardClock.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class IAxisController : public IUpdatable diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoard.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoard.hpp similarity index 92% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoard.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoard.hpp index 803ffa5..8f7c7cb 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoard.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoard.hpp @@ -6,7 +6,7 @@ #include "IBoardOutputPins.hpp" #include "IBoardSensors.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class IBoard : public IUpdatable diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardClock.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardClock.hpp similarity index 84% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardClock.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardClock.hpp index 689e2b3..3f1685f 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardClock.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardClock.hpp @@ -2,7 +2,7 @@ #include -namespace tiltrotor_simple +namespace vtol_simple { class IBoardClock diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardInputPins.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardInputPins.hpp similarity index 89% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardInputPins.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardInputPins.hpp index 540669c..d6b32e2 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardInputPins.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardInputPins.hpp @@ -2,7 +2,7 @@ #include -namespace tiltrotor_simple +namespace vtol_simple { class IBoardInputPins diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardOutputPins.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardOutputPins.hpp similarity index 90% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardOutputPins.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardOutputPins.hpp index 79a9ae3..60412f0 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardOutputPins.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardOutputPins.hpp @@ -2,7 +2,7 @@ #include -namespace tiltrotor_simple +namespace vtol_simple { class IBoardOutputPins diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardSensors.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardSensors.hpp similarity index 88% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardSensors.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardSensors.hpp index f5c3021..6333d4b 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IBoardSensors.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IBoardSensors.hpp @@ -1,6 +1,6 @@ #pragma once -namespace tiltrotor_simple +namespace vtol_simple { class IBoardSensors diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/ICommLink.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/ICommLink.hpp similarity index 93% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/ICommLink.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/ICommLink.hpp index f52e17c..5addf12 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/ICommLink.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/ICommLink.hpp @@ -3,7 +3,7 @@ #include "IUpdatable.hpp" #include -namespace tiltrotor_simple +namespace vtol_simple { class ICommLink : public IUpdatable diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IController.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IController.hpp similarity index 93% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IController.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IController.hpp index 7bc8cb8..848b009 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IController.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IController.hpp @@ -6,7 +6,7 @@ #include "IStateEstimator.hpp" #include "IBoardClock.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class IController : public IUpdatable diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IFirmware.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IFirmware.hpp similarity index 91% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IFirmware.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IFirmware.hpp index 5909bb5..8ec1408 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IFirmware.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IFirmware.hpp @@ -4,7 +4,7 @@ #include "IOffboardApi.hpp" #include "IStateEstimator.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class IFirmware : public IUpdatable diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IGoal.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IGoal.hpp similarity index 88% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IGoal.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IGoal.hpp index 20a8bea..6e6747a 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IGoal.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IGoal.hpp @@ -3,7 +3,7 @@ #include "IUpdatable.hpp" #include "CommonStructs.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class IGoal diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IOffboardApi.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IOffboardApi.hpp similarity index 96% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IOffboardApi.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IOffboardApi.hpp index 70762dc..22ab2ea 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IOffboardApi.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IOffboardApi.hpp @@ -4,7 +4,7 @@ #include "CommonStructs.hpp" #include "IGoal.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class IOffboardApi : public IGoal diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IPidIntegrator.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IPidIntegrator.hpp similarity index 92% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IPidIntegrator.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IPidIntegrator.hpp index 34f57f9..87c793e 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IPidIntegrator.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IPidIntegrator.hpp @@ -3,7 +3,7 @@ #include "CommonStructs.hpp" #include -namespace tiltrotor_simple +namespace vtol_simple { template diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IStateEstimator.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IStateEstimator.hpp similarity index 95% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IStateEstimator.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IStateEstimator.hpp index 87ac149..dd9e72e 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IStateEstimator.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IStateEstimator.hpp @@ -2,7 +2,7 @@ #include "CommonStructs.hpp" -namespace tiltrotor_simple +namespace vtol_simple { class IStateEstimator diff --git a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IUpdatable.hpp b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IUpdatable.hpp similarity index 96% rename from Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IUpdatable.hpp rename to Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IUpdatable.hpp index f17e02f..4bd574e 100644 --- a/Source/AirLib/include/vehicles/tiltrotor/firmwares/tiltrotor_simple/firmware/interfaces/IUpdatable.hpp +++ b/Source/AirLib/include/vehicles/vtol/firmwares/vtol_simple/firmware/interfaces/IUpdatable.hpp @@ -1,6 +1,6 @@ #pragma once -namespace tiltrotor_simple +namespace vtol_simple { class IUpdatable diff --git a/Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorApiBase.cpp b/Source/AirLib/src/vehicles/vtol/api/VtolApiBase.cpp similarity index 83% rename from Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorApiBase.cpp rename to Source/AirLib/src/vehicles/vtol/api/VtolApiBase.cpp index ea8abb2..8435360 100644 --- a/Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorApiBase.cpp +++ b/Source/AirLib/src/vehicles/vtol/api/VtolApiBase.cpp @@ -4,7 +4,7 @@ //in header only mode, control library is not available #ifndef AIRLIB_HEADER_ONLY -#include "vehicles/tiltrotor/api/TiltrotorApiBase.hpp" +#include "vehicles/vtol/api/VtolApiBase.hpp" #include #include #include @@ -16,13 +16,13 @@ namespace msr namespace airlib { - void TiltrotorApiBase::resetImplementation() + void VtolApiBase::resetImplementation() { cancelLastTask(); SingleTaskCall lock(this); //cancel previous tasks } - bool TiltrotorApiBase::takeoff(float timeout_sec) + bool VtolApiBase::takeoff(float timeout_sec) { SingleTaskCall lock(this); @@ -49,7 +49,7 @@ namespace airlib return ret; } - bool TiltrotorApiBase::land(float timeout_sec) + bool VtolApiBase::land(float timeout_sec) { SingleTaskCall lock(this); @@ -76,14 +76,14 @@ namespace airlib .isComplete(); } - bool TiltrotorApiBase::goHome(float timeout_sec) + bool VtolApiBase::goHome(float timeout_sec) { SingleTaskCall lock(this); return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); } - bool TiltrotorApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + bool VtolApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { SingleTaskCall lock(this); @@ -106,7 +106,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + bool VtolApiBase::moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { SingleTaskCall lock(this); @@ -129,7 +129,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByMotorPWMs(const vector& pwm_values, float duration) + bool VtolApiBase::moveByMotorPWMs(const vector& pwm_values, float duration) { SingleTaskCall lock(this); @@ -149,7 +149,7 @@ namespace airlib return res; } - bool TiltrotorApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration) + bool VtolApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration) { SingleTaskCall lock(this); @@ -164,7 +164,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration) + bool VtolApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration) { SingleTaskCall lock(this); @@ -179,7 +179,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration) + bool VtolApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration) { SingleTaskCall lock(this); @@ -194,7 +194,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration) + bool VtolApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration) { SingleTaskCall lock(this); @@ -209,7 +209,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration) + bool VtolApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration) { SingleTaskCall lock(this); @@ -224,7 +224,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration) + bool VtolApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration) { SingleTaskCall lock(this); @@ -239,7 +239,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + bool VtolApiBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { SingleTaskCall lock(this); @@ -257,7 +257,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + bool VtolApiBase::moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { SingleTaskCall lock(this); @@ -275,7 +275,7 @@ namespace airlib .isTimeout(); } - bool TiltrotorApiBase::moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, + bool VtolApiBase::moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) { SingleTaskCall lock(this); @@ -327,10 +327,10 @@ namespace airlib //when path ends, we want to slow down float breaking_dist = 0; - if (velocity > getTiltrotorApiParams().breaking_vel) { - breaking_dist = Utils::clip(velocity * getTiltrotorApiParams().vel_to_breaking_dist, - getTiltrotorApiParams().min_breaking_dist, - getTiltrotorApiParams().max_breaking_dist); + if (velocity > getVtolApiParams().breaking_vel) { + breaking_dist = Utils::clip(velocity * getVtolApiParams().vel_to_breaking_dist, + getVtolApiParams().min_breaking_dist, + getVtolApiParams().max_breaking_dist); } //else no need to change velocities for last segments @@ -354,8 +354,8 @@ namespace airlib float seg_velocity = path_segs.at(next_path_loc.seg_index).seg_velocity; float path_length_remaining = path_length - path_segs.at(cur_path_loc.seg_index).seg_path_length - cur_path_loc.offset; - if (seg_velocity > getTiltrotorApiParams().min_vel_for_breaking && path_length_remaining <= breaking_dist) { - seg_velocity = getTiltrotorApiParams().breaking_vel; + if (seg_velocity > getVtolApiParams().min_vel_for_breaking && path_length_remaining <= breaking_dist) { + seg_velocity = getVtolApiParams().breaking_vel; //Utils::logMessage("path_length_remaining = %f, Switched to breaking vel %f", path_length_remaining, seg_velocity); } @@ -454,7 +454,7 @@ namespace airlib return waiter.isComplete(); } - bool TiltrotorApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + bool VtolApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) { SingleTaskCall lock(this); @@ -463,7 +463,7 @@ namespace airlib return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); } - bool TiltrotorApiBase::moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, + bool VtolApiBase::moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) { SingleTaskCall lock(this); @@ -473,7 +473,7 @@ namespace airlib return moveOnPath(path, velocity, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, yaw_mode, lookahead, adaptive_lookahead); } - bool TiltrotorApiBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + bool VtolApiBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { SingleTaskCall lock(this); @@ -508,7 +508,7 @@ namespace airlib float vz = (rc_data.throttle / kMaxRCValue) * z_min + getPosition().z(); moveByVelocityZInternal(vel_body.x(), vel_body.y(), vz, adj_yaw_mode); } - catch (const TiltrotorApiBase::UnsafeMoveException& ex) { + catch (const VtolApiBase::UnsafeMoveException& ex) { Utils::log(Utils::stringf("Safety violation: %s", ex.result.message.c_str()), Utils::kLogLevelWarn); } } @@ -521,7 +521,7 @@ namespace airlib return waiter.isTimeout(); } - bool TiltrotorApiBase::rotateToYaw(float yaw, float timeout_sec, float margin) + bool VtolApiBase::rotateToYaw(float yaw, float timeout_sec, float margin) { SingleTaskCall lock(this); @@ -552,7 +552,7 @@ namespace airlib .isComplete(); } - bool TiltrotorApiBase::rotateByYawRate(float yaw_rate, float duration) + bool VtolApiBase::rotateByYawRate(float yaw_rate, float duration) { SingleTaskCall lock(this); @@ -570,93 +570,93 @@ namespace airlib .isTimeout(); } - void TiltrotorApiBase::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd) + void VtolApiBase::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd) { uint8_t controller_type = 2; setControllerGains(controller_type, kp, ki, kd); } - void TiltrotorApiBase::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd) + void VtolApiBase::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd) { uint8_t controller_type = 3; setControllerGains(controller_type, kp, ki, kd); } - void TiltrotorApiBase::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd) + void VtolApiBase::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd) { uint8_t controller_type = 4; setControllerGains(controller_type, kp, ki, kd); } - void TiltrotorApiBase::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd) + void VtolApiBase::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd) { uint8_t controller_type = 5; setControllerGains(controller_type, kp, ki, kd); } - bool TiltrotorApiBase::hover() + bool VtolApiBase::hover() { SingleTaskCall lock(this); return moveToZ(getPosition().z(), 0.5f, Utils::max(), YawMode{ true, 0 }, 1.0f, false); } - void TiltrotorApiBase::moveByRC(const RCData& rc_data) + void VtolApiBase::moveByRC(const RCData& rc_data) { unused(rc_data); //by default we say that this command is not supported - throw VehicleCommandNotImplementedException("moveByRC API is not implemented for this tiltrotor"); + throw VehicleCommandNotImplementedException("moveByRC API is not implemented for this vtol"); } - void TiltrotorApiBase::moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode) + void VtolApiBase::moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode) { if (safetyCheckVelocity(Vector3r(vx, vy, vz))) commandVelocity(vx, vy, vz, yaw_mode); } - void TiltrotorApiBase::moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode) + void VtolApiBase::moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode) { if (safetyCheckVelocityZ(vx, vy, z)) commandVelocityZ(vx, vy, z, yaw_mode); } - void TiltrotorApiBase::moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode) + void VtolApiBase::moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode) { if (safetyCheckDestination(dest)) commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode); } - void TiltrotorApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z) + void VtolApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z) { if (safetyCheckVelocity(getVelocity())) commandRollPitchYawZ(roll, pitch, yaw, z); } - void TiltrotorApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle) + void VtolApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle) { if (safetyCheckVelocity(getVelocity())) commandRollPitchYawThrottle(roll, pitch, yaw, throttle); } - void TiltrotorApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle) + void VtolApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle) { if (safetyCheckVelocity(getVelocity())) commandRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle); } - void TiltrotorApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z) + void VtolApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z) { if (safetyCheckVelocity(getVelocity())) commandRollPitchYawrateZ(roll, pitch, yaw_rate, z); } - void TiltrotorApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z) + void VtolApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z) { if (safetyCheckVelocity(getVelocity())) commandAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z); } - void TiltrotorApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle) + void VtolApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle) { if (safetyCheckVelocity(getVelocity())) commandAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle); @@ -664,7 +664,7 @@ namespace airlib //executes a given function until it returns true. Each execution is spaced apart at command period. //return value is true if exit was due to given function returning true, otherwise false (due to timeout) - Waiter TiltrotorApiBase::waitForFunction(WaitFunction function, float timeout_sec) + Waiter VtolApiBase::waitForFunction(WaitFunction function, float timeout_sec) { Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); if (timeout_sec <= 0) @@ -679,7 +679,7 @@ namespace airlib return waiter; } - bool TiltrotorApiBase::waitForZ(float timeout_sec, float z, float margin) + bool VtolApiBase::waitForZ(float timeout_sec, float z, float margin) { float cur_z = 100000; return waitForFunction([&]() { @@ -690,13 +690,13 @@ namespace airlib .isComplete(); } - void TiltrotorApiBase::setSafetyEval(const shared_ptr safety_eval_ptr) + void VtolApiBase::setSafetyEval(const shared_ptr safety_eval_ptr) { SingleCall lock(this); safety_eval_ptr_ = safety_eval_ptr; } - RCData TiltrotorApiBase::estimateRCTrims(float trimduration, float minCountForTrim, float maxTrim) + RCData VtolApiBase::estimateRCTrims(float trimduration, float minCountForTrim, float maxTrim) { rc_data_trims_ = RCData(); @@ -732,7 +732,7 @@ namespace airlib return rc_data_trims_; } - void TiltrotorApiBase::moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z) + void VtolApiBase::moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z) { unused(last_z); //validate dest @@ -773,7 +773,7 @@ namespace airlib moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode); } - bool TiltrotorApiBase::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + bool VtolApiBase::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z) { if (safety_eval_ptr_ == nullptr) @@ -788,7 +788,7 @@ namespace airlib return true; } - bool TiltrotorApiBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result) + bool VtolApiBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result) { if (!result.is_safe) { if (result.reason == SafetyEval::SafetyViolationType_::Obstacle) { @@ -815,7 +815,7 @@ namespace airlib return true; } - bool TiltrotorApiBase::safetyCheckVelocity(const Vector3r& velocity) + bool VtolApiBase::safetyCheckVelocity(const Vector3r& velocity) { if (safety_eval_ptr_ == nullptr) //safety checks disabled return true; @@ -823,7 +823,7 @@ namespace airlib const auto& result = safety_eval_ptr_->isSafeVelocity(getPosition(), velocity, getOrientation()); return emergencyManeuverIfUnsafe(result); } - bool TiltrotorApiBase::safetyCheckVelocityZ(float vx, float vy, float z) + bool VtolApiBase::safetyCheckVelocityZ(float vx, float vy, float z) { if (safety_eval_ptr_ == nullptr) //safety checks disabled return true; @@ -831,7 +831,7 @@ namespace airlib const auto& result = safety_eval_ptr_->isSafeVelocityZ(getPosition(), vx, vy, z, getOrientation()); return emergencyManeuverIfUnsafe(result); } - bool TiltrotorApiBase::safetyCheckDestination(const Vector3r& dest_pos) + bool VtolApiBase::safetyCheckDestination(const Vector3r& dest_pos) { if (safety_eval_ptr_ == nullptr) //safety checks disabled return true; @@ -840,7 +840,7 @@ namespace airlib return emergencyManeuverIfUnsafe(result); } - float TiltrotorApiBase::setNextPathPosition(const vector& path, const vector& path_segs, + float VtolApiBase::setNextPathPosition(const vector& path, const vector& path_segs, const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc) { //note: cur_path_loc and next_path_loc may both point to same object @@ -874,7 +874,7 @@ namespace airlib return next_dist; } - void TiltrotorApiBase::adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode) + void VtolApiBase::adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode) { //adjust yaw for the direction of travel in forward-only mode if (drivetrain == DrivetrainType::ForwardOnly && !yaw_mode.is_rate) { @@ -888,18 +888,18 @@ namespace airlib //else no adjustment needed } - void TiltrotorApiBase::adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode) + void VtolApiBase::adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode) { adjustYaw(Vector3r(x, y, 0), drivetrain, yaw_mode); } - bool TiltrotorApiBase::isYawWithinMargin(float yaw_target, float margin) const + bool VtolApiBase::isYawWithinMargin(float yaw_target, float margin) const { const float yaw_current = VectorMath::getYaw(getOrientation()) * 180 / M_PIf; return std::abs(yaw_current - yaw_target) <= margin; } - float TiltrotorApiBase::getAutoLookahead(float velocity, float adaptive_lookahead, + float VtolApiBase::getAutoLookahead(float velocity, float adaptive_lookahead, float max_factor, float min_factor) const { //if auto mode requested for lookahead then calculate based on velocity @@ -909,7 +909,7 @@ namespace airlib return lookahead; } - float TiltrotorApiBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const + float VtolApiBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const { unused(risk_dist); return max_obs_avoidance_vel; diff --git a/Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorRpcLibClient.cpp b/Source/AirLib/src/vehicles/vtol/api/VtolRpcLibClient.cpp similarity index 63% rename from Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorRpcLibClient.cpp rename to Source/AirLib/src/vehicles/vtol/api/VtolRpcLibClient.cpp index 3eaadc3..9dcda76 100644 --- a/Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorRpcLibClient.cpp +++ b/Source/AirLib/src/vehicles/vtol/api/VtolRpcLibClient.cpp @@ -7,7 +7,7 @@ #ifndef AIRLIB_NO_RPC //if using Unreal Build system then include precompiled header file first -#include "vehicles/tiltrotor/api/TiltrotorRpcLibClient.hpp" +#include "vehicles/vtol/api/VtolRpcLibClient.hpp" #include "common/Common.hpp" #include @@ -31,7 +31,7 @@ STRICT_MODE_OFF #endif #include "common/common_utils/WindowsApisCommonPost.hpp" -#include "vehicles/tiltrotor/api/TiltrotorRpcLibAdaptors.hpp" +#include "vehicles/vtol/api/VtolRpcLibAdaptors.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibAdaptors.hpp" STRICT_MODE_ON @@ -44,204 +44,204 @@ __pragma(warning(disable : 4239)) namespace airlib { - typedef msr::airlib_rpclib::TiltrotorRpcLibAdaptors TiltrotorRpcLibAdaptors; + typedef msr::airlib_rpclib::VtolRpcLibAdaptors VtolRpcLibAdaptors; typedef msr::airlib_rpclib::MultirotorRpcLibAdaptors MultirotorRpcLibAdaptors; - struct TiltrotorRpcLibClient::impl + struct VtolRpcLibClient::impl { public: std::future last_future; }; - TiltrotorRpcLibClient::TiltrotorRpcLibClient(const string& ip_address, uint16_t port, float timeout_sec) + VtolRpcLibClient::VtolRpcLibClient(const string& ip_address, uint16_t port, float timeout_sec) : RpcLibClientBase(ip_address, port, timeout_sec) { pimpl_.reset(new impl()); } - TiltrotorRpcLibClient::~TiltrotorRpcLibClient() + VtolRpcLibClient::~VtolRpcLibClient() { } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::takeoffAsync(float timeout_sec, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::takeoffAsync(float timeout_sec, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("takeoff", timeout_sec, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::landAsync(float timeout_sec, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::landAsync(float timeout_sec, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("land", timeout_sec, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::goHomeAsync(float timeout_sec, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::goHomeAsync(float timeout_sec, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("goHome", timeout_sec, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, + VtolRpcLibClient* VtolRpcLibClient::moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityBodyFrame", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByVelocityZBodyFrameAsync(float vx, float vy, float z, float duration, + VtolRpcLibClient* VtolRpcLibClient::moveByVelocityZBodyFrameAsync(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityZBodyFrame", vx, vy, z, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByMotorPWMsAsync(const vector& pwm_values, float duration, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::moveByMotorPWMsAsync(const vector& pwm_values, float duration, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByMotorPWMs", pwm_values, duration, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawZ", roll, pitch, yaw, z, duration, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawThrottle", roll, pitch, yaw, throttle, duration, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateThrottle", roll, pitch, yaw_rate, throttle, duration, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateZ", roll, pitch, yaw_rate, z, duration, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesZ", roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesThrottle", roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByVelocityAsync(float vx, float vy, float vz, float duration, + VtolRpcLibClient* VtolRpcLibClient::moveByVelocityAsync(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocity", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByVelocityZAsync(float vx, float vy, float z, float duration, + VtolRpcLibClient* VtolRpcLibClient::moveByVelocityZAsync(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityZ", vx, vy, z, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveOnPathAsync(const vector& path, float velocity, float duration, + VtolRpcLibClient* VtolRpcLibClient::moveOnPathAsync(const vector& path, float velocity, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) { - vector conv_path; - TiltrotorRpcLibAdaptors::from(path, conv_path); + vector conv_path; + VtolRpcLibAdaptors::from(path, conv_path); pimpl_->last_future = static_cast(getClient())->async_call("moveOnPath", conv_path, velocity, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec, + VtolRpcLibClient* VtolRpcLibClient::moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveToPosition", x, y, z, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveToZAsync(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::moveToZAsync(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveToZ", z, velocity, timeout_sec, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, + VtolRpcLibClient* VtolRpcLibClient::moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("moveByManual", vx_max, vy_max, z_min, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::rotateToYawAsync(float yaw, float timeout_sec, float margin, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::rotateToYawAsync(float yaw, float timeout_sec, float margin, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("rotateToYaw", yaw, timeout_sec, margin, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("rotateByYawRate", yaw_rate, duration, vehicle_name); return this; } - TiltrotorRpcLibClient* TiltrotorRpcLibClient::hoverAsync(const std::string& vehicle_name) + VtolRpcLibClient* VtolRpcLibClient::hoverAsync(const std::string& vehicle_name) { pimpl_->last_future = static_cast(getClient())->async_call("hover", vehicle_name); return this; } - void TiltrotorRpcLibClient::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + void VtolRpcLibClient::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) { static_cast(getClient())->call("setAngleLevelControllerGains", kp, ki, kd, vehicle_name); } - void TiltrotorRpcLibClient::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + void VtolRpcLibClient::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) { static_cast(getClient())->call("setAngleRateControllerGains", kp, ki, kd, vehicle_name); } - void TiltrotorRpcLibClient::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + void VtolRpcLibClient::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) { static_cast(getClient())->call("setVelocityControllerGains", kp, ki, kd, vehicle_name); } - void TiltrotorRpcLibClient::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + void VtolRpcLibClient::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) { static_cast(getClient())->call("setPositionControllerGains", kp, ki, kd, vehicle_name); } - bool TiltrotorRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + bool VtolRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) { - return static_cast(getClient())->call("setSafety", static_cast(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, TiltrotorRpcLibAdaptors::Vector3r(origin), xy_length, max_z, min_z, vehicle_name).as(); + return static_cast(getClient())->call("setSafety", static_cast(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, VtolRpcLibAdaptors::Vector3r(origin), xy_length, max_z, min_z, vehicle_name).as(); } //status getters // Rotor state getter - RotorTiltableStates TiltrotorRpcLibClient::getRotorStates(const std::string& vehicle_name) + RotorTiltableStates VtolRpcLibClient::getRotorStates(const std::string& vehicle_name) { - return static_cast(getClient())->call("getRotorStates", vehicle_name).as().to(); + return static_cast(getClient())->call("getRotorStates", vehicle_name).as().to(); } - // Tiltrotor state getter - TiltrotorState TiltrotorRpcLibClient::getTiltrotorState(const std::string& vehicle_name) + // Vtol state getter + VtolState VtolRpcLibClient::getVtolState(const std::string& vehicle_name) { - return static_cast(getClient())->call("getTiltrotorState", vehicle_name).as().to(); + return static_cast(getClient())->call("getVtolState", vehicle_name).as().to(); } - void TiltrotorRpcLibClient::moveByRC(const RCData& rc_data, const std::string& vehicle_name) + void VtolRpcLibClient::moveByRC(const RCData& rc_data, const std::string& vehicle_name) { - static_cast(getClient())->call("moveByRC", TiltrotorRpcLibAdaptors::RCData(rc_data), vehicle_name); + static_cast(getClient())->call("moveByRC", VtolRpcLibAdaptors::RCData(rc_data), vehicle_name); } //return value of last task. It should be true if task completed without //cancellation or timeout - TiltrotorRpcLibClient* TiltrotorRpcLibClient::waitOnLastTask(bool* task_result, float timeout_sec) + VtolRpcLibClient* VtolRpcLibClient::waitOnLastTask(bool* task_result, float timeout_sec) { bool result; if (std::isnan(timeout_sec) || timeout_sec == Utils::max()) diff --git a/Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorRpcLibServer.cpp b/Source/AirLib/src/vehicles/vtol/api/VtolRpcLibServer.cpp similarity index 85% rename from Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorRpcLibServer.cpp rename to Source/AirLib/src/vehicles/vtol/api/VtolRpcLibServer.cpp index 525001b..7734b3b 100644 --- a/Source/AirLib/src/vehicles/tiltrotor/api/TiltrotorRpcLibServer.cpp +++ b/Source/AirLib/src/vehicles/vtol/api/VtolRpcLibServer.cpp @@ -7,7 +7,7 @@ #ifndef AIRLIB_NO_RPC //if using Unreal Build system then include pre-compiled header file first -#include "vehicles/tiltrotor/api/TiltrotorRpcLibServer.hpp" +#include "vehicles/vtol/api/VtolRpcLibServer.hpp" #include "common/Common.hpp" STRICT_MODE_OFF @@ -28,7 +28,7 @@ STRICT_MODE_OFF #endif #include "common/common_utils/WindowsApisCommonPost.hpp" -#include "vehicles/tiltrotor/api/TiltrotorRpcLibAdaptors.hpp" +#include "vehicles/vtol/api/VtolRpcLibAdaptors.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibAdaptors.hpp" STRICT_MODE_ON @@ -38,10 +38,10 @@ namespace msr namespace airlib { - typedef msr::airlib_rpclib::TiltrotorRpcLibAdaptors TiltrotorRpcLibAdaptors; + typedef msr::airlib_rpclib::VtolRpcLibAdaptors VtolRpcLibAdaptors; typedef msr::airlib_rpclib::MultirotorRpcLibAdaptors MultirotorRpcLibAdaptors; - TiltrotorRpcLibServer::TiltrotorRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port) + VtolRpcLibServer::VtolRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port) : RpcLibServerBase(api_provider, server_address, port) { (static_cast(getServer()))->bind("takeoff", [&](float timeout_sec, const std::string& vehicle_name) -> bool { @@ -86,9 +86,9 @@ namespace airlib (static_cast(getServer()))->bind("moveByVelocityZ", [&](float vx, float vy, float z, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->moveByVelocityZ(vx, vy, z, duration, drivetrain, yaw_mode.to()); }); - (static_cast(getServer()))->bind("moveOnPath", [&](const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + (static_cast(getServer()))->bind("moveOnPath", [&](const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { vector conv_path; - TiltrotorRpcLibAdaptors::to(path, conv_path); + VtolRpcLibAdaptors::to(path, conv_path); return getVehicleApi(vehicle_name)->moveOnPath(conv_path, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); }); (static_cast(getServer()))->bind("moveToPosition", [&](float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { @@ -122,31 +122,31 @@ namespace airlib (static_cast(getServer()))->bind("setPositionControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->setPositionControllerGains(kp, ki, kd); }); - (static_cast(getServer()))->bind("moveByRC", [&](const TiltrotorRpcLibAdaptors::RCData& data, const std::string& vehicle_name) -> void { + (static_cast(getServer()))->bind("moveByRC", [&](const VtolRpcLibAdaptors::RCData& data, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->moveByRC(data.to()); }); - (static_cast(getServer()))->bind("setSafety", [&](uint enable_reasons, float obs_clearance, const SafetyEval::ObsAvoidanceStrategy& obs_startegy, float obs_avoidance_vel, const TiltrotorRpcLibAdaptors::Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) -> bool { + (static_cast(getServer()))->bind("setSafety", [&](uint enable_reasons, float obs_clearance, const SafetyEval::ObsAvoidanceStrategy& obs_startegy, float obs_avoidance_vel, const VtolRpcLibAdaptors::Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->setSafety(SafetyEval::SafetyViolationType(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, origin.to(), xy_length, max_z, min_z); }); - (static_cast(getServer()))->bind("simSetTiltrotorPose", [&](const TiltrotorRpcLibAdaptors::Pose& pose, const vector& tilt_angles, bool ignore_collision, bool spin_props, const std::string& vehicle_name) -> void { + (static_cast(getServer()))->bind("simSetVtolPose", [&](const VtolRpcLibAdaptors::Pose& pose, const vector& tilt_angles, bool ignore_collision, bool spin_props, const std::string& vehicle_name) -> void { getVehicleSimApi(vehicle_name)->setPoseCustom(pose.to(), tilt_angles, ignore_collision, spin_props); }); //getters // Rotor state - (static_cast(getServer()))->bind("getRotorStates", [&](const std::string& vehicle_name) -> TiltrotorRpcLibAdaptors::RotorTiltableStates { - return TiltrotorRpcLibAdaptors::RotorTiltableStates(getVehicleApi(vehicle_name)->getRotorStates()); + (static_cast(getServer()))->bind("getRotorStates", [&](const std::string& vehicle_name) -> VtolRpcLibAdaptors::RotorTiltableStates { + return VtolRpcLibAdaptors::RotorTiltableStates(getVehicleApi(vehicle_name)->getRotorStates()); }); - // Tiltrotor state - (static_cast(getServer()))->bind("getTiltrotorState", [&](const std::string& vehicle_name) -> TiltrotorRpcLibAdaptors::TiltrotorState { - return TiltrotorRpcLibAdaptors::TiltrotorState(getVehicleApi(vehicle_name)->getTiltrotorState()); + // Vtol state + (static_cast(getServer()))->bind("getVtolState", [&](const std::string& vehicle_name) -> VtolRpcLibAdaptors::VtolState { + return VtolRpcLibAdaptors::VtolState(getVehicleApi(vehicle_name)->getVtolState()); }); } //required for pimpl - TiltrotorRpcLibServer::~TiltrotorRpcLibServer() + VtolRpcLibServer::~VtolRpcLibServer() { } } diff --git a/Source/SimHUD/SimHUD.cpp b/Source/SimHUD/SimHUD.cpp index 2a2ca80..94477fe 100644 --- a/Source/SimHUD/SimHUD.cpp +++ b/Source/SimHUD/SimHUD.cpp @@ -299,7 +299,7 @@ void ASimHUD::createSimMode() simmode_ = this->GetWorld()->SpawnActor(FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); - else if (simmode_name == AirSimSettings::kSimModeTypeTiltrotor) + else if (simmode_name == AirSimSettings::kSimModeTypeVtol) simmode_ = this->GetWorld()->SpawnActor(FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); diff --git a/Source/Vehicles/Tiltrotor/SimModeWorldTiltrotor.cpp b/Source/Vehicles/Tiltrotor/SimModeWorldTiltrotor.cpp index 16eb9ac..0c93e08 100644 --- a/Source/Vehicles/Tiltrotor/SimModeWorldTiltrotor.cpp +++ b/Source/Vehicles/Tiltrotor/SimModeWorldTiltrotor.cpp @@ -8,12 +8,12 @@ #include "GameFramework/PlayerController.h" #include "AirBlueprintLib.h" -#include "vehicles/tiltrotor/api/TiltrotorApiBase.hpp" +#include "vehicles/vtol/api/VtolApiBase.hpp" #include "TiltrotorPawnSimApi.h" #include "physics/PhysicsBody.hpp" #include "common/ClockFactory.hpp" #include -#include "vehicles/tiltrotor/api/TiltrotorRpcLibServer.hpp" +#include "vehicles/vtol/api/VtolRpcLibServer.hpp" #include "common/SteppableClock.hpp" void ASimModeWorldTiltrotor::BeginPlay() @@ -82,7 +82,7 @@ std::unique_ptr ASimModeWorldTiltrotor::createApiSer #ifdef AIRLIB_NO_RPC return ASimModeBase::createApiServer(); #else - return std::unique_ptr(new msr::airlib::TiltrotorRpcLibServer( + return std::unique_ptr(new msr::airlib::VtolRpcLibServer( getApiProvider(), getSettings().api_server_address, getSettings().api_port)); #endif } @@ -94,8 +94,8 @@ void ASimModeWorldTiltrotor::getExistingVehiclePawns(TArray& pawns) con bool ASimModeWorldTiltrotor::isVehicleTypeSupported(const std::string& vehicle_type) const { - return ((vehicle_type == AirSimSettings::kVehicleTypeTiltrotorSimple) || - (vehicle_type == AirSimSettings::kVehicleTypePX4Tiltrotor)); + return ((vehicle_type == AirSimSettings::kVehicleTypeVtolSimple) || + (vehicle_type == AirSimSettings::kVehicleTypePX4Vtol)); } std::string ASimModeWorldTiltrotor::getVehiclePawnPathName(const AirSimSettings::VehicleSetting& vehicle_setting) const @@ -103,7 +103,7 @@ std::string ASimModeWorldTiltrotor::getVehiclePawnPathName(const AirSimSettings: //decide which derived BP to use std::string pawn_path = vehicle_setting.pawn_path; if (pawn_path == "") - pawn_path = "DefaultTiltrotor"; + pawn_path = "DefaultVtol"; return pawn_path; } @@ -133,6 +133,6 @@ std::unique_ptr ASimModeWorldTiltrotor::createVehicleSimApi( msr::airlib::VehicleApiBase* ASimModeWorldTiltrotor::getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, const PawnSimApi* sim_api) const { - const auto tiltrotor_sim_api = static_cast(sim_api); - return tiltrotor_sim_api->getVehicleApi(); + const auto vtol_sim_api = static_cast(sim_api); + return vtol_sim_api->getVehicleApi(); } diff --git a/Source/Vehicles/Tiltrotor/TiltrotorPawnSimApi.cpp b/Source/Vehicles/Tiltrotor/TiltrotorPawnSimApi.cpp index 35d6961..0cc34d8 100644 --- a/Source/Vehicles/Tiltrotor/TiltrotorPawnSimApi.cpp +++ b/Source/Vehicles/Tiltrotor/TiltrotorPawnSimApi.cpp @@ -1,6 +1,6 @@ #include "TiltrotorPawnSimApi.h" #include "AirBlueprintLib.h" -#include "vehicles/tiltrotor/AeroBodyParamsFactory.hpp" +#include "vehicles/vtol/AeroBodyParamsFactory.hpp" #include "UnrealSensors/UnrealSensorFactory.h" #include @@ -18,7 +18,7 @@ void TiltrotorPawnSimApi::initialize() //create vehicle API std::shared_ptr sensor_factory = std::make_shared(getPawn(), &getNedTransform()); vehicle_params_ = AeroBodyParamsFactory::createConfig(getVehicleSetting(), sensor_factory); - vehicle_api_ = vehicle_params_->createTiltrotorApi(); + vehicle_api_ = vehicle_params_->createVtolApi(); //setup physics vehicle aero_physics_body_ = std::unique_ptr(new AeroBody(vehicle_params_.get(), vehicle_api_.get(), getKinematics(), getEnvironment())); diff --git a/Source/Vehicles/Tiltrotor/TiltrotorPawnSimApi.h b/Source/Vehicles/Tiltrotor/TiltrotorPawnSimApi.h index 5851a05..0f98c99 100644 --- a/Source/Vehicles/Tiltrotor/TiltrotorPawnSimApi.h +++ b/Source/Vehicles/Tiltrotor/TiltrotorPawnSimApi.h @@ -3,8 +3,8 @@ #include "CoreMinimal.h" #include "PawnSimApi.h" -#include "vehicles/tiltrotor/AeroBody.hpp" -#include "vehicles/tiltrotor/AeroBodyParams.hpp" +#include "vehicles/vtol/AeroBody.hpp" +#include "vehicles/vtol/AeroBodyParams.hpp" #include "common/Common.hpp" #include "common/CommonStructs.hpp" #include "common/common_utils/UniqueValueMap.hpp" @@ -47,7 +47,7 @@ class TiltrotorPawnSimApi : public PawnSimApi virtual void setPoseCustom(const Pose& pose, const vector& tilt_angles, bool ignore_collision, bool spin_props) override; virtual void updateRotors(); - msr::airlib::TiltrotorApiBase* getVehicleApi() const + msr::airlib::VtolApiBase* getVehicleApi() const { return vehicle_api_.get(); } @@ -58,7 +58,7 @@ class TiltrotorPawnSimApi : public PawnSimApi } private: - std::unique_ptr vehicle_api_; + std::unique_ptr vehicle_api_; std::unique_ptr vehicle_params_; std::unique_ptr aero_physics_body_; From 5dfe247c3e498fab9b99639e7e15ab2ef9ff62eb Mon Sep 17 00:00:00 2001 From: Seth Nielsen Date: Tue, 12 Oct 2021 15:54:49 -0600 Subject: [PATCH 3/7] Add clean.sh for cleaning between builds --- clean.sh | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100755 clean.sh diff --git a/clean.sh b/clean.sh new file mode 100755 index 0000000..a04510d --- /dev/null +++ b/clean.sh @@ -0,0 +1,20 @@ +#! /bin/bash + +# get path of current script: https://stackoverflow.com/a/39340259/207661 +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +pushd "$SCRIPT_DIR" >/dev/null + +set -e +set -x + +# clean temporary unreal folders +rm -rf Binaries +rm -rf Intermediate +rm -rf Saved +rm -rf ../../Binaries +rm -rf ../../Intermediate +rm -rf ../../Saved +rm -f ../../CMakeLists.txt +rm -f ../../Makefile + +popd >/dev/null From a0b8380ed2be61e5eaf84ec61e3349119ccbe148 Mon Sep 17 00:00:00 2001 From: Seth Nielsen Date: Thu, 14 Oct 2021 14:41:57 -0600 Subject: [PATCH 4/7] Rename in docs --- DEVELOPMENT.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DEVELOPMENT.md b/DEVELOPMENT.md index 710ff68..f5adb97 100644 --- a/DEVELOPMENT.md +++ b/DEVELOPMENT.md @@ -8,7 +8,7 @@ Run `build_static_libs.sh` **first** if you are setting up vtol-AirSim for the f You will also need to re-run the script whenever you modify the `.hpp` or `.cpp` files associated with any of the following C++ classes: - Any class with `*RpcLibClient*` or `*RpcLibServer*` in the name -- `MultirotorApiBase` or `TiltrotorApiBase` +- `MultirotorApiBase` or `VtolApiBase` - `SafetyEval`, `ObstacleMap`, or `FileSystem` If you have modified any of those files, then run `build_static_libs.sh` **first**, and then perform one of the build methods listed below. From 6aadf08591c6d2786669ca8dc1a50c6d28050819 Mon Sep 17 00:00:00 2001 From: Seth Nielsen Date: Thu, 14 Oct 2021 16:42:26 -0600 Subject: [PATCH 5/7] Add check for cmake installed --- build_static_libs.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build_static_libs.sh b/build_static_libs.sh index f7d3fd1..d863b5e 100755 --- a/build_static_libs.sh +++ b/build_static_libs.sh @@ -122,7 +122,7 @@ else build_dir=build_release fi -CMAKE=$(which cmake) +CMAKE=$(which cmake) || (echo "cmake not found -- install cmake first and rerun script" && exit 1) export CC="clang" export CXX="clang++" From ab4446023c3862a568e258a3949c301ecedc407e Mon Sep 17 00:00:00 2001 From: Seth Nielsen Date: Sat, 16 Oct 2021 13:29:56 -0600 Subject: [PATCH 6/7] Remove comment --- Source/AirLib/include/vehicles/vtol/RotorTiltable.hpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/Source/AirLib/include/vehicles/vtol/RotorTiltable.hpp b/Source/AirLib/include/vehicles/vtol/RotorTiltable.hpp index 2d6dfed..1d2e81e 100644 --- a/Source/AirLib/include/vehicles/vtol/RotorTiltable.hpp +++ b/Source/AirLib/include/vehicles/vtol/RotorTiltable.hpp @@ -107,14 +107,6 @@ namespace airlib tilt_output_.rotor_output.torque_scaler = 0.0f; tilt_output_.rotor_output.control_signal_input = 0.0f; tilt_output_.rotor_output.control_signal_filtered = 0.0f; - - // compute angle from nominal given angle from vertical - // probably not worth the extra computation - // real_T angle_nom_from_vert = VectorMath::sgn(normal_nominal_(0)) * std::acos(-normal_nominal_(2)); - // AngleAxisr rotate_cv = AngleAxisr(angle, rotation_axis_); - // AngleAxisr rotate_nc = AngleAxisr(-angle_nom_from_vert, rotation_axis_); - // Vector3r normal = rotate_cv * rotate_nc * normal_nominal_; - // tilt_output_.angle = std::acos( normal.dot(normal_nominal_) / ( normal.norm() * normal_nominal_.norm() ) ); } TiltOutput getOutput() const From ab74ae9ab0458b902344065e0c95c77aa75fdc4a Mon Sep 17 00:00:00 2001 From: Seth Nielsen Date: Tue, 19 Oct 2021 11:38:29 -0600 Subject: [PATCH 7/7] Increase default parasitic drag (CD_P) --- Source/AirLib/include/vehicles/vtol/AeroParams.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Source/AirLib/include/vehicles/vtol/AeroParams.hpp b/Source/AirLib/include/vehicles/vtol/AeroParams.hpp index a564af0..16ecdbf 100644 --- a/Source/AirLib/include/vehicles/vtol/AeroParams.hpp +++ b/Source/AirLib/include/vehicles/vtol/AeroParams.hpp @@ -88,7 +88,9 @@ namespace airlib // //aerodynamic coefficients AeroCoefficient CL{ 0.005, 2.819, 0.0, 0.0, 3.242, 0.0, 0.0, 0.2, 0.0 }; - AeroCoefficient CD{ 0.022, 0.3, 0.0, 0.05, 0.0, 0.0, 0.0, 0.5, 0.0 }; + // CD_p is parasitic drag; was set to be 0.05, but increased its value to 0.15 after + // tests showed that longitudinal drag was too low + AeroCoefficient CD{ 0.022, 0.3, 0.0, 0.15, 0.0, 0.0, 0.0, 0.5, 0.0 }; AeroCoefficient CY{ 0.0, 0.0, -0.318, 0.078, 0.0, 0.288, 0.000536, 0.0, 0.0 }; AeroCoefficient Cl{ 0.0, 0.0, -0.032, -0.207, 0.0, 0.036, 0.018, 0.0, 0.0 }; AeroCoefficient Cm{ 0.0, -0.185, 0.0, 0.0, -1.093, 0.0, 0.0, -0.05, 0.0 };