##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » SOLO_LFR » DEV_PLE
RSS

Clone from http://pc-instru.lpp.polytechnique.fr:5000/DEV_PLE

3.2.0.10...
paul -
r360:cae68a260aa2 R3++ draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -66,6 +66,40 typedef struct{
66 unsigned char dpu_spw_rx_too_big;
66 unsigned char dpu_spw_rx_too_big;
67 } hk_lfr_me_t;
67 } hk_lfr_me_t;
68
68
69 #define B00 23
70 #define B01 23
71 #define B02 0
72 #define B10 1024
73 #define B11 -1771
74 #define B12 1024
75 #define B20 1024
76 #define B21 -1937
77 #define B22 1024
78
79 #define A00 1
80 #define A01 -28324
81 #define A02 0
82 #define A10 1
83 #define A11 -1828
84 #define A12 822
85 #define A20 1
86 #define A21 -1956
87 #define A22 950
88
89 #define G0 15
90 #define G1 10
91 #define G2 10
92
93 #define NB_COEFFS 3
94 #define COEFF0 0
95 #define COEFF1 1
96 #define COEFF2 2
97
98 typedef struct filter_ctx
99 {
100 int W[NB_COEFFS][NB_COEFFS];
101 }filter_ctx;
102
69 extern gptimer_regs_t *gptimer_regs;
103 extern gptimer_regs_t *gptimer_regs;
70 extern void ASR16_get_FPRF_IURF_ErrorCounters( unsigned int*, unsigned int* );
104 extern void ASR16_get_FPRF_IURF_ErrorCounters( unsigned int*, unsigned int* );
71 extern void CCR_getInstructionAndDataErrorCounters( unsigned int*, unsigned int* );
105 extern void CCR_getInstructionAndDataErrorCounters( unsigned int*, unsigned int* );
Show file before | Show file after | Hide comments
@@ -64,7 +64,7 option(FSW_debug_tch "?" OFF)
64 set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE)
64 set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE)
65 set(SW_VERSION_N2 "2" CACHE STRING "Choose N2 FSW Version." FORCE)
65 set(SW_VERSION_N2 "2" CACHE STRING "Choose N2 FSW Version." FORCE)
66 set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE)
66 set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE)
67 set(SW_VERSION_N4 "9" CACHE STRING "Choose N4 FSW Version." FORCE)
67 set(SW_VERSION_N4 "10" CACHE STRING "Choose N4 FSW Version." FORCE)
68
68
69 if(FSW_verbose)
69 if(FSW_verbose)
70 add_definitions(-DPRINT_MESSAGES_ON_CONSOLE)
70 add_definitions(-DPRINT_MESSAGES_ON_CONSOLE)
Show file before | Show file after | Hide comments
@@ -347,6 +347,34 rtems_task hous_task(rtems_task_argument
347 return;
347 return;
348 }
348 }
349
349
350 int filter( int x, filter_ctx* ctx )
351 {
352 static const int b[NB_COEFFS][NB_COEFFS]={ {B00, B01, B02}, {B10, B11, B12}, {B20, B21, B22} };
353 static const int a[NB_COEFFS][NB_COEFFS]={ {A00, A01, A02}, {A10, A11, A12}, {A20, A21, A22} };
354 static const int g_pow2[NB_COEFFS]={G0, G1, G2};
355
356 int W;
357 int i;
358
359 W = INIT_INT;
360 i = INIT_INT;
361
362 //Direct-Form-II
363 for ( i = 0; i < NB_COEFFS; i++ )
364 {
365 x = x << g_pow2[ i ];
366 W = ( x - ( a[i][COEFF1] * ctx->W[i][COEFF0] )
367 - ( a[i][COEFF2] * ctx->W[i][COEFF1] ) ) >> g_pow2[ i ];
368 x = ( b[i][COEFF0] * W )
369 + ( b[i][COEFF1] * ctx->W[i][COEFF0] )
370 + ( b[i][COEFF2] * ctx->W[i][COEFF1] );
371 x =- ( x >> g_pow2[i] );
372 ctx->W[i][COEFF1] = ctx->W[i][COEFF0];
373 ctx->W[i][COEFF0] = W;
374 }
375 return x;
376 }
377
350 rtems_task avgv_task(rtems_task_argument argument)
378 rtems_task avgv_task(rtems_task_argument argument)
351 {
379 {
352 #define MOVING_AVERAGE 16
380 #define MOVING_AVERAGE 16
@@ -369,6 +397,10 rtems_task avgv_task(rtems_task_argument
369 unsigned char k;
397 unsigned char k;
370 unsigned char indexOfOldValue;
398 unsigned char indexOfOldValue;
371
399
400 static filter_ctx ctx_v = { { {0,0,0}, {0,0,0}, {0,0,0} } };
401 static filter_ctx ctx_e1 = { { {0,0,0}, {0,0,0}, {0,0,0} } };
402 static filter_ctx ctx_e2 = { { {0,0,0}, {0,0,0}, {0,0,0} } };
403
372 BOOT_PRINTF("in AVGV ***\n");
404 BOOT_PRINTF("in AVGV ***\n");
373
405
374 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &AVGV_id) != RTEMS_SUCCESSFUL) {
406 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &AVGV_id) != RTEMS_SUCCESSFUL) {
@@ -416,29 +448,9 rtems_task avgv_task(rtems_task_argument
416 || (current_e1 != old_e1)
448 || (current_e1 != old_e1)
417 || (current_e2 != old_e2))
449 || (current_e2 != old_e2))
418 {
450 {
419 // get new values
451 average_v = filter( current_v, &ctx_v );
420 newValue_v = current_v;
452 average_e1 = filter( current_e1, &ctx_e1 );
421 newValue_e1 = current_e1;
453 average_e2 = filter( current_e2, &ctx_e2 );
422 newValue_e2 = current_e2;
423
424 // compute the moving average
425 average_v = average_v + newValue_v - v[k];
426 average_e1 = average_e1 + newValue_e1 - e1[k];
427 average_e2 = average_e2 + newValue_e2 - e2[k];
428
429 // store new values in buffers
430 v[k] = newValue_v;
431 e1[k] = newValue_e1;
432 e2[k] = newValue_e2;
433
434 if (k == (MOVING_AVERAGE-1))
435 {
436 k = 0;
437 }
438 else
439 {
440 k++;
441 }
442
454
443 //update int16 values
455 //update int16 values
444 hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / MOVING_AVERAGE );
456 hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / MOVING_AVERAGE );
General Comments 0
You need to be logged in to leave comments. Login now