diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
index 6de33263dc..9d4ff4de32 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
@@ -185,7 +185,8 @@ class ArduCopterApi : public MultirotorApiBase {
         unused(roll);
         unused(pitch);
         unused(yaw_rate);
-        Utils::log("Not Implemented: commandRollPitchThrottle", Utils::kLogLevelInfo);
+        unused(throttle);
+        Utils::log("Not Implemented: commandRollPitchYawrateThrottle", Utils::kLogLevelInfo);
     }
 
     virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
@@ -194,7 +195,7 @@ class ArduCopterApi : public MultirotorApiBase {
         unused(pitch);
         unused(yaw);
         unused(z);
-        Utils::log("Not Implemented", Utils::kLogLevelInfo);
+        Utils::log("Not Implemented: commandRollPitchYawZ", Utils::kLogLevelInfo);
     }
 
     virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
@@ -202,7 +203,35 @@ class ArduCopterApi : public MultirotorApiBase {
         unused(roll);
         unused(pitch);
         unused(yaw);
-        Utils::log("Not Implemented: commandRollPitchZ", Utils::kLogLevelInfo);
+        unused(throttle);
+        Utils::log("Not Implemented: commandRollPitchYawThrottle", Utils::kLogLevelInfo);
+    }
+
+    virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
+    {
+        unused(roll);
+        unused(pitch);
+        unused(yaw_rate);
+        unused(z);
+        Utils::log("Not Implemented: commandRollPitchYawrateZ", Utils::kLogLevelInfo);
+    }
+
+    virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
+    {
+        unused(roll_rate);
+        unused(pitch_rate);
+        unused(yaw_rate);
+        unused(z);
+        Utils::log("Not Implemented: commandAngleRatesZ", Utils::kLogLevelInfo);
+    }
+
+    virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
+    {
+        unused(roll_rate);
+        unused(pitch_rate);
+        unused(yaw_rate);
+        unused(throttle);
+        Utils::log("Not Implemented: commandAngleRatesZ", Utils::kLogLevelInfo);
     }
 
     virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 64e107c2e8..6fe9bf4150 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -501,13 +501,11 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
     {
         checkValidVehicle();
-        // todo  in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale?
         mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle);
     }
     virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
     {
         checkValidVehicle();
-        // todo  in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale?
         mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle);
     }
     virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
@@ -524,7 +522,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
     {
         checkValidVehicle();
-        // todo  in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale?
         mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle);
     }
 
diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp
index 07c41a9e60..cba144d634 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp
@@ -70,7 +70,7 @@ struct Params {
         const float kI = 0.0f;
         const float kD = 0.0f;
         
-        //max_pitch/roll_angle > 5.5 would produce versicle thrust that is not enough to keep vehicle in air at extremities of controls
+        //max_pitch/roll_angle > 5.5 would produce verticle thrust that is not enough to keep vehicle in air at extremities of controls
         Axis4r max_limit = Axis4r(pi / 5.5f, pi / 5.5f, pi, 1.0f); //roll, pitch, yaw - in radians/sec
 
         Axis4r p = Axis4r(kP, kP, kP, 1.0f);
@@ -80,7 +80,7 @@ struct Params {
 
     struct PositionPid {
         const float kMaxLimit = 8.8E26f; //some big number like size of known universe
-        const float kP = 2.5f;
+        const float kP = 0.25f;
         const float kI = 0.0f;
         const float kD = 0.0f;
 
diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp
index b6ca98d556..3ff83e962a 100644
--- a/DroneShell/src/main.cpp
+++ b/DroneShell/src/main.cpp
@@ -505,7 +505,7 @@ class MoveByAngleZCommand : public DroneCommand {
         CommandContext* context = params.context;
 
         context->tasker.execute([=]() {
-            context->client.moveByRollPitchYawZAsync(pitch, roll, z, yaw, duration);
+            context->client.moveByRollPitchYawZAsync(roll, pitch, yaw, z, duration);
         });
 
         return false;
@@ -693,13 +693,13 @@ class BackForthByAngleCommand : public DroneCommand {
         CommandContext* context = params.context;
 
         context->tasker.execute([=]() {
-            context->client.moveByRollPitchYawZAsync(pitch, roll, z, yaw, duration);
+            context->client.moveByRollPitchYawZAsync(roll, pitch, yaw, z, duration);
             if (!context->client.waitOnLastTask()) {
                 throw std::runtime_error("BackForthByAngleCommand canceled");
             }
             context->client.hoverAsync();
             context->sleep_for(pause_time);
-            context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, duration);
+            context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, duration);
             if (!context->client.waitOnLastTask()){
                 throw std::runtime_error("BackForthByAngleCommand canceled");
             }
@@ -782,28 +782,28 @@ class SquareByAngleCommand : public DroneCommand {
         CommandContext* context = params.context;
 
         context->tasker.execute([=]() {
-            context->client.moveByRollPitchYawZAsync(pitch, -roll, z, yaw, 0);
+            context->client.moveByRollPitchYawZAsync(-roll, pitch, yaw, z, 0);
             if (!context->client.waitOnLastTask()) {
                 throw std::runtime_error("SquareByAngleCommand canceled");
             }
             context->client.hoverAsync();
             context->sleep_for(pause_time);
 
-            context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, 0);
+            context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, 0);
             if (!context->client.waitOnLastTask()) {
                 throw std::runtime_error("SquareByAngleCommand canceled");
             }
             context->client.hoverAsync();
             context->sleep_for(pause_time);
 
-            context->client.moveByRollPitchYawZAsync(-pitch, roll, z, yaw, 0);
+            context->client.moveByRollPitchYawZAsync(roll, -pitch, yaw, z, 0);
             if (!context->client.waitOnLastTask()) {
                 throw std::runtime_error("SquareByAngleCommand canceled");
             }
             context->client.hoverAsync();
             context->sleep_for(pause_time);
 
-            context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, 0);
+            context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, 0);
             if (!context->client.waitOnLastTask()){
                 throw std::runtime_error("SquareByAngleCommand canceled");
             }