diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp index 1de732384f..aa688eac56 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp @@ -89,6 +89,12 @@ class MultirotorApi : public VehicleApiBase { return enqueueCommand(cmd); } + bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration) + { + std::shared_ptr cmd = std::make_shared(controller_, pitch, roll, throttle, yaw_rate, duration); + return enqueueCommand(cmd); + } + bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { std::shared_ptr cmd = std::make_shared(controller_, vx, vy, vz, duration, drivetrain, yaw_mode); @@ -384,6 +390,21 @@ class MultirotorApi : public VehicleApiBase { return true; } + class MoveByAngleThrottle : public OffboardCommand { + float pitch_, roll_, throttle_, yaw_rate_, duration_; + public: + MoveByAngleThrottle(DroneControllerBase* controller, float pitch, float roll, float throttle, float yaw_rate, float duration) : OffboardCommand(controller) { + this->pitch_ = pitch; + this->roll_ = roll; + this->throttle_ = throttle; + this->yaw_rate_ = yaw_rate; + this->duration_ = duration; + } + virtual void executeImpl(DroneControllerBase* controller, CancelableBase& cancelable) override { + controller->moveByAngleThrottle (pitch_, roll_, throttle_, yaw_rate_, duration_, cancelable); + } + }; + class MoveByAngleZ : public OffboardCommand { float pitch_, roll_, z_, yaw_, duration_; public: diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index e09a577ed9..ed07d69a61 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -24,6 +24,7 @@ class MultirotorRpcLibClient : public RpcLibClientBase { bool land(float max_wait_seconds = 60); bool goHome(); bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration); + bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration); bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode()); diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp index 445c36700a..77d94644a7 100644 --- a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp @@ -108,6 +108,9 @@ class DroneControllerBase : public VehicleControllerBase { virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration , CancelableBase& cancelable_action); + /// Move by providing angles and throttles just lik ein RC + virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration + , CancelableBase& cancelable_action); /// Move the drone by controlling the velocity vector of the drone. A little bit of vx can /// make the drone move forwards, a little bit of vy can make it move sideways. A bit of vz can move @@ -240,6 +243,7 @@ class DroneControllerBase : public VehicleControllerBase { //all angles in degrees, lengths in meters, velocities in m/s, durations in seconds //all coordinates systems are world NED (+x is North, +y is East, +z is down) virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) = 0; + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) = 0; virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; @@ -272,6 +276,7 @@ class DroneControllerBase : public VehicleControllerBase { virtual bool moveByVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode); virtual bool moveToPosition(const Vector3r& dest, const YawMode& yaw_mode); virtual bool moveByRollPitchZ(float pitch, float roll, float z, float yaw); + virtual bool moveByRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate); //**************************************************************************************************************************** /************* safety checks & emergency manuevers ************/ diff --git a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp index 8cd4b3968d..91a4c7fb44 100644 --- a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp @@ -144,6 +144,7 @@ class MavLinkDroneController : public DroneControllerBase virtual void loopCommandPost() override; protected: virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override; + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override; virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override; virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override; virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override; @@ -1165,6 +1166,11 @@ struct MavLinkDroneController::impl { return rc; } + void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) + { + checkVehicle(); + mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle); + } void commandRollPitchZ(float pitch, float roll, float z, float yaw) { if (target_height_ != -z) { @@ -1490,6 +1496,10 @@ bool MavLinkDroneController::goHome(CancelableBase& cancelable_action) return pimpl_->goHome(cancelable_action); } +void MavLinkDroneController::commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) +{ + return pimpl_->commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); +} void MavLinkDroneController::commandRollPitchZ(float pitch, float roll, float z, float yaw) { return pimpl_->commandRollPitchZ(pitch, roll, z, yaw); diff --git a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp index f7eeef8024..5c5a6fda58 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp @@ -249,6 +249,16 @@ class RosFlightDroneController : public DroneControllerBase { } protected: + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + { + unused(pitch); + unused(roll); + unused(throttle); + unused(yaw_rate); + + //TODO: implement this + } + virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override { unused(pitch); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp index 08b824d429..b98c7a91cf 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp @@ -225,6 +225,19 @@ class SimpleFlightDroneController : public DroneControllerBase { } protected: + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + { + Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll, pitch, yaw_rate, throttle); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override { Utils::log(Utils::stringf("commandRollPitchZ %f, %f, %f, %f", pitch, roll, z, yaw)); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 529130cd72..46df2b0c89 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -66,6 +66,11 @@ bool MultirotorRpcLibClient::moveByAngleZ(float pitch, float roll, float z, floa return static_cast(getClient())->call("moveByAngleZ", pitch, roll, z, yaw, duration).as(); } +bool MultirotorRpcLibClient::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration) +{ + return static_cast(getClient())->call("moveByAngleThrottle", pitch, roll, throttle, yaw_rate, duration).as(); +} + bool MultirotorRpcLibClient::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { return static_cast(getClient())->call("moveByVelocity", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode)).as(); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 2c7829c1f8..bee4df9ba2 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -48,6 +48,9 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(MultirotorApi* drone, string serv (static_cast(getServer()))-> bind("moveByAngleZ", [&](float pitch, float roll, float z, float yaw, float duration) -> bool { return getDroneApi()->moveByAngleZ(pitch, roll, z, yaw, duration); }); + (static_cast(getServer()))-> + bind("moveByAngleThrottle", [&](float pitch, float roll, float throttle, float yaw_rate, float duration) -> + bool { return getDroneApi()->moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); }); (static_cast(getServer()))-> bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode) -> bool { return getDroneApi()->moveByVelocity(vx, vy, vz, duration, drivetrain, yaw_mode.to()); }); diff --git a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp index 07c06b9f13..73f0e75733 100644 --- a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp +++ b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp @@ -57,6 +57,17 @@ bool DroneControllerBase::moveByAngleZ(float pitch, float roll, float z, float y }, duration, cancelable_action); } +bool DroneControllerBase::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration + , CancelableBase& cancelable_action) +{ + if (duration <= 0) + return true; + + return !waitForFunction([&]() { + return !moveByRollPitchThrottle(pitch, roll, throttle, yaw_rate); + }, duration, cancelable_action); +} + bool DroneControllerBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action) { @@ -374,6 +385,14 @@ bool DroneControllerBase::moveToPosition(const Vector3r& dest, const YawMode& ya return true; } +bool DroneControllerBase::moveByRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); + + return true; +} + bool DroneControllerBase::moveByRollPitchZ(float pitch, float roll, float z, float yaw) { if (safetyCheckVelocity(getVelocity())) diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp index 12b28b2c17..a51e6ea775 100644 --- a/DroneShell/src/main.cpp +++ b/DroneShell/src/main.cpp @@ -510,6 +510,35 @@ class MoveByAngleZCommand : public DroneCommand { } }; + +class MoveByAngleThrottleCommand : public DroneCommand { +public: + MoveByAngleThrottleCommand() : DroneCommand("MoveByAngleThrottle", "Move with specified roll and pitch, leaving z as-is") + { + this->addSwitch({ "-pitch", "0", "pitch angle in degrees (default 0)" }); + this->addSwitch({ "-roll", "0", "roll angle in degrees (default 0)" }); + this->addSwitch({ "-yaw_rate", "0", "target yaw rate in degrees/sec (default is 0)" }); + this->addSwitch({ "-throttle", "-2.5", "z position in meters (default -2.5)" }); + this->addSwitch({ "-duration", "5", "the duration of this command in seconds (default 5)" }); + } + + bool execute(const DroneCommandParameters& params) + { + float pitch = getSwitch("-pitch").toFloat(); + float roll = getSwitch("-roll").toFloat(); + float yaw_rate = getSwitch("-yaw_rate").toFloat(); + float throttle = getSwitch("-throttle").toFloat(); + float duration = getSwitch("-duration").toFloat(); + CommandContext* context = params.context; + + context->tasker.execute([=]() { + context->client.moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); + }); + + return false; + } +}; + class MoveByVelocityCommand : public DroneCommand { public: MoveByVelocityCommand() : DroneCommand("MoveByVelocity", "Move by specified velocity components vx, vy, vz, axis wrt body") @@ -1352,6 +1381,7 @@ int main(int argc, const char *argv[]) { GetPositionCommand getPosition; MoveByManualCommand moveByManual; MoveByAngleZCommand moveByAngleZ; + MoveByAngleThrottleCommand moveByAngleThrottle; MoveByVelocityCommand moveByVelocity; MoveByVelocityZCommand moveByVelocityZ; MoveOnPathCommand moveOnPath; @@ -1384,6 +1414,7 @@ int main(int argc, const char *argv[]) { shell.addCommand(moveToPosition); shell.addCommand(moveByManual); shell.addCommand(moveByAngleZ); + shell.addCommand(moveByAngleThrottle); shell.addCommand(moveByVelocity); shell.addCommand(moveByVelocityZ); shell.addCommand(moveOnPath); diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 787571e054..0072811df0 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -550,6 +550,9 @@ def getServerDebugInfo(self): def moveByAngleZ(self, pitch, roll, z, yaw, duration): return self.client.call('moveByAngleZ', pitch, roll, z, yaw, duration) + def moveByAngleThrottle(self, pitch, roll, throttle, yaw_rate, duration): + return self.client.call('moveByAngleThrottle', pitch, roll, throttle, yaw_rate, duration) + def moveByVelocity(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): return self.client.call('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode)