Merge pull request #1 from Microsoft/master

Bring fork up to date

AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp

@@ -18,7 +18,7 @@ using namespace msr::airlib;
 
 namespace msr { namespace airlib {
 
-// We want to make it possible for MultirotorRpcLibClient to call the offboard movement methods (moveByAngle, moveByVelocity, etc) at a high
+// We want to make it possible for MultirotorRpcLibClient to call the offboard movement methods (moveByAngleZ, moveByVelocity, etc) at a high
 // rate, like 30 times a second.  But we also want these movement methods to drive the drone at a reliable rate which we do inside
 // DroneControllerBase using the Waiter object so it pumps the virtual commandVelocity method at a fixed rate defined by getCommandPeriod.
 // This fixed rate is needed by the drone flight controller (for example PX4) because the flight controller usually reverts to
@@ -83,9 +83,15 @@ class MultirotorApi : public VehicleApiBase {
         return controller_->goHome(*pending_);
     }
 
-    bool moveByAngle(float pitch, float roll, float z, float yaw, float duration)
+    bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration)
     {
-        std::shared_ptr<OffboardCommand> cmd = std::make_shared<MoveByAngle>(controller_, pitch, roll, z, yaw, duration);
+        std::shared_ptr<OffboardCommand> cmd = std::make_shared<MoveByAngleZ>(controller_, pitch, roll, z, yaw, duration);
+        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);
     }
 
@@ -384,18 +390,33 @@ class MultirotorApi : public VehicleApiBase {
         return true;
     }
 
-    class MoveByAngle : public OffboardCommand {
+    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:
-        MoveByAngle(DroneControllerBase* controller, float pitch, float roll, float z, float yaw, float duration) : OffboardCommand(controller) {
+        MoveByAngleZ(DroneControllerBase* controller, float pitch, float roll, float z, float yaw, float duration) : OffboardCommand(controller) {
             this->pitch_ = pitch;
             this->roll_ = roll;
             this->z_ = z;
             this->yaw_ = yaw;
             this->duration_ = duration;
         }
         virtual void executeImpl(DroneControllerBase* controller, CancelableBase& cancelable) override {
-            controller->moveByAngle(pitch_, roll_, z_, yaw_, duration_, cancelable);
+            controller->moveByAngleZ(pitch_, roll_, z_, yaw_, duration_, cancelable);
         }
     };
 

AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp

@@ -23,7 +23,8 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
     bool takeoff(float max_wait_ms = 15);
     bool land(float max_wait_seconds = 60);
     bool goHome();
-    bool moveByAngle(float pitch, float roll, float z, float yaw, float duration);
+    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

@@ -102,12 +102,15 @@ class DroneControllerBase : public VehicleControllerBase {
     /// make the drone move forwards, a little bit of roll can make it move sideways.  The yaw control can
     /// make the drone spin around on the spot.  The duration says how long you want to apply these settings
     /// before reverting to a hover command.  So you can say "fly forwards slowly for 1 second" using 
-    /// moveByAngle(0.1, 0, z, yaw, 1, ...).  The cancelable_action can be used to canel all actions.  In fact,
+    /// moveByAngleZ(0.1, 0, z, yaw, 1, ...).  The cancelable_action can be used to canel all actions.  In fact,
     /// every time you call another move* method you will automatically cancel any previous action that is
     /// happening.
-    virtual bool moveByAngle(float pitch, float roll, float z, float yaw, float duration
+    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/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp

@@ -117,13 +117,13 @@ class AdaptiveController : public IController {
 
         // bias modification for level imu implementing deadband
 
-        if (abs(phi_in) <= 0.0001)
+        if (std::abs(phi_in) <= 0.0001)
             phi_in = 0;
 
-        if (abs(theta_in) <= 0.00001)
+        if (std::abs(theta_in) <= 0.00001)
             theta_in = 0;
 
-        if (abs(psi_in) <= 0.0001)
+        if (std::abs(psi_in) <= 0.0001)
             psi_in = 0;
     }
 
@@ -581,7 +581,7 @@ class AdaptiveController : public IController {
 
         // Rescale such that the outputs normalize from -1,1
 
-        U1 =  sqrt(abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal
+        U1 =  sqrt(std::abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal
 
         U2 =  U2 / 80;
 
@@ -678,4 +678,4 @@ class AdaptiveController : public IController {
 
 
 }
-#endif
+#endif

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

@@ -61,9 +61,14 @@ bool MultirotorRpcLibClient::goHome()
     return static_cast<rpc::client*>(getClient())->call("goHome").as<bool>();
 }
 
-bool MultirotorRpcLibClient::moveByAngle(float pitch, float roll, float z, float yaw, float duration)
+bool MultirotorRpcLibClient::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration)
 {
-    return static_cast<rpc::client*>(getClient())->call("moveByAngle", pitch, roll, z, yaw, duration).as<bool>();
+    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)

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

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

@@ -46,7 +46,7 @@ void DroneControllerBase::loopCommandPost()
     //no-op by default. derived class can override it if needed
 }
 
-bool DroneControllerBase::moveByAngle(float pitch, float roll, float z, float yaw, float duration
+bool DroneControllerBase::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration
     , CancelableBase& cancelable_action)
 {
     if (duration <= 0)
@@ -57,6 +57,17 @@ bool DroneControllerBase::moveByAngle(float pitch, float roll, float z, float ya
     }, 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

@@ -482,9 +482,9 @@ class MoveByManualCommand : public DroneCommand {
 };
 
 
-class MoveByAngleCommand : public DroneCommand {
+class MoveByAngleZCommand : public DroneCommand {
 public:
-    MoveByAngleCommand() : DroneCommand("MoveByAngle", "Move with specified roll and pitch, leaving z as-is")
+    MoveByAngleZCommand() : DroneCommand("MoveByAngleZ", "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)" });
@@ -503,7 +503,36 @@ class MoveByAngleCommand : public DroneCommand {
         CommandContext* context = params.context;
 
         context->tasker.execute([=]() {
-            context->client.moveByAngle(pitch, roll, z, yaw, duration);
+            context->client.moveByAngleZ(pitch, roll, z, yaw, duration);
+        });
+
+        return false;
+    }
+};
+
+
+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;
@@ -662,12 +691,12 @@ class BackForthByAngleCommand : public DroneCommand {
         CommandContext* context = params.context;
 
         context->tasker.execute([=]() {
-            if (!context->client.moveByAngle(pitch, roll, z, yaw, duration)) {
+            if (!context->client.moveByAngleZ(pitch, roll, z, yaw, duration)) {
                 throw std::runtime_error("BackForthByAngleCommand cancelled");
             }
             context->client.hover();
             context->sleep_for(pause_time);
-            if (!context->client.moveByAngle(-pitch, -roll, z, yaw, duration)){
+            if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, duration)){
                 throw std::runtime_error("BackForthByAngleCommand cancelled");
             }
         }, iterations);
@@ -747,22 +776,22 @@ class SquareByAngleCommand : public DroneCommand {
         CommandContext* context = params.context;
 
         context->tasker.execute([=]() {
-            if (!context->client.moveByAngle(pitch, -roll, z, yaw, 0)) {
+            if (!context->client.moveByAngleZ(pitch, -roll, z, yaw, 0)) {
                 throw std::runtime_error("SquareByAngleCommand cancelled");
             }
             context->client.hover();
             context->sleep_for(pause_time);
-            if (!context->client.moveByAngle(-pitch, -roll, z, yaw, 0)) {
+            if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, 0)) {
                 throw std::runtime_error("SquareByAngleCommand cancelled");
             }
             context->client.hover();
             context->sleep_for(pause_time);
-            if (!context->client.moveByAngle(-pitch, roll, z, yaw, 0)) {
+            if (!context->client.moveByAngleZ(-pitch, roll, z, yaw, 0)) {
                 throw std::runtime_error("SquareByAngleCommand cancelled");
             }
             context->client.hover();
             context->sleep_for(pause_time);
-            if (!context->client.moveByAngle(-pitch, -roll, z, yaw, 0)){
+            if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, 0)){
                 throw std::runtime_error("SquareByAngleCommand cancelled");
             }
             context->client.hover();
@@ -1351,7 +1380,8 @@ int main(int argc, const char *argv[]) {
     MoveToPositionCommand moveToPosition;
     GetPositionCommand getPosition;
     MoveByManualCommand moveByManual;
-    MoveByAngleCommand moveByAngle;
+    MoveByAngleZCommand moveByAngleZ;
+    MoveByAngleThrottleCommand moveByAngleThrottle;
     MoveByVelocityCommand moveByVelocity;
     MoveByVelocityZCommand moveByVelocityZ;
     MoveOnPathCommand moveOnPath;
@@ -1383,7 +1413,8 @@ int main(int argc, const char *argv[]) {
     shell.addCommand(hover);
     shell.addCommand(moveToPosition);
     shell.addCommand(moveByManual);
-    shell.addCommand(moveByAngle);
+    shell.addCommand(moveByAngleZ);
+    shell.addCommand(moveByAngleThrottle);
     shell.addCommand(moveByVelocity);
     shell.addCommand(moveByVelocityZ);
     shell.addCommand(moveOnPath);