[simpleflight] enable on the fly PID gain setting

Co-Authored-By: madratman <ratneshmadaan@gmail.com>
Co-Authored-By: Sai Vemprala <saihemachandra.v@gmail.com>

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<float>& kp, const vector<float>& ki, const vector<float>& 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<float>& kp, const vector<float>& ki, const vector<float>& kd); 
+    virtual void setAngleRateControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd); 
+    virtual void setVelocityControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd); 
+    virtual void setPositionControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd); 
+
     /************************* Safety APIs *********************************/
     virtual void setSafetyEval(const shared_ptr<SafetyEval> safety_eval_ptr);
     virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,

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<float>& kp, const vector<float>& ki, const vector<float>& kd, const std::string& vehicle_name="");
+    void setAngleRateControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd, const std::string& vehicle_name="");
+    void setVelocityControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd, const std::string& vehicle_name="");
+    void setPositionControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& 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,

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<float>& kp, const vector<float>& ki, const vector<float>& kd) override
+    {
+        simple_flight::GoalModeType controller_type_enum = static_cast<simple_flight::GoalModeType>(controller_type);
+
+        vector<float> kp_axis4(4);
+        vector<float> ki_axis4(4);
+        vector<float> 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));

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<float>(clock_,
-            PidConfig<float>(params_->angle_level_pid.p[axis], 0, 0)));
+            PidConfig<float>(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<PidController<float>> pid_;
     std::unique_ptr<AngleRateController> rate_controller_;

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<float>(clock_,
-           PidConfig<float>(params_->angle_rate_pid.p[axis], 0, 0)));
+           PidConfig<float>(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<PidController<float>> pid_;
 };

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_;

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_;

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 {

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<float>(clock_,
-            PidConfig<float>(params_->position_pid.p[axis], 0, 0)));
+            PidConfig<float>(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<PidController<float>> pid_;
     std::unique_ptr<VelocityController> velocity_controller_;

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<float> 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<PidController<float>> pid_;
     std::unique_ptr<IAxisController> child_controller_;

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<T>& vals)
+    {
+        vals_[0] = vals[0];
+        vals_[1] = vals[1];
+        vals_[2] = vals[2];
+    }
+
     virtual std::string toString() const
     {
         return std::to_string(static_cast<float>(vals_[0]))
@@ -95,6 +104,14 @@ class Axis4 : public Axis3<T> {
             (*this)[axis] = axis3[axis];
     }
 
+    void setValues(const vector<T>& vals)
+    {
+        (*this)[0] = vals[0];
+        (*this)[1] = vals[1];
+        (*this)[2] = vals[2];
+        val4_ = vals[3];
+    }
+
     T& val4()
     {
         return val4_;

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<float>& kp, const vector<float>& ki, const vector<float>& kd) 
+{
+    uint8_t controller_type = 2;
+    setControllerGains(controller_type, kp, ki, kd);
+}
+
+void MultirotorApiBase::setAngleRateControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) 
+{
+    uint8_t controller_type = 3;
+    setControllerGains(controller_type, kp, ki, kd);
+}
+
+void MultirotorApiBase::setVelocityControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) 
+{
+    uint8_t controller_type = 4;
+    setControllerGains(controller_type, kp, ki, kd);
+}
+
+void MultirotorApiBase::setPositionControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) 
+{
+    uint8_t controller_type = 5;
+    setControllerGains(controller_type, kp, ki, kd);
+}
+
 bool MultirotorApiBase::hover()
 {
     SingleTaskCall lock(this);