diff --git a/.hgsubstate b/.hgsubstate --- a/.hgsubstate +++ b/.hgsubstate @@ -1,2 +1,2 @@ 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters -94f0f2fccbcb8030d9437ffbb69ee0eefaaea188 header/lfr_common_headers +5cf0bac6095ff491d937622f8253c6f05095cb39 header/lfr_common_headers diff --git a/FSW-qt/fsw-qt.pro b/FSW-qt/fsw-qt.pro --- a/FSW-qt/fsw-qt.pro +++ b/FSW-qt/fsw-qt.pro @@ -1,4 +1,15 @@ TEMPLATE = app +CONFIG += console +CONFIG -= app_bundle +CONFIG -= qt + +include(sparc.pri) + +INCLUDEPATH += ./ + +OBJECTS_DIR=obj +DESTDIR=bin + # CONFIG += console v8 sim # CONFIG options = # verbose @@ -11,9 +22,6 @@ TEMPLATE = app # lpp_dpu_destid /!\ REMOVE BEFORE DELIVERY TO LESIA /!\ # debug_watchdog CONFIG += console verbose lpp_dpu_destid cpu_usage_report -CONFIG -= qt - -include(./sparc.pri) INCLUDEPATH += /opt/rtems-4.10/sparc-rtems/leon3/lib/include diff --git a/FSW-qt/sparc.pri b/FSW-qt/sparc.pri --- a/FSW-qt/sparc.pri +++ b/FSW-qt/sparc.pri @@ -1,5 +1,4 @@ -CONFIG += console -CONFIG -= qt + QMAKE_CC=sparc-rtems-gcc message(C compiler forced to: $$QMAKE_CC) QMAKE_CXX=sparc-rtems-g++ @@ -13,14 +12,14 @@ QMAKE_OBJCOPY= sparc-rtems-objcopy QMAKE_STRIP=sparc-rtems-strip QMAKE_GDB=sparc-rtems-gdb -#INCLUDEPATH += /opt/rtems-4.10 -INCLUDEPATH += /opt/rtems-4.10/sparc-rtems/leon3/lib/include - +QMAKE_CFLAGS_APP="" +QMAKE_CXXFLAGS_APP="" +QMAKE_CFLAGS= QMAKE_CFLAGS_DEBUG= -g QMAKE_CFLAGS_RELEASE="" QMAKE_CXXFLAGS_DEBUG= -g QMAKE_CXXFLAGS_RELEASE="" -QMAKE_LFLAGS_RELEASE="" +QMAKE_LFLAGS_RELEASE= QMAKE_LFLAGS_DEBUG= -g QMAKE_CXXFLAGS_DEPS = QMAKE_CXXFLAGS_WARN_ON = -Wall @@ -39,7 +38,6 @@ QMAKE_CFLAGS_RELEASE = QMAKE_CFLAGS_YACC = QMAKE_LFLAGS_EXCEPTIONS_ON = QMAKE_LFLAGS_EXCEPTIONS_OFF = -QMAKE_LFLAGS_RELEASE = -Xlinker -Map=output.map QMAKE_LFLAGS_CONSOLE = QMAKE_LFLAGS_WINDOWS = QMAKE_LFLAGS_DLL = @@ -53,47 +51,19 @@ QMAKE_LIBS="" INCLUDEPATH="" DEFINES="" -contains( TEMPLATE, app ) { - OBJECTS_DIR=obj - DESTDIR=bin -} - -#QMAKE_CFLAGS_RELEASE += -O0 -#QMAKE_CFLAGS_DEBUG += -O0 -#QMAKE_CXXFLAGS_RELEASE += -O0 -#QMAKE_CXXFLAGS_DEBUG += -O0 QMAKE_CFLAGS_RELEASE += -O3 QMAKE_CFLAGS_DEBUG += -O3 QMAKE_CXXFLAGS_RELEASE += -O3 QMAKE_CXXFLAGS_DEBUG += -O3 -#QMAKE_CFLAGS_RELEASE += -O3 -std=c99 -#QMAKE_CFLAGS_DEBUG += -O3 -std=c99 -#QMAKE_CXXFLAGS_RELEASE += -O3 -std=c99 -#QMAKE_CXXFLAGS_DEBUG += -O3 -std=c99 - -contains( TEMPLATE, app ) { - grmon.target = grmon - grmon.commands = cd $$DESTDIR && C:/opt/grmon-eval-2.0.29b/win32/bin/grmon.exe -uart COM4 -u - QMAKE_EXTRA_TARGETS += grmon -} - - - +#QMAKE_CFLAGS_RELEASE += -O0 +#QMAKE_CFLAGS_DEBUG += -O0 +#QMAKE_CXXFLAGS_RELEASE += -O0 +#QMAKE_CXXFLAGS_DEBUG += -O0 - - - - - - - - - - - - - - +#QMAKE_CFLAGS_RELEASE+= -O3 -std=c99 +#QMAKE_CFLAGS_DEBUG+= -O3 -std=c99 +#QMAKE_CXXFLAGS_RELEASE+= -O3 -std=c99 +#QMAKE_CXXFLAGS_DEBUG+= -O3 -std=c99 diff --git a/header/fsw_init.h b/header/fsw_init.h --- a/header/fsw_init.h +++ b/header/fsw_init.h @@ -21,16 +21,11 @@ extern rtems_id Task_id[]; /* extern rtems_name timecode_timer_name; extern rtems_id timecode_timer_id; extern unsigned char pa_bia_status_info; -extern unsigned char cp_rpw_sc_rw_f_flags; -extern float cp_rpw_sc_rw1_f1; -extern float cp_rpw_sc_rw1_f2; -extern float cp_rpw_sc_rw2_f1; -extern float cp_rpw_sc_rw2_f2; -extern float cp_rpw_sc_rw3_f1; -extern float cp_rpw_sc_rw3_f2; -extern float cp_rpw_sc_rw4_f1; -extern float cp_rpw_sc_rw4_f2; +extern unsigned char cp_rpw_sc_rw1_rw2_f_flags; +extern unsigned char cp_rpw_sc_rw3_rw4_f_flags; + extern filterPar_t filterPar; +extern rw_f_t rw_f; // RTEMS TASKS rtems_task Init( rtems_task_argument argument); diff --git a/header/tc_load_dump_parameters.h b/header/tc_load_dump_parameters.h --- a/header/tc_load_dump_parameters.h +++ b/header/tc_load_dump_parameters.h @@ -58,6 +58,8 @@ int set_sy_lfr_s2_bp_p1( ccsdsTelecomman unsigned int check_update_info_hk_lfr_mode( unsigned char mode ); unsigned int check_update_info_hk_tds_mode( unsigned char mode ); unsigned int check_update_info_hk_thr_mode( unsigned char mode ); +void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ); +void set_hk_lfr_sc_rw_f_flags( void ); void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ); void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ); void build_sy_lfr_rw_mask( unsigned int channel ); diff --git a/python_scripts/00_LFRControlPlugin_reload_fsw.py b/python_scripts/00_LFRControlPlugin_reload_fsw.py --- a/python_scripts/00_LFRControlPlugin_reload_fsw.py +++ b/python_scripts/00_LFRControlPlugin_reload_fsw.py @@ -1,7 +1,7 @@ # LOAD FSW USING LINK 1 SpwPlugin0.StarDundeeSelectLinkNumber( 1 ) -dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw") +dsu3plugin0.openFile("/home/pleroy/DEV/DEV_PLE/FSW-qt/bin/fsw") #dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw") dsu3plugin0.loadFile() diff --git a/src/fsw_globals.c b/src/fsw_globals.c --- a/src/fsw_globals.c +++ b/src/fsw_globals.c @@ -66,7 +66,8 @@ unsigned int lastValidEnterModeTime; // HK PACKETS Packet_TM_LFR_HK_t housekeeping_packet; -unsigned char cp_rpw_sc_rw_f_flags; +unsigned char cp_rpw_sc_rw1_rw2_f_flags; +unsigned char cp_rpw_sc_rw3_rw4_f_flags; // message queues occupancy unsigned char hk_lfr_q_sd_fifo_size_max; unsigned char hk_lfr_q_rv_fifo_size_max; @@ -82,14 +83,7 @@ unsigned short sequenceCounterHK; spw_stats grspw_stats; // TC_LFR_UPDATE_INFO -float cp_rpw_sc_rw1_f1; -float cp_rpw_sc_rw1_f2; -float cp_rpw_sc_rw2_f1; -float cp_rpw_sc_rw2_f2; -float cp_rpw_sc_rw3_f1; -float cp_rpw_sc_rw3_f2; -float cp_rpw_sc_rw4_f1; -float cp_rpw_sc_rw4_f2; +rw_f_t rw_f; // TC_LFR_LOAD_FILTER_PAR filterPar_t filterPar; diff --git a/src/fsw_init.c b/src/fsw_init.c --- a/src/fsw_init.c +++ b/src/fsw_init.c @@ -164,15 +164,28 @@ rtems_task Init( rtems_task_argument ign init_k_coefficients_prc1(); init_k_coefficients_prc2(); pa_bia_status_info = 0x00; - cp_rpw_sc_rw_f_flags = 0x00; - cp_rpw_sc_rw1_f1 = 0.0; - cp_rpw_sc_rw1_f2 = 0.0; - cp_rpw_sc_rw2_f1 = 0.0; - cp_rpw_sc_rw2_f2 = 0.0; - cp_rpw_sc_rw3_f1 = 0.0; - cp_rpw_sc_rw3_f2 = 0.0; - cp_rpw_sc_rw4_f1 = 0.0; - cp_rpw_sc_rw4_f2 = 0.0; + + // initialize all reaction wheels frequencies to NaN + rw_f.cp_rpw_sc_rw1_f1 = NAN; + rw_f.cp_rpw_sc_rw1_f2 = NAN; + rw_f.cp_rpw_sc_rw1_f3 = NAN; + rw_f.cp_rpw_sc_rw1_f4 = NAN; + rw_f.cp_rpw_sc_rw2_f1 = NAN; + rw_f.cp_rpw_sc_rw2_f2 = NAN; + rw_f.cp_rpw_sc_rw2_f3 = NAN; + rw_f.cp_rpw_sc_rw2_f4 = NAN; + rw_f.cp_rpw_sc_rw3_f1 = NAN; + rw_f.cp_rpw_sc_rw3_f2 = NAN; + rw_f.cp_rpw_sc_rw3_f3 = NAN; + rw_f.cp_rpw_sc_rw3_f4 = NAN; + rw_f.cp_rpw_sc_rw4_f1 = NAN; + rw_f.cp_rpw_sc_rw4_f2 = NAN; + rw_f.cp_rpw_sc_rw4_f3 = NAN; + rw_f.cp_rpw_sc_rw4_f4 = NAN; + + cp_rpw_sc_rw1_rw2_f_flags = 0x00; + cp_rpw_sc_rw3_rw4_f_flags = 0x00; + // initialize filtering parameters filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED; filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS; diff --git a/src/fsw_misc.c b/src/fsw_misc.c --- a/src/fsw_misc.c +++ b/src/fsw_misc.c @@ -7,6 +7,10 @@ #include "fsw_misc.h" +int16_t hk_lfr_sc_v_f3_as_int16; +int16_t hk_lfr_sc_e1_f3_as_int16; +int16_t hk_lfr_sc_e2_f3_as_int16; + void timer_configure(unsigned char timer, unsigned int clock_divider, unsigned char interrupt_level, rtems_isr (*timer_isr)() ) { @@ -317,7 +321,8 @@ rtems_task hous_task(rtems_task_argument hk_lfr_le_me_he_update(); - housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags; + housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = cp_rpw_sc_rw1_rw2_f_flags; + housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = cp_rpw_sc_rw3_rw4_f_flags; // SEND PACKET status = rtems_message_queue_send( queue_id, &housekeeping_packet, @@ -345,6 +350,9 @@ rtems_task avgv_task(rtems_task_argument float average_v; float average_e1; float average_e2; + float newValue_v; + float newValue_e1; + float newValue_e2; unsigned char k; unsigned char indexOfOldValue; @@ -376,6 +384,9 @@ rtems_task avgv_task(rtems_task_argument average_v = 0.; average_e1 = 0.; average_e2 = 0.; + newValue_v = 0.; + newValue_e1 = 0.; + newValue_e2 = 0.; } k = 0; @@ -386,21 +397,20 @@ rtems_task avgv_task(rtems_task_argument PRINTF1( "in AVGV *** ERR period: %d\n", status); } else { - // store new value in buffer - v[k] = waveform_picker_regs->v; - e1[k] = waveform_picker_regs->e1; - e2[k] = waveform_picker_regs->e2; - if (k == (MOVING_AVERAGE - 1)) - { - indexOfOldValue = 0; - } - else - { - indexOfOldValue = k + 1; - } - average_v = average_v + v[k] - v[indexOfOldValue]; - average_e1 = average_e1 + e1[k] - e1[indexOfOldValue]; - average_e2 = average_e2 + e2[k] - e2[indexOfOldValue]; + // get new values + newValue_v = waveform_picker_regs->v; + newValue_e1 = waveform_picker_regs->e1; + newValue_e2 = waveform_picker_regs->e2; + + // compute the moving average + average_v = average_v + newValue_v - v[k]; + average_e1 = average_e1 + newValue_e1 - e1[k]; + average_e2 = average_e2 + newValue_e2 - e2[k]; + + // store new values in buffers + v[k] = newValue_v; + e1[k] = newValue_e1; + e2[k] = newValue_e2; } if (k == (MOVING_AVERAGE-1)) { @@ -413,9 +423,14 @@ rtems_task avgv_task(rtems_task_argument } } - PRINTF("in AVGV *** deleting task\n") + //update int16 values + hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / ((float) MOVING_AVERAGE) ); + hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / ((float) MOVING_AVERAGE) ); + hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / ((float) MOVING_AVERAGE) ); - status = rtems_task_delete( RTEMS_SELF ); // should not return + PRINTF("in AVGV *** deleting task\n"); + + status = rtems_task_delete( RTEMS_SELF ); // should not return return; } @@ -682,16 +697,16 @@ void get_v_e1_e2_f3( unsigned char *spac unsigned char* e1_ptr; unsigned char* e2_ptr; - v_ptr = (unsigned char *) &waveform_picker_regs->v; - e1_ptr = (unsigned char *) &waveform_picker_regs->e1; - e2_ptr = (unsigned char *) &waveform_picker_regs->e2; + v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16; + e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16; + e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16; - spacecraft_potential[0] = v_ptr[2]; - spacecraft_potential[1] = v_ptr[3]; - spacecraft_potential[2] = e1_ptr[2]; - spacecraft_potential[3] = e1_ptr[3]; - spacecraft_potential[4] = e2_ptr[2]; - spacecraft_potential[5] = e2_ptr[3]; + spacecraft_potential[0] = v_ptr[0]; + spacecraft_potential[1] = v_ptr[1]; + spacecraft_potential[2] = e1_ptr[0]; + spacecraft_potential[3] = e1_ptr[1]; + spacecraft_potential[4] = e2_ptr[0]; + spacecraft_potential[5] = e2_ptr[1]; } void get_cpu_load( unsigned char *resource_statistics ) diff --git a/src/tc_handler.c b/src/tc_handler.c --- a/src/tc_handler.c +++ b/src/tc_handler.c @@ -295,8 +295,9 @@ int action_update_info(ccsdsTelecommandP | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1); // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets) - cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; + //cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; getReactionWheelsFrequencies( TC ); + set_hk_lfr_sc_rw_f_flags(); build_sy_lfr_rw_masks(); result = status; diff --git a/src/tc_load_dump_parameters.c b/src/tc_load_dump_parameters.c --- a/src/tc_load_dump_parameters.c +++ b/src/tc_load_dump_parameters.c @@ -320,8 +320,10 @@ int action_load_filter_par(ccsdsTelecomm */ int flag; + unsigned char k; flag = LFR_DEFAULT; + k = 0; flag = check_sy_lfr_filter_parameters( TC, queue_id ); @@ -363,6 +365,36 @@ int action_load_filter_par(ccsdsTelecomm // store the parameter sy_lfr_sc_rw_delta_f as a float copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f, parameter_dump_packet.sy_lfr_sc_rw_delta_f ); + + // copy rw.._k.. from the incoming TC to the local parameter_dump_packet + for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++) + { + parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ]; + } + + //*********************************************** + // store the parameter sy_lfr_rw.._k.. as a float + // rw1_k + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 ); + // rw2_k + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 ); + // rw3_k + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 ); + // rw4_k + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 ); + } return flag; @@ -914,6 +946,81 @@ unsigned int check_update_info_hk_thr_mo return status; } +void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ) +{ + unsigned char flag; + unsigned char flagPosInByte; + unsigned char newFlag; + unsigned char flagMask; + + // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered + if (isnan(value)) + { + flag = 0; + } + else + { + flag = 1; + } + + switch(wheel) + { + case 1: + flagPosInByte = 8 - freq; + flagMask = ~(1 << flagPosInByte); + newFlag = flag << flagPosInByte; + housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; + break; + case 2: + flagPosInByte = 4 - freq; + flagMask = ~(1 << flagPosInByte); + newFlag = flag << flagPosInByte; + housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; + break; + case 3: + flagPosInByte = 8 - freq; + flagMask = ~(1 << flagPosInByte); + newFlag = flag << flagPosInByte; + housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; + break; + case 4: + flagPosInByte = 4 - freq; + flagMask = ~(1 << flagPosInByte); + newFlag = flag << flagPosInByte; + housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; + break; + default: + break; + } +} + +void set_hk_lfr_sc_rw_f_flags( void ) +{ + // RW1 + set_hk_lfr_sc_rw_f_flag( 1, 1, rw_f.cp_rpw_sc_rw1_f1 ); + set_hk_lfr_sc_rw_f_flag( 1, 2, rw_f.cp_rpw_sc_rw1_f2 ); + set_hk_lfr_sc_rw_f_flag( 1, 3, rw_f.cp_rpw_sc_rw1_f3 ); + set_hk_lfr_sc_rw_f_flag( 1, 4, rw_f.cp_rpw_sc_rw1_f4 ); + + // RW2 + set_hk_lfr_sc_rw_f_flag( 2, 1, rw_f.cp_rpw_sc_rw2_f1 ); + set_hk_lfr_sc_rw_f_flag( 2, 2, rw_f.cp_rpw_sc_rw2_f2 ); + set_hk_lfr_sc_rw_f_flag( 2, 3, rw_f.cp_rpw_sc_rw2_f3 ); + set_hk_lfr_sc_rw_f_flag( 2, 4, rw_f.cp_rpw_sc_rw2_f4 ); + + // RW3 + set_hk_lfr_sc_rw_f_flag( 3, 1, rw_f.cp_rpw_sc_rw3_f1 ); + set_hk_lfr_sc_rw_f_flag( 3, 2, rw_f.cp_rpw_sc_rw3_f2 ); + set_hk_lfr_sc_rw_f_flag( 3, 3, rw_f.cp_rpw_sc_rw3_f3 ); + set_hk_lfr_sc_rw_f_flag( 3, 4, rw_f.cp_rpw_sc_rw3_f4 ); + + // RW4 + set_hk_lfr_sc_rw_f_flag( 4, 1, rw_f.cp_rpw_sc_rw4_f1 ); + set_hk_lfr_sc_rw_f_flag( 4, 2, rw_f.cp_rpw_sc_rw4_f2 ); + set_hk_lfr_sc_rw_f_flag( 4, 3, rw_f.cp_rpw_sc_rw4_f3 ); + set_hk_lfr_sc_rw_f_flag( 4, 4, rw_f.cp_rpw_sc_rw4_f4 ); +} + void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ) { /** 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 bytePosPtr = (unsigned char *) &TC->packetID; - // cp_rpw_sc_rw1_f1 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] ); - - // cp_rpw_sc_rw1_f2 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); + // rw1_f + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] ); - // cp_rpw_sc_rw2_f1 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] ); - - // cp_rpw_sc_rw2_f2 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); + // rw2_f + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] ); - // cp_rpw_sc_rw3_f1 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] ); - - // cp_rpw_sc_rw3_f2 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); + // rw3_f + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] ); - // cp_rpw_sc_rw4_f1 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] ); + // rw4_f + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] ); - // cp_rpw_sc_rw4_f2 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); + // test each reaction wheel frequency value. NaN means that the frequency is not filtered + + } 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 local_rw_fbins_mask[k] = 0xff; } - // RW1 F1 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x80) >> 7 ); // [1000 0000] - - // RW1 F2 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x40) >> 6 ); // [0100 0000] + // RW1 + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x80) >> 7 ); // [1000 0000] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x40) >> 6 ); // [0100 0000] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x20) >> 5 ); // [0010 0000] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x10) >> 4 ); // [0001 0000] - // RW2 F1 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x20) >> 5 ); // [0010 0000] - - // RW2 F2 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x10) >> 4 ); // [0001 0000] + // RW2 + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x08) >> 3 ); // [0000 1000] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x04) >> 2 ); // [0000 0100] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x02) >> 1 ); // [0000 0010] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x01) ); // [0000 0001] - // RW3 F1 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x08) >> 3 ); // [0000 1000] - - // RW3 F2 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x04) >> 2 ); // [0000 0100] + // RW3 + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x80) >> 7 ); // [1000 0000] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x40) >> 6 ); // [0100 0000] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x20) >> 5 ); // [0010 0000] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x10) >> 4 ); // [0001 0000] - // RW4 F1 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x02) >> 1 ); // [0000 0010] - - // RW4 F2 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x01) ); // [0000 0001] + // RW4 + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x08) >> 3 ); // [0000 1000] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x04) >> 2 ); // [0000 0100] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x02) >> 1 ); // [0000 0010] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x03) ); // [0000 0001] // update the value of the fbins related to reaction wheels frequency filtering if (maskPtr != NULL) @@ -1468,7 +1571,8 @@ void init_parameter_dump( void ) parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff; } - // PAS FILTER PARAMETERS + //****************** + // FILTER PARAMETERS parameter_dump_packet.pa_rpw_spare8_2 = 0x00; parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = 0x00; parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS; @@ -1477,6 +1581,27 @@ void init_parameter_dump( void ) floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift ); floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f ); + // RW1_K + floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1); + floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2); + floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3); + floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4); + // RW2_K + floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1); + floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2); + floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3); + floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4); + // RW3_K + floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1); + floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2); + floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3); + floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4); + // RW4_K + floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1); + floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2); + floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3); + floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4); + // LFR_RW_MASK for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++) {