ARM DSP-PID Algorithmus mit Windup-Effekt

/**

 * @brief Instance structure for the Q31 PID Control.

*/

typedef struct

{

    q31_t A0;            /**< The derived gain, A0 = Kp + Ki + Kd . */

    q31_t A1;            /**< The derived gain, A1 = -Kp - 2Kd. */

    q31_t A2;            /**< The derived gain, A2 = Kd . */

    q31_t state[3];      /**< The state array of length 3. */

    q31_t Kp;            /**< The proportional gain. */

    q31_t Ki;            /**< The integral gain. */

    q31_t Kd;            /**< The derivative gain. */

} arm_pid_instance_q31;

void arm_pid_init_q31(arm_pid_instance_q31 * S, int32_t resetStateFlag)

{

 /* Derived coefficient A0 */

  S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd);

  /* Derived coefficient A1 */

  S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp);

}

static __INLINE q31_t arm_pid_q31(arm_pid_instance_q31 * S, q31_t in)

{

    q63_t acc;

    q31_t out;

    /* acc = A0 * x[n]  */

    acc = (q63_t) S->A0 * in;

    /* acc += A1 * x[n-1] */

    acc += (q63_t) S->A1 * S->state[0];

    /* acc += A2 * x[n-2]  */

    acc += (q63_t) S->A2 * S->state[1];

    /* convert output to 1.31 format to add y[n-1] */

    out = (q31_t) (acc >> 31u);

    /* out += y[n-1] */

    out += S->state[2];

    /* Update state */

    S->state[1] = S->state[0];

    S->state[0] = in;

    S->state[2] = out;

    /* return to application */

    return (out);

}