Fix PID derivative kickoff and reset

src/modules/interface/controller/attitude_controller.h

@@ -56,17 +56,17 @@ void attitudeControllerCorrectRatePID(
 /**
  * Reset controller roll attitude PID
  */
-void attitudeControllerResetRollAttitudePID(void);
+void attitudeControllerResetRollAttitudePID(float rollActual);
 
 /**
  * Reset controller pitch attitude PID
  */
-void attitudeControllerResetPitchAttitudePID(void);
+void attitudeControllerResetPitchAttitudePID(float pitchActual);
 
 /**
  * Reset controller roll, pitch and yaw PID's.
  */
-void attitudeControllerResetAllPID(void);
+void attitudeControllerResetAllPID(float rollActual, float pitchActual, float yawActual);
 
 /**
  * Get the actuator output.

src/modules/interface/controller/position_controller.h

@@ -31,7 +31,7 @@
 // A position controller calculate the thrust, roll, pitch to approach
 // a 3D position setpoint
 void positionControllerInit();
-void positionControllerResetAllPID();
+void positionControllerResetAllPID(float xActual, float yActual, float zActual);
 void positionControllerResetAllfilters();
 void positionController(float* thrust, attitude_t *attitude, const setpoint_t *setpoint,
                                                              const state_t *state);

src/modules/src/controller/attitude_pid_controller.c

@@ -143,14 +143,14 @@ void attitudeControllerCorrectRatePID(
        float rollRateDesired, float pitchRateDesired, float yawRateDesired)
 {
   pidSetDesired(&pidRollRate, rollRateDesired);
-  rollOutput = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, true));
+  rollOutput = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, false));
 
   pidSetDesired(&pidPitchRate, pitchRateDesired);
-  pitchOutput = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, true));
+  pitchOutput = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, false));
 
   pidSetDesired(&pidYawRate, yawRateDesired);
 
-  yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true));
+  yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, false));
 }
 
 void attitudeControllerCorrectAttitudePID(
@@ -159,41 +159,35 @@ void attitudeControllerCorrectAttitudePID(
        float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired)
 {
   pidSetDesired(&pidRoll, eulerRollDesired);
-  *rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, true);
+  *rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, false);
 
   // Update PID for pitch axis
   pidSetDesired(&pidPitch, eulerPitchDesired);
-  *pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, true);
+  *pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, false);
 
   // Update PID for yaw axis
-  float yawError;
-  yawError = eulerYawDesired - eulerYawActual;
-  if (yawError > 180.0f)
-    yawError -= 360.0f;
-  else if (yawError < -180.0f)
-    yawError += 360.0f;
-  pidSetError(&pidYaw, yawError);
-  *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false);
+  pidSetDesired(&pidYaw, eulerYawDesired);
+  *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, true);
 }
 
-void attitudeControllerResetRollAttitudePID(void)
+void attitudeControllerResetRollAttitudePID(float rollActual)
 {
-    pidReset(&pidRoll);
+    pidReset(&pidRoll, rollActual);
 }
 
-void attitudeControllerResetPitchAttitudePID(void)
+void attitudeControllerResetPitchAttitudePID(float pitchActual)
 {
-    pidReset(&pidPitch);
+    pidReset(&pidPitch, pitchActual);
 }
 
-void attitudeControllerResetAllPID(void)
+void attitudeControllerResetAllPID(float rollActual, float pitchActual, float yawActual)
 {
-  pidReset(&pidRoll);
-  pidReset(&pidPitch);
-  pidReset(&pidYaw);
-  pidReset(&pidRollRate);
-  pidReset(&pidPitchRate);
-  pidReset(&pidYawRate);
+  pidReset(&pidRoll, rollActual);
+  pidReset(&pidPitch, pitchActual);
+  pidReset(&pidYaw, yawActual);
+  pidReset(&pidRollRate, 0);
+  pidReset(&pidPitchRate, 0);
+  pidReset(&pidYawRate, 0);
 }
 
 void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw)

src/modules/src/controller/controller_indi.c

@@ -225,11 +225,11 @@ void controllerINDI(control_t *control, const setpoint_t *setpoint,
 		// behavior if level mode is engaged later
 		if (setpoint->mode.roll == modeVelocity) {
 			rateDesired.roll = radians(setpoint->attitudeRate.roll);
-			attitudeControllerResetRollAttitudePID();
+			attitudeControllerResetRollAttitudePID(state->attitude.roll);
 		}
 		if (setpoint->mode.pitch == modeVelocity) {
 			rateDesired.pitch = radians(setpoint->attitudeRate.pitch);
-			attitudeControllerResetPitchAttitudePID();
+			attitudeControllerResetPitchAttitudePID(state->attitude.pitch);
 		}
 
 		/*
@@ -317,8 +317,8 @@ void controllerINDI(control_t *control, const setpoint_t *setpoint,
 		float_rates_zero(&indi.u_in);
 
 		if(indi.thrust == 0){
-			attitudeControllerResetAllPID();
-			positionControllerResetAllPID();
+			attitudeControllerResetAllPID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw);
+			positionControllerResetAllPID(state->position.x, state->position.y, state->position.z);
 
 			// Reset the calculated YAW angle for rate control
 			attitudeDesired.yaw = -state->attitude.yaw;

src/modules/src/controller/controller_pid.c

@@ -113,11 +113,11 @@ void controllerPid(control_t *control, const setpoint_t *setpoint,
     // behavior if level mode is engaged later
     if (setpoint->mode.roll == modeVelocity) {
       rateDesired.roll = setpoint->attitudeRate.roll;
-      attitudeControllerResetRollAttitudePID();
+      attitudeControllerResetRollAttitudePID(state->attitude.roll);
     }
     if (setpoint->mode.pitch == modeVelocity) {
       rateDesired.pitch = setpoint->attitudeRate.pitch;
-      attitudeControllerResetPitchAttitudePID();
+      attitudeControllerResetPitchAttitudePID(state->attitude.pitch);
     }
 
     // TODO: Investigate possibility to subtract gyro drift.
@@ -154,8 +154,8 @@ void controllerPid(control_t *control, const setpoint_t *setpoint,
     cmd_pitch = control->pitch;
     cmd_yaw = control->yaw;
 
-    attitudeControllerResetAllPID();
-    positionControllerResetAllPID();
+    attitudeControllerResetAllPID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw);
+    positionControllerResetAllPID(state->position.x, state->position.y, state->position.z);
 
     // Reset the calculated YAW angle for rate control
     attitudeDesired.yaw = state->attitude.yaw;

src/modules/src/controller/position_controller_pid.c

@@ -184,7 +184,7 @@ static float runPid(float input, struct pidAxis_s *axis, float setpoint, float d
   axis->setpoint = setpoint;
 
   pidSetDesired(&axis->pid, axis->setpoint);
-  return pidUpdate(&axis->pid, input, true);
+  return pidUpdate(&axis->pid, input, false);
 }
 
 
@@ -266,14 +266,14 @@ void velocityController(float* thrust, attitude_t *attitude, const Axis3f* setpo
   *thrust = constrain(*thrust, 0, UINT16_MAX);
 }
 
-void positionControllerResetAllPID()
+void positionControllerResetAllPID(float xActual, float yActual, float zActual)
 {
-  pidReset(&this.pidX.pid);
-  pidReset(&this.pidY.pid);
-  pidReset(&this.pidZ.pid);
-  pidReset(&this.pidVX.pid);
-  pidReset(&this.pidVY.pid);
-  pidReset(&this.pidVZ.pid);
+  pidReset(&this.pidX.pid, xActual);
+  pidReset(&this.pidY.pid, yActual);
+  pidReset(&this.pidZ.pid, zActual);
+  pidReset(&this.pidVX.pid, 0);
+  pidReset(&this.pidVY.pid, 0);
+  pidReset(&this.pidVZ.pid, 0);
 }
 
 void positionControllerResetAllfilters() {

src/modules/src/crtp_commander_rpyt.c

@@ -140,7 +140,7 @@ void crtpCommanderRpytDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk)
   if (altHoldMode) {
     if (!modeSet) {             //Reset filter and PID values on first initiation of assist mode to prevent sudden reactions.
       modeSet = true;
-      positionControllerResetAllPID();
+      positionControllerResetAllPID(0, 0, 0); // TODO: initialize with actual x, y, z position
       positionControllerResetAllfilters();
     }
     setpoint->thrust = 0;

src/utils/interface/pid.h

@@ -86,21 +86,20 @@ void pidSetIntegralLimit(PidObject* pid, const float limit);
 /**
  * Reset the PID error values
  *
- * @param[in] pid   A pointer to the pid object.
- * @param[in] limit Pid integral swing limit.
+ * @param[in] pid    A pointer to the pid object.
+ * @param[in] initial Initial value of the controlled state.
  */
-void pidReset(PidObject* pid);
+void pidReset(PidObject* pid, float initial);
 
 /**
  * Update the PID parameters.
  *
  * @param[in] pid         A pointer to the pid object.
  * @param[in] measured    The measured value
- * @param[in] updateError Set to TRUE if error should be calculated.
- *                        Set to False if pidSetError() has been used.
+ * @param[in] isYawAngle  Set to TRUE if it is a PID on yaw angle, set to false otherwise
  * @return PID algorithm output
  */
-float pidUpdate(PidObject* pid, const float measured, const bool updateError);
+float pidUpdate(PidObject* pid, const float measured, const bool isYawAngle);
 
 /**
  * Set a new set point for the PID to track.
@@ -122,14 +121,6 @@ float pidGetDesired(PidObject* pid);
  */
 bool pidIsActive(PidObject* pid);
 
-/**
- * Set a new error. Use if a special error calculation is needed.
- *
- * @param[in] pid   A pointer to the pid object.
- * @param[in] error The new error
- */
-void pidSetError(PidObject* pid, const float error);
-
 /**
  * Set a new proportional gain for the PID.
  *

src/utils/src/pid.c

@@ -54,15 +54,20 @@ void pidInit(PidObject* pid, const float desired, const float kp,
   }
 }
 
-float pidUpdate(PidObject* pid, const float measured, const bool updateError)
+float pidUpdate(PidObject* pid, const float measured, const bool isYawAngle)
 {
   float output = 0.0f;
 
-  if (updateError)
-  {
-      pid->error = pid->desired - measured;
+  pid->error = pid->desired - measured;
+
+  if (isYawAngle){
+    if (pid->error > 180.0f){
+      pid->error -= 360.0f;
+    } else if (pid->error < -180.0f){
+      pid->error += 360.0f;
+    }
   }
-
+  
   pid->outP = pid->kp * pid->error;
   output += pid->outP;
 
@@ -73,15 +78,24 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError)
   * 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;
+  float delta = -(measured - pid->prevMeasured);
+
+  // For yaw measurements, take care of spikes when crossing 180deg <-> -180deg  
+  if (isYawAngle){
+    if (delta > 180.0f){
+      delta -= 360.0f;
+    } else if (delta < -180.0f){
+      delta += 360.0f;
+    }
+  }
 
   #if CONFIG_CONTROLLER_PID_FILTER_ALL
-    pid->deriv = deriv;
+    pid->deriv = delta / pid->dt;
   #else
     if (pid->enableDFilter){
-      pid->deriv = lpf2pApply(&pid->dFilter, deriv);
+      pid->deriv = lpf2pApply(&pid->dFilter, delta / pid->dt);
     } else {
-      pid->deriv = deriv;
+      pid->deriv = delta / pid->dt;
     }
   #endif
   if (isnan(pid->deriv)) {
@@ -116,7 +130,7 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError)
     if (isnan(output)) {
       output = 0;
     }
-    #endif
+  #endif
 
   // Constrain the total PID output (unless the outputLimit is zero)
   if(pid->outputLimit != 0)
@@ -133,19 +147,14 @@ void pidSetIntegralLimit(PidObject* pid, const float limit) {
     pid->iLimit = limit;
 }
 
-void pidReset(PidObject* pid)
+void pidReset(PidObject* pid, const float actual)
 {
   pid->error        = 0;
-  pid->prevMeasured = 0;
+  pid->prevMeasured = actual;
   pid->integ        = 0;
   pid->deriv        = 0;
 }
 
-void pidSetError(PidObject* pid, const float error)
-{
-  pid->error = error;
-}
-
 void pidSetDesired(PidObject* pid, const float desired)
 {
   pid->desired = desired;