diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index b42473f9ca..a39b831fc0 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -24,12 +24,20 @@ class MultirotorApiBase : public VehicleApiBase { protected: //must be implemented /************************* low level move APIs *********************************/ - 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 commandMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm) = 0; + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0; + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0; + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0; + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0; + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0; + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 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; + /************************* set Controller Gains APIs *********************************/ + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) = 0; + /************************* State APIs *********************************/ virtual Kinematics::State getKinematicsEstimated() const = 0; virtual LandedState getLandedState() const = 0; @@ -81,8 +89,13 @@ class MultirotorApiBase : public VehicleApiBase { virtual bool land(float timeout_sec); virtual bool goHome(float timeout_sec); - virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration); - virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration); + virtual bool moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration); + virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); + virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); + virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); + virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration); virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, @@ -97,6 +110,12 @@ class MultirotorApiBase : public VehicleApiBase { virtual bool hover(); virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); + /************************* set angle gain APIs *********************************/ + virtual void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd); + /************************* Safety APIs *********************************/ virtual void setSafetyEval(const shared_ptr safety_eval_ptr); virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, @@ -127,11 +146,15 @@ class MultirotorApiBase : public VehicleApiBase { typedef std::function WaitFunction; //*********************************safe wrapper around low level commands*************************************************** + virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z); + virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle); + virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle); + virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z); + virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z); + virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle); virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); - virtual void moveByRollPitchZInternal(float pitch, float roll, float z, float yaw); - virtual void moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate); /************* safety checks & emergency maneuvers ************/ virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index b90177e2ff..05e63cbc9b 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -23,8 +23,13 @@ class MultirotorRpcLibClient : public RpcLibClientBase { MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByAngleZAsync(float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveByAngleThrottleAsync(float pitch, float roll, float throttle, float yaw_rate, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration, DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration, @@ -43,9 +48,12 @@ class MultirotorRpcLibClient : public RpcLibClientBase { MultirotorRpcLibClient* rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name = ""); MultirotorRpcLibClient* hoverAsync(const std::string& vehicle_name = ""); + void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); + void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); + void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); + void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name=""); void moveByRC(const RCData& rc_data, const std::string& vehicle_name = ""); - MultirotorState getMultirotorState(const std::string& vehicle_name = ""); bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp index 4169ff8087..5e9e473ecb 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -180,22 +180,76 @@ class ArduCopterApi : public MultirotorApiBase { return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled } - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) override + { + unused(controllerType); + unused(kp); + unused(ki); + unused(kd); + Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo); + } + + virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override + { + unused(front_right_pwm); + unused(front_left_pwm); + unused(rear_right_pwm); + unused(rear_left_pwm); + Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override { - unused(pitch); unused(roll); - unused(throttle); + unused(pitch); unused(yaw_rate); - Utils::log("Not Implemented: commandRollPitchThrottle", Utils::kLogLevelInfo); + unused(throttle); + Utils::log("Not Implemented: commandRollPitchYawrateThrottle", Utils::kLogLevelInfo); } - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override { - unused(pitch); unused(roll); + unused(pitch); + unused(yaw); unused(z); + Utils::log("Not Implemented: commandRollPitchYawZ", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + 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 363d6f7dfb..1e7a9bb823 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -473,7 +473,25 @@ class MavLinkMultirotorApi : public MultirotorApiBase } protected: //methods - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) override + { + unused(controllerType); + unused(kp); + unused(ki); + unused(kd); + Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo); + } + + virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override + { + unused(front_right_pwm); + unused(front_left_pwm); + unused(rear_right_pwm); + unused(rear_left_pwm); + Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override { if (target_height_ != -z) { // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best @@ -487,11 +505,44 @@ class MavLinkMultirotorApi : public MultirotorApiBase float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust); } - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override + { + if (target_height_ != -z) { + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkValidVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust); + } + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override { checkValidVehicle(); - mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle); + mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle); } + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override + { + checkValidVehicle(); + 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 + { + if (target_height_ != -z) { + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkValidVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust); + } + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + checkValidVehicle(); + mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle); + } + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override { checkValidVehicle(); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 71dfa99105..955e094cc3 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -195,22 +195,22 @@ class SimpleFlightApi : public MultirotorApiBase { return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled } - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + virtual void commandMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm) { - //Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate)); + //Utils::log(Utils::stringf("commandMotorPWMs %f, %f, %f, %f", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm)); typedef simple_flight::GoalModeType GoalModeType; - simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); + simple_flight::GoalMode mode(GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough); - simple_flight::Axis4r goal(roll, pitch, yaw_rate, throttle); + simple_flight::Axis4r goal(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); std::string message; firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override { - //Utils::log(Utils::stringf("commandRollPitchZ %f, %f, %f, %f", pitch, roll, z, yaw)); + //Utils::log(Utils::stringf("commandRollPitchYawZ %f, %f, %f, %f", pitch, roll, z, yaw)); typedef simple_flight::GoalModeType GoalModeType; simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld); @@ -221,6 +221,71 @@ class SimpleFlightApi : public MultirotorApiBase { firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll, pitch, yaw, throttle); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); + + 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 commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::PositionWorld); + + simple_flight::Axis4r goal(roll, pitch, yaw_rate, z); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::PositionWorld); + + simple_flight::Axis4r goal(roll_rate, pitch_rate, yaw_rate, z); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll_rate, pitch_rate, yaw_rate, throttle); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override { //Utils::log(Utils::stringf("commandVelocity %f, %f, %f, %f", vx, vy, vz, yaw_mode.yaw_or_rate)); @@ -251,6 +316,55 @@ class SimpleFlightApi : public MultirotorApiBase { firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } + virtual void setControllerGains(uint8_t controller_type, const vector& kp, const vector& ki, const vector& kd) override + { + simple_flight::GoalModeType controller_type_enum = static_cast(controller_type); + + vector kp_axis4(4); + vector ki_axis4(4); + vector kd_axis4(4); + + switch(controller_type_enum) { + // roll gain, pitch gain, yaw gain, and no gains in throttle / z axis + case simple_flight::GoalModeType::AngleRate: + kp_axis4 = {kp[0], kp[1], kp[2], 1.0}; + ki_axis4 ={ki[0], ki[1], ki[2], 0.0}; + kd_axis4 = {kd[0], kd[1], kd[2], 0.0}; + params_.angle_rate_pid.p.setValues(kp_axis4); + params_.angle_rate_pid.i.setValues(ki_axis4); + params_.angle_rate_pid.d.setValues(kd_axis4); + params_.gains_changed = true; + break; + case simple_flight::GoalModeType::AngleLevel: + kp_axis4 = {kp[0], kp[1], kp[2], 1.0}; + ki_axis4 = {ki[0], ki[1], ki[2], 0.0}; + kd_axis4 = {kd[0], kd[1], kd[2], 0.0}; + params_.angle_level_pid.p.setValues(kp_axis4); + params_.angle_level_pid.i.setValues(ki_axis4); + params_.angle_level_pid.d.setValues(kd_axis4); + params_.gains_changed = true; + break; + case simple_flight::GoalModeType::VelocityWorld: + kp_axis4 = {kp[1], kp[0], 0.0, kp[2]}; + ki_axis4 = {ki[1], ki[0], 0.0, ki[2]}; + kd_axis4 = {kd[1], kd[0], 0.0, kd[2]}; + params_.velocity_pid.p.setValues(kp_axis4); + params_.velocity_pid.i.setValues(ki_axis4); + params_.velocity_pid.d.setValues(kd_axis4); + params_.gains_changed = true; + break; + case simple_flight::GoalModeType::PositionWorld: + kp_axis4 = {kp[1], kp[0], 0.0, kp[2]}; + ki_axis4 = {ki[1], ki[0], 0.0, ki[2]}; + kd_axis4 = {kd[1], kd[0], 0.0, kd[2]}; + params_.position_pid.p.setValues(kp_axis4); + params_.position_pid.i.setValues(ki_axis4); + params_.position_pid.d.setValues(kd_axis4); + params_.gains_changed = true; + break; + } + } + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override { //Utils::log(Utils::stringf("commandPosition %f, %f, %f, %f", x, y, z, yaw_mode.yaw_or_rate)); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index 42156bb4e8..7de5f9a12d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -58,6 +58,19 @@ class AdaptiveController : public IController { return output_controls_; } + virtual bool isLastGoalModeAllPassthrough() override + { + is_last_goal_mode_all_passthrough_ = true; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + if (last_mode_[axis] != GoalModeType::Passthrough) { + is_last_goal_mode_all_passthrough_ = false; + } + } + + return is_last_goal_mode_all_passthrough_; + } + private: const IBoardClock* clock_; const IGoal* goal_; @@ -101,6 +114,7 @@ class AdaptiveController : public IController { bool reset = true; double x_0[12]; GoalMode last_mode_; + bool is_last_goal_mode_all_passthrough_; //double error[3] = { 0 }; double ref_vec[10][3] = {{ 0 }}; double ref_sum[3] = { 0 }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp index 42d6d34c85..95324951db 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp @@ -21,7 +21,7 @@ class AngleLevelController : public IGoal //for internal rate controller { public: - AngleLevelController(const Params* params, const IBoardClock* clock = nullptr) + AngleLevelController(Params* params, const IBoardClock* clock = nullptr) : params_(params), clock_(clock) { } @@ -37,7 +37,7 @@ class AngleLevelController : //initialize level PID pid_.reset(new PidController(clock_, - PidConfig(params_->angle_level_pid.p[axis], 0, 0))); + PidConfig(params_->angle_level_pid.p[axis], params_->angle_level_pid.i[axis], params_->angle_level_pid.d[axis]))); //initialize rate controller rate_controller_.reset(new AngleRateController(params_, clock_)); @@ -135,7 +135,7 @@ class AngleLevelController : TReal output_; - const Params* params_; + Params* params_; const IBoardClock* clock_; std::unique_ptr> pid_; std::unique_ptr rate_controller_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp index e961c963f6..19e6e12d36 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp @@ -16,7 +16,7 @@ namespace simple_flight { class AngleRateController : public IAxisController { public: - AngleRateController(const Params* params, const IBoardClock* clock) + AngleRateController(Params* params, const IBoardClock* clock) : params_(params), clock_(clock) { } @@ -31,7 +31,7 @@ class AngleRateController : public IAxisController { state_estimator_ = state_estimator; pid_.reset(new PidController(clock_, - PidConfig(params_->angle_rate_pid.p[axis], 0, 0))); + PidConfig(params_->angle_rate_pid.p[axis], params_->angle_rate_pid.i[axis], params_->angle_rate_pid.d[axis]))); } virtual void reset() override @@ -65,7 +65,7 @@ class AngleRateController : public IAxisController { TReal output_; - const Params* params_; + Params* params_; const IBoardClock* clock_; std::unique_ptr> pid_; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/CascadeController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/CascadeController.hpp index 73b06f088d..c480541ad2 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/CascadeController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/CascadeController.hpp @@ -19,7 +19,7 @@ namespace simple_flight { class CascadeController : public IController { public: - CascadeController(const Params* params, const IBoardClock* clock, ICommLink* comm_link) + CascadeController(Params* params, const IBoardClock* clock, ICommLink* comm_link) : params_(params), clock_(clock), comm_link_(comm_link) { } @@ -62,8 +62,8 @@ class CascadeController : public IController { } for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { - //re-create axis controllers if goal mode was changed since last time - if (goal_mode[axis] != last_goal_mode_[axis]) { + //re-create axis controllers if goal mode was changed since last time, or if gains have been updated + if (goal_mode[axis] != last_goal_mode_[axis] || params_->gains_changed == true) { switch (goal_mode[axis]) { case GoalModeType::AngleRate: axis_controllers_[axis].reset(new AngleRateController(params_, clock_)); @@ -107,6 +107,7 @@ class CascadeController : public IController { else comm_link_->log(std::string("Axis controller type is not set for axis ").append(std::to_string(axis)), ICommLink::kLogLevelInfo); } + params_->gains_changed = false; } virtual const Axis4r& getOutput() override @@ -114,9 +115,21 @@ class CascadeController : public IController { return output_; } + virtual bool isLastGoalModeAllPassthrough() override + { + is_last_goal_mode_all_passthrough_ = true; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + if (last_goal_mode_[axis] != GoalModeType::Passthrough) { + is_last_goal_mode_all_passthrough_ = false; + } + } + + return is_last_goal_mode_all_passthrough_; + } private: - const Params* params_; + Params* params_; const IBoardClock* clock_; const IGoal* goal_; @@ -127,6 +140,7 @@ class CascadeController : public IController { GoalMode last_goal_mode_; Axis4r last_goal_val_; + bool is_last_goal_mode_all_passthrough_; std::unique_ptr axis_controllers_[Axis4r::AxisCount()]; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp index 506df25d5e..e830b43878 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp @@ -18,7 +18,7 @@ namespace simple_flight { class Firmware : public IFirmware { public: - Firmware(const Params* params, IBoard* board, ICommLink* comm_link, IStateEstimator* state_estimator) + Firmware(Params* params, IBoard* board, ICommLink* comm_link, IStateEstimator* state_estimator) : params_(params), board_(board), comm_link_(comm_link), state_estimator_(state_estimator), offboard_api_(params, board, board, state_estimator, comm_link), mixer_(params) { @@ -59,8 +59,17 @@ class Firmware : public IFirmware { const Axis4r& output_controls = controller_->getOutput(); - //convert controller output in to motor outputs - mixer_.getMotorOutput(output_controls, motor_outputs_); + // if last goal mode is passthrough for all axes (which means moveByMotorPWMs was called), + // we directly set the motor outputs to controller outputs + // note that the order of motors is as explained MultiRotorParams::initializeRotorQuadX() + if (controller_->isLastGoalModeAllPassthrough()) { + for (uint16_t motor_index = 0; motor_index < params_->motor.motor_count; ++motor_index) + motor_outputs_[motor_index] = output_controls[motor_index]; + } + else { + // apply motor mixing matrix to convert from controller output to motor outputs + mixer_.getMotorOutput(output_controls, motor_outputs_); + } //finally write the motor outputs for (uint16_t motor_index = 0; motor_index < params_->motor.motor_count; ++motor_index) @@ -77,7 +86,7 @@ class Firmware : public IFirmware { private: //objects we use - const Params* params_; + Params* params_; IBoard* board_; ICommLink* comm_link_; IStateEstimator* state_estimator_; 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 7af8486b57..cba144d634 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp @@ -51,44 +51,64 @@ struct Params { struct AngleRatePid { //max_xxx_rate > 5 would introduce wobble/oscillations const float kMaxLimit = 2.5f; + const float kP = 0.25f; + const float kI = 0.0f; + const float kD = 0.0f; + Axis3r max_limit = Axis3r(kMaxLimit, kMaxLimit, kMaxLimit); //roll, pitch, yaw - in radians/sec //p_xxx_rate params are sensitive to gyro noise. Values higher than 0.5 would require //noise filtration - const float kP = 0.25f; Axis4r p = Axis4r(kP, kP, kP, 1.0f); + Axis4r i = Axis4r(kI, kI, kI, 0.0f); + Axis4r d = Axis4r(kD, kD, kD, 0.0f); } angle_rate_pid; struct AngleLevelPid { const float pi = 3.14159265359f; //180-degrees + const float kP = 2.5f; + 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 - const float kP = 2.5f; Axis4r p = Axis4r(kP, kP, kP, 1.0f); + Axis4r i = Axis4r(kI, kI, kI, 0.0f); + Axis4r d = Axis4r(kD, kD, kD, 0.0f); } angle_level_pid; struct PositionPid { const float kMaxLimit = 8.8E26f; //some big number like size of known universe + const float kP = 0.25f; + const float kI = 0.0f; + const float kD = 0.0f; + Axis4r max_limit = Axis4r(kMaxLimit, kMaxLimit, kMaxLimit, 1.0f); //x, y, z in meters - Axis4r p = Axis4r( 0.25f, 0.25f, 0, 0.25f); + Axis4r p = Axis4r(kP, kP, 0, kP); + Axis4r i = Axis4r(kI, kI, kI, kI); + Axis4r d = Axis4r(kD, kD, kD, kD); } position_pid; struct VelocityPid { const float kMinThrottle = std::min(1.0f, Params::min_armed_throttle() * 3.0f); const float kMaxLimit = 6.0f; // m/s + const float kP = 0.2f; + const float kI = 2.0f; + const float kD = 0.0f; + Axis4r max_limit = Axis4r(kMaxLimit, kMaxLimit, 0, kMaxLimit); //x, y, yaw, z in meters - Axis4r p = Axis4r(0.2f, 0.2f, 0, 2.0f); + Axis4r p = Axis4r(kP, kP, 0.0f, 2.0f); // todo why 2.0f hardcoded + Axis4r i = Axis4r(0.0f, 0.0f, 0.0f, kI); + Axis4r d = Axis4r(kD, kD, kD, kD); - Axis4r i = Axis4r(0, 0, 0, 2.0f); Axis4r iterm_discount = Axis4r(1, 1, 1, 0.9999f); Axis4r output_bias = Axis4r(0, 0, 0, 0); //we keep min throttle higher so that if we are angling a lot, its still supported - float min_throttle =kMinThrottle ; + float min_throttle = kMinThrottle ; } velocity_pid; struct Takeoff { @@ -105,6 +125,7 @@ struct Params { VehicleStateType default_vehicle_state = VehicleStateType::Inactive; uint64_t api_goal_timeout = 60; //milliseconds ControllerType controller_type = ControllerType::Cascade; + bool gains_changed; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PositionController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PositionController.hpp index cf4e3b92a2..1db9340dde 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PositionController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PositionController.hpp @@ -18,7 +18,7 @@ class PositionController : public IGoal //for internal child controller { public: - PositionController(const Params* params, const IBoardClock* clock = nullptr) + PositionController(Params* params, const IBoardClock* clock = nullptr) : params_(params), clock_(clock) { } @@ -34,7 +34,7 @@ class PositionController : //initialize parent PID pid_.reset(new PidController(clock_, - PidConfig(params_->position_pid.p[axis], 0, 0))); + PidConfig(params_->position_pid.p[axis], params_->position_pid.i[axis], params_->position_pid.d[axis]))); //initialize child controller velocity_controller_.reset(new VelocityController(params_, clock_)); @@ -100,7 +100,7 @@ class PositionController : TReal output_; - const Params* params_; + Params* params_; const IBoardClock* clock_; std::unique_ptr> pid_; std::unique_ptr velocity_controller_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp index 34ab4bed06..3918e170b8 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/VelocityController.hpp @@ -18,7 +18,7 @@ class VelocityController : public IGoal //for internal child controller { public: - VelocityController(const Params* params, const IBoardClock* clock = nullptr) + VelocityController(Params* params, const IBoardClock* clock = nullptr) : params_(params), clock_(clock) { } @@ -30,7 +30,7 @@ class VelocityController : state_estimator_ = state_estimator; PidConfig pid_config(params_->velocity_pid.p[axis], - params_->velocity_pid.i[axis], 0); + params_->velocity_pid.i[axis], params_->velocity_pid.d[axis]); pid_config.iterm_discount = params_->velocity_pid.iterm_discount[axis]; pid_config.output_bias = params_->velocity_pid.output_bias[axis]; @@ -146,7 +146,7 @@ class VelocityController : TReal output_; - const Params* params_; + Params* params_; const IBoardClock* clock_; std::unique_ptr> pid_; std::unique_ptr child_controller_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp index 3a0b794c77..2ffcea4c35 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp @@ -20,10 +20,12 @@ class Axis3 { { return vals_[index]; } + virtual const T& operator[] (unsigned int index) const { return vals_[index]; } + virtual std::string toString() const { return std::to_string(static_cast(vals_[0])) @@ -95,6 +97,14 @@ class Axis4 : public Axis3 { (*this)[axis] = axis3[axis]; } + void setValues(const vector& vals) + { + (*this)[0] = vals[0]; + (*this)[1] = vals[1]; + (*this)[2] = vals[2]; + val4_ = vals[3]; + } + T& val4() { return val4_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IController.hpp index 28944282c3..4a7e51bacf 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IController.hpp @@ -12,6 +12,7 @@ class IController : public IUpdatable { public: virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) = 0; virtual const Axis4r& getOutput() = 0; + virtual bool isLastGoalModeAllPassthrough() = 0; }; } //namespace \ No newline at end of file diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index be32d215fd..44d0261974 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -72,7 +72,7 @@ bool MultirotorApiBase::goHome(float timeout_sec) return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); } -bool MultirotorApiBase::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration) +bool MultirotorApiBase::moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration) { SingleTaskCall lock(this); @@ -80,12 +80,12 @@ bool MultirotorApiBase::moveByAngleZ(float pitch, float roll, float z, float yaw return true; return waitForFunction([&]() { - moveByRollPitchZInternal(pitch, roll, z, yaw); + commandMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); return false; //keep moving until timeout }, duration).isTimeout(); } -bool MultirotorApiBase::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration) +bool MultirotorApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration) { SingleTaskCall lock(this); @@ -93,7 +93,72 @@ bool MultirotorApiBase::moveByAngleThrottle(float pitch, float roll, float throt return true; return waitForFunction([&]() { - moveByRollPitchThrottleInternal(pitch, roll, throttle, yaw_rate); + moveByRollPitchYawZInternal(roll, pitch, yaw, z); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawThrottleInternal(roll, pitch, yaw, throttle); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawrateThrottleInternal(roll, pitch, yaw_rate, throttle); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawrateZInternal(roll, pitch, yaw_rate, z); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByAngleRatesZInternal(roll_rate, pitch_rate, yaw_rate, z); + return false; //keep moving until timeout + }, duration).isTimeout(); +} + +bool MultirotorApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration) +{ + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByAngleRatesThrottleInternal(roll_rate, pitch_rate, yaw_rate, throttle); return false; //keep moving until timeout }, duration).isTimeout(); } @@ -419,6 +484,30 @@ bool MultirotorApiBase::rotateByYawRate(float yaw_rate, float duration) return waiter.isTimeout(); } +void MultirotorApiBase::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd) +{ + uint8_t controller_type = 2; + setControllerGains(controller_type, kp, ki, kd); +} + +void MultirotorApiBase::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd) +{ + uint8_t controller_type = 3; + setControllerGains(controller_type, kp, ki, kd); +} + +void MultirotorApiBase::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd) +{ + uint8_t controller_type = 4; + setControllerGains(controller_type, kp, ki, kd); +} + +void MultirotorApiBase::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd) +{ + uint8_t controller_type = 5; + setControllerGains(controller_type, kp, ki, kd); +} + bool MultirotorApiBase::hover() { SingleTaskCall lock(this); @@ -451,16 +540,40 @@ void MultirotorApiBase::moveToPositionInternal(const Vector3r& dest, const YawMo commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode); } -void MultirotorApiBase::moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate) +void MultirotorApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawZ(roll, pitch, yaw, z); +} + +void MultirotorApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawThrottle(roll, pitch, yaw, throttle); +} + +void MultirotorApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle); +} + +void MultirotorApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateZ(roll, pitch, yaw_rate, z); +} + +void MultirotorApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z) { if (safetyCheckVelocity(getVelocity())) - commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); + commandAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z); } -void MultirotorApiBase::moveByRollPitchZInternal(float pitch, float roll, float z, float yaw) +void MultirotorApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle) { if (safetyCheckVelocity(getVelocity())) - commandRollPitchZ(pitch, roll, z, yaw); + commandAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle); } //executes a given function until it returns true. Each execution is spaced apart at command period. diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 01f17fcd52..769fad6849 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -75,15 +75,45 @@ MultirotorRpcLibClient* MultirotorRpcLibClient::goHomeAsync(float timeout_sec, c return this; } -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleZAsync(float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name) +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) { - pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleZ", pitch, roll, z, yaw, duration, vehicle_name); + pimpl_->last_future = static_cast(getClient())->async_call("moveByMotorPWMs", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name); return this; } -MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleThrottleAsync(float pitch, float roll, float throttle, float yaw_rate, float duration, const std::string& vehicle_name) +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) { - pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleThrottle", pitch, roll, throttle, yaw_rate, duration, vehicle_name); + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawZ", roll, pitch, yaw, z, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawThrottle", roll, pitch, yaw, throttle, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateThrottle", roll, pitch, yaw_rate, throttle, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateZ", roll, pitch, yaw_rate, z, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesZ", roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name); + return this; +} + +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesThrottle", roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name); return this; } @@ -155,6 +185,26 @@ MultirotorRpcLibClient* MultirotorRpcLibClient::hoverAsync(const std::string& ve return this; } +void MultirotorRpcLibClient::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) +{ + static_cast(getClient())->call("setAngleLevelControllerGains", kp, ki, kd, vehicle_name); +} + +void MultirotorRpcLibClient::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) +{ + static_cast(getClient())->call("setAngleRateControllerGains", kp, ki, kd, vehicle_name); +} + +void MultirotorRpcLibClient::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) +{ + static_cast(getClient())->call("setVelocityControllerGains", kp, ki, kd, vehicle_name); +} + +void MultirotorRpcLibClient::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) +{ + static_cast(getClient())->call("setPositionControllerGains", kp, ki, kd, vehicle_name); +} + bool MultirotorRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) { diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 7051bc58da..9035b304f3 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -53,14 +53,38 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string bind("goHome", [&](float timeout_sec, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->goHome(timeout_sec); }); - (static_cast(getServer()))-> - bind("moveByAngleZ", [&](float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name) -> - bool { return getVehicleApi(vehicle_name)->moveByAngleZ(pitch, roll, z, yaw, duration); }); + bind("moveByMotorPWMs", [&](float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) -> + bool { return getVehicleApi(vehicle_name)->moveByMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawZ", [&](float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) -> + bool { return getVehicleApi(vehicle_name)->moveByRollPitchYawZ(roll, pitch, yaw, z, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawThrottle", [&](float roll, float pitch, float yaw, float throttle, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawThrottle(roll, pitch, yaw, throttle, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawrateThrottle", [&](float roll, float pitch, float yaw_rate, float throttle, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle, duration); + }); + (static_cast(getServer()))-> + bind("moveByRollPitchYawrateZ", [&](float roll, float pitch, float yaw_rate, float z, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateZ(roll, pitch, yaw_rate, z, duration); + }); + (static_cast(getServer()))-> + bind("moveByAngleRatesZ", [&](float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, + const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z, duration); + }); (static_cast(getServer()))-> - bind("moveByAngleThrottle", [&](float pitch, float roll, float throttle, float yaw_rate, float duration, + bind("moveByAngleRatesThrottle", [&](float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); + return getVehicleApi(vehicle_name)->moveByAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle, duration); }); (static_cast(getServer()))-> bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, @@ -107,6 +131,22 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string bind("hover", [&](const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->hover(); }); + (static_cast(getServer()))-> + bind("setAngleLevelControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setAngleLevelControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))-> + bind("setAngleRateControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setAngleRateControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))-> + bind("setVelocityControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setVelocityControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))-> + bind("setPositionControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setPositionControllerGains(kp, ki, kd); + }); (static_cast(getServer()))-> bind("moveByRC", [&](const MultirotorRpcLibAdapators::RCData& data, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->moveByRC(data.to()); diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp index 791ae73bf7..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.moveByAngleZAsync(pitch, roll, z, yaw, duration); + context->client.moveByRollPitchYawZAsync(roll, pitch, yaw, z, duration); }); return false; @@ -534,7 +534,7 @@ class MoveByAngleThrottleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleThrottleAsync(pitch, roll, throttle, yaw_rate, duration); + context->client.moveByRollPitchYawrateThrottleAsync(roll, pitch, yaw_rate, throttle, duration); }); return false; @@ -693,13 +693,13 @@ class BackForthByAngleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngleZAsync(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.moveByAngleZAsync(-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.moveByAngleZAsync(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.moveByAngleZAsync(-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.moveByAngleZAsync(-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.moveByAngleZAsync(-pitch, -roll, z, yaw, 0); + context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, 0); if (!context->client.waitOnLastTask()){ throw std::runtime_error("SquareByAngleCommand canceled"); } diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index d2aebcec47..3bfddb9f30 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -247,10 +247,203 @@ def goHome(self): raise Exception("goHome API is deprecated. Please use goHomeAsync() API." + self.upgrade_api_help) def hover(self): raise Exception("hover API is deprecated. Please use hoverAsync() API." + self.upgrade_api_help) - def moveByAngleZ(self, pitch, roll, z, yaw, duration): - raise Exception("moveByAngleZ API is deprecated. Please use moveByAngleZAsync() API." + self.upgrade_api_help) - def moveByAngleThrottle(self, pitch, roll, throttle, yaw_rate, duration): - raise Exception("moveByAngleThrottle API is deprecated. Please use moveByAngleThrottleAsync() API." + self.upgrade_api_help) + + # low-level control API + def moveByMotorPWMsAsync(self, front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name = ''): + return self.client.call_async('moveByMotorPWMs', front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name) + + def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll angle, pitch angle, and yaw angle set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, vehicle_name) + + def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll angle, pitch angle, and yaw angle are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawThrottle', roll, -pitch, -yaw, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw_rate (float): Desired yaw rate, in radian per second. + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw_rate (float): Desired yaw rate, in radian per second. + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawrateZ', roll, -pitch, -yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll_rate (float): Desired roll rate, in radians / second + pitch_rate (float): Desired pitch rate, in radians / second + yaw_rate (float): Desired yaw rate, in radians / second + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByAngleRatesZ', roll_rate, -pitch_rate, -yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll_rate (float): Desired roll rate, in radians / second + pitch_rate (float): Desired pitch rate, in radians / second + yaw_rate (float): Desired yaw rate, in radians / second + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByAngleRatesThrottle', roll_rate, -pitch_rate, -yaw_rate, throttle, duration, vehicle_name) + def moveByVelocity(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): raise Exception("moveByVelocity API is deprecated. Please use moveByVelocityAsync() API." + self.upgrade_api_help) def moveByVelocityZ(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): @@ -270,6 +463,66 @@ def rotateByYawRate(self, yaw_rate, duration): def setRCData(self, rcdata = RCData()): raise Exception("setRCData API is deprecated. Please use moveByRC() API." + self.upgrade_api_help) + def setAngleRateControllerGains(self, angle_rate_gains=AngleRateControllerGains(), vehicle_name = ''): + """ + - Modifying these gains will have an affect on *ALL* move*() APIs. + This is because any velocity setpoint is converted to an angle level setpoint which is tracked with an angle level controllers. + That angle level setpoint is itself tracked with and angle rate controller. + - This function should only be called if the default angle rate control PID gains need to be modified. + + Args: + angle_rate_gains (AngleRateControllerGains): + - Correspond to the roll, pitch, yaw axes, defined in the body frame. + - Pass AngleRateControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setAngleRateControllerGains', *(angle_rate_gains.to_lists()+(vehicle_name,))) + + def setAngleLevelControllerGains(self, angle_level_gains=AngleLevelControllerGains(), vehicle_name = ''): + """ + - Sets angle level controller gains (used by any API setting angle references - for ex: moveByRollPitchYawZAsync(), moveByRollPitchYawThrottleAsync(), etc) + - Modifying these gains will also affect the behaviour of moveByVelocityAsync() API. + This is because the AirSim flight controller will track velocity setpoints by converting them to angle set points. + - This function should only be called if the default angle level control PID gains need to be modified. + - Passing AngleLevelControllerGains() sets gains to default airsim values. + + Args: + angle_level_gains (AngleLevelControllerGains): + - Correspond to the roll, pitch, yaw axes, defined in the body frame. + - Pass AngleLevelControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setAngleLevelControllerGains', *(angle_level_gains.to_lists()+(vehicle_name,))) + + def setVelocityControllerGains(self, velocity_gains=VelocityControllerGains(), vehicle_name = ''): + """ + - Sets velocity controller gains for moveByVelocityAsync(). + - This function should only be called if the default velocity control PID gains need to be modified. + - Passing VelocityControllerGains() sets gains to default airsim values. + + Args: + velocity_gains (VelocityControllerGains): + - Correspond to the world X, Y, Z axes. + - Pass VelocityControllerGains() to reset gains to default recommended values. + - Modifying velocity controller gains will have an affect on the behaviour of moveOnSplineAsync() and moveOnSplineVelConstraintsAsync(), as they both use velocity control to track the trajectory. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setVelocityControllerGains', *(velocity_gains.to_lists()+(vehicle_name,))) + + + def setPositionControllerGains(self, position_gains=PositionControllerGains(), vehicle_name = ''): + """ + Sets position controller gains for moveByPositionAsync. + This function should only be called if the default position control PID gains need to be modified. + + Args: + position_gains (PositionControllerGains): + - Correspond to the X, Y, Z axes. + - Pass PositionControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setPositionControllerGains', *(position_gains.to_lists()+(vehicle_name,))) + # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): def __init__(self, ip = "", port = 41451, timeout_value = 3600): diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 6bbbcfbae4..44fe5584fe 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -403,4 +403,98 @@ class DistanceSensorData(MsgpackMixin): distance = Quaternionr() min_distance = Quaternionr() max_distance = Quaternionr() - relative_pose = Pose() \ No newline at end of file + relative_pose = Pose() + +class PIDGains(): + """ + Struct to store values of PID gains. Used to transmit controller gain values while instantiating + AngleLevel/AngleRate/Velocity/PositionControllerGains objects. + + Attributes: + kP (float): Proportional gain + kI (float): Integrator gain + kD (float): Derivative gain + """ + def __init__(self, kp, ki, kd): + self.kp = kp + self.ki = ki + self.kd = kd + + def to_list(self): + return [self.kp, self.ki, self.kd] + +class AngleRateControllerGains(): + """ + Struct to contain controller gains used by angle level PID controller + + Attributes: + roll_gains (PIDGains): kP, kI, kD for roll axis + pitch_gains (PIDGains): kP, kI, kD for pitch axis + yaw_gains (PIDGains): kP, kI, kD for yaw axis + """ + def __init__(self, roll_gains = PIDGains(0.25, 0, 0), + pitch_gains = PIDGains(0.25, 0, 0), + yaw_gains = PIDGains(0.25, 0, 0)): + self.roll_gains = roll_gains + self.pitch_gains = pitch_gains + self.yaw_gains = yaw_gains + + def to_lists(self): + return [self.roll_gains.kp, self.pitch_gains.kp, self.yaw_gains.kp], [self.roll_gains.ki, self.pitch_gains.ki, self.yaw_gains.ki], [self.roll_gains.kd, self.pitch_gains.kd, self.yaw_gains.kd] + +class AngleLevelControllerGains(): + """ + Struct to contain controller gains used by angle rate PID controller + + Attributes: + roll_gains (PIDGains): kP, kI, kD for roll axis + pitch_gains (PIDGains): kP, kI, kD for pitch axis + yaw_gains (PIDGains): kP, kI, kD for yaw axis + """ + def __init__(self, roll_gains = PIDGains(2.5, 0, 0), + pitch_gains = PIDGains(2.5, 0, 0), + yaw_gains = PIDGains(2.5, 0, 0)): + self.roll_gains = roll_gains + self.pitch_gains = pitch_gains + self.yaw_gains = yaw_gains + + def to_lists(self): + return [self.roll_gains.kp, self.pitch_gains.kp, self.yaw_gains.kp], [self.roll_gains.ki, self.pitch_gains.ki, self.yaw_gains.ki], [self.roll_gains.kd, self.pitch_gains.kd, self.yaw_gains.kd] + +class VelocityControllerGains(): + """ + Struct to contain controller gains used by velocity PID controller + + Attributes: + x_gains (PIDGains): kP, kI, kD for X axis + y_gains (PIDGains): kP, kI, kD for Y axis + z_gains (PIDGains): kP, kI, kD for Z axis + """ + def __init__(self, x_gains = PIDGains(0.2, 0, 0), + y_gains = PIDGains(0.2, 0, 0), + z_gains = PIDGains(2.0, 2.0, 0)): + self.x_gains = x_gains + self.y_gains = y_gains + self.z_gains = z_gains + + def to_lists(self): + return [self.x_gains.kp, self.y_gains.kp, self.z_gains.kp], [self.x_gains.ki, self.y_gains.ki, self.z_gains.ki], [self.x_gains.kd, self.y_gains.kd, self.z_gains.kd] + +class PositionControllerGains(): + """ + Struct to contain controller gains used by position PID controller + + Attributes: + x_gains (PIDGains): kP, kI, kD for X axis + y_gains (PIDGains): kP, kI, kD for Y axis + z_gains (PIDGains): kP, kI, kD for Z axis + """ + def __init__(self, x_gains = PIDGains(0.25, 0, 0), + y_gains = PIDGains(0.25, 0, 0), + z_gains = PIDGains(0.25, 0, 0)): + self.x_gains = x_gains + self.y_gains = y_gains + self.z_gains = z_gains + + def to_lists(self): + return [self.x_gains.kp, self.y_gains.kp, self.z_gains.kp], [self.x_gains.ki, self.y_gains.ki, self.z_gains.ki], [self.x_gains.kd, self.y_gains.kd, self.z_gains.kd]