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 20 : 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 20 : parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
68 20 : parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
69 20 : set_wfp_data_shaping();
70 20 : return LFR_SUCCESSFUL;
71 : }
72 :
73 14 : 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 14 : int flag = LFR_SUCCESSFUL;
85 :
86 28 : if ((lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
87 28 : || (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 14 : if (flag == LFR_SUCCESSFUL)
95 : {
96 14 : flag = check_normal_par_consistency(TC, queue_id);
97 : }
98 :
99 : // SET THE PARAMETERS IF THEY ARE CONSISTENT
100 14 : if (flag == LFR_SUCCESSFUL)
101 : {
102 5 : flag |= set_sy_lfr_n_swf_l(TC);
103 5 : flag |= set_sy_lfr_n_swf_p(TC);
104 5 : flag |= set_sy_lfr_n_bp_p0(TC);
105 5 : flag |= set_sy_lfr_n_bp_p1(TC);
106 5 : flag |= set_sy_lfr_n_asm_p(TC);
107 5 : flag |= set_sy_lfr_n_cwf_long_f3(TC);
108 : }
109 :
110 14 : return flag;
111 : }
112 :
113 21 : 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 21 : flag = LFR_SUCCESSFUL;
130 :
131 21 : 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 21 : sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
139 21 : 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 21 : if (flag == LFR_SUCCESSFUL && sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0)
143 : {
144 4 : 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 4 : flag = WRONG_APP_DATA;
148 : }
149 : // sy_lfr_b_bp_p1 shall not be lower than its default value
150 21 : 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 21 : if (flag == LFR_SUCCESSFUL)
161 : {
162 17 : sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
163 17 : sy_lfr_b_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P1];
164 17 : aux = ((float)sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0) - floorf(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
165 17 : if (aux > FLOAT_EQUAL_ZERO)
166 : {
167 1 : status = send_tm_lfr_tc_exe_inconsistent(
168 : TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0);
169 : DEBUG_CHECK_STATUS(status);
170 1 : flag = LFR_DEFAULT;
171 : }
172 : }
173 :
174 : // SET THE PARAMETERS
175 21 : if (flag == LFR_SUCCESSFUL)
176 : {
177 16 : flag = set_sy_lfr_b_bp_p0(TC);
178 16 : flag |= set_sy_lfr_b_bp_p1(TC);
179 : }
180 :
181 21 : return flag;
182 : }
183 :
184 21 : 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 21 : flag = LFR_SUCCESSFUL;
202 :
203 21 : 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 21 : sy_lfr_s1_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P0];
211 21 : sy_lfr_s1_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P1];
212 :
213 : // sy_lfr_s1_bp_p0
214 21 : if (flag == LFR_SUCCESSFUL && sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0)
215 : {
216 :
217 4 : 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 4 : flag = WRONG_APP_DATA;
221 : }
222 : // sy_lfr_s1_bp_p1
223 21 : 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 21 : if (flag == LFR_SUCCESSFUL)
234 : {
235 34 : aux = ((float)sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * (float)S1_BP_P0_SCALE))
236 17 : - floorf(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * (float)S1_BP_P0_SCALE));
237 17 : if (aux > FLOAT_EQUAL_ZERO)
238 : {
239 1 : status = send_tm_lfr_tc_exe_inconsistent(
240 : TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0);
241 : DEBUG_CHECK_STATUS(status);
242 1 : flag = LFR_DEFAULT;
243 : }
244 : }
245 :
246 : // SET THE PARAMETERS
247 21 : if (flag == LFR_SUCCESSFUL)
248 : {
249 16 : flag = set_sy_lfr_s1_bp_p0(TC);
250 16 : flag |= set_sy_lfr_s1_bp_p1(TC);
251 : }
252 :
253 21 : return flag;
254 : }
255 :
256 21 : 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 21 : flag = LFR_SUCCESSFUL;
274 :
275 21 : 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 21 : sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
283 21 : sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
284 :
285 : // sy_lfr_s2_bp_p0
286 21 : if (flag == LFR_SUCCESSFUL && sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0)
287 : {
288 :
289 4 : status = send_tm_lfr_tc_exe_inconsistent(
290 : TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0);
291 : DEBUG_CHECK_STATUS(status);
292 4 : flag = WRONG_APP_DATA;
293 : }
294 : // sy_lfr_s2_bp_p1
295 21 : 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 21 : if (flag == LFR_SUCCESSFUL)
306 : {
307 17 : sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
308 17 : sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
309 34 : aux = ((float)sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0)
310 17 : - floorf(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
311 17 : if (aux > FLOAT_EQUAL_ZERO)
312 : {
313 1 : status = send_tm_lfr_tc_exe_inconsistent(
314 : TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0);
315 : DEBUG_CHECK_STATUS(status);
316 1 : flag = LFR_DEFAULT;
317 : }
318 : }
319 :
320 : // SET THE PARAMETERS
321 21 : if (flag == LFR_SUCCESSFUL)
322 : {
323 16 : flag = set_sy_lfr_s2_bp_p0(TC);
324 16 : flag |= set_sy_lfr_s2_bp_p1(TC);
325 : }
326 :
327 21 : 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 16 : 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 16 : int flag = check_sy_lfr_filter_parameters(TC, queue_id);
388 :
389 16 : if (flag == LFR_SUCCESSFUL)
390 : {
391 6 : parameter_dump_packet.spare_sy_lfr_pas_filter_enabled
392 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED];
393 6 : parameter_dump_packet.sy_lfr_pas_filter_modulus
394 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS];
395 6 : parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0]
396 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0];
397 6 : parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1]
398 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1];
399 6 : parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2]
400 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2];
401 6 : parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3]
402 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3];
403 6 : parameter_dump_packet.sy_lfr_pas_filter_offset
404 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET];
405 6 : parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0]
406 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0];
407 6 : parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1]
408 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1];
409 6 : parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2]
410 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2];
411 6 : parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3]
412 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3];
413 6 : parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0]
414 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0];
415 6 : parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1]
416 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1];
417 6 : parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2]
418 6 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2];
419 6 : parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3]
420 6 : = 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 6 : filterPar.spare_sy_lfr_pas_filter_enabled
427 6 : = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
428 6 : set_sy_lfr_pas_filter_enabled(
429 6 : parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED);
430 :
431 : // sy_lfr_pas_filter_modulus
432 6 : filterPar.modulus_in_finetime
433 6 : = ((uint64_t)parameter_dump_packet.sy_lfr_pas_filter_modulus) * CONST_65536;
434 :
435 : // sy_lfr_pas_filter_tbad
436 6 : copyFloatByChar((unsigned char*)&filterPar.sy_lfr_pas_filter_tbad,
437 : parameter_dump_packet.sy_lfr_pas_filter_tbad);
438 6 : filterPar.tbad_in_finetime = (uint64_t)(filterPar.sy_lfr_pas_filter_tbad * CONST_65536);
439 :
440 : // sy_lfr_pas_filter_offset
441 6 : filterPar.offset_in_finetime
442 6 : = ((uint64_t)parameter_dump_packet.sy_lfr_pas_filter_offset) * CONST_65536;
443 :
444 : // sy_lfr_pas_filter_shift
445 6 : copyFloatByChar((unsigned char*)&filterPar.sy_lfr_pas_filter_shift,
446 : parameter_dump_packet.sy_lfr_pas_filter_shift);
447 6 : 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 6 : 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 390 : 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 768 : parameter_dump_packet.sy_lfr_rw1_k1[k]
459 384 : = 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 6 : copyFloatByChar(
466 : (unsigned char*)&filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1);
467 6 : copyFloatByChar(
468 : (unsigned char*)&filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2);
469 6 : copyFloatByChar(
470 : (unsigned char*)&filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3);
471 6 : copyFloatByChar(
472 : (unsigned char*)&filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4);
473 : // rw2_k
474 6 : copyFloatByChar(
475 : (unsigned char*)&filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1);
476 6 : copyFloatByChar(
477 : (unsigned char*)&filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2);
478 6 : copyFloatByChar(
479 : (unsigned char*)&filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3);
480 6 : copyFloatByChar(
481 : (unsigned char*)&filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4);
482 : // rw3_k
483 6 : copyFloatByChar(
484 : (unsigned char*)&filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1);
485 6 : copyFloatByChar(
486 : (unsigned char*)&filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2);
487 6 : copyFloatByChar(
488 : (unsigned char*)&filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3);
489 6 : copyFloatByChar(
490 : (unsigned char*)&filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4);
491 : // rw4_k
492 6 : copyFloatByChar(
493 : (unsigned char*)&filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1);
494 6 : copyFloatByChar(
495 : (unsigned char*)&filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2);
496 6 : copyFloatByChar(
497 : (unsigned char*)&filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3);
498 6 : copyFloatByChar(
499 : (unsigned char*)&filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4);
500 : }
501 :
502 16 : 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 74 : 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 74 : increment_seq_counter_destination_id_dump(
654 74 : parameter_dump_packet.packetSequenceControl, TC->sourceID);
655 74 : parameter_dump_packet.destinationID = TC->sourceID;
656 :
657 : // UPDATE TIME
658 74 : parameter_dump_packet.time[BYTE_0]
659 74 : = (unsigned char)(time_management_regs->coarse_time >> SHIFT_3_BYTES);
660 74 : parameter_dump_packet.time[BYTE_1]
661 74 : = (unsigned char)(time_management_regs->coarse_time >> SHIFT_2_BYTES);
662 74 : parameter_dump_packet.time[BYTE_2]
663 74 : = (unsigned char)(time_management_regs->coarse_time >> SHIFT_1_BYTE);
664 74 : parameter_dump_packet.time[BYTE_3] = (unsigned char)(time_management_regs->coarse_time);
665 74 : parameter_dump_packet.time[BYTE_4]
666 74 : = (unsigned char)(time_management_regs->fine_time >> SHIFT_1_BYTE);
667 74 : parameter_dump_packet.time[BYTE_5] = (unsigned char)(time_management_regs->fine_time);
668 : // SEND DATA
669 74 : status
670 74 : = rtems_message_queue_send(queue_id, ¶meter_dump_packet, sizeof(parameter_dump_packet));
671 : DEBUG_CHECK_STATUS(status);
672 :
673 74 : 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 14 : 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 14 : flag = LFR_SUCCESSFUL;
695 :
696 : //***************
697 : // get parameters
698 14 : msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L];
699 14 : lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L + 1];
700 14 : sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
701 :
702 14 : msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P];
703 14 : lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P + 1];
704 14 : sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
705 :
706 14 : msb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P];
707 14 : lsb = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P + 1];
708 14 : sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
709 :
710 14 : sy_lfr_n_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P0];
711 :
712 14 : 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 14 : if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
718 : {
719 1 : 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 1 : flag = WRONG_APP_DATA;
723 : }
724 : // sy_lfr_n_swf_p
725 14 : if (flag == LFR_SUCCESSFUL && sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P)
726 : {
727 5 : 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 5 : flag = WRONG_APP_DATA;
731 : }
732 : // sy_lfr_n_bp_p0
733 14 : if (flag == LFR_SUCCESSFUL && sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
734 : {
735 1 : status = send_tm_lfr_tc_exe_inconsistent(
736 : TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0);
737 : DEBUG_CHECK_STATUS(status);
738 1 : flag = WRONG_APP_DATA;
739 : }
740 : // sy_lfr_n_asm_p
741 14 : 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 14 : if (flag == LFR_SUCCESSFUL)
750 : {
751 7 : 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 7 : if (aux > FLOAT_EQUAL_ZERO)
754 : {
755 1 : status = send_tm_lfr_tc_exe_inconsistent(
756 : TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p);
757 : DEBUG_CHECK_STATUS(status);
758 1 : flag = WRONG_APP_DATA;
759 : }
760 : }
761 : // sy_lfr_n_bp_p1
762 14 : 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 14 : if (flag == LFR_SUCCESSFUL)
772 : {
773 6 : aux = ((float)sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0) - floorf(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
774 6 : if (aux > FLOAT_EQUAL_ZERO)
775 : {
776 1 : status = send_tm_lfr_tc_exe_inconsistent(
777 : TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1);
778 : DEBUG_CHECK_STATUS(status);
779 1 : flag = LFR_DEFAULT;
780 : }
781 : }
782 :
783 14 : 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 5 : parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L];
796 5 : parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_L + 1];
797 :
798 5 : 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 5 : parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P];
811 5 : parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_SWF_P + 1];
812 :
813 5 : 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 5 : parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P];
827 5 : parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_ASM_P + 1];
828 :
829 5 : 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 5 : parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P0];
842 :
843 5 : 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 5 : parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_BP_P1];
857 :
858 5 : 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 5 : parameter_dump_packet.sy_lfr_n_cwf_long_f3
871 5 : = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3] & BIT_CWF_LONG_F3;
872 :
873 5 : 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 16 : parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P0];
889 :
890 16 : 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 16 : parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_B_BP_P1];
903 :
904 16 : 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 16 : parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P0];
920 :
921 16 : 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 16 : parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S1_BP_P1];
934 :
935 16 : 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 16 : parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P0];
951 :
952 16 : 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 16 : parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_S2_BP_P1];
965 :
966 16 : return LFR_SUCCESSFUL;
967 : }
968 :
969 : //*******************
970 : // TC_LFR_UPDATE_INFO
971 :
972 18 : unsigned int check_update_info_hk_lfr_mode(unsigned char mode)
973 : {
974 18 : unsigned int status = LFR_DEFAULT;
975 :
976 18 : if ((mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL) || (mode == LFR_MODE_BURST)
977 : || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
978 : {
979 18 : status = LFR_SUCCESSFUL;
980 : }
981 : else
982 : {
983 0 : status = LFR_DEFAULT;
984 : }
985 :
986 18 : return status;
987 : }
988 :
989 18 : unsigned int check_update_info_hk_tds_mode(unsigned char mode)
990 : {
991 18 : unsigned int status = LFR_DEFAULT;
992 :
993 18 : 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 18 : status = LFR_SUCCESSFUL;
997 : }
998 : else
999 : {
1000 0 : status = LFR_DEFAULT;
1001 : }
1002 :
1003 18 : return status;
1004 : }
1005 :
1006 18 : unsigned int check_update_info_hk_thr_mode(unsigned char mode)
1007 : {
1008 18 : unsigned int status = LFR_DEFAULT;
1009 :
1010 18 : if ((mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL) || (mode == THR_MODE_BURST))
1011 : {
1012 18 : status = LFR_SUCCESSFUL;
1013 : }
1014 : else
1015 : {
1016 0 : status = LFR_DEFAULT;
1017 : }
1018 :
1019 18 : return status;
1020 : }
1021 :
1022 192 : 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 192 : if (isnan(value))
1032 : {
1033 175 : flag = FLAG_NAN;
1034 : }
1035 : else
1036 : {
1037 17 : flag = FLAG_IAN;
1038 : }
1039 :
1040 192 : switch (wheel)
1041 : {
1042 : case WHEEL_1:
1043 48 : flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1044 48 : flagMask = (unsigned char)(~(1 << flagPosInByte));
1045 48 : newFlag = (unsigned char)(flag << flagPosInByte);
1046 48 : housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags
1047 : = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1048 48 : break;
1049 : case WHEEL_2:
1050 48 : flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1051 48 : flagMask = (unsigned char)(~(1 << flagPosInByte));
1052 48 : newFlag = (unsigned char)(flag << flagPosInByte);
1053 48 : housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags
1054 : = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1055 48 : break;
1056 : case WHEEL_3:
1057 48 : flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1058 48 : flagMask = (unsigned char)(~(1 << flagPosInByte));
1059 48 : newFlag = (unsigned char)(flag << flagPosInByte);
1060 48 : housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags
1061 : = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1062 48 : break;
1063 : case WHEEL_4:
1064 48 : flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1065 48 : flagMask = (unsigned char)(~(1 << flagPosInByte));
1066 48 : newFlag = (unsigned char)(flag << flagPosInByte);
1067 48 : 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 192 : }
1074 :
1075 12 : void set_hk_lfr_sc_rw_f_flags(void)
1076 : {
1077 : // RW1
1078 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1);
1079 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2);
1080 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3);
1081 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4);
1082 :
1083 : // RW2
1084 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1);
1085 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2);
1086 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3);
1087 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4);
1088 :
1089 : // RW3
1090 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1);
1091 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2);
1092 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3);
1093 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4);
1094 :
1095 : // RW4
1096 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1);
1097 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2);
1098 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3);
1099 12 : set_hk_lfr_sc_rw_f_flag(WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4);
1100 12 : }
1101 :
1102 198 : 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 198 : ret = LFR_SUCCESSFUL;
1108 198 : rw_k = INIT_FLOAT;
1109 :
1110 198 : copyFloatByChar((unsigned char*)&rw_k, &TC->packetID[offset]);
1111 :
1112 198 : *pos = offset;
1113 198 : *value = rw_k;
1114 :
1115 198 : if (rw_k < MIN_SY_LFR_RW_F)
1116 : {
1117 6 : ret = WRONG_APP_DATA;
1118 : }
1119 :
1120 198 : return ret;
1121 : }
1122 :
1123 18 : int check_all_sy_lfr_rw_f(const ccsdsTelecommandPacket_t* const TC, int* pos, float* value)
1124 : {
1125 18 : int ret = LFR_SUCCESSFUL;
1126 :
1127 : //****
1128 : //****
1129 : // RW1
1130 18 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value); // F1
1131 18 : if (ret == LFR_SUCCESSFUL) // F2
1132 : {
1133 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value);
1134 : }
1135 18 : if (ret == LFR_SUCCESSFUL) // F3
1136 : {
1137 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value);
1138 : }
1139 18 : if (ret == LFR_SUCCESSFUL) // F4
1140 : {
1141 12 : 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 18 : if (ret == LFR_SUCCESSFUL) // F1
1148 : {
1149 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value);
1150 : }
1151 18 : if (ret == LFR_SUCCESSFUL) // F2
1152 : {
1153 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value);
1154 : }
1155 18 : if (ret == LFR_SUCCESSFUL) // F3
1156 : {
1157 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value);
1158 : }
1159 18 : if (ret == LFR_SUCCESSFUL) // F4
1160 : {
1161 12 : 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 18 : if (ret == LFR_SUCCESSFUL) // F1
1168 : {
1169 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value);
1170 : }
1171 18 : if (ret == LFR_SUCCESSFUL) // F2
1172 : {
1173 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value);
1174 : }
1175 18 : if (ret == LFR_SUCCESSFUL) // F3
1176 : {
1177 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value);
1178 : }
1179 18 : if (ret == LFR_SUCCESSFUL) // F4
1180 : {
1181 12 : 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 18 : if (ret == LFR_SUCCESSFUL) // F1
1188 : {
1189 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value);
1190 : }
1191 18 : if (ret == LFR_SUCCESSFUL) // F2
1192 : {
1193 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value);
1194 : }
1195 18 : if (ret == LFR_SUCCESSFUL) // F3
1196 : {
1197 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value);
1198 : }
1199 18 : if (ret == LFR_SUCCESSFUL) // F4
1200 : {
1201 12 : ret = check_sy_lfr_rw_f(TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value);
1202 : }
1203 :
1204 18 : return ret;
1205 : }
1206 :
1207 12 : 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 12 : const unsigned char* const bytePosPtr = (const unsigned char* const)&TC->packetID;
1217 :
1218 : // rw1_f
1219 12 : copyFloatByChar(
1220 : (unsigned char*)&rw_f.cp_rpw_sc_rw1_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1]);
1221 12 : copyFloatByChar(
1222 : (unsigned char*)&rw_f.cp_rpw_sc_rw1_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2]);
1223 12 : copyFloatByChar(
1224 : (unsigned char*)&rw_f.cp_rpw_sc_rw1_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3]);
1225 12 : 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 12 : copyFloatByChar(
1230 : (unsigned char*)&rw_f.cp_rpw_sc_rw2_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1]);
1231 12 : copyFloatByChar(
1232 : (unsigned char*)&rw_f.cp_rpw_sc_rw2_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2]);
1233 12 : copyFloatByChar(
1234 : (unsigned char*)&rw_f.cp_rpw_sc_rw2_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3]);
1235 12 : 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 12 : copyFloatByChar(
1240 : (unsigned char*)&rw_f.cp_rpw_sc_rw3_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1]);
1241 12 : copyFloatByChar(
1242 : (unsigned char*)&rw_f.cp_rpw_sc_rw3_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2]);
1243 12 : copyFloatByChar(
1244 : (unsigned char*)&rw_f.cp_rpw_sc_rw3_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3]);
1245 12 : 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 12 : copyFloatByChar(
1250 : (unsigned char*)&rw_f.cp_rpw_sc_rw4_f1, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1]);
1251 12 : copyFloatByChar(
1252 : (unsigned char*)&rw_f.cp_rpw_sc_rw4_f2, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2]);
1253 12 : copyFloatByChar(
1254 : (unsigned char*)&rw_f.cp_rpw_sc_rw4_f3, &bytePosPtr[BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3]);
1255 12 : 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 12 : }
1260 :
1261 576 : 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 576 : int binAbove = 0;
1287 : int closestBin;
1288 576 : unsigned int whichByte = 0;
1289 : int selectedByte;
1290 576 : int bin = 0;
1291 : int binToRemove[NB_BINS_TO_REMOVE];
1292 : int k;
1293 576 : bool filteringSet = false;
1294 :
1295 2304 : for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1296 : {
1297 1728 : binToRemove[k] = -1;
1298 : }
1299 :
1300 576 : if (!isnan(rw_f_comp))
1301 : {
1302 : // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
1303 51 : f_RW_min = rw_f_comp - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1304 51 : f_RW_MAX = rw_f_comp + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1305 :
1306 51 : freqToFilterOut = f_RW_min;
1307 168 : while (filteringSet == false)
1308 : {
1309 : // compute the index of the frequency bin immediately below rw_f
1310 66 : binBelow = (int)(floor(((double)freqToFilterOut) / ((double)deltaFreq)));
1311 66 : deltaBelow = freqToFilterOut - (float)(binBelow * deltaFreq);
1312 :
1313 : // compute the index of the frequency bin immediately above rw_f
1314 66 : binAbove = (int)(ceil(((double)freqToFilterOut) / ((double)deltaFreq)));
1315 66 : deltaAbove = (float)(binAbove * deltaFreq) - freqToFilterOut;
1316 :
1317 : // search the closest bin
1318 66 : if (deltaAbove > deltaBelow)
1319 : {
1320 15 : closestBin = binBelow;
1321 : }
1322 : else
1323 : {
1324 51 : closestBin = binAbove;
1325 : }
1326 :
1327 : // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1328 66 : fi = (float)closestBin * deltaFreq;
1329 66 : fi_min = fi - ((float)deltaFreq * (float)FI_INTERVAL_COEFF);
1330 66 : fi_MAX = fi + ((float)deltaFreq * (float)FI_INTERVAL_COEFF);
1331 :
1332 : //**************************************************************************************
1333 : // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1334 : // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1335 : //**************************************************************************************
1336 :
1337 : // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
1338 : // => remove f_(i), f_(i-1) and f_(i+1)
1339 99 : if ((freqToFilterOut > fi_min) && (freqToFilterOut < fi_MAX))
1340 : {
1341 33 : binToRemove[0] = (closestBin - 1) - 1;
1342 33 : binToRemove[1] = closestBin - 1;
1343 33 : binToRemove[2] = (closestBin + 1) - 1;
1344 : }
1345 : // 2. ELSE
1346 : // => remove the two f_(i) which are around f_RW
1347 : else
1348 : {
1349 33 : binToRemove[0] = binBelow - 1;
1350 33 : binToRemove[1] = binAbove - 1;
1351 33 : binToRemove[2] = -1;
1352 : }
1353 :
1354 264 : for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1355 : {
1356 198 : bin = binToRemove[k];
1357 198 : if ((bin >= BIN_MIN) && (bin <= BIN_MAX))
1358 : {
1359 88 : whichByte = (bin >> SHIFT_3_BITS); // division by 8
1360 88 : selectedByte = (1 << (bin - (whichByte * BITS_PER_BYTE)));
1361 176 : fbins_mask[BYTES_PER_MASK - 1 - whichByte]
1362 88 : = fbins_mask[BYTES_PER_MASK - 1 - whichByte]
1363 88 : & ((unsigned char)(~selectedByte)); // bytes are ordered MSB first in the
1364 : // packets
1365 : }
1366 : }
1367 :
1368 : // update freqToFilterOut
1369 66 : if (freqToFilterOut == f_RW_MAX)
1370 : {
1371 51 : filteringSet = true; // end of the loop
1372 : }
1373 : else
1374 : {
1375 15 : freqToFilterOut = freqToFilterOut + deltaFreq;
1376 : }
1377 :
1378 66 : if (freqToFilterOut > f_RW_MAX)
1379 : {
1380 15 : freqToFilterOut = f_RW_MAX;
1381 : }
1382 : }
1383 : }
1384 576 : }
1385 :
1386 36 : void build_sy_lfr_rw_mask(unsigned int channel)
1387 : {
1388 : unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1389 36 : unsigned char* maskPtr = NULL;
1390 36 : unsigned char deltaF = (unsigned char)DELTAF_F2;
1391 : unsigned k;
1392 :
1393 36 : switch (channel)
1394 : {
1395 : case CHANNELF0:
1396 12 : maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1397 12 : deltaF = (unsigned char)DELTAF_F0;
1398 12 : break;
1399 : case CHANNELF1:
1400 12 : maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1401 12 : deltaF = (unsigned char)DELTAF_F1;
1402 12 : break;
1403 : case CHANNELF2:
1404 12 : maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1405 12 : deltaF = (unsigned char)DELTAF_F2;
1406 : break;
1407 : default:
1408 : break;
1409 : }
1410 :
1411 612 : for (k = 0; k < BYTES_PER_MASK; k++)
1412 : {
1413 576 : local_rw_fbins_mask[k] = INT8_ALL_F;
1414 : }
1415 :
1416 : // RW1
1417 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1);
1418 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2);
1419 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3);
1420 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4);
1421 :
1422 : // RW2
1423 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1);
1424 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2);
1425 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3);
1426 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4);
1427 :
1428 : // RW3
1429 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1);
1430 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2);
1431 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3);
1432 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4);
1433 :
1434 : // RW4
1435 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1);
1436 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2);
1437 36 : setFBinMask(local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3);
1438 36 : 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 36 : if (maskPtr != NULL)
1442 : {
1443 612 : for (k = 0; k < BYTES_PER_MASK; k++)
1444 : {
1445 576 : maskPtr[k] = local_rw_fbins_mask[k];
1446 : }
1447 : }
1448 36 : }
1449 :
1450 12 : void build_sy_lfr_rw_masks(void)
1451 : {
1452 12 : build_sy_lfr_rw_mask(CHANNELF0);
1453 12 : build_sy_lfr_rw_mask(CHANNELF1);
1454 12 : build_sy_lfr_rw_mask(CHANNELF2);
1455 12 : }
1456 :
1457 13 : void merge_fbins_masks(void)
1458 : {
1459 13 : const unsigned char* const fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1460 13 : const unsigned char* const fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1461 13 : const unsigned char* const fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1462 13 : const unsigned char* const rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1463 13 : const unsigned char* const rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1464 13 : const unsigned char* const rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1465 :
1466 :
1467 221 : for (unsigned char k = 0; k < BYTES_PER_MASK; k++)
1468 : {
1469 208 : fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1470 208 : fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1471 208 : fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1472 : }
1473 13 : }
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 96 : 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 96 : ret = LFR_SUCCESSFUL;
1500 96 : rw_k = INIT_FLOAT;
1501 :
1502 96 : copyFloatByChar((unsigned char*)&rw_k, &TC->dataAndCRC[offset]);
1503 :
1504 96 : *pos = offset;
1505 96 : *value = rw_k;
1506 :
1507 96 : if (rw_k < MIN_SY_LFR_RW_F)
1508 : {
1509 0 : ret = WRONG_APP_DATA;
1510 : }
1511 :
1512 96 : return ret;
1513 : }
1514 :
1515 6 : int check_all_sy_lfr_rw_k(const ccsdsTelecommandPacket_t* const TC, int* pos, float* value)
1516 : {
1517 6 : int ret = LFR_SUCCESSFUL;
1518 :
1519 : //****
1520 : //****
1521 : // RW1
1522 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value); // K1
1523 6 : if (ret == LFR_SUCCESSFUL) // K2
1524 : {
1525 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value);
1526 : }
1527 6 : if (ret == LFR_SUCCESSFUL) // K3
1528 : {
1529 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value);
1530 : }
1531 6 : if (ret == LFR_SUCCESSFUL) // K4
1532 : {
1533 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value);
1534 : }
1535 :
1536 : //****
1537 : //****
1538 : // RW2
1539 6 : if (ret == LFR_SUCCESSFUL) // K1
1540 : {
1541 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value);
1542 : }
1543 6 : if (ret == LFR_SUCCESSFUL) // K2
1544 : {
1545 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value);
1546 : }
1547 6 : if (ret == LFR_SUCCESSFUL) // K3
1548 : {
1549 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value);
1550 : }
1551 6 : if (ret == LFR_SUCCESSFUL) // K4
1552 : {
1553 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value);
1554 : }
1555 :
1556 : //****
1557 : //****
1558 : // RW3
1559 6 : if (ret == LFR_SUCCESSFUL) // K1
1560 : {
1561 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value);
1562 : }
1563 6 : if (ret == LFR_SUCCESSFUL) // K2
1564 : {
1565 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value);
1566 : }
1567 6 : if (ret == LFR_SUCCESSFUL) // K3
1568 : {
1569 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value);
1570 : }
1571 6 : if (ret == LFR_SUCCESSFUL) // K4
1572 : {
1573 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value);
1574 : }
1575 :
1576 : //****
1577 : //****
1578 : // RW4
1579 6 : if (ret == LFR_SUCCESSFUL) // K1
1580 : {
1581 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value);
1582 : }
1583 6 : if (ret == LFR_SUCCESSFUL) // K2
1584 : {
1585 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value);
1586 : }
1587 6 : if (ret == LFR_SUCCESSFUL) // K3
1588 : {
1589 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value);
1590 : }
1591 6 : if (ret == LFR_SUCCESSFUL) // K4
1592 : {
1593 6 : ret = check_sy_lfr_rw_k(TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value);
1594 : }
1595 :
1596 6 : return ret;
1597 : }
1598 :
1599 16 : int check_sy_lfr_filter_parameters(const ccsdsTelecommandPacket_t* const TC, rtems_id queue_id)
1600 : {
1601 16 : int flag = LFR_SUCCESSFUL;
1602 :
1603 : unsigned char sy_lfr_pas_filter_modulus;
1604 16 : float sy_lfr_pas_filter_tbad = 0.f;
1605 : unsigned char sy_lfr_pas_filter_offset;
1606 16 : float sy_lfr_pas_filter_shift = 0.f;
1607 16 : float sy_lfr_sc_rw_delta_f = 0.f;
1608 16 : const char* parPtr = NULL;
1609 16 : int datafield_pos = 0;
1610 16 : float rw_k = 0.f;
1611 :
1612 :
1613 : //***************
1614 : // get parameters
1615 16 : sy_lfr_pas_filter_modulus = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS];
1616 16 : copyFloatByChar((unsigned char*)&sy_lfr_pas_filter_tbad,
1617 : &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD]);
1618 16 : sy_lfr_pas_filter_offset = TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET];
1619 16 : copyFloatByChar((unsigned char*)&sy_lfr_pas_filter_shift,
1620 : &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT]);
1621 16 : 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 16 : if ((sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS)
1634 : || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS))
1635 : {
1636 1 : 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 1 : flag = WRONG_APP_DATA;
1639 : }
1640 :
1641 : //***********************
1642 : // sy_lfr_pas_filter_tbad
1643 16 : if (flag == LFR_SUCCESSFUL)
1644 : {
1645 30 : if ((sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD)
1646 15 : || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD))
1647 : {
1648 1 : parPtr = (const char*)&sy_lfr_pas_filter_tbad;
1649 1 : 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 1 : flag = WRONG_APP_DATA;
1652 : }
1653 : }
1654 :
1655 : //*************************
1656 : // sy_lfr_pas_filter_offset
1657 16 : if (flag == LFR_SUCCESSFUL && sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET)
1658 : {
1659 1 : DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
1660 : DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset));
1661 1 : flag = WRONG_APP_DATA;
1662 : }
1663 :
1664 : //************************
1665 : // sy_lfr_pas_filter_shift
1666 16 : if (flag == LFR_SUCCESSFUL)
1667 : {
1668 26 : if ((sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT)
1669 13 : || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT))
1670 : {
1671 1 : parPtr = (const char*)&sy_lfr_pas_filter_shift;
1672 1 : DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
1673 : DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE]));
1674 1 : flag = WRONG_APP_DATA;
1675 : }
1676 : }
1677 :
1678 : //*************************************
1679 : // check global coherency of the values
1680 16 : if (flag == LFR_SUCCESSFUL)
1681 : {
1682 12 : if ((sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) >= sy_lfr_pas_filter_modulus)
1683 : {
1684 6 : DEBUG_CHECK_STATUS(send_tm_lfr_tc_exe_inconsistent(TC, queue_id,
1685 : DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET,
1686 : sy_lfr_pas_filter_modulus));
1687 6 : flag = WRONG_APP_DATA;
1688 : }
1689 : }
1690 :
1691 : //*********************
1692 : // sy_lfr_sc_rw_delta_f
1693 16 : 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 16 : if (flag == LFR_SUCCESSFUL)
1704 : {
1705 6 : flag = check_all_sy_lfr_rw_k(TC, &datafield_pos, &rw_k);
1706 6 : 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 16 : 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 648 : destination[BYTE_0] = source[BYTE_0];
1884 648 : destination[BYTE_1] = source[BYTE_1];
1885 648 : destination[BYTE_2] = source[BYTE_2];
1886 648 : destination[BYTE_3] = source[BYTE_3];
1887 0 : }
1888 :
1889 9 : void copyInt32ByChar(unsigned char* destination, const unsigned char* const source)
1890 : {
1891 9 : destination[BYTE_0] = source[BYTE_0];
1892 9 : destination[BYTE_1] = source[BYTE_1];
1893 9 : destination[BYTE_2] = source[BYTE_2];
1894 9 : destination[BYTE_3] = source[BYTE_3];
1895 9 : }
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 74 : 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 74 : switch (destination_id)
2117 : {
2118 : case SID_TC_GROUND:
2119 0 : i = GROUND;
2120 0 : break;
2121 : case SID_TC_MISSION_TIMELINE:
2122 74 : i = MISSION_TIMELINE;
2123 74 : 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 74 : segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
2160 74 : sequence_cnt = sequenceCounters_TM_DUMP[i] & SEQ_CNT_MASK;
2161 :
2162 74 : new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt;
2163 :
2164 74 : packet_sequence_control[0] = (unsigned char)(new_packet_sequence_control >> SHIFT_1_BYTE);
2165 74 : packet_sequence_control[1] = (unsigned char)(new_packet_sequence_control);
2166 :
2167 : // increment the sequence counter
2168 74 : if (sequenceCounters_TM_DUMP[i] < SEQ_CNT_MAX)
2169 : {
2170 74 : sequenceCounters_TM_DUMP[i] = sequenceCounters_TM_DUMP[i] + 1;
2171 : }
2172 : else
2173 : {
2174 0 : sequenceCounters_TM_DUMP[i] = 0;
2175 : }
2176 74 : }
|