Added moveByRotorSpeed command for QuadX

AirLib/include/common/HelperFunctions.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include "vehicles/multirotor/RotorParams.hpp"
+
+namespace msr { namespace airlib { namespace helper_func {
+
+class Converter {
+public:
+    void getCommandArgumentsFromSpeed(const simple_flight::Axis4r& input_command_args, simple_flight::Axis4r& output_command_args)
+    {
+        simple_flight::Axis4r motor_outputs;
+        rotor_params_.calculateMaxThrust();
+        motor_outputs[0] = input_command_args.roll() / rotor_params_.max_speed;
+        motor_outputs[1] = input_command_args.pitch() / rotor_params_.max_speed;
+        motor_outputs[2] = input_command_args.yaw() / rotor_params_.max_speed;
+        motor_outputs[3] = input_command_args.throttle() / rotor_params_.max_speed;
+
+        simple_flight::Axis4r output_command_args_temp;
+        for (int motor_index = 0; motor_index < kMotorCount; ++motor_index) {
+            output_command_args_temp[motor_index] =
+                motor_outputs[0] * mixerQuadXTransposeInv[motor_index].throttle
+                + motor_outputs[1] * mixerQuadXTransposeInv[motor_index].pitch
+                + motor_outputs[2] * mixerQuadXTransposeInv[motor_index].roll
+                + motor_outputs[3] * mixerQuadXTransposeInv[motor_index].yaw
+                ;
+        }
+        output_command_args = simple_flight::Axis4r(
+            output_command_args_temp[2],
+            output_command_args_temp[1],
+            output_command_args_temp[3],
+            output_command_args_temp[0]
+        );
+    }
+
+private:
+    const int kMotorCount = 4;
+    msr::airlib::RotorParams rotor_params_;
+
+    // Custom mixer data per motor
+    typedef struct motorMixer_t {
+        float throttle;
+        float roll;
+        float pitch;
+        float yaw;
+    } motorMixer_t;
+
+    //only thing that this matrix does is change the sign
+    const motorMixer_t mixerQuadXTransposeInv[4] = { //QuadX config
+        { 0.25f, 0.25f, 0.25f, 0.25f },          // FRONT_R
+        { -0.25f, 0.25f, 0.25f, -0.25f },          // REAR_L
+        { 0.25f, -0.25f, 0.25f, -0.25f },          // FRONT_L
+        { 0.25f, 0.25f, -0.25f, -0.25f },          // REAR_R
+    };
+};
+
+}}} //namespace

AirLib/include/physics/PhysicsWorld.hpp

@@ -16,7 +16,7 @@ namespace msr { namespace airlib {
 class PhysicsWorld {
 public:
     PhysicsWorld(PhysicsEngineBase* physics_engine, const std::vector<UpdatableObject*>& bodies,
-            uint64_t update_period_nanos = 3000000LL, bool state_reporter_enabled = false,
+            uint64_t update_period_nanos = 1000000LL, bool state_reporter_enabled = false,
             bool start_async_updator = true
         )
         : world_(physics_engine)

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

@@ -115,6 +115,12 @@ class MultirotorApi : public VehicleApiBase {
         return enqueueCommandAndWait(cmd, max_wait_seconds);
     }
 
+    bool moveByRotorSpeed(float o0, float o1, float o2, float o3, float duration)
+    {
+        std::shared_ptr<OffboardCommand> cmd = std::make_shared<MoveByRotorSpeed>(controller_, o0, o1, o2, o3, duration);
+        return enqueueCommand(cmd);
+    }
+
     bool moveToZ(float z, float velocity, float max_wait_seconds, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead)
     {
         std::shared_ptr<OffboardCommand> cmd = std::make_shared<MoveToZ>(controller_, z, velocity, yaw_mode, lookahead, adaptive_lookahead);
@@ -484,6 +490,20 @@ class MultirotorApi : public VehicleApiBase {
         }
     };
 
+    class MoveByRotorSpeed : public OffboardCommand {
+        float o0_, o1_, o2_, o3_, duration_;
+    public:
+        MoveByRotorSpeed(DroneControllerBase* controller, float o0, float o1, float o2, float o3, float duration) : OffboardCommand(controller) {
+            this->o0_ = o0;
+            this->o1_ = o1;
+            this->o2_ = o2;
+            this->o3_ = o3;
+            this->duration_ = duration;
+        }
+        virtual void executeImpl(DroneControllerBase* controller, CancelableBase& cancelable) override {
+            controller->moveByRotorSpeed(o0_, o1_, o2_, o3_, duration_, cancelable);
+        }
+    };
 
     class MoveToZ : public OffboardCommand {
         float z_;

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

@@ -33,6 +33,7 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
         DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), float lookahead = -1, float adaptive_lookahead = 1);
     bool moveToPosition(float x, float y, float z, float velocity, float max_wait_seconds = 60,
         DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), float lookahead = -1, float adaptive_lookahead = 1);
+    bool moveByRotorSpeed(float o0, float o1, float o2, float o3, float duration);
     bool moveToZ(float z, float velocity, float max_wait_seconds = 60,
         const YawMode& yaw_mode = YawMode(), float lookahead = -1, float adaptive_lookahead = 1);
     bool moveByManual(float vx_max, float vy_max, float z_min, float duration, 

AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp

@@ -152,6 +152,12 @@ class DroneControllerBase : public VehicleControllerBase {
     virtual bool moveToPosition(float x, float y, float z, float velocity, DrivetrainType drivetrain,
         const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action);
 
+
+    /// Move the drone by setting the individual rotor speed [rad/sec] of the drone.
+    virtual bool moveByRotorSpeed(float o0, float o1, float o2, float o3, float duration,
+        CancelableBase & cancelable_action);
+
+
     /// moveToZ is a shortcut for moveToPosition at the current x, y location.
     virtual bool moveToZ(float z, float velocity, const YawMode& yaw_mode,
         float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action);
@@ -243,6 +249,7 @@ class DroneControllerBase : public VehicleControllerBase {
     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;
+    virtual void commandRotorSpeed(float o0, float o1, float o2, float o3) = 0;
 
     //config commands
     virtual float getCommandPeriod() = 0; //time between two command required for drone in seconds
@@ -272,6 +279,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 moveByRotorSpeed(float o0, float o1, float o2, float o3);
     //****************************************************************************************************************************
 
     /************* safety checks & emergency manuevers ************/

AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp

@@ -147,6 +147,7 @@ class MavLinkDroneController : public DroneControllerBase
     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;
+    virtual void commandRotorSpeed(float o0, float o1, float o2, float o3) override;
     const VehicleParams& getVehicleParams() override;
     //*** End: DroneControllerBase implementation ***//
 
@@ -1198,6 +1199,16 @@ struct MavLinkDroneController::impl {
         mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw);
     }
 
+    void commandRotorSpeed(float o0, float o1, float o2, float o3)
+    {
+        unused(o0);
+        unused(o1);
+        unused(o2);
+        unused(o3);
+
+        //TODO: implement this
+    }
+
     RCData getRCData()
     {
         throw VehicleCommandNotImplementedException("getRCData() function is not yet implemented");
@@ -1218,7 +1229,7 @@ struct MavLinkDroneController::impl {
     //drone parameters
     float getCommandPeriod()
     {
-        return 1.0f / 50; //1 period of 50hz
+        return 1.0f / 1000; //1 period of 50hz
     }
     float getTakeoffZ()
     {
@@ -1506,6 +1517,10 @@ void MavLinkDroneController::commandPosition(float x, float y, float z, const Ya
 {
     return pimpl_->commandPosition(x, y, z, yaw_mode);
 }
+void MavLinkDroneController::commandRotorSpeed(float o0, float o1, float o2, float o3)
+{
+    return pimpl_->commandRotorSpeed(o0, o1, o2, o3);
+}
 
 RCData MavLinkDroneController::getRCData()
 {

AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp

@@ -289,6 +289,16 @@ class RosFlightDroneController : public DroneControllerBase {
         //TODO: implement this
     }
 
+    virtual void commandRotorSpeed(float o0, float o1, float o2, float o3) override
+    {
+        unused(o0);
+        unused(o1);
+        unused(o2);
+        unused(o3);
+
+        //TODO: implement this
+    }
+
     virtual const VehicleParams& getVehicleParams() override
     {
         //used for safety algos. For now just use defaults

AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp

@@ -16,6 +16,7 @@
 #include "AirSimSimpleFlightEstimator.hpp"
 #include "AirSimSimpleFlightCommon.hpp"
 #include "common/AirSimSettings.hpp"
+#include "common/HelperFunctions.hpp"
 
 
 namespace msr { namespace airlib {
@@ -283,6 +284,22 @@ class SimpleFlightDroneController : public DroneControllerBase {
         firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
     }
 
+    virtual void commandRotorSpeed(float o0, float o1, float o2, float o3) override
+    {
+        Utils::log(Utils::stringf("commandRotorSpeed %f, %f, %f, %f", o0, o1, o2, o3));
+
+        typedef simple_flight::GoalModeType GoalModeType;
+        simple_flight::GoalMode mode = simple_flight::GoalMode::getRotorSpeedMode();
+
+        simple_flight::Axis4r speedGoal(o0, o1, o2, o3);
+        simple_flight::Axis4r goal;
+
+        converter.getCommandArgumentsFromSpeed(speedGoal, goal);
+
+        std::string message;
+        firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
+    }
+
     virtual const VehicleParams& getVehicleParams() override
     {
         return safety_params_;
@@ -338,6 +355,7 @@ class SimpleFlightDroneController : public DroneControllerBase {
     VehicleParams safety_params_;
 
     RCData last_rcData_;
+    msr::airlib::helper_func::Converter converter;
 };
 
 }} //namespace

AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp

@@ -43,10 +43,11 @@ class OffboardApi :
         else {
             if (clock_->millis() - goal_timestamp_ > params_->api_goal_timeout) {
                 if (!is_api_timedout_) {
+                    /*
                     comm_link_->log("API call timed out, entering hover mode");
                     goal_mode_ = GoalMode::getPositionMode();
                     goal_ = Axis4r::xyzToAxis4(state_estimator_->getPosition(), true);
-
+                    */
                     is_api_timedout_ = true;
                 }
 

AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp

@@ -291,6 +291,13 @@ class GoalMode : public Axis4<GoalModeType> {
         return mode;
     }
 
+    static const GoalMode& getRotorSpeedMode()
+    {
+        static const GoalMode mode = GoalMode(GoalModeType::Passthrough,
+            GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough);
+        return mode;
+    }
+
     static const GoalMode& getAllRateMode()
     {
         static const GoalMode mode = GoalMode(GoalModeType::AngleRate, 

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

@@ -88,6 +88,11 @@ bool MultirotorRpcLibClient::moveToPosition(float x, float y, float z, float vel
     return static_cast<rpc::client*>(getClient())->call("moveToPosition", x, y, z, velocity, max_wait_seconds, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead).as<bool>();
 }
 
+bool MultirotorRpcLibClient::moveByRotorSpeed(float o0, float o1, float o2, float o3, float duration)
+{
+    return static_cast<rpc::client*>(getClient())->call("moveByRotorSpeed", o0, o1, o2, o3, duration).as<bool>();
+}
+
 bool MultirotorRpcLibClient::moveToZ(float z, float velocity, float max_wait_seconds, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead)
 {
     return static_cast<rpc::client*>(getClient())->call("moveToZ", z, velocity, max_wait_seconds, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead).as<bool>();

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

@@ -9,7 +9,6 @@
 
 #include "vehicles/multirotor/api/MultirotorRpcLibServer.hpp"
 
-
 #include "common/Common.hpp"
 STRICT_MODE_OFF
 #ifndef RPCLIB_MSGPACK
@@ -66,6 +65,9 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(MultirotorApi* drone, string serv
         bind("moveToPosition", [&](float x, float y, float z, float velocity, float max_wait_seconds, DrivetrainType drivetrain,
         const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead) -> 
         bool { return getDroneApi()->moveToPosition(x, y, z, velocity, max_wait_seconds, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); });
+    (static_cast<rpc::server*>(getServer()))->
+        bind("moveByRotorSpeed", [&](float o0, float o1, float o2, float o3, float duration) ->
+        bool { return getDroneApi()->moveByRotorSpeed(o0, o1, o2, o3, duration); });
     (static_cast<rpc::server*>(getServer()))->
         bind("moveToZ", [&](float z, float velocity, float max_wait_seconds, const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead) ->
         bool { return getDroneApi()->moveToZ(z, velocity, max_wait_seconds, yaw_mode.to(), lookahead, adaptive_lookahead); });

AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp

@@ -270,6 +270,20 @@ bool DroneControllerBase::moveToPosition(float x, float y, float z, float veloci
     return moveOnPath(path, velocity, drivetrain, yaw_mode, lookahead, adaptive_lookahead, cancelable_action);
 }
 
+bool DroneControllerBase::moveByRotorSpeed(float o0, float o1, float o2, float o3, float duration, CancelableBase & cancelable_action)
+{
+    if (duration <= 0)
+        return false;
+
+    Waiter waiter(getCommandPeriod());
+    if (!waiter.sleep(cancelable_action))
+        return false;
+
+    return !waitForFunction([&]() {
+        return !moveByRotorSpeed(o0, o1, o2, o3);
+    }, duration, cancelable_action);
+}
+
 bool DroneControllerBase::moveToZ(float z, float velocity, const YawMode& yaw_mode,
     float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action)
 {
@@ -374,6 +388,13 @@ bool DroneControllerBase::moveToPosition(const Vector3r& dest, const YawMode& ya
     return true;
 }
 
+bool DroneControllerBase::moveByRotorSpeed(float o0, float o1, float o2, float o3)
+{
+    commandRotorSpeed(o0, o1, o2, o3);
+
+    return true;
+}
+
 bool DroneControllerBase::moveByRollPitchZ(float pitch, float roll, float z, float yaw)
 {
     if (safetyCheckVelocity(getVelocity()))