Line data Source code
1 : /*------------------------------------------------------------------------------ 2 : -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW), 3 : -- This file is a part of the LFR FSW 4 : -- Copyright (C) 2021, Plasma Physics Laboratory - CNRS 5 : -- 6 : -- This program is free software; you can redistribute it and/or modify 7 : -- it under the terms of the GNU General Public License as published by 8 : -- the Free Software Foundation; either version 2 of the License, or 9 : -- (at your option) any later version. 10 : -- 11 : -- This program is distributed in the hope that it will be useful, 12 : -- but WITHOUT ANY WARRANTY; without even the implied warranty of 13 : -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 : -- GNU General Public License for more details. 15 : -- 16 : -- You should have received a copy of the GNU General Public License 17 : -- along with this program; if not, write to the Free Software 18 : -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 : -------------------------------------------------------------------------------*/ 20 : /*-- Author : Alexis Jeandet 21 : -- Contact : Alexis Jeandet 22 : -- Mail : alexis.jeandet@lpp.polytechnique.fr 23 : ----------------------------------------------------------------------------*/ 24 : #pragma once 25 : #include <stdint.h> 26 : 27 : #ifdef __cplusplus 28 : extern "C" 29 : { 30 : #endif 31 : 32 : #define B00 196 33 : #define B01 196 34 : #define B02 0 35 : #define B10 131 36 : #define B11 -244 37 : #define B12 131 38 : #define B20 161 39 : #define B21 -314 40 : #define B22 161 41 : 42 : #define A00 1 43 : #define A01 -925 44 : #define A02 0 45 : #define A10 1 46 : #define A11 -947 47 : #define A12 439 48 : #define A20 1 49 : #define A21 -993 50 : #define A22 486 51 : 52 : #define GAIN_B0 12 53 : #define GAIN_B1 11 54 : #define GAIN_B2 10 55 : 56 : #define GAIN_A0 10 57 : #define GAIN_A1 9 58 : #define GAIN_A2 9 59 : 60 : #define NB_COEFFS 3 61 : #define COEFF0 0 62 : #define COEFF1 1 63 : #define COEFF2 2 64 : 65 : 66 : typedef struct filter_ctx 67 : { 68 : int W[NB_COEFFS][NB_COEFFS]; 69 : } filter_ctx; 70 : 71 : 72 : /** 73 : * @brief filter is a Direct-Form-II filter implementation, mostly used to filter electric field 74 : * for HK 75 : * @param x, new sample 76 : * @param ctx, filter context, used to store previous input and output samples 77 : * @return a new filtered sample 78 : */ 79 : static inline int filter(int x, filter_ctx* ctx) 80 : { 81 : static const int b[NB_COEFFS][NB_COEFFS] 82 : = { { B00, B01, B02 }, { B10, B11, B12 }, { B20, B21, B22 } }; 83 : static const int a[NB_COEFFS][NB_COEFFS] 84 : = { { A00, A01, A02 }, { A10, A11, A12 }, { A20, A21, A22 } }; 85 : static const int b_gain[NB_COEFFS] = { GAIN_B0, GAIN_B1, GAIN_B2 }; 86 : static const int a_gain[NB_COEFFS] = { GAIN_A0, GAIN_A1, GAIN_A2 }; 87 : 88 : // Direct-Form-II 89 296172 : for (int i = 0; i < NB_COEFFS; i++) 90 : { 91 222129 : x = x << a_gain[i]; 92 : int32_t W 93 222129 : = (x - (a[i][COEFF1] * ctx->W[i][COEFF0]) - (a[i][COEFF2] * ctx->W[i][COEFF1])) 94 222129 : >> a_gain[i]; 95 888516 : x = (b[i][COEFF0] * W) + (b[i][COEFF1] * ctx->W[i][COEFF0]) 96 666387 : + (b[i][COEFF2] * ctx->W[i][COEFF1]); 97 222129 : x = x >> b_gain[i]; 98 222129 : ctx->W[i][1] = ctx->W[i][0]; 99 222129 : ctx->W[i][0] = W; 100 : } 101 74043 : return x; 102 : } 103 : 104 : #ifdef __cplusplus 105 : } 106 : #endif