GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/tc_load_dump_parameters.c Lines: 135 834 16.2 %
Date: 2018-10-22 12:27:55 Branches: 8 280 2.9 %

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