# HG changeset patch
# User paul
# Date 2017-03-28 06:10:57
# Node ID cae68a260aa21e2bfbe2826d3fc01a8ce2a1c474
# Parent  18c114e265c0aa022440383848590ac72453c6ab

3.2.0.10
AJE filter implemented in AVGV

diff --git a/header/fsw_misc.h b/header/fsw_misc.h
--- a/header/fsw_misc.h
+++ b/header/fsw_misc.h
@@ -66,6 +66,40 @@ typedef struct{
     unsigned char dpu_spw_rx_too_big;
 } hk_lfr_me_t;
 
+#define B00 23
+#define B01 23
+#define B02 0
+#define B10 1024
+#define B11 -1771
+#define B12 1024
+#define B20 1024
+#define B21 -1937
+#define B22 1024
+
+#define A00 1
+#define A01 -28324
+#define A02 0
+#define A10 1
+#define A11 -1828
+#define A12 822
+#define A20 1
+#define A21 -1956
+#define A22 950
+
+#define G0  15
+#define G1  10
+#define G2  10
+
+#define NB_COEFFS   3
+#define COEFF0  0
+#define COEFF1  1
+#define COEFF2  2
+
+typedef struct filter_ctx
+{
+    int W[NB_COEFFS][NB_COEFFS];
+}filter_ctx;
+
 extern gptimer_regs_t *gptimer_regs;
 extern void ASR16_get_FPRF_IURF_ErrorCounters( unsigned int*, unsigned int* );
 extern void CCR_getInstructionAndDataErrorCounters( unsigned int*, unsigned int* );
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -64,7 +64,7 @@ option(FSW_debug_tch "?" OFF)
 set(SW_VERSION_N1 "3" CACHE STRING  "Choose N1 FSW Version." FORCE)
 set(SW_VERSION_N2 "2" CACHE STRING  "Choose N2 FSW Version." FORCE)
 set(SW_VERSION_N3 "0" CACHE STRING  "Choose N3 FSW Version." FORCE)
-set(SW_VERSION_N4 "9" CACHE STRING  "Choose N4 FSW Version." FORCE)
+set(SW_VERSION_N4 "10" CACHE STRING  "Choose N4 FSW Version." FORCE)
 
 if(FSW_verbose)
 	add_definitions(-DPRINT_MESSAGES_ON_CONSOLE)
diff --git a/src/fsw_misc.c b/src/fsw_misc.c
--- a/src/fsw_misc.c
+++ b/src/fsw_misc.c
@@ -347,6 +347,34 @@ rtems_task hous_task(rtems_task_argument
     return;
 }
 
+int filter( int x, filter_ctx* ctx )
+{
+    static const int b[NB_COEFFS][NB_COEFFS]={ {B00, B01, B02}, {B10, B11, B12}, {B20, B21, B22} };
+    static const int a[NB_COEFFS][NB_COEFFS]={ {A00, A01, A02}, {A10, A11, A12}, {A20, A21, A22} };
+    static const int g_pow2[NB_COEFFS]={G0, G1, G2};
+
+    int W;
+    int i;
+
+    W = INIT_INT;
+    i = INIT_INT;
+
+    //Direct-Form-II
+    for ( i = 0; i < NB_COEFFS; i++ )
+    {
+        x = x << g_pow2[ i ];
+        W = ( x - ( a[i][COEFF1] * ctx->W[i][COEFF0] )
+                - ( a[i][COEFF2] * ctx->W[i][COEFF1] ) ) >> g_pow2[ i ];
+        x = ( b[i][COEFF0] * W )
+                + ( b[i][COEFF1] * ctx->W[i][COEFF0] )
+                + ( b[i][COEFF2] * ctx->W[i][COEFF1] );
+        x =- ( x >> g_pow2[i] );
+        ctx->W[i][COEFF1] = ctx->W[i][COEFF0];
+        ctx->W[i][COEFF0] = W;
+    }
+    return x;
+}
+
 rtems_task avgv_task(rtems_task_argument argument)
 {
 #define MOVING_AVERAGE 16
@@ -369,6 +397,10 @@ rtems_task avgv_task(rtems_task_argument
     unsigned char k;
     unsigned char indexOfOldValue;
 
+    static filter_ctx ctx_v = { { {0,0,0}, {0,0,0}, {0,0,0} } };
+    static filter_ctx ctx_e1 = { { {0,0,0}, {0,0,0}, {0,0,0} } };
+    static filter_ctx ctx_e2 = { { {0,0,0}, {0,0,0}, {0,0,0} } };
+
     BOOT_PRINTF("in AVGV ***\n");
 
     if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &AVGV_id) != RTEMS_SUCCESSFUL) {
@@ -416,29 +448,9 @@ rtems_task avgv_task(rtems_task_argument
                  || (current_e1 != old_e1)
                  || (current_e2 != old_e2))
             {
-                // get new values
-                newValue_v =  current_v;
-                newValue_e1 = current_e1;
-                newValue_e2 = current_e2;
-
-                // compute the moving average
-                average_v = average_v + newValue_v - v[k];
-                average_e1 = average_e1 + newValue_e1 - e1[k];
-                average_e2 = average_e2 + newValue_e2 - e2[k];
-
-                // store new values in buffers
-                v[k] = newValue_v;
-                e1[k] = newValue_e1;
-                e2[k] = newValue_e2;
-
-                if (k == (MOVING_AVERAGE-1))
-                {
-                    k = 0;
-                }
-                else
-                {
-                    k++;
-                }
+                average_v = filter( current_v, &ctx_v );
+                average_e1 = filter( current_e1, &ctx_e1 );
+                average_e2 = filter( current_e2, &ctx_e2 );
 
                 //update int16 values
                 hk_lfr_sc_v_f3_as_int16 =  (int16_t) (average_v / MOVING_AVERAGE );