From 065d1b5cd4804632c50da2ffe220543e1d26d633 Mon Sep 17 00:00:00 2001 From: madratman Date: Fri, 8 Nov 2019 16:07:30 -0800 Subject: [PATCH] [simpleflight] cleanup --- .../firmwares/mavlink/MavLinkMultirotorApi.hpp | 3 --- .../firmwares/simple_flight/firmware/Params.hpp | 4 ++-- DroneShell/src/main.cpp | 14 +++++++------- 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index fe74c0cb91..b2420b8a2a 100644 --- 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 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"); }