| @@ -66,6 +66,40 typedef struct{ | |||
|
|
66 | 66 | unsigned char dpu_spw_rx_too_big; |
|
|
67 | 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 | 103 | extern gptimer_regs_t *gptimer_regs; |
|
|
70 | 104 | extern void ASR16_get_FPRF_IURF_ErrorCounters( unsigned int*, unsigned int* ); |
|
|
71 | 105 | extern void CCR_getInstructionAndDataErrorCounters( unsigned int*, unsigned int* ); |
| @@ -64,7 +64,7 option(FSW_debug_tch "?" OFF) | |||
|
|
64 | 64 | set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE) |
|
|
65 | 65 | set(SW_VERSION_N2 "2" CACHE STRING "Choose N2 FSW Version." FORCE) |
|
|
66 | 66 | set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE) |
|
|
67 |
set(SW_VERSION_N4 " |
|
|
|
67 | set(SW_VERSION_N4 "10" CACHE STRING "Choose N4 FSW Version." FORCE) | |
|
|
68 | 68 | |
|
|
69 | 69 | if(FSW_verbose) |
|
|
70 | 70 | add_definitions(-DPRINT_MESSAGES_ON_CONSOLE) |
| @@ -347,6 +347,34 rtems_task hous_task(rtems_task_argument | |||
|
|
347 | 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 | 378 | rtems_task avgv_task(rtems_task_argument argument) |
|
|
351 | 379 | { |
|
|
352 | 380 | #define MOVING_AVERAGE 16 |
| @@ -369,6 +397,10 rtems_task avgv_task(rtems_task_argument | |||
|
|
369 | 397 | unsigned char k; |
|
|
370 | 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 | 404 | BOOT_PRINTF("in AVGV ***\n"); |
|
|
373 | 405 | |
|
|
374 | 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 | 448 | || (current_e1 != old_e1) |
|
|
417 | 449 | || (current_e2 != old_e2)) |
|
|
418 | 450 | { |
|
|
419 | // get new values | |
|
|
420 |
|
|
|
|
421 |
|
|
|
|
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 | } | |
|
|
451 | average_v = filter( current_v, &ctx_v ); | |
|
|
452 | average_e1 = filter( current_e1, &ctx_e1 ); | |
|
|
453 | average_e2 = filter( current_e2, &ctx_e2 ); | |
|
|
442 | 454 | |
|
|
443 | 455 | //update int16 values |
|
|
444 | 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