LCOV - code coverage report
Current view: top level - src/tc_tm - tc_load_dump_parameters.c (source / functions) Hit Total Coverage
Test: trace.info Lines: 835 869 96.1 %
Date: 2023-02-20 11:47:18 Functions: 33 49 67.3 %

          Line data    Source code
       1             : /*------------------------------------------------------------------------------
       2             : --  Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
       3             : --  This file is a part of the LFR FSW
       4             : --  Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
       5             : --
       6             : --  This program is free software; you can redistribute it and/or modify
       7             : --  it under the terms of the GNU General Public License as published by
       8             : --  the Free Software Foundation; either version 2 of the License, or
       9             : --  (at your option) any later version.
      10             : --
      11             : --  This program is distributed in the hope that it will be useful,
      12             : --  but WITHOUT ANY WARRANTY; without even the implied warranty of
      13             : --  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      14             : --  GNU General Public License for more details.
      15             : --
      16             : --  You should have received a copy of the GNU General Public License
      17             : --  along with this program; if not, write to the Free Software
      18             : --  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
      19             : -------------------------------------------------------------------------------*/
      20             : /*--                  Author : Paul Leroy
      21             : --                   Contact : Alexis Jeandet
      22             : --                      Mail : alexis.jeandet@lpp.polytechnique.fr
      23             : ----------------------------------------------------------------------------*/
      24             : 
      25             : /** Functions to load and dump parameters in the LFR registers.
      26             :  *
      27             :  * @file
      28             :  * @author P. LEROY
      29             :  *
      30             :  * A group of functions to handle TC related to parameter loading and dumping.\n
      31             :  * TC_LFR_LOAD_COMMON_PAR\n
      32             :  * TC_LFR_LOAD_NORMAL_PAR\n
      33             :  * TC_LFR_LOAD_BURST_PAR\n
      34             :  * TC_LFR_LOAD_SBM1_PAR\n
      35             :  * TC_LFR_LOAD_SBM2_PAR\n
      36             :  *
      37             :  */
      38             : 
      39             : #include <math.h>
      40             : #include <string.h>
      41             : 
      42             : #include "fsw_compile_warnings.h"
      43             : #include "fsw_debug.h"
      44             : #include "fsw_housekeeping.h"
      45             : #include "fsw_misc.h"
      46             : #include "hw/lfr_regs.h"
      47             : #include "processing/calibration_matrices.h"
      48             : #include "tc_tm/tc_load_dump_parameters.h"
      49             : 
      50             : 
      51             : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
      52             : Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1 = { 0 };
      53             : Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = { 0 };
      54             : ring_node kcoefficient_node_1 = { 0 };
      55             : ring_node kcoefficient_node_2 = { 0 };
      56             : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
      57             : 
      58          92 : int action_load_common_par(const ccsdsTelecommandPacket_t* const TC)
      59             : {
      60             :     /** This function updates the LFR registers with the incoming common parameters.
      61             :      *
      62             :      * @param TC points to the TeleCommand packet that is being processed
      63             :      *
      64             :      *
      65             :      */
      66             : 
      67          92 :     parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
      68          92 :     parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
      69          92 :     set_wfp_data_shaping();
      70          92 :     return LFR_SUCCESSFUL;
      71             : }
      72             : 
      73          99 : int action_load_normal_par(
      74             :     const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id, const unsigned char* const time)
      75             : {
      76             :     /** This function updates the LFR registers with the incoming normal parameters.
      77             :      *
      78             :      * @param TC points to the TeleCommand packet that is being processed
      79             :      * @param queue_id is the id of the queue which handles TM related to this execution step
      80             :      *
      81             :      */
      82             :     IGNORE_UNUSED_PARAMETER(time);
      83             : 
      84          99 :     int flag = LFR_SUCCESSFUL;
      85             : 
      86         171 :     if ((lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
      87         171 :         || (lfrCurrentMode == LFR_MODE_SBM2))
      88             :     {
      89          43 :         DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_not_executable(TC, queue_id));
      90          43 :         flag = LFR_DEFAULT;
      91             :     }
      92             : 
      93             :     // CHECK THE PARAMETERS SET CONSISTENCY
      94          99 :     if (flag == LFR_SUCCESSFUL)
      95             :     {
      96          56 :         flag = check_normal_par_consistency(TC, queue_id);
      97             :     }
      98             : 
      99             :     // SET THE PARAMETERS IF THEY ARE CONSISTENT
     100          99 :     if (flag == LFR_SUCCESSFUL)
     101             :     {
     102          42 :         flag |= set_sy_lfr_n_swf_l(TC);
     103          42 :         flag |= set_sy_lfr_n_swf_p(TC);
     104          42 :         flag |= set_sy_lfr_n_bp_p0(TC);
     105          42 :         flag |= set_sy_lfr_n_bp_p1(TC);
     106          42 :         flag |= set_sy_lfr_n_asm_p(TC);
     107          42 :         flag |= set_sy_lfr_n_cwf_long_f3(TC);
     108             :     }
     109             : 
     110          99 :     return flag;
     111             : }
     112             : 
     113         101 : int action_load_burst_par(
     114             :     const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id, const unsigned char* const time)
     115             : {
     116             :     /** This function updates the LFR registers with the incoming burst parameters.
     117             :      *
     118             :      * @param TC points to the TeleCommand packet that is being processed
     119             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     120             :      *
     121             :      */
     122             :     IGNORE_UNUSED_PARAMETER(time);
     123             :     int flag;
     124             :     rtems_status_code status;
     125             :     unsigned char sy_lfr_b_bp_p0;
     126             :     unsigned char sy_lfr_b_bp_p1;
     127             :     float aux;
     128             : 
     129         101 :     flag = LFR_SUCCESSFUL;
     130             : 
     131         101 :     if (lfrCurrentMode == LFR_MODE_BURST)
     132             :     {
     133          14 :         status = send_tm_lfr_tc_exe_not_executable(TC, queue_id);
     134             :         DEBUG_CHECK_STATUS(status);
     135          14 :         flag = LFR_DEFAULT;
     136             :     }
     137             : 
     138         101 :     sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
     139         101 :     sy_lfr_b_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P1];
     140             : 
     141             :     // sy_lfr_b_bp_p0 shall not be lower than its default value
     142         101 :     if (flag == LFR_SUCCESSFUL && sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0)
     143             :     {
     144           5 :         status = send_tm_lfr_tc_exe_inconsistent(
     145             :             TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0);
     146             :         DEBUG_CHECK_STATUS(status);
     147           5 :         flag = WRONG_APP_DATA;
     148             :     }
     149             :     // sy_lfr_b_bp_p1 shall not be lower than its default value
     150         101 :     if (flag == LFR_SUCCESSFUL && sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1)
     151             :     {
     152             : 
     153           1 :         status = send_tm_lfr_tc_exe_inconsistent(
     154             :             TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1 + DATAFIELD_OFFSET, sy_lfr_b_bp_p1);
     155             :         DEBUG_CHECK_STATUS(status);
     156           1 :         flag = WRONG_APP_DATA;
     157             :     }
     158             :     //****************************************************************
     159             :     // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
     160         101 :     if (flag == LFR_SUCCESSFUL)
     161             :     {
     162          81 :         sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
     163          81 :         sy_lfr_b_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P1];
     164          81 :         aux = ((float)sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0) - floorf(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
     165          81 :         if (aux > FLOAT_EQUAL_ZERO)
     166             :         {
     167           1 :             status = send_tm_lfr_tc_exe_inconsistent(
     168             :                 TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0);
     169             :             DEBUG_CHECK_STATUS(status);
     170           1 :             flag = LFR_DEFAULT;
     171             :         }
     172             :     }
     173             : 
     174             :     // SET THE PARAMETERS
     175         101 :     if (flag == LFR_SUCCESSFUL)
     176             :     {
     177          80 :         flag = set_sy_lfr_b_bp_p0(TC);
     178          80 :         flag |= set_sy_lfr_b_bp_p1(TC);
     179             :     }
     180             : 
     181         101 :     return flag;
     182             : }
     183             : 
     184         101 : int action_load_sbm1_par(
     185             :     const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id, const unsigned char* const time)
     186             : {
     187             :     /** This function updates the LFR registers with the incoming sbm1 parameters.
     188             :      *
     189             :      * @param TC points to the TeleCommand packet that is being processed
     190             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     191             :      *
     192             :      */
     193             : 
     194             :     IGNORE_UNUSED_PARAMETER(time);
     195             :     int flag;
     196             :     rtems_status_code status;
     197             :     unsigned char sy_lfr_s1_bp_p0;
     198             :     unsigned char sy_lfr_s1_bp_p1;
     199             :     float aux;
     200             : 
     201         101 :     flag = LFR_SUCCESSFUL;
     202             : 
     203         101 :     if (lfrCurrentMode == LFR_MODE_SBM1)
     204             :     {
     205          14 :         status = send_tm_lfr_tc_exe_not_executable(TC, queue_id);
     206             :         DEBUG_CHECK_STATUS(status);
     207          14 :         flag = LFR_DEFAULT;
     208             :     }
     209             : 
     210         101 :     sy_lfr_s1_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P0];
     211         101 :     sy_lfr_s1_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P1];
     212             : 
     213             :     // sy_lfr_s1_bp_p0
     214         101 :     if (flag == LFR_SUCCESSFUL && sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0)
     215             :     {
     216             : 
     217           5 :         status = send_tm_lfr_tc_exe_inconsistent(
     218             :             TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0);
     219             :         DEBUG_CHECK_STATUS(status);
     220           5 :         flag = WRONG_APP_DATA;
     221             :     }
     222             :     // sy_lfr_s1_bp_p1
     223         101 :     if (flag == LFR_SUCCESSFUL && sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1)
     224             :     {
     225             : 
     226           1 :         status = send_tm_lfr_tc_exe_inconsistent(
     227             :             TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p1);
     228             :         DEBUG_CHECK_STATUS(status);
     229           1 :         flag = WRONG_APP_DATA;
     230             :     }
     231             :     //******************************************************************
     232             :     // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
     233         101 :     if (flag == LFR_SUCCESSFUL)
     234             :     {
     235         162 :         aux = ((float)sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * (float)S1_BP_P0_SCALE))
     236          81 :             - floorf(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * (float)S1_BP_P0_SCALE));
     237          81 :         if (aux > FLOAT_EQUAL_ZERO)
     238             :         {
     239           1 :             status = send_tm_lfr_tc_exe_inconsistent(
     240             :                 TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0);
     241             :             DEBUG_CHECK_STATUS(status);
     242           1 :             flag = LFR_DEFAULT;
     243             :         }
     244             :     }
     245             : 
     246             :     // SET THE PARAMETERS
     247         101 :     if (flag == LFR_SUCCESSFUL)
     248             :     {
     249          80 :         flag = set_sy_lfr_s1_bp_p0(TC);
     250          80 :         flag |= set_sy_lfr_s1_bp_p1(TC);
     251             :     }
     252             : 
     253         101 :     return flag;
     254             : }
     255             : 
     256         101 : int action_load_sbm2_par(
     257             :     const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id, const unsigned char* const time)
     258             : {
     259             :     /** This function updates the LFR registers with the incoming sbm2 parameters.
     260             :      *
     261             :      * @param TC points to the TeleCommand packet that is being processed
     262             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     263             :      *
     264             :      */
     265             : 
     266             :     IGNORE_UNUSED_PARAMETER(time);
     267             :     int flag;
     268             :     rtems_status_code status;
     269             :     unsigned char sy_lfr_s2_bp_p0;
     270             :     unsigned char sy_lfr_s2_bp_p1;
     271             :     float aux;
     272             : 
     273         101 :     flag = LFR_SUCCESSFUL;
     274             : 
     275         101 :     if (lfrCurrentMode == LFR_MODE_SBM2)
     276             :     {
     277          19 :         status = send_tm_lfr_tc_exe_not_executable(TC, queue_id);
     278             :         DEBUG_CHECK_STATUS(status);
     279          19 :         flag = LFR_DEFAULT;
     280             :     }
     281             : 
     282         101 :     sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
     283         101 :     sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
     284             : 
     285             :     // sy_lfr_s2_bp_p0
     286         101 :     if (flag == LFR_SUCCESSFUL && sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0)
     287             :     {
     288             : 
     289           4 :         status = send_tm_lfr_tc_exe_inconsistent(
     290             :             TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0);
     291             :         DEBUG_CHECK_STATUS(status);
     292           4 :         flag = WRONG_APP_DATA;
     293             :     }
     294             :     // sy_lfr_s2_bp_p1
     295         101 :     if (flag == LFR_SUCCESSFUL && sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1)
     296             :     {
     297             : 
     298           0 :         status = send_tm_lfr_tc_exe_inconsistent(
     299             :             TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p1);
     300             :         DEBUG_CHECK_STATUS(status);
     301           0 :         flag = WRONG_APP_DATA;
     302             :     }
     303             :     //******************************************************************
     304             :     // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
     305         101 :     if (flag == LFR_SUCCESSFUL)
     306             :     {
     307          78 :         sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
     308          78 :         sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
     309         156 :         aux = ((float)sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0)
     310          78 :             - floorf(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
     311          78 :         if (aux > FLOAT_EQUAL_ZERO)
     312             :         {
     313           1 :             status = send_tm_lfr_tc_exe_inconsistent(
     314             :                 TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0);
     315             :             DEBUG_CHECK_STATUS(status);
     316           1 :             flag = LFR_DEFAULT;
     317             :         }
     318             :     }
     319             : 
     320             :     // SET THE PARAMETERS
     321         101 :     if (flag == LFR_SUCCESSFUL)
     322             :     {
     323          77 :         flag = set_sy_lfr_s2_bp_p0(TC);
     324          77 :         flag |= set_sy_lfr_s2_bp_p1(TC);
     325             :     }
     326             : 
     327         101 :     return flag;
     328             : }
     329             : 
     330         114 : int action_load_kcoefficients(
     331             :     const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id, const unsigned char* const time)
     332             : {
     333             :     /** This function updates the LFR registers with the incoming sbm2 parameters.
     334             :      *
     335             :      * @param TC points to the TeleCommand packet that is being processed
     336             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     337             :      *
     338             :      */
     339             : 
     340             :     IGNORE_UNUSED_PARAMETER(time);
     341         114 :     int flag = LFR_DEFAULT;
     342             :     rtems_status_code status;
     343             : 
     344         114 :     if (lfrCurrentMode != LFR_MODE_STANDBY)
     345             :     {
     346          55 :         status = send_tm_lfr_tc_exe_not_executable(TC, queue_id);
     347             :         DEBUG_CHECK_STATUS(status);
     348             :     }
     349             :     else
     350             :     {
     351          59 :         flag = set_sy_lfr_kcoeff(TC, queue_id);
     352             :     }
     353             : 
     354         114 :     return flag;
     355             : }
     356             : 
     357          82 : int action_load_fbins_mask(
     358             :     const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id, const unsigned char* const time)
     359             : {
     360             :     /** This function updates the LFR registers with the incoming sbm2 parameters.
     361             :      *
     362             :      * @param TC points to the TeleCommand packet that is being processed
     363             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     364             :      *
     365             :      */
     366             : 
     367             :     IGNORE_UNUSED_PARAMETER(time);
     368             :     IGNORE_UNUSED_PARAMETER(queue_id);
     369          82 :     int flag = set_sy_lfr_fbins(TC);
     370             : 
     371             :     // once the fbins masks have been stored, they have to be merged with the masks which handle the
     372             :     // reaction wheels frequencies filtering
     373          82 :     merge_fbins_masks();
     374             : 
     375          82 :     return flag;
     376             : }
     377             : 
     378         110 : int action_load_filter_par(const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id)
     379             : {
     380             :     /** This function updates the LFR registers with the incoming sbm2 parameters.
     381             :      *
     382             :      * @param TC points to the TeleCommand packet that is being processed
     383             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     384             :      *
     385             :      */
     386             : 
     387         110 :     int flag = check_sy_lfr_filter_parameters(TC, queue_id);
     388             : 
     389         110 :     if (flag == LFR_SUCCESSFUL)
     390             :     {
     391          98 :         parameter_dump_packet.spare_sy_lfr_pas_filter_enabled
     392          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED];
     393          98 :         parameter_dump_packet.sy_lfr_pas_filter_modulus
     394          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS];
     395          98 :         parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0]
     396          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0];
     397          98 :         parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1]
     398          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1];
     399          98 :         parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2]
     400          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2];
     401          98 :         parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3]
     402          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3];
     403          98 :         parameter_dump_packet.sy_lfr_pas_filter_offset
     404          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET];
     405          98 :         parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0]
     406          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0];
     407          98 :         parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1]
     408          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1];
     409          98 :         parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2]
     410          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2];
     411          98 :         parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3]
     412          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3];
     413          98 :         parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0]
     414          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0];
     415          98 :         parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1]
     416          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1];
     417          98 :         parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2]
     418          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2];
     419          98 :         parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3]
     420          98 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3];
     421             : 
     422             :         //****************************
     423             :         // store PAS filter parameters
     424             : 
     425             :         // sy_lfr_pas_filter_enabled
     426          98 :         filterPar.spare_sy_lfr_pas_filter_enabled
     427          98 :             = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
     428          98 :         set_sy_lfr_pas_filter_enabled(
     429          98 :             parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED);
     430             : 
     431             :         // sy_lfr_pas_filter_modulus
     432          98 :         filterPar.modulus_in_finetime
     433          98 :             = ((uint64_t)parameter_dump_packet.sy_lfr_pas_filter_modulus) * CONST_65536;
     434             : 
     435             :         // sy_lfr_pas_filter_tbad
     436          98 :         copyFloatByChar((unsigned char*)&filterPar.sy_lfr_pas_filter_tbad,
     437             :             parameter_dump_packet.sy_lfr_pas_filter_tbad);
     438          98 :         filterPar.tbad_in_finetime = (uint64_t)(filterPar.sy_lfr_pas_filter_tbad * CONST_65536);
     439             : 
     440             :         // sy_lfr_pas_filter_offset
     441          98 :         filterPar.offset_in_finetime
     442          98 :             = ((uint64_t)parameter_dump_packet.sy_lfr_pas_filter_offset) * CONST_65536;
     443             : 
     444             :         // sy_lfr_pas_filter_shift
     445          98 :         copyFloatByChar((unsigned char*)&filterPar.sy_lfr_pas_filter_shift,
     446             :             parameter_dump_packet.sy_lfr_pas_filter_shift);
     447          98 :         filterPar.shift_in_finetime = (uint64_t)(filterPar.sy_lfr_pas_filter_shift * CONST_65536);
     448             : 
     449             :         //****************************************************
     450             :         // store the parameter sy_lfr_sc_rw_delta_f as a float
     451          98 :         copyFloatByChar((unsigned char*)&filterPar.sy_lfr_sc_rw_delta_f,
     452             :             parameter_dump_packet.sy_lfr_sc_rw_delta_f);
     453             : 
     454             :         // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
     455        6370 :         for (unsigned char k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
     456             :         {
     457             :             // TODO clean this, sy_lfr_rw1_k1 is a 4 bytes array, this is UB
     458       12544 :             parameter_dump_packet.sy_lfr_rw1_k1[k]
     459        6272 :                 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_RW1_K1 + k];
     460             :         }
     461             : 
     462             :         //***********************************************
     463             :         // store the parameter sy_lfr_rw.._k.. as a float
     464             :         // rw1_k
     465          98 :         copyFloatByChar(
     466             :             (unsigned char*)&filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1);
     467          98 :         copyFloatByChar(
     468             :             (unsigned char*)&filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2);
     469          98 :         copyFloatByChar(
     470             :             (unsigned char*)&filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3);
     471          98 :         copyFloatByChar(
     472             :             (unsigned char*)&filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4);
     473             :         // rw2_k
     474          98 :         copyFloatByChar(
     475             :             (unsigned char*)&filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1);
     476          98 :         copyFloatByChar(
     477             :             (unsigned char*)&filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2);
     478          98 :         copyFloatByChar(
     479             :             (unsigned char*)&filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3);
     480          98 :         copyFloatByChar(
     481             :             (unsigned char*)&filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4);
     482             :         // rw3_k
     483          98 :         copyFloatByChar(
     484             :             (unsigned char*)&filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1);
     485          98 :         copyFloatByChar(
     486             :             (unsigned char*)&filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2);
     487          98 :         copyFloatByChar(
     488             :             (unsigned char*)&filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3);
     489          98 :         copyFloatByChar(
     490             :             (unsigned char*)&filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4);
     491             :         // rw4_k
     492          98 :         copyFloatByChar(
     493             :             (unsigned char*)&filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1);
     494          98 :         copyFloatByChar(
     495             :             (unsigned char*)&filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2);
     496          98 :         copyFloatByChar(
     497             :             (unsigned char*)&filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3);
     498          98 :         copyFloatByChar(
     499             :             (unsigned char*)&filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4);
     500             :     }
     501             : 
     502         110 :     return flag;
     503             : }
     504             : 
     505         115 : int action_dump_kcoefficients(
     506             :     const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id, const unsigned char* const time)
     507             : {
     508             :     /** This function updates the LFR registers with the incoming sbm2 parameters.
     509             :      *
     510             :      * @param TC points to the TeleCommand packet that is being processed
     511             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     512             :      *
     513             :      */
     514             : 
     515             :     IGNORE_UNUSED_PARAMETER(time);
     516             :     void* address;
     517             :     rtems_status_code status;
     518             :     unsigned char* kCoeffDumpPtr;
     519             : 
     520             :     // Each packet is 3900 bytes
     521             :     // Packet1:
     522             :     // F0 needs 12 bins * (2 bytes + 26 floats + 6 floats) -> 1560 bytes
     523             :     // F1 needs 14 bins * (2 bytes + 26 floats + 6 floats) -> 1820 bytes
     524             :     // total -> 3380 bytes
     525             : 
     526             :     // Packet2:
     527             :     // F2 needs 13 bins * (2 bytes + 26 floats + 6 floats) -> 1690 bytes
     528             :     // total -> 1690 bytes
     529             : 
     530             : 
     531             :     //*********
     532             :     // PACKET 1
     533             :     // 12 F0 bins, 14 F1 bins
     534         115 :     kcoefficients_dump_1.destinationID = TC->sourceID;
     535         115 :     increment_seq_counter_destination_id_dump(
     536         115 :         kcoefficients_dump_1.packetSequenceControl, TC->sourceID);
     537         115 :     kCoeffDumpPtr = kcoefficients_dump_1.kcoeff_blks;
     538        1495 :     for (int freq = 0; freq <= NB_BINS_COMPRESSED_SM_F0; freq++)
     539             :     {
     540        1380 :         uint16_t bin = 0;
     541        1380 :         copyInt16ByChar(kCoeffDumpPtr, (unsigned char*)(&bin));
     542        1380 :         kCoeffDumpPtr += 2;
     543        2760 :         memcpy(kCoeffDumpPtr,
     544             :             mag_calibration_matrices_f0
     545        1380 :                 + (freq * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F0),
     546             :             NB_BYTES_MAG_CAL_MATRIX);
     547        1380 :         kCoeffDumpPtr += NB_BYTES_MAG_CAL_MATRIX;
     548        2760 :         memcpy(kCoeffDumpPtr,
     549             :             elec_calibration_matrices_f0
     550        1380 :                 + (freq * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F0),
     551             :             NB_BYTES_ELEC_CAL_MATRIX);
     552             :         // stuff with 6 empty floats to keep previous layout -> freq_index[int16] & 32 floats
     553        1380 :         kCoeffDumpPtr += NB_BYTES_ELEC_CAL_MATRIX + (6 * sizeof(float));
     554             :     }
     555        1725 :     for (int freq = 0; freq <= NB_BINS_COMPRESSED_SM_F1; freq++)
     556             :     {
     557        1610 :         uint16_t bin = 1;
     558        1610 :         copyInt16ByChar(kCoeffDumpPtr, (unsigned char*)(&bin));
     559        1610 :         kCoeffDumpPtr += 2;
     560        3220 :         memcpy(kCoeffDumpPtr,
     561             :             mag_calibration_matrices_f1
     562        1610 :                 + (freq * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F1),
     563             :             NB_BYTES_MAG_CAL_MATRIX);
     564        1610 :         kCoeffDumpPtr += NB_BYTES_MAG_CAL_MATRIX;
     565        3220 :         memcpy(kCoeffDumpPtr,
     566             :             elec_calibration_matrices_f1
     567        1610 :                 + (freq * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F1),
     568             :             NB_BYTES_ELEC_CAL_MATRIX);
     569             :         // stuff with 6 empty floats to keep previous layout -> freq_index[int16] & 32 floats
     570        1610 :         kCoeffDumpPtr += NB_BYTES_ELEC_CAL_MATRIX + (6 * sizeof(float));
     571             :     }
     572             : 
     573         115 :     kcoefficients_dump_1.time[BYTE_0]
     574         115 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
     575         115 :     kcoefficients_dump_1.time[BYTE_1]
     576         115 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
     577         115 :     kcoefficients_dump_1.time[BYTE_2]
     578         115 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
     579         115 :     kcoefficients_dump_1.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
     580         115 :     kcoefficients_dump_1.time[BYTE_4]
     581         115 :         = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
     582         115 :     kcoefficients_dump_1.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
     583             :     // SEND DATA
     584         115 :     kcoefficient_node_1.status = 1;
     585         115 :     address = (void*)&kcoefficient_node_1;
     586         115 :     status = rtems_message_queue_send(queue_id, &address, sizeof(ring_node*));
     587             :     DEBUG_CHECK_STATUS(status);
     588             : 
     589             :     //********
     590             :     // PACKET 2
     591             :     // 13 F2 bins
     592         115 :     kcoefficients_dump_2.destinationID = TC->sourceID;
     593         115 :     increment_seq_counter_destination_id_dump(
     594         115 :         kcoefficients_dump_2.packetSequenceControl, TC->sourceID);
     595             : 
     596         115 :     kCoeffDumpPtr = kcoefficients_dump_2.kcoeff_blks;
     597        1610 :     for (int freq = 0; freq <= NB_BINS_COMPRESSED_SM_F2; freq++)
     598             :     {
     599        1495 :         uint16_t bin = 2;
     600        1495 :         copyInt16ByChar(kCoeffDumpPtr, (unsigned char*)(&bin));
     601        1495 :         kCoeffDumpPtr += 2;
     602        2990 :         memcpy(kCoeffDumpPtr,
     603             :             mag_calibration_matrices_f2
     604        1495 :                 + (freq * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F2),
     605             :             NB_BYTES_MAG_CAL_MATRIX);
     606        1495 :         kCoeffDumpPtr += NB_BYTES_MAG_CAL_MATRIX;
     607        2990 :         memcpy(kCoeffDumpPtr,
     608             :             elec_calibration_matrices_f2
     609        1495 :                 + (freq * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F2),
     610             :             NB_BYTES_ELEC_CAL_MATRIX);
     611             :         // stuff with 6 empty floats to keep previous layout -> freq_index[int16] & 32 floats
     612        1495 :         kCoeffDumpPtr += NB_BYTES_ELEC_CAL_MATRIX + (6 * sizeof(float));
     613             :     }
     614             : 
     615         115 :     kcoefficients_dump_2.time[BYTE_0]
     616         115 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
     617         115 :     kcoefficients_dump_2.time[BYTE_1]
     618         115 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
     619         115 :     kcoefficients_dump_2.time[BYTE_2]
     620         115 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
     621         115 :     kcoefficients_dump_2.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
     622         115 :     kcoefficients_dump_2.time[BYTE_4]
     623         115 :         = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
     624         115 :     kcoefficients_dump_2.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
     625             :     // SEND DATA
     626         115 :     kcoefficient_node_2.status = 1;
     627         115 :     address = (void*)&kcoefficient_node_2;
     628         115 :     status = rtems_message_queue_send(queue_id, &address, sizeof(ring_node*));
     629             :     DEBUG_CHECK_STATUS(status);
     630             : 
     631         115 :     return status;
     632             : }
     633             : 
     634         190 : int action_dump_par(const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id)
     635             : {
     636             :     /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated
     637             :      * RTEMS message queue.
     638             :      *
     639             :      * @param queue_id is the id of the queue which handles TM related to this execution step.
     640             :      *
     641             :      * @return RTEMS directive status codes:
     642             :      * - RTEMS_SUCCESSFUL - message sent successfully
     643             :      * - RTEMS_INVALID_ID - invalid queue id
     644             :      * - RTEMS_INVALID_SIZE - invalid message size
     645             :      * - RTEMS_INVALID_ADDRESS - buffer is NULL
     646             :      * - RTEMS_UNSATISFIED - out of message buffers
     647             :      * - RTEMS_TOO_MANY - queue s limit has been reached
     648             :      *
     649             :      */
     650             : 
     651             :     int status;
     652             : 
     653         190 :     increment_seq_counter_destination_id_dump(
     654         190 :         parameter_dump_packet.packetSequenceControl, TC->sourceID);
     655         190 :     parameter_dump_packet.destinationID = TC->sourceID;
     656             : 
     657             :     // UPDATE TIME
     658         190 :     parameter_dump_packet.time[BYTE_0]
     659         190 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
     660         190 :     parameter_dump_packet.time[BYTE_1]
     661         190 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
     662         190 :     parameter_dump_packet.time[BYTE_2]
     663         190 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
     664         190 :     parameter_dump_packet.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
     665         190 :     parameter_dump_packet.time[BYTE_4]
     666         190 :         = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
     667         190 :     parameter_dump_packet.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
     668             :     // SEND DATA
     669         190 :     status
     670         190 :         = rtems_message_queue_send(queue_id, &parameter_dump_packet, sizeof(parameter_dump_packet));
     671             :     DEBUG_CHECK_STATUS(status);
     672             : 
     673         190 :     return status;
     674             : }
     675             : 
     676             : //***********************
     677             : // NORMAL MODE PARAMETERS
     678             : // See https://hephaistos.lpp.polytechnique.fr/redmine/issues/481
     679             : // and https://hephaistos.lpp.polytechnique.fr/redmine/issues/482
     680          56 : int check_normal_par_consistency(const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id)
     681             : {
     682             :     unsigned char msb;
     683             :     unsigned char lsb;
     684             :     int flag;
     685             :     float aux;
     686             :     rtems_status_code status;
     687             : 
     688             :     unsigned int sy_lfr_n_swf_l;
     689             :     unsigned int sy_lfr_n_swf_p;
     690             :     unsigned int sy_lfr_n_asm_p;
     691             :     unsigned char sy_lfr_n_bp_p0;
     692             :     unsigned char sy_lfr_n_bp_p1;
     693             : 
     694          56 :     flag = LFR_SUCCESSFUL;
     695             : 
     696             :     //***************
     697             :     // get parameters
     698          56 :     msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L];
     699          56 :     lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L + 1];
     700          56 :     sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
     701             : 
     702          56 :     msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P];
     703          56 :     lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P + 1];
     704          56 :     sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
     705             : 
     706          56 :     msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P];
     707          56 :     lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P + 1];
     708          56 :     sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
     709             : 
     710          56 :     sy_lfr_n_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P0];
     711             : 
     712          56 :     sy_lfr_n_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P1];
     713             : 
     714             :     //******************
     715             :     // check consistency
     716             :     // sy_lfr_n_swf_l
     717          56 :     if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
     718             :     {
     719           3 :         status = send_tm_lfr_tc_exe_inconsistent(
     720             :             TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, (char)sy_lfr_n_swf_l);
     721             :         DEBUG_CHECK_STATUS(status);
     722           3 :         flag = WRONG_APP_DATA;
     723             :     }
     724             :     // sy_lfr_n_swf_p
     725          56 :     if (flag == LFR_SUCCESSFUL && sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P)
     726             :     {
     727           6 :         status = send_tm_lfr_tc_exe_inconsistent(
     728             :             TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p);
     729             :         DEBUG_CHECK_STATUS(status);
     730           6 :         flag = WRONG_APP_DATA;
     731             :     }
     732             :     // sy_lfr_n_bp_p0
     733          56 :     if (flag == LFR_SUCCESSFUL && sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
     734             :     {
     735           1 :         status = send_tm_lfr_tc_exe_inconsistent(
     736             :             TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0);
     737             :         DEBUG_CHECK_STATUS(status);
     738           1 :         flag = WRONG_APP_DATA;
     739             :     }
     740             :     // sy_lfr_n_asm_p
     741          56 :     if (flag == LFR_SUCCESSFUL && sy_lfr_n_asm_p == 0)
     742             :     {
     743           1 :         status = send_tm_lfr_tc_exe_inconsistent(
     744             :             TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p);
     745             :         DEBUG_CHECK_STATUS(status);
     746           1 :         flag = WRONG_APP_DATA;
     747             :     }
     748             :     // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
     749          56 :     if (flag == LFR_SUCCESSFUL)
     750             :     {
     751          45 :         aux = ((float)sy_lfr_n_asm_p / sy_lfr_n_bp_p0)
     752             :             - floorf((float)sy_lfr_n_asm_p / (float)sy_lfr_n_bp_p0);
     753          45 :         if (aux > FLOAT_EQUAL_ZERO)
     754             :         {
     755           1 :             status = send_tm_lfr_tc_exe_inconsistent(
     756             :                 TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p);
     757             :             DEBUG_CHECK_STATUS(status);
     758           1 :             flag = WRONG_APP_DATA;
     759             :         }
     760             :     }
     761             :     // sy_lfr_n_bp_p1
     762          56 :     if (flag == LFR_SUCCESSFUL && sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
     763             :     {
     764             : 
     765           1 :         status = send_tm_lfr_tc_exe_inconsistent(
     766             :             TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1);
     767             :         DEBUG_CHECK_STATUS(status);
     768           1 :         flag = WRONG_APP_DATA;
     769             :     }
     770             :     // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
     771          56 :     if (flag == LFR_SUCCESSFUL)
     772             :     {
     773          43 :         aux = ((float)sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0) - floorf(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
     774          43 :         if (aux > FLOAT_EQUAL_ZERO)
     775             :         {
     776           1 :             status = send_tm_lfr_tc_exe_inconsistent(
     777             :                 TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1);
     778             :             DEBUG_CHECK_STATUS(status);
     779           1 :             flag = LFR_DEFAULT;
     780             :         }
     781             :     }
     782             : 
     783          56 :     return flag;
     784             : }
     785             : 
     786           0 : int set_sy_lfr_n_swf_l(const ccsdsTelecommandPacket_t* const TC)
     787             : {
     788             :     /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
     789             :      *
     790             :      * @param TC points to the TeleCommand packet that is being processed
     791             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     792             :      *
     793             :      */
     794             : 
     795          42 :     parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L];
     796          42 :     parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L + 1];
     797             : 
     798          42 :     return LFR_SUCCESSFUL;
     799             : }
     800             : 
     801           0 : int set_sy_lfr_n_swf_p(const ccsdsTelecommandPacket_t* const TC)
     802             : {
     803             :     /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
     804             :      *
     805             :      * @param TC points to the TeleCommand packet that is being processed
     806             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     807             :      *
     808             :      */
     809             : 
     810          42 :     parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P];
     811          42 :     parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P + 1];
     812             : 
     813          42 :     return LFR_SUCCESSFUL;
     814             : }
     815             : 
     816           0 : int set_sy_lfr_n_asm_p(const ccsdsTelecommandPacket_t* const TC)
     817             : {
     818             :     /** This function sets the time between two full spectral matrices transmission, in s
     819             :      * (SY_LFR_N_ASM_P).
     820             :      *
     821             :      * @param TC points to the TeleCommand packet that is being processed
     822             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     823             :      *
     824             :      */
     825             : 
     826          42 :     parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P];
     827          42 :     parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P + 1];
     828             : 
     829          42 :     return LFR_SUCCESSFUL;
     830             : }
     831             : 
     832           0 : int set_sy_lfr_n_bp_p0(const ccsdsTelecommandPacket_t* const TC)
     833             : {
     834             :     /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
     835             :      *
     836             :      * @param TC points to the TeleCommand packet that is being processed
     837             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     838             :      *
     839             :      */
     840             : 
     841          42 :     parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P0];
     842             : 
     843          42 :     return LFR_SUCCESSFUL;
     844             : }
     845             : 
     846           0 : int set_sy_lfr_n_bp_p1(const ccsdsTelecommandPacket_t* const TC)
     847             : {
     848             :     /** This function sets the time between two basic parameter sets (autocorrelation +
     849             :      * crosscorrelation), in s (sy_lfr_n_bp_p1).
     850             :      *
     851             :      * @param TC points to the TeleCommand packet that is being processed
     852             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     853             :      *
     854             :      */
     855             : 
     856          42 :     parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P1];
     857             : 
     858          42 :     return LFR_SUCCESSFUL;
     859             : }
     860             : 
     861           0 : int set_sy_lfr_n_cwf_long_f3(const ccsdsTelecommandPacket_t* const TC)
     862             : {
     863             :     /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
     864             :      *
     865             :      * @param TC points to the TeleCommand packet that is being processed
     866             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     867             :      *
     868             :      */
     869             : 
     870          42 :     parameter_dump_packet.sy_lfr_n_cwf_long_f3
     871          42 :         = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3] & BIT_CWF_LONG_F3;
     872             : 
     873          42 :     return LFR_SUCCESSFUL;
     874             : }
     875             : 
     876             : //**********************
     877             : // BURST MODE PARAMETERS
     878             : 
     879           0 : int set_sy_lfr_b_bp_p0(const ccsdsTelecommandPacket_t* const TC)
     880             : {
     881             :     /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
     882             :      *
     883             :      * @param TC points to the TeleCommand packet that is being processed
     884             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     885             :      *
     886             :      */
     887             : 
     888          80 :     parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
     889             : 
     890          80 :     return LFR_SUCCESSFUL;
     891             : }
     892             : 
     893           0 : int set_sy_lfr_b_bp_p1(const ccsdsTelecommandPacket_t* const TC)
     894             : {
     895             :     /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
     896             :      *
     897             :      * @param TC points to the TeleCommand packet that is being processed
     898             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     899             :      *
     900             :      */
     901             : 
     902          80 :     parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P1];
     903             : 
     904          80 :     return LFR_SUCCESSFUL;
     905             : }
     906             : 
     907             : //*********************
     908             : // SBM1 MODE PARAMETERS
     909             : 
     910           0 : int set_sy_lfr_s1_bp_p0(const ccsdsTelecommandPacket_t* const TC)
     911             : {
     912             :     /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
     913             :      *
     914             :      * @param TC points to the TeleCommand packet that is being processed
     915             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     916             :      *
     917             :      */
     918             : 
     919          80 :     parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P0];
     920             : 
     921          80 :     return LFR_SUCCESSFUL;
     922             : }
     923             : 
     924           0 : int set_sy_lfr_s1_bp_p1(const ccsdsTelecommandPacket_t* const TC)
     925             : {
     926             :     /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
     927             :      *
     928             :      * @param TC points to the TeleCommand packet that is being processed
     929             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     930             :      *
     931             :      */
     932             : 
     933          80 :     parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P1];
     934             : 
     935          80 :     return LFR_SUCCESSFUL;
     936             : }
     937             : 
     938             : //*********************
     939             : // SBM2 MODE PARAMETERS
     940             : 
     941           0 : int set_sy_lfr_s2_bp_p0(const ccsdsTelecommandPacket_t* const TC)
     942             : {
     943             :     /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
     944             :      *
     945             :      * @param TC points to the TeleCommand packet that is being processed
     946             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     947             :      *
     948             :      */
     949             : 
     950          77 :     parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
     951             : 
     952          77 :     return LFR_SUCCESSFUL;
     953             : }
     954             : 
     955           0 : int set_sy_lfr_s2_bp_p1(const ccsdsTelecommandPacket_t* const TC)
     956             : {
     957             :     /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
     958             :      *
     959             :      * @param TC points to the TeleCommand packet that is being processed
     960             :      * @param queue_id is the id of the queue which handles TM related to this execution step
     961             :      *
     962             :      */
     963             : 
     964          77 :     parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
     965             : 
     966          77 :     return LFR_SUCCESSFUL;
     967             : }
     968             : 
     969             : //*******************
     970             : // TC_LFR_UPDATE_INFO
     971             : 
     972          33 : unsigned int check_update_info_hk_lfr_mode(unsigned char mode)
     973             : {
     974          33 :     unsigned int status = LFR_DEFAULT;
     975             : 
     976          33 :     if ((mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL) || (mode == LFR_MODE_BURST)
     977             :         || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
     978             :     {
     979          33 :         status = LFR_SUCCESSFUL;
     980             :     }
     981             :     else
     982             :     {
     983           0 :         status = LFR_DEFAULT;
     984             :     }
     985             : 
     986          33 :     return status;
     987             : }
     988             : 
     989          33 : unsigned int check_update_info_hk_tds_mode(unsigned char mode)
     990             : {
     991          33 :     unsigned int status = LFR_DEFAULT;
     992             : 
     993          33 :     if ((mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL) || (mode == TDS_MODE_BURST)
     994             :         || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2) || (mode == TDS_MODE_LFM))
     995             :     {
     996          33 :         status = LFR_SUCCESSFUL;
     997             :     }
     998             :     else
     999             :     {
    1000           0 :         status = LFR_DEFAULT;
    1001             :     }
    1002             : 
    1003          33 :     return status;
    1004             : }
    1005             : 
    1006          33 : unsigned int check_update_info_hk_thr_mode(unsigned char mode)
    1007             : {
    1008          33 :     unsigned int status = LFR_DEFAULT;
    1009             : 
    1010          33 :     if ((mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL) || (mode == THR_MODE_BURST))
    1011             :     {
    1012          33 :         status = LFR_SUCCESSFUL;
    1013             :     }
    1014             :     else
    1015             :     {
    1016           0 :         status = LFR_DEFAULT;
    1017             :     }
    1018             : 
    1019          33 :     return status;
    1020             : }
    1021             : 
    1022         432 : void set_hk_lfr_sc_rw_f_flag(unsigned char wheel, unsigned char freq, float value)
    1023             : {
    1024             :     unsigned char flag;
    1025             :     unsigned char flagPosInByte;
    1026             :     unsigned char newFlag;
    1027             :     unsigned char flagMask;
    1028             : 
    1029             :     // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not
    1030             :     // filtered
    1031         432 :     if (isnan(value))
    1032             :     {
    1033         415 :         flag = FLAG_NAN;
    1034             :     }
    1035             :     else
    1036             :     {
    1037          17 :         flag = FLAG_IAN;
    1038             :     }
    1039             : 
    1040         432 :     switch (wheel)
    1041             :     {
    1042             :         case WHEEL_1:
    1043         108 :             flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
    1044         108 :             flagMask = (unsigned char)(~(1 << flagPosInByte));
    1045         108 :             newFlag = (unsigned char)(flag << flagPosInByte);
    1046         108 :             housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags
    1047             :                 = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
    1048         108 :             break;
    1049             :         case WHEEL_2:
    1050         108 :             flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
    1051         108 :             flagMask = (unsigned char)(~(1 << flagPosInByte));
    1052         108 :             newFlag = (unsigned char)(flag << flagPosInByte);
    1053         108 :             housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags
    1054             :                 = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
    1055         108 :             break;
    1056             :         case WHEEL_3:
    1057         108 :             flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
    1058         108 :             flagMask = (unsigned char)(~(1 << flagPosInByte));
    1059         108 :             newFlag = (unsigned char)(flag << flagPosInByte);
    1060         108 :             housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags
    1061             :                 = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
    1062         108 :             break;
    1063             :         case WHEEL_4:
    1064         108 :             flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
    1065         108 :             flagMask = (unsigned char)(~(1 << flagPosInByte));
    1066         108 :             newFlag = (unsigned char)(flag << flagPosInByte);
    1067         108 :             housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags
    1068             :                 = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
    1069             :             break;
    1070             :         default:
    1071             :             break;
    1072             :     }
    1073         432 : }
    1074             : 
    1075          27 : void set_hk_lfr_sc_rw_f_flags(void)
    1076             : {
    1077             :     // RW1
    1078          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1);
    1079          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2);
    1080          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3);
    1081          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4);
    1082             : 
    1083             :     // RW2
    1084          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1);
    1085          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2);
    1086          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3);
    1087          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4);
    1088             : 
    1089             :     // RW3
    1090          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1);
    1091          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2);
    1092          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3);
    1093          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4);
    1094             : 
    1095             :     // RW4
    1096          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1);
    1097          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2);
    1098          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3);
    1099          27 :     set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4);
    1100          27 : }
    1101             : 
    1102         438 : int check_sy_lfr_rw_f(const ccsdsTelecommandPacket_t* const TC, int offset, int* pos, float* value)
    1103             : {
    1104             :     float rw_k;
    1105             :     int ret;
    1106             : 
    1107         438 :     ret = LFR_SUCCESSFUL;
    1108         438 :     rw_k = INIT_FLOAT;
    1109             : 
    1110         438 :     copyFloatByChar((unsigned char*)&rw_k, &TC->packetID[offset]);
    1111             : 
    1112         438 :     *pos = offset;
    1113         438 :     *value = rw_k;
    1114             : 
    1115         438 :     if (rw_k < MIN_SY_LFR_RW_F)
    1116             :     {
    1117           6 :         ret = WRONG_APP_DATA;
    1118             :     }
    1119             : 
    1120         438 :     return ret;
    1121             : }
    1122             : 
    1123          33 : int check_all_sy_lfr_rw_f(const ccsdsTelecommandPacket_t* const TC, int* pos, float* value)
    1124             : {
    1125          33 :     int ret = LFR_SUCCESSFUL;
    1126             : 
    1127             :     //****
    1128             :     //****
    1129             :     // RW1
    1130          33 :     ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value); // F1
    1131          33 :     if (ret == LFR_SUCCESSFUL) // F2
    1132             :     {
    1133          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value);
    1134             :     }
    1135          33 :     if (ret == LFR_SUCCESSFUL) // F3
    1136             :     {
    1137          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value);
    1138             :     }
    1139          33 :     if (ret == LFR_SUCCESSFUL) // F4
    1140             :     {
    1141          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4, pos, value);
    1142             :     }
    1143             : 
    1144             :     //****
    1145             :     //****
    1146             :     // RW2
    1147          33 :     if (ret == LFR_SUCCESSFUL) // F1
    1148             :     {
    1149          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value);
    1150             :     }
    1151          33 :     if (ret == LFR_SUCCESSFUL) // F2
    1152             :     {
    1153          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value);
    1154             :     }
    1155          33 :     if (ret == LFR_SUCCESSFUL) // F3
    1156             :     {
    1157          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value);
    1158             :     }
    1159          33 :     if (ret == LFR_SUCCESSFUL) // F4
    1160             :     {
    1161          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4, pos, value);
    1162             :     }
    1163             : 
    1164             :     //****
    1165             :     //****
    1166             :     // RW3
    1167          33 :     if (ret == LFR_SUCCESSFUL) // F1
    1168             :     {
    1169          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value);
    1170             :     }
    1171          33 :     if (ret == LFR_SUCCESSFUL) // F2
    1172             :     {
    1173          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value);
    1174             :     }
    1175          33 :     if (ret == LFR_SUCCESSFUL) // F3
    1176             :     {
    1177          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value);
    1178             :     }
    1179          33 :     if (ret == LFR_SUCCESSFUL) // F4
    1180             :     {
    1181          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4, pos, value);
    1182             :     }
    1183             : 
    1184             :     //****
    1185             :     //****
    1186             :     // RW4
    1187          33 :     if (ret == LFR_SUCCESSFUL) // F1
    1188             :     {
    1189          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value);
    1190             :     }
    1191          33 :     if (ret == LFR_SUCCESSFUL) // F2
    1192             :     {
    1193          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value);
    1194             :     }
    1195          33 :     if (ret == LFR_SUCCESSFUL) // F3
    1196             :     {
    1197          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value);
    1198             :     }
    1199          33 :     if (ret == LFR_SUCCESSFUL) // F4
    1200             :     {
    1201          27 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value);
    1202             :     }
    1203             : 
    1204          33 :     return ret;
    1205             : }
    1206             : 
    1207          27 : void getReactionWheelsFrequencies(const ccsdsTelecommandPacket_t* const TC)
    1208             : {
    1209             :     /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and
    1210             :      * copy the values locally.
    1211             :      *
    1212             :      * @param TC points to the TeleCommand packet that is being processed
    1213             :      *
    1214             :      */
    1215             :     // pointer to the beginning of the incoming TC packet
    1216          27 :     const unsigned char* const bytePosPtr = (const unsigned char* const)&TC->packetID;
    1217             : 
    1218             :     // rw1_f
    1219          27 :     copyFloatByChar(
    1220             :         (unsigned char*)&rw_f.cp_rpw_sc_rw1_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1]);
    1221          27 :     copyFloatByChar(
    1222             :         (unsigned char*)&rw_f.cp_rpw_sc_rw1_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2]);
    1223          27 :     copyFloatByChar(
    1224             :         (unsigned char*)&rw_f.cp_rpw_sc_rw1_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3]);
    1225          27 :     copyFloatByChar(
    1226             :         (unsigned char*)&rw_f.cp_rpw_sc_rw1_f4, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4]);
    1227             : 
    1228             :     // rw2_f
    1229          27 :     copyFloatByChar(
    1230             :         (unsigned char*)&rw_f.cp_rpw_sc_rw2_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1]);
    1231          27 :     copyFloatByChar(
    1232             :         (unsigned char*)&rw_f.cp_rpw_sc_rw2_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2]);
    1233          27 :     copyFloatByChar(
    1234             :         (unsigned char*)&rw_f.cp_rpw_sc_rw2_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3]);
    1235          27 :     copyFloatByChar(
    1236             :         (unsigned char*)&rw_f.cp_rpw_sc_rw2_f4, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4]);
    1237             : 
    1238             :     // rw3_f
    1239          27 :     copyFloatByChar(
    1240             :         (unsigned char*)&rw_f.cp_rpw_sc_rw3_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1]);
    1241          27 :     copyFloatByChar(
    1242             :         (unsigned char*)&rw_f.cp_rpw_sc_rw3_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2]);
    1243          27 :     copyFloatByChar(
    1244             :         (unsigned char*)&rw_f.cp_rpw_sc_rw3_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3]);
    1245          27 :     copyFloatByChar(
    1246             :         (unsigned char*)&rw_f.cp_rpw_sc_rw3_f4, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4]);
    1247             : 
    1248             :     // rw4_f
    1249          27 :     copyFloatByChar(
    1250             :         (unsigned char*)&rw_f.cp_rpw_sc_rw4_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1]);
    1251          27 :     copyFloatByChar(
    1252             :         (unsigned char*)&rw_f.cp_rpw_sc_rw4_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2]);
    1253          27 :     copyFloatByChar(
    1254             :         (unsigned char*)&rw_f.cp_rpw_sc_rw4_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3]);
    1255          27 :     copyFloatByChar(
    1256             :         (unsigned char*)&rw_f.cp_rpw_sc_rw4_f4, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4]);
    1257             : 
    1258             :     // test each reaction wheel frequency value. NaN means that the frequency is not filtered
    1259          27 : }
    1260             : 
    1261        1296 : void setFBinMask(
    1262             :     unsigned char* fbins_mask, float rw_f_comp, unsigned char deltaFreq, float sy_lfr_rw_k)
    1263             : {
    1264             :     /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been
    1265             :      * received.
    1266             :      *
    1267             :      * @param fbins_mask
    1268             :      * @param rw_f is the reaction wheel frequency to filter
    1269             :      * @param delta_f is the frequency step between the frequency bins, it depends on the frequency
    1270             :      * channel
    1271             :      * @param flag [true] filtering enabled [false] filtering disabled
    1272             :      *
    1273             :      * @return void
    1274             :      *
    1275             :      */
    1276             : 
    1277             :     float f_RW_min;
    1278             :     float f_RW_MAX;
    1279             :     float fi_min;
    1280             :     float fi_MAX;
    1281             :     float fi;
    1282             :     float deltaBelow;
    1283             :     float deltaAbove;
    1284             :     float freqToFilterOut;
    1285             :     int binBelow;
    1286        1296 :     int binAbove = 0;
    1287             :     int closestBin;
    1288        1296 :     unsigned int whichByte = 0;
    1289             :     int selectedByte;
    1290        1296 :     int bin = 0;
    1291             :     int binToRemove[NB_BINS_TO_REMOVE];
    1292             :     int k;
    1293        1296 :     bool filteringSet = false;
    1294             : 
    1295        5184 :     for (k = 0; k < NB_BINS_TO_REMOVE; k++)
    1296             :     {
    1297        3888 :         binToRemove[k] = -1;
    1298             :     }
    1299             : 
    1300        1296 :     if (!isnan(rw_f_comp))
    1301             :     {
    1302             :         // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
    1303          51 :         f_RW_min = rw_f_comp - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
    1304          51 :         f_RW_MAX = rw_f_comp + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
    1305             : 
    1306          51 :         freqToFilterOut = f_RW_min;
    1307         168 :         while (filteringSet == false)
    1308             :         {
    1309             :             // compute the index of the frequency bin immediately below rw_f
    1310          66 :             binBelow = (int)(floor(((double)freqToFilterOut) / ((double)deltaFreq)));
    1311          66 :             deltaBelow = freqToFilterOut - (float)(binBelow * deltaFreq);
    1312             : 
    1313             :             // compute the index of the frequency bin immediately above rw_f
    1314          66 :             binAbove = (int)(ceil(((double)freqToFilterOut) / ((double)deltaFreq)));
    1315          66 :             deltaAbove = (float)(binAbove * deltaFreq) - freqToFilterOut;
    1316             : 
    1317             :             // search the closest bin
    1318          66 :             if (deltaAbove > deltaBelow)
    1319             :             {
    1320          15 :                 closestBin = binBelow;
    1321             :             }
    1322             :             else
    1323             :             {
    1324          51 :                 closestBin = binAbove;
    1325             :             }
    1326             : 
    1327             :             // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
    1328          66 :             fi = (float)closestBin * deltaFreq;
    1329          66 :             fi_min = fi - ((float)deltaFreq * (float)FI_INTERVAL_COEFF);
    1330          66 :             fi_MAX = fi + ((float)deltaFreq * (float)FI_INTERVAL_COEFF);
    1331             : 
    1332             :             //**************************************************************************************
    1333             :             // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
    1334             :             // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
    1335             :             //**************************************************************************************
    1336             : 
    1337             :             // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
    1338             :             // => remove f_(i), f_(i-1) and f_(i+1)
    1339          99 :             if ((freqToFilterOut > fi_min) && (freqToFilterOut < fi_MAX))
    1340             :             {
    1341          33 :                 binToRemove[0] = (closestBin - 1) - 1;
    1342          33 :                 binToRemove[1] = closestBin - 1;
    1343          33 :                 binToRemove[2] = (closestBin + 1) - 1;
    1344             :             }
    1345             :             // 2. ELSE
    1346             :             // => remove the two f_(i) which are around f_RW
    1347             :             else
    1348             :             {
    1349          33 :                 binToRemove[0] = binBelow - 1;
    1350          33 :                 binToRemove[1] = binAbove - 1;
    1351          33 :                 binToRemove[2] = -1;
    1352             :             }
    1353             : 
    1354         264 :             for (k = 0; k < NB_BINS_TO_REMOVE; k++)
    1355             :             {
    1356         198 :                 bin = binToRemove[k];
    1357         198 :                 if ((bin >= BIN_MIN) && (bin <= BIN_MAX))
    1358             :                 {
    1359          88 :                     whichByte = (bin >> SHIFT_3_BITS); // division by 8
    1360          88 :                     selectedByte = (1 << (bin - (whichByte * BITS_PER_BYTE)));
    1361         176 :                     fbins_mask[BYTES_PER_MASK - 1 - whichByte]
    1362          88 :                         = fbins_mask[BYTES_PER_MASK - 1 - whichByte]
    1363          88 :                         & ((unsigned char)(~selectedByte)); // bytes are ordered MSB first in the
    1364             :                                                             // packets
    1365             :                 }
    1366             :             }
    1367             : 
    1368             :             // update freqToFilterOut
    1369          66 :             if (freqToFilterOut == f_RW_MAX)
    1370             :             {
    1371          51 :                 filteringSet = true; // end of the loop
    1372             :             }
    1373             :             else
    1374             :             {
    1375          15 :                 freqToFilterOut = freqToFilterOut + deltaFreq;
    1376             :             }
    1377             : 
    1378          66 :             if (freqToFilterOut > f_RW_MAX)
    1379             :             {
    1380          15 :                 freqToFilterOut = f_RW_MAX;
    1381             :             }
    1382             :         }
    1383             :     }
    1384        1296 : }
    1385             : 
    1386          81 : void build_sy_lfr_rw_mask(unsigned int channel)
    1387             : {
    1388             :     unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
    1389          81 :     unsigned char* maskPtr = NULL;
    1390          81 :     unsigned char deltaF = (unsigned char)DELTAF_F2;
    1391             :     unsigned k;
    1392             : 
    1393          81 :     switch (channel)
    1394             :     {
    1395             :         case CHANNELF0:
    1396          27 :             maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
    1397          27 :             deltaF = (unsigned char)DELTAF_F0;
    1398          27 :             break;
    1399             :         case CHANNELF1:
    1400          27 :             maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
    1401          27 :             deltaF = (unsigned char)DELTAF_F1;
    1402          27 :             break;
    1403             :         case CHANNELF2:
    1404          27 :             maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
    1405          27 :             deltaF = (unsigned char)DELTAF_F2;
    1406             :             break;
    1407             :         default:
    1408             :             break;
    1409             :     }
    1410             : 
    1411        1377 :     for (k = 0; k < BYTES_PER_MASK; k++)
    1412             :     {
    1413        1296 :         local_rw_fbins_mask[k] = INT8_ALL_F;
    1414             :     }
    1415             : 
    1416             :     // RW1
    1417          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1);
    1418          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2);
    1419          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3);
    1420          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4);
    1421             : 
    1422             :     // RW2
    1423          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1);
    1424          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2);
    1425          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3);
    1426          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4);
    1427             : 
    1428             :     // RW3
    1429          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1);
    1430          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2);
    1431          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3);
    1432          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4);
    1433             : 
    1434             :     // RW4
    1435          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1);
    1436          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2);
    1437          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3);
    1438          81 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4);
    1439             : 
    1440             :     // update the value of the fbins related to reaction wheels frequency filtering
    1441          81 :     if (maskPtr != NULL)
    1442             :     {
    1443        1377 :         for (k = 0; k < BYTES_PER_MASK; k++)
    1444             :         {
    1445        1296 :             maskPtr[k] = local_rw_fbins_mask[k];
    1446             :         }
    1447             :     }
    1448          81 : }
    1449             : 
    1450          27 : void build_sy_lfr_rw_masks(void)
    1451             : {
    1452          27 :     build_sy_lfr_rw_mask(CHANNELF0);
    1453          27 :     build_sy_lfr_rw_mask(CHANNELF1);
    1454          27 :     build_sy_lfr_rw_mask(CHANNELF2);
    1455          27 : }
    1456             : 
    1457         127 : void merge_fbins_masks(void)
    1458             : {
    1459         127 :     const unsigned char* const fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
    1460         127 :     const unsigned char* const fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
    1461         127 :     const unsigned char* const fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
    1462         127 :     const unsigned char* const rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
    1463         127 :     const unsigned char* const rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
    1464         127 :     const unsigned char* const rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
    1465             : 
    1466             : 
    1467        2159 :     for (unsigned char k = 0; k < BYTES_PER_MASK; k++)
    1468             :     {
    1469        2032 :         fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
    1470        2032 :         fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
    1471        2032 :         fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
    1472             :     }
    1473         127 : }
    1474             : 
    1475             : //***********
    1476             : // FBINS MASK
    1477             : 
    1478           0 : int set_sy_lfr_fbins(const ccsdsTelecommandPacket_t* const TC)
    1479             : {
    1480          82 :     unsigned char* const fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
    1481          82 :     const unsigned char* const fbins_mask_TC = TC->dataAndCRC;
    1482             : 
    1483        4018 :     for (unsigned int k = 0; k < BYTES_PER_MASKS_SET; k++)
    1484             :     {
    1485        3936 :         fbins_mask_dump[k] = fbins_mask_TC[k];
    1486             :     }
    1487             : 
    1488          82 :     return LFR_SUCCESSFUL;
    1489             : }
    1490             : 
    1491             : //***************************
    1492             : // TC_LFR_LOAD_PAS_FILTER_PAR
    1493             : 
    1494        1568 : int check_sy_lfr_rw_k(const ccsdsTelecommandPacket_t* const TC, int offset, int* pos, float* value)
    1495             : {
    1496             :     float rw_k;
    1497             :     int ret;
    1498             : 
    1499        1568 :     ret = LFR_SUCCESSFUL;
    1500        1568 :     rw_k = INIT_FLOAT;
    1501             : 
    1502        1568 :     copyFloatByChar((unsigned char*)&rw_k, &TC->dataAndCRC[offset]);
    1503             : 
    1504        1568 :     *pos = offset;
    1505        1568 :     *value = rw_k;
    1506             : 
    1507        1568 :     if (rw_k < MIN_SY_LFR_RW_F)
    1508             :     {
    1509           0 :         ret = WRONG_APP_DATA;
    1510             :     }
    1511             : 
    1512        1568 :     return ret;
    1513             : }
    1514             : 
    1515          98 : int check_all_sy_lfr_rw_k(const ccsdsTelecommandPacket_t* const TC, int* pos, float* value)
    1516             : {
    1517          98 :     int ret = LFR_SUCCESSFUL;
    1518             : 
    1519             :     //****
    1520             :     //****
    1521             :     // RW1
    1522          98 :     ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value); // K1
    1523          98 :     if (ret == LFR_SUCCESSFUL) // K2
    1524             :     {
    1525          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value);
    1526             :     }
    1527          98 :     if (ret == LFR_SUCCESSFUL) // K3
    1528             :     {
    1529          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value);
    1530             :     }
    1531          98 :     if (ret == LFR_SUCCESSFUL) // K4
    1532             :     {
    1533          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value);
    1534             :     }
    1535             : 
    1536             :     //****
    1537             :     //****
    1538             :     // RW2
    1539          98 :     if (ret == LFR_SUCCESSFUL) // K1
    1540             :     {
    1541          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value);
    1542             :     }
    1543          98 :     if (ret == LFR_SUCCESSFUL) // K2
    1544             :     {
    1545          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value);
    1546             :     }
    1547          98 :     if (ret == LFR_SUCCESSFUL) // K3
    1548             :     {
    1549          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value);
    1550             :     }
    1551          98 :     if (ret == LFR_SUCCESSFUL) // K4
    1552             :     {
    1553          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value);
    1554             :     }
    1555             : 
    1556             :     //****
    1557             :     //****
    1558             :     // RW3
    1559          98 :     if (ret == LFR_SUCCESSFUL) // K1
    1560             :     {
    1561          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value);
    1562             :     }
    1563          98 :     if (ret == LFR_SUCCESSFUL) // K2
    1564             :     {
    1565          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value);
    1566             :     }
    1567          98 :     if (ret == LFR_SUCCESSFUL) // K3
    1568             :     {
    1569          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value);
    1570             :     }
    1571          98 :     if (ret == LFR_SUCCESSFUL) // K4
    1572             :     {
    1573          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value);
    1574             :     }
    1575             : 
    1576             :     //****
    1577             :     //****
    1578             :     // RW4
    1579          98 :     if (ret == LFR_SUCCESSFUL) // K1
    1580             :     {
    1581          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value);
    1582             :     }
    1583          98 :     if (ret == LFR_SUCCESSFUL) // K2
    1584             :     {
    1585          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value);
    1586             :     }
    1587          98 :     if (ret == LFR_SUCCESSFUL) // K3
    1588             :     {
    1589          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value);
    1590             :     }
    1591          98 :     if (ret == LFR_SUCCESSFUL) // K4
    1592             :     {
    1593          98 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value);
    1594             :     }
    1595             : 
    1596          98 :     return ret;
    1597             : }
    1598             : 
    1599         110 : int check_sy_lfr_filter_parameters(const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id)
    1600             : {
    1601         110 :     int flag = LFR_SUCCESSFUL;
    1602             : 
    1603             :     unsigned char sy_lfr_pas_filter_modulus;
    1604         110 :     float sy_lfr_pas_filter_tbad = 0.f;
    1605             :     unsigned char sy_lfr_pas_filter_offset;
    1606         110 :     float sy_lfr_pas_filter_shift = 0.f;
    1607         110 :     float sy_lfr_sc_rw_delta_f = 0.f;
    1608         110 :     const char* parPtr = NULL;
    1609         110 :     int datafield_pos = 0;
    1610         110 :     float rw_k = 0.f;
    1611             : 
    1612             : 
    1613             :     //***************
    1614             :     // get parameters
    1615         110 :     sy_lfr_pas_filter_modulus = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS];
    1616         110 :     copyFloatByChar((unsigned char*)&sy_lfr_pas_filter_tbad,
    1617             :         &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD]);
    1618         110 :     sy_lfr_pas_filter_offset = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET];
    1619         110 :     copyFloatByChar((unsigned char*)&sy_lfr_pas_filter_shift,
    1620             :         &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT]);
    1621         110 :     copyFloatByChar(
    1622             :         (unsigned char*)&sy_lfr_sc_rw_delta_f, &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F]);
    1623             : 
    1624             :     //******************
    1625             :     // CHECK CONSISTENCY
    1626             : 
    1627             :     //**************************
    1628             :     // sy_lfr_pas_filter_enabled
    1629             :     // nothing to check, value is 0 or 1
    1630             : 
    1631             :     //**************************
    1632             :     // sy_lfr_pas_filter_modulus
    1633         110 :     if ((sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS)
    1634             :         || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS))
    1635             :     {
    1636           2 :         DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
    1637             :             DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus));
    1638           2 :         flag = WRONG_APP_DATA;
    1639             :     }
    1640             : 
    1641             :     //***********************
    1642             :     // sy_lfr_pas_filter_tbad
    1643         110 :     if (flag == LFR_SUCCESSFUL)
    1644             :     {
    1645         216 :         if ((sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD)
    1646         108 :             || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD))
    1647             :         {
    1648           2 :             parPtr = (const char*)&sy_lfr_pas_filter_tbad;
    1649           2 :             DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
    1650             :                 DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE]));
    1651           2 :             flag = WRONG_APP_DATA;
    1652             :         }
    1653             :     }
    1654             : 
    1655             :     //*************************
    1656             :     // sy_lfr_pas_filter_offset
    1657         110 :     if (flag == LFR_SUCCESSFUL && sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET)
    1658             :     {
    1659           1 :         DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
    1660             :             DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset));
    1661           1 :         flag = WRONG_APP_DATA;
    1662             :     }
    1663             : 
    1664             :     //************************
    1665             :     // sy_lfr_pas_filter_shift
    1666         110 :     if (flag == LFR_SUCCESSFUL)
    1667             :     {
    1668         210 :         if ((sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT)
    1669         105 :             || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT))
    1670             :         {
    1671           1 :             parPtr = (const char*)&sy_lfr_pas_filter_shift;
    1672           1 :             DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
    1673             :                 DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE]));
    1674           1 :             flag = WRONG_APP_DATA;
    1675             :         }
    1676             :     }
    1677             : 
    1678             :     //*************************************
    1679             :     // check global coherency of the values
    1680         110 :     if (flag == LFR_SUCCESSFUL)
    1681             :     {
    1682         104 :         if ((sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) >= sy_lfr_pas_filter_modulus)
    1683             :         {
    1684           6 :             DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
    1685             :                 DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET,
    1686             :                 sy_lfr_pas_filter_modulus));
    1687           6 :             flag = WRONG_APP_DATA;
    1688             :         }
    1689             :     }
    1690             : 
    1691             :     //*********************
    1692             :     // sy_lfr_sc_rw_delta_f
    1693         110 :     if (flag == LFR_SUCCESSFUL && sy_lfr_sc_rw_delta_f < MIN_SY_LFR_SC_RW_DELTA_F)
    1694             :     {
    1695           0 :         parPtr = (const char*)&sy_lfr_sc_rw_delta_f;
    1696           0 :         DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
    1697             :             DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE]));
    1698           0 :         flag = WRONG_APP_DATA;
    1699             :     }
    1700             : 
    1701             :     //************
    1702             :     // sy_lfr_rw_k
    1703         110 :     if (flag == LFR_SUCCESSFUL)
    1704             :     {
    1705          98 :         flag = check_all_sy_lfr_rw_k(TC, &datafield_pos, &rw_k);
    1706          98 :         if (flag != LFR_SUCCESSFUL)
    1707             :         {
    1708           0 :             parPtr = (const char*)&sy_lfr_pas_filter_shift;
    1709           0 :             DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(
    1710             :                 TC, queue_id, datafield_pos + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE]));
    1711             :         }
    1712             :     }
    1713             : 
    1714         110 :     return flag;
    1715             : }
    1716             : 
    1717             : 
    1718           6 : void interpolate_calibration_matrix(
    1719             :     float* matrix, unsigned int floats_per_matrix, unsigned int matrices_count, unsigned int gap)
    1720             : {
    1721             :     // Distance between floats of major matrices
    1722           6 :     unsigned int elements_gap = gap * floats_per_matrix;
    1723          78 :     for (unsigned int major_bin = 0; major_bin < matrices_count; major_bin++)
    1724             :     {
    1725        1008 :         for (unsigned int element = 0; element < floats_per_matrix; element++)
    1726             :         {
    1727         936 :             float alpha = (matrix[elements_gap] - matrix[0]) / (float)gap;
    1728         936 :             float previous_value = matrix[0];
    1729        8424 :             for (unsigned int minor_bin = floats_per_matrix; minor_bin < elements_gap;
    1730        6552 :                  minor_bin += floats_per_matrix)
    1731             :             {
    1732        6552 :                 previous_value += alpha;
    1733        6552 :                 matrix[minor_bin] = previous_value;
    1734             :             }
    1735         936 :             matrix++;
    1736             :         }
    1737          72 :         matrix += floats_per_matrix * (gap - 1);
    1738             :     }
    1739           6 : }
    1740             : 
    1741             : //**************
    1742             : // KCOEFFICIENTS
    1743          59 : int set_sy_lfr_kcoeff(const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id)
    1744             : {
    1745             : 
    1746             : #define F0_COMPRESSED_BIN_OFFSET            0
    1747             : #define F1_COMPRESSED_BIN_OFFSET            NB_BINS_COMPRESSED_SM_F0
    1748             : #define F2_COMPRESSED_BIN_OFFSET            (NB_BINS_COMPRESSED_SM_F1 + F1_COMPRESSED_BIN_OFFSET)
    1749             : #define TOTAL_COMPRESSED_BIN_COUNT          (F2_COMPRESSED_BIN_OFFSET + NB_BINS_COMPRESSED_SM_F2)
    1750             : #define DATAFIELD_POS_SY_LFR_MAG_CAL_MATRIX DATAFIELD_POS_SY_LFR_KCOEFF_1
    1751             : #define DATAFIELD_POS_SY_LFR_ELEC_CAL_MATRIX                                                       \
    1752             :     (DATAFIELD_POS_SY_LFR_MAG_CAL_MATRIX + NB_BYTES_MAG_CAL_MATRIX)
    1753             : #define DATAFIELD_POS_SY_LFR_EXTRA_CAL_MATRIX                                                      \
    1754             :     (DATAFIELD_POS_SY_LFR_ELEC_CAL_MATRIX + NB_BYTES_ELEC_CAL_MATRIX)
    1755             : #if TOTAL_COMPRESSED_BIN_COUNT != NB_BINS_COMPRESSED_SM
    1756             :     #error "TOTAL_COMPRESSED_BIN_COUNT must match NB_BINS_COMPRESSED_SM"
    1757             : #endif
    1758             : 
    1759          59 :     unsigned short sy_lfr_kcoeff_frequency = 255;
    1760          59 :     int status = LFR_SUCCESSFUL;
    1761             : 
    1762          59 :     unsigned int matrix_index = 0;
    1763          59 :     float* mag_matrix_ptr = NULL;
    1764          59 :     float* elec_matrix_ptr = NULL;
    1765          59 :     float* extra_mag_matrix_ptr = NULL;
    1766          59 :     float* extra_elec_matrix_ptr = NULL;
    1767             :     enum
    1768             :     {
    1769             :         no = 0,
    1770             :         yes = 1
    1771             :     } interpolate
    1772          59 :         = 0;
    1773             : 
    1774             :     // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
    1775          59 :     copyInt16ByChar((unsigned char*)&sy_lfr_kcoeff_frequency,
    1776             :         &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY]);
    1777             : 
    1778             : 
    1779          59 :     if (sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM)
    1780             :     {
    1781             :         LFR_PRINTF("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n",
    1782             :             sy_lfr_kcoeff_frequency);
    1783           1 :         status = send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
    1784             :             DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET,
    1785           1 :             TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY
    1786             :                 + 1]); // +1 to get the LSB instead of the MSB
    1787             :         DEBUG_CHECK_STATUS(status);
    1788           1 :         status = LFR_DEFAULT;
    1789             :     }
    1790             :     else
    1791             :     {
    1792          58 :         if (sy_lfr_kcoeff_frequency >= F2_COMPRESSED_BIN_OFFSET)
    1793             :         {
    1794          12 :             matrix_index = (sy_lfr_kcoeff_frequency - F2_COMPRESSED_BIN_OFFSET);
    1795          12 :             mag_matrix_ptr = mag_calibration_matrices_f2
    1796          12 :                 + (matrix_index * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F2);
    1797          12 :             elec_matrix_ptr = elec_calibration_matrices_f2
    1798          12 :                 + (matrix_index * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F2);
    1799          12 :             extra_mag_matrix_ptr
    1800             :                 = mag_calibration_matrices_f2 + (ASM_F2_KEEP_BINS * NB_FLOATS_MAG_CAL_MATRIX);
    1801          12 :             extra_elec_matrix_ptr
    1802             :                 = elec_calibration_matrices_f2 + (ASM_F2_KEEP_BINS * NB_FLOATS_ELEC_CAL_MATRIX);
    1803             : 
    1804          12 :             if (sy_lfr_kcoeff_frequency == NB_BINS_COMPRESSED_SM - 1)
    1805             :             {
    1806           1 :                 interpolate = yes;
    1807             :             }
    1808             :         }
    1809          46 :         else if (sy_lfr_kcoeff_frequency >= F1_COMPRESSED_BIN_OFFSET)
    1810             :         {
    1811          13 :             matrix_index = (sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0);
    1812          13 :             mag_matrix_ptr = mag_calibration_matrices_f1
    1813          13 :                 + (matrix_index * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F1);
    1814          13 :             elec_matrix_ptr = elec_calibration_matrices_f1
    1815          13 :                 + (matrix_index * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F1);
    1816          13 :             extra_mag_matrix_ptr
    1817             :                 = mag_calibration_matrices_f1 + (ASM_F1_KEEP_BINS * NB_FLOATS_MAG_CAL_MATRIX);
    1818          13 :             extra_elec_matrix_ptr
    1819             :                 = elec_calibration_matrices_f1 + (ASM_F1_KEEP_BINS * NB_FLOATS_ELEC_CAL_MATRIX);
    1820             :         }
    1821             :         else
    1822             :         {
    1823          33 :             matrix_index = sy_lfr_kcoeff_frequency;
    1824          33 :             mag_matrix_ptr = mag_calibration_matrices_f0
    1825          33 :                 + (matrix_index * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F0);
    1826          33 :             elec_matrix_ptr = elec_calibration_matrices_f0
    1827          33 :                 + (matrix_index * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F0);
    1828          33 :             extra_mag_matrix_ptr
    1829             :                 = mag_calibration_matrices_f0 + (ASM_F0_KEEP_BINS * NB_FLOATS_MAG_CAL_MATRIX);
    1830          33 :             extra_elec_matrix_ptr
    1831             :                 = elec_calibration_matrices_f0 + (ASM_F0_KEEP_BINS * NB_FLOATS_ELEC_CAL_MATRIX);
    1832             :         }
    1833             :     }
    1834             : 
    1835          59 :     if (mag_matrix_ptr != NULL && elec_matrix_ptr != NULL)
    1836             :     {
    1837          58 :         memcpy((void*)mag_matrix_ptr, TC->dataAndCRC + DATAFIELD_POS_SY_LFR_MAG_CAL_MATRIX,
    1838             :             NB_BYTES_MAG_CAL_MATRIX);
    1839          58 :         memcpy((void*)elec_matrix_ptr, TC->dataAndCRC + DATAFIELD_POS_SY_LFR_ELEC_CAL_MATRIX,
    1840             :             NB_BYTES_ELEC_CAL_MATRIX);
    1841             :     }
    1842             : 
    1843             :     // The 3 first packets contains one line of last MAG CAL MATRIX
    1844          59 :     if (extra_mag_matrix_ptr != NULL && matrix_index < 3)
    1845             :     {
    1846          31 :         memcpy(extra_mag_matrix_ptr + (matrix_index * NB_MAG_COMPONENT_PER_SM * FLOATS_PER_COMPLEX),
    1847             :             TC->dataAndCRC + DATAFIELD_POS_SY_LFR_EXTRA_CAL_MATRIX,
    1848             :             NB_MAG_COMPONENT_PER_SM * FLOATS_PER_COMPLEX * NB_BYTES_PER_FLOAT);
    1849             :     }
    1850          28 :     else if (extra_elec_matrix_ptr != NULL
    1851          28 :         && matrix_index < 5) // The 2 following packets contains one line of last ELEC CAL MATRIX
    1852             :     {
    1853          12 :         memcpy(extra_elec_matrix_ptr
    1854           6 :                 + ((matrix_index - 3) * NB_ELEC_COMPONENT_PER_SM * FLOATS_PER_COMPLEX),
    1855             :             TC->dataAndCRC + DATAFIELD_POS_SY_LFR_EXTRA_CAL_MATRIX,
    1856             :             NB_ELEC_COMPONENT_PER_SM * FLOATS_PER_COMPLEX * NB_BYTES_PER_FLOAT);
    1857             :     }
    1858             : 
    1859          59 :     if (interpolate == yes)
    1860             :     {
    1861           1 :         interpolate_calibration_matrix(mag_calibration_matrices_f0, NB_FLOATS_MAG_CAL_MATRIX,
    1862             :             NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0);
    1863           1 :         interpolate_calibration_matrix(elec_calibration_matrices_f0, NB_FLOATS_ELEC_CAL_MATRIX,
    1864             :             NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0);
    1865             : 
    1866           1 :         interpolate_calibration_matrix(mag_calibration_matrices_f1, NB_FLOATS_MAG_CAL_MATRIX,
    1867             :             NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1);
    1868           1 :         interpolate_calibration_matrix(elec_calibration_matrices_f1, NB_FLOATS_ELEC_CAL_MATRIX,
    1869             :             NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1);
    1870             : 
    1871             : 
    1872           1 :         interpolate_calibration_matrix(mag_calibration_matrices_f2, NB_FLOATS_MAG_CAL_MATRIX,
    1873             :             NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2);
    1874           1 :         interpolate_calibration_matrix(elec_calibration_matrices_f2, NB_FLOATS_ELEC_CAL_MATRIX,
    1875             :             NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2);
    1876             :     }
    1877             : 
    1878          59 :     return status;
    1879             : }
    1880             : 
    1881           0 : void copyFloatByChar(unsigned char* destination, const unsigned char* const source)
    1882             : {
    1883        4630 :     destination[BYTE_0] = source[BYTE_0];
    1884        4630 :     destination[BYTE_1] = source[BYTE_1];
    1885        4630 :     destination[BYTE_2] = source[BYTE_2];
    1886        4630 :     destination[BYTE_3] = source[BYTE_3];
    1887           0 : }
    1888             : 
    1889         346 : void copyInt32ByChar(unsigned char* destination, const unsigned char* const source)
    1890             : {
    1891         346 :     destination[BYTE_0] = source[BYTE_0];
    1892         346 :     destination[BYTE_1] = source[BYTE_1];
    1893         346 :     destination[BYTE_2] = source[BYTE_2];
    1894         346 :     destination[BYTE_3] = source[BYTE_3];
    1895         346 : }
    1896             : 
    1897           0 : void copyInt16ByChar(unsigned char* destination, const unsigned char* const source)
    1898             : {
    1899        4544 :     destination[BYTE_0] = source[BYTE_0];
    1900        4544 :     destination[BYTE_1] = source[BYTE_1];
    1901           0 : }
    1902             : 
    1903           0 : void floatToChar(float value, unsigned char* ptr)
    1904             : {
    1905         342 :     const unsigned char* const valuePtr = (const unsigned char*)&value;
    1906             : 
    1907         342 :     ptr[BYTE_0] = valuePtr[BYTE_0];
    1908         342 :     ptr[BYTE_1] = valuePtr[BYTE_1];
    1909         342 :     ptr[BYTE_2] = valuePtr[BYTE_2];
    1910         342 :     ptr[BYTE_3] = valuePtr[BYTE_3];
    1911           0 : }
    1912             : 
    1913             : //**********
    1914             : // init dump
    1915             : 
    1916          18 : void init_parameter_dump(void)
    1917             : {
    1918             :     /** This function initialize the parameter_dump_packet global variable with default values.
    1919             :      *
    1920             :      */
    1921             : 
    1922             :     unsigned int k;
    1923             : 
    1924          18 :     parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
    1925          18 :     parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
    1926          18 :     parameter_dump_packet.reserved = CCSDS_RESERVED;
    1927          18 :     parameter_dump_packet.userApplication = CCSDS_USER_APP;
    1928          18 :     parameter_dump_packet.packetID[0] = (unsigned char)(APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
    1929          18 :     parameter_dump_packet.packetID[1] = (unsigned char)APID_TM_PARAMETER_DUMP;
    1930          18 :     parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
    1931          18 :     parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
    1932          18 :     parameter_dump_packet.packetLength[0]
    1933             :         = (unsigned char)(PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
    1934          18 :     parameter_dump_packet.packetLength[1] = (unsigned char)PACKET_LENGTH_PARAMETER_DUMP;
    1935             :     // DATA FIELD HEADER
    1936          18 :     parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
    1937          18 :     parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
    1938          18 :     parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
    1939          18 :     parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
    1940          18 :     parameter_dump_packet.time[BYTE_0]
    1941          18 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
    1942          18 :     parameter_dump_packet.time[BYTE_1]
    1943          18 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
    1944          18 :     parameter_dump_packet.time[BYTE_2]
    1945          18 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
    1946          18 :     parameter_dump_packet.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
    1947          18 :     parameter_dump_packet.time[BYTE_4]
    1948          18 :         = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
    1949          18 :     parameter_dump_packet.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
    1950          18 :     parameter_dump_packet.sid = SID_PARAMETER_DUMP;
    1951             : 
    1952             :     //******************
    1953             :     // COMMON PARAMETERS
    1954          18 :     parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
    1955          18 :     parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
    1956             : 
    1957             :     //******************
    1958             :     // NORMAL PARAMETERS
    1959          18 :     parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char)(DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
    1960          18 :     parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char)(DFLT_SY_LFR_N_SWF_L);
    1961          18 :     parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char)(DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
    1962          18 :     parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char)(DFLT_SY_LFR_N_SWF_P);
    1963          18 :     parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char)(DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
    1964          18 :     parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char)(DFLT_SY_LFR_N_ASM_P);
    1965          18 :     parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char)DFLT_SY_LFR_N_BP_P0;
    1966          18 :     parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char)DFLT_SY_LFR_N_BP_P1;
    1967          18 :     parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char)DFLT_SY_LFR_N_CWF_LONG_F3;
    1968             : 
    1969             :     //*****************
    1970             :     // BURST PARAMETERS
    1971          18 :     parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char)DEFAULT_SY_LFR_B_BP_P0;
    1972          18 :     parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char)DEFAULT_SY_LFR_B_BP_P1;
    1973             : 
    1974             :     //****************
    1975             :     // SBM1 PARAMETERS
    1976          18 :     parameter_dump_packet.sy_lfr_s1_bp_p0
    1977             :         = (unsigned char)DEFAULT_SY_LFR_S1_BP_P0; // min value is 0.25 s for the period
    1978          18 :     parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char)DEFAULT_SY_LFR_S1_BP_P1;
    1979             : 
    1980             :     //****************
    1981             :     // SBM2 PARAMETERS
    1982          18 :     parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char)DEFAULT_SY_LFR_S2_BP_P0;
    1983          18 :     parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char)DEFAULT_SY_LFR_S2_BP_P1;
    1984             : 
    1985             :     //************
    1986             :     // FBINS MASKS
    1987         882 :     for (k = 0; k < BYTES_PER_MASKS_SET; k++)
    1988             :     {
    1989         864 :         parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
    1990             :     }
    1991             : 
    1992             :     // PAS FILTER PARAMETERS
    1993          18 :     parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
    1994          18 :     parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
    1995          18 :     parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
    1996             :     floatToChar(DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad);
    1997          18 :     parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
    1998             :     floatToChar(DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift);
    1999             :     floatToChar(DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f);
    2000             : 
    2001             :     // RW1_K
    2002             :     floatToChar(DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
    2003             :     floatToChar(DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
    2004             :     floatToChar(DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
    2005             :     floatToChar(DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
    2006             :     // RW2_K
    2007             :     floatToChar(DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
    2008             :     floatToChar(DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
    2009             :     floatToChar(DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
    2010             :     floatToChar(DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
    2011             :     // RW3_K
    2012             :     floatToChar(DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
    2013             :     floatToChar(DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
    2014             :     floatToChar(DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
    2015             :     floatToChar(DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
    2016             :     // RW4_K
    2017             :     floatToChar(DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
    2018             :     floatToChar(DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
    2019             :     floatToChar(DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
    2020             :     floatToChar(DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
    2021             : 
    2022             :     // LFR_RW_MASK
    2023         882 :     for (k = 0; k < BYTES_PER_MASKS_SET; k++)
    2024             :     {
    2025         864 :         parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
    2026             :     }
    2027             : 
    2028             :     // once the reaction wheels masks have been initialized, they have to be merged with the fbins
    2029             :     // masks
    2030          18 :     merge_fbins_masks();
    2031          18 : }
    2032             : 
    2033          18 : void init_kcoefficients_dump(void)
    2034             : {
    2035          18 :     init_kcoefficients_dump_packet(&kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1);
    2036          18 :     init_kcoefficients_dump_packet(&kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2);
    2037             : 
    2038          18 :     kcoefficient_node_1.previous = NULL;
    2039          18 :     kcoefficient_node_1.next = NULL;
    2040          18 :     kcoefficient_node_1.packet_id = TM_K_DUMP_PKT_ID;
    2041          18 :     kcoefficient_node_1.coarseTime = INIT_CHAR;
    2042          18 :     kcoefficient_node_1.fineTime = INIT_CHAR;
    2043          18 :     kcoefficient_node_1.buffer_address = &kcoefficients_dump_1;
    2044          18 :     kcoefficient_node_1.status = INIT_CHAR;
    2045             : 
    2046          18 :     kcoefficient_node_2.previous = NULL;
    2047          18 :     kcoefficient_node_2.next = NULL;
    2048          18 :     kcoefficient_node_2.packet_id = TM_K_DUMP_PKT_ID;
    2049          18 :     kcoefficient_node_2.coarseTime = INIT_CHAR;
    2050          18 :     kcoefficient_node_2.fineTime = INIT_CHAR;
    2051          18 :     kcoefficient_node_2.buffer_address = &kcoefficients_dump_2;
    2052          18 :     kcoefficient_node_2.status = INIT_CHAR;
    2053          18 : }
    2054             : 
    2055          36 : void init_kcoefficients_dump_packet(Packet_TM_LFR_KCOEFFICIENTS_DUMP_t* const kcoefficients_dump,
    2056             :     unsigned char pkt_nr, unsigned char blk_nr)
    2057             : {
    2058             :     unsigned int packetLength;
    2059             : 
    2060          36 :     packetLength = ((blk_nr * (NB_BYTES_PER_KCOEFFICIENTS_BLOCK))
    2061             :                        + BYTE_POS_KCOEFFICIENTS_PARAMETES)
    2062             :         - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
    2063             : 
    2064          36 :     kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
    2065          36 :     kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
    2066          36 :     kcoefficients_dump->reserved = CCSDS_RESERVED;
    2067          36 :     kcoefficients_dump->userApplication = CCSDS_USER_APP;
    2068          36 :     kcoefficients_dump->packetID[0] = (unsigned char)(APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
    2069          36 :     kcoefficients_dump->packetID[1] = (unsigned char)APID_TM_PARAMETER_DUMP;
    2070          36 :     kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
    2071          36 :     kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
    2072          36 :     kcoefficients_dump->packetLength[0] = (unsigned char)(packetLength >> SHIFT_1_BYTE);
    2073          36 :     kcoefficients_dump->packetLength[1] = (unsigned char)packetLength;
    2074             :     // DATA FIELD HEADER
    2075          36 :     kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
    2076          36 :     kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
    2077          36 :     kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
    2078          36 :     kcoefficients_dump->destinationID = TM_DESTINATION_ID_GROUND;
    2079          36 :     kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
    2080          36 :     kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
    2081          36 :     kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
    2082          36 :     kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
    2083          36 :     kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
    2084          36 :     kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
    2085          36 :     kcoefficients_dump->sid = SID_K_DUMP;
    2086             : 
    2087          36 :     kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
    2088          36 :     kcoefficients_dump->pkt_nr = pkt_nr;
    2089          36 :     kcoefficients_dump->blk_nr = blk_nr;
    2090             : 
    2091             :     //******************
    2092             :     // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
    2093             :     // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
    2094          36 :     memset(kcoefficients_dump->kcoeff_blks, 0, KCOEFF_BLK_MAX_SZ);
    2095          36 : }
    2096             : 
    2097         420 : void increment_seq_counter_destination_id_dump(
    2098             :     unsigned char* const packet_sequence_control, unsigned char destination_id)
    2099             : {
    2100             :     /** This function increment the packet sequence control parameter of a TC, depending on its
    2101             :      * destination ID.
    2102             :      *
    2103             :      * @param packet_sequence_control points to the packet sequence control which will be
    2104             :      * incremented
    2105             :      * @param destination_id is the destination ID of the TM, there is one counter by destination ID
    2106             :      *
    2107             :      * If the destination ID is not known, a dedicated counter is incremented.
    2108             :      *
    2109             :      */
    2110             : 
    2111             :     unsigned short sequence_cnt;
    2112             :     unsigned short segmentation_grouping_flag;
    2113             :     unsigned short new_packet_sequence_control;
    2114             :     unsigned char i;
    2115             : 
    2116         420 :     switch (destination_id)
    2117             :     {
    2118             :         case SID_TC_GROUND:
    2119           0 :             i = GROUND;
    2120           0 :             break;
    2121             :         case SID_TC_MISSION_TIMELINE:
    2122         390 :             i = MISSION_TIMELINE;
    2123         390 :             break;
    2124             :         case SID_TC_TC_SEQUENCES:
    2125           3 :             i = TC_SEQUENCES;
    2126           3 :             break;
    2127             :         case SID_TC_RECOVERY_ACTION_CMD:
    2128           3 :             i = RECOVERY_ACTION_CMD;
    2129           3 :             break;
    2130             :         case SID_TC_BACKUP_MISSION_TIMELINE:
    2131           3 :             i = BACKUP_MISSION_TIMELINE;
    2132           3 :             break;
    2133             :         case SID_TC_DIRECT_CMD:
    2134           3 :             i = DIRECT_CMD;
    2135           3 :             break;
    2136             :         case SID_TC_SPARE_GRD_SRC1:
    2137           3 :             i = SPARE_GRD_SRC1;
    2138           3 :             break;
    2139             :         case SID_TC_SPARE_GRD_SRC2:
    2140           3 :             i = SPARE_GRD_SRC2;
    2141           3 :             break;
    2142             :         case SID_TC_OBCP:
    2143           3 :             i = OBCP;
    2144           3 :             break;
    2145             :         case SID_TC_SYSTEM_CONTROL:
    2146           3 :             i = SYSTEM_CONTROL;
    2147           3 :             break;
    2148             :         case SID_TC_AOCS:
    2149           3 :             i = AOCS;
    2150           3 :             break;
    2151             :         case SID_TC_RPW_INTERNAL:
    2152           3 :             i = RPW_INTERNAL;
    2153           3 :             break;
    2154             :         default:
    2155           0 :             i = GROUND;
    2156             :             break;
    2157             :     }
    2158             : 
    2159         420 :     segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
    2160         420 :     sequence_cnt = sequenceCounters_TM_DUMP[i] & SEQ_CNT_MASK;
    2161             : 
    2162         420 :     new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt;
    2163             : 
    2164         420 :     packet_sequence_control[0] = (unsigned char)(new_packet_sequence_control >> SHIFT_1_BYTE);
    2165         420 :     packet_sequence_control[1] = (unsigned char)(new_packet_sequence_control);
    2166             : 
    2167             :     // increment the sequence counter
    2168         420 :     if (sequenceCounters_TM_DUMP[i] < SEQ_CNT_MAX)
    2169             :     {
    2170         420 :         sequenceCounters_TM_DUMP[i] = sequenceCounters_TM_DUMP[i] + 1;
    2171             :     }
    2172             :     else
    2173             :     {
    2174           0 :         sequenceCounters_TM_DUMP[i] = 0;
    2175             :     }
    2176         420 : }

Generated by: LCOV version 1.14