##// END OF EJS Templates
compliance with ICD 4.3...
paul -
r299:7f467c56a168 R3_plus draft
parent child
Show More
@@ -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
71 #QMAKE_CFLAGS_RELEASE += -O3 -std=c99
72 #QMAKE_CFLAGS_DEBUG += -O3 -std=c99
73 #QMAKE_CXXFLAGS_RELEASE += -O3 -std=c99
74 #QMAKE_CXXFLAGS_DEBUG += -O3 -std=c99
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
60 #QMAKE_CFLAGS_RELEASE += -O0
61 #QMAKE_CFLAGS_DEBUG += -O0
62 #QMAKE_CXXFLAGS_RELEASE += -O0
63 #QMAKE_CXXFLAGS_DEBUG += -O0
84 64
85 65
86
87
88
89
90
91
92
93
94
95
96
97
98
99
66 #QMAKE_CFLAGS_RELEASE+= -O3 -std=c99
67 #QMAKE_CFLAGS_DEBUG+= -O3 -std=c99
68 #QMAKE_CXXFLAGS_RELEASE+= -O3 -std=c99
69 #QMAKE_CXXFLAGS_DEBUG+= -O3 -std=c99
@@ -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 float cp_rpw_sc_rw1_f1;
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("/opt/DEV_PLE/FSW-qt/bin/fsw")
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_f2 = 0.0;
170 cp_rpw_sc_rw2_f1 = 0.0;
171 cp_rpw_sc_rw2_f2 = 0.0;
172 cp_rpw_sc_rw3_f1 = 0.0;
173 cp_rpw_sc_rw3_f2 = 0.0;
174 cp_rpw_sc_rw4_f1 = 0.0;
175 cp_rpw_sc_rw4_f2 = 0.0;
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 // store new value in buffer
390 v[k] = waveform_picker_regs->v;
391 e1[k] = waveform_picker_regs->e1;
392 e2[k] = waveform_picker_regs->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,9 +423,14 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) );
417 430
418 status = rtems_task_delete( RTEMS_SELF ); // should not return
431 PRINTF("in AVGV *** deleting task\n");
432
433 status = rtems_task_delete( RTEMS_SELF ); // should not return
419 434
420 435 return;
421 436 }
@@ -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 *) &waveform_picker_regs->v;
686 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
687 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
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[2];
690 spacecraft_potential[1] = v_ptr[3];
691 spacecraft_potential[2] = e1_ptr[2];
692 spacecraft_potential[3] = e1_ptr[3];
693 spacecraft_potential[4] = e2_ptr[2];
694 spacecraft_potential[5] = e2_ptr[3];
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 // cp_rpw_sc_rw1_f1
930 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1,
931 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
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 // cp_rpw_sc_rw2_f1
938 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1,
939 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
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 // cp_rpw_sc_rw3_f1
946 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1,
947 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
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 // cp_rpw_sc_rw4_f1
954 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1,
955 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
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 F1
1107 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x80) >> 7 ); // [1000 0000]
1108
1109 // RW1 F2
1110 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x40) >> 6 ); // [0100 0000]
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 F1
1113 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x20) >> 5 ); // [0010 0000]
1114
1115 // RW2 F2
1116 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x10) >> 4 ); // [0001 0000]
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 F1
1119 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x08) >> 3 ); // [0000 1000]
1120
1121 // RW3 F2
1122 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x04) >> 2 ); // [0000 0100]
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 F1
1125 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x02) >> 1 ); // [0000 0010]
1126
1127 // RW4 F2
1128 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x01) ); // [0000 0001]
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