diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index 8676101fa0..020889f8a9 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -1,4 +1,4 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. + // Licensed under the MIT License. #ifndef air_DroneControlServer_hpp @@ -34,6 +34,9 @@ class MultirotorApiBase : public VehicleApiBase { 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) { } + /************************* State APIs *********************************/ virtual Kinematics::State getKinematicsEstimated() const = 0; virtual LandedState getLandedState() const = 0; @@ -105,6 +108,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, diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index bc832b5bbc..9d0f472fce 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -47,9 +47,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/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index ca619faf06..b7569e33fc 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -303,6 +303,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_changedgains_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/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..eb9dd68333 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 @@ -116,7 +117,7 @@ class CascadeController : public IController { private: - const Params* params_; + Params* params_; const IBoardClock* clock_; const IGoal* goal_; 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..640472bf22 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) { @@ -77,7 +77,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..1c89273c79 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 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 = 2.5f; + 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 { 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 30b8e0a0df..1003c8640f 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..3abef2e282 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,19 @@ class Axis3 { { return vals_[index]; } + virtual const T& operator[] (unsigned int index) const { return vals_[index]; } + + T& operator= (const vector& vals) + { + vals_[0] = vals[0]; + vals_[1] = vals[1]; + vals_[2] = vals[2]; + } + virtual std::string toString() const { return std::to_string(static_cast(vals_[0])) @@ -95,6 +104,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/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 75dc37e9dd..411e167b71 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -471,6 +471,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); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 7e2cf9869b..7e6d0250eb 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -179,6 +179,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 edd460b77a..13240901c3 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -127,6 +127,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());