@@ -1,2 +1,2 | |||||
1 | 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters |
|
1 | 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters | |
2 |
|
|
2 | 058c1234c2defe215d3dd655a7ef65abb33c922d header/lfr_common_headers |
@@ -21,16 +21,11 extern rtems_id Task_id[]; /* | |||||
21 | extern rtems_name timecode_timer_name; |
|
21 | extern rtems_name timecode_timer_name; | |
22 | extern rtems_id timecode_timer_id; |
|
22 | extern rtems_id timecode_timer_id; | |
23 | extern unsigned char pa_bia_status_info; |
|
23 | extern unsigned char pa_bia_status_info; | |
24 | extern unsigned char cp_rpw_sc_rw_f_flags; |
|
24 | extern unsigned char cp_rpw_sc_rw1_rw2_f_flags; | |
25 |
extern |
|
25 | extern unsigned char cp_rpw_sc_rw3_rw4_f_flags; | |
26 | extern float cp_rpw_sc_rw1_f2; |
|
26 | ||
27 | extern float cp_rpw_sc_rw2_f1; |
|
|||
28 | extern float cp_rpw_sc_rw2_f2; |
|
|||
29 | extern float cp_rpw_sc_rw3_f1; |
|
|||
30 | extern float cp_rpw_sc_rw3_f2; |
|
|||
31 | extern float cp_rpw_sc_rw4_f1; |
|
|||
32 | extern float cp_rpw_sc_rw4_f2; |
|
|||
33 | extern filterPar_t filterPar; |
|
27 | extern filterPar_t filterPar; | |
|
28 | extern rw_f_t rw_f; | |||
34 |
|
29 | |||
35 | // RTEMS TASKS |
|
30 | // RTEMS TASKS | |
36 | rtems_task Init( rtems_task_argument argument); |
|
31 | rtems_task Init( rtems_task_argument argument); |
@@ -29,6 +29,20 | |||||
29 | #define BIT_RW4_F1 0x02 |
|
29 | #define BIT_RW4_F1 0x02 | |
30 | #define BIT_RW4_F2 0x01 |
|
30 | #define BIT_RW4_F2 0x01 | |
31 |
|
31 | |||
|
32 | #define WHEEL_1 1 | |||
|
33 | #define WHEEL_2 2 | |||
|
34 | #define WHEEL_3 3 | |||
|
35 | #define WHEEL_4 4 | |||
|
36 | #define FREQ_1 1 | |||
|
37 | #define FREQ_2 2 | |||
|
38 | #define FREQ_3 3 | |||
|
39 | #define FREQ_4 4 | |||
|
40 | #define FLAG_OFFSET_WHEELS_1_3 8 | |||
|
41 | #define FLAG_OFFSET_WHEELS_2_4 4 | |||
|
42 | ||||
|
43 | #define FLAG_NAN 0 // Not A NUMBER | |||
|
44 | #define FLAG_IAN 1 // Is A Number | |||
|
45 | ||||
32 | #define SBM_KCOEFF_PER_NORM_KCOEFF 2 |
|
46 | #define SBM_KCOEFF_PER_NORM_KCOEFF 2 | |
33 |
|
47 | |||
34 | extern unsigned short sequenceCounterParameterDump; |
|
48 | extern unsigned short sequenceCounterParameterDump; | |
@@ -76,6 +90,8 int set_sy_lfr_s2_bp_p1( ccsdsTelecomman | |||||
76 | unsigned int check_update_info_hk_lfr_mode( unsigned char mode ); |
|
90 | unsigned int check_update_info_hk_lfr_mode( unsigned char mode ); | |
77 | unsigned int check_update_info_hk_tds_mode( unsigned char mode ); |
|
91 | unsigned int check_update_info_hk_tds_mode( unsigned char mode ); | |
78 | unsigned int check_update_info_hk_thr_mode( unsigned char mode ); |
|
92 | unsigned int check_update_info_hk_thr_mode( unsigned char mode ); | |
|
93 | void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ); | |||
|
94 | void set_hk_lfr_sc_rw_f_flags( void ); | |||
79 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ); |
|
95 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ); | |
80 | void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ); |
|
96 | void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ); | |
81 | void build_sy_lfr_rw_mask( unsigned int channel ); |
|
97 | void build_sy_lfr_rw_mask( unsigned int channel ); |
@@ -73,7 +73,8 unsigned int lastValidEnterModeTime | |||||
73 |
|
73 | |||
74 | // HK PACKETS |
|
74 | // HK PACKETS | |
75 | Packet_TM_LFR_HK_t housekeeping_packet = {0}; |
|
75 | Packet_TM_LFR_HK_t housekeeping_packet = {0}; | |
76 |
unsigned char cp_rpw_sc_rw_f_flags |
|
76 | unsigned char cp_rpw_sc_rw1_rw2_f_flags = 0; | |
|
77 | unsigned char cp_rpw_sc_rw3_rw4_f_flags = 0; | |||
77 | // message queues occupancy |
|
78 | // message queues occupancy | |
78 | unsigned char hk_lfr_q_sd_fifo_size_max = 0; |
|
79 | unsigned char hk_lfr_q_sd_fifo_size_max = 0; | |
79 | unsigned char hk_lfr_q_rv_fifo_size_max = 0; |
|
80 | unsigned char hk_lfr_q_rv_fifo_size_max = 0; | |
@@ -89,14 +90,7 unsigned short sequenceCounterHK | |||||
89 | spw_stats grspw_stats = {0}; |
|
90 | spw_stats grspw_stats = {0}; | |
90 |
|
91 | |||
91 | // TC_LFR_UPDATE_INFO |
|
92 | // TC_LFR_UPDATE_INFO | |
92 | float cp_rpw_sc_rw1_f1 = INIT_FLOAT; |
|
93 | rw_f_t rw_f; | |
93 | float cp_rpw_sc_rw1_f2 = INIT_FLOAT; |
|
|||
94 | float cp_rpw_sc_rw2_f1 = INIT_FLOAT; |
|
|||
95 | float cp_rpw_sc_rw2_f2 = INIT_FLOAT; |
|
|||
96 | float cp_rpw_sc_rw3_f1 = INIT_FLOAT; |
|
|||
97 | float cp_rpw_sc_rw3_f2 = INIT_FLOAT; |
|
|||
98 | float cp_rpw_sc_rw4_f1 = INIT_FLOAT; |
|
|||
99 | float cp_rpw_sc_rw4_f2 = INIT_FLOAT; |
|
|||
100 |
|
94 | |||
101 | // TC_LFR_LOAD_FILTER_PAR |
|
95 | // TC_LFR_LOAD_FILTER_PAR | |
102 | filterPar_t filterPar = {0}; |
|
96 | filterPar_t filterPar = {0}; |
@@ -168,14 +168,28 rtems_task Init( rtems_task_argument ign | |||||
168 | init_k_coefficients_prc2(); |
|
168 | init_k_coefficients_prc2(); | |
169 | pa_bia_status_info = INIT_CHAR; |
|
169 | pa_bia_status_info = INIT_CHAR; | |
170 | cp_rpw_sc_rw_f_flags = INIT_CHAR; |
|
170 | cp_rpw_sc_rw_f_flags = INIT_CHAR; | |
171 | cp_rpw_sc_rw1_f1 = INIT_FLOAT; |
|
171 | ||
172 | cp_rpw_sc_rw1_f2 = INIT_FLOAT; |
|
172 | // initialize all reaction wheels frequencies to NaN | |
173 |
cp_rpw_sc_rw |
|
173 | rw_f.cp_rpw_sc_rw1_f1 = NAN; | |
174 |
cp_rpw_sc_rw |
|
174 | rw_f.cp_rpw_sc_rw1_f2 = NAN; | |
175 |
cp_rpw_sc_rw |
|
175 | rw_f.cp_rpw_sc_rw1_f3 = NAN; | |
176 |
cp_rpw_sc_rw |
|
176 | rw_f.cp_rpw_sc_rw1_f4 = NAN; | |
177 |
cp_rpw_sc_rw |
|
177 | rw_f.cp_rpw_sc_rw2_f1 = NAN; | |
178 |
cp_rpw_sc_rw |
|
178 | rw_f.cp_rpw_sc_rw2_f2 = NAN; | |
|
179 | rw_f.cp_rpw_sc_rw2_f3 = NAN; | |||
|
180 | rw_f.cp_rpw_sc_rw2_f4 = NAN; | |||
|
181 | rw_f.cp_rpw_sc_rw3_f1 = NAN; | |||
|
182 | rw_f.cp_rpw_sc_rw3_f2 = NAN; | |||
|
183 | rw_f.cp_rpw_sc_rw3_f3 = NAN; | |||
|
184 | rw_f.cp_rpw_sc_rw3_f4 = NAN; | |||
|
185 | rw_f.cp_rpw_sc_rw4_f1 = NAN; | |||
|
186 | rw_f.cp_rpw_sc_rw4_f2 = NAN; | |||
|
187 | rw_f.cp_rpw_sc_rw4_f3 = NAN; | |||
|
188 | rw_f.cp_rpw_sc_rw4_f4 = NAN; | |||
|
189 | ||||
|
190 | cp_rpw_sc_rw1_rw2_f_flags = INIT_CHAR; | |||
|
191 | cp_rpw_sc_rw3_rw4_f_flags = INIT_CHAR; | |||
|
192 | ||||
179 | // initialize filtering parameters |
|
193 | // initialize filtering parameters | |
180 | filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED; |
|
194 | filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED; | |
181 | filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS; |
|
195 | filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS; |
@@ -7,6 +7,10 | |||||
7 |
|
7 | |||
8 | #include "fsw_misc.h" |
|
8 | #include "fsw_misc.h" | |
9 |
|
9 | |||
|
10 | int16_t hk_lfr_sc_v_f3_as_int16 = 0; | |||
|
11 | int16_t hk_lfr_sc_e1_f3_as_int16 = 0; | |||
|
12 | int16_t hk_lfr_sc_e2_f3_as_int16 = 0; | |||
|
13 | ||||
10 | void timer_configure(unsigned char timer, unsigned int clock_divider, |
|
14 | void timer_configure(unsigned char timer, unsigned int clock_divider, | |
11 | unsigned char interrupt_level, rtems_isr (*timer_isr)() ) |
|
15 | unsigned char interrupt_level, rtems_isr (*timer_isr)() ) | |
12 | { |
|
16 | { | |
@@ -327,7 +331,8 rtems_task hous_task(rtems_task_argument | |||||
327 |
|
331 | |||
328 | hk_lfr_le_me_he_update(); |
|
332 | hk_lfr_le_me_he_update(); | |
329 |
|
333 | |||
330 | housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags; |
|
334 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = cp_rpw_sc_rw1_rw2_f_flags; | |
|
335 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = cp_rpw_sc_rw3_rw4_f_flags; | |||
331 |
|
336 | |||
332 | // SEND PACKET |
|
337 | // SEND PACKET | |
333 | status = rtems_message_queue_send( queue_id, &housekeeping_packet, |
|
338 | status = rtems_message_queue_send( queue_id, &housekeeping_packet, | |
@@ -355,6 +360,9 rtems_task avgv_task(rtems_task_argument | |||||
355 | float average_v; |
|
360 | float average_v; | |
356 | float average_e1; |
|
361 | float average_e1; | |
357 | float average_e2; |
|
362 | float average_e2; | |
|
363 | float newValue_v; | |||
|
364 | float newValue_e1; | |||
|
365 | float newValue_e2; | |||
358 | unsigned char k; |
|
366 | unsigned char k; | |
359 | unsigned char indexOfOldValue; |
|
367 | unsigned char indexOfOldValue; | |
360 |
|
368 | |||
@@ -377,33 +385,38 rtems_task avgv_task(rtems_task_argument | |||||
377 |
|
385 | |||
378 | // initialize values |
|
386 | // initialize values | |
379 | indexOfOldValue = MOVING_AVERAGE - 1; |
|
387 | indexOfOldValue = MOVING_AVERAGE - 1; | |
380 |
average_v = |
|
388 | average_v = INIT_FLOAT; | |
381 |
average_e1 = |
|
389 | average_e1 = INIT_FLOAT; | |
382 |
average_e2 = |
|
390 | average_e2 = INIT_FLOAT; | |
|
391 | newValue_v = INIT_FLOAT; | |||
|
392 | newValue_e1 = INIT_FLOAT; | |||
|
393 | newValue_e2 = INIT_FLOAT; | |||
383 |
|
394 | |||
384 |
k = |
|
395 | k = INIT_CHAR; | |
385 |
|
396 | |||
386 | while(1){ // launch the rate monotonic task |
|
397 | while(1) | |
|
398 | { // launch the rate monotonic task | |||
387 | status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD ); |
|
399 | status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD ); | |
388 |
if ( status != RTEMS_SUCCESSFUL ) |
|
400 | if ( status != RTEMS_SUCCESSFUL ) | |
|
401 | { | |||
389 | PRINTF1( "in AVGV *** ERR period: %d\n", status); |
|
402 | PRINTF1( "in AVGV *** ERR period: %d\n", status); | |
390 | } |
|
403 | } | |
391 |
else |
|
404 | else | |
392 | // store new value in buffer |
|
405 | { | |
393 | v[k] = waveform_picker_regs->v; |
|
406 | // get new values | |
394 |
|
|
407 | newValue_v = waveform_picker_regs->v; | |
395 |
|
|
408 | newValue_e1 = waveform_picker_regs->e1; | |
396 | if (k == (MOVING_AVERAGE - 1)) |
|
409 | newValue_e2 = waveform_picker_regs->e2; | |
397 | { |
|
410 | ||
398 | indexOfOldValue = 0; |
|
411 | // compute the moving average | |
399 | } |
|
412 | average_v = average_v + newValue_v - v[k]; | |
400 | else |
|
413 | average_e1 = average_e1 + newValue_e1 - e1[k]; | |
401 | { |
|
414 | average_e2 = average_e2 + newValue_e2 - e2[k]; | |
402 | indexOfOldValue = k + 1; |
|
415 | ||
403 | } |
|
416 | // store new values in buffers | |
404 | average_v = average_v + v[k] - v[indexOfOldValue]; |
|
417 | v[k] = newValue_v; | |
405 | average_e1 = average_e1 + e1[k] - e1[indexOfOldValue]; |
|
418 | e1[k] = newValue_e1; | |
406 | average_e2 = average_e2 + e2[k] - e2[indexOfOldValue]; |
|
419 | e2[k] = newValue_e2; | |
407 | } |
|
420 | } | |
408 | if (k == (MOVING_AVERAGE-1)) |
|
421 | if (k == (MOVING_AVERAGE-1)) | |
409 | { |
|
422 | { | |
@@ -414,11 +427,15 rtems_task avgv_task(rtems_task_argument | |||||
414 | { |
|
427 | { | |
415 | k++; |
|
428 | k++; | |
416 | } |
|
429 | } | |
|
430 | //update int16 values | |||
|
431 | hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / ((float) MOVING_AVERAGE) ); | |||
|
432 | hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / ((float) MOVING_AVERAGE) ); | |||
|
433 | hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / ((float) MOVING_AVERAGE) ); | |||
417 | } |
|
434 | } | |
418 |
|
435 | |||
419 | PRINTF("in AVGV *** deleting task\n") |
|
436 | PRINTF("in AVGV *** deleting task\n"); | |
420 |
|
437 | |||
421 |
|
|
438 | status = rtems_task_delete( RTEMS_SELF ); // should not return | |
422 |
|
439 | |||
423 | return; |
|
440 | return; | |
424 | } |
|
441 | } | |
@@ -672,16 +689,16 void get_v_e1_e2_f3( unsigned char *spac | |||||
672 | unsigned char* e1_ptr; |
|
689 | unsigned char* e1_ptr; | |
673 | unsigned char* e2_ptr; |
|
690 | unsigned char* e2_ptr; | |
674 |
|
691 | |||
675 |
v_ptr = (unsigned char *) & |
|
692 | v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16; | |
676 |
e1_ptr = (unsigned char *) & |
|
693 | e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16; | |
677 |
e2_ptr = (unsigned char *) & |
|
694 | e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16; | |
678 |
|
695 | |||
679 |
spacecraft_potential[ |
|
696 | spacecraft_potential[BYTE_0] = v_ptr[0]; | |
680 |
spacecraft_potential[ |
|
697 | spacecraft_potential[BYTE_1] = v_ptr[1]; | |
681 |
spacecraft_potential[ |
|
698 | spacecraft_potential[BYTE_2] = e1_ptr[0]; | |
682 |
spacecraft_potential[ |
|
699 | spacecraft_potential[BYTE_3] = e1_ptr[1]; | |
683 |
spacecraft_potential[ |
|
700 | spacecraft_potential[BYTE_4] = e2_ptr[0]; | |
684 |
spacecraft_potential[ |
|
701 | spacecraft_potential[BYTE_5] = e2_ptr[1]; | |
685 | } |
|
702 | } | |
686 |
|
703 | |||
687 | void get_cpu_load( unsigned char *resource_statistics ) |
|
704 | void get_cpu_load( unsigned char *resource_statistics ) |
@@ -301,8 +301,9 int action_update_info(ccsdsTelecommandP | |||||
301 |
|
301 | |||
302 | // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets) |
|
302 | // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets) | |
303 |
|
303 | |||
304 | cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; |
|
304 | //cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; | |
305 | getReactionWheelsFrequencies( TC ); |
|
305 | getReactionWheelsFrequencies( TC ); | |
|
306 | set_hk_lfr_sc_rw_f_flags(); | |||
306 | build_sy_lfr_rw_masks(); |
|
307 | build_sy_lfr_rw_masks(); | |
307 |
|
308 | |||
308 | // once the masks are built, they have to be merged with the fbins_mask |
|
309 | // once the masks are built, they have to be merged with the fbins_mask |
@@ -324,8 +324,10 int action_load_filter_par(ccsdsTelecomm | |||||
324 | */ |
|
324 | */ | |
325 |
|
325 | |||
326 | int flag; |
|
326 | int flag; | |
|
327 | unsigned char k; | |||
327 |
|
328 | |||
328 | flag = LFR_DEFAULT; |
|
329 | flag = LFR_DEFAULT; | |
|
330 | k = INIT_CHAR; | |||
329 |
|
331 | |||
330 | flag = check_sy_lfr_filter_parameters( TC, queue_id ); |
|
332 | flag = check_sy_lfr_filter_parameters( TC, queue_id ); | |
331 |
|
333 | |||
@@ -367,6 +369,36 int action_load_filter_par(ccsdsTelecomm | |||||
367 | // store the parameter sy_lfr_sc_rw_delta_f as a float |
|
369 | // store the parameter sy_lfr_sc_rw_delta_f as a float | |
368 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f, |
|
370 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f, | |
369 | parameter_dump_packet.sy_lfr_sc_rw_delta_f ); |
|
371 | parameter_dump_packet.sy_lfr_sc_rw_delta_f ); | |
|
372 | ||||
|
373 | // copy rw.._k.. from the incoming TC to the local parameter_dump_packet | |||
|
374 | for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++) | |||
|
375 | { | |||
|
376 | parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ]; | |||
|
377 | } | |||
|
378 | ||||
|
379 | //*********************************************** | |||
|
380 | // store the parameter sy_lfr_rw.._k.. as a float | |||
|
381 | // rw1_k | |||
|
382 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 ); | |||
|
383 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 ); | |||
|
384 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 ); | |||
|
385 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 ); | |||
|
386 | // rw2_k | |||
|
387 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 ); | |||
|
388 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 ); | |||
|
389 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 ); | |||
|
390 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 ); | |||
|
391 | // rw3_k | |||
|
392 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 ); | |||
|
393 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 ); | |||
|
394 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 ); | |||
|
395 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 ); | |||
|
396 | // rw4_k | |||
|
397 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 ); | |||
|
398 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 ); | |||
|
399 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 ); | |||
|
400 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 ); | |||
|
401 | ||||
370 | } |
|
402 | } | |
371 |
|
403 | |||
372 | return flag; |
|
404 | return flag; | |
@@ -933,6 +965,81 unsigned int check_update_info_hk_thr_mo | |||||
933 | return status; |
|
965 | return status; | |
934 | } |
|
966 | } | |
935 |
|
967 | |||
|
968 | void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ) | |||
|
969 | { | |||
|
970 | unsigned char flag; | |||
|
971 | unsigned char flagPosInByte; | |||
|
972 | unsigned char newFlag; | |||
|
973 | unsigned char flagMask; | |||
|
974 | ||||
|
975 | // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered | |||
|
976 | if (isnan(value)) | |||
|
977 | { | |||
|
978 | flag = FLAG_NAN; | |||
|
979 | } | |||
|
980 | else | |||
|
981 | { | |||
|
982 | flag = FLAG_IAN; | |||
|
983 | } | |||
|
984 | ||||
|
985 | switch(wheel) | |||
|
986 | { | |||
|
987 | case WHEEL_1: | |||
|
988 | flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq; | |||
|
989 | flagMask = ~(1 << flagPosInByte); | |||
|
990 | newFlag = flag << flagPosInByte; | |||
|
991 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; | |||
|
992 | break; | |||
|
993 | case WHEEL_2: | |||
|
994 | flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq; | |||
|
995 | flagMask = ~(1 << flagPosInByte); | |||
|
996 | newFlag = flag << flagPosInByte; | |||
|
997 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; | |||
|
998 | break; | |||
|
999 | case WHEEL_3: | |||
|
1000 | flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq; | |||
|
1001 | flagMask = ~(1 << flagPosInByte); | |||
|
1002 | newFlag = flag << flagPosInByte; | |||
|
1003 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; | |||
|
1004 | break; | |||
|
1005 | case WHEEL_4: | |||
|
1006 | flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq; | |||
|
1007 | flagMask = ~(1 << flagPosInByte); | |||
|
1008 | newFlag = flag << flagPosInByte; | |||
|
1009 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; | |||
|
1010 | break; | |||
|
1011 | default: | |||
|
1012 | break; | |||
|
1013 | } | |||
|
1014 | } | |||
|
1015 | ||||
|
1016 | void set_hk_lfr_sc_rw_f_flags( void ) | |||
|
1017 | { | |||
|
1018 | // RW1 | |||
|
1019 | set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 ); | |||
|
1020 | set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 ); | |||
|
1021 | set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 ); | |||
|
1022 | set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 ); | |||
|
1023 | ||||
|
1024 | // RW2 | |||
|
1025 | set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 ); | |||
|
1026 | set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 ); | |||
|
1027 | set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 ); | |||
|
1028 | set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 ); | |||
|
1029 | ||||
|
1030 | // RW3 | |||
|
1031 | set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 ); | |||
|
1032 | set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 ); | |||
|
1033 | set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 ); | |||
|
1034 | set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 ); | |||
|
1035 | ||||
|
1036 | // RW4 | |||
|
1037 | set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 ); | |||
|
1038 | set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 ); | |||
|
1039 | set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 ); | |||
|
1040 | set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 ); | |||
|
1041 | } | |||
|
1042 | ||||
936 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ) |
|
1043 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ) | |
937 | { |
|
1044 | { | |
938 | /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally. |
|
1045 | /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally. | |
@@ -945,37 +1052,32 void getReactionWheelsFrequencies( ccsds | |||||
945 |
|
1052 | |||
946 | bytePosPtr = (unsigned char *) &TC->packetID; |
|
1053 | bytePosPtr = (unsigned char *) &TC->packetID; | |
947 |
|
1054 | |||
948 |
// |
|
1055 | // rw1_f | |
949 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1, |
|
1056 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] ); | |
950 |
|
|
1057 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); | |
951 |
|
1058 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] ); | ||
952 | // cp_rpw_sc_rw1_f2 |
|
1059 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] ); | |
953 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2, |
|
|||
954 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); |
|
|||
955 |
|
1060 | |||
956 |
// |
|
1061 | // rw2_f | |
957 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1, |
|
1062 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] ); | |
958 |
|
|
1063 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); | |
959 |
|
1064 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] ); | ||
960 | // cp_rpw_sc_rw2_f2 |
|
1065 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] ); | |
961 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2, |
|
|||
962 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); |
|
|||
963 |
|
1066 | |||
964 |
// |
|
1067 | // rw3_f | |
965 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1, |
|
1068 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] ); | |
966 |
|
|
1069 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); | |
967 |
|
1070 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] ); | ||
968 | // cp_rpw_sc_rw3_f2 |
|
1071 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] ); | |
969 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2, |
|
|||
970 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); |
|
|||
971 |
|
1072 | |||
972 |
// |
|
1073 | // rw4_f | |
973 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1, |
|
1074 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] ); | |
974 |
|
|
1075 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); | |
|
1076 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] ); | |||
|
1077 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] ); | |||
975 |
|
1078 | |||
976 | // cp_rpw_sc_rw4_f2 |
|
1079 | // test each reaction wheel frequency value. NaN means that the frequency is not filtered | |
977 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2, |
|
1080 | ||
978 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); |
|
|||
979 | } |
|
1081 | } | |
980 |
|
1082 | |||
981 | void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ) |
|
1083 | void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ) | |
@@ -1116,29 +1218,29 void build_sy_lfr_rw_mask( unsigned int | |||||
1116 | local_rw_fbins_mask[k] = INT8_ALL_F; |
|
1218 | local_rw_fbins_mask[k] = INT8_ALL_F; | |
1117 | } |
|
1219 | } | |
1118 |
|
1220 | |||
1119 |
// RW1 |
|
1221 | // RW1 | |
1120 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
1222 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_7) >> SHIFT_7_BITS ); // [1000 0000] | |
1121 |
|
1223 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_6) >> SHIFT_6_BITS ); // [0100 0000] | ||
1122 | // RW1 F2 |
|
1224 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_5) >> SHIFT_5_BITS ); // [0010 0000] | |
1123 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
1225 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_4) >> SHIFT_4_BITS ); // [0001 0000] | |
1124 |
|
1226 | |||
1125 |
// RW2 |
|
1227 | // RW2 | |
1126 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
1228 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_3) >> SHIFT_3_BITS ); // [0000 1000] | |
1127 |
|
1229 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_2) >> SHIFT_2_BITS ); // [0000 0100] | ||
1128 | // RW2 F2 |
|
1230 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_1) >> SHIFT_1_BITS ); // [0000 0010] | |
1129 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
1231 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_0) ); // [0000 0001] | |
1130 |
|
1232 | |||
1131 |
// RW3 |
|
1233 | // RW3 | |
1132 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
1234 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_7) >> SHIFT_7_BITS ); // [1000 0000] | |
1133 |
|
1235 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_6) >> SHIFT_6_BITS ); // [0100 0000] | ||
1134 | // RW3 F2 |
|
1236 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_5) >> SHIFT_5_BITS ); // [0010 0000] | |
1135 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
1237 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_4) >> SHIFT_4_BITS ); // [0001 0000] | |
1136 |
|
1238 | |||
1137 |
// RW4 |
|
1239 | // RW4 | |
1138 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
1240 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_3) >> SHIFT_3_BITS ); // [0000 1000] | |
1139 |
|
1241 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_2) >> SHIFT_2_BITS ); // [0000 0100] | ||
1140 | // RW4 F2 |
|
1242 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_1) >> SHIFT_1_BITS ); // [0000 0010] | |
1141 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
1243 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_0) ); // [0000 0001] | |
1142 |
|
1244 | |||
1143 | // update the value of the fbins related to reaction wheels frequency filtering |
|
1245 | // update the value of the fbins related to reaction wheels frequency filtering | |
1144 | if (maskPtr != NULL) |
|
1246 | if (maskPtr != NULL) | |
@@ -1502,6 +1604,27 void init_parameter_dump( void ) | |||||
1502 | floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift ); |
|
1604 | floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift ); | |
1503 | floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f ); |
|
1605 | floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f ); | |
1504 |
|
1606 | |||
|
1607 | // RW1_K | |||
|
1608 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1); | |||
|
1609 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2); | |||
|
1610 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3); | |||
|
1611 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4); | |||
|
1612 | // RW2_K | |||
|
1613 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1); | |||
|
1614 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2); | |||
|
1615 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3); | |||
|
1616 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4); | |||
|
1617 | // RW3_K | |||
|
1618 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1); | |||
|
1619 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2); | |||
|
1620 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3); | |||
|
1621 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4); | |||
|
1622 | // RW4_K | |||
|
1623 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1); | |||
|
1624 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2); | |||
|
1625 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3); | |||
|
1626 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4); | |||
|
1627 | ||||
1505 | // LFR_RW_MASK |
|
1628 | // LFR_RW_MASK | |
1506 | for (k=0; k < BYTES_PER_MASKS_SET; k++) |
|
1629 | for (k=0; k < BYTES_PER_MASKS_SET; k++) | |
1507 | { |
|
1630 | { |
General Comments 0
You need to be logged in to leave comments.
Login now