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, ¶meter_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 |
|
89 |
void copyInt32ByChar( unsigned char *destination, unsigned char *source ) |
1808 |
|
|
{ |
1809 |
|
89 |
destination[BYTE_0] = source[BYTE_0]; |
1810 |
|
89 |
destination[BYTE_1] = source[BYTE_1]; |
1811 |
|
89 |
destination[BYTE_2] = source[BYTE_2]; |
1812 |
|
89 |
destination[BYTE_3] = source[BYTE_3]; |
1813 |
|
89 |
} |
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 |
|
|
} |