Merge pull request #1387 from bitcraze/rik/derivative_kick_documentation

Improved PID documentation

docs/functional-areas/sensor-to-control/controllers.md

@@ -26,13 +26,11 @@ Here is an overview of the types of controllers there are per level:
 
 We will now explain per controller how exactly they are being implemented in the [crazyflie-firmware](https://github.com/bitcraze/crazyflie-firmware/).
 
-
-
 ## Cascaded PID controller
 
+By default, the Crazyflie firmware utilizes [proportional integral derivative (PID)](https://en.wikipedia.org/wiki/PID_controller) control to manage the drone's state. The firmware employs distinct PID controllers for each control level: position, velocity, attitude, and attitude rate. The output of each controller feeds into the input of the next, lower level controller, forming a cascaded PID structure. Depending on the [control mode](/docs/functional-areas/sensor-to-control/commanders_setpoints/#setpoint-structure), different setpoints can be fed into the system, influencing which PID controllers are activated. For instance, when using attitude rate setpoints, only the attitude rate PID controller is active; for attitude setpoints, both the attitude and attitude rate PID controllers are used; and so on for velocity and position setpoints. Ultimately, regardless of the control mode, the angle rate controller translates the desired angle rates into PWM commands for the motors.
 
-So the default settings in the Crazyflie firmware is the [proportional integral derivative (PID)](https://en.wikipedia.org/wiki/PID_controller) control for all desired state aspects. So the High Level Commander (HLC) or position will send desired position set-points to the PID position controller. These result in desired pitch and roll angles, which are sent directly to the attitude PID controller. These determine the desired angle rates which is send to the angle rate controller. This is also called Cascaded PID controller. That results in the desired thrusts for the roll pitch yaw and height that will be handled by the power distribution by the motors.
-
+To enhance control stability and prevent issues during significant setpoint changes, we have implemented a workaround to avoid derivative kick—a sudden spike in control output caused by changes in the setpoint. This workaround calculates the derivative term using the rate of change of the measured process variable rather than the rate of change of the error. By preventing derivative kick, particularly during large setpoint changes, this approach ensures smoother and more reliable control.
 
 Here is a block schematics of how the PID controllers are implemented.
 

src/utils/src/pid.c

@@ -66,6 +66,13 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError)
   pid->outP = pid->kp * pid->error;
   output += pid->outP;
 
+  /*
+  * Note: The derivative term in this PID controller is implemented based on the
+  * derivative of the measured process variable instead of the error.
+  * This approach avoids derivative kick, which can occur due to sudden changes
+  * in the setpoint. By using the process variable for the derivative calculation, we achieve
+  * smoother and more stable control during setpoint changes.
+  */
   float deriv = -(measured - pid->prevMeasured) / pid->dt;
 
   #if CONFIG_CONTROLLER_PID_FILTER_ALL