SHARE
TWEET

Untitled

a guest Jul 18th, 2019 64 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define P 3
  2. #define Q 3
  3.  
  4. const float feedforward_coeffs[P] = { 0.0316, -0.06319, 0.0316 };
  5. const float feedback_coeffs[Q] = { 0.99835, -1.99835, 1.0 };
  6.  
  7. float feedforward_buf[P] = { 0.0 };
  8. float feedback_buf[Q] = { 0.0 };
  9.  
  10. float pass(float inp) {
  11.     float out = 0.0;
  12.  
  13.     // shift value
  14.     for (int i = P-1; i > 0; i--) {
  15.         feedforward_buf[i-1] = feedforward_buf[i];
  16.     }
  17.  
  18.     for (int i = Q-1; i > 0; --i) {
  19.         feedback_buf[i-1] = feedback_buf[i];
  20.     }
  21.  
  22.     // add new input value
  23.     feedforward_buf[0] = inp;
  24.  
  25.     // feedfoward
  26.     for (int i = 0; i < P; ++i) {
  27.         out += feedforward_buf[i] * feedforward_coeffs[i];
  28.     }
  29.  
  30.     // feedback
  31.     for (int i = 1; i < Q; ++i) {
  32.         out -= feedback_buf[i] * feedback_coeffs[i];
  33.     }
  34.  
  35.     // add new output value
  36.     feedback_buf[0] = out;
  37.  
  38.     return out;
  39. }
  40.  
  41. int main() {
  42.     for (;;) {
  43.         // Read without from ADC
  44.         float inp = adc_read_value();
  45.  
  46.         // Filter
  47.         float out = pass(inp);
  48.  
  49.         // out is filtered with coeffs stored in feedforward_coeffs and
  50.         // feedback_coeffs
  51.  
  52.         // Do something with out ...
  53.     }
  54. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top