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: 122 869 14.0 %
Date: 2023-02-20 11:47:12 Functions: 5 49 10.2 %

          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           0 : 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           0 :     parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
      68           0 :     parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
      69           0 :     set_wfp_data_shaping();
      70           0 :     return LFR_SUCCESSFUL;
      71             : }
      72             : 
      73           0 : 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           0 :     int flag = LFR_SUCCESSFUL;
      85             : 
      86           0 :     if ((lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
      87           0 :         || (lfrCurrentMode == LFR_MODE_SBM2))
      88             :     {
      89           0 :         DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_not_executable(TC, queue_id));
      90           0 :         flag = LFR_DEFAULT;
      91             :     }
      92             : 
      93             :     // CHECK THE PARAMETERS SET CONSISTENCY
      94           0 :     if (flag == LFR_SUCCESSFUL)
      95             :     {
      96           0 :         flag = check_normal_par_consistency(TC, queue_id);
      97             :     }
      98             : 
      99             :     // SET THE PARAMETERS IF THEY ARE CONSISTENT
     100           0 :     if (flag == LFR_SUCCESSFUL)
     101             :     {
     102           0 :         flag |= set_sy_lfr_n_swf_l(TC);
     103           0 :         flag |= set_sy_lfr_n_swf_p(TC);
     104           0 :         flag |= set_sy_lfr_n_bp_p0(TC);
     105           0 :         flag |= set_sy_lfr_n_bp_p1(TC);
     106           0 :         flag |= set_sy_lfr_n_asm_p(TC);
     107           0 :         flag |= set_sy_lfr_n_cwf_long_f3(TC);
     108             :     }
     109             : 
     110           0 :     return flag;
     111             : }
     112             : 
     113           0 : 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           0 :     flag = LFR_SUCCESSFUL;
     130             : 
     131           0 :     if (lfrCurrentMode == LFR_MODE_BURST)
     132             :     {
     133           0 :         status = send_tm_lfr_tc_exe_not_executable(TC, queue_id);
     134             :         DEBUG_CHECK_STATUS(status);
     135           0 :         flag = LFR_DEFAULT;
     136             :     }
     137             : 
     138           0 :     sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
     139           0 :     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           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0)
     143             :     {
     144           0 :         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           0 :         flag = WRONG_APP_DATA;
     148             :     }
     149             :     // sy_lfr_b_bp_p1 shall not be lower than its default value
     150           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1)
     151             :     {
     152             : 
     153           0 :         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           0 :         flag = WRONG_APP_DATA;
     157             :     }
     158             :     //****************************************************************
     159             :     // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
     160           0 :     if (flag == LFR_SUCCESSFUL)
     161             :     {
     162           0 :         sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
     163           0 :         sy_lfr_b_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P1];
     164           0 :         aux = ((float)sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0) - floorf(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
     165           0 :         if (aux > FLOAT_EQUAL_ZERO)
     166             :         {
     167           0 :             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           0 :             flag = LFR_DEFAULT;
     171             :         }
     172             :     }
     173             : 
     174             :     // SET THE PARAMETERS
     175           0 :     if (flag == LFR_SUCCESSFUL)
     176             :     {
     177           0 :         flag = set_sy_lfr_b_bp_p0(TC);
     178           0 :         flag |= set_sy_lfr_b_bp_p1(TC);
     179             :     }
     180             : 
     181           0 :     return flag;
     182             : }
     183             : 
     184           0 : 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           0 :     flag = LFR_SUCCESSFUL;
     202             : 
     203           0 :     if (lfrCurrentMode == LFR_MODE_SBM1)
     204             :     {
     205           0 :         status = send_tm_lfr_tc_exe_not_executable(TC, queue_id);
     206             :         DEBUG_CHECK_STATUS(status);
     207           0 :         flag = LFR_DEFAULT;
     208             :     }
     209             : 
     210           0 :     sy_lfr_s1_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P0];
     211           0 :     sy_lfr_s1_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P1];
     212             : 
     213             :     // sy_lfr_s1_bp_p0
     214           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0)
     215             :     {
     216             : 
     217           0 :         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           0 :         flag = WRONG_APP_DATA;
     221             :     }
     222             :     // sy_lfr_s1_bp_p1
     223           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1)
     224             :     {
     225             : 
     226           0 :         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           0 :         flag = WRONG_APP_DATA;
     230             :     }
     231             :     //******************************************************************
     232             :     // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
     233           0 :     if (flag == LFR_SUCCESSFUL)
     234             :     {
     235           0 :         aux = ((float)sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * (float)S1_BP_P0_SCALE))
     236           0 :             - floorf(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * (float)S1_BP_P0_SCALE));
     237           0 :         if (aux > FLOAT_EQUAL_ZERO)
     238             :         {
     239           0 :             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           0 :             flag = LFR_DEFAULT;
     243             :         }
     244             :     }
     245             : 
     246             :     // SET THE PARAMETERS
     247           0 :     if (flag == LFR_SUCCESSFUL)
     248             :     {
     249           0 :         flag = set_sy_lfr_s1_bp_p0(TC);
     250           0 :         flag |= set_sy_lfr_s1_bp_p1(TC);
     251             :     }
     252             : 
     253           0 :     return flag;
     254             : }
     255             : 
     256           0 : 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           0 :     flag = LFR_SUCCESSFUL;
     274             : 
     275           0 :     if (lfrCurrentMode == LFR_MODE_SBM2)
     276             :     {
     277           0 :         status = send_tm_lfr_tc_exe_not_executable(TC, queue_id);
     278             :         DEBUG_CHECK_STATUS(status);
     279           0 :         flag = LFR_DEFAULT;
     280             :     }
     281             : 
     282           0 :     sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
     283           0 :     sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
     284             : 
     285             :     // sy_lfr_s2_bp_p0
     286           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0)
     287             :     {
     288             : 
     289           0 :         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           0 :         flag = WRONG_APP_DATA;
     293             :     }
     294             :     // sy_lfr_s2_bp_p1
     295           0 :     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           0 :     if (flag == LFR_SUCCESSFUL)
     306             :     {
     307           0 :         sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
     308           0 :         sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
     309           0 :         aux = ((float)sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0)
     310           0 :             - floorf(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
     311           0 :         if (aux > FLOAT_EQUAL_ZERO)
     312             :         {
     313           0 :             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           0 :             flag = LFR_DEFAULT;
     317             :         }
     318             :     }
     319             : 
     320             :     // SET THE PARAMETERS
     321           0 :     if (flag == LFR_SUCCESSFUL)
     322             :     {
     323           0 :         flag = set_sy_lfr_s2_bp_p0(TC);
     324           0 :         flag |= set_sy_lfr_s2_bp_p1(TC);
     325             :     }
     326             : 
     327           0 :     return flag;
     328             : }
     329             : 
     330           0 : 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           0 :     int flag = LFR_DEFAULT;
     342             :     rtems_status_code status;
     343             : 
     344           0 :     if (lfrCurrentMode != LFR_MODE_STANDBY)
     345             :     {
     346           0 :         status = send_tm_lfr_tc_exe_not_executable(TC, queue_id);
     347             :         DEBUG_CHECK_STATUS(status);
     348             :     }
     349             :     else
     350             :     {
     351           0 :         flag = set_sy_lfr_kcoeff(TC, queue_id);
     352             :     }
     353             : 
     354           0 :     return flag;
     355             : }
     356             : 
     357           0 : 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           0 :     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           0 :     merge_fbins_masks();
     374             : 
     375           0 :     return flag;
     376             : }
     377             : 
     378           0 : 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           0 :     int flag = check_sy_lfr_filter_parameters(TC, queue_id);
     388             : 
     389           0 :     if (flag == LFR_SUCCESSFUL)
     390             :     {
     391           0 :         parameter_dump_packet.spare_sy_lfr_pas_filter_enabled
     392           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED];
     393           0 :         parameter_dump_packet.sy_lfr_pas_filter_modulus
     394           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS];
     395           0 :         parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0]
     396           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0];
     397           0 :         parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1]
     398           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1];
     399           0 :         parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2]
     400           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2];
     401           0 :         parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3]
     402           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3];
     403           0 :         parameter_dump_packet.sy_lfr_pas_filter_offset
     404           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET];
     405           0 :         parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0]
     406           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0];
     407           0 :         parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1]
     408           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1];
     409           0 :         parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2]
     410           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2];
     411           0 :         parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3]
     412           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3];
     413           0 :         parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0]
     414           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0];
     415           0 :         parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1]
     416           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1];
     417           0 :         parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2]
     418           0 :             = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2];
     419           0 :         parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3]
     420           0 :             = 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           0 :         filterPar.spare_sy_lfr_pas_filter_enabled
     427           0 :             = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
     428           0 :         set_sy_lfr_pas_filter_enabled(
     429           0 :             parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED);
     430             : 
     431             :         // sy_lfr_pas_filter_modulus
     432           0 :         filterPar.modulus_in_finetime
     433           0 :             = ((uint64_t)parameter_dump_packet.sy_lfr_pas_filter_modulus) * CONST_65536;
     434             : 
     435             :         // sy_lfr_pas_filter_tbad
     436           0 :         copyFloatByChar((unsigned char*)&filterPar.sy_lfr_pas_filter_tbad,
     437             :             parameter_dump_packet.sy_lfr_pas_filter_tbad);
     438           0 :         filterPar.tbad_in_finetime = (uint64_t)(filterPar.sy_lfr_pas_filter_tbad * CONST_65536);
     439             : 
     440             :         // sy_lfr_pas_filter_offset
     441           0 :         filterPar.offset_in_finetime
     442           0 :             = ((uint64_t)parameter_dump_packet.sy_lfr_pas_filter_offset) * CONST_65536;
     443             : 
     444             :         // sy_lfr_pas_filter_shift
     445           0 :         copyFloatByChar((unsigned char*)&filterPar.sy_lfr_pas_filter_shift,
     446             :             parameter_dump_packet.sy_lfr_pas_filter_shift);
     447           0 :         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           0 :         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           0 :         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           0 :             parameter_dump_packet.sy_lfr_rw1_k1[k]
     459           0 :                 = 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           0 :         copyFloatByChar(
     466             :             (unsigned char*)&filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1);
     467           0 :         copyFloatByChar(
     468             :             (unsigned char*)&filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2);
     469           0 :         copyFloatByChar(
     470             :             (unsigned char*)&filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3);
     471           0 :         copyFloatByChar(
     472             :             (unsigned char*)&filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4);
     473             :         // rw2_k
     474           0 :         copyFloatByChar(
     475             :             (unsigned char*)&filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1);
     476           0 :         copyFloatByChar(
     477             :             (unsigned char*)&filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2);
     478           0 :         copyFloatByChar(
     479             :             (unsigned char*)&filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3);
     480           0 :         copyFloatByChar(
     481             :             (unsigned char*)&filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4);
     482             :         // rw3_k
     483           0 :         copyFloatByChar(
     484             :             (unsigned char*)&filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1);
     485           0 :         copyFloatByChar(
     486             :             (unsigned char*)&filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2);
     487           0 :         copyFloatByChar(
     488             :             (unsigned char*)&filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3);
     489           0 :         copyFloatByChar(
     490             :             (unsigned char*)&filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4);
     491             :         // rw4_k
     492           0 :         copyFloatByChar(
     493             :             (unsigned char*)&filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1);
     494           0 :         copyFloatByChar(
     495             :             (unsigned char*)&filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2);
     496           0 :         copyFloatByChar(
     497             :             (unsigned char*)&filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3);
     498           0 :         copyFloatByChar(
     499             :             (unsigned char*)&filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4);
     500             :     }
     501             : 
     502           0 :     return flag;
     503             : }
     504             : 
     505           0 : 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           0 :     kcoefficients_dump_1.destinationID = TC->sourceID;
     535           0 :     increment_seq_counter_destination_id_dump(
     536           0 :         kcoefficients_dump_1.packetSequenceControl, TC->sourceID);
     537           0 :     kCoeffDumpPtr = kcoefficients_dump_1.kcoeff_blks;
     538           0 :     for (int freq = 0; freq <= NB_BINS_COMPRESSED_SM_F0; freq++)
     539             :     {
     540           0 :         uint16_t bin = 0;
     541           0 :         copyInt16ByChar(kCoeffDumpPtr, (unsigned char*)(&bin));
     542           0 :         kCoeffDumpPtr += 2;
     543           0 :         memcpy(kCoeffDumpPtr,
     544             :             mag_calibration_matrices_f0
     545           0 :                 + (freq * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F0),
     546             :             NB_BYTES_MAG_CAL_MATRIX);
     547           0 :         kCoeffDumpPtr += NB_BYTES_MAG_CAL_MATRIX;
     548           0 :         memcpy(kCoeffDumpPtr,
     549             :             elec_calibration_matrices_f0
     550           0 :                 + (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           0 :         kCoeffDumpPtr += NB_BYTES_ELEC_CAL_MATRIX + (6 * sizeof(float));
     554             :     }
     555           0 :     for (int freq = 0; freq <= NB_BINS_COMPRESSED_SM_F1; freq++)
     556             :     {
     557           0 :         uint16_t bin = 1;
     558           0 :         copyInt16ByChar(kCoeffDumpPtr, (unsigned char*)(&bin));
     559           0 :         kCoeffDumpPtr += 2;
     560           0 :         memcpy(kCoeffDumpPtr,
     561             :             mag_calibration_matrices_f1
     562           0 :                 + (freq * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F1),
     563             :             NB_BYTES_MAG_CAL_MATRIX);
     564           0 :         kCoeffDumpPtr += NB_BYTES_MAG_CAL_MATRIX;
     565           0 :         memcpy(kCoeffDumpPtr,
     566             :             elec_calibration_matrices_f1
     567           0 :                 + (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           0 :         kCoeffDumpPtr += NB_BYTES_ELEC_CAL_MATRIX + (6 * sizeof(float));
     571             :     }
     572             : 
     573           0 :     kcoefficients_dump_1.time[BYTE_0]
     574           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
     575           0 :     kcoefficients_dump_1.time[BYTE_1]
     576           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
     577           0 :     kcoefficients_dump_1.time[BYTE_2]
     578           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
     579           0 :     kcoefficients_dump_1.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
     580           0 :     kcoefficients_dump_1.time[BYTE_4]
     581           0 :         = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
     582           0 :     kcoefficients_dump_1.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
     583             :     // SEND DATA
     584           0 :     kcoefficient_node_1.status = 1;
     585           0 :     address = (void*)&kcoefficient_node_1;
     586           0 :     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           0 :     kcoefficients_dump_2.destinationID = TC->sourceID;
     593           0 :     increment_seq_counter_destination_id_dump(
     594           0 :         kcoefficients_dump_2.packetSequenceControl, TC->sourceID);
     595             : 
     596           0 :     kCoeffDumpPtr = kcoefficients_dump_2.kcoeff_blks;
     597           0 :     for (int freq = 0; freq <= NB_BINS_COMPRESSED_SM_F2; freq++)
     598             :     {
     599           0 :         uint16_t bin = 2;
     600           0 :         copyInt16ByChar(kCoeffDumpPtr, (unsigned char*)(&bin));
     601           0 :         kCoeffDumpPtr += 2;
     602           0 :         memcpy(kCoeffDumpPtr,
     603             :             mag_calibration_matrices_f2
     604           0 :                 + (freq * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F2),
     605             :             NB_BYTES_MAG_CAL_MATRIX);
     606           0 :         kCoeffDumpPtr += NB_BYTES_MAG_CAL_MATRIX;
     607           0 :         memcpy(kCoeffDumpPtr,
     608             :             elec_calibration_matrices_f2
     609           0 :                 + (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           0 :         kCoeffDumpPtr += NB_BYTES_ELEC_CAL_MATRIX + (6 * sizeof(float));
     613             :     }
     614             : 
     615           0 :     kcoefficients_dump_2.time[BYTE_0]
     616           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
     617           0 :     kcoefficients_dump_2.time[BYTE_1]
     618           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
     619           0 :     kcoefficients_dump_2.time[BYTE_2]
     620           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
     621           0 :     kcoefficients_dump_2.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
     622           0 :     kcoefficients_dump_2.time[BYTE_4]
     623           0 :         = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
     624           0 :     kcoefficients_dump_2.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
     625             :     // SEND DATA
     626           0 :     kcoefficient_node_2.status = 1;
     627           0 :     address = (void*)&kcoefficient_node_2;
     628           0 :     status = rtems_message_queue_send(queue_id, &address, sizeof(ring_node*));
     629             :     DEBUG_CHECK_STATUS(status);
     630             : 
     631           0 :     return status;
     632             : }
     633             : 
     634           0 : 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           0 :     increment_seq_counter_destination_id_dump(
     654           0 :         parameter_dump_packet.packetSequenceControl, TC->sourceID);
     655           0 :     parameter_dump_packet.destinationID = TC->sourceID;
     656             : 
     657             :     // UPDATE TIME
     658           0 :     parameter_dump_packet.time[BYTE_0]
     659           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
     660           0 :     parameter_dump_packet.time[BYTE_1]
     661           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
     662           0 :     parameter_dump_packet.time[BYTE_2]
     663           0 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
     664           0 :     parameter_dump_packet.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
     665           0 :     parameter_dump_packet.time[BYTE_4]
     666           0 :         = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
     667           0 :     parameter_dump_packet.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
     668             :     // SEND DATA
     669           0 :     status
     670           0 :         = rtems_message_queue_send(queue_id, &parameter_dump_packet, sizeof(parameter_dump_packet));
     671             :     DEBUG_CHECK_STATUS(status);
     672             : 
     673           0 :     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           0 : 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           0 :     flag = LFR_SUCCESSFUL;
     695             : 
     696             :     //***************
     697             :     // get parameters
     698           0 :     msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L];
     699           0 :     lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L + 1];
     700           0 :     sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
     701             : 
     702           0 :     msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P];
     703           0 :     lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P + 1];
     704           0 :     sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
     705             : 
     706           0 :     msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P];
     707           0 :     lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P + 1];
     708           0 :     sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
     709             : 
     710           0 :     sy_lfr_n_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P0];
     711             : 
     712           0 :     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           0 :     if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
     718             :     {
     719           0 :         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           0 :         flag = WRONG_APP_DATA;
     723             :     }
     724             :     // sy_lfr_n_swf_p
     725           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P)
     726             :     {
     727           0 :         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           0 :         flag = WRONG_APP_DATA;
     731             :     }
     732             :     // sy_lfr_n_bp_p0
     733           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
     734             :     {
     735           0 :         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           0 :         flag = WRONG_APP_DATA;
     739             :     }
     740             :     // sy_lfr_n_asm_p
     741           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_n_asm_p == 0)
     742             :     {
     743           0 :         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           0 :         flag = WRONG_APP_DATA;
     747             :     }
     748             :     // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
     749           0 :     if (flag == LFR_SUCCESSFUL)
     750             :     {
     751           0 :         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           0 :         if (aux > FLOAT_EQUAL_ZERO)
     754             :         {
     755           0 :             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           0 :             flag = WRONG_APP_DATA;
     759             :         }
     760             :     }
     761             :     // sy_lfr_n_bp_p1
     762           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
     763             :     {
     764             : 
     765           0 :         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           0 :         flag = WRONG_APP_DATA;
     769             :     }
     770             :     // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
     771           0 :     if (flag == LFR_SUCCESSFUL)
     772             :     {
     773           0 :         aux = ((float)sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0) - floorf(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
     774           0 :         if (aux > FLOAT_EQUAL_ZERO)
     775             :         {
     776           0 :             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           0 :             flag = LFR_DEFAULT;
     780             :         }
     781             :     }
     782             : 
     783           0 :     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           0 :     parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L];
     796           0 :     parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L + 1];
     797             : 
     798           0 :     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           0 :     parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P];
     811           0 :     parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P + 1];
     812             : 
     813           0 :     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           0 :     parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P];
     827           0 :     parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P + 1];
     828             : 
     829           0 :     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           0 :     parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P0];
     842             : 
     843           0 :     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           0 :     parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P1];
     857             : 
     858           0 :     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           0 :     parameter_dump_packet.sy_lfr_n_cwf_long_f3
     871           0 :         = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3] & BIT_CWF_LONG_F3;
     872             : 
     873           0 :     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           0 :     parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
     889             : 
     890           0 :     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           0 :     parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P1];
     903             : 
     904           0 :     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           0 :     parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P0];
     920             : 
     921           0 :     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           0 :     parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P1];
     934             : 
     935           0 :     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           0 :     parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
     951             : 
     952           0 :     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           0 :     parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
     965             : 
     966           0 :     return LFR_SUCCESSFUL;
     967             : }
     968             : 
     969             : //*******************
     970             : // TC_LFR_UPDATE_INFO
     971             : 
     972           0 : unsigned int check_update_info_hk_lfr_mode(unsigned char mode)
     973             : {
     974           0 :     unsigned int status = LFR_DEFAULT;
     975             : 
     976           0 :     if ((mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL) || (mode == LFR_MODE_BURST)
     977             :         || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
     978             :     {
     979           0 :         status = LFR_SUCCESSFUL;
     980             :     }
     981             :     else
     982             :     {
     983           0 :         status = LFR_DEFAULT;
     984             :     }
     985             : 
     986           0 :     return status;
     987             : }
     988             : 
     989           0 : unsigned int check_update_info_hk_tds_mode(unsigned char mode)
     990             : {
     991           0 :     unsigned int status = LFR_DEFAULT;
     992             : 
     993           0 :     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           0 :         status = LFR_SUCCESSFUL;
     997             :     }
     998             :     else
     999             :     {
    1000           0 :         status = LFR_DEFAULT;
    1001             :     }
    1002             : 
    1003           0 :     return status;
    1004             : }
    1005             : 
    1006           0 : unsigned int check_update_info_hk_thr_mode(unsigned char mode)
    1007             : {
    1008           0 :     unsigned int status = LFR_DEFAULT;
    1009             : 
    1010           0 :     if ((mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL) || (mode == THR_MODE_BURST))
    1011             :     {
    1012           0 :         status = LFR_SUCCESSFUL;
    1013             :     }
    1014             :     else
    1015             :     {
    1016           0 :         status = LFR_DEFAULT;
    1017             :     }
    1018             : 
    1019           0 :     return status;
    1020             : }
    1021             : 
    1022           0 : 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           0 :     if (isnan(value))
    1032             :     {
    1033           0 :         flag = FLAG_NAN;
    1034             :     }
    1035             :     else
    1036             :     {
    1037           0 :         flag = FLAG_IAN;
    1038             :     }
    1039             : 
    1040           0 :     switch (wheel)
    1041             :     {
    1042             :         case WHEEL_1:
    1043           0 :             flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
    1044           0 :             flagMask = (unsigned char)(~(1 << flagPosInByte));
    1045           0 :             newFlag = (unsigned char)(flag << flagPosInByte);
    1046           0 :             housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags
    1047             :                 = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
    1048           0 :             break;
    1049             :         case WHEEL_2:
    1050           0 :             flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
    1051           0 :             flagMask = (unsigned char)(~(1 << flagPosInByte));
    1052           0 :             newFlag = (unsigned char)(flag << flagPosInByte);
    1053           0 :             housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags
    1054             :                 = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
    1055           0 :             break;
    1056             :         case WHEEL_3:
    1057           0 :             flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
    1058           0 :             flagMask = (unsigned char)(~(1 << flagPosInByte));
    1059           0 :             newFlag = (unsigned char)(flag << flagPosInByte);
    1060           0 :             housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags
    1061             :                 = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
    1062           0 :             break;
    1063             :         case WHEEL_4:
    1064           0 :             flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
    1065           0 :             flagMask = (unsigned char)(~(1 << flagPosInByte));
    1066           0 :             newFlag = (unsigned char)(flag << flagPosInByte);
    1067           0 :             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           0 : }
    1074             : 
    1075           0 : void set_hk_lfr_sc_rw_f_flags(void)
    1076             : {
    1077             :     // RW1
    1078           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1);
    1079           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2);
    1080           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3);
    1081           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4);
    1082             : 
    1083             :     // RW2
    1084           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1);
    1085           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2);
    1086           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3);
    1087           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4);
    1088             : 
    1089             :     // RW3
    1090           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1);
    1091           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2);
    1092           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3);
    1093           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4);
    1094             : 
    1095             :     // RW4
    1096           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1);
    1097           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2);
    1098           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3);
    1099           0 :     set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4);
    1100           0 : }
    1101             : 
    1102           0 : 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           0 :     ret = LFR_SUCCESSFUL;
    1108           0 :     rw_k = INIT_FLOAT;
    1109             : 
    1110           0 :     copyFloatByChar((unsigned char*)&rw_k, &TC->packetID[offset]);
    1111             : 
    1112           0 :     *pos = offset;
    1113           0 :     *value = rw_k;
    1114             : 
    1115           0 :     if (rw_k < MIN_SY_LFR_RW_F)
    1116             :     {
    1117           0 :         ret = WRONG_APP_DATA;
    1118             :     }
    1119             : 
    1120           0 :     return ret;
    1121             : }
    1122             : 
    1123           0 : int check_all_sy_lfr_rw_f(const ccsdsTelecommandPacket_t* const TC, int* pos, float* value)
    1124             : {
    1125           0 :     int ret = LFR_SUCCESSFUL;
    1126             : 
    1127             :     //****
    1128             :     //****
    1129             :     // RW1
    1130           0 :     ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value); // F1
    1131           0 :     if (ret == LFR_SUCCESSFUL) // F2
    1132             :     {
    1133           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value);
    1134             :     }
    1135           0 :     if (ret == LFR_SUCCESSFUL) // F3
    1136             :     {
    1137           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value);
    1138             :     }
    1139           0 :     if (ret == LFR_SUCCESSFUL) // F4
    1140             :     {
    1141           0 :         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           0 :     if (ret == LFR_SUCCESSFUL) // F1
    1148             :     {
    1149           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value);
    1150             :     }
    1151           0 :     if (ret == LFR_SUCCESSFUL) // F2
    1152             :     {
    1153           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value);
    1154             :     }
    1155           0 :     if (ret == LFR_SUCCESSFUL) // F3
    1156             :     {
    1157           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value);
    1158             :     }
    1159           0 :     if (ret == LFR_SUCCESSFUL) // F4
    1160             :     {
    1161           0 :         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           0 :     if (ret == LFR_SUCCESSFUL) // F1
    1168             :     {
    1169           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value);
    1170             :     }
    1171           0 :     if (ret == LFR_SUCCESSFUL) // F2
    1172             :     {
    1173           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value);
    1174             :     }
    1175           0 :     if (ret == LFR_SUCCESSFUL) // F3
    1176             :     {
    1177           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value);
    1178             :     }
    1179           0 :     if (ret == LFR_SUCCESSFUL) // F4
    1180             :     {
    1181           0 :         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           0 :     if (ret == LFR_SUCCESSFUL) // F1
    1188             :     {
    1189           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value);
    1190             :     }
    1191           0 :     if (ret == LFR_SUCCESSFUL) // F2
    1192             :     {
    1193           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value);
    1194             :     }
    1195           0 :     if (ret == LFR_SUCCESSFUL) // F3
    1196             :     {
    1197           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value);
    1198             :     }
    1199           0 :     if (ret == LFR_SUCCESSFUL) // F4
    1200             :     {
    1201           0 :         ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value);
    1202             :     }
    1203             : 
    1204           0 :     return ret;
    1205             : }
    1206             : 
    1207           0 : 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           0 :     const unsigned char* const bytePosPtr = (const unsigned char* const)&TC->packetID;
    1217             : 
    1218             :     // rw1_f
    1219           0 :     copyFloatByChar(
    1220             :         (unsigned char*)&rw_f.cp_rpw_sc_rw1_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1]);
    1221           0 :     copyFloatByChar(
    1222             :         (unsigned char*)&rw_f.cp_rpw_sc_rw1_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2]);
    1223           0 :     copyFloatByChar(
    1224             :         (unsigned char*)&rw_f.cp_rpw_sc_rw1_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3]);
    1225           0 :     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           0 :     copyFloatByChar(
    1230             :         (unsigned char*)&rw_f.cp_rpw_sc_rw2_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1]);
    1231           0 :     copyFloatByChar(
    1232             :         (unsigned char*)&rw_f.cp_rpw_sc_rw2_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2]);
    1233           0 :     copyFloatByChar(
    1234             :         (unsigned char*)&rw_f.cp_rpw_sc_rw2_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3]);
    1235           0 :     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           0 :     copyFloatByChar(
    1240             :         (unsigned char*)&rw_f.cp_rpw_sc_rw3_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1]);
    1241           0 :     copyFloatByChar(
    1242             :         (unsigned char*)&rw_f.cp_rpw_sc_rw3_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2]);
    1243           0 :     copyFloatByChar(
    1244             :         (unsigned char*)&rw_f.cp_rpw_sc_rw3_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3]);
    1245           0 :     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           0 :     copyFloatByChar(
    1250             :         (unsigned char*)&rw_f.cp_rpw_sc_rw4_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1]);
    1251           0 :     copyFloatByChar(
    1252             :         (unsigned char*)&rw_f.cp_rpw_sc_rw4_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2]);
    1253           0 :     copyFloatByChar(
    1254             :         (unsigned char*)&rw_f.cp_rpw_sc_rw4_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3]);
    1255           0 :     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           0 : }
    1260             : 
    1261           0 : 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           0 :     int binAbove = 0;
    1287             :     int closestBin;
    1288           0 :     unsigned int whichByte = 0;
    1289             :     int selectedByte;
    1290           0 :     int bin = 0;
    1291             :     int binToRemove[NB_BINS_TO_REMOVE];
    1292             :     int k;
    1293           0 :     bool filteringSet = false;
    1294             : 
    1295           0 :     for (k = 0; k < NB_BINS_TO_REMOVE; k++)
    1296             :     {
    1297           0 :         binToRemove[k] = -1;
    1298             :     }
    1299             : 
    1300           0 :     if (!isnan(rw_f_comp))
    1301             :     {
    1302             :         // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
    1303           0 :         f_RW_min = rw_f_comp - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
    1304           0 :         f_RW_MAX = rw_f_comp + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
    1305             : 
    1306           0 :         freqToFilterOut = f_RW_min;
    1307           0 :         while (filteringSet == false)
    1308             :         {
    1309             :             // compute the index of the frequency bin immediately below rw_f
    1310           0 :             binBelow = (int)(floor(((double)freqToFilterOut) / ((double)deltaFreq)));
    1311           0 :             deltaBelow = freqToFilterOut - (float)(binBelow * deltaFreq);
    1312             : 
    1313             :             // compute the index of the frequency bin immediately above rw_f
    1314           0 :             binAbove = (int)(ceil(((double)freqToFilterOut) / ((double)deltaFreq)));
    1315           0 :             deltaAbove = (float)(binAbove * deltaFreq) - freqToFilterOut;
    1316             : 
    1317             :             // search the closest bin
    1318           0 :             if (deltaAbove > deltaBelow)
    1319             :             {
    1320           0 :                 closestBin = binBelow;
    1321             :             }
    1322             :             else
    1323             :             {
    1324           0 :                 closestBin = binAbove;
    1325             :             }
    1326             : 
    1327             :             // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
    1328           0 :             fi = (float)closestBin * deltaFreq;
    1329           0 :             fi_min = fi - ((float)deltaFreq * (float)FI_INTERVAL_COEFF);
    1330           0 :             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           0 :             if ((freqToFilterOut > fi_min) && (freqToFilterOut < fi_MAX))
    1340             :             {
    1341           0 :                 binToRemove[0] = (closestBin - 1) - 1;
    1342           0 :                 binToRemove[1] = closestBin - 1;
    1343           0 :                 binToRemove[2] = (closestBin + 1) - 1;
    1344             :             }
    1345             :             // 2. ELSE
    1346             :             // => remove the two f_(i) which are around f_RW
    1347             :             else
    1348             :             {
    1349           0 :                 binToRemove[0] = binBelow - 1;
    1350           0 :                 binToRemove[1] = binAbove - 1;
    1351           0 :                 binToRemove[2] = -1;
    1352             :             }
    1353             : 
    1354           0 :             for (k = 0; k < NB_BINS_TO_REMOVE; k++)
    1355             :             {
    1356           0 :                 bin = binToRemove[k];
    1357           0 :                 if ((bin >= BIN_MIN) && (bin <= BIN_MAX))
    1358             :                 {
    1359           0 :                     whichByte = (bin >> SHIFT_3_BITS); // division by 8
    1360           0 :                     selectedByte = (1 << (bin - (whichByte * BITS_PER_BYTE)));
    1361           0 :                     fbins_mask[BYTES_PER_MASK - 1 - whichByte]
    1362           0 :                         = fbins_mask[BYTES_PER_MASK - 1 - whichByte]
    1363           0 :                         & ((unsigned char)(~selectedByte)); // bytes are ordered MSB first in the
    1364             :                                                             // packets
    1365             :                 }
    1366             :             }
    1367             : 
    1368             :             // update freqToFilterOut
    1369           0 :             if (freqToFilterOut == f_RW_MAX)
    1370             :             {
    1371           0 :                 filteringSet = true; // end of the loop
    1372             :             }
    1373             :             else
    1374             :             {
    1375           0 :                 freqToFilterOut = freqToFilterOut + deltaFreq;
    1376             :             }
    1377             : 
    1378           0 :             if (freqToFilterOut > f_RW_MAX)
    1379             :             {
    1380           0 :                 freqToFilterOut = f_RW_MAX;
    1381             :             }
    1382             :         }
    1383             :     }
    1384           0 : }
    1385             : 
    1386           0 : void build_sy_lfr_rw_mask(unsigned int channel)
    1387             : {
    1388             :     unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
    1389           0 :     unsigned char* maskPtr = NULL;
    1390           0 :     unsigned char deltaF = (unsigned char)DELTAF_F2;
    1391             :     unsigned k;
    1392             : 
    1393           0 :     switch (channel)
    1394             :     {
    1395             :         case CHANNELF0:
    1396           0 :             maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
    1397           0 :             deltaF = (unsigned char)DELTAF_F0;
    1398           0 :             break;
    1399             :         case CHANNELF1:
    1400           0 :             maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
    1401           0 :             deltaF = (unsigned char)DELTAF_F1;
    1402           0 :             break;
    1403             :         case CHANNELF2:
    1404           0 :             maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
    1405           0 :             deltaF = (unsigned char)DELTAF_F2;
    1406             :             break;
    1407             :         default:
    1408             :             break;
    1409             :     }
    1410             : 
    1411           0 :     for (k = 0; k < BYTES_PER_MASK; k++)
    1412             :     {
    1413           0 :         local_rw_fbins_mask[k] = INT8_ALL_F;
    1414             :     }
    1415             : 
    1416             :     // RW1
    1417           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1);
    1418           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2);
    1419           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3);
    1420           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4);
    1421             : 
    1422             :     // RW2
    1423           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1);
    1424           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2);
    1425           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3);
    1426           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4);
    1427             : 
    1428             :     // RW3
    1429           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1);
    1430           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2);
    1431           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3);
    1432           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4);
    1433             : 
    1434             :     // RW4
    1435           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1);
    1436           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2);
    1437           0 :     setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3);
    1438           0 :     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           0 :     if (maskPtr != NULL)
    1442             :     {
    1443           0 :         for (k = 0; k < BYTES_PER_MASK; k++)
    1444             :         {
    1445           0 :             maskPtr[k] = local_rw_fbins_mask[k];
    1446             :         }
    1447             :     }
    1448           0 : }
    1449             : 
    1450           0 : void build_sy_lfr_rw_masks(void)
    1451             : {
    1452           0 :     build_sy_lfr_rw_mask(CHANNELF0);
    1453           0 :     build_sy_lfr_rw_mask(CHANNELF1);
    1454           0 :     build_sy_lfr_rw_mask(CHANNELF2);
    1455           0 : }
    1456             : 
    1457           1 : void merge_fbins_masks(void)
    1458             : {
    1459           1 :     const unsigned char* const fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
    1460           1 :     const unsigned char* const fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
    1461           1 :     const unsigned char* const fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
    1462           1 :     const unsigned char* const rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
    1463           1 :     const unsigned char* const rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
    1464           1 :     const unsigned char* const rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
    1465             : 
    1466             : 
    1467          17 :     for (unsigned char k = 0; k < BYTES_PER_MASK; k++)
    1468             :     {
    1469          16 :         fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
    1470          16 :         fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
    1471          16 :         fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
    1472             :     }
    1473           1 : }
    1474             : 
    1475             : //***********
    1476             : // FBINS MASK
    1477             : 
    1478           0 : int set_sy_lfr_fbins(const ccsdsTelecommandPacket_t* const TC)
    1479             : {
    1480           0 :     unsigned char* const fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
    1481           0 :     const unsigned char* const fbins_mask_TC = TC->dataAndCRC;
    1482             : 
    1483           0 :     for (unsigned int k = 0; k < BYTES_PER_MASKS_SET; k++)
    1484             :     {
    1485           0 :         fbins_mask_dump[k] = fbins_mask_TC[k];
    1486             :     }
    1487             : 
    1488           0 :     return LFR_SUCCESSFUL;
    1489             : }
    1490             : 
    1491             : //***************************
    1492             : // TC_LFR_LOAD_PAS_FILTER_PAR
    1493             : 
    1494           0 : 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           0 :     ret = LFR_SUCCESSFUL;
    1500           0 :     rw_k = INIT_FLOAT;
    1501             : 
    1502           0 :     copyFloatByChar((unsigned char*)&rw_k, &TC->dataAndCRC[offset]);
    1503             : 
    1504           0 :     *pos = offset;
    1505           0 :     *value = rw_k;
    1506             : 
    1507           0 :     if (rw_k < MIN_SY_LFR_RW_F)
    1508             :     {
    1509           0 :         ret = WRONG_APP_DATA;
    1510             :     }
    1511             : 
    1512           0 :     return ret;
    1513             : }
    1514             : 
    1515           0 : int check_all_sy_lfr_rw_k(const ccsdsTelecommandPacket_t* const TC, int* pos, float* value)
    1516             : {
    1517           0 :     int ret = LFR_SUCCESSFUL;
    1518             : 
    1519             :     //****
    1520             :     //****
    1521             :     // RW1
    1522           0 :     ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value); // K1
    1523           0 :     if (ret == LFR_SUCCESSFUL) // K2
    1524             :     {
    1525           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value);
    1526             :     }
    1527           0 :     if (ret == LFR_SUCCESSFUL) // K3
    1528             :     {
    1529           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value);
    1530             :     }
    1531           0 :     if (ret == LFR_SUCCESSFUL) // K4
    1532             :     {
    1533           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value);
    1534             :     }
    1535             : 
    1536             :     //****
    1537             :     //****
    1538             :     // RW2
    1539           0 :     if (ret == LFR_SUCCESSFUL) // K1
    1540             :     {
    1541           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value);
    1542             :     }
    1543           0 :     if (ret == LFR_SUCCESSFUL) // K2
    1544             :     {
    1545           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value);
    1546             :     }
    1547           0 :     if (ret == LFR_SUCCESSFUL) // K3
    1548             :     {
    1549           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value);
    1550             :     }
    1551           0 :     if (ret == LFR_SUCCESSFUL) // K4
    1552             :     {
    1553           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value);
    1554             :     }
    1555             : 
    1556             :     //****
    1557             :     //****
    1558             :     // RW3
    1559           0 :     if (ret == LFR_SUCCESSFUL) // K1
    1560             :     {
    1561           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value);
    1562             :     }
    1563           0 :     if (ret == LFR_SUCCESSFUL) // K2
    1564             :     {
    1565           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value);
    1566             :     }
    1567           0 :     if (ret == LFR_SUCCESSFUL) // K3
    1568             :     {
    1569           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value);
    1570             :     }
    1571           0 :     if (ret == LFR_SUCCESSFUL) // K4
    1572             :     {
    1573           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value);
    1574             :     }
    1575             : 
    1576             :     //****
    1577             :     //****
    1578             :     // RW4
    1579           0 :     if (ret == LFR_SUCCESSFUL) // K1
    1580             :     {
    1581           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value);
    1582             :     }
    1583           0 :     if (ret == LFR_SUCCESSFUL) // K2
    1584             :     {
    1585           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value);
    1586             :     }
    1587           0 :     if (ret == LFR_SUCCESSFUL) // K3
    1588             :     {
    1589           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value);
    1590             :     }
    1591           0 :     if (ret == LFR_SUCCESSFUL) // K4
    1592             :     {
    1593           0 :         ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value);
    1594             :     }
    1595             : 
    1596           0 :     return ret;
    1597             : }
    1598             : 
    1599           0 : int check_sy_lfr_filter_parameters(const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id)
    1600             : {
    1601           0 :     int flag = LFR_SUCCESSFUL;
    1602             : 
    1603             :     unsigned char sy_lfr_pas_filter_modulus;
    1604           0 :     float sy_lfr_pas_filter_tbad = 0.f;
    1605             :     unsigned char sy_lfr_pas_filter_offset;
    1606           0 :     float sy_lfr_pas_filter_shift = 0.f;
    1607           0 :     float sy_lfr_sc_rw_delta_f = 0.f;
    1608           0 :     const char* parPtr = NULL;
    1609           0 :     int datafield_pos = 0;
    1610           0 :     float rw_k = 0.f;
    1611             : 
    1612             : 
    1613             :     //***************
    1614             :     // get parameters
    1615           0 :     sy_lfr_pas_filter_modulus = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS];
    1616           0 :     copyFloatByChar((unsigned char*)&sy_lfr_pas_filter_tbad,
    1617             :         &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD]);
    1618           0 :     sy_lfr_pas_filter_offset = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET];
    1619           0 :     copyFloatByChar((unsigned char*)&sy_lfr_pas_filter_shift,
    1620             :         &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT]);
    1621           0 :     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           0 :     if ((sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS)
    1634             :         || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS))
    1635             :     {
    1636           0 :         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           0 :         flag = WRONG_APP_DATA;
    1639             :     }
    1640             : 
    1641             :     //***********************
    1642             :     // sy_lfr_pas_filter_tbad
    1643           0 :     if (flag == LFR_SUCCESSFUL)
    1644             :     {
    1645           0 :         if ((sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD)
    1646           0 :             || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD))
    1647             :         {
    1648           0 :             parPtr = (const char*)&sy_lfr_pas_filter_tbad;
    1649           0 :             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           0 :             flag = WRONG_APP_DATA;
    1652             :         }
    1653             :     }
    1654             : 
    1655             :     //*************************
    1656             :     // sy_lfr_pas_filter_offset
    1657           0 :     if (flag == LFR_SUCCESSFUL && sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET)
    1658             :     {
    1659           0 :         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           0 :         flag = WRONG_APP_DATA;
    1662             :     }
    1663             : 
    1664             :     //************************
    1665             :     // sy_lfr_pas_filter_shift
    1666           0 :     if (flag == LFR_SUCCESSFUL)
    1667             :     {
    1668           0 :         if ((sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT)
    1669           0 :             || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT))
    1670             :         {
    1671           0 :             parPtr = (const char*)&sy_lfr_pas_filter_shift;
    1672           0 :             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           0 :             flag = WRONG_APP_DATA;
    1675             :         }
    1676             :     }
    1677             : 
    1678             :     //*************************************
    1679             :     // check global coherency of the values
    1680           0 :     if (flag == LFR_SUCCESSFUL)
    1681             :     {
    1682           0 :         if ((sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) >= sy_lfr_pas_filter_modulus)
    1683             :         {
    1684           0 :             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           0 :             flag = WRONG_APP_DATA;
    1688             :         }
    1689             :     }
    1690             : 
    1691             :     //*********************
    1692             :     // sy_lfr_sc_rw_delta_f
    1693           0 :     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           0 :     if (flag == LFR_SUCCESSFUL)
    1704             :     {
    1705           0 :         flag = check_all_sy_lfr_rw_k(TC, &datafield_pos, &rw_k);
    1706           0 :         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           0 :     return flag;
    1715             : }
    1716             : 
    1717             : 
    1718           0 : 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           0 :     unsigned int elements_gap = gap * floats_per_matrix;
    1723           0 :     for (unsigned int major_bin = 0; major_bin < matrices_count; major_bin++)
    1724             :     {
    1725           0 :         for (unsigned int element = 0; element < floats_per_matrix; element++)
    1726             :         {
    1727           0 :             float alpha = (matrix[elements_gap] - matrix[0]) / (float)gap;
    1728           0 :             float previous_value = matrix[0];
    1729           0 :             for (unsigned int minor_bin = floats_per_matrix; minor_bin < elements_gap;
    1730           0 :                  minor_bin += floats_per_matrix)
    1731             :             {
    1732           0 :                 previous_value += alpha;
    1733           0 :                 matrix[minor_bin] = previous_value;
    1734             :             }
    1735           0 :             matrix++;
    1736             :         }
    1737           0 :         matrix += floats_per_matrix * (gap - 1);
    1738             :     }
    1739           0 : }
    1740             : 
    1741             : //**************
    1742             : // KCOEFFICIENTS
    1743           0 : 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           0 :     unsigned short sy_lfr_kcoeff_frequency = 255;
    1760           0 :     int status = LFR_SUCCESSFUL;
    1761             : 
    1762           0 :     unsigned int matrix_index = 0;
    1763           0 :     float* mag_matrix_ptr = NULL;
    1764           0 :     float* elec_matrix_ptr = NULL;
    1765           0 :     float* extra_mag_matrix_ptr = NULL;
    1766           0 :     float* extra_elec_matrix_ptr = NULL;
    1767             :     enum
    1768             :     {
    1769             :         no = 0,
    1770             :         yes = 1
    1771             :     } interpolate
    1772           0 :         = 0;
    1773             : 
    1774             :     // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
    1775           0 :     copyInt16ByChar((unsigned char*)&sy_lfr_kcoeff_frequency,
    1776             :         &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY]);
    1777             : 
    1778             : 
    1779           0 :     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           0 :         status = send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
    1784             :             DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET,
    1785           0 :             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           0 :         status = LFR_DEFAULT;
    1789             :     }
    1790             :     else
    1791             :     {
    1792           0 :         if (sy_lfr_kcoeff_frequency >= F2_COMPRESSED_BIN_OFFSET)
    1793             :         {
    1794           0 :             matrix_index = (sy_lfr_kcoeff_frequency - F2_COMPRESSED_BIN_OFFSET);
    1795           0 :             mag_matrix_ptr = mag_calibration_matrices_f2
    1796           0 :                 + (matrix_index * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F2);
    1797           0 :             elec_matrix_ptr = elec_calibration_matrices_f2
    1798           0 :                 + (matrix_index * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F2);
    1799           0 :             extra_mag_matrix_ptr
    1800             :                 = mag_calibration_matrices_f2 + (ASM_F2_KEEP_BINS * NB_FLOATS_MAG_CAL_MATRIX);
    1801           0 :             extra_elec_matrix_ptr
    1802             :                 = elec_calibration_matrices_f2 + (ASM_F2_KEEP_BINS * NB_FLOATS_ELEC_CAL_MATRIX);
    1803             : 
    1804           0 :             if (sy_lfr_kcoeff_frequency == NB_BINS_COMPRESSED_SM - 1)
    1805             :             {
    1806           0 :                 interpolate = yes;
    1807             :             }
    1808             :         }
    1809           0 :         else if (sy_lfr_kcoeff_frequency >= F1_COMPRESSED_BIN_OFFSET)
    1810             :         {
    1811           0 :             matrix_index = (sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0);
    1812           0 :             mag_matrix_ptr = mag_calibration_matrices_f1
    1813           0 :                 + (matrix_index * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F1);
    1814           0 :             elec_matrix_ptr = elec_calibration_matrices_f1
    1815           0 :                 + (matrix_index * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F1);
    1816           0 :             extra_mag_matrix_ptr
    1817             :                 = mag_calibration_matrices_f1 + (ASM_F1_KEEP_BINS * NB_FLOATS_MAG_CAL_MATRIX);
    1818           0 :             extra_elec_matrix_ptr
    1819             :                 = elec_calibration_matrices_f1 + (ASM_F1_KEEP_BINS * NB_FLOATS_ELEC_CAL_MATRIX);
    1820             :         }
    1821             :         else
    1822             :         {
    1823           0 :             matrix_index = sy_lfr_kcoeff_frequency;
    1824           0 :             mag_matrix_ptr = mag_calibration_matrices_f0
    1825           0 :                 + (matrix_index * NB_FLOATS_MAG_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F0);
    1826           0 :             elec_matrix_ptr = elec_calibration_matrices_f0
    1827           0 :                 + (matrix_index * NB_FLOATS_ELEC_CAL_MATRIX * NB_BINS_TO_AVERAGE_ASM_F0);
    1828           0 :             extra_mag_matrix_ptr
    1829             :                 = mag_calibration_matrices_f0 + (ASM_F0_KEEP_BINS * NB_FLOATS_MAG_CAL_MATRIX);
    1830           0 :             extra_elec_matrix_ptr
    1831             :                 = elec_calibration_matrices_f0 + (ASM_F0_KEEP_BINS * NB_FLOATS_ELEC_CAL_MATRIX);
    1832             :         }
    1833             :     }
    1834             : 
    1835           0 :     if (mag_matrix_ptr != NULL && elec_matrix_ptr != NULL)
    1836             :     {
    1837           0 :         memcpy((void*)mag_matrix_ptr, TC->dataAndCRC + DATAFIELD_POS_SY_LFR_MAG_CAL_MATRIX,
    1838             :             NB_BYTES_MAG_CAL_MATRIX);
    1839           0 :         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           0 :     if (extra_mag_matrix_ptr != NULL && matrix_index < 3)
    1845             :     {
    1846           0 :         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           0 :     else if (extra_elec_matrix_ptr != NULL
    1851           0 :         && matrix_index < 5) // The 2 following packets contains one line of last ELEC CAL MATRIX
    1852             :     {
    1853           0 :         memcpy(extra_elec_matrix_ptr
    1854           0 :                 + ((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           0 :     if (interpolate == yes)
    1860             :     {
    1861           0 :         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           0 :         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           0 :         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           0 :         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           0 :         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           0 :         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           0 :     return status;
    1879             : }
    1880             : 
    1881           0 : void copyFloatByChar(unsigned char* destination, const unsigned char* const source)
    1882             : {
    1883           0 :     destination[BYTE_0] = source[BYTE_0];
    1884           0 :     destination[BYTE_1] = source[BYTE_1];
    1885           0 :     destination[BYTE_2] = source[BYTE_2];
    1886           0 :     destination[BYTE_3] = source[BYTE_3];
    1887           0 : }
    1888             : 
    1889           6 : void copyInt32ByChar(unsigned char* destination, const unsigned char* const source)
    1890             : {
    1891           6 :     destination[BYTE_0] = source[BYTE_0];
    1892           6 :     destination[BYTE_1] = source[BYTE_1];
    1893           6 :     destination[BYTE_2] = source[BYTE_2];
    1894           6 :     destination[BYTE_3] = source[BYTE_3];
    1895           6 : }
    1896             : 
    1897           0 : void copyInt16ByChar(unsigned char* destination, const unsigned char* const source)
    1898             : {
    1899           0 :     destination[BYTE_0] = source[BYTE_0];
    1900           0 :     destination[BYTE_1] = source[BYTE_1];
    1901           0 : }
    1902             : 
    1903           0 : void floatToChar(float value, unsigned char* ptr)
    1904             : {
    1905          19 :     const unsigned char* const valuePtr = (const unsigned char*)&value;
    1906             : 
    1907          19 :     ptr[BYTE_0] = valuePtr[BYTE_0];
    1908          19 :     ptr[BYTE_1] = valuePtr[BYTE_1];
    1909          19 :     ptr[BYTE_2] = valuePtr[BYTE_2];
    1910          19 :     ptr[BYTE_3] = valuePtr[BYTE_3];
    1911           0 : }
    1912             : 
    1913             : //**********
    1914             : // init dump
    1915             : 
    1916           1 : 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           1 :     parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
    1925           1 :     parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
    1926           1 :     parameter_dump_packet.reserved = CCSDS_RESERVED;
    1927           1 :     parameter_dump_packet.userApplication = CCSDS_USER_APP;
    1928           1 :     parameter_dump_packet.packetID[0] = (unsigned char)(APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
    1929           1 :     parameter_dump_packet.packetID[1] = (unsigned char)APID_TM_PARAMETER_DUMP;
    1930           1 :     parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
    1931           1 :     parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
    1932           1 :     parameter_dump_packet.packetLength[0]
    1933             :         = (unsigned char)(PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
    1934           1 :     parameter_dump_packet.packetLength[1] = (unsigned char)PACKET_LENGTH_PARAMETER_DUMP;
    1935             :     // DATA FIELD HEADER
    1936           1 :     parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
    1937           1 :     parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
    1938           1 :     parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
    1939           1 :     parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
    1940           1 :     parameter_dump_packet.time[BYTE_0]
    1941           1 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
    1942           1 :     parameter_dump_packet.time[BYTE_1]
    1943           1 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
    1944           1 :     parameter_dump_packet.time[BYTE_2]
    1945           1 :         = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
    1946           1 :     parameter_dump_packet.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
    1947           1 :     parameter_dump_packet.time[BYTE_4]
    1948           1 :         = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
    1949           1 :     parameter_dump_packet.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
    1950           1 :     parameter_dump_packet.sid = SID_PARAMETER_DUMP;
    1951             : 
    1952             :     //******************
    1953             :     // COMMON PARAMETERS
    1954           1 :     parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
    1955           1 :     parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
    1956             : 
    1957             :     //******************
    1958             :     // NORMAL PARAMETERS
    1959           1 :     parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char)(DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
    1960           1 :     parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char)(DFLT_SY_LFR_N_SWF_L);
    1961           1 :     parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char)(DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
    1962           1 :     parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char)(DFLT_SY_LFR_N_SWF_P);
    1963           1 :     parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char)(DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
    1964           1 :     parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char)(DFLT_SY_LFR_N_ASM_P);
    1965           1 :     parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char)DFLT_SY_LFR_N_BP_P0;
    1966           1 :     parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char)DFLT_SY_LFR_N_BP_P1;
    1967           1 :     parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char)DFLT_SY_LFR_N_CWF_LONG_F3;
    1968             : 
    1969             :     //*****************
    1970             :     // BURST PARAMETERS
    1971           1 :     parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char)DEFAULT_SY_LFR_B_BP_P0;
    1972           1 :     parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char)DEFAULT_SY_LFR_B_BP_P1;
    1973             : 
    1974             :     //****************
    1975             :     // SBM1 PARAMETERS
    1976           1 :     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           1 :     parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char)DEFAULT_SY_LFR_S1_BP_P1;
    1979             : 
    1980             :     //****************
    1981             :     // SBM2 PARAMETERS
    1982           1 :     parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char)DEFAULT_SY_LFR_S2_BP_P0;
    1983           1 :     parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char)DEFAULT_SY_LFR_S2_BP_P1;
    1984             : 
    1985             :     //************
    1986             :     // FBINS MASKS
    1987          49 :     for (k = 0; k < BYTES_PER_MASKS_SET; k++)
    1988             :     {
    1989          48 :         parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
    1990             :     }
    1991             : 
    1992             :     // PAS FILTER PARAMETERS
    1993           1 :     parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
    1994           1 :     parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
    1995           1 :     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           1 :     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          49 :     for (k = 0; k < BYTES_PER_MASKS_SET; k++)
    2024             :     {
    2025          48 :         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           1 :     merge_fbins_masks();
    2031           1 : }
    2032             : 
    2033           1 : void init_kcoefficients_dump(void)
    2034             : {
    2035           1 :     init_kcoefficients_dump_packet(&kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1);
    2036           1 :     init_kcoefficients_dump_packet(&kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2);
    2037             : 
    2038           1 :     kcoefficient_node_1.previous = NULL;
    2039           1 :     kcoefficient_node_1.next = NULL;
    2040           1 :     kcoefficient_node_1.packet_id = TM_K_DUMP_PKT_ID;
    2041           1 :     kcoefficient_node_1.coarseTime = INIT_CHAR;
    2042           1 :     kcoefficient_node_1.fineTime = INIT_CHAR;
    2043           1 :     kcoefficient_node_1.buffer_address = &kcoefficients_dump_1;
    2044           1 :     kcoefficient_node_1.status = INIT_CHAR;
    2045             : 
    2046           1 :     kcoefficient_node_2.previous = NULL;
    2047           1 :     kcoefficient_node_2.next = NULL;
    2048           1 :     kcoefficient_node_2.packet_id = TM_K_DUMP_PKT_ID;
    2049           1 :     kcoefficient_node_2.coarseTime = INIT_CHAR;
    2050           1 :     kcoefficient_node_2.fineTime = INIT_CHAR;
    2051           1 :     kcoefficient_node_2.buffer_address = &kcoefficients_dump_2;
    2052           1 :     kcoefficient_node_2.status = INIT_CHAR;
    2053           1 : }
    2054             : 
    2055           2 : 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           2 :     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           2 :     kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
    2065           2 :     kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
    2066           2 :     kcoefficients_dump->reserved = CCSDS_RESERVED;
    2067           2 :     kcoefficients_dump->userApplication = CCSDS_USER_APP;
    2068           2 :     kcoefficients_dump->packetID[0] = (unsigned char)(APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
    2069           2 :     kcoefficients_dump->packetID[1] = (unsigned char)APID_TM_PARAMETER_DUMP;
    2070           2 :     kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
    2071           2 :     kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
    2072           2 :     kcoefficients_dump->packetLength[0] = (unsigned char)(packetLength >> SHIFT_1_BYTE);
    2073           2 :     kcoefficients_dump->packetLength[1] = (unsigned char)packetLength;
    2074             :     // DATA FIELD HEADER
    2075           2 :     kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
    2076           2 :     kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
    2077           2 :     kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
    2078           2 :     kcoefficients_dump->destinationID = TM_DESTINATION_ID_GROUND;
    2079           2 :     kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
    2080           2 :     kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
    2081           2 :     kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
    2082           2 :     kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
    2083           2 :     kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
    2084           2 :     kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
    2085           2 :     kcoefficients_dump->sid = SID_K_DUMP;
    2086             : 
    2087           2 :     kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
    2088           2 :     kcoefficients_dump->pkt_nr = pkt_nr;
    2089           2 :     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           2 :     memset(kcoefficients_dump->kcoeff_blks, 0, KCOEFF_BLK_MAX_SZ);
    2095           2 : }
    2096             : 
    2097           0 : 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           0 :     switch (destination_id)
    2117             :     {
    2118             :         case SID_TC_GROUND:
    2119           0 :             i = GROUND;
    2120           0 :             break;
    2121             :         case SID_TC_MISSION_TIMELINE:
    2122           0 :             i = MISSION_TIMELINE;
    2123           0 :             break;
    2124             :         case SID_TC_TC_SEQUENCES:
    2125           0 :             i = TC_SEQUENCES;
    2126           0 :             break;
    2127             :         case SID_TC_RECOVERY_ACTION_CMD:
    2128           0 :             i = RECOVERY_ACTION_CMD;
    2129           0 :             break;
    2130             :         case SID_TC_BACKUP_MISSION_TIMELINE:
    2131           0 :             i = BACKUP_MISSION_TIMELINE;
    2132           0 :             break;
    2133             :         case SID_TC_DIRECT_CMD:
    2134           0 :             i = DIRECT_CMD;
    2135           0 :             break;
    2136             :         case SID_TC_SPARE_GRD_SRC1:
    2137           0 :             i = SPARE_GRD_SRC1;
    2138           0 :             break;
    2139             :         case SID_TC_SPARE_GRD_SRC2:
    2140           0 :             i = SPARE_GRD_SRC2;
    2141           0 :             break;
    2142             :         case SID_TC_OBCP:
    2143           0 :             i = OBCP;
    2144           0 :             break;
    2145             :         case SID_TC_SYSTEM_CONTROL:
    2146           0 :             i = SYSTEM_CONTROL;
    2147           0 :             break;
    2148             :         case SID_TC_AOCS:
    2149           0 :             i = AOCS;
    2150           0 :             break;
    2151             :         case SID_TC_RPW_INTERNAL:
    2152           0 :             i = RPW_INTERNAL;
    2153           0 :             break;
    2154             :         default:
    2155           0 :             i = GROUND;
    2156             :             break;
    2157             :     }
    2158             : 
    2159           0 :     segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
    2160           0 :     sequence_cnt = sequenceCounters_TM_DUMP[i] & SEQ_CNT_MASK;
    2161             : 
    2162           0 :     new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt;
    2163             : 
    2164           0 :     packet_sequence_control[0] = (unsigned char)(new_packet_sequence_control >> SHIFT_1_BYTE);
    2165           0 :     packet_sequence_control[1] = (unsigned char)(new_packet_sequence_control);
    2166             : 
    2167             :     // increment the sequence counter
    2168           0 :     if (sequenceCounters_TM_DUMP[i] < SEQ_CNT_MAX)
    2169             :     {
    2170           0 :         sequenceCounters_TM_DUMP[i] = sequenceCounters_TM_DUMP[i] + 1;
    2171             :     }
    2172             :     else
    2173             :     {
    2174           0 :         sequenceCounters_TM_DUMP[i] = 0;
    2175             :     }
    2176           0 : }

Generated by: LCOV version 1.14