diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index bbbe6af9..5062b695 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -181,6 +181,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
 
     // Position control
     // TODO Decide if we want to use encoder or pll position here
+    float gain_scheduling_multiplier = 1.0f;
     float vel_des = vel_setpoint_;
     if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) {
         float pos_err;
@@ -197,6 +198,11 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
             pos_err = pos_setpoint_ - pos_estimate;
         }
         vel_des += config_.pos_gain * pos_err;
+                // V-shaped gain shedule based on position error
+        float abs_pos_err = fabsf(pos_err);
+        if (config_.enable_gain_scheduling && abs_pos_err <= config_.gain_scheduling_width) {
+            gain_scheduling_multiplier = abs_pos_err / config_.gain_scheduling_width;
+        }
     }
 
     // Velocity limiting
@@ -224,7 +230,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
 
     float v_err = vel_des - vel_estimate;
     if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
-        Iq += config_.vel_gain * v_err;
+        Iq += (config_.vel_gain * gain_scheduling_multiplier) * v_err;
     }
 
     // Velocity integral action before limiting
@@ -251,7 +257,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
             // TODO make decayfactor configurable
             vel_integrator_current_ *= 0.99f;
         } else {
-            vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err;
+            vel_integrator_current_ += ((config_.vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err;
         }
     }
 
diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index 5b26940c..4c954a9a 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -56,6 +56,8 @@ public:
         float input_filter_bandwidth = 2.0f; // [1/s]
         float homing_speed = 2000.0f;   // [counts/s]
         Anticogging_t anticogging;
+        float gain_scheduling_width = 10.0f;
+        bool enable_gain_scheduling = false;
     };
 
     explicit Controller(Config_t& config);
@@ -120,6 +122,8 @@ public:
             make_protocol_ro_property("trajectory_done", &trajectory_done_),
             make_protocol_property("vel_integrator_current", &vel_integrator_current_),
             make_protocol_property("anticogging_valid", &anticogging_valid_),
+            make_protocol_property("gain_scheduling_width", &config_.gain_scheduling_width),
+            make_protocol_property("enable_gain_scheduling", &config_.enable_gain_scheduling),
             make_protocol_object("config",
                 make_protocol_property("control_mode", &config_.control_mode),
                 make_protocol_property("input_mode", &config_.input_mode),