##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » SOLO_LFR » DEV_PLE
RSS

Clone from http://pc-instru.lpp.polytechnique.fr:5000/DEV_PLE

modulus, tbad, offset, shift are stored in finetime (uint64_t)...
paul -
r355:c502a62ca98f R3++ draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 26659466eb11170e587645c796142ac8a7fd0add header/lfr_common_headers
2 e904b329ff977514bf36af92617afefd22fd06ab header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,972 +1,974
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 21 // number of tasks concurrently active including INIT
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 6 // [hous] [load] [avgv]
38 38 #define CONFIGURE_MAXIMUM_TIMERS 6 // [spiq] [link] [spacewire_reset_link]
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50
51 51 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
52 52 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
53 53 #endif
54 54
55 55 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
56 56 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
57 57 #endif
58 58
59 59 #endif
60 60 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
61 61
62 62 #include <drvmgr/drvmgr_confdefs.h>
63 63 #endif
64 64
65 65 #include "fsw_init.h"
66 66 #include "fsw_config.c"
67 67 #include "GscMemoryLPP.hpp"
68 68
69 69 void initCache()
70 70 {
71 71 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
72 72 // These should only be read and written using 32-bit LDA/STA instructions.
73 73 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
74 74 // The table below shows the register addresses:
75 75 // 0x00 Cache control register
76 76 // 0x04 Reserved
77 77 // 0x08 Instruction cache configuration register
78 78 // 0x0C Data cache configuration register
79 79
80 80 // Cache Control Register Leon3 / Leon3FT
81 81 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
82 82 // RFT PS TB DS FD FI FT ST IB
83 83 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
84 84 // IP DP ITE IDE DTE DDE DF IF DCS ICS
85 85
86 86 unsigned int cacheControlRegister;
87 87
88 88 CCR_resetCacheControlRegister();
89 89 ASR16_resetRegisterProtectionControlRegister();
90 90
91 91 cacheControlRegister = CCR_getValue();
92 92 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
93 93 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
94 94
95 95 CCR_enableInstructionCache(); // ICS bits
96 96 CCR_enableDataCache(); // DCS bits
97 97 CCR_enableInstructionBurstFetch(); // IB bit
98 98
99 99 faultTolerantScheme();
100 100
101 101 cacheControlRegister = CCR_getValue();
102 102 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
103 103 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
104 104
105 105 PRINTF("\n");
106 106 }
107 107
108 108 rtems_task Init( rtems_task_argument ignored )
109 109 {
110 110 /** This is the RTEMS INIT taks, it is the first task launched by the system.
111 111 *
112 112 * @param unused is the starting argument of the RTEMS task
113 113 *
114 114 * The INIT task create and run all other RTEMS tasks.
115 115 *
116 116 */
117 117
118 118 //***********
119 119 // INIT CACHE
120 120
121 121 unsigned char *vhdlVersion;
122 122
123 123 reset_lfr();
124 124
125 125 reset_local_time();
126 126
127 127 rtems_cpu_usage_reset();
128 128
129 129 rtems_status_code status;
130 130 rtems_status_code status_spw;
131 131 rtems_isr_entry old_isr_handler;
132 132
133 133 old_isr_handler = NULL;
134 134
135 135 // UART settings
136 136 enable_apbuart_transmitter();
137 137 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
138 138
139 139 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
140 140
141 141
142 142 PRINTF("\n\n\n\n\n")
143 143
144 144 initCache();
145 145
146 146 PRINTF("*************************\n")
147 147 PRINTF("** LFR Flight Software **\n")
148 148
149 149 PRINTF1("** %d-", SW_VERSION_N1)
150 150 PRINTF1("%d-" , SW_VERSION_N2)
151 151 PRINTF1("%d-" , SW_VERSION_N3)
152 152 PRINTF1("%d **\n", SW_VERSION_N4)
153 153
154 154 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
155 155 PRINTF("** VHDL **\n")
156 156 PRINTF1("** %d-", vhdlVersion[1])
157 157 PRINTF1("%d-" , vhdlVersion[2])
158 158 PRINTF1("%d **\n", vhdlVersion[3])
159 159 PRINTF("*************************\n")
160 160 PRINTF("\n\n")
161 161
162 162 init_parameter_dump();
163 163 init_kcoefficients_dump();
164 164 init_local_mode_parameters();
165 165 init_housekeeping_parameters();
166 166 init_k_coefficients_prc0();
167 167 init_k_coefficients_prc1();
168 168 init_k_coefficients_prc2();
169 169 pa_bia_status_info = INIT_CHAR;
170 170
171 171 // initialize all reaction wheels frequencies to NaN
172 172 rw_f.cp_rpw_sc_rw1_f1 = NAN;
173 173 rw_f.cp_rpw_sc_rw1_f2 = NAN;
174 174 rw_f.cp_rpw_sc_rw1_f3 = NAN;
175 175 rw_f.cp_rpw_sc_rw1_f4 = NAN;
176 176 rw_f.cp_rpw_sc_rw2_f1 = NAN;
177 177 rw_f.cp_rpw_sc_rw2_f2 = NAN;
178 178 rw_f.cp_rpw_sc_rw2_f3 = NAN;
179 179 rw_f.cp_rpw_sc_rw2_f4 = NAN;
180 180 rw_f.cp_rpw_sc_rw3_f1 = NAN;
181 181 rw_f.cp_rpw_sc_rw3_f2 = NAN;
182 182 rw_f.cp_rpw_sc_rw3_f3 = NAN;
183 183 rw_f.cp_rpw_sc_rw3_f4 = NAN;
184 184 rw_f.cp_rpw_sc_rw4_f1 = NAN;
185 185 rw_f.cp_rpw_sc_rw4_f2 = NAN;
186 186 rw_f.cp_rpw_sc_rw4_f3 = NAN;
187 187 rw_f.cp_rpw_sc_rw4_f4 = NAN;
188 188
189 189 // initialize filtering parameters
190 190 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
191 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
191 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
192 192 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
193 filterPar.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
194 193 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
195 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
194 filterPar.modulus_in_finetime = DEFAULT_MODULUS;
195 filterPar.tbad_in_finetime = DEFAULT_TBAD;
196 filterPar.offset_in_finetime = DEFAULT_OFFSET;
197 filterPar.shift_in_finetime = DEFAULT_SHIFT;
196 198 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
197 199
198 200 // waveform picker initialization
199 201 WFP_init_rings();
200 202 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
201 203 WFP_reset_current_ring_nodes();
202 204 reset_waveform_picker_regs();
203 205
204 206 // spectral matrices initialization
205 207 SM_init_rings(); // initialize spectral matrices rings
206 208 SM_reset_current_ring_nodes();
207 209 reset_spectral_matrix_regs();
208 210
209 211 // configure calibration
210 212 configureCalibration( false ); // true means interleaved mode, false is for normal mode
211 213
212 214 updateLFRCurrentMode( LFR_MODE_STANDBY );
213 215
214 216 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
215 217
216 218 create_names(); // create all names
217 219
218 220 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
219 221 if (status != RTEMS_SUCCESSFUL)
220 222 {
221 223 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
222 224 }
223 225
224 226 status = create_message_queues(); // create message queues
225 227 if (status != RTEMS_SUCCESSFUL)
226 228 {
227 229 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
228 230 }
229 231
230 232 status = create_all_tasks(); // create all tasks
231 233 if (status != RTEMS_SUCCESSFUL)
232 234 {
233 235 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
234 236 }
235 237
236 238 // **************************
237 239 // <SPACEWIRE INITIALIZATION>
238 240 status_spw = spacewire_open_link(); // (1) open the link
239 241 if ( status_spw != RTEMS_SUCCESSFUL )
240 242 {
241 243 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
242 244 }
243 245
244 246 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
245 247 {
246 248 status_spw = spacewire_configure_link( fdSPW );
247 249 if ( status_spw != RTEMS_SUCCESSFUL )
248 250 {
249 251 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
250 252 }
251 253 }
252 254
253 255 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
254 256 {
255 257 status_spw = spacewire_start_link( fdSPW );
256 258 if ( status_spw != RTEMS_SUCCESSFUL )
257 259 {
258 260 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
259 261 }
260 262 }
261 263 // </SPACEWIRE INITIALIZATION>
262 264 // ***************************
263 265
264 266 status = start_all_tasks(); // start all tasks
265 267 if (status != RTEMS_SUCCESSFUL)
266 268 {
267 269 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
268 270 }
269 271
270 272 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
271 273 status = start_recv_send_tasks();
272 274 if ( status != RTEMS_SUCCESSFUL )
273 275 {
274 276 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
275 277 }
276 278
277 279 // suspend science tasks, they will be restarted later depending on the mode
278 280 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
279 281 if (status != RTEMS_SUCCESSFUL)
280 282 {
281 283 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
282 284 }
283 285
284 286 // configure IRQ handling for the waveform picker unit
285 287 status = rtems_interrupt_catch( waveforms_isr,
286 288 IRQ_SPARC_WAVEFORM_PICKER,
287 289 &old_isr_handler) ;
288 290 // configure IRQ handling for the spectral matrices unit
289 291 status = rtems_interrupt_catch( spectral_matrices_isr,
290 292 IRQ_SPARC_SPECTRAL_MATRIX,
291 293 &old_isr_handler) ;
292 294
293 295 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
294 296 if ( status_spw != RTEMS_SUCCESSFUL )
295 297 {
296 298 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
297 299 if ( status != RTEMS_SUCCESSFUL ) {
298 300 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
299 301 }
300 302 }
301 303
302 304 BOOT_PRINTF("delete INIT\n")
303 305
304 306 set_hk_lfr_sc_potential_flag( true );
305 307
306 308 // start the timer to detect a missing spacewire timecode
307 309 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
308 310 // if a tickout is generated, the timer is restarted
309 311 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
310 312
311 313 grspw_timecode_callback = &timecode_irq_handler;
312 314
313 315 status = rtems_task_delete(RTEMS_SELF);
314 316
315 317 }
316 318
317 319 void init_local_mode_parameters( void )
318 320 {
319 321 /** This function initialize the param_local global variable with default values.
320 322 *
321 323 */
322 324
323 325 unsigned int i;
324 326
325 327 // LOCAL PARAMETERS
326 328
327 329 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
328 330 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
329 331
330 332 // init sequence counters
331 333
332 334 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
333 335 {
334 336 sequenceCounters_TC_EXE[i] = INIT_CHAR;
335 337 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
336 338 }
337 339 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
338 340 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
339 341 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
340 342 }
341 343
342 344 void reset_local_time( void )
343 345 {
344 346 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
345 347 }
346 348
347 349 void create_names( void ) // create all names for tasks and queues
348 350 {
349 351 /** This function creates all RTEMS names used in the software for tasks and queues.
350 352 *
351 353 * @return RTEMS directive status codes:
352 354 * - RTEMS_SUCCESSFUL - successful completion
353 355 *
354 356 */
355 357
356 358 // task names
357 359 Task_name[TASKID_AVGV] = rtems_build_name( 'A', 'V', 'G', 'V' );
358 360 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
359 361 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
360 362 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
361 363 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
362 364 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
363 365 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
364 366 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
365 367 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
366 368 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
367 369 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
368 370 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
369 371 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
370 372 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
371 373 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
372 374 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
373 375 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
374 376 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
375 377 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
376 378 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
377 379
378 380 // rate monotonic period names
379 381 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
380 382 name_avgv_rate_monotonic = rtems_build_name( 'A', 'V', 'G', 'V' );
381 383
382 384 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
383 385 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
384 386 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
385 387 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
386 388 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
387 389
388 390 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
389 391 }
390 392
391 393 int create_all_tasks( void ) // create all tasks which run in the software
392 394 {
393 395 /** This function creates all RTEMS tasks used in the software.
394 396 *
395 397 * @return RTEMS directive status codes:
396 398 * - RTEMS_SUCCESSFUL - task created successfully
397 399 * - RTEMS_INVALID_ADDRESS - id is NULL
398 400 * - RTEMS_INVALID_NAME - invalid task name
399 401 * - RTEMS_INVALID_PRIORITY - invalid task priority
400 402 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
401 403 * - RTEMS_TOO_MANY - too many tasks created
402 404 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
403 405 * - RTEMS_TOO_MANY - too many global objects
404 406 *
405 407 */
406 408
407 409 rtems_status_code status;
408 410
409 411 //**********
410 412 // SPACEWIRE
411 413 // RECV
412 414 status = rtems_task_create(
413 415 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
414 416 RTEMS_DEFAULT_MODES,
415 417 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
416 418 );
417 419 if (status == RTEMS_SUCCESSFUL) // SEND
418 420 {
419 421 status = rtems_task_create(
420 422 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
421 423 RTEMS_DEFAULT_MODES,
422 424 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
423 425 );
424 426 }
425 427 if (status == RTEMS_SUCCESSFUL) // LINK
426 428 {
427 429 status = rtems_task_create(
428 430 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
429 431 RTEMS_DEFAULT_MODES,
430 432 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
431 433 );
432 434 }
433 435 if (status == RTEMS_SUCCESSFUL) // ACTN
434 436 {
435 437 status = rtems_task_create(
436 438 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
437 439 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
438 440 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
439 441 );
440 442 }
441 443 if (status == RTEMS_SUCCESSFUL) // SPIQ
442 444 {
443 445 status = rtems_task_create(
444 446 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
445 447 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
446 448 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
447 449 );
448 450 }
449 451
450 452 //******************
451 453 // SPECTRAL MATRICES
452 454 if (status == RTEMS_SUCCESSFUL) // AVF0
453 455 {
454 456 status = rtems_task_create(
455 457 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
456 458 RTEMS_DEFAULT_MODES,
457 459 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
458 460 );
459 461 }
460 462 if (status == RTEMS_SUCCESSFUL) // PRC0
461 463 {
462 464 status = rtems_task_create(
463 465 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
464 466 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
465 467 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
466 468 );
467 469 }
468 470 if (status == RTEMS_SUCCESSFUL) // AVF1
469 471 {
470 472 status = rtems_task_create(
471 473 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
472 474 RTEMS_DEFAULT_MODES,
473 475 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
474 476 );
475 477 }
476 478 if (status == RTEMS_SUCCESSFUL) // PRC1
477 479 {
478 480 status = rtems_task_create(
479 481 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
480 482 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
481 483 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
482 484 );
483 485 }
484 486 if (status == RTEMS_SUCCESSFUL) // AVF2
485 487 {
486 488 status = rtems_task_create(
487 489 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
488 490 RTEMS_DEFAULT_MODES,
489 491 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
490 492 );
491 493 }
492 494 if (status == RTEMS_SUCCESSFUL) // PRC2
493 495 {
494 496 status = rtems_task_create(
495 497 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
496 498 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
497 499 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
498 500 );
499 501 }
500 502
501 503 //****************
502 504 // WAVEFORM PICKER
503 505 if (status == RTEMS_SUCCESSFUL) // WFRM
504 506 {
505 507 status = rtems_task_create(
506 508 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
507 509 RTEMS_DEFAULT_MODES,
508 510 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
509 511 );
510 512 }
511 513 if (status == RTEMS_SUCCESSFUL) // CWF3
512 514 {
513 515 status = rtems_task_create(
514 516 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
515 517 RTEMS_DEFAULT_MODES,
516 518 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
517 519 );
518 520 }
519 521 if (status == RTEMS_SUCCESSFUL) // CWF2
520 522 {
521 523 status = rtems_task_create(
522 524 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
523 525 RTEMS_DEFAULT_MODES,
524 526 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
525 527 );
526 528 }
527 529 if (status == RTEMS_SUCCESSFUL) // CWF1
528 530 {
529 531 status = rtems_task_create(
530 532 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
531 533 RTEMS_DEFAULT_MODES,
532 534 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
533 535 );
534 536 }
535 537 if (status == RTEMS_SUCCESSFUL) // SWBD
536 538 {
537 539 status = rtems_task_create(
538 540 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
539 541 RTEMS_DEFAULT_MODES,
540 542 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
541 543 );
542 544 }
543 545
544 546 //*****
545 547 // MISC
546 548 if (status == RTEMS_SUCCESSFUL) // LOAD
547 549 {
548 550 status = rtems_task_create(
549 551 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
550 552 RTEMS_DEFAULT_MODES,
551 553 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
552 554 );
553 555 }
554 556 if (status == RTEMS_SUCCESSFUL) // DUMB
555 557 {
556 558 status = rtems_task_create(
557 559 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
558 560 RTEMS_DEFAULT_MODES,
559 561 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
560 562 );
561 563 }
562 564 if (status == RTEMS_SUCCESSFUL) // HOUS
563 565 {
564 566 status = rtems_task_create(
565 567 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
566 568 RTEMS_DEFAULT_MODES,
567 569 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
568 570 );
569 571 }
570 572 if (status == RTEMS_SUCCESSFUL) // AVGV
571 573 {
572 574 status = rtems_task_create(
573 575 Task_name[TASKID_AVGV], TASK_PRIORITY_AVGV, RTEMS_MINIMUM_STACK_SIZE,
574 576 RTEMS_DEFAULT_MODES,
575 577 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVGV]
576 578 );
577 579 }
578 580
579 581 return status;
580 582 }
581 583
582 584 int start_recv_send_tasks( void )
583 585 {
584 586 rtems_status_code status;
585 587
586 588 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
587 589 if (status!=RTEMS_SUCCESSFUL) {
588 590 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
589 591 }
590 592
591 593 if (status == RTEMS_SUCCESSFUL) // SEND
592 594 {
593 595 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
594 596 if (status!=RTEMS_SUCCESSFUL) {
595 597 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
596 598 }
597 599 }
598 600
599 601 return status;
600 602 }
601 603
602 604 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
603 605 {
604 606 /** This function starts all RTEMS tasks used in the software.
605 607 *
606 608 * @return RTEMS directive status codes:
607 609 * - RTEMS_SUCCESSFUL - ask started successfully
608 610 * - RTEMS_INVALID_ADDRESS - invalid task entry point
609 611 * - RTEMS_INVALID_ID - invalid task id
610 612 * - RTEMS_INCORRECT_STATE - task not in the dormant state
611 613 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
612 614 *
613 615 */
614 616 // starts all the tasks fot eh flight software
615 617
616 618 rtems_status_code status;
617 619
618 620 //**********
619 621 // SPACEWIRE
620 622 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
621 623 if (status!=RTEMS_SUCCESSFUL) {
622 624 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
623 625 }
624 626
625 627 if (status == RTEMS_SUCCESSFUL) // LINK
626 628 {
627 629 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
628 630 if (status!=RTEMS_SUCCESSFUL) {
629 631 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
630 632 }
631 633 }
632 634
633 635 if (status == RTEMS_SUCCESSFUL) // ACTN
634 636 {
635 637 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
636 638 if (status!=RTEMS_SUCCESSFUL) {
637 639 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
638 640 }
639 641 }
640 642
641 643 //******************
642 644 // SPECTRAL MATRICES
643 645 if (status == RTEMS_SUCCESSFUL) // AVF0
644 646 {
645 647 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
646 648 if (status!=RTEMS_SUCCESSFUL) {
647 649 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
648 650 }
649 651 }
650 652 if (status == RTEMS_SUCCESSFUL) // PRC0
651 653 {
652 654 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
653 655 if (status!=RTEMS_SUCCESSFUL) {
654 656 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
655 657 }
656 658 }
657 659 if (status == RTEMS_SUCCESSFUL) // AVF1
658 660 {
659 661 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
660 662 if (status!=RTEMS_SUCCESSFUL) {
661 663 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
662 664 }
663 665 }
664 666 if (status == RTEMS_SUCCESSFUL) // PRC1
665 667 {
666 668 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
667 669 if (status!=RTEMS_SUCCESSFUL) {
668 670 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
669 671 }
670 672 }
671 673 if (status == RTEMS_SUCCESSFUL) // AVF2
672 674 {
673 675 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
674 676 if (status!=RTEMS_SUCCESSFUL) {
675 677 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
676 678 }
677 679 }
678 680 if (status == RTEMS_SUCCESSFUL) // PRC2
679 681 {
680 682 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
681 683 if (status!=RTEMS_SUCCESSFUL) {
682 684 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
683 685 }
684 686 }
685 687
686 688 //****************
687 689 // WAVEFORM PICKER
688 690 if (status == RTEMS_SUCCESSFUL) // WFRM
689 691 {
690 692 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
691 693 if (status!=RTEMS_SUCCESSFUL) {
692 694 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
693 695 }
694 696 }
695 697 if (status == RTEMS_SUCCESSFUL) // CWF3
696 698 {
697 699 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
698 700 if (status!=RTEMS_SUCCESSFUL) {
699 701 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
700 702 }
701 703 }
702 704 if (status == RTEMS_SUCCESSFUL) // CWF2
703 705 {
704 706 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
705 707 if (status!=RTEMS_SUCCESSFUL) {
706 708 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
707 709 }
708 710 }
709 711 if (status == RTEMS_SUCCESSFUL) // CWF1
710 712 {
711 713 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
712 714 if (status!=RTEMS_SUCCESSFUL) {
713 715 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
714 716 }
715 717 }
716 718 if (status == RTEMS_SUCCESSFUL) // SWBD
717 719 {
718 720 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
719 721 if (status!=RTEMS_SUCCESSFUL) {
720 722 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
721 723 }
722 724 }
723 725
724 726 //*****
725 727 // MISC
726 728 if (status == RTEMS_SUCCESSFUL) // HOUS
727 729 {
728 730 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
729 731 if (status!=RTEMS_SUCCESSFUL) {
730 732 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
731 733 }
732 734 }
733 735 if (status == RTEMS_SUCCESSFUL) // AVGV
734 736 {
735 737 status = rtems_task_start( Task_id[TASKID_AVGV], avgv_task, 1 );
736 738 if (status!=RTEMS_SUCCESSFUL) {
737 739 BOOT_PRINTF("in INIT *** Error starting TASK_AVGV\n")
738 740 }
739 741 }
740 742 if (status == RTEMS_SUCCESSFUL) // DUMB
741 743 {
742 744 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
743 745 if (status!=RTEMS_SUCCESSFUL) {
744 746 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
745 747 }
746 748 }
747 749 if (status == RTEMS_SUCCESSFUL) // LOAD
748 750 {
749 751 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
750 752 if (status!=RTEMS_SUCCESSFUL) {
751 753 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
752 754 }
753 755 }
754 756
755 757 return status;
756 758 }
757 759
758 760 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
759 761 {
760 762 rtems_status_code status_recv;
761 763 rtems_status_code status_send;
762 764 rtems_status_code status_q_p0;
763 765 rtems_status_code status_q_p1;
764 766 rtems_status_code status_q_p2;
765 767 rtems_status_code ret;
766 768 rtems_id queue_id;
767 769
768 770 ret = RTEMS_SUCCESSFUL;
769 771 queue_id = RTEMS_ID_NONE;
770 772
771 773 //****************************************
772 774 // create the queue for handling valid TCs
773 775 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
774 776 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
775 777 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
776 778 if ( status_recv != RTEMS_SUCCESSFUL ) {
777 779 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
778 780 }
779 781
780 782 //************************************************
781 783 // create the queue for handling TM packet sending
782 784 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
783 785 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
784 786 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
785 787 if ( status_send != RTEMS_SUCCESSFUL ) {
786 788 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
787 789 }
788 790
789 791 //*****************************************************************************
790 792 // create the queue for handling averaged spectral matrices for processing @ f0
791 793 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
792 794 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
793 795 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
794 796 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
795 797 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
796 798 }
797 799
798 800 //*****************************************************************************
799 801 // create the queue for handling averaged spectral matrices for processing @ f1
800 802 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
801 803 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
802 804 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
803 805 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
804 806 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
805 807 }
806 808
807 809 //*****************************************************************************
808 810 // create the queue for handling averaged spectral matrices for processing @ f2
809 811 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
810 812 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
811 813 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
812 814 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
813 815 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
814 816 }
815 817
816 818 if ( status_recv != RTEMS_SUCCESSFUL )
817 819 {
818 820 ret = status_recv;
819 821 }
820 822 else if( status_send != RTEMS_SUCCESSFUL )
821 823 {
822 824 ret = status_send;
823 825 }
824 826 else if( status_q_p0 != RTEMS_SUCCESSFUL )
825 827 {
826 828 ret = status_q_p0;
827 829 }
828 830 else if( status_q_p1 != RTEMS_SUCCESSFUL )
829 831 {
830 832 ret = status_q_p1;
831 833 }
832 834 else
833 835 {
834 836 ret = status_q_p2;
835 837 }
836 838
837 839 return ret;
838 840 }
839 841
840 842 rtems_status_code create_timecode_timer( void )
841 843 {
842 844 rtems_status_code status;
843 845
844 846 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
845 847
846 848 if ( status != RTEMS_SUCCESSFUL )
847 849 {
848 850 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
849 851 }
850 852 else
851 853 {
852 854 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
853 855 }
854 856
855 857 return status;
856 858 }
857 859
858 860 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
859 861 {
860 862 rtems_status_code status;
861 863 rtems_name queue_name;
862 864
863 865 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
864 866
865 867 status = rtems_message_queue_ident( queue_name, 0, queue_id );
866 868
867 869 return status;
868 870 }
869 871
870 872 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
871 873 {
872 874 rtems_status_code status;
873 875 rtems_name queue_name;
874 876
875 877 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
876 878
877 879 status = rtems_message_queue_ident( queue_name, 0, queue_id );
878 880
879 881 return status;
880 882 }
881 883
882 884 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
883 885 {
884 886 rtems_status_code status;
885 887 rtems_name queue_name;
886 888
887 889 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
888 890
889 891 status = rtems_message_queue_ident( queue_name, 0, queue_id );
890 892
891 893 return status;
892 894 }
893 895
894 896 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
895 897 {
896 898 rtems_status_code status;
897 899 rtems_name queue_name;
898 900
899 901 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
900 902
901 903 status = rtems_message_queue_ident( queue_name, 0, queue_id );
902 904
903 905 return status;
904 906 }
905 907
906 908 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
907 909 {
908 910 rtems_status_code status;
909 911 rtems_name queue_name;
910 912
911 913 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
912 914
913 915 status = rtems_message_queue_ident( queue_name, 0, queue_id );
914 916
915 917 return status;
916 918 }
917 919
918 920 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
919 921 {
920 922 u_int32_t count;
921 923 rtems_status_code status;
922 924
923 925 count = 0;
924 926
925 927 status = rtems_message_queue_get_number_pending( queue_id, &count );
926 928
927 929 count = count + 1;
928 930
929 931 if (status != RTEMS_SUCCESSFUL)
930 932 {
931 933 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
932 934 }
933 935 else
934 936 {
935 937 if (count > *fifo_size_max)
936 938 {
937 939 *fifo_size_max = count;
938 940 }
939 941 }
940 942 }
941 943
942 944 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
943 945 {
944 946 unsigned char i;
945 947
946 948 //***************
947 949 // BUFFER ADDRESS
948 950 for(i=0; i<nbNodes; i++)
949 951 {
950 952 ring[i].coarseTime = INT32_ALL_F;
951 953 ring[i].fineTime = INT32_ALL_F;
952 954 ring[i].sid = INIT_CHAR;
953 955 ring[i].status = INIT_CHAR;
954 956 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
955 957 }
956 958
957 959 //*****
958 960 // NEXT
959 961 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
960 962 for(i=0; i<nbNodes-1; i++)
961 963 {
962 964 ring[i].next = (ring_node*) &ring[ i + 1 ];
963 965 }
964 966
965 967 //*********
966 968 // PREVIOUS
967 969 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
968 970 for(i=1; i<nbNodes; i++)
969 971 {
970 972 ring[i].previous = (ring_node*) &ring[ i - 1 ];
971 973 }
972 974 }
Show file before | Show file after | Hide comments
@@ -1,830 +1,840
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "fsw_processing.h"
11 11 #include "fsw_processing_globals.c"
12 12 #include "fsw_init.h"
13 13
14 14 unsigned int nb_sm_f0 = 0;
15 15 unsigned int nb_sm_f0_aux_f1= 0;
16 16 unsigned int nb_sm_f1 = 0;
17 17 unsigned int nb_sm_f0_aux_f2= 0;
18 18
19 19 typedef enum restartState_t
20 20 {
21 21 WAIT_FOR_F2,
22 22 WAIT_FOR_F1,
23 23 WAIT_FOR_F0
24 24 } restartState;
25 25
26 26 //************************
27 27 // spectral matrices rings
28 28 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0};
29 29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0};
30 30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0};
31 31 ring_node *current_ring_node_sm_f0 = NULL;
32 32 ring_node *current_ring_node_sm_f1 = NULL;
33 33 ring_node *current_ring_node_sm_f2 = NULL;
34 34 ring_node *ring_node_for_averaging_sm_f0= NULL;
35 35 ring_node *ring_node_for_averaging_sm_f1= NULL;
36 36 ring_node *ring_node_for_averaging_sm_f2= NULL;
37 37
38 38 //
39 39 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 40 {
41 41 ring_node *node;
42 42
43 43 node = NULL;
44 44 switch ( frequencyChannel ) {
45 45 case CHANNELF0:
46 46 node = ring_node_for_averaging_sm_f0;
47 47 break;
48 48 case CHANNELF1:
49 49 node = ring_node_for_averaging_sm_f1;
50 50 break;
51 51 case CHANNELF2:
52 52 node = ring_node_for_averaging_sm_f2;
53 53 break;
54 54 default:
55 55 break;
56 56 }
57 57
58 58 return node;
59 59 }
60 60
61 61 //***********************************************************
62 62 // Interrupt Service Routine for spectral matrices processing
63 63
64 64 void spectral_matrices_isr_f0( int statusReg )
65 65 {
66 66 unsigned char status;
67 67 rtems_status_code status_code;
68 68 ring_node *full_ring_node;
69 69
70 70 status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits
71 71
72 72 switch(status)
73 73 {
74 74 case 0:
75 75 break;
76 76 case BIT_READY_0_1:
77 77 // UNEXPECTED VALUE
78 78 spectral_matrix_regs->status = BIT_READY_0_1; // [0011]
79 79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 80 break;
81 81 case BIT_READY_0:
82 82 full_ring_node = current_ring_node_sm_f0->previous;
83 83 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 84 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 85 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 86 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 87 // if there are enough ring nodes ready, wake up an AVFx task
88 88 nb_sm_f0 = nb_sm_f0 + 1;
89 89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
90 90 {
91 91 ring_node_for_averaging_sm_f0 = full_ring_node;
92 92 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 93 {
94 94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 95 }
96 96 nb_sm_f0 = 0;
97 97 }
98 98 spectral_matrix_regs->status = BIT_READY_0; // [0000 0001]
99 99 break;
100 100 case BIT_READY_1:
101 101 full_ring_node = current_ring_node_sm_f0->previous;
102 102 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 103 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 104 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 105 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 106 // if there are enough ring nodes ready, wake up an AVFx task
107 107 nb_sm_f0 = nb_sm_f0 + 1;
108 108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
109 109 {
110 110 ring_node_for_averaging_sm_f0 = full_ring_node;
111 111 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 112 {
113 113 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 114 }
115 115 nb_sm_f0 = 0;
116 116 }
117 117 spectral_matrix_regs->status = BIT_READY_1; // [0000 0010]
118 118 break;
119 119 default:
120 120 break;
121 121 }
122 122 }
123 123
124 124 void spectral_matrices_isr_f1( int statusReg )
125 125 {
126 126 rtems_status_code status_code;
127 127 unsigned char status;
128 128 ring_node *full_ring_node;
129 129
130 130 status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits
131 131
132 132 switch(status)
133 133 {
134 134 case 0:
135 135 break;
136 136 case BIT_READY_0_1:
137 137 // UNEXPECTED VALUE
138 138 spectral_matrix_regs->status = BITS_STATUS_F1; // [1100]
139 139 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
140 140 break;
141 141 case BIT_READY_0:
142 142 full_ring_node = current_ring_node_sm_f1->previous;
143 143 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
144 144 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
145 145 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
146 146 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
147 147 // if there are enough ring nodes ready, wake up an AVFx task
148 148 nb_sm_f1 = nb_sm_f1 + 1;
149 149 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
150 150 {
151 151 ring_node_for_averaging_sm_f1 = full_ring_node;
152 152 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
153 153 {
154 154 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
155 155 }
156 156 nb_sm_f1 = 0;
157 157 }
158 158 spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100]
159 159 break;
160 160 case BIT_READY_1:
161 161 full_ring_node = current_ring_node_sm_f1->previous;
162 162 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
163 163 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
164 164 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
165 165 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
166 166 // if there are enough ring nodes ready, wake up an AVFx task
167 167 nb_sm_f1 = nb_sm_f1 + 1;
168 168 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
169 169 {
170 170 ring_node_for_averaging_sm_f1 = full_ring_node;
171 171 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
172 172 {
173 173 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
174 174 }
175 175 nb_sm_f1 = 0;
176 176 }
177 177 spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000]
178 178 break;
179 179 default:
180 180 break;
181 181 }
182 182 }
183 183
184 184 void spectral_matrices_isr_f2( int statusReg )
185 185 {
186 186 unsigned char status;
187 187 rtems_status_code status_code;
188 188
189 189 status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits
190 190
191 191 switch(status)
192 192 {
193 193 case 0:
194 194 break;
195 195 case BIT_READY_0_1:
196 196 // UNEXPECTED VALUE
197 197 spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000]
198 198 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
199 199 break;
200 200 case BIT_READY_0:
201 201 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
202 202 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
203 203 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
204 204 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
205 205 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
206 206 spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000]
207 207 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
208 208 {
209 209 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
210 210 }
211 211 break;
212 212 case BIT_READY_1:
213 213 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
214 214 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
215 215 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
216 216 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
217 217 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
218 218 spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000]
219 219 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
220 220 {
221 221 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
222 222 }
223 223 break;
224 224 default:
225 225 break;
226 226 }
227 227 }
228 228
229 229 void spectral_matrix_isr_error_handler( int statusReg )
230 230 {
231 231 // STATUS REGISTER
232 232 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
233 233 // 10 9 8
234 234 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
235 235 // 7 6 5 4 3 2 1 0
236 236 // [bad_component_err] not defined in the last version of the VHDL code
237 237
238 238 rtems_status_code status_code;
239 239
240 240 //***************************************************
241 241 // the ASM status register is copied in the HK packet
242 242 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000]
243 243
244 244 if (statusReg & BITS_SM_ERR) // [0111 1100 0000]
245 245 {
246 246 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
247 247 }
248 248
249 249 spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
250 250
251 251 }
252 252
253 253 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
254 254 {
255 255 // STATUS REGISTER
256 256 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
257 257 // 10 9 8
258 258 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
259 259 // 7 6 5 4 3 2 1 0
260 260
261 261 int statusReg;
262 262
263 263 static restartState state = WAIT_FOR_F2;
264 264
265 265 statusReg = spectral_matrix_regs->status;
266 266
267 267 if (thisIsAnASMRestart == 0)
268 268 { // this is not a restart sequence, process incoming matrices normally
269 269 spectral_matrices_isr_f0( statusReg );
270 270
271 271 spectral_matrices_isr_f1( statusReg );
272 272
273 273 spectral_matrices_isr_f2( statusReg );
274 274 }
275 275 else
276 276 { // a restart sequence has to be launched
277 277 switch (state) {
278 278 case WAIT_FOR_F2:
279 279 if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits
280 280 {
281 281 state = WAIT_FOR_F1;
282 282 }
283 283 break;
284 284 case WAIT_FOR_F1:
285 285 if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits
286 286 {
287 287 state = WAIT_FOR_F0;
288 288 }
289 289 break;
290 290 case WAIT_FOR_F0:
291 291 if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits
292 292 {
293 293 state = WAIT_FOR_F2;
294 294 thisIsAnASMRestart = 0;
295 295 }
296 296 break;
297 297 default:
298 298 break;
299 299 }
300 300 reset_sm_status();
301 301 }
302 302
303 303 spectral_matrix_isr_error_handler( statusReg );
304 304
305 305 }
306 306
307 307 //******************
308 308 // Spectral Matrices
309 309
310 310 void reset_nb_sm( void )
311 311 {
312 312 nb_sm_f0 = 0;
313 313 nb_sm_f0_aux_f1 = 0;
314 314 nb_sm_f0_aux_f2 = 0;
315 315
316 316 nb_sm_f1 = 0;
317 317 }
318 318
319 319 void SM_init_rings( void )
320 320 {
321 321 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
322 322 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
323 323 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
324 324
325 325 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
326 326 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
327 327 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
328 328 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
329 329 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
330 330 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
331 331 }
332 332
333 333 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
334 334 {
335 335 unsigned char i;
336 336
337 337 ring[ nbNodes - 1 ].next
338 338 = (ring_node_asm*) &ring[ 0 ];
339 339
340 340 for(i=0; i<nbNodes-1; i++)
341 341 {
342 342 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
343 343 }
344 344 }
345 345
346 346 void SM_reset_current_ring_nodes( void )
347 347 {
348 348 current_ring_node_sm_f0 = sm_ring_f0[0].next;
349 349 current_ring_node_sm_f1 = sm_ring_f1[0].next;
350 350 current_ring_node_sm_f2 = sm_ring_f2[0].next;
351 351
352 352 ring_node_for_averaging_sm_f0 = NULL;
353 353 ring_node_for_averaging_sm_f1 = NULL;
354 354 ring_node_for_averaging_sm_f2 = NULL;
355 355 }
356 356
357 357 //*****************
358 358 // Basic Parameters
359 359
360 360 void BP_init_header( bp_packet *packet,
361 361 unsigned int apid, unsigned char sid,
362 362 unsigned int packetLength, unsigned char blkNr )
363 363 {
364 364 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
365 365 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
366 366 packet->reserved = INIT_CHAR;
367 367 packet->userApplication = CCSDS_USER_APP;
368 368 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
369 369 packet->packetID[1] = (unsigned char) (apid);
370 370 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
371 371 packet->packetSequenceControl[1] = INIT_CHAR;
372 372 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
373 373 packet->packetLength[1] = (unsigned char) (packetLength);
374 374 // DATA FIELD HEADER
375 375 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
376 376 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
377 377 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
378 378 packet->destinationID = TM_DESTINATION_ID_GROUND;
379 379 packet->time[BYTE_0] = INIT_CHAR;
380 380 packet->time[BYTE_1] = INIT_CHAR;
381 381 packet->time[BYTE_2] = INIT_CHAR;
382 382 packet->time[BYTE_3] = INIT_CHAR;
383 383 packet->time[BYTE_4] = INIT_CHAR;
384 384 packet->time[BYTE_5] = INIT_CHAR;
385 385 // AUXILIARY DATA HEADER
386 386 packet->sid = sid;
387 387 packet->pa_bia_status_info = INIT_CHAR;
388 388 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
389 389 packet->sy_lfr_common_parameters = INIT_CHAR;
390 390 packet->acquisitionTime[BYTE_0] = INIT_CHAR;
391 391 packet->acquisitionTime[BYTE_1] = INIT_CHAR;
392 392 packet->acquisitionTime[BYTE_2] = INIT_CHAR;
393 393 packet->acquisitionTime[BYTE_3] = INIT_CHAR;
394 394 packet->acquisitionTime[BYTE_4] = INIT_CHAR;
395 395 packet->acquisitionTime[BYTE_5] = INIT_CHAR;
396 396 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
397 397 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
398 398 }
399 399
400 400 void BP_init_header_with_spare( bp_packet_with_spare *packet,
401 401 unsigned int apid, unsigned char sid,
402 402 unsigned int packetLength , unsigned char blkNr)
403 403 {
404 404 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
405 405 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
406 406 packet->reserved = INIT_CHAR;
407 407 packet->userApplication = CCSDS_USER_APP;
408 408 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
409 409 packet->packetID[1] = (unsigned char) (apid);
410 410 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
411 411 packet->packetSequenceControl[1] = INIT_CHAR;
412 412 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
413 413 packet->packetLength[1] = (unsigned char) (packetLength);
414 414 // DATA FIELD HEADER
415 415 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
416 416 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
417 417 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
418 418 packet->destinationID = TM_DESTINATION_ID_GROUND;
419 419 // AUXILIARY DATA HEADER
420 420 packet->sid = sid;
421 421 packet->pa_bia_status_info = INIT_CHAR;
422 422 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
423 423 packet->sy_lfr_common_parameters = INIT_CHAR;
424 424 packet->time[BYTE_0] = INIT_CHAR;
425 425 packet->time[BYTE_1] = INIT_CHAR;
426 426 packet->time[BYTE_2] = INIT_CHAR;
427 427 packet->time[BYTE_3] = INIT_CHAR;
428 428 packet->time[BYTE_4] = INIT_CHAR;
429 429 packet->time[BYTE_5] = INIT_CHAR;
430 430 packet->source_data_spare = INIT_CHAR;
431 431 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
432 432 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
433 433 }
434 434
435 435 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
436 436 {
437 437 rtems_status_code status;
438 438
439 439 // SEND PACKET
440 440 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
441 441 if (status != RTEMS_SUCCESSFUL)
442 442 {
443 443 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
444 444 }
445 445 }
446 446
447 447 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
448 448 {
449 449 /** This function is used to send the BP paquets when needed.
450 450 *
451 451 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
452 452 *
453 453 * @return void
454 454 *
455 455 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
456 456 * BURST paquets are sent everytime.
457 457 *
458 458 */
459 459
460 460 rtems_status_code status;
461 461
462 462 // SEND PACKET
463 463 // before lastValidTransitionDate, the data are drops even if they are ready
464 464 // this guarantees that no SBM packets will be received before the requested enter mode time
465 465 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
466 466 {
467 467 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
468 468 if (status != RTEMS_SUCCESSFUL)
469 469 {
470 470 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
471 471 }
472 472 }
473 473 }
474 474
475 475 //******************
476 476 // general functions
477 477
478 478 void reset_sm_status( void )
479 479 {
480 480 // error
481 481 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
482 482 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
483 483 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
484 484 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
485 485
486 486 spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111]
487 487 }
488 488
489 489 void reset_spectral_matrix_regs( void )
490 490 {
491 491 /** This function resets the spectral matrices module registers.
492 492 *
493 493 * The registers affected by this function are located at the following offset addresses:
494 494 *
495 495 * - 0x00 config
496 496 * - 0x04 status
497 497 * - 0x08 matrixF0_Address0
498 498 * - 0x10 matrixFO_Address1
499 499 * - 0x14 matrixF1_Address
500 500 * - 0x18 matrixF2_Address
501 501 *
502 502 */
503 503
504 504 set_sm_irq_onError( 0 );
505 505
506 506 set_sm_irq_onNewMatrix( 0 );
507 507
508 508 reset_sm_status();
509 509
510 510 // F1
511 511 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
512 512 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
513 513 // F2
514 514 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
515 515 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
516 516 // F3
517 517 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
518 518 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
519 519
520 520 spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
521 521 }
522 522
523 523 void set_time( unsigned char *time, unsigned char * timeInBuffer )
524 524 {
525 525 time[BYTE_0] = timeInBuffer[BYTE_0];
526 526 time[BYTE_1] = timeInBuffer[BYTE_1];
527 527 time[BYTE_2] = timeInBuffer[BYTE_2];
528 528 time[BYTE_3] = timeInBuffer[BYTE_3];
529 529 time[BYTE_4] = timeInBuffer[BYTE_6];
530 530 time[BYTE_5] = timeInBuffer[BYTE_7];
531 531 }
532 532
533 533 unsigned long long int get_acquisition_time( unsigned char *timePtr )
534 534 {
535 535 unsigned long long int acquisitionTimeAslong;
536 536 acquisitionTimeAslong = INIT_CHAR;
537 537 acquisitionTimeAslong =
538 538 ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
539 539 + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
540 540 + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
541 541 + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
542 542 + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE )
543 543 + ( (unsigned long long int) timePtr[BYTE_7] );
544 544 return acquisitionTimeAslong;
545 545 }
546 546
547 547 unsigned char getSID( rtems_event_set event )
548 548 {
549 549 unsigned char sid;
550 550
551 551 rtems_event_set eventSetBURST;
552 552 rtems_event_set eventSetSBM;
553 553
554 554 sid = 0;
555 555
556 556 //******
557 557 // BURST
558 558 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
559 559 | RTEMS_EVENT_BURST_BP1_F1
560 560 | RTEMS_EVENT_BURST_BP2_F0
561 561 | RTEMS_EVENT_BURST_BP2_F1;
562 562
563 563 //****
564 564 // SBM
565 565 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
566 566 | RTEMS_EVENT_SBM_BP1_F1
567 567 | RTEMS_EVENT_SBM_BP2_F0
568 568 | RTEMS_EVENT_SBM_BP2_F1;
569 569
570 570 if (event & eventSetBURST)
571 571 {
572 572 sid = SID_BURST_BP1_F0;
573 573 }
574 574 else if (event & eventSetSBM)
575 575 {
576 576 sid = SID_SBM1_BP1_F0;
577 577 }
578 578 else
579 579 {
580 580 sid = 0;
581 581 }
582 582
583 583 return sid;
584 584 }
585 585
586 586 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
587 587 {
588 588 unsigned int i;
589 589 float re;
590 590 float im;
591 591
592 592 for (i=0; i<NB_BINS_PER_SM; i++){
593 593 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ];
594 594 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
595 595 outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re;
596 596 outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im;
597 597 }
598 598 }
599 599
600 600 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
601 601 {
602 602 unsigned int i;
603 603 float re;
604 604
605 605 for (i=0; i<NB_BINS_PER_SM; i++){
606 606 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
607 607 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
608 608 }
609 609 }
610 610
611 611 void ASM_patch( float *inputASM, float *outputASM )
612 612 {
613 613 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2
614 614 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3
615 615 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1
616 616 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2
617 617 extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3
618 618 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1
619 619 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2
620 620 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1
621 621 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2
622 622 extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2
623 623
624 624 copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1
625 625 copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2
626 626 copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3
627 627 copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1
628 628 copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2
629 629 }
630 630
631 631 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
632 632 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
633 633 unsigned char ASMIndexStart,
634 634 unsigned char channel )
635 635 {
636 636 //*************
637 637 // input format
638 638 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
639 639 //**************
640 640 // output format
641 641 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
642 642 //************
643 643 // compression
644 644 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
645 645 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
646 646
647 647 int frequencyBin;
648 648 int asmComponent;
649 649 int offsetASM;
650 650 int offsetCompressed;
651 651 int offsetFBin;
652 652 int fBinMask;
653 653 int k;
654 654
655 655 // BUILD DATA
656 656 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
657 657 {
658 658 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
659 659 {
660 660 offsetCompressed = // NO TIME OFFSET
661 661 (frequencyBin * NB_VALUES_PER_SM)
662 662 + asmComponent;
663 663 offsetASM = // NO TIME OFFSET
664 664 (asmComponent * NB_BINS_PER_SM)
665 665 + ASMIndexStart
666 666 + (frequencyBin * nbBinsToAverage);
667 667 offsetFBin = ASMIndexStart
668 668 + (frequencyBin * nbBinsToAverage);
669 669 compressed_spec_mat[ offsetCompressed ] = 0;
670 670 for ( k = 0; k < nbBinsToAverage; k++ )
671 671 {
672 672 fBinMask = getFBinMask( offsetFBin + k, channel );
673 673 compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
674 674 + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
675 675 }
676 676 if (divider != 0)
677 677 {
678 678 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
679 679 }
680 680 else
681 681 {
682 682 compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
683 683 }
684 684 }
685 685 }
686 686
687 687 }
688 688
689 689 int getFBinMask( int index, unsigned char channel )
690 690 {
691 691 unsigned int indexInChar;
692 692 unsigned int indexInTheChar;
693 693 int fbin;
694 694 unsigned char *sy_lfr_fbins_fx_word1;
695 695
696 696 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
697 697
698 698 switch(channel)
699 699 {
700 700 case CHANNELF0:
701 701 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
702 702 break;
703 703 case CHANNELF1:
704 704 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
705 705 break;
706 706 case CHANNELF2:
707 707 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
708 708 break;
709 709 default:
710 710 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
711 711 }
712 712
713 713 indexInChar = index >> SHIFT_3_BITS;
714 714 indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
715 715
716 716 fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
717 717
718 718 return fbin;
719 719 }
720 720
721 721 unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 )
722 722 {
723 723 unsigned char polluted;
724 724
725 725 polluted = MATRIX_IS_NOT_POLLUTED;
726 726
727 727 if ( ((tbad0 < t0) && (t0 < tbad1)) // t0 is inside the polluted range
728 728 || ((tbad0 < t1) && (t1 < tbad1)) // t1 is inside the polluted range
729 729 || ((t0 < tbad0) && (tbad1 < t1)) // the polluted range is inside the signal range
730 730 || ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range
731 731 {
732 732 polluted = MATRIX_IS_POLLUTED;
733 733 }
734 734
735 735 return polluted;
736 736 }
737 737
738 738 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
739 739 {
740 740 u_int64_t t0;
741 741 u_int64_t t1;
742 742 u_int64_t tc;
743 743 u_int64_t tbad0;
744 744 u_int64_t tbad1;
745 745
746 746 u_int64_t modulusInFineTime;
747 747 u_int64_t offsetInFineTime;
748 748 u_int64_t shiftInFineTime;
749 749 u_int64_t tbadInFineTime;
750 750
751 751 u_int64_t timecodeReference;
752 752
753 753 unsigned char pasFilteringIsEnabled;
754 754 unsigned char ret;
755 755
756 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
757 ret = MATRIX_IS_NOT_POLLUTED;
758
759 // compute the acquitionTime range
760 modulusInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_modulus) * CONST_65536;
761 offsetInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_offset) * CONST_65536;
762 shiftInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_shift) * CONST_65536;
763 tbadInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_tbad) * CONST_65536;
764
765 756 // compute acquisition time from caoarseTime and fineTime
766 757 t0 = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES ) + (u_int64_t) fineTime;
758 t1 = t0;
759 tc = t0;
760 tbad0 = t0;
761 tbad1 = t0;
762
767 763 switch(channel)
768 764 {
769 765 case CHANNELF0:
770 766 t1 = t0 + ACQUISITION_DURATION_F0;
771 767 tc = t0 + HALF_ACQUISITION_DURATION_F0;
772 768 break;
773 769 case CHANNELF1:
774 770 t1 = t0 + ACQUISITION_DURATION_F1;
775 771 tc = t0 + HALF_ACQUISITION_DURATION_F1;
776 772 break;
777 773 case CHANNELF2:
778 774 t1 = t0 + ACQUISITION_DURATION_F2;
779 775 tc = t0 + HALF_ACQUISITION_DURATION_F2;
780 776 break;
777 default:
778 break;
781 779 }
782 780
783 // INTERSECTION TEST #1
784 timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ;
785 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
786 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
787 ret = isPolluted( t0, t1, tbad0, tbad1 );
781 // compute the acquitionTime range
782 modulusInFineTime = filterPar.modulus_in_finetime;
783 offsetInFineTime = filterPar.offset_in_finetime;
784 shiftInFineTime = filterPar.shift_in_finetime;
785 tbadInFineTime = filterPar.tbad_in_finetime;
786 timecodeReference = INIT_INT;
788 787
789 // INTERSECTION TEST #2
790 timecodeReference = (tc - (tc % modulusInFineTime)) ;
791 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
792 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
793 if (ret == MATRIX_IS_NOT_POLLUTED)
794 {
795 ret = isPolluted( t0, t1, tbad0, tbad1 );
796 }
788 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
789 ret = MATRIX_IS_NOT_POLLUTED;
797 790
798 // INTERSECTION TEST #3
799 timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ;
800 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
801 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
802 if (ret == MATRIX_IS_NOT_POLLUTED)
803 {
804 ret = isPolluted( t0, t1, tbad0, tbad1 );
805 }
806
807 if (pasFilteringIsEnabled == 0)
791 if ( (tbadInFineTime == 0) || (pasFilteringIsEnabled == 0) )
808 792 {
809 793 ret = MATRIX_IS_NOT_POLLUTED;
810 794 }
795 else
796 {
797 // INTERSECTION TEST #1
798 timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ;
799 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
800 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
801 ret = isPolluted( t0, t1, tbad0, tbad1 );
802
803 // INTERSECTION TEST #2
804 if (ret == MATRIX_IS_NOT_POLLUTED)
805 {
806 timecodeReference = (tc - (tc % modulusInFineTime)) ;
807 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
808 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
809 ret = isPolluted( t0, t1, tbad0, tbad1 );
810 }
811
812 // INTERSECTION TEST #3
813 if (ret == MATRIX_IS_NOT_POLLUTED)
814 {
815 timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ;
816 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
817 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
818 ret = isPolluted( t0, t1, tbad0, tbad1 );
819 }
820 }
811 821
812 822 return ret;
813 823 }
814 824
815 825 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
816 826 {
817 827 unsigned char bin;
818 828 unsigned char kcoeff;
819 829
820 830 for (bin=0; bin<nb_bins_norm; bin++)
821 831 {
822 832 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
823 833 {
824 834 output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ]
825 835 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
826 836 output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ]
827 837 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
828 838 }
829 839 }
830 840 }
Show file before | Show file after | Hide comments
@@ -1,2061 +1,2068
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1 = {0};
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = {0};
19 19 ring_node kcoefficient_node_1 = {0};
20 20 ring_node kcoefficient_node_2 = {0};
21 21
22 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 23 {
24 24 /** This function updates the LFR registers with the incoming common parameters.
25 25 *
26 26 * @param TC points to the TeleCommand packet that is being processed
27 27 *
28 28 *
29 29 */
30 30
31 31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 33 set_wfp_data_shaping( );
34 34 return LFR_SUCCESSFUL;
35 35 }
36 36
37 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 38 {
39 39 /** This function updates the LFR registers with the incoming normal parameters.
40 40 *
41 41 * @param TC points to the TeleCommand packet that is being processed
42 42 * @param queue_id is the id of the queue which handles TM related to this execution step
43 43 *
44 44 */
45 45
46 46 int result;
47 47 int flag;
48 48 rtems_status_code status;
49 49
50 50 flag = LFR_SUCCESSFUL;
51 51
52 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 55 flag = LFR_DEFAULT;
56 56 }
57 57
58 58 // CHECK THE PARAMETERS SET CONSISTENCY
59 59 if (flag == LFR_SUCCESSFUL)
60 60 {
61 61 flag = check_normal_par_consistency( TC, queue_id );
62 62 }
63 63
64 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 65 if (flag == LFR_SUCCESSFUL)
66 66 {
67 67 result = set_sy_lfr_n_swf_l( TC );
68 68 result = set_sy_lfr_n_swf_p( TC );
69 69 result = set_sy_lfr_n_bp_p0( TC );
70 70 result = set_sy_lfr_n_bp_p1( TC );
71 71 result = set_sy_lfr_n_asm_p( TC );
72 72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 73 }
74 74
75 75 return flag;
76 76 }
77 77
78 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 79 {
80 80 /** This function updates the LFR registers with the incoming burst parameters.
81 81 *
82 82 * @param TC points to the TeleCommand packet that is being processed
83 83 * @param queue_id is the id of the queue which handles TM related to this execution step
84 84 *
85 85 */
86 86
87 87 int flag;
88 88 rtems_status_code status;
89 89 unsigned char sy_lfr_b_bp_p0;
90 90 unsigned char sy_lfr_b_bp_p1;
91 91 float aux;
92 92
93 93 flag = LFR_SUCCESSFUL;
94 94
95 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 97 flag = LFR_DEFAULT;
98 98 }
99 99
100 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 102
103 103 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 104 if (flag == LFR_SUCCESSFUL)
105 105 {
106 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 107 {
108 108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
109 109 flag = WRONG_APP_DATA;
110 110 }
111 111 }
112 112 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 113 if (flag == LFR_SUCCESSFUL)
114 114 {
115 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 116 {
117 117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1 + DATAFIELD_OFFSET, sy_lfr_b_bp_p1 );
118 118 flag = WRONG_APP_DATA;
119 119 }
120 120 }
121 121 //****************************************************************
122 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 123 if (flag == LFR_SUCCESSFUL)
124 124 {
125 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 128 if (aux > FLOAT_EQUAL_ZERO)
129 129 {
130 130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
131 131 flag = LFR_DEFAULT;
132 132 }
133 133 }
134 134
135 135 // SET THE PARAMETERS
136 136 if (flag == LFR_SUCCESSFUL)
137 137 {
138 138 flag = set_sy_lfr_b_bp_p0( TC );
139 139 flag = set_sy_lfr_b_bp_p1( TC );
140 140 }
141 141
142 142 return flag;
143 143 }
144 144
145 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 146 {
147 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 148 *
149 149 * @param TC points to the TeleCommand packet that is being processed
150 150 * @param queue_id is the id of the queue which handles TM related to this execution step
151 151 *
152 152 */
153 153
154 154 int flag;
155 155 rtems_status_code status;
156 156 unsigned char sy_lfr_s1_bp_p0;
157 157 unsigned char sy_lfr_s1_bp_p1;
158 158 float aux;
159 159
160 160 flag = LFR_SUCCESSFUL;
161 161
162 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 164 flag = LFR_DEFAULT;
165 165 }
166 166
167 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 169
170 170 // sy_lfr_s1_bp_p0
171 171 if (flag == LFR_SUCCESSFUL)
172 172 {
173 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 174 {
175 175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
176 176 flag = WRONG_APP_DATA;
177 177 }
178 178 }
179 179 // sy_lfr_s1_bp_p1
180 180 if (flag == LFR_SUCCESSFUL)
181 181 {
182 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 183 {
184 184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p1 );
185 185 flag = WRONG_APP_DATA;
186 186 }
187 187 }
188 188 //******************************************************************
189 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 190 if (flag == LFR_SUCCESSFUL)
191 191 {
192 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
193 193 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
194 194 if (aux > FLOAT_EQUAL_ZERO)
195 195 {
196 196 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
197 197 flag = LFR_DEFAULT;
198 198 }
199 199 }
200 200
201 201 // SET THE PARAMETERS
202 202 if (flag == LFR_SUCCESSFUL)
203 203 {
204 204 flag = set_sy_lfr_s1_bp_p0( TC );
205 205 flag = set_sy_lfr_s1_bp_p1( TC );
206 206 }
207 207
208 208 return flag;
209 209 }
210 210
211 211 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
212 212 {
213 213 /** This function updates the LFR registers with the incoming sbm2 parameters.
214 214 *
215 215 * @param TC points to the TeleCommand packet that is being processed
216 216 * @param queue_id is the id of the queue which handles TM related to this execution step
217 217 *
218 218 */
219 219
220 220 int flag;
221 221 rtems_status_code status;
222 222 unsigned char sy_lfr_s2_bp_p0;
223 223 unsigned char sy_lfr_s2_bp_p1;
224 224 float aux;
225 225
226 226 flag = LFR_SUCCESSFUL;
227 227
228 228 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
229 229 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
230 230 flag = LFR_DEFAULT;
231 231 }
232 232
233 233 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
234 234 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
235 235
236 236 // sy_lfr_s2_bp_p0
237 237 if (flag == LFR_SUCCESSFUL)
238 238 {
239 239 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
240 240 {
241 241 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
242 242 flag = WRONG_APP_DATA;
243 243 }
244 244 }
245 245 // sy_lfr_s2_bp_p1
246 246 if (flag == LFR_SUCCESSFUL)
247 247 {
248 248 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
249 249 {
250 250 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p1 );
251 251 flag = WRONG_APP_DATA;
252 252 }
253 253 }
254 254 //******************************************************************
255 255 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
256 256 if (flag == LFR_SUCCESSFUL)
257 257 {
258 258 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
259 259 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
260 260 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
261 261 if (aux > FLOAT_EQUAL_ZERO)
262 262 {
263 263 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
264 264 flag = LFR_DEFAULT;
265 265 }
266 266 }
267 267
268 268 // SET THE PARAMETERS
269 269 if (flag == LFR_SUCCESSFUL)
270 270 {
271 271 flag = set_sy_lfr_s2_bp_p0( TC );
272 272 flag = set_sy_lfr_s2_bp_p1( TC );
273 273 }
274 274
275 275 return flag;
276 276 }
277 277
278 278 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
279 279 {
280 280 /** This function updates the LFR registers with the incoming sbm2 parameters.
281 281 *
282 282 * @param TC points to the TeleCommand packet that is being processed
283 283 * @param queue_id is the id of the queue which handles TM related to this execution step
284 284 *
285 285 */
286 286
287 287 int flag;
288 288
289 289 flag = LFR_DEFAULT;
290 290
291 291 flag = set_sy_lfr_kcoeff( TC, queue_id );
292 292
293 293 return flag;
294 294 }
295 295
296 296 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
297 297 {
298 298 /** This function updates the LFR registers with the incoming sbm2 parameters.
299 299 *
300 300 * @param TC points to the TeleCommand packet that is being processed
301 301 * @param queue_id is the id of the queue which handles TM related to this execution step
302 302 *
303 303 */
304 304
305 305 int flag;
306 306
307 307 flag = LFR_DEFAULT;
308 308
309 309 flag = set_sy_lfr_fbins( TC );
310 310
311 311 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
312 312 merge_fbins_masks();
313 313
314 314 return flag;
315 315 }
316 316
317 317 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
318 318 {
319 319 /** This function updates the LFR registers with the incoming sbm2 parameters.
320 320 *
321 321 * @param TC points to the TeleCommand packet that is being processed
322 322 * @param queue_id is the id of the queue which handles TM related to this execution step
323 323 *
324 324 */
325 325
326 326 int flag;
327 327 unsigned char k;
328 328
329 329 flag = LFR_DEFAULT;
330 330 k = INIT_CHAR;
331 331
332 332 flag = check_sy_lfr_filter_parameters( TC, queue_id );
333 333
334 334 if (flag == LFR_SUCCESSFUL)
335 335 {
336 336 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
337 337 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
338 338 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
339 339 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
340 340 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
341 341 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
342 342 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
343 343 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
344 344 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
345 345 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
346 346 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
347 347 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0 ];
348 348 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1 ];
349 349 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2 ];
350 350 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3 ];
351 351
352 352 //****************************
353 353 // store PAS filter parameters
354
354 355 // sy_lfr_pas_filter_enabled
355 356 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
356 357 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
358
357 359 // sy_lfr_pas_filter_modulus
358 filterPar.sy_lfr_pas_filter_modulus = parameter_dump_packet.sy_lfr_pas_filter_modulus;
360 filterPar.modulus_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_modulus) * CONST_65536;
361
359 362 // sy_lfr_pas_filter_tbad
360 363 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
361 364 parameter_dump_packet.sy_lfr_pas_filter_tbad );
365 filterPar.tbad_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_tbad * CONST_65536);
366
362 367 // sy_lfr_pas_filter_offset
363 filterPar.sy_lfr_pas_filter_offset = parameter_dump_packet.sy_lfr_pas_filter_offset;
368 filterPar.offset_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_offset) * CONST_65536;
369
364 370 // sy_lfr_pas_filter_shift
365 371 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
366 372 parameter_dump_packet.sy_lfr_pas_filter_shift );
373 filterPar.shift_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_shift * CONST_65536);
367 374
368 375 //****************************************************
369 376 // store the parameter sy_lfr_sc_rw_delta_f as a float
370 377 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
371 378 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
372 379
373 380 // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
374 381 for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
375 382 {
376 383 parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ];
377 384 }
378 385
379 386 //***********************************************
380 387 // store the parameter sy_lfr_rw.._k.. as a float
381 388 // rw1_k
382 389 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 );
383 390 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 );
384 391 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 );
385 392 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 );
386 393 // rw2_k
387 394 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 );
388 395 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 );
389 396 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 );
390 397 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 );
391 398 // rw3_k
392 399 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 );
393 400 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 );
394 401 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 );
395 402 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 );
396 403 // rw4_k
397 404 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 );
398 405 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 );
399 406 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 );
400 407 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 );
401 408
402 409 }
403 410
404 411 return flag;
405 412 }
406 413
407 414 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
408 415 {
409 416 /** This function updates the LFR registers with the incoming sbm2 parameters.
410 417 *
411 418 * @param TC points to the TeleCommand packet that is being processed
412 419 * @param queue_id is the id of the queue which handles TM related to this execution step
413 420 *
414 421 */
415 422
416 423 unsigned int address;
417 424 rtems_status_code status;
418 425 unsigned int freq;
419 426 unsigned int bin;
420 427 unsigned int coeff;
421 428 unsigned char *kCoeffPtr;
422 429 unsigned char *kCoeffDumpPtr;
423 430
424 431 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
425 432 // F0 => 11 bins
426 433 // F1 => 13 bins
427 434 // F2 => 12 bins
428 435 // 36 bins to dump in two packets (30 bins max per packet)
429 436
430 437 //*********
431 438 // PACKET 1
432 439 // 11 F0 bins, 13 F1 bins and 6 F2 bins
433 440 kcoefficients_dump_1.destinationID = TC->sourceID;
434 441 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
435 442 for( freq = 0;
436 443 freq < NB_BINS_COMPRESSED_SM_F0;
437 444 freq++ )
438 445 {
439 446 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
440 447 bin = freq;
441 448 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
442 449 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
443 450 {
444 451 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
445 452 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
446 453 ]; // 2 for the kcoeff_frequency
447 454 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
448 455 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
449 456 }
450 457 }
451 458 for( freq = NB_BINS_COMPRESSED_SM_F0;
452 459 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
453 460 freq++ )
454 461 {
455 462 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
456 463 bin = freq - NB_BINS_COMPRESSED_SM_F0;
457 464 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
458 465 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
459 466 {
460 467 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
461 468 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
462 469 ]; // 2 for the kcoeff_frequency
463 470 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
464 471 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
465 472 }
466 473 }
467 474 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
468 475 freq < KCOEFF_BLK_NR_PKT1 ;
469 476 freq++ )
470 477 {
471 478 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
472 479 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
473 480 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
474 481 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
475 482 {
476 483 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
477 484 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
478 485 ]; // 2 for the kcoeff_frequency
479 486 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
480 487 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
481 488 }
482 489 }
483 490 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
484 491 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
485 492 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
486 493 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
487 494 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
488 495 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
489 496 // SEND DATA
490 497 kcoefficient_node_1.status = 1;
491 498 address = (unsigned int) &kcoefficient_node_1;
492 499 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
493 500 if (status != RTEMS_SUCCESSFUL) {
494 501 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
495 502 }
496 503
497 504 //********
498 505 // PACKET 2
499 506 // 6 F2 bins
500 507 kcoefficients_dump_2.destinationID = TC->sourceID;
501 508 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
502 509 for( freq = 0;
503 510 freq < KCOEFF_BLK_NR_PKT2;
504 511 freq++ )
505 512 {
506 513 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
507 514 bin = freq + KCOEFF_BLK_NR_PKT2;
508 515 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
509 516 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
510 517 {
511 518 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
512 519 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
513 520 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
514 521 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
515 522 }
516 523 }
517 524 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
518 525 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
519 526 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
520 527 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
521 528 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
522 529 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
523 530 // SEND DATA
524 531 kcoefficient_node_2.status = 1;
525 532 address = (unsigned int) &kcoefficient_node_2;
526 533 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
527 534 if (status != RTEMS_SUCCESSFUL) {
528 535 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
529 536 }
530 537
531 538 return status;
532 539 }
533 540
534 541 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
535 542 {
536 543 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
537 544 *
538 545 * @param queue_id is the id of the queue which handles TM related to this execution step.
539 546 *
540 547 * @return RTEMS directive status codes:
541 548 * - RTEMS_SUCCESSFUL - message sent successfully
542 549 * - RTEMS_INVALID_ID - invalid queue id
543 550 * - RTEMS_INVALID_SIZE - invalid message size
544 551 * - RTEMS_INVALID_ADDRESS - buffer is NULL
545 552 * - RTEMS_UNSATISFIED - out of message buffers
546 553 * - RTEMS_TOO_MANY - queue s limit has been reached
547 554 *
548 555 */
549 556
550 557 int status;
551 558
552 559 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
553 560 parameter_dump_packet.destinationID = TC->sourceID;
554 561
555 562 // UPDATE TIME
556 563 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
557 564 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
558 565 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
559 566 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
560 567 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
561 568 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
562 569 // SEND DATA
563 570 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
564 571 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
565 572 if (status != RTEMS_SUCCESSFUL) {
566 573 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
567 574 }
568 575
569 576 return status;
570 577 }
571 578
572 579 //***********************
573 580 // NORMAL MODE PARAMETERS
574 581
575 582 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
576 583 {
577 584 unsigned char msb;
578 585 unsigned char lsb;
579 586 int flag;
580 587 float aux;
581 588 rtems_status_code status;
582 589
583 590 unsigned int sy_lfr_n_swf_l;
584 591 unsigned int sy_lfr_n_swf_p;
585 592 unsigned int sy_lfr_n_asm_p;
586 593 unsigned char sy_lfr_n_bp_p0;
587 594 unsigned char sy_lfr_n_bp_p1;
588 595 unsigned char sy_lfr_n_cwf_long_f3;
589 596
590 597 flag = LFR_SUCCESSFUL;
591 598
592 599 //***************
593 600 // get parameters
594 601 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
595 602 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
596 603 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
597 604
598 605 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
599 606 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
600 607 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
601 608
602 609 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
603 610 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
604 611 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
605 612
606 613 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
607 614
608 615 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
609 616
610 617 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
611 618
612 619 //******************
613 620 // check consistency
614 621 // sy_lfr_n_swf_l
615 622 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
616 623 {
617 624 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
618 625 flag = WRONG_APP_DATA;
619 626 }
620 627 // sy_lfr_n_swf_p
621 628 if (flag == LFR_SUCCESSFUL)
622 629 {
623 630 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
624 631 {
625 632 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
626 633 flag = WRONG_APP_DATA;
627 634 }
628 635 }
629 636 // sy_lfr_n_bp_p0
630 637 if (flag == LFR_SUCCESSFUL)
631 638 {
632 639 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
633 640 {
634 641 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
635 642 flag = WRONG_APP_DATA;
636 643 }
637 644 }
638 645 // sy_lfr_n_asm_p
639 646 if (flag == LFR_SUCCESSFUL)
640 647 {
641 648 if (sy_lfr_n_asm_p == 0)
642 649 {
643 650 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
644 651 flag = WRONG_APP_DATA;
645 652 }
646 653 }
647 654 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
648 655 if (flag == LFR_SUCCESSFUL)
649 656 {
650 657 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
651 658 if (aux > FLOAT_EQUAL_ZERO)
652 659 {
653 660 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
654 661 flag = WRONG_APP_DATA;
655 662 }
656 663 }
657 664 // sy_lfr_n_bp_p1
658 665 if (flag == LFR_SUCCESSFUL)
659 666 {
660 667 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
661 668 {
662 669 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
663 670 flag = WRONG_APP_DATA;
664 671 }
665 672 }
666 673 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
667 674 if (flag == LFR_SUCCESSFUL)
668 675 {
669 676 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
670 677 if (aux > FLOAT_EQUAL_ZERO)
671 678 {
672 679 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
673 680 flag = LFR_DEFAULT;
674 681 }
675 682 }
676 683 // sy_lfr_n_cwf_long_f3
677 684
678 685 return flag;
679 686 }
680 687
681 688 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
682 689 {
683 690 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
684 691 *
685 692 * @param TC points to the TeleCommand packet that is being processed
686 693 * @param queue_id is the id of the queue which handles TM related to this execution step
687 694 *
688 695 */
689 696
690 697 int result;
691 698
692 699 result = LFR_SUCCESSFUL;
693 700
694 701 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
695 702 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
696 703
697 704 return result;
698 705 }
699 706
700 707 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
701 708 {
702 709 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
703 710 *
704 711 * @param TC points to the TeleCommand packet that is being processed
705 712 * @param queue_id is the id of the queue which handles TM related to this execution step
706 713 *
707 714 */
708 715
709 716 int result;
710 717
711 718 result = LFR_SUCCESSFUL;
712 719
713 720 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
714 721 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
715 722
716 723 return result;
717 724 }
718 725
719 726 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
720 727 {
721 728 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
722 729 *
723 730 * @param TC points to the TeleCommand packet that is being processed
724 731 * @param queue_id is the id of the queue which handles TM related to this execution step
725 732 *
726 733 */
727 734
728 735 int result;
729 736
730 737 result = LFR_SUCCESSFUL;
731 738
732 739 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
733 740 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
734 741
735 742 return result;
736 743 }
737 744
738 745 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
739 746 {
740 747 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
741 748 *
742 749 * @param TC points to the TeleCommand packet that is being processed
743 750 * @param queue_id is the id of the queue which handles TM related to this execution step
744 751 *
745 752 */
746 753
747 754 int status;
748 755
749 756 status = LFR_SUCCESSFUL;
750 757
751 758 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
752 759
753 760 return status;
754 761 }
755 762
756 763 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
757 764 {
758 765 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
759 766 *
760 767 * @param TC points to the TeleCommand packet that is being processed
761 768 * @param queue_id is the id of the queue which handles TM related to this execution step
762 769 *
763 770 */
764 771
765 772 int status;
766 773
767 774 status = LFR_SUCCESSFUL;
768 775
769 776 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
770 777
771 778 return status;
772 779 }
773 780
774 781 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
775 782 {
776 783 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
777 784 *
778 785 * @param TC points to the TeleCommand packet that is being processed
779 786 * @param queue_id is the id of the queue which handles TM related to this execution step
780 787 *
781 788 */
782 789
783 790 int status;
784 791
785 792 status = LFR_SUCCESSFUL;
786 793
787 794 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
788 795
789 796 return status;
790 797 }
791 798
792 799 //**********************
793 800 // BURST MODE PARAMETERS
794 801
795 802 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
796 803 {
797 804 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
798 805 *
799 806 * @param TC points to the TeleCommand packet that is being processed
800 807 * @param queue_id is the id of the queue which handles TM related to this execution step
801 808 *
802 809 */
803 810
804 811 int status;
805 812
806 813 status = LFR_SUCCESSFUL;
807 814
808 815 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
809 816
810 817 return status;
811 818 }
812 819
813 820 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
814 821 {
815 822 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
816 823 *
817 824 * @param TC points to the TeleCommand packet that is being processed
818 825 * @param queue_id is the id of the queue which handles TM related to this execution step
819 826 *
820 827 */
821 828
822 829 int status;
823 830
824 831 status = LFR_SUCCESSFUL;
825 832
826 833 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
827 834
828 835 return status;
829 836 }
830 837
831 838 //*********************
832 839 // SBM1 MODE PARAMETERS
833 840
834 841 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
835 842 {
836 843 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
837 844 *
838 845 * @param TC points to the TeleCommand packet that is being processed
839 846 * @param queue_id is the id of the queue which handles TM related to this execution step
840 847 *
841 848 */
842 849
843 850 int status;
844 851
845 852 status = LFR_SUCCESSFUL;
846 853
847 854 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
848 855
849 856 return status;
850 857 }
851 858
852 859 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
853 860 {
854 861 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
855 862 *
856 863 * @param TC points to the TeleCommand packet that is being processed
857 864 * @param queue_id is the id of the queue which handles TM related to this execution step
858 865 *
859 866 */
860 867
861 868 int status;
862 869
863 870 status = LFR_SUCCESSFUL;
864 871
865 872 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
866 873
867 874 return status;
868 875 }
869 876
870 877 //*********************
871 878 // SBM2 MODE PARAMETERS
872 879
873 880 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
874 881 {
875 882 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
876 883 *
877 884 * @param TC points to the TeleCommand packet that is being processed
878 885 * @param queue_id is the id of the queue which handles TM related to this execution step
879 886 *
880 887 */
881 888
882 889 int status;
883 890
884 891 status = LFR_SUCCESSFUL;
885 892
886 893 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
887 894
888 895 return status;
889 896 }
890 897
891 898 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
892 899 {
893 900 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
894 901 *
895 902 * @param TC points to the TeleCommand packet that is being processed
896 903 * @param queue_id is the id of the queue which handles TM related to this execution step
897 904 *
898 905 */
899 906
900 907 int status;
901 908
902 909 status = LFR_SUCCESSFUL;
903 910
904 911 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
905 912
906 913 return status;
907 914 }
908 915
909 916 //*******************
910 917 // TC_LFR_UPDATE_INFO
911 918
912 919 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
913 920 {
914 921 unsigned int status;
915 922
916 923 status = LFR_DEFAULT;
917 924
918 925 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
919 926 || (mode == LFR_MODE_BURST)
920 927 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
921 928 {
922 929 status = LFR_SUCCESSFUL;
923 930 }
924 931 else
925 932 {
926 933 status = LFR_DEFAULT;
927 934 }
928 935
929 936 return status;
930 937 }
931 938
932 939 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
933 940 {
934 941 unsigned int status;
935 942
936 943 status = LFR_DEFAULT;
937 944
938 945 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
939 946 || (mode == TDS_MODE_BURST)
940 947 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
941 948 || (mode == TDS_MODE_LFM))
942 949 {
943 950 status = LFR_SUCCESSFUL;
944 951 }
945 952 else
946 953 {
947 954 status = LFR_DEFAULT;
948 955 }
949 956
950 957 return status;
951 958 }
952 959
953 960 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
954 961 {
955 962 unsigned int status;
956 963
957 964 status = LFR_DEFAULT;
958 965
959 966 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
960 967 || (mode == THR_MODE_BURST))
961 968 {
962 969 status = LFR_SUCCESSFUL;
963 970 }
964 971 else
965 972 {
966 973 status = LFR_DEFAULT;
967 974 }
968 975
969 976 return status;
970 977 }
971 978
972 979 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value )
973 980 {
974 981 unsigned char flag;
975 982 unsigned char flagPosInByte;
976 983 unsigned char newFlag;
977 984 unsigned char flagMask;
978 985
979 986 // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered
980 987 if (isnan(value))
981 988 {
982 989 flag = FLAG_NAN;
983 990 }
984 991 else
985 992 {
986 993 flag = FLAG_IAN;
987 994 }
988 995
989 996 switch(wheel)
990 997 {
991 998 case WHEEL_1:
992 999 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
993 1000 flagMask = ~(1 << flagPosInByte);
994 1001 newFlag = flag << flagPosInByte;
995 1002 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
996 1003 break;
997 1004 case WHEEL_2:
998 1005 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
999 1006 flagMask = ~(1 << flagPosInByte);
1000 1007 newFlag = flag << flagPosInByte;
1001 1008 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1002 1009 break;
1003 1010 case WHEEL_3:
1004 1011 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1005 1012 flagMask = ~(1 << flagPosInByte);
1006 1013 newFlag = flag << flagPosInByte;
1007 1014 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1008 1015 break;
1009 1016 case WHEEL_4:
1010 1017 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1011 1018 flagMask = ~(1 << flagPosInByte);
1012 1019 newFlag = flag << flagPosInByte;
1013 1020 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1014 1021 break;
1015 1022 default:
1016 1023 break;
1017 1024 }
1018 1025 }
1019 1026
1020 1027 void set_hk_lfr_sc_rw_f_flags( void )
1021 1028 {
1022 1029 // RW1
1023 1030 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 );
1024 1031 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 );
1025 1032 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 );
1026 1033 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 );
1027 1034
1028 1035 // RW2
1029 1036 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 );
1030 1037 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 );
1031 1038 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 );
1032 1039 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 );
1033 1040
1034 1041 // RW3
1035 1042 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 );
1036 1043 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 );
1037 1044 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 );
1038 1045 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 );
1039 1046
1040 1047 // RW4
1041 1048 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 );
1042 1049 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 );
1043 1050 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 );
1044 1051 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 );
1045 1052 }
1046 1053
1047 1054 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1048 1055 {
1049 1056 float rw_k;
1050 1057 int ret;
1051 1058
1052 1059 ret = LFR_SUCCESSFUL;
1053 1060 rw_k = INIT_FLOAT;
1054 1061
1055 1062 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->packetID[ offset ] );
1056 1063
1057 1064 *pos = offset;
1058 1065 *value = rw_k;
1059 1066
1060 1067 if (rw_k < MIN_SY_LFR_RW_F)
1061 1068 {
1062 1069 ret = WRONG_APP_DATA;
1063 1070 }
1064 1071
1065 1072 return ret;
1066 1073 }
1067 1074
1068 1075 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value )
1069 1076 {
1070 1077 int ret;
1071 1078
1072 1079 ret = LFR_SUCCESSFUL;
1073 1080
1074 1081 //****
1075 1082 //****
1076 1083 // RW1
1077 1084 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value ); // F1
1078 1085 if (ret == LFR_SUCCESSFUL) // F2
1079 1086 {
1080 1087 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value );
1081 1088 }
1082 1089 if (ret == LFR_SUCCESSFUL) // F3
1083 1090 {
1084 1091 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value );
1085 1092 }
1086 1093 if (ret == LFR_SUCCESSFUL) // F4
1087 1094 {
1088 1095 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4, pos, value );
1089 1096 }
1090 1097
1091 1098 //****
1092 1099 //****
1093 1100 // RW2
1094 1101 if (ret == LFR_SUCCESSFUL) // F1
1095 1102 {
1096 1103 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value );
1097 1104 }
1098 1105 if (ret == LFR_SUCCESSFUL) // F2
1099 1106 {
1100 1107 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value );
1101 1108 }
1102 1109 if (ret == LFR_SUCCESSFUL) // F3
1103 1110 {
1104 1111 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value );
1105 1112 }
1106 1113 if (ret == LFR_SUCCESSFUL) // F4
1107 1114 {
1108 1115 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4, pos, value );
1109 1116 }
1110 1117
1111 1118 //****
1112 1119 //****
1113 1120 // RW3
1114 1121 if (ret == LFR_SUCCESSFUL) // F1
1115 1122 {
1116 1123 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value );
1117 1124 }
1118 1125 if (ret == LFR_SUCCESSFUL) // F2
1119 1126 {
1120 1127 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value );
1121 1128 }
1122 1129 if (ret == LFR_SUCCESSFUL) // F3
1123 1130 {
1124 1131 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value );
1125 1132 }
1126 1133 if (ret == LFR_SUCCESSFUL) // F4
1127 1134 {
1128 1135 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4, pos, value );
1129 1136 }
1130 1137
1131 1138 //****
1132 1139 //****
1133 1140 // RW4
1134 1141 if (ret == LFR_SUCCESSFUL) // F1
1135 1142 {
1136 1143 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value );
1137 1144 }
1138 1145 if (ret == LFR_SUCCESSFUL) // F2
1139 1146 {
1140 1147 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value );
1141 1148 }
1142 1149 if (ret == LFR_SUCCESSFUL) // F3
1143 1150 {
1144 1151 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value );
1145 1152 }
1146 1153 if (ret == LFR_SUCCESSFUL) // F4
1147 1154 {
1148 1155 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value );
1149 1156 }
1150 1157
1151 1158 return ret;
1152 1159 }
1153 1160
1154 1161 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
1155 1162 {
1156 1163 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
1157 1164 *
1158 1165 * @param TC points to the TeleCommand packet that is being processed
1159 1166 *
1160 1167 */
1161 1168
1162 1169 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
1163 1170
1164 1171 bytePosPtr = (unsigned char *) &TC->packetID;
1165 1172
1166 1173 // rw1_f
1167 1174 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
1168 1175 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1169 1176 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
1170 1177 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
1171 1178
1172 1179 // rw2_f
1173 1180 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
1174 1181 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1175 1182 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
1176 1183 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
1177 1184
1178 1185 // rw3_f
1179 1186 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
1180 1187 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1181 1188 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
1182 1189 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
1183 1190
1184 1191 // rw4_f
1185 1192 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1186 1193 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1187 1194 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] );
1188 1195 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] );
1189 1196
1190 1197 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
1191 1198
1192 1199 }
1193 1200
1194 1201 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k )
1195 1202 {
1196 1203 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1197 1204 *
1198 1205 * @param fbins_mask
1199 1206 * @param rw_f is the reaction wheel frequency to filter
1200 1207 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
1201 1208 * @param flag [true] filtering enabled [false] filtering disabled
1202 1209 *
1203 1210 * @return void
1204 1211 *
1205 1212 */
1206 1213
1207 1214 float f_RW_min;
1208 1215 float f_RW_MAX;
1209 1216 float fi_min;
1210 1217 float fi_MAX;
1211 1218 float fi;
1212 1219 float deltaBelow;
1213 1220 float deltaAbove;
1214 1221 float freqToFilterOut;
1215 1222 int binBelow;
1216 1223 int binAbove;
1217 1224 int closestBin;
1218 1225 unsigned int whichByte;
1219 1226 int selectedByte;
1220 1227 int bin;
1221 1228 int binToRemove[NB_BINS_TO_REMOVE];
1222 1229 int k;
1223 1230 bool filteringSet;
1224 1231
1225 1232 closestBin = 0;
1226 1233 whichByte = 0;
1227 1234 bin = 0;
1228 1235 filteringSet = false;
1229 1236
1230 1237 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1231 1238 {
1232 1239 binToRemove[k] = -1;
1233 1240 }
1234 1241
1235 1242 if (!isnan(rw_f))
1236 1243 {
1237 1244 // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
1238 1245 f_RW_min = rw_f - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1239 1246 f_RW_MAX = rw_f + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1240 1247
1241 1248 freqToFilterOut = f_RW_min;
1242 1249 while ( filteringSet == false )
1243 1250 {
1244 1251 // compute the index of the frequency bin immediately below rw_f
1245 1252 binBelow = (int) ( floor( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1246 1253 deltaBelow = freqToFilterOut - binBelow * deltaFreq;
1247 1254
1248 1255 // compute the index of the frequency bin immediately above rw_f
1249 1256 binAbove = (int) ( ceil( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1250 1257 deltaAbove = binAbove * deltaFreq - freqToFilterOut;
1251 1258
1252 1259 // search the closest bin
1253 1260 if (deltaAbove > deltaBelow)
1254 1261 {
1255 1262 closestBin = binBelow;
1256 1263 }
1257 1264 else
1258 1265 {
1259 1266 closestBin = binAbove;
1260 1267 }
1261 1268
1262 1269 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1263 1270 fi = closestBin * deltaFreq;
1264 1271 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1265 1272 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1266 1273
1267 1274 //**************************************************************************************
1268 1275 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1269 1276 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1270 1277 //**************************************************************************************
1271 1278
1272 1279 // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
1273 1280 // => remove f_(i), f_(i-1) and f_(i+1)
1274 1281 if ( ( freqToFilterOut > fi_min ) && ( freqToFilterOut < fi_MAX ) )
1275 1282 {
1276 1283 binToRemove[0] = (closestBin - 1) - 1;
1277 1284 binToRemove[1] = (closestBin) - 1;
1278 1285 binToRemove[2] = (closestBin + 1) - 1;
1279 1286 }
1280 1287 // 2. ELSE
1281 1288 // => remove the two f_(i) which are around f_RW
1282 1289 else
1283 1290 {
1284 1291 binToRemove[0] = (binBelow) - 1;
1285 1292 binToRemove[1] = (binAbove) - 1;
1286 1293 binToRemove[2] = (-1);
1287 1294 }
1288 1295
1289 1296 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1290 1297 {
1291 1298 bin = binToRemove[k];
1292 1299 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1293 1300 {
1294 1301 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1295 1302 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1296 1303 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1297 1304 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1298 1305
1299 1306 }
1300 1307 }
1301 1308
1302 1309 // update freqToFilterOut
1303 1310 if ( freqToFilterOut == f_RW_MAX )
1304 1311 {
1305 1312 filteringSet = true; // end of the loop
1306 1313 }
1307 1314 else
1308 1315 {
1309 1316 freqToFilterOut = freqToFilterOut + deltaFreq;
1310 1317 }
1311 1318
1312 1319 if ( freqToFilterOut > f_RW_MAX)
1313 1320 {
1314 1321 freqToFilterOut = f_RW_MAX;
1315 1322 }
1316 1323 }
1317 1324 }
1318 1325 }
1319 1326
1320 1327 void build_sy_lfr_rw_mask( unsigned int channel )
1321 1328 {
1322 1329 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1323 1330 unsigned char *maskPtr;
1324 1331 double deltaF;
1325 1332 unsigned k;
1326 1333
1327 1334 maskPtr = NULL;
1328 1335 deltaF = DELTAF_F2;
1329 1336
1330 1337 switch (channel)
1331 1338 {
1332 1339 case CHANNELF0:
1333 1340 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1334 1341 deltaF = DELTAF_F0;
1335 1342 break;
1336 1343 case CHANNELF1:
1337 1344 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1338 1345 deltaF = DELTAF_F1;
1339 1346 break;
1340 1347 case CHANNELF2:
1341 1348 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1342 1349 deltaF = DELTAF_F2;
1343 1350 break;
1344 1351 default:
1345 1352 break;
1346 1353 }
1347 1354
1348 1355 for (k = 0; k < BYTES_PER_MASK; k++)
1349 1356 {
1350 1357 local_rw_fbins_mask[k] = INT8_ALL_F;
1351 1358 }
1352 1359
1353 1360 // RW1
1354 1361 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1355 1362 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1356 1363 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1357 1364 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1358 1365
1359 1366 // RW2
1360 1367 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1361 1368 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1362 1369 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1363 1370 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1364 1371
1365 1372 // RW3
1366 1373 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1367 1374 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1368 1375 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1369 1376 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1370 1377
1371 1378 // RW4
1372 1379 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1373 1380 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1374 1381 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1375 1382 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1376 1383
1377 1384 // update the value of the fbins related to reaction wheels frequency filtering
1378 1385 if (maskPtr != NULL)
1379 1386 {
1380 1387 for (k = 0; k < BYTES_PER_MASK; k++)
1381 1388 {
1382 1389 maskPtr[k] = local_rw_fbins_mask[k];
1383 1390 }
1384 1391 }
1385 1392 }
1386 1393
1387 1394 void build_sy_lfr_rw_masks( void )
1388 1395 {
1389 1396 build_sy_lfr_rw_mask( CHANNELF0 );
1390 1397 build_sy_lfr_rw_mask( CHANNELF1 );
1391 1398 build_sy_lfr_rw_mask( CHANNELF2 );
1392 1399 }
1393 1400
1394 1401 void merge_fbins_masks( void )
1395 1402 {
1396 1403 unsigned char k;
1397 1404
1398 1405 unsigned char *fbins_f0;
1399 1406 unsigned char *fbins_f1;
1400 1407 unsigned char *fbins_f2;
1401 1408 unsigned char *rw_mask_f0;
1402 1409 unsigned char *rw_mask_f1;
1403 1410 unsigned char *rw_mask_f2;
1404 1411
1405 1412 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1406 1413 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1407 1414 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1408 1415 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1409 1416 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1410 1417 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1411 1418
1412 1419 for( k=0; k < BYTES_PER_MASK; k++ )
1413 1420 {
1414 1421 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1415 1422 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1416 1423 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1417 1424 }
1418 1425 }
1419 1426
1420 1427 //***********
1421 1428 // FBINS MASK
1422 1429
1423 1430 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1424 1431 {
1425 1432 int status;
1426 1433 unsigned int k;
1427 1434 unsigned char *fbins_mask_dump;
1428 1435 unsigned char *fbins_mask_TC;
1429 1436
1430 1437 status = LFR_SUCCESSFUL;
1431 1438
1432 1439 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1433 1440 fbins_mask_TC = TC->dataAndCRC;
1434 1441
1435 1442 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1436 1443 {
1437 1444 fbins_mask_dump[k] = fbins_mask_TC[k];
1438 1445 }
1439 1446
1440 1447 return status;
1441 1448 }
1442 1449
1443 1450 //***************************
1444 1451 // TC_LFR_LOAD_PAS_FILTER_PAR
1445 1452
1446 1453 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1447 1454 {
1448 1455 float rw_k;
1449 1456 int ret;
1450 1457
1451 1458 ret = LFR_SUCCESSFUL;
1452 1459 rw_k = INIT_FLOAT;
1453 1460
1454 1461 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->dataAndCRC[ offset ] );
1455 1462
1456 1463 *pos = offset;
1457 1464 *value = rw_k;
1458 1465
1459 1466 if (rw_k < MIN_SY_LFR_RW_F)
1460 1467 {
1461 1468 ret = WRONG_APP_DATA;
1462 1469 }
1463 1470
1464 1471 return ret;
1465 1472 }
1466 1473
1467 1474 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float *value )
1468 1475 {
1469 1476 int ret;
1470 1477
1471 1478 ret = LFR_SUCCESSFUL;
1472 1479
1473 1480 //****
1474 1481 //****
1475 1482 // RW1
1476 1483 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value ); // K1
1477 1484 if (ret == LFR_SUCCESSFUL) // K2
1478 1485 {
1479 1486 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value );
1480 1487 }
1481 1488 if (ret == LFR_SUCCESSFUL) // K3
1482 1489 {
1483 1490 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value );
1484 1491 }
1485 1492 if (ret == LFR_SUCCESSFUL) // K4
1486 1493 {
1487 1494 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value );
1488 1495 }
1489 1496
1490 1497 //****
1491 1498 //****
1492 1499 // RW2
1493 1500 if (ret == LFR_SUCCESSFUL) // K1
1494 1501 {
1495 1502 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value );
1496 1503 }
1497 1504 if (ret == LFR_SUCCESSFUL) // K2
1498 1505 {
1499 1506 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value );
1500 1507 }
1501 1508 if (ret == LFR_SUCCESSFUL) // K3
1502 1509 {
1503 1510 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value );
1504 1511 }
1505 1512 if (ret == LFR_SUCCESSFUL) // K4
1506 1513 {
1507 1514 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value );
1508 1515 }
1509 1516
1510 1517 //****
1511 1518 //****
1512 1519 // RW3
1513 1520 if (ret == LFR_SUCCESSFUL) // K1
1514 1521 {
1515 1522 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value );
1516 1523 }
1517 1524 if (ret == LFR_SUCCESSFUL) // K2
1518 1525 {
1519 1526 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value );
1520 1527 }
1521 1528 if (ret == LFR_SUCCESSFUL) // K3
1522 1529 {
1523 1530 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value );
1524 1531 }
1525 1532 if (ret == LFR_SUCCESSFUL) // K4
1526 1533 {
1527 1534 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value );
1528 1535 }
1529 1536
1530 1537 //****
1531 1538 //****
1532 1539 // RW4
1533 1540 if (ret == LFR_SUCCESSFUL) // K1
1534 1541 {
1535 1542 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value );
1536 1543 }
1537 1544 if (ret == LFR_SUCCESSFUL) // K2
1538 1545 {
1539 1546 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value );
1540 1547 }
1541 1548 if (ret == LFR_SUCCESSFUL) // K3
1542 1549 {
1543 1550 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value );
1544 1551 }
1545 1552 if (ret == LFR_SUCCESSFUL) // K4
1546 1553 {
1547 1554 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value );
1548 1555 }
1549 1556
1550 1557 return ret;
1551 1558 }
1552 1559
1553 1560 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1554 1561 {
1555 1562 int flag;
1556 1563 rtems_status_code status;
1557 1564
1558 1565 unsigned char sy_lfr_pas_filter_enabled;
1559 1566 unsigned char sy_lfr_pas_filter_modulus;
1560 1567 float sy_lfr_pas_filter_tbad;
1561 1568 unsigned char sy_lfr_pas_filter_offset;
1562 1569 float sy_lfr_pas_filter_shift;
1563 1570 float sy_lfr_sc_rw_delta_f;
1564 1571 char *parPtr;
1565 1572 int datafield_pos;
1566 1573 float rw_k;
1567 1574
1568 1575 flag = LFR_SUCCESSFUL;
1569 1576 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1570 1577 sy_lfr_pas_filter_shift = INIT_FLOAT;
1571 1578 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1572 1579 parPtr = NULL;
1573 1580 datafield_pos = INIT_INT;
1574 1581 rw_k = INIT_FLOAT;
1575 1582
1576 1583 //***************
1577 1584 // get parameters
1578 1585 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1579 1586 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1580 1587 copyFloatByChar(
1581 1588 (unsigned char*) &sy_lfr_pas_filter_tbad,
1582 1589 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1583 1590 );
1584 1591 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1585 1592 copyFloatByChar(
1586 1593 (unsigned char*) &sy_lfr_pas_filter_shift,
1587 1594 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1588 1595 );
1589 1596 copyFloatByChar(
1590 1597 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1591 1598 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1592 1599 );
1593 1600
1594 1601 //******************
1595 1602 // CHECK CONSISTENCY
1596 1603
1597 1604 //**************************
1598 1605 // sy_lfr_pas_filter_enabled
1599 1606 // nothing to check, value is 0 or 1
1600 1607
1601 1608 //**************************
1602 1609 // sy_lfr_pas_filter_modulus
1603 1610 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1604 1611 {
1605 1612 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1606 1613 flag = WRONG_APP_DATA;
1607 1614 }
1608 1615
1609 1616 //***********************
1610 1617 // sy_lfr_pas_filter_tbad
1611 1618 if (flag == LFR_SUCCESSFUL)
1612 1619 {
1613 1620 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1614 1621 {
1615 1622 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1616 1623 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1617 1624 flag = WRONG_APP_DATA;
1618 1625 }
1619 1626 }
1620 1627
1621 1628 //*************************
1622 1629 // sy_lfr_pas_filter_offset
1623 1630 if (flag == LFR_SUCCESSFUL)
1624 1631 {
1625 1632 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1626 1633 {
1627 1634 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1628 1635 flag = WRONG_APP_DATA;
1629 1636 }
1630 1637 }
1631 1638
1632 1639 //************************
1633 1640 // sy_lfr_pas_filter_shift
1634 1641 if (flag == LFR_SUCCESSFUL)
1635 1642 {
1636 1643 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1637 1644 {
1638 1645 parPtr = (char*) &sy_lfr_pas_filter_shift;
1639 1646 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1640 1647 flag = WRONG_APP_DATA;
1641 1648 }
1642 1649 }
1643 1650
1644 1651 //*************************************
1645 1652 // check global coherency of the values
1646 1653 if (flag == LFR_SUCCESSFUL)
1647 1654 {
1648 1655 if ( (sy_lfr_pas_filter_tbad + sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) > sy_lfr_pas_filter_modulus )
1649 1656 {
1650 1657 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1651 1658 flag = WRONG_APP_DATA;
1652 1659 }
1653 1660 }
1654 1661
1655 1662 //*********************
1656 1663 // sy_lfr_sc_rw_delta_f
1657 1664 if (flag == LFR_SUCCESSFUL)
1658 1665 {
1659 1666 if ( sy_lfr_sc_rw_delta_f < MIN_SY_LFR_SC_RW_DELTA_F )
1660 1667 {
1661 1668 parPtr = (char*) &sy_lfr_pas_filter_shift;
1662 1669 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + DATAFIELD_OFFSET, sy_lfr_sc_rw_delta_f );
1663 1670 flag = WRONG_APP_DATA;
1664 1671 }
1665 1672 }
1666 1673
1667 1674 //************
1668 1675 // sy_lfr_rw_k
1669 1676 if (flag == LFR_SUCCESSFUL)
1670 1677 {
1671 1678 flag = check_all_sy_lfr_rw_k( TC, &datafield_pos, &rw_k );
1672 1679 if (flag != LFR_SUCCESSFUL)
1673 1680 {
1674 1681 parPtr = (char*) &sy_lfr_pas_filter_shift;
1675 1682 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, datafield_pos + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1676 1683 }
1677 1684 }
1678 1685
1679 1686 return flag;
1680 1687 }
1681 1688
1682 1689 //**************
1683 1690 // KCOEFFICIENTS
1684 1691 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1685 1692 {
1686 1693 unsigned int kcoeff;
1687 1694 unsigned short sy_lfr_kcoeff_frequency;
1688 1695 unsigned short bin;
1689 1696 float *kcoeffPtr_norm;
1690 1697 float *kcoeffPtr_sbm;
1691 1698 int status;
1692 1699 unsigned char *kcoeffLoadPtr;
1693 1700 unsigned char *kcoeffNormPtr;
1694 1701 unsigned char *kcoeffSbmPtr_a;
1695 1702 unsigned char *kcoeffSbmPtr_b;
1696 1703
1697 1704 sy_lfr_kcoeff_frequency = 0;
1698 1705 bin = 0;
1699 1706 kcoeffPtr_norm = NULL;
1700 1707 kcoeffPtr_sbm = NULL;
1701 1708 status = LFR_SUCCESSFUL;
1702 1709
1703 1710 // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
1704 1711 copyInt16ByChar( (unsigned char*) &sy_lfr_kcoeff_frequency, &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY] );
1705 1712
1706 1713
1707 1714 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1708 1715 {
1709 1716 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1710 1717 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET + 1,
1711 1718 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1712 1719 status = LFR_DEFAULT;
1713 1720 }
1714 1721 else
1715 1722 {
1716 1723 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1717 1724 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1718 1725 {
1719 1726 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1720 1727 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1721 1728 bin = sy_lfr_kcoeff_frequency;
1722 1729 }
1723 1730 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1724 1731 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1725 1732 {
1726 1733 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1727 1734 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1728 1735 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1729 1736 }
1730 1737 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1731 1738 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1732 1739 {
1733 1740 kcoeffPtr_norm = k_coeff_intercalib_f2;
1734 1741 kcoeffPtr_sbm = NULL;
1735 1742 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1736 1743 }
1737 1744 }
1738 1745
1739 1746 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1740 1747 {
1741 1748 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1742 1749 {
1743 1750 // destination
1744 1751 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1745 1752 // source
1746 1753 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1747 1754 // copy source to destination
1748 1755 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1749 1756 }
1750 1757 }
1751 1758
1752 1759 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1753 1760 {
1754 1761 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1755 1762 {
1756 1763 // destination
1757 1764 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1758 1765 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1759 1766 // source
1760 1767 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1761 1768 // copy source to destination
1762 1769 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1763 1770 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1764 1771 }
1765 1772 }
1766 1773
1767 1774 // print_k_coeff();
1768 1775
1769 1776 return status;
1770 1777 }
1771 1778
1772 1779 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1773 1780 {
1774 1781 destination[BYTE_0] = source[BYTE_0];
1775 1782 destination[BYTE_1] = source[BYTE_1];
1776 1783 destination[BYTE_2] = source[BYTE_2];
1777 1784 destination[BYTE_3] = source[BYTE_3];
1778 1785 }
1779 1786
1780 1787 void copyInt32ByChar( unsigned char *destination, unsigned char *source )
1781 1788 {
1782 1789 destination[BYTE_0] = source[BYTE_0];
1783 1790 destination[BYTE_1] = source[BYTE_1];
1784 1791 destination[BYTE_2] = source[BYTE_2];
1785 1792 destination[BYTE_3] = source[BYTE_3];
1786 1793 }
1787 1794
1788 1795 void copyInt16ByChar( unsigned char *destination, unsigned char *source )
1789 1796 {
1790 1797 destination[BYTE_0] = source[BYTE_0];
1791 1798 destination[BYTE_1] = source[BYTE_1];
1792 1799 }
1793 1800
1794 1801 void floatToChar( float value, unsigned char* ptr)
1795 1802 {
1796 1803 unsigned char* valuePtr;
1797 1804
1798 1805 valuePtr = (unsigned char*) &value;
1799 1806
1800 1807 ptr[BYTE_0] = valuePtr[BYTE_0];
1801 1808 ptr[BYTE_1] = valuePtr[BYTE_1];
1802 1809 ptr[BYTE_2] = valuePtr[BYTE_2];
1803 1810 ptr[BYTE_3] = valuePtr[BYTE_3];
1804 1811 }
1805 1812
1806 1813 //**********
1807 1814 // init dump
1808 1815
1809 1816 void init_parameter_dump( void )
1810 1817 {
1811 1818 /** This function initialize the parameter_dump_packet global variable with default values.
1812 1819 *
1813 1820 */
1814 1821
1815 1822 unsigned int k;
1816 1823
1817 1824 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1818 1825 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1819 1826 parameter_dump_packet.reserved = CCSDS_RESERVED;
1820 1827 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1821 1828 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1822 1829 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1823 1830 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1824 1831 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1825 1832 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1826 1833 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1827 1834 // DATA FIELD HEADER
1828 1835 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1829 1836 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1830 1837 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1831 1838 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1832 1839 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1833 1840 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1834 1841 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1835 1842 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1836 1843 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1837 1844 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1838 1845 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1839 1846
1840 1847 //******************
1841 1848 // COMMON PARAMETERS
1842 1849 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1843 1850 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1844 1851
1845 1852 //******************
1846 1853 // NORMAL PARAMETERS
1847 1854 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1848 1855 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1849 1856 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1850 1857 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1851 1858 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1852 1859 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1853 1860 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1854 1861 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1855 1862 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1856 1863
1857 1864 //*****************
1858 1865 // BURST PARAMETERS
1859 1866 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1860 1867 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1861 1868
1862 1869 //****************
1863 1870 // SBM1 PARAMETERS
1864 1871 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
1865 1872 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1866 1873
1867 1874 //****************
1868 1875 // SBM2 PARAMETERS
1869 1876 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1870 1877 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1871 1878
1872 1879 //************
1873 1880 // FBINS MASKS
1874 1881 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1875 1882 {
1876 1883 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
1877 1884 }
1878 1885
1879 1886 // PAS FILTER PARAMETERS
1880 1887 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1881 1888 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1882 1889 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1883 1890 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1884 1891 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1885 1892 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1886 1893 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1887 1894
1888 1895 // RW1_K
1889 1896 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
1890 1897 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
1891 1898 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
1892 1899 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
1893 1900 // RW2_K
1894 1901 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
1895 1902 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
1896 1903 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
1897 1904 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
1898 1905 // RW3_K
1899 1906 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
1900 1907 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
1901 1908 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
1902 1909 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
1903 1910 // RW4_K
1904 1911 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
1905 1912 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
1906 1913 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
1907 1914 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
1908 1915
1909 1916 // LFR_RW_MASK
1910 1917 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1911 1918 {
1912 1919 parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
1913 1920 }
1914 1921
1915 1922 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1916 1923 merge_fbins_masks();
1917 1924 }
1918 1925
1919 1926 void init_kcoefficients_dump( void )
1920 1927 {
1921 1928 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1922 1929 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1923 1930
1924 1931 kcoefficient_node_1.previous = NULL;
1925 1932 kcoefficient_node_1.next = NULL;
1926 1933 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1927 1934 kcoefficient_node_1.coarseTime = INIT_CHAR;
1928 1935 kcoefficient_node_1.fineTime = INIT_CHAR;
1929 1936 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1930 1937 kcoefficient_node_1.status = INIT_CHAR;
1931 1938
1932 1939 kcoefficient_node_2.previous = NULL;
1933 1940 kcoefficient_node_2.next = NULL;
1934 1941 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1935 1942 kcoefficient_node_2.coarseTime = INIT_CHAR;
1936 1943 kcoefficient_node_2.fineTime = INIT_CHAR;
1937 1944 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1938 1945 kcoefficient_node_2.status = INIT_CHAR;
1939 1946 }
1940 1947
1941 1948 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1942 1949 {
1943 1950 unsigned int k;
1944 1951 unsigned int packetLength;
1945 1952
1946 1953 packetLength =
1947 1954 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1948 1955
1949 1956 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1950 1957 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1951 1958 kcoefficients_dump->reserved = CCSDS_RESERVED;
1952 1959 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1953 1960 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1954 1961 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1955 1962 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1956 1963 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1957 1964 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1958 1965 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1959 1966 // DATA FIELD HEADER
1960 1967 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1961 1968 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1962 1969 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1963 1970 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1964 1971 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1965 1972 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1966 1973 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1967 1974 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1968 1975 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1969 1976 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1970 1977 kcoefficients_dump->sid = SID_K_DUMP;
1971 1978
1972 1979 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1973 1980 kcoefficients_dump->pkt_nr = PKTNR_1;
1974 1981 kcoefficients_dump->blk_nr = blk_nr;
1975 1982
1976 1983 //******************
1977 1984 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1978 1985 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1979 1986 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1980 1987 {
1981 1988 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1982 1989 }
1983 1990 }
1984 1991
1985 1992 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1986 1993 {
1987 1994 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1988 1995 *
1989 1996 * @param packet_sequence_control points to the packet sequence control which will be incremented
1990 1997 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1991 1998 *
1992 1999 * If the destination ID is not known, a dedicated counter is incremented.
1993 2000 *
1994 2001 */
1995 2002
1996 2003 unsigned short sequence_cnt;
1997 2004 unsigned short segmentation_grouping_flag;
1998 2005 unsigned short new_packet_sequence_control;
1999 2006 unsigned char i;
2000 2007
2001 2008 switch (destination_id)
2002 2009 {
2003 2010 case SID_TC_GROUND:
2004 2011 i = GROUND;
2005 2012 break;
2006 2013 case SID_TC_MISSION_TIMELINE:
2007 2014 i = MISSION_TIMELINE;
2008 2015 break;
2009 2016 case SID_TC_TC_SEQUENCES:
2010 2017 i = TC_SEQUENCES;
2011 2018 break;
2012 2019 case SID_TC_RECOVERY_ACTION_CMD:
2013 2020 i = RECOVERY_ACTION_CMD;
2014 2021 break;
2015 2022 case SID_TC_BACKUP_MISSION_TIMELINE:
2016 2023 i = BACKUP_MISSION_TIMELINE;
2017 2024 break;
2018 2025 case SID_TC_DIRECT_CMD:
2019 2026 i = DIRECT_CMD;
2020 2027 break;
2021 2028 case SID_TC_SPARE_GRD_SRC1:
2022 2029 i = SPARE_GRD_SRC1;
2023 2030 break;
2024 2031 case SID_TC_SPARE_GRD_SRC2:
2025 2032 i = SPARE_GRD_SRC2;
2026 2033 break;
2027 2034 case SID_TC_OBCP:
2028 2035 i = OBCP;
2029 2036 break;
2030 2037 case SID_TC_SYSTEM_CONTROL:
2031 2038 i = SYSTEM_CONTROL;
2032 2039 break;
2033 2040 case SID_TC_AOCS:
2034 2041 i = AOCS;
2035 2042 break;
2036 2043 case SID_TC_RPW_INTERNAL:
2037 2044 i = RPW_INTERNAL;
2038 2045 break;
2039 2046 default:
2040 2047 i = GROUND;
2041 2048 break;
2042 2049 }
2043 2050
2044 2051 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
2045 2052 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
2046 2053
2047 2054 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
2048 2055
2049 2056 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
2050 2057 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
2051 2058
2052 2059 // increment the sequence counter
2053 2060 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
2054 2061 {
2055 2062 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
2056 2063 }
2057 2064 else
2058 2065 {
2059 2066 sequenceCounters_TM_DUMP[ i ] = 0;
2060 2067 }
2061 2068 }
General Comments 0
You need to be logged in to leave comments. Login now