[simpleflight] cleanup

microsoft · Mar 24, 2020 · 065d1b5 · 065d1b5
1 parent 18d4ce4
commit 065d1b5
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 12 deletions.
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -440,13 +440,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
@@ -463,7 +461,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
@@ -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
@@ -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");
             }