added moveByAngleThrottle command

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<OffboardCommand> cmd = std::make_shared<MoveByAngleThrottle>(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<OffboardCommand> cmd = std::make_shared<MoveByVelocity>(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:

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());

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 ************/

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);

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);

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));

AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp

@@ -66,6 +66,11 @@ bool MultirotorRpcLibClient::moveByAngleZ(float pitch, float roll, float z, floa
     return static_cast<rpc::client*>(getClient())->call("moveByAngleZ", pitch, roll, z, yaw, duration).as<bool>();
 }
 
+bool MultirotorRpcLibClient::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration)
+{
+    return static_cast<rpc::client*>(getClient())->call("moveByAngleThrottle", pitch, roll, throttle, yaw_rate, duration).as<bool>();
+}
+
 bool MultirotorRpcLibClient::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode)
 {
     return static_cast<rpc::client*>(getClient())->call("moveByVelocity", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode)).as<bool>();

AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp

@@ -48,6 +48,9 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(MultirotorApi* drone, string serv
     (static_cast<rpc::server*>(getServer()))->
         bind("moveByAngleZ", [&](float pitch, float roll, float z, float yaw, float duration) -> 
         bool { return getDroneApi()->moveByAngleZ(pitch, roll, z, yaw, duration); });
+    (static_cast<rpc::server*>(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<rpc::server*>(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()); });

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()))

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);

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)