Subversion Repositories testOled

Rev

Blame | Last modification | View Log | Download | RSS feed

  1. #include "ref.h"
  2.  
  3. float32_t ref_pid_f32(
  4.         arm_pid_instance_f32 * S,
  5.         float32_t in)
  6. {
  7.         float32_t out;
  8.  
  9.         /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]  */
  10.         out = S->state[2] + S->A0 * in + S->A1 * S->state[0] + S->A2 * S->state[1];
  11.  
  12.         /* Update state */
  13.         S->state[1] = S->state[0];
  14.         S->state[0] = in;
  15.         S->state[2] = out;
  16.  
  17.         /* return to application */
  18.         return (out);
  19. }
  20.  
  21. q31_t ref_pid_q31(
  22.         arm_pid_instance_q31 * S,
  23.         q31_t in)
  24. {
  25.         q63_t acc;
  26.         q31_t out;
  27.  
  28.         /* acc = A0 * x[n]  */
  29.         acc = (q63_t) S->A0 * in;
  30.  
  31.         /* acc += A1 * x[n-1] */
  32.         acc += (q63_t) S->A1 * S->state[0];
  33.  
  34.         /* acc += A2 * x[n-2]  */
  35.         acc += (q63_t) S->A2 * S->state[1];
  36.  
  37.         /* convert output to 1.31 format to add y[n-1] */
  38.         out = (q31_t) (acc >> 31U);
  39.  
  40.         /* out += y[n-1] */
  41.         out += S->state[2];
  42.  
  43.         /* Update state */
  44.         S->state[1] = S->state[0];
  45.         S->state[0] = in;
  46.         S->state[2] = out;
  47.  
  48.         /* return to application */
  49.         return (out);
  50. }
  51.  
  52. q15_t ref_pid_q15(
  53.         arm_pid_instance_q15 * S,
  54.         q15_t in)
  55. {
  56.         q63_t acc;
  57.         q15_t out;
  58.         q15_t A1, A2;
  59.        
  60. #if defined (ARM_MATH_DSP)
  61.    
  62. #ifndef  ARM_MATH_BIG_ENDIAN
  63.         A2 = S->A1 >> 16;
  64.         A1 = (q15_t)S->A1;     
  65. #else
  66.         A1 = S->A1 >> 16;
  67.         A2 = (q15_t)S->A1;     
  68. #endif
  69.    
  70. #else
  71.        
  72.         A1 = S->A1;
  73.         A2 = S->A2;
  74.        
  75. #endif 
  76.        
  77.         /* acc = A0 * x[n]  */
  78.         acc = ((q31_t) S->A0) * in;
  79.  
  80.         /* acc += A1 * x[n-1] + A2 * x[n-2]  */
  81.         acc += (q31_t) A1 * S->state[0];
  82.         acc += (q31_t) A2 * S->state[1];
  83.  
  84.         /* acc += y[n-1] */
  85.         acc += (q31_t) S->state[2] << 15;
  86.  
  87.         /* saturate the output */
  88.         out = ref_sat_q15(acc >> 15);
  89.  
  90.         /* Update state */
  91.         S->state[1] = S->state[0];
  92.         S->state[0] = in;
  93.         S->state[2] = out;
  94.  
  95.         /* return to application */
  96.         return (out);
  97. }
  98.