Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define P 3
- #define Q 3
- const float feedforward_coeffs[P] = { 0.0316, -0.06319, 0.0316 };
- const float feedback_coeffs[Q] = { 0.99835, -1.99835, 1.0 };
- float feedforward_buf[P] = { 0.0 };
- float feedback_buf[Q] = { 0.0 };
- float pass(float inp) {
- float out = 0.0;
- // shift value
- for (int i = P-1; i > 0; i--) {
- feedforward_buf[i-1] = feedforward_buf[i];
- }
- for (int i = Q-1; i > 0; --i) {
- feedback_buf[i-1] = feedback_buf[i];
- }
- // add new input value
- feedforward_buf[0] = inp;
- // feedfoward
- for (int i = 0; i < P; ++i) {
- out += feedforward_buf[i] * feedforward_coeffs[i];
- }
- // feedback
- for (int i = 1; i < Q; ++i) {
- out -= feedback_buf[i] * feedback_coeffs[i];
- }
- // add new output value
- feedback_buf[0] = out;
- return out;
- }
- int main() {
- for (;;) {
- // Read without from ADC
- float inp = adc_read_value();
- // Filter
- float out = pass(inp);
- // out is filtered with coeffs stored in feedforward_coeffs and
- // feedback_coeffs
- // Do something with out ...
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement