@@ -1,2 +1,2 | |||
|
1 | 1 | 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters |
|
2 | 94f0f2fccbcb8030d9437ffbb69ee0eefaaea188 header/lfr_common_headers | |
|
2 | 5cf0bac6095ff491d937622f8253c6f05095cb39 header/lfr_common_headers |
@@ -1,4 +1,15 | |||
|
1 | 1 | TEMPLATE = app |
|
2 | CONFIG += console | |
|
3 | CONFIG -= app_bundle | |
|
4 | CONFIG -= qt | |
|
5 | ||
|
6 | include(sparc.pri) | |
|
7 | ||
|
8 | INCLUDEPATH += ./ | |
|
9 | ||
|
10 | OBJECTS_DIR=obj | |
|
11 | DESTDIR=bin | |
|
12 | ||
|
2 | 13 | # CONFIG += console v8 sim |
|
3 | 14 | # CONFIG options = |
|
4 | 15 | # verbose |
@@ -11,9 +22,6 TEMPLATE = app | |||
|
11 | 22 | # lpp_dpu_destid /!\ REMOVE BEFORE DELIVERY TO LESIA /!\ |
|
12 | 23 | # debug_watchdog |
|
13 | 24 | CONFIG += console verbose lpp_dpu_destid cpu_usage_report |
|
14 | CONFIG -= qt | |
|
15 | ||
|
16 | include(./sparc.pri) | |
|
17 | 25 | |
|
18 | 26 | INCLUDEPATH += /opt/rtems-4.10/sparc-rtems/leon3/lib/include |
|
19 | 27 |
@@ -1,5 +1,4 | |||
|
1 | CONFIG += console | |
|
2 | CONFIG -= qt | |
|
1 | ||
|
3 | 2 | QMAKE_CC=sparc-rtems-gcc |
|
4 | 3 | message(C compiler forced to: $$QMAKE_CC) |
|
5 | 4 | QMAKE_CXX=sparc-rtems-g++ |
@@ -13,14 +12,14 QMAKE_OBJCOPY= sparc-rtems-objcopy | |||
|
13 | 12 | QMAKE_STRIP=sparc-rtems-strip |
|
14 | 13 | QMAKE_GDB=sparc-rtems-gdb |
|
15 | 14 | |
|
16 | #INCLUDEPATH += /opt/rtems-4.10 | |
|
17 | INCLUDEPATH += /opt/rtems-4.10/sparc-rtems/leon3/lib/include | |
|
18 | ||
|
15 | QMAKE_CFLAGS_APP="" | |
|
16 | QMAKE_CXXFLAGS_APP="" | |
|
17 | QMAKE_CFLAGS= | |
|
19 | 18 | QMAKE_CFLAGS_DEBUG= -g |
|
20 | 19 | QMAKE_CFLAGS_RELEASE="" |
|
21 | 20 | QMAKE_CXXFLAGS_DEBUG= -g |
|
22 | 21 | QMAKE_CXXFLAGS_RELEASE="" |
|
23 |
QMAKE_LFLAGS_RELEASE= |
|
|
22 | QMAKE_LFLAGS_RELEASE= | |
|
24 | 23 | QMAKE_LFLAGS_DEBUG= -g |
|
25 | 24 | QMAKE_CXXFLAGS_DEPS = |
|
26 | 25 | QMAKE_CXXFLAGS_WARN_ON = -Wall |
@@ -39,7 +38,6 QMAKE_CFLAGS_RELEASE = | |||
|
39 | 38 | QMAKE_CFLAGS_YACC = |
|
40 | 39 | QMAKE_LFLAGS_EXCEPTIONS_ON = |
|
41 | 40 | QMAKE_LFLAGS_EXCEPTIONS_OFF = |
|
42 | QMAKE_LFLAGS_RELEASE = -Xlinker -Map=output.map | |
|
43 | 41 | QMAKE_LFLAGS_CONSOLE = |
|
44 | 42 | QMAKE_LFLAGS_WINDOWS = |
|
45 | 43 | QMAKE_LFLAGS_DLL = |
@@ -53,47 +51,19 QMAKE_LIBS="" | |||
|
53 | 51 | INCLUDEPATH="" |
|
54 | 52 | DEFINES="" |
|
55 | 53 | |
|
56 | contains( TEMPLATE, app ) { | |
|
57 | OBJECTS_DIR=obj | |
|
58 | DESTDIR=bin | |
|
59 | } | |
|
60 | ||
|
61 | #QMAKE_CFLAGS_RELEASE += -O0 | |
|
62 | #QMAKE_CFLAGS_DEBUG += -O0 | |
|
63 | #QMAKE_CXXFLAGS_RELEASE += -O0 | |
|
64 | #QMAKE_CXXFLAGS_DEBUG += -O0 | |
|
65 | 54 | |
|
66 | 55 | QMAKE_CFLAGS_RELEASE += -O3 |
|
67 | 56 | QMAKE_CFLAGS_DEBUG += -O3 |
|
68 | 57 | QMAKE_CXXFLAGS_RELEASE += -O3 |
|
69 | 58 | QMAKE_CXXFLAGS_DEBUG += -O3 |
|
70 | 59 | |
|
60 | #QMAKE_CFLAGS_RELEASE += -O0 | |
|
61 | #QMAKE_CFLAGS_DEBUG += -O0 | |
|
62 | #QMAKE_CXXFLAGS_RELEASE += -O0 | |
|
63 | #QMAKE_CXXFLAGS_DEBUG += -O0 | |
|
64 | ||
|
65 | ||
|
71 | 66 |
#QMAKE_CFLAGS_RELEASE |
|
72 | 67 |
#QMAKE_CFLAGS_DEBUG |
|
73 | 68 |
#QMAKE_CXXFLAGS_RELEASE |
|
74 | 69 |
#QMAKE_CXXFLAGS_DEBUG |
|
75 | ||
|
76 | contains( TEMPLATE, app ) { | |
|
77 | grmon.target = grmon | |
|
78 | grmon.commands = cd $$DESTDIR && C:/opt/grmon-eval-2.0.29b/win32/bin/grmon.exe -uart COM4 -u | |
|
79 | QMAKE_EXTRA_TARGETS += grmon | |
|
80 | } | |
|
81 | ||
|
82 | ||
|
83 | ||
|
84 | ||
|
85 | ||
|
86 | ||
|
87 | ||
|
88 | ||
|
89 | ||
|
90 | ||
|
91 | ||
|
92 | ||
|
93 | ||
|
94 | ||
|
95 | ||
|
96 | ||
|
97 | ||
|
98 | ||
|
99 |
@@ -21,16 +21,11 extern rtems_id Task_id[]; /* | |||
|
21 | 21 | extern rtems_name timecode_timer_name; |
|
22 | 22 | extern rtems_id timecode_timer_id; |
|
23 | 23 | extern unsigned char pa_bia_status_info; |
|
24 | extern unsigned char cp_rpw_sc_rw_f_flags; | |
|
25 |
extern |
|
|
26 | extern float cp_rpw_sc_rw1_f2; | |
|
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; | |
|
24 | extern unsigned char cp_rpw_sc_rw1_rw2_f_flags; | |
|
25 | extern unsigned char cp_rpw_sc_rw3_rw4_f_flags; | |
|
26 | ||
|
33 | 27 | extern filterPar_t filterPar; |
|
28 | extern rw_f_t rw_f; | |
|
34 | 29 | |
|
35 | 30 | // RTEMS TASKS |
|
36 | 31 | rtems_task Init( rtems_task_argument argument); |
@@ -58,6 +58,8 int set_sy_lfr_s2_bp_p1( ccsdsTelecomman | |||
|
58 | 58 | unsigned int check_update_info_hk_lfr_mode( unsigned char mode ); |
|
59 | 59 | unsigned int check_update_info_hk_tds_mode( unsigned char mode ); |
|
60 | 60 | unsigned int check_update_info_hk_thr_mode( unsigned char mode ); |
|
61 | void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ); | |
|
62 | void set_hk_lfr_sc_rw_f_flags( void ); | |
|
61 | 63 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ); |
|
62 | 64 | void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ); |
|
63 | 65 | void build_sy_lfr_rw_mask( unsigned int channel ); |
@@ -1,7 +1,7 | |||
|
1 | 1 | # LOAD FSW USING LINK 1 |
|
2 | 2 | SpwPlugin0.StarDundeeSelectLinkNumber( 1 ) |
|
3 | 3 | |
|
4 |
dsu3plugin0.openFile("/ |
|
|
4 | dsu3plugin0.openFile("/home/pleroy/DEV/DEV_PLE/FSW-qt/bin/fsw") | |
|
5 | 5 | #dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw") |
|
6 | 6 | dsu3plugin0.loadFile() |
|
7 | 7 |
@@ -66,7 +66,8 unsigned int lastValidEnterModeTime; | |||
|
66 | 66 | |
|
67 | 67 | // HK PACKETS |
|
68 | 68 | Packet_TM_LFR_HK_t housekeeping_packet; |
|
69 | unsigned char cp_rpw_sc_rw_f_flags; | |
|
69 | unsigned char cp_rpw_sc_rw1_rw2_f_flags; | |
|
70 | unsigned char cp_rpw_sc_rw3_rw4_f_flags; | |
|
70 | 71 | // message queues occupancy |
|
71 | 72 | unsigned char hk_lfr_q_sd_fifo_size_max; |
|
72 | 73 | unsigned char hk_lfr_q_rv_fifo_size_max; |
@@ -82,14 +83,7 unsigned short sequenceCounterHK; | |||
|
82 | 83 | spw_stats grspw_stats; |
|
83 | 84 | |
|
84 | 85 | // TC_LFR_UPDATE_INFO |
|
85 | float cp_rpw_sc_rw1_f1; | |
|
86 | float cp_rpw_sc_rw1_f2; | |
|
87 | float cp_rpw_sc_rw2_f1; | |
|
88 | float cp_rpw_sc_rw2_f2; | |
|
89 | float cp_rpw_sc_rw3_f1; | |
|
90 | float cp_rpw_sc_rw3_f2; | |
|
91 | float cp_rpw_sc_rw4_f1; | |
|
92 | float cp_rpw_sc_rw4_f2; | |
|
86 | rw_f_t rw_f; | |
|
93 | 87 | |
|
94 | 88 | // TC_LFR_LOAD_FILTER_PAR |
|
95 | 89 | filterPar_t filterPar; |
@@ -164,15 +164,28 rtems_task Init( rtems_task_argument ign | |||
|
164 | 164 | init_k_coefficients_prc1(); |
|
165 | 165 | init_k_coefficients_prc2(); |
|
166 | 166 | pa_bia_status_info = 0x00; |
|
167 | cp_rpw_sc_rw_f_flags = 0x00; | |
|
168 | cp_rpw_sc_rw1_f1 = 0.0; | |
|
169 |
cp_rpw_sc_rw1_f |
|
|
170 |
cp_rpw_sc_rw |
|
|
171 |
cp_rpw_sc_rw |
|
|
172 |
cp_rpw_sc_rw |
|
|
173 |
cp_rpw_sc_rw |
|
|
174 |
cp_rpw_sc_rw |
|
|
175 |
cp_rpw_sc_rw |
|
|
167 | ||
|
168 | // initialize all reaction wheels frequencies to NaN | |
|
169 | rw_f.cp_rpw_sc_rw1_f1 = NAN; | |
|
170 | rw_f.cp_rpw_sc_rw1_f2 = NAN; | |
|
171 | rw_f.cp_rpw_sc_rw1_f3 = NAN; | |
|
172 | rw_f.cp_rpw_sc_rw1_f4 = NAN; | |
|
173 | rw_f.cp_rpw_sc_rw2_f1 = NAN; | |
|
174 | rw_f.cp_rpw_sc_rw2_f2 = NAN; | |
|
175 | rw_f.cp_rpw_sc_rw2_f3 = NAN; | |
|
176 | rw_f.cp_rpw_sc_rw2_f4 = NAN; | |
|
177 | rw_f.cp_rpw_sc_rw3_f1 = NAN; | |
|
178 | rw_f.cp_rpw_sc_rw3_f2 = NAN; | |
|
179 | rw_f.cp_rpw_sc_rw3_f3 = NAN; | |
|
180 | rw_f.cp_rpw_sc_rw3_f4 = NAN; | |
|
181 | rw_f.cp_rpw_sc_rw4_f1 = NAN; | |
|
182 | rw_f.cp_rpw_sc_rw4_f2 = NAN; | |
|
183 | rw_f.cp_rpw_sc_rw4_f3 = NAN; | |
|
184 | rw_f.cp_rpw_sc_rw4_f4 = NAN; | |
|
185 | ||
|
186 | cp_rpw_sc_rw1_rw2_f_flags = 0x00; | |
|
187 | cp_rpw_sc_rw3_rw4_f_flags = 0x00; | |
|
188 | ||
|
176 | 189 | // initialize filtering parameters |
|
177 | 190 | filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED; |
|
178 | 191 | filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS; |
@@ -7,6 +7,10 | |||
|
7 | 7 | |
|
8 | 8 | #include "fsw_misc.h" |
|
9 | 9 | |
|
10 | int16_t hk_lfr_sc_v_f3_as_int16; | |
|
11 | int16_t hk_lfr_sc_e1_f3_as_int16; | |
|
12 | int16_t hk_lfr_sc_e2_f3_as_int16; | |
|
13 | ||
|
10 | 14 | void timer_configure(unsigned char timer, unsigned int clock_divider, |
|
11 | 15 | unsigned char interrupt_level, rtems_isr (*timer_isr)() ) |
|
12 | 16 | { |
@@ -317,7 +321,8 rtems_task hous_task(rtems_task_argument | |||
|
317 | 321 | |
|
318 | 322 | hk_lfr_le_me_he_update(); |
|
319 | 323 | |
|
320 | housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags; | |
|
324 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = cp_rpw_sc_rw1_rw2_f_flags; | |
|
325 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = cp_rpw_sc_rw3_rw4_f_flags; | |
|
321 | 326 | |
|
322 | 327 | // SEND PACKET |
|
323 | 328 | status = rtems_message_queue_send( queue_id, &housekeeping_packet, |
@@ -345,6 +350,9 rtems_task avgv_task(rtems_task_argument | |||
|
345 | 350 | float average_v; |
|
346 | 351 | float average_e1; |
|
347 | 352 | float average_e2; |
|
353 | float newValue_v; | |
|
354 | float newValue_e1; | |
|
355 | float newValue_e2; | |
|
348 | 356 | unsigned char k; |
|
349 | 357 | unsigned char indexOfOldValue; |
|
350 | 358 | |
@@ -376,6 +384,9 rtems_task avgv_task(rtems_task_argument | |||
|
376 | 384 | average_v = 0.; |
|
377 | 385 | average_e1 = 0.; |
|
378 | 386 | average_e2 = 0.; |
|
387 | newValue_v = 0.; | |
|
388 | newValue_e1 = 0.; | |
|
389 | newValue_e2 = 0.; | |
|
379 | 390 | } |
|
380 | 391 | |
|
381 | 392 | k = 0; |
@@ -386,21 +397,20 rtems_task avgv_task(rtems_task_argument | |||
|
386 | 397 | PRINTF1( "in AVGV *** ERR period: %d\n", status); |
|
387 | 398 | } |
|
388 | 399 | else { |
|
389 |
// |
|
|
390 |
v |
|
|
391 |
e1 |
|
|
392 |
e2 |
|
|
393 | if (k == (MOVING_AVERAGE - 1)) | |
|
394 | { | |
|
395 | indexOfOldValue = 0; | |
|
396 | } | |
|
397 | else | |
|
398 | { | |
|
399 | indexOfOldValue = k + 1; | |
|
400 | } | |
|
401 | average_v = average_v + v[k] - v[indexOfOldValue]; | |
|
402 | average_e1 = average_e1 + e1[k] - e1[indexOfOldValue]; | |
|
403 | average_e2 = average_e2 + e2[k] - e2[indexOfOldValue]; | |
|
400 | // get new values | |
|
401 | newValue_v = waveform_picker_regs->v; | |
|
402 | newValue_e1 = waveform_picker_regs->e1; | |
|
403 | newValue_e2 = waveform_picker_regs->e2; | |
|
404 | ||
|
405 | // compute the moving average | |
|
406 | average_v = average_v + newValue_v - v[k]; | |
|
407 | average_e1 = average_e1 + newValue_e1 - e1[k]; | |
|
408 | average_e2 = average_e2 + newValue_e2 - e2[k]; | |
|
409 | ||
|
410 | // store new values in buffers | |
|
411 | v[k] = newValue_v; | |
|
412 | e1[k] = newValue_e1; | |
|
413 | e2[k] = newValue_e2; | |
|
404 | 414 | } |
|
405 | 415 | if (k == (MOVING_AVERAGE-1)) |
|
406 | 416 | { |
@@ -413,7 +423,12 rtems_task avgv_task(rtems_task_argument | |||
|
413 | 423 | } |
|
414 | 424 | } |
|
415 | 425 | |
|
416 | PRINTF("in AVGV *** deleting task\n") | |
|
426 | //update int16 values | |
|
427 | hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / ((float) MOVING_AVERAGE) ); | |
|
428 | hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / ((float) MOVING_AVERAGE) ); | |
|
429 | hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / ((float) MOVING_AVERAGE) ); | |
|
430 | ||
|
431 | PRINTF("in AVGV *** deleting task\n"); | |
|
417 | 432 | |
|
418 | 433 |
|
|
419 | 434 | |
@@ -682,16 +697,16 void get_v_e1_e2_f3( unsigned char *spac | |||
|
682 | 697 | unsigned char* e1_ptr; |
|
683 | 698 | unsigned char* e2_ptr; |
|
684 | 699 | |
|
685 |
v_ptr = (unsigned char *) & |
|
|
686 |
e1_ptr = (unsigned char *) & |
|
|
687 |
e2_ptr = (unsigned char *) & |
|
|
700 | v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16; | |
|
701 | e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16; | |
|
702 | e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16; | |
|
688 | 703 | |
|
689 |
spacecraft_potential[0] = v_ptr[ |
|
|
690 |
spacecraft_potential[1] = v_ptr[ |
|
|
691 |
spacecraft_potential[2] = e1_ptr[ |
|
|
692 |
spacecraft_potential[3] = e1_ptr[ |
|
|
693 |
spacecraft_potential[4] = e2_ptr[ |
|
|
694 |
spacecraft_potential[5] = e2_ptr[ |
|
|
704 | spacecraft_potential[0] = v_ptr[0]; | |
|
705 | spacecraft_potential[1] = v_ptr[1]; | |
|
706 | spacecraft_potential[2] = e1_ptr[0]; | |
|
707 | spacecraft_potential[3] = e1_ptr[1]; | |
|
708 | spacecraft_potential[4] = e2_ptr[0]; | |
|
709 | spacecraft_potential[5] = e2_ptr[1]; | |
|
695 | 710 | } |
|
696 | 711 | |
|
697 | 712 | void get_cpu_load( unsigned char *resource_statistics ) |
@@ -295,8 +295,9 int action_update_info(ccsdsTelecommandP | |||
|
295 | 295 | | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1); |
|
296 | 296 | |
|
297 | 297 | // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets) |
|
298 | cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; | |
|
298 | //cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; | |
|
299 | 299 | getReactionWheelsFrequencies( TC ); |
|
300 | set_hk_lfr_sc_rw_f_flags(); | |
|
300 | 301 | build_sy_lfr_rw_masks(); |
|
301 | 302 | |
|
302 | 303 | result = status; |
@@ -320,8 +320,10 int action_load_filter_par(ccsdsTelecomm | |||
|
320 | 320 | */ |
|
321 | 321 | |
|
322 | 322 | int flag; |
|
323 | unsigned char k; | |
|
323 | 324 | |
|
324 | 325 | flag = LFR_DEFAULT; |
|
326 | k = 0; | |
|
325 | 327 | |
|
326 | 328 | flag = check_sy_lfr_filter_parameters( TC, queue_id ); |
|
327 | 329 | |
@@ -363,6 +365,36 int action_load_filter_par(ccsdsTelecomm | |||
|
363 | 365 | // store the parameter sy_lfr_sc_rw_delta_f as a float |
|
364 | 366 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f, |
|
365 | 367 | parameter_dump_packet.sy_lfr_sc_rw_delta_f ); |
|
368 | ||
|
369 | // copy rw.._k.. from the incoming TC to the local parameter_dump_packet | |
|
370 | for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++) | |
|
371 | { | |
|
372 | parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ]; | |
|
373 | } | |
|
374 | ||
|
375 | //*********************************************** | |
|
376 | // store the parameter sy_lfr_rw.._k.. as a float | |
|
377 | // rw1_k | |
|
378 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 ); | |
|
379 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 ); | |
|
380 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 ); | |
|
381 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 ); | |
|
382 | // rw2_k | |
|
383 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 ); | |
|
384 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 ); | |
|
385 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 ); | |
|
386 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 ); | |
|
387 | // rw3_k | |
|
388 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 ); | |
|
389 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 ); | |
|
390 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 ); | |
|
391 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 ); | |
|
392 | // rw4_k | |
|
393 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 ); | |
|
394 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 ); | |
|
395 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 ); | |
|
396 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 ); | |
|
397 | ||
|
366 | 398 | } |
|
367 | 399 | |
|
368 | 400 | return flag; |
@@ -914,6 +946,81 unsigned int check_update_info_hk_thr_mo | |||
|
914 | 946 | return status; |
|
915 | 947 | } |
|
916 | 948 | |
|
949 | void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ) | |
|
950 | { | |
|
951 | unsigned char flag; | |
|
952 | unsigned char flagPosInByte; | |
|
953 | unsigned char newFlag; | |
|
954 | unsigned char flagMask; | |
|
955 | ||
|
956 | // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered | |
|
957 | if (isnan(value)) | |
|
958 | { | |
|
959 | flag = 0; | |
|
960 | } | |
|
961 | else | |
|
962 | { | |
|
963 | flag = 1; | |
|
964 | } | |
|
965 | ||
|
966 | switch(wheel) | |
|
967 | { | |
|
968 | case 1: | |
|
969 | flagPosInByte = 8 - freq; | |
|
970 | flagMask = ~(1 << flagPosInByte); | |
|
971 | newFlag = flag << flagPosInByte; | |
|
972 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; | |
|
973 | break; | |
|
974 | case 2: | |
|
975 | flagPosInByte = 4 - freq; | |
|
976 | flagMask = ~(1 << flagPosInByte); | |
|
977 | newFlag = flag << flagPosInByte; | |
|
978 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; | |
|
979 | break; | |
|
980 | case 3: | |
|
981 | flagPosInByte = 8 - freq; | |
|
982 | flagMask = ~(1 << flagPosInByte); | |
|
983 | newFlag = flag << flagPosInByte; | |
|
984 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; | |
|
985 | break; | |
|
986 | case 4: | |
|
987 | flagPosInByte = 4 - freq; | |
|
988 | flagMask = ~(1 << flagPosInByte); | |
|
989 | newFlag = flag << flagPosInByte; | |
|
990 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; | |
|
991 | break; | |
|
992 | default: | |
|
993 | break; | |
|
994 | } | |
|
995 | } | |
|
996 | ||
|
997 | void set_hk_lfr_sc_rw_f_flags( void ) | |
|
998 | { | |
|
999 | // RW1 | |
|
1000 | set_hk_lfr_sc_rw_f_flag( 1, 1, rw_f.cp_rpw_sc_rw1_f1 ); | |
|
1001 | set_hk_lfr_sc_rw_f_flag( 1, 2, rw_f.cp_rpw_sc_rw1_f2 ); | |
|
1002 | set_hk_lfr_sc_rw_f_flag( 1, 3, rw_f.cp_rpw_sc_rw1_f3 ); | |
|
1003 | set_hk_lfr_sc_rw_f_flag( 1, 4, rw_f.cp_rpw_sc_rw1_f4 ); | |
|
1004 | ||
|
1005 | // RW2 | |
|
1006 | set_hk_lfr_sc_rw_f_flag( 2, 1, rw_f.cp_rpw_sc_rw2_f1 ); | |
|
1007 | set_hk_lfr_sc_rw_f_flag( 2, 2, rw_f.cp_rpw_sc_rw2_f2 ); | |
|
1008 | set_hk_lfr_sc_rw_f_flag( 2, 3, rw_f.cp_rpw_sc_rw2_f3 ); | |
|
1009 | set_hk_lfr_sc_rw_f_flag( 2, 4, rw_f.cp_rpw_sc_rw2_f4 ); | |
|
1010 | ||
|
1011 | // RW3 | |
|
1012 | set_hk_lfr_sc_rw_f_flag( 3, 1, rw_f.cp_rpw_sc_rw3_f1 ); | |
|
1013 | set_hk_lfr_sc_rw_f_flag( 3, 2, rw_f.cp_rpw_sc_rw3_f2 ); | |
|
1014 | set_hk_lfr_sc_rw_f_flag( 3, 3, rw_f.cp_rpw_sc_rw3_f3 ); | |
|
1015 | set_hk_lfr_sc_rw_f_flag( 3, 4, rw_f.cp_rpw_sc_rw3_f4 ); | |
|
1016 | ||
|
1017 | // RW4 | |
|
1018 | set_hk_lfr_sc_rw_f_flag( 4, 1, rw_f.cp_rpw_sc_rw4_f1 ); | |
|
1019 | set_hk_lfr_sc_rw_f_flag( 4, 2, rw_f.cp_rpw_sc_rw4_f2 ); | |
|
1020 | set_hk_lfr_sc_rw_f_flag( 4, 3, rw_f.cp_rpw_sc_rw4_f3 ); | |
|
1021 | set_hk_lfr_sc_rw_f_flag( 4, 4, rw_f.cp_rpw_sc_rw4_f4 ); | |
|
1022 | } | |
|
1023 | ||
|
917 | 1024 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ) |
|
918 | 1025 | { |
|
919 | 1026 | /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally. |
@@ -926,37 +1033,33 void getReactionWheelsFrequencies( ccsds | |||
|
926 | 1033 | |
|
927 | 1034 | bytePosPtr = (unsigned char *) &TC->packetID; |
|
928 | 1035 | |
|
929 |
// |
|
|
930 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1, | |
|
931 |
|
|
|
932 | ||
|
933 | // cp_rpw_sc_rw1_f2 | |
|
934 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2, | |
|
935 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); | |
|
1036 | // rw1_f | |
|
1037 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] ); | |
|
1038 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); | |
|
1039 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] ); | |
|
1040 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] ); | |
|
936 | 1041 | |
|
937 |
// |
|
|
938 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1, | |
|
939 |
|
|
|
940 | ||
|
941 | // cp_rpw_sc_rw2_f2 | |
|
942 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2, | |
|
943 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); | |
|
1042 | // rw2_f | |
|
1043 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] ); | |
|
1044 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); | |
|
1045 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] ); | |
|
1046 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] ); | |
|
944 | 1047 | |
|
945 |
// |
|
|
946 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1, | |
|
947 |
|
|
|
948 | ||
|
949 | // cp_rpw_sc_rw3_f2 | |
|
950 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2, | |
|
951 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); | |
|
1048 | // rw3_f | |
|
1049 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] ); | |
|
1050 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); | |
|
1051 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] ); | |
|
1052 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] ); | |
|
952 | 1053 | |
|
953 |
// |
|
|
954 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1, | |
|
955 |
|
|
|
1054 | // rw4_f | |
|
1055 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] ); | |
|
1056 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); | |
|
1057 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] ); | |
|
1058 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] ); | |
|
956 | 1059 | |
|
957 | // cp_rpw_sc_rw4_f2 | |
|
958 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2, | |
|
959 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); | |
|
1060 | // test each reaction wheel frequency value. NaN means that the frequency is not filtered | |
|
1061 | ||
|
1062 | ||
|
960 | 1063 | } |
|
961 | 1064 | |
|
962 | 1065 | void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ) |
@@ -1103,29 +1206,29 void build_sy_lfr_rw_mask( unsigned int | |||
|
1103 | 1206 | local_rw_fbins_mask[k] = 0xff; |
|
1104 | 1207 | } |
|
1105 | 1208 | |
|
1106 |
// RW1 |
|
|
1107 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x80) >> 7 ); |
|
|
1108 | ||
|
1109 | // RW1 F2 | |
|
1110 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x |
|
|
1209 | // RW1 | |
|
1210 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x80) >> 7 ); // [1000 0000] | |
|
1211 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x40) >> 6 ); // [0100 0000] | |
|
1212 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x20) >> 5 ); // [0010 0000] | |
|
1213 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x10) >> 4 ); // [0001 0000] | |
|
1111 | 1214 | |
|
1112 |
// RW2 |
|
|
1113 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x |
|
|
1114 | ||
|
1115 | // RW2 F2 | |
|
1116 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x |
|
|
1215 | // RW2 | |
|
1216 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x08) >> 3 ); // [0000 1000] | |
|
1217 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x04) >> 2 ); // [0000 0100] | |
|
1218 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x02) >> 1 ); // [0000 0010] | |
|
1219 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x01) ); // [0000 0001] | |
|
1117 | 1220 | |
|
1118 |
// RW3 |
|
|
1119 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x |
|
|
1120 | ||
|
1121 | // RW3 F2 | |
|
1122 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x0 |
|
|
1221 | // RW3 | |
|
1222 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x80) >> 7 ); // [1000 0000] | |
|
1223 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x40) >> 6 ); // [0100 0000] | |
|
1224 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x20) >> 5 ); // [0010 0000] | |
|
1225 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x10) >> 4 ); // [0001 0000] | |
|
1123 | 1226 | |
|
1124 |
// RW4 |
|
|
1125 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x0 |
|
|
1126 | ||
|
1127 | // RW4 F2 | |
|
1128 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x0 |
|
|
1227 | // RW4 | |
|
1228 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x08) >> 3 ); // [0000 1000] | |
|
1229 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x04) >> 2 ); // [0000 0100] | |
|
1230 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x02) >> 1 ); // [0000 0010] | |
|
1231 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x03) ); // [0000 0001] | |
|
1129 | 1232 | |
|
1130 | 1233 | // update the value of the fbins related to reaction wheels frequency filtering |
|
1131 | 1234 | if (maskPtr != NULL) |
@@ -1468,7 +1571,8 void init_parameter_dump( void ) | |||
|
1468 | 1571 | parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff; |
|
1469 | 1572 | } |
|
1470 | 1573 | |
|
1471 | // PAS FILTER PARAMETERS | |
|
1574 | //****************** | |
|
1575 | // FILTER PARAMETERS | |
|
1472 | 1576 | parameter_dump_packet.pa_rpw_spare8_2 = 0x00; |
|
1473 | 1577 | parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = 0x00; |
|
1474 | 1578 | parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS; |
@@ -1477,6 +1581,27 void init_parameter_dump( void ) | |||
|
1477 | 1581 | floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift ); |
|
1478 | 1582 | floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f ); |
|
1479 | 1583 | |
|
1584 | // RW1_K | |
|
1585 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1); | |
|
1586 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2); | |
|
1587 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3); | |
|
1588 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4); | |
|
1589 | // RW2_K | |
|
1590 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1); | |
|
1591 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2); | |
|
1592 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3); | |
|
1593 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4); | |
|
1594 | // RW3_K | |
|
1595 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1); | |
|
1596 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2); | |
|
1597 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3); | |
|
1598 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4); | |
|
1599 | // RW4_K | |
|
1600 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1); | |
|
1601 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2); | |
|
1602 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3); | |
|
1603 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4); | |
|
1604 | ||
|
1480 | 1605 | // LFR_RW_MASK |
|
1481 | 1606 | for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++) |
|
1482 | 1607 | { |
General Comments 0
You need to be logged in to leave comments.
Login now