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

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

Bug 649 corrected, TC with Length > 228 are dropped by the driver, not parsed
paul -
r316:f8bdd7f20886 R3_plus draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 57edc38eadba4601cf0b1e2fa1eeab85082e9f41 header/lfr_common_headers
2 3e4216a0e6981bead8bcb201012ebadb53f60dff header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,1599 +1,1609
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 unsigned char previousTimecodeCtr = 0;
26 26 unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
27 27
28 28 //***********
29 29 // RTEMS TASK
30 30 rtems_task spiq_task(rtems_task_argument unused)
31 31 {
32 32 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
33 33 *
34 34 * @param unused is the starting argument of the RTEMS task
35 35 *
36 36 */
37 37
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 int linkStatus;
41 41
42 42 BOOT_PRINTF("in SPIQ *** \n")
43 43
44 44 while(true){
45 45 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
46 46 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
47 47
48 48 // [0] SUSPEND RECV AND SEND TASKS
49 49 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
50 50 if ( status != RTEMS_SUCCESSFUL ) {
51 51 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
52 52 }
53 53 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
54 54 if ( status != RTEMS_SUCCESSFUL ) {
55 55 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
56 56 }
57 57
58 58 // [1] CHECK THE LINK
59 59 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
60 60 if ( linkStatus != 5) {
61 61 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
62 62 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
63 63 }
64 64
65 65 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
66 66 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
67 67 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
68 68 {
69 69 spacewire_read_statistics();
70 70 status = spacewire_several_connect_attemps( );
71 71 }
72 72 else // [2.b] in run state, start the link
73 73 {
74 74 status = spacewire_stop_and_start_link( fdSPW ); // start the link
75 75 if ( status != RTEMS_SUCCESSFUL)
76 76 {
77 77 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
78 78 }
79 79 }
80 80
81 81 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
82 82 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
83 83 {
84 84 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
85 85 if ( status != RTEMS_SUCCESSFUL ) {
86 86 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
87 87 }
88 88 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
89 89 if ( status != RTEMS_SUCCESSFUL ) {
90 90 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
91 91 }
92 92 }
93 93 else // [3.b] the link is not in run state, go in STANDBY mode
94 94 {
95 95 status = enter_mode_standby();
96 96 if ( status != RTEMS_SUCCESSFUL )
97 97 {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 {
101 101 updateLFRCurrentMode( LFR_MODE_STANDBY );
102 102 }
103 103 // wake the LINK task up to wait for the link recovery
104 104 status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 );
105 105 status = rtems_task_suspend( RTEMS_SELF );
106 106 }
107 107 }
108 108 }
109 109
110 110 rtems_task recv_task( rtems_task_argument unused )
111 111 {
112 112 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
113 113 *
114 114 * @param unused is the starting argument of the RTEMS task
115 115 *
116 116 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
117 117 * 1. It reads the incoming data.
118 118 * 2. Launches the acceptance procedure.
119 119 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
120 120 *
121 121 */
122 122
123 123 int len;
124 124 ccsdsTelecommandPacket_t currentTC;
125 125 unsigned char computed_CRC[ 2 ];
126 126 unsigned char currentTC_LEN_RCV[ 2 ];
127 127 unsigned char destinationID;
128 128 unsigned int estimatedPacketLength;
129 129 unsigned int parserCode;
130 130 rtems_status_code status;
131 131 rtems_id queue_recv_id;
132 132 rtems_id queue_send_id;
133 133
134 134 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
135 135
136 136 status = get_message_queue_id_recv( &queue_recv_id );
137 137 if (status != RTEMS_SUCCESSFUL)
138 138 {
139 139 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
140 140 }
141 141
142 142 status = get_message_queue_id_send( &queue_send_id );
143 143 if (status != RTEMS_SUCCESSFUL)
144 144 {
145 145 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
146 146 }
147 147
148 148 BOOT_PRINTF("in RECV *** \n")
149 149
150 150 while(1)
151 151 {
152 152 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
153 153 if (len == -1){ // error during the read call
154 154 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
155 155 }
156 156 else {
157 157 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
158 158 PRINTF("in RECV *** packet lenght too short\n")
159 159 }
160 160 else {
161 // PRINTF1("incoming TC with len: %d\n", len);
162 161 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
162 PRINTF1("incoming TC with Length (byte): %d\n", len - 3);
163 163 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
164 164 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
165 165 // CHECK THE TC
166 166 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
167 167 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
168 168 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
169 169 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
170 170 || (parserCode == WRONG_SRC_ID) )
171 171 { // send TM_LFR_TC_EXE_CORRUPTED
172 172 PRINTF1("TC corrupted received, with code: %d\n", parserCode);
173 173 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
174 174 &&
175 175 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
176 176 )
177 177 {
178 178 if ( parserCode == WRONG_SRC_ID )
179 179 {
180 180 destinationID = SID_TC_GROUND;
181 181 }
182 182 else
183 183 {
184 184 destinationID = currentTC.sourceID;
185 185 }
186 186 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
187 187 computed_CRC, currentTC_LEN_RCV,
188 188 destinationID );
189 189 }
190 190 }
191 191 else
192 192 { // send valid TC to the action launcher
193 193 status = rtems_message_queue_send( queue_recv_id, &currentTC,
194 194 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
195 195 }
196 196 }
197 197 }
198 198
199 199 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
200 200
201 201 }
202 202 }
203 203
204 204 rtems_task send_task( rtems_task_argument argument)
205 205 {
206 206 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
207 207 *
208 208 * @param unused is the starting argument of the RTEMS task
209 209 *
210 210 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
211 211 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
212 212 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
213 213 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
214 214 * data it contains.
215 215 *
216 216 */
217 217
218 218 rtems_status_code status; // RTEMS status code
219 219 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
220 220 ring_node *incomingRingNodePtr;
221 221 int ring_node_address;
222 222 char *charPtr;
223 223 spw_ioctl_pkt_send *spw_ioctl_send;
224 224 size_t size; // size of the incoming TC packet
225 225 rtems_id queue_send_id;
226 226 unsigned int sid;
227 227 unsigned char sidAsUnsignedChar;
228 228 unsigned char type;
229 229
230 230 incomingRingNodePtr = NULL;
231 231 ring_node_address = 0;
232 232 charPtr = (char *) &ring_node_address;
233 233 sid = 0;
234 234 sidAsUnsignedChar = 0;
235 235
236 236 init_header_cwf( &headerCWF );
237 237 init_header_swf( &headerSWF );
238 238 init_header_asm( &headerASM );
239 239
240 240 status = get_message_queue_id_send( &queue_send_id );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245
246 246 BOOT_PRINTF("in SEND *** \n")
247 247
248 248 while(1)
249 249 {
250 250 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
251 251 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
252 252
253 253 if (status!=RTEMS_SUCCESSFUL)
254 254 {
255 255 PRINTF1("in SEND *** (1) ERR = %d\n", status)
256 256 }
257 257 else
258 258 {
259 259 if ( size == sizeof(ring_node*) )
260 260 {
261 261 charPtr[0] = incomingData[0];
262 262 charPtr[1] = incomingData[1];
263 263 charPtr[2] = incomingData[2];
264 264 charPtr[3] = incomingData[3];
265 265 incomingRingNodePtr = (ring_node*) ring_node_address;
266 266 sid = incomingRingNodePtr->sid;
267 267 if ( (sid==SID_NORM_CWF_LONG_F3)
268 268 || (sid==SID_BURST_CWF_F2 )
269 269 || (sid==SID_SBM1_CWF_F1 )
270 270 || (sid==SID_SBM2_CWF_F2 ))
271 271 {
272 272 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
273 273 }
274 274 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
275 275 {
276 276 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
277 277 }
278 278 else if ( (sid==SID_NORM_CWF_F3) )
279 279 {
280 280 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
281 281 }
282 282 else if (sid==SID_NORM_ASM_F0)
283 283 {
284 284 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
285 285 }
286 286 else if (sid==SID_NORM_ASM_F1)
287 287 {
288 288 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
289 289 }
290 290 else if (sid==SID_NORM_ASM_F2)
291 291 {
292 292 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
293 293 }
294 294 else if ( sid==TM_CODE_K_DUMP )
295 295 {
296 296 spw_send_k_dump( incomingRingNodePtr );
297 297 }
298 298 else
299 299 {
300 300 PRINTF1("unexpected sid = %d\n", sid);
301 301 }
302 302 }
303 303 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
304 304 {
305 305 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
306 306 sid = sidAsUnsignedChar;
307 307 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
308 308 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
309 309 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
310 310 {
311 311 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
312 312 }
313 313
314 314 status = write( fdSPW, incomingData, size );
315 315 if (status == -1){
316 316 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
317 317 }
318 318 }
319 319 else // the incoming message is a spw_ioctl_pkt_send structure
320 320 {
321 321 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
322 322 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
323 323 if (status == -1){
324 324 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
325 325 }
326 326 }
327 327 }
328 328
329 329 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
330 330
331 331 }
332 332 }
333 333
334 334 rtems_task link_task( rtems_task_argument argument )
335 335 {
336 336 rtems_event_set event_out;
337 337 rtems_status_code status;
338 338 int linkStatus;
339 339
340 340 BOOT_PRINTF("in LINK ***\n")
341 341
342 342 while(1)
343 343 {
344 344 // wait for an RTEMS_EVENT
345 345 rtems_event_receive( RTEMS_EVENT_0,
346 346 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
347 347 PRINTF("in LINK *** wait for the link\n")
348 348 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
349 349 while( linkStatus != 5) // wait for the link
350 350 {
351 351 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
352 352 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
353 353 watchdog_reload();
354 354 }
355 355
356 356 spacewire_read_statistics();
357 357 status = spacewire_stop_and_start_link( fdSPW );
358 358
359 359 if (status != RTEMS_SUCCESSFUL)
360 360 {
361 361 PRINTF1("in LINK *** ERR link not started %d\n", status)
362 362 }
363 363 else
364 364 {
365 365 PRINTF("in LINK *** OK link started\n")
366 366 }
367 367
368 368 // restart the SPIQ task
369 369 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
370 370 if ( status != RTEMS_SUCCESSFUL ) {
371 371 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
372 372 }
373 373
374 374 // restart RECV and SEND
375 375 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
376 376 if ( status != RTEMS_SUCCESSFUL ) {
377 377 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
378 378 }
379 379 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
380 380 if ( status != RTEMS_SUCCESSFUL ) {
381 381 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
382 382 }
383 383 }
384 384 }
385 385
386 386 //****************
387 387 // OTHER FUNCTIONS
388 388 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
389 389 {
390 390 /** This function opens the SpaceWire link.
391 391 *
392 392 * @return a valid file descriptor in case of success, -1 in case of a failure
393 393 *
394 394 */
395 395 rtems_status_code status;
396 396
397 397 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
398 398 if ( fdSPW < 0 ) {
399 399 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
400 400 }
401 401 else
402 402 {
403 403 status = RTEMS_SUCCESSFUL;
404 404 }
405 405
406 406 return status;
407 407 }
408 408
409 409 int spacewire_start_link( int fd )
410 410 {
411 411 rtems_status_code status;
412 412
413 413 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
414 414 // -1 default hardcoded driver timeout
415 415
416 416 return status;
417 417 }
418 418
419 419 int spacewire_stop_and_start_link( int fd )
420 420 {
421 421 rtems_status_code status;
422 422
423 423 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
424 424 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
425 425 // -1 default hardcoded driver timeout
426 426
427 427 return status;
428 428 }
429 429
430 430 int spacewire_configure_link( int fd )
431 431 {
432 432 /** This function configures the SpaceWire link.
433 433 *
434 434 * @return GR-RTEMS-DRIVER directive status codes:
435 435 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
436 436 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
437 437 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
438 438 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
439 439 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
440 440 * - 5 EIO - Error when writing to grswp hardware registers.
441 441 * - 2 ENOENT - No such file or directory
442 442 */
443 443
444 444 rtems_status_code status;
445 445
446 446 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
447 447 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
448 spw_ioctl_packetsize packetsize;
449
450 packetsize.rxsize = 228;
451 packetsize.txdsize = 4096;
452 packetsize.txhsize = 34;
448 453
449 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
450 455 if (status!=RTEMS_SUCCESSFUL) {
451 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
452 457 }
453 458 //
454 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
455 460 if (status!=RTEMS_SUCCESSFUL) {
456 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
457 462 }
458 463 //
459 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
460 465 if (status!=RTEMS_SUCCESSFUL) {
461 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
462 467 }
463 468 //
464 469 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
465 470 if (status!=RTEMS_SUCCESSFUL) {
466 471 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
467 472 }
468 473 //
469 474 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
470 475 if (status!=RTEMS_SUCCESSFUL) {
471 476 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
472 477 }
473 478 //
474 479 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
475 480 if (status!=RTEMS_SUCCESSFUL) {
476 481 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
477 482 }
478 483 //
479 484 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
480 485 if (status!=RTEMS_SUCCESSFUL) {
481 486 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
482 487 }
488 //
489 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_PACKETSIZE, packetsize); // set rxsize, txdsize and txhsize
490 if (status!=RTEMS_SUCCESSFUL) {
491 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_PACKETSIZE,\n")
492 }
483 493
484 494 return status;
485 495 }
486 496
487 497 int spacewire_several_connect_attemps( void )
488 498 {
489 499 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
490 500 *
491 501 * @return RTEMS directive status code:
492 502 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
493 503 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
494 504 *
495 505 */
496 506
497 507 rtems_status_code status_spw;
498 508 rtems_status_code status;
499 509 int i;
500 510
501 511 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
502 512 {
503 513 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
504 514
505 515 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
506 516
507 517 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
508 518
509 519 status_spw = spacewire_stop_and_start_link( fdSPW );
510 520
511 521 if ( status_spw != RTEMS_SUCCESSFUL )
512 522 {
513 523 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
514 524 }
515 525
516 526 if ( status_spw == RTEMS_SUCCESSFUL)
517 527 {
518 528 break;
519 529 }
520 530 }
521 531
522 532 return status_spw;
523 533 }
524 534
525 535 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
526 536 {
527 537 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
528 538 *
529 539 * @param val is the value, 0 or 1, used to set the value of the NP bit.
530 540 * @param regAddr is the address of the GRSPW control register.
531 541 *
532 542 * NP is the bit 20 of the GRSPW control register.
533 543 *
534 544 */
535 545
536 546 unsigned int *spwptr = (unsigned int*) regAddr;
537 547
538 548 if (val == 1) {
539 549 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
540 550 }
541 551 if (val== 0) {
542 552 *spwptr = *spwptr & 0xffdfffff;
543 553 }
544 554 }
545 555
546 556 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
547 557 {
548 558 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
549 559 *
550 560 * @param val is the value, 0 or 1, used to set the value of the RE bit.
551 561 * @param regAddr is the address of the GRSPW control register.
552 562 *
553 563 * RE is the bit 16 of the GRSPW control register.
554 564 *
555 565 */
556 566
557 567 unsigned int *spwptr = (unsigned int*) regAddr;
558 568
559 569 if (val == 1)
560 570 {
561 571 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
562 572 }
563 573 if (val== 0)
564 574 {
565 575 *spwptr = *spwptr & 0xfffdffff;
566 576 }
567 577 }
568 578
569 579 void spacewire_read_statistics( void )
570 580 {
571 581 /** This function reads the SpaceWire statistics from the grspw RTEMS driver.
572 582 *
573 583 * @param void
574 584 *
575 585 * @return void
576 586 *
577 587 * Once they are read, the counters are stored in a global variable used during the building of the
578 588 * HK packets.
579 589 *
580 590 */
581 591
582 592 rtems_status_code status;
583 593 spw_stats current;
584 594
585 595 spacewire_get_last_error();
586 596
587 597 // read the current statistics
588 598 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
589 599
590 600 // clear the counters
591 601 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS );
592 602
593 603 // typedef struct {
594 604 // unsigned int tx_link_err; // NOT IN HK
595 605 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
596 606 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
597 607 // unsigned int rx_eep_err;
598 608 // unsigned int rx_truncated;
599 609 // unsigned int parity_err;
600 610 // unsigned int escape_err;
601 611 // unsigned int credit_err;
602 612 // unsigned int write_sync_err;
603 613 // unsigned int disconnect_err;
604 614 // unsigned int early_ep;
605 615 // unsigned int invalid_address;
606 616 // unsigned int packets_sent;
607 617 // unsigned int packets_received;
608 618 // } spw_stats;
609 619
610 620 // rx_eep_err
611 621 grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err;
612 622 // rx_truncated
613 623 grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated;
614 624 // parity_err
615 625 grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err;
616 626 // escape_err
617 627 grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err;
618 628 // credit_err
619 629 grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err;
620 630 // write_sync_err
621 631 grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err;
622 632 // disconnect_err
623 633 grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err;
624 634 // early_ep
625 635 grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep;
626 636 // invalid_address
627 637 grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address;
628 638 // packets_sent
629 639 grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent;
630 640 // packets_received
631 641 grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received;
632 642
633 643 }
634 644
635 645 void spacewire_get_last_error( void )
636 646 {
637 647 static spw_stats previous;
638 648 spw_stats current;
639 649 rtems_status_code status;
640 650
641 651 unsigned int hk_lfr_last_er_rid;
642 652 unsigned char hk_lfr_last_er_code;
643 653 int coarseTime;
644 654 int fineTime;
645 655 unsigned char update_hk_lfr_last_er;
646 656
647 657 update_hk_lfr_last_er = 0;
648 658
649 659 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
650 660
651 661 // get current time
652 662 coarseTime = time_management_regs->coarse_time;
653 663 fineTime = time_management_regs->fine_time;
654 664
655 665 // typedef struct {
656 666 // unsigned int tx_link_err; // NOT IN HK
657 667 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
658 668 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
659 669 // unsigned int rx_eep_err;
660 670 // unsigned int rx_truncated;
661 671 // unsigned int parity_err;
662 672 // unsigned int escape_err;
663 673 // unsigned int credit_err;
664 674 // unsigned int write_sync_err;
665 675 // unsigned int disconnect_err;
666 676 // unsigned int early_ep;
667 677 // unsigned int invalid_address;
668 678 // unsigned int packets_sent;
669 679 // unsigned int packets_received;
670 680 // } spw_stats;
671 681
672 682 // tx_link_err *** no code associated to this field
673 683 // rx_rmap_header_crc_err *** LE *** in HK
674 684 if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err)
675 685 {
676 686 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
677 687 hk_lfr_last_er_code = CODE_HEADER_CRC;
678 688 update_hk_lfr_last_er = 1;
679 689 }
680 690 // rx_rmap_data_crc_err *** LE *** NOT IN HK
681 691 if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err)
682 692 {
683 693 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
684 694 hk_lfr_last_er_code = CODE_DATA_CRC;
685 695 update_hk_lfr_last_er = 1;
686 696 }
687 697 // rx_eep_err
688 698 if (previous.rx_eep_err != current.rx_eep_err)
689 699 {
690 700 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
691 701 hk_lfr_last_er_code = CODE_EEP;
692 702 update_hk_lfr_last_er = 1;
693 703 }
694 704 // rx_truncated
695 705 if (previous.rx_truncated != current.rx_truncated)
696 706 {
697 707 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
698 708 hk_lfr_last_er_code = CODE_RX_TOO_BIG;
699 709 update_hk_lfr_last_er = 1;
700 710 }
701 711 // parity_err
702 712 if (previous.parity_err != current.parity_err)
703 713 {
704 714 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
705 715 hk_lfr_last_er_code = CODE_PARITY;
706 716 update_hk_lfr_last_er = 1;
707 717 }
708 718 // escape_err
709 719 if (previous.parity_err != current.parity_err)
710 720 {
711 721 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
712 722 hk_lfr_last_er_code = CODE_ESCAPE;
713 723 update_hk_lfr_last_er = 1;
714 724 }
715 725 // credit_err
716 726 if (previous.credit_err != current.credit_err)
717 727 {
718 728 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
719 729 hk_lfr_last_er_code = CODE_CREDIT;
720 730 update_hk_lfr_last_er = 1;
721 731 }
722 732 // write_sync_err
723 733 if (previous.write_sync_err != current.write_sync_err)
724 734 {
725 735 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
726 736 hk_lfr_last_er_code = CODE_WRITE_SYNC;
727 737 update_hk_lfr_last_er = 1;
728 738 }
729 739 // disconnect_err
730 740 if (previous.disconnect_err != current.disconnect_err)
731 741 {
732 742 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
733 743 hk_lfr_last_er_code = CODE_DISCONNECT;
734 744 update_hk_lfr_last_er = 1;
735 745 }
736 746 // early_ep
737 747 if (previous.early_ep != current.early_ep)
738 748 {
739 749 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
740 750 hk_lfr_last_er_code = CODE_EARLY_EOP_EEP;
741 751 update_hk_lfr_last_er = 1;
742 752 }
743 753 // invalid_address
744 754 if (previous.invalid_address != current.invalid_address)
745 755 {
746 756 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
747 757 hk_lfr_last_er_code = CODE_INVALID_ADDRESS;
748 758 update_hk_lfr_last_er = 1;
749 759 }
750 760
751 761 // if a field has changed, update the hk_last_er fields
752 762 if (update_hk_lfr_last_er == 1)
753 763 {
754 764 update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code );
755 765 }
756 766
757 767 previous = current;
758 768 }
759 769
760 770 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code)
761 771 {
762 772 unsigned char *coarseTimePtr;
763 773 unsigned char *fineTimePtr;
764 774
765 775 coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time;
766 776 fineTimePtr = (unsigned char*) &time_management_regs->fine_time;
767 777
768 778 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & 0xff00) >> 8 );
769 779 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & 0x00ff);
770 780 housekeeping_packet.hk_lfr_last_er_code = code;
771 781 housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0];
772 782 housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1];
773 783 housekeeping_packet.hk_lfr_last_er_time[2] = coarseTimePtr[2];
774 784 housekeeping_packet.hk_lfr_last_er_time[3] = coarseTimePtr[3];
775 785 housekeeping_packet.hk_lfr_last_er_time[4] = fineTimePtr[2];
776 786 housekeeping_packet.hk_lfr_last_er_time[5] = fineTimePtr[3];
777 787 }
778 788
779 789 void update_hk_with_grspw_stats( void )
780 790 {
781 791 //****************************
782 792 // DPU_SPACEWIRE_IF_STATISTICS
783 793 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> 8);
784 794 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received);
785 795 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> 8);
786 796 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent);
787 797
788 798 //******************************************
789 799 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
790 800 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err;
791 801 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err;
792 802 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err;
793 803 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err;
794 804 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err;
795 805
796 806 //*********************************************
797 807 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
798 808 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep;
799 809 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address;
800 810 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err;
801 811 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated;
802 812 }
803 813
804 814 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 )
805 815 {
806 816 unsigned int *statusRegisterPtr;
807 817 unsigned char linkState;
808 818
809 819 statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER);
810 820 linkState = (unsigned char) ( ( (*statusRegisterPtr) >> 21) & 0x07); // [0000 0111]
811 821
812 822 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & 0xf8; // [1111 1000] set link state to 0
813 823
814 824 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state
815 825 }
816 826
817 827 void increase_unsigned_char_counter( unsigned char *counter )
818 828 {
819 829 // update the number of valid timecodes that have been received
820 830 if (*counter == 255)
821 831 {
822 832 *counter = 0;
823 833 }
824 834 else
825 835 {
826 836 *counter = *counter + 1;
827 837 }
828 838 }
829 839
830 840 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
831 841 {
832 842 /** This function checks the coherency between the incoming timecode and the last valid timecode.
833 843 *
834 844 * @param currentTimecodeCtr is the incoming timecode
835 845 *
836 846 * @return returned codes::
837 847 * - LFR_DEFAULT
838 848 * - LFR_SUCCESSFUL
839 849 *
840 850 */
841 851
842 852 static unsigned char firstTickout = 1;
843 853 unsigned char ret;
844 854
845 855 ret = LFR_DEFAULT;
846 856
847 857 if (firstTickout == 0)
848 858 {
849 859 if (currentTimecodeCtr == 0)
850 860 {
851 861 if (previousTimecodeCtr == 63)
852 862 {
853 863 ret = LFR_SUCCESSFUL;
854 864 }
855 865 else
856 866 {
857 867 ret = LFR_DEFAULT;
858 868 }
859 869 }
860 870 else
861 871 {
862 872 if (currentTimecodeCtr == (previousTimecodeCtr +1))
863 873 {
864 874 ret = LFR_SUCCESSFUL;
865 875 }
866 876 else
867 877 {
868 878 ret = LFR_DEFAULT;
869 879 }
870 880 }
871 881 }
872 882 else
873 883 {
874 884 firstTickout = 0;
875 885 ret = LFR_SUCCESSFUL;
876 886 }
877 887
878 888 return ret;
879 889 }
880 890
881 891 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
882 892 {
883 893 unsigned int ret;
884 894
885 895 ret = LFR_DEFAULT;
886 896
887 897 if (timecode == internalTime)
888 898 {
889 899 ret = LFR_SUCCESSFUL;
890 900 }
891 901 else
892 902 {
893 903 ret = LFR_DEFAULT;
894 904 }
895 905
896 906 return ret;
897 907 }
898 908
899 909 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
900 910 {
901 911 // a tickout has been emitted, perform actions on the incoming timecode
902 912
903 913 unsigned char incomingTimecode;
904 914 unsigned char updateTime;
905 915 unsigned char internalTime;
906 916 rtems_status_code status;
907 917
908 918 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
909 919 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
910 920 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
911 921
912 922 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
913 923
914 924 // update the number of tickout that have been generated
915 925 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
916 926
917 927 //**************************
918 928 // HK_LFR_TIMECODE_ERRONEOUS
919 929 // MISSING and INVALID are handled by the timecode_timer_routine service routine
920 930 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
921 931 {
922 932 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
923 933 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
924 934 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS );
925 935 }
926 936
927 937 //************************
928 938 // HK_LFR_TIME_TIMECODE_IT
929 939 // check the coherency between the SpaceWire timecode and the Internal Time
930 940 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
931 941 {
932 942 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
933 943 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT );
934 944 }
935 945
936 946 //********************
937 947 // HK_LFR_TIMECODE_CTR
938 948 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
939 949 if (oneTcLfrUpdateTimeReceived == 1)
940 950 {
941 951 if ( incomingTimecode != updateTime )
942 952 {
943 953 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
944 954 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR );
945 955 }
946 956 }
947 957
948 958 // launch the timecode timer to detect missing or invalid timecodes
949 959 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
950 960 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
951 961 if (status != RTEMS_SUCCESSFUL)
952 962 {
953 963 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 );
954 964 }
955 965 }
956 966
957 967 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
958 968 {
959 969 static unsigned char initStep = 1;
960 970
961 971 unsigned char currentTimecodeCtr;
962 972
963 973 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
964 974
965 975 if (initStep == 1)
966 976 {
967 977 if (currentTimecodeCtr == previousTimecodeCtr)
968 978 {
969 979 //************************
970 980 // HK_LFR_TIMECODE_MISSING
971 981 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
972 982 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
973 983 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
974 984 }
975 985 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
976 986 {
977 987 // the timecode value has changed and the value is valid, this is unexpected because
978 988 // the timer should not have fired, the timecode_irq_handler should have been raised
979 989 }
980 990 else
981 991 {
982 992 //************************
983 993 // HK_LFR_TIMECODE_INVALID
984 994 // the timecode value has changed and the value is not valid, no tickout has been generated
985 995 // this is why the timer has fired
986 996 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
987 997 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID );
988 998 }
989 999 }
990 1000 else
991 1001 {
992 1002 initStep = 1;
993 1003 //************************
994 1004 // HK_LFR_TIMECODE_MISSING
995 1005 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
996 1006 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
997 1007 }
998 1008
999 1009 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
1000 1010 }
1001 1011
1002 1012 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
1003 1013 {
1004 1014 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1005 1015 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1006 1016 header->reserved = DEFAULT_RESERVED;
1007 1017 header->userApplication = CCSDS_USER_APP;
1008 1018 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
1009 1019 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
1010 1020 header->packetLength[0] = 0x00;
1011 1021 header->packetLength[1] = 0x00;
1012 1022 // DATA FIELD HEADER
1013 1023 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1014 1024 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1015 1025 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1016 1026 header->destinationID = TM_DESTINATION_ID_GROUND;
1017 1027 header->time[0] = 0x00;
1018 1028 header->time[0] = 0x00;
1019 1029 header->time[0] = 0x00;
1020 1030 header->time[0] = 0x00;
1021 1031 header->time[0] = 0x00;
1022 1032 header->time[0] = 0x00;
1023 1033 // AUXILIARY DATA HEADER
1024 1034 header->sid = 0x00;
1025 1035 header->pa_bia_status_info = DEFAULT_HKBIA;
1026 1036 header->blkNr[0] = 0x00;
1027 1037 header->blkNr[1] = 0x00;
1028 1038 }
1029 1039
1030 1040 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
1031 1041 {
1032 1042 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1033 1043 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1034 1044 header->reserved = DEFAULT_RESERVED;
1035 1045 header->userApplication = CCSDS_USER_APP;
1036 1046 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1037 1047 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1038 1048 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1039 1049 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1040 1050 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
1041 1051 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1042 1052 // DATA FIELD HEADER
1043 1053 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1044 1054 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1045 1055 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1046 1056 header->destinationID = TM_DESTINATION_ID_GROUND;
1047 1057 header->time[0] = 0x00;
1048 1058 header->time[0] = 0x00;
1049 1059 header->time[0] = 0x00;
1050 1060 header->time[0] = 0x00;
1051 1061 header->time[0] = 0x00;
1052 1062 header->time[0] = 0x00;
1053 1063 // AUXILIARY DATA HEADER
1054 1064 header->sid = 0x00;
1055 1065 header->pa_bia_status_info = DEFAULT_HKBIA;
1056 1066 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
1057 1067 header->pktNr = 0x00;
1058 1068 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
1059 1069 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1060 1070 }
1061 1071
1062 1072 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
1063 1073 {
1064 1074 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1065 1075 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1066 1076 header->reserved = DEFAULT_RESERVED;
1067 1077 header->userApplication = CCSDS_USER_APP;
1068 1078 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1069 1079 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1070 1080 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1071 1081 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1072 1082 header->packetLength[0] = 0x00;
1073 1083 header->packetLength[1] = 0x00;
1074 1084 // DATA FIELD HEADER
1075 1085 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1076 1086 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1077 1087 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
1078 1088 header->destinationID = TM_DESTINATION_ID_GROUND;
1079 1089 header->time[0] = 0x00;
1080 1090 header->time[0] = 0x00;
1081 1091 header->time[0] = 0x00;
1082 1092 header->time[0] = 0x00;
1083 1093 header->time[0] = 0x00;
1084 1094 header->time[0] = 0x00;
1085 1095 // AUXILIARY DATA HEADER
1086 1096 header->sid = 0x00;
1087 1097 header->pa_bia_status_info = 0x00;
1088 1098 header->pa_lfr_pkt_cnt_asm = 0x00;
1089 1099 header->pa_lfr_pkt_nr_asm = 0x00;
1090 1100 header->pa_lfr_asm_blk_nr[0] = 0x00;
1091 1101 header->pa_lfr_asm_blk_nr[1] = 0x00;
1092 1102 }
1093 1103
1094 1104 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
1095 1105 Header_TM_LFR_SCIENCE_CWF_t *header )
1096 1106 {
1097 1107 /** This function sends CWF CCSDS packets (F2, F1 or F0).
1098 1108 *
1099 1109 * @param waveform points to the buffer containing the data that will be send.
1100 1110 * @param sid is the source identifier of the data that will be sent.
1101 1111 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1102 1112 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1103 1113 * contain information to setup the transmission of the data packets.
1104 1114 *
1105 1115 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1106 1116 *
1107 1117 */
1108 1118
1109 1119 unsigned int i;
1110 1120 int ret;
1111 1121 unsigned int coarseTime;
1112 1122 unsigned int fineTime;
1113 1123 rtems_status_code status;
1114 1124 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1115 1125 int *dataPtr;
1116 1126 unsigned char sid;
1117 1127
1118 1128 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1119 1129 spw_ioctl_send_CWF.options = 0;
1120 1130
1121 1131 ret = LFR_DEFAULT;
1122 1132 sid = (unsigned char) ring_node_to_send->sid;
1123 1133
1124 1134 coarseTime = ring_node_to_send->coarseTime;
1125 1135 fineTime = ring_node_to_send->fineTime;
1126 1136 dataPtr = (int*) ring_node_to_send->buffer_address;
1127 1137
1128 1138 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
1129 1139 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1130 1140 header->pa_bia_status_info = pa_bia_status_info;
1131 1141 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1132 1142 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
1133 1143 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1134 1144
1135 1145 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
1136 1146 {
1137 1147 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
1138 1148 spw_ioctl_send_CWF.hdr = (char*) header;
1139 1149 // BUILD THE DATA
1140 1150 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
1141 1151
1142 1152 // SET PACKET SEQUENCE CONTROL
1143 1153 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1144 1154
1145 1155 // SET SID
1146 1156 header->sid = sid;
1147 1157
1148 1158 // SET PACKET TIME
1149 1159 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
1150 1160 //
1151 1161 header->time[0] = header->acquisitionTime[0];
1152 1162 header->time[1] = header->acquisitionTime[1];
1153 1163 header->time[2] = header->acquisitionTime[2];
1154 1164 header->time[3] = header->acquisitionTime[3];
1155 1165 header->time[4] = header->acquisitionTime[4];
1156 1166 header->time[5] = header->acquisitionTime[5];
1157 1167
1158 1168 // SET PACKET ID
1159 1169 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
1160 1170 {
1161 1171 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
1162 1172 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
1163 1173 }
1164 1174 else
1165 1175 {
1166 1176 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1167 1177 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1168 1178 }
1169 1179
1170 1180 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1171 1181 if (status != RTEMS_SUCCESSFUL) {
1172 1182 ret = LFR_DEFAULT;
1173 1183 }
1174 1184 }
1175 1185
1176 1186 return ret;
1177 1187 }
1178 1188
1179 1189 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
1180 1190 Header_TM_LFR_SCIENCE_SWF_t *header )
1181 1191 {
1182 1192 /** This function sends SWF CCSDS packets (F2, F1 or F0).
1183 1193 *
1184 1194 * @param waveform points to the buffer containing the data that will be send.
1185 1195 * @param sid is the source identifier of the data that will be sent.
1186 1196 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1187 1197 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1188 1198 * contain information to setup the transmission of the data packets.
1189 1199 *
1190 1200 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1191 1201 *
1192 1202 */
1193 1203
1194 1204 unsigned int i;
1195 1205 int ret;
1196 1206 unsigned int coarseTime;
1197 1207 unsigned int fineTime;
1198 1208 rtems_status_code status;
1199 1209 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1200 1210 int *dataPtr;
1201 1211 unsigned char sid;
1202 1212
1203 1213 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1204 1214 spw_ioctl_send_SWF.options = 0;
1205 1215
1206 1216 ret = LFR_DEFAULT;
1207 1217
1208 1218 coarseTime = ring_node_to_send->coarseTime;
1209 1219 fineTime = ring_node_to_send->fineTime;
1210 1220 dataPtr = (int*) ring_node_to_send->buffer_address;
1211 1221 sid = ring_node_to_send->sid;
1212 1222
1213 1223 header->pa_bia_status_info = pa_bia_status_info;
1214 1224 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1215 1225
1216 1226 for (i=0; i<7; i++) // send waveform
1217 1227 {
1218 1228 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1219 1229 spw_ioctl_send_SWF.hdr = (char*) header;
1220 1230
1221 1231 // SET PACKET SEQUENCE CONTROL
1222 1232 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1223 1233
1224 1234 // SET PACKET LENGTH AND BLKNR
1225 1235 if (i == 6)
1226 1236 {
1227 1237 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1228 1238 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
1229 1239 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1230 1240 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
1231 1241 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1232 1242 }
1233 1243 else
1234 1244 {
1235 1245 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1236 1246 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
1237 1247 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1238 1248 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
1239 1249 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1240 1250 }
1241 1251
1242 1252 // SET PACKET TIME
1243 1253 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1244 1254 //
1245 1255 header->time[0] = header->acquisitionTime[0];
1246 1256 header->time[1] = header->acquisitionTime[1];
1247 1257 header->time[2] = header->acquisitionTime[2];
1248 1258 header->time[3] = header->acquisitionTime[3];
1249 1259 header->time[4] = header->acquisitionTime[4];
1250 1260 header->time[5] = header->acquisitionTime[5];
1251 1261
1252 1262 // SET SID
1253 1263 header->sid = sid;
1254 1264
1255 1265 // SET PKTNR
1256 1266 header->pktNr = i+1; // PKT_NR
1257 1267
1258 1268 // SEND PACKET
1259 1269 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1260 1270 if (status != RTEMS_SUCCESSFUL) {
1261 1271 ret = LFR_DEFAULT;
1262 1272 }
1263 1273 }
1264 1274
1265 1275 return ret;
1266 1276 }
1267 1277
1268 1278 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1269 1279 Header_TM_LFR_SCIENCE_CWF_t *header )
1270 1280 {
1271 1281 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1272 1282 *
1273 1283 * @param waveform points to the buffer containing the data that will be send.
1274 1284 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1275 1285 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1276 1286 * contain information to setup the transmission of the data packets.
1277 1287 *
1278 1288 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1279 1289 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1280 1290 *
1281 1291 */
1282 1292
1283 1293 unsigned int i;
1284 1294 int ret;
1285 1295 unsigned int coarseTime;
1286 1296 unsigned int fineTime;
1287 1297 rtems_status_code status;
1288 1298 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1289 1299 char *dataPtr;
1290 1300 unsigned char sid;
1291 1301
1292 1302 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1293 1303 spw_ioctl_send_CWF.options = 0;
1294 1304
1295 1305 ret = LFR_DEFAULT;
1296 1306 sid = ring_node_to_send->sid;
1297 1307
1298 1308 coarseTime = ring_node_to_send->coarseTime;
1299 1309 fineTime = ring_node_to_send->fineTime;
1300 1310 dataPtr = (char*) ring_node_to_send->buffer_address;
1301 1311
1302 1312 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
1303 1313 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1304 1314 header->pa_bia_status_info = pa_bia_status_info;
1305 1315 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1306 1316 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
1307 1317 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1308 1318
1309 1319 //*********************
1310 1320 // SEND CWF3_light DATA
1311 1321 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1312 1322 {
1313 1323 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1314 1324 spw_ioctl_send_CWF.hdr = (char*) header;
1315 1325 // BUILD THE DATA
1316 1326 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1317 1327
1318 1328 // SET PACKET SEQUENCE COUNTER
1319 1329 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1320 1330
1321 1331 // SET SID
1322 1332 header->sid = sid;
1323 1333
1324 1334 // SET PACKET TIME
1325 1335 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1326 1336 //
1327 1337 header->time[0] = header->acquisitionTime[0];
1328 1338 header->time[1] = header->acquisitionTime[1];
1329 1339 header->time[2] = header->acquisitionTime[2];
1330 1340 header->time[3] = header->acquisitionTime[3];
1331 1341 header->time[4] = header->acquisitionTime[4];
1332 1342 header->time[5] = header->acquisitionTime[5];
1333 1343
1334 1344 // SET PACKET ID
1335 1345 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1336 1346 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1337 1347
1338 1348 // SEND PACKET
1339 1349 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1340 1350 if (status != RTEMS_SUCCESSFUL) {
1341 1351 ret = LFR_DEFAULT;
1342 1352 }
1343 1353 }
1344 1354
1345 1355 return ret;
1346 1356 }
1347 1357
1348 1358 void spw_send_asm_f0( ring_node *ring_node_to_send,
1349 1359 Header_TM_LFR_SCIENCE_ASM_t *header )
1350 1360 {
1351 1361 unsigned int i;
1352 1362 unsigned int length = 0;
1353 1363 rtems_status_code status;
1354 1364 unsigned int sid;
1355 1365 float *spectral_matrix;
1356 1366 int coarseTime;
1357 1367 int fineTime;
1358 1368 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1359 1369
1360 1370 sid = ring_node_to_send->sid;
1361 1371 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1362 1372 coarseTime = ring_node_to_send->coarseTime;
1363 1373 fineTime = ring_node_to_send->fineTime;
1364 1374
1365 1375 header->pa_bia_status_info = pa_bia_status_info;
1366 1376 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1367 1377
1368 1378 for (i=0; i<3; i++)
1369 1379 {
1370 1380 if ((i==0) || (i==1))
1371 1381 {
1372 1382 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1373 1383 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1374 1384 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1375 1385 ];
1376 1386 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1377 1387 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1378 1388 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1379 1389 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1380 1390 }
1381 1391 else
1382 1392 {
1383 1393 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1384 1394 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1385 1395 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1386 1396 ];
1387 1397 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1388 1398 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1389 1399 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1390 1400 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1391 1401 }
1392 1402
1393 1403 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1394 1404 spw_ioctl_send_ASM.hdr = (char *) header;
1395 1405 spw_ioctl_send_ASM.options = 0;
1396 1406
1397 1407 // (2) BUILD THE HEADER
1398 1408 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1399 1409 header->packetLength[0] = (unsigned char) (length>>8);
1400 1410 header->packetLength[1] = (unsigned char) (length);
1401 1411 header->sid = (unsigned char) sid; // SID
1402 1412 header->pa_lfr_pkt_cnt_asm = 3;
1403 1413 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1404 1414
1405 1415 // (3) SET PACKET TIME
1406 1416 header->time[0] = (unsigned char) (coarseTime>>24);
1407 1417 header->time[1] = (unsigned char) (coarseTime>>16);
1408 1418 header->time[2] = (unsigned char) (coarseTime>>8);
1409 1419 header->time[3] = (unsigned char) (coarseTime);
1410 1420 header->time[4] = (unsigned char) (fineTime>>8);
1411 1421 header->time[5] = (unsigned char) (fineTime);
1412 1422 //
1413 1423 header->acquisitionTime[0] = header->time[0];
1414 1424 header->acquisitionTime[1] = header->time[1];
1415 1425 header->acquisitionTime[2] = header->time[2];
1416 1426 header->acquisitionTime[3] = header->time[3];
1417 1427 header->acquisitionTime[4] = header->time[4];
1418 1428 header->acquisitionTime[5] = header->time[5];
1419 1429
1420 1430 // (4) SEND PACKET
1421 1431 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1422 1432 if (status != RTEMS_SUCCESSFUL) {
1423 1433 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1424 1434 }
1425 1435 }
1426 1436 }
1427 1437
1428 1438 void spw_send_asm_f1( ring_node *ring_node_to_send,
1429 1439 Header_TM_LFR_SCIENCE_ASM_t *header )
1430 1440 {
1431 1441 unsigned int i;
1432 1442 unsigned int length = 0;
1433 1443 rtems_status_code status;
1434 1444 unsigned int sid;
1435 1445 float *spectral_matrix;
1436 1446 int coarseTime;
1437 1447 int fineTime;
1438 1448 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1439 1449
1440 1450 sid = ring_node_to_send->sid;
1441 1451 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1442 1452 coarseTime = ring_node_to_send->coarseTime;
1443 1453 fineTime = ring_node_to_send->fineTime;
1444 1454
1445 1455 header->pa_bia_status_info = pa_bia_status_info;
1446 1456 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1447 1457
1448 1458 for (i=0; i<3; i++)
1449 1459 {
1450 1460 if ((i==0) || (i==1))
1451 1461 {
1452 1462 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1453 1463 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1454 1464 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1455 1465 ];
1456 1466 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1457 1467 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1458 1468 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1459 1469 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1460 1470 }
1461 1471 else
1462 1472 {
1463 1473 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1464 1474 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1465 1475 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1466 1476 ];
1467 1477 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1468 1478 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1469 1479 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1470 1480 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1471 1481 }
1472 1482
1473 1483 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1474 1484 spw_ioctl_send_ASM.hdr = (char *) header;
1475 1485 spw_ioctl_send_ASM.options = 0;
1476 1486
1477 1487 // (2) BUILD THE HEADER
1478 1488 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1479 1489 header->packetLength[0] = (unsigned char) (length>>8);
1480 1490 header->packetLength[1] = (unsigned char) (length);
1481 1491 header->sid = (unsigned char) sid; // SID
1482 1492 header->pa_lfr_pkt_cnt_asm = 3;
1483 1493 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1484 1494
1485 1495 // (3) SET PACKET TIME
1486 1496 header->time[0] = (unsigned char) (coarseTime>>24);
1487 1497 header->time[1] = (unsigned char) (coarseTime>>16);
1488 1498 header->time[2] = (unsigned char) (coarseTime>>8);
1489 1499 header->time[3] = (unsigned char) (coarseTime);
1490 1500 header->time[4] = (unsigned char) (fineTime>>8);
1491 1501 header->time[5] = (unsigned char) (fineTime);
1492 1502 //
1493 1503 header->acquisitionTime[0] = header->time[0];
1494 1504 header->acquisitionTime[1] = header->time[1];
1495 1505 header->acquisitionTime[2] = header->time[2];
1496 1506 header->acquisitionTime[3] = header->time[3];
1497 1507 header->acquisitionTime[4] = header->time[4];
1498 1508 header->acquisitionTime[5] = header->time[5];
1499 1509
1500 1510 // (4) SEND PACKET
1501 1511 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1502 1512 if (status != RTEMS_SUCCESSFUL) {
1503 1513 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1504 1514 }
1505 1515 }
1506 1516 }
1507 1517
1508 1518 void spw_send_asm_f2( ring_node *ring_node_to_send,
1509 1519 Header_TM_LFR_SCIENCE_ASM_t *header )
1510 1520 {
1511 1521 unsigned int i;
1512 1522 unsigned int length = 0;
1513 1523 rtems_status_code status;
1514 1524 unsigned int sid;
1515 1525 float *spectral_matrix;
1516 1526 int coarseTime;
1517 1527 int fineTime;
1518 1528 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1519 1529
1520 1530 sid = ring_node_to_send->sid;
1521 1531 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1522 1532 coarseTime = ring_node_to_send->coarseTime;
1523 1533 fineTime = ring_node_to_send->fineTime;
1524 1534
1525 1535 header->pa_bia_status_info = pa_bia_status_info;
1526 1536 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1527 1537
1528 1538 for (i=0; i<3; i++)
1529 1539 {
1530 1540
1531 1541 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1532 1542 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1533 1543 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1534 1544 ];
1535 1545 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1536 1546 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1537 1547 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1538 1548 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1539 1549
1540 1550 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1541 1551 spw_ioctl_send_ASM.hdr = (char *) header;
1542 1552 spw_ioctl_send_ASM.options = 0;
1543 1553
1544 1554 // (2) BUILD THE HEADER
1545 1555 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1546 1556 header->packetLength[0] = (unsigned char) (length>>8);
1547 1557 header->packetLength[1] = (unsigned char) (length);
1548 1558 header->sid = (unsigned char) sid; // SID
1549 1559 header->pa_lfr_pkt_cnt_asm = 3;
1550 1560 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1551 1561
1552 1562 // (3) SET PACKET TIME
1553 1563 header->time[0] = (unsigned char) (coarseTime>>24);
1554 1564 header->time[1] = (unsigned char) (coarseTime>>16);
1555 1565 header->time[2] = (unsigned char) (coarseTime>>8);
1556 1566 header->time[3] = (unsigned char) (coarseTime);
1557 1567 header->time[4] = (unsigned char) (fineTime>>8);
1558 1568 header->time[5] = (unsigned char) (fineTime);
1559 1569 //
1560 1570 header->acquisitionTime[0] = header->time[0];
1561 1571 header->acquisitionTime[1] = header->time[1];
1562 1572 header->acquisitionTime[2] = header->time[2];
1563 1573 header->acquisitionTime[3] = header->time[3];
1564 1574 header->acquisitionTime[4] = header->time[4];
1565 1575 header->acquisitionTime[5] = header->time[5];
1566 1576
1567 1577 // (4) SEND PACKET
1568 1578 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1569 1579 if (status != RTEMS_SUCCESSFUL) {
1570 1580 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1571 1581 }
1572 1582 }
1573 1583 }
1574 1584
1575 1585 void spw_send_k_dump( ring_node *ring_node_to_send )
1576 1586 {
1577 1587 rtems_status_code status;
1578 1588 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1579 1589 unsigned int packetLength;
1580 1590 unsigned int size;
1581 1591
1582 1592 PRINTF("spw_send_k_dump\n")
1583 1593
1584 1594 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1585 1595
1586 1596 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1587 1597
1588 1598 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1589 1599
1590 1600 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1591 1601
1592 1602 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1593 1603
1594 1604 if (status == -1){
1595 1605 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1596 1606 }
1597 1607
1598 1608 ring_node_to_send->status = 0x00;
1599 1609 }
Show file before | Show file after | Hide comments
@@ -1,474 +1,474
1 1 /** Functions related to TeleCommand acceptance.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands parsing.\n
7 7 *
8 8 */
9 9
10 10 #include "tc_acceptance.h"
11 11 #include <stdio.h>
12 12
13 13 unsigned int lookUpTableForCRC[256];
14 14
15 15 //**********************
16 16 // GENERAL USE FUNCTIONS
17 17 unsigned int Crc_opt( unsigned char D, unsigned int Chk)
18 18 {
19 19 /** This function generate the CRC for one byte and returns the value of the new syndrome.
20 20 *
21 21 * @param D is the current byte of data.
22 22 * @param Chk is the current syndrom value.
23 23 *
24 24 * @return the value of the new syndrome on two bytes.
25 25 *
26 26 */
27 27
28 28 return(((Chk << 8) & 0xff00)^lookUpTableForCRC [(((Chk >> 8)^D) & 0x00ff)]);
29 29 }
30 30
31 31 void initLookUpTableForCRC( void )
32 32 {
33 33 /** This function is used to initiates the look-up table for fast CRC computation.
34 34 *
35 35 * The global table lookUpTableForCRC[256] is initiated.
36 36 *
37 37 */
38 38
39 39 unsigned int i;
40 40 unsigned int tmp;
41 41
42 42 for (i=0; i<256; i++)
43 43 {
44 44 tmp = 0;
45 45 if((i & 1) != 0) {
46 46 tmp = tmp ^ 0x1021;
47 47 }
48 48 if((i & 2) != 0) {
49 49 tmp = tmp ^ 0x2042;
50 50 }
51 51 if((i & 4) != 0) {
52 52 tmp = tmp ^ 0x4084;
53 53 }
54 54 if((i & 8) != 0) {
55 55 tmp = tmp ^ 0x8108;
56 56 }
57 57 if((i & 16) != 0) {
58 58 tmp = tmp ^ 0x1231;
59 59 }
60 60 if((i & 32) != 0) {
61 61 tmp = tmp ^ 0x2462;
62 62 }
63 63 if((i & 64) != 0) {
64 64 tmp = tmp ^ 0x48c4;
65 65 }
66 66 if((i & 128) != 0) {
67 67 tmp = tmp ^ 0x9188;
68 68 }
69 69 lookUpTableForCRC[i] = tmp;
70 70 }
71 71 }
72 72
73 73 void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData)
74 74 {
75 75 /** This function calculates a two bytes Cyclic Redundancy Code.
76 76 *
77 77 * @param data points to a buffer containing the data on which to compute the CRC.
78 78 * @param crcAsTwoBytes points points to a two bytes buffer in which the CRC is stored.
79 79 * @param sizeOfData is the number of bytes of *data* used to compute the CRC.
80 80 *
81 81 * The specification of the Cyclic Redundancy Code is described in the following document: ECSS-E-70-41-A.
82 82 *
83 83 */
84 84
85 85 unsigned int Chk;
86 86 int j;
87 87 Chk = 0xffff; // reset the syndrom to all ones
88 88 for (j=0; j<sizeOfData; j++) {
89 89 Chk = Crc_opt(data[j], Chk);
90 90 }
91 91 crcAsTwoBytes[0] = (unsigned char) (Chk >> 8);
92 92 crcAsTwoBytes[1] = (unsigned char) (Chk & 0x00ff);
93 93 }
94 94
95 95 //*********************
96 96 // ACCEPTANCE FUNCTIONS
97 97 int tc_parser(ccsdsTelecommandPacket_t * TCPacket, unsigned int estimatedPacketLength, unsigned char *computed_CRC)
98 98 {
99 99 /** This function parses TeleCommands.
100 100 *
101 101 * @param TC points to the TeleCommand that will be parsed.
102 102 * @param estimatedPacketLength is the PACKET_LENGTH field calculated from the effective length of the received packet.
103 103 *
104 104 * @return Status code of the parsing.
105 105 *
106 106 * The parsing checks:
107 107 * - process id
108 108 * - category
109 109 * - length: a global check is performed and a per subtype check also
110 110 * - type
111 111 * - subtype
112 112 * - crc
113 113 *
114 114 */
115 115
116 116 int status;
117 117 int status_crc;
118 118 unsigned char pid;
119 119 unsigned char category;
120 120 unsigned int packetLength;
121 121 unsigned char packetType;
122 122 unsigned char packetSubtype;
123 123 unsigned char sid;
124 124
125 125 status = CCSDS_TM_VALID;
126 126
127 127 // APID check *** APID on 2 bytes
128 128 pid = ((TCPacket->packetID[0] & 0x07)<<4) + ( (TCPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
129 129 category = (TCPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
130 130 packetLength = (TCPacket->packetLength[0] * 256) + TCPacket->packetLength[1];
131 131 packetType = TCPacket->serviceType;
132 132 packetSubtype = TCPacket->serviceSubType;
133 133 sid = TCPacket->sourceID;
134 134
135 135 if ( pid != CCSDS_PROCESS_ID ) // CHECK THE PROCESS ID
136 136 {
137 137 status = ILLEGAL_APID;
138 138 }
139 139 if (status == CCSDS_TM_VALID) // CHECK THE CATEGORY
140 140 {
141 141 if ( category != CCSDS_PACKET_CATEGORY )
142 142 {
143 143 status = ILLEGAL_APID;
144 144 }
145 145 }
146 146 if (status == CCSDS_TM_VALID) // CHECK THE PACKET_LENGTH FIELD AND THE ESTIMATED PACKET_LENGTH COMPLIANCE
147 147 {
148 148 if (packetLength != estimatedPacketLength ) {
149 149 status = WRONG_LEN_PKT;
150 150 }
151 151 }
152 152 if (status == CCSDS_TM_VALID) // CHECK THAT THE PACKET DOES NOT EXCEED THE MAX SIZE
153 153 {
154 if ( packetLength >= CCSDS_TC_PKT_MAX_SIZE ) {
154 if ( packetLength > CCSDS_TC_PKT_MAX_SIZE ) {
155 155 status = WRONG_LEN_PKT;
156 156 }
157 157 }
158 158 if (status == CCSDS_TM_VALID) // CHECK THE TYPE
159 159 {
160 160 status = tc_check_type( packetType );
161 161 }
162 162 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE
163 163 {
164 164 status = tc_check_type_subtype( packetType, packetSubtype );
165 165 }
166 166 if (status == CCSDS_TM_VALID) // CHECK THE SID
167 167 {
168 168 status = tc_check_sid( sid );
169 169 }
170 170 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE AND LENGTH COMPLIANCE
171 171 {
172 172 status = tc_check_length( packetSubtype, packetLength );
173 173 }
174 174 status_crc = tc_check_crc( TCPacket, estimatedPacketLength, computed_CRC );
175 175 if (status == CCSDS_TM_VALID ) // CHECK CRC
176 176 {
177 177 status = status_crc;
178 178 }
179 179
180 180 return status;
181 181 }
182 182
183 183 int tc_check_type( unsigned char packetType )
184 184 {
185 185 /** This function checks that the type of a TeleCommand is valid.
186 186 *
187 187 * @param packetType is the type to check.
188 188 *
189 189 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
190 190 *
191 191 */
192 192
193 193 int status;
194 194
195 195 if ( (packetType == TC_TYPE_GEN) || (packetType == TC_TYPE_TIME))
196 196 {
197 197 status = CCSDS_TM_VALID;
198 198 }
199 199 else
200 200 {
201 201 status = ILL_TYPE;
202 202 }
203 203
204 204 return status;
205 205 }
206 206
207 207 int tc_check_type_subtype( unsigned char packetType, unsigned char packetSubType )
208 208 {
209 209 /** This function checks that the subtype of a TeleCommand is valid and coherent with the type.
210 210 *
211 211 * @param packetType is the type of the TC.
212 212 * @param packetSubType is the subtype to check.
213 213 *
214 214 * @return Status code CCSDS_TM_VALID or ILL_SUBTYPE.
215 215 *
216 216 */
217 217
218 218 int status;
219 219
220 220 switch(packetType)
221 221 {
222 222 case TC_TYPE_GEN:
223 223 if ( (packetSubType == TC_SUBTYPE_RESET)
224 224 || (packetSubType == TC_SUBTYPE_LOAD_COMM)
225 225 || (packetSubType == TC_SUBTYPE_LOAD_NORM) || (packetSubType == TC_SUBTYPE_LOAD_BURST)
226 226 || (packetSubType == TC_SUBTYPE_LOAD_SBM1) || (packetSubType == TC_SUBTYPE_LOAD_SBM2)
227 227 || (packetSubType == TC_SUBTYPE_DUMP)
228 228 || (packetSubType == TC_SUBTYPE_ENTER)
229 229 || (packetSubType == TC_SUBTYPE_UPDT_INFO)
230 230 || (packetSubType == TC_SUBTYPE_EN_CAL) || (packetSubType == TC_SUBTYPE_DIS_CAL)
231 231 || (packetSubType == TC_SUBTYPE_LOAD_K) || (packetSubType == TC_SUBTYPE_DUMP_K)
232 232 || (packetSubType == TC_SUBTYPE_LOAD_FBINS)
233 233 || (packetSubType == TC_SUBTYPE_LOAD_FILTER_PAR))
234 234 {
235 235 status = CCSDS_TM_VALID;
236 236 }
237 237 else
238 238 {
239 239 status = ILL_SUBTYPE;
240 240 }
241 241 break;
242 242
243 243 case TC_TYPE_TIME:
244 244 if (packetSubType == TC_SUBTYPE_UPDT_TIME)
245 245 {
246 246 status = CCSDS_TM_VALID;
247 247 }
248 248 else
249 249 {
250 250 status = ILL_SUBTYPE;
251 251 }
252 252 break;
253 253
254 254 default:
255 255 status = ILL_SUBTYPE;
256 256 break;
257 257 }
258 258
259 259 return status;
260 260 }
261 261
262 262 int tc_check_sid( unsigned char sid )
263 263 {
264 264 /** This function checks that the sid of a TeleCommand is valid.
265 265 *
266 266 * @param sid is the sid to check.
267 267 *
268 268 * @return Status code CCSDS_TM_VALID or CORRUPTED.
269 269 *
270 270 */
271 271
272 272 int status;
273 273
274 274 if ( (sid == SID_TC_MISSION_TIMELINE) || (sid == SID_TC_TC_SEQUENCES) || (sid == SID_TC_RECOVERY_ACTION_CMD)
275 275 || (sid == SID_TC_BACKUP_MISSION_TIMELINE)
276 276 || (sid == SID_TC_DIRECT_CMD) || (sid == SID_TC_SPARE_GRD_SRC1) || (sid == SID_TC_SPARE_GRD_SRC2)
277 277 || (sid == SID_TC_OBCP) || (sid == SID_TC_SYSTEM_CONTROL) || (sid == SID_TC_AOCS)
278 278 || (sid == SID_TC_RPW_INTERNAL))
279 279 {
280 280 status = CCSDS_TM_VALID;
281 281 }
282 282 else
283 283 {
284 284 status = WRONG_SRC_ID;
285 285 }
286 286
287 287 return status;
288 288 }
289 289
290 290 int tc_check_length( unsigned char packetSubType, unsigned int length )
291 291 {
292 292 /** This function checks that the subtype and the length are compliant.
293 293 *
294 294 * @param packetSubType is the subtype to check.
295 295 * @param length is the length to check.
296 296 *
297 297 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
298 298 *
299 299 */
300 300
301 301 int status;
302 302
303 303 status = LFR_SUCCESSFUL;
304 304
305 305 switch(packetSubType)
306 306 {
307 307 case TC_SUBTYPE_RESET:
308 308 if (length!=(TC_LEN_RESET-CCSDS_TC_TM_PACKET_OFFSET)) {
309 309 status = WRONG_LEN_PKT;
310 310 }
311 311 else {
312 312 status = CCSDS_TM_VALID;
313 313 }
314 314 break;
315 315 case TC_SUBTYPE_LOAD_COMM:
316 316 if (length!=(TC_LEN_LOAD_COMM-CCSDS_TC_TM_PACKET_OFFSET)) {
317 317 status = WRONG_LEN_PKT;
318 318 }
319 319 else {
320 320 status = CCSDS_TM_VALID;
321 321 }
322 322 break;
323 323 case TC_SUBTYPE_LOAD_NORM:
324 324 if (length!=(TC_LEN_LOAD_NORM-CCSDS_TC_TM_PACKET_OFFSET)) {
325 325 status = WRONG_LEN_PKT;
326 326 }
327 327 else {
328 328 status = CCSDS_TM_VALID;
329 329 }
330 330 break;
331 331 case TC_SUBTYPE_LOAD_BURST:
332 332 if (length!=(TC_LEN_LOAD_BURST-CCSDS_TC_TM_PACKET_OFFSET)) {
333 333 status = WRONG_LEN_PKT;
334 334 }
335 335 else {
336 336 status = CCSDS_TM_VALID;
337 337 }
338 338 break;
339 339 case TC_SUBTYPE_LOAD_SBM1:
340 340 if (length!=(TC_LEN_LOAD_SBM1-CCSDS_TC_TM_PACKET_OFFSET)) {
341 341 status = WRONG_LEN_PKT;
342 342 }
343 343 else {
344 344 status = CCSDS_TM_VALID;
345 345 }
346 346 break;
347 347 case TC_SUBTYPE_LOAD_SBM2:
348 348 if (length!=(TC_LEN_LOAD_SBM2-CCSDS_TC_TM_PACKET_OFFSET)) {
349 349 status = WRONG_LEN_PKT;
350 350 }
351 351 else {
352 352 status = CCSDS_TM_VALID;
353 353 }
354 354 break;
355 355 case TC_SUBTYPE_DUMP:
356 356 if (length!=(TC_LEN_DUMP-CCSDS_TC_TM_PACKET_OFFSET)) {
357 357 status = WRONG_LEN_PKT;
358 358 }
359 359 else {
360 360 status = CCSDS_TM_VALID;
361 361 }
362 362 break;
363 363 case TC_SUBTYPE_ENTER:
364 364 if (length!=(TC_LEN_ENTER-CCSDS_TC_TM_PACKET_OFFSET)) {
365 365 status = WRONG_LEN_PKT;
366 366 }
367 367 else {
368 368 status = CCSDS_TM_VALID;
369 369 }
370 370 break;
371 371 case TC_SUBTYPE_UPDT_INFO:
372 372 if (length!=(TC_LEN_UPDT_INFO-CCSDS_TC_TM_PACKET_OFFSET)) {
373 373 status = WRONG_LEN_PKT;
374 374 }
375 375 else {
376 376 status = CCSDS_TM_VALID;
377 377 }
378 378 break;
379 379 case TC_SUBTYPE_EN_CAL:
380 380 if (length!=(TC_LEN_EN_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
381 381 status = WRONG_LEN_PKT;
382 382 }
383 383 else {
384 384 status = CCSDS_TM_VALID;
385 385 }
386 386 break;
387 387 case TC_SUBTYPE_DIS_CAL:
388 388 if (length!=(TC_LEN_DIS_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
389 389 status = WRONG_LEN_PKT;
390 390 }
391 391 else {
392 392 status = CCSDS_TM_VALID;
393 393 }
394 394 break;
395 395 case TC_SUBTYPE_LOAD_K:
396 396 if (length!=(TC_LEN_LOAD_K-CCSDS_TC_TM_PACKET_OFFSET)) {
397 397 status = WRONG_LEN_PKT;
398 398 }
399 399 else {
400 400 status = CCSDS_TM_VALID;
401 401 }
402 402 break;
403 403 case TC_SUBTYPE_DUMP_K:
404 404 if (length!=(TC_LEN_DUMP_K-CCSDS_TC_TM_PACKET_OFFSET)) {
405 405 status = WRONG_LEN_PKT;
406 406 }
407 407 else {
408 408 status = CCSDS_TM_VALID;
409 409 }
410 410 break;
411 411 case TC_SUBTYPE_LOAD_FBINS:
412 412 if (length!=(TC_LEN_LOAD_FBINS-CCSDS_TC_TM_PACKET_OFFSET)) {
413 413 status = WRONG_LEN_PKT;
414 414 }
415 415 else {
416 416 status = CCSDS_TM_VALID;
417 417 }
418 418 break;
419 419 case TC_SUBTYPE_LOAD_FILTER_PAR:
420 420 if (length!=(TC_LEN_LOAD_FILTER_PAR-CCSDS_TC_TM_PACKET_OFFSET)) {
421 421 status = WRONG_LEN_PKT;
422 422 }
423 423 else {
424 424 status = CCSDS_TM_VALID;
425 425 }
426 426 break;
427 427 case TC_SUBTYPE_UPDT_TIME:
428 428 if (length!=(TC_LEN_UPDT_TIME-CCSDS_TC_TM_PACKET_OFFSET)) {
429 429 status = WRONG_LEN_PKT;
430 430 }
431 431 else {
432 432 status = CCSDS_TM_VALID;
433 433 }
434 434 break;
435 435 default: // if the subtype is not a legal value, return ILL_SUBTYPE
436 436 status = ILL_SUBTYPE;
437 437 break ;
438 438 }
439 439
440 440 return status;
441 441 }
442 442
443 443 int tc_check_crc( ccsdsTelecommandPacket_t * TCPacket, unsigned int length, unsigned char *computed_CRC )
444 444 {
445 445 /** This function checks the CRC validity of the corresponding TeleCommand packet.
446 446 *
447 447 * @param TCPacket points to the TeleCommand packet to check.
448 448 * @param length is the length of the TC packet.
449 449 *
450 450 * @return Status code CCSDS_TM_VALID or INCOR_CHECKSUM.
451 451 *
452 452 */
453 453
454 454 int status;
455 455 unsigned char * CCSDSContent;
456 456
457 457 CCSDSContent = (unsigned char*) TCPacket->packetID;
458 458 GetCRCAsTwoBytes(CCSDSContent, computed_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC
459 459
460 460 if (computed_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) {
461 461 status = INCOR_CHECKSUM;
462 462 }
463 463 else if (computed_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) {
464 464 status = INCOR_CHECKSUM;
465 465 }
466 466 else {
467 467 status = CCSDS_TM_VALID;
468 468 }
469 469
470 470 return status;
471 471 }
472 472
473 473
474 474
Show file before | Show file after | Hide comments
@@ -1,1642 +1,1645
1 1 /** Functions and tasks related to TeleCommand handling.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands:\n
7 7 * action launching\n
8 8 * TC parsing\n
9 9 * ...
10 10 *
11 11 */
12 12
13 13 #include "tc_handler.h"
14 14 #include "math.h"
15 15
16 16 //***********
17 17 // RTEMS TASK
18 18
19 19 rtems_task actn_task( rtems_task_argument unused )
20 20 {
21 21 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 22 *
23 23 * @param unused is the starting argument of the RTEMS task
24 24 *
25 25 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 26 * on the incoming TeleCommand.
27 27 *
28 28 */
29 29
30 30 int result;
31 31 rtems_status_code status; // RTEMS status code
32 32 ccsdsTelecommandPacket_t TC; // TC sent to the ACTN task
33 33 size_t size; // size of the incoming TC packet
34 34 unsigned char subtype; // subtype of the current TC packet
35 35 unsigned char time[6];
36 36 rtems_id queue_rcv_id;
37 37 rtems_id queue_snd_id;
38 38
39 39 status = get_message_queue_id_recv( &queue_rcv_id );
40 40 if (status != RTEMS_SUCCESSFUL)
41 41 {
42 42 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
43 43 }
44 44
45 45 status = get_message_queue_id_send( &queue_snd_id );
46 46 if (status != RTEMS_SUCCESSFUL)
47 47 {
48 48 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
49 49 }
50 50
51 51 result = LFR_SUCCESSFUL;
52 52 subtype = 0; // subtype of the current TC packet
53 53
54 54 BOOT_PRINTF("in ACTN *** \n");
55 55
56 56 while(1)
57 57 {
58 58 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
59 59 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
60 60 getTime( time ); // set time to the current time
61 61 if (status!=RTEMS_SUCCESSFUL)
62 62 {
63 63 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
64 64 }
65 65 else
66 66 {
67 67 subtype = TC.serviceSubType;
68 68 switch(subtype)
69 69 {
70 70 case TC_SUBTYPE_RESET:
71 71 result = action_reset( &TC, queue_snd_id, time );
72 72 close_action( &TC, result, queue_snd_id );
73 73 break;
74 74 case TC_SUBTYPE_LOAD_COMM:
75 75 result = action_load_common_par( &TC );
76 76 close_action( &TC, result, queue_snd_id );
77 77 break;
78 78 case TC_SUBTYPE_LOAD_NORM:
79 79 result = action_load_normal_par( &TC, queue_snd_id, time );
80 80 close_action( &TC, result, queue_snd_id );
81 81 break;
82 82 case TC_SUBTYPE_LOAD_BURST:
83 83 result = action_load_burst_par( &TC, queue_snd_id, time );
84 84 close_action( &TC, result, queue_snd_id );
85 85 break;
86 86 case TC_SUBTYPE_LOAD_SBM1:
87 87 result = action_load_sbm1_par( &TC, queue_snd_id, time );
88 88 close_action( &TC, result, queue_snd_id );
89 89 break;
90 90 case TC_SUBTYPE_LOAD_SBM2:
91 91 result = action_load_sbm2_par( &TC, queue_snd_id, time );
92 92 close_action( &TC, result, queue_snd_id );
93 93 break;
94 94 case TC_SUBTYPE_DUMP:
95 95 result = action_dump_par( &TC, queue_snd_id );
96 96 close_action( &TC, result, queue_snd_id );
97 97 break;
98 98 case TC_SUBTYPE_ENTER:
99 99 result = action_enter_mode( &TC, queue_snd_id );
100 100 close_action( &TC, result, queue_snd_id );
101 101 break;
102 102 case TC_SUBTYPE_UPDT_INFO:
103 103 result = action_update_info( &TC, queue_snd_id );
104 104 close_action( &TC, result, queue_snd_id );
105 105 break;
106 106 case TC_SUBTYPE_EN_CAL:
107 107 result = action_enable_calibration( &TC, queue_snd_id, time );
108 108 close_action( &TC, result, queue_snd_id );
109 109 break;
110 110 case TC_SUBTYPE_DIS_CAL:
111 111 result = action_disable_calibration( &TC, queue_snd_id, time );
112 112 close_action( &TC, result, queue_snd_id );
113 113 break;
114 114 case TC_SUBTYPE_LOAD_K:
115 115 result = action_load_kcoefficients( &TC, queue_snd_id, time );
116 116 close_action( &TC, result, queue_snd_id );
117 117 break;
118 118 case TC_SUBTYPE_DUMP_K:
119 119 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
120 120 close_action( &TC, result, queue_snd_id );
121 121 break;
122 122 case TC_SUBTYPE_LOAD_FBINS:
123 123 result = action_load_fbins_mask( &TC, queue_snd_id, time );
124 124 close_action( &TC, result, queue_snd_id );
125 125 break;
126 126 case TC_SUBTYPE_LOAD_FILTER_PAR:
127 127 result = action_load_filter_par( &TC, queue_snd_id, time );
128 128 close_action( &TC, result, queue_snd_id );
129 129 break;
130 130 case TC_SUBTYPE_UPDT_TIME:
131 131 result = action_update_time( &TC );
132 132 close_action( &TC, result, queue_snd_id );
133 133 break;
134 134 default:
135 135 break;
136 136 }
137 137 }
138 138 }
139 139 }
140 140
141 141 //***********
142 142 // TC ACTIONS
143 143
144 144 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
145 145 {
146 146 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
147 147 *
148 148 * @param TC points to the TeleCommand packet that is being processed
149 149 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
150 150 *
151 151 */
152 152
153 153 PRINTF("this is the end!!!\n");
154 154 exit(0);
155 155
156 156 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
157 157
158 158 return LFR_DEFAULT;
159 159 }
160 160
161 161 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
162 162 {
163 163 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
164 164 *
165 165 * @param TC points to the TeleCommand packet that is being processed
166 166 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
167 167 *
168 168 */
169 169
170 170 rtems_status_code status;
171 171 unsigned char requestedMode;
172 172 unsigned int *transitionCoarseTime_ptr;
173 173 unsigned int transitionCoarseTime;
174 174 unsigned char * bytePosPtr;
175 175
176 176 bytePosPtr = (unsigned char *) &TC->packetID;
177 177
178 178 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
179 179 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
180 180 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
181 181
182 182 status = check_mode_value( requestedMode );
183 183
184 184 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
185 185 {
186 186 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
187 187 }
188 188
189 189 else // the mode value is valid, check the transition
190 190 {
191 191 status = check_mode_transition(requestedMode);
192 192 if (status != LFR_SUCCESSFUL)
193 193 {
194 194 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
195 195 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
196 196 }
197 197 }
198 198
199 199 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
200 200 {
201 201 status = check_transition_date( transitionCoarseTime );
202 202 if (status != LFR_SUCCESSFUL)
203 203 {
204 204 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n");
205 205 send_tm_lfr_tc_exe_not_executable(TC, queue_id );
206 206 }
207 207 }
208 208
209 209 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
210 210 {
211 211 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
212 212
213 213 switch(requestedMode)
214 214 {
215 215 case LFR_MODE_STANDBY:
216 216 status = enter_mode_standby();
217 217 break;
218 218 case LFR_MODE_NORMAL:
219 219 status = enter_mode_normal( transitionCoarseTime );
220 220 break;
221 221 case LFR_MODE_BURST:
222 222 status = enter_mode_burst( transitionCoarseTime );
223 223 break;
224 224 case LFR_MODE_SBM1:
225 225 status = enter_mode_sbm1( transitionCoarseTime );
226 226 break;
227 227 case LFR_MODE_SBM2:
228 228 status = enter_mode_sbm2( transitionCoarseTime );
229 229 break;
230 230 default:
231 231 break;
232 232 }
233 233
234 234 if (status != RTEMS_SUCCESSFUL)
235 235 {
236 236 status = LFR_EXE_ERROR;
237 237 }
238 238 }
239 239
240 240 return status;
241 241 }
242 242
243 243 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
244 244 {
245 245 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
246 246 *
247 247 * @param TC points to the TeleCommand packet that is being processed
248 248 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
249 249 *
250 250 * @return LFR directive status code:
251 251 * - LFR_DEFAULT
252 252 * - LFR_SUCCESSFUL
253 253 *
254 254 */
255 255
256 256 unsigned int val;
257 257 int result;
258 258 unsigned int status;
259 259 unsigned char mode;
260 260 unsigned char * bytePosPtr;
261 261
262 262 bytePosPtr = (unsigned char *) &TC->packetID;
263 263
264 264 // check LFR mode
265 265 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
266 266 status = check_update_info_hk_lfr_mode( mode );
267 267 if (status == LFR_SUCCESSFUL) // check TDS mode
268 268 {
269 269 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
270 270 status = check_update_info_hk_tds_mode( mode );
271 271 }
272 272 if (status == LFR_SUCCESSFUL) // check THR mode
273 273 {
274 274 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
275 275 status = check_update_info_hk_thr_mode( mode );
276 276 }
277 277 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
278 278 {
279 279 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
280 280 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
281 281 val++;
282 282 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
283 283 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
284 284 }
285 285
286 286 // pa_bia_status_info
287 287 // => pa_bia_mode_mux_set 3 bits
288 288 // => pa_bia_mode_hv_enabled 1 bit
289 289 // => pa_bia_mode_bias1_enabled 1 bit
290 290 // => pa_bia_mode_bias2_enabled 1 bit
291 291 // => pa_bia_mode_bias3_enabled 1 bit
292 292 // => pa_bia_on_off (cp_dpu_bias_on_off)
293 293 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & 0xfe; // [1111 1110]
294 294 pa_bia_status_info = pa_bia_status_info
295 295 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1);
296 296
297 297 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
298 298
299 299 cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ];
300 300 getReactionWheelsFrequencies( TC );
301 301 build_sy_lfr_rw_masks();
302 302
303 // once the masks are built, they have to be merged with the fbins_mask
304 merge_fbins_masks();
305
303 306 result = status;
304 307
305 308 return result;
306 309 }
307 310
308 311 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
309 312 {
310 313 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
311 314 *
312 315 * @param TC points to the TeleCommand packet that is being processed
313 316 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
314 317 *
315 318 */
316 319
317 320 int result;
318 321
319 322 result = LFR_DEFAULT;
320 323
321 324 setCalibration( true );
322 325
323 326 result = LFR_SUCCESSFUL;
324 327
325 328 return result;
326 329 }
327 330
328 331 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
329 332 {
330 333 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
331 334 *
332 335 * @param TC points to the TeleCommand packet that is being processed
333 336 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
334 337 *
335 338 */
336 339
337 340 int result;
338 341
339 342 result = LFR_DEFAULT;
340 343
341 344 setCalibration( false );
342 345
343 346 result = LFR_SUCCESSFUL;
344 347
345 348 return result;
346 349 }
347 350
348 351 int action_update_time(ccsdsTelecommandPacket_t *TC)
349 352 {
350 353 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
351 354 *
352 355 * @param TC points to the TeleCommand packet that is being processed
353 356 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
354 357 *
355 358 * @return LFR_SUCCESSFUL
356 359 *
357 360 */
358 361
359 362 unsigned int val;
360 363
361 364 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
362 365 + (TC->dataAndCRC[1] << 16)
363 366 + (TC->dataAndCRC[2] << 8)
364 367 + TC->dataAndCRC[3];
365 368
366 369 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
367 370 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
368 371 val++;
369 372 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
370 373 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
371 374
372 375 oneTcLfrUpdateTimeReceived = 1;
373 376
374 377 return LFR_SUCCESSFUL;
375 378 }
376 379
377 380 //*******************
378 381 // ENTERING THE MODES
379 382 int check_mode_value( unsigned char requestedMode )
380 383 {
381 384 int status;
382 385
383 386 if ( (requestedMode != LFR_MODE_STANDBY)
384 387 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
385 388 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
386 389 {
387 390 status = LFR_DEFAULT;
388 391 }
389 392 else
390 393 {
391 394 status = LFR_SUCCESSFUL;
392 395 }
393 396
394 397 return status;
395 398 }
396 399
397 400 int check_mode_transition( unsigned char requestedMode )
398 401 {
399 402 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
400 403 *
401 404 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
402 405 *
403 406 * @return LFR directive status codes:
404 407 * - LFR_SUCCESSFUL - the transition is authorized
405 408 * - LFR_DEFAULT - the transition is not authorized
406 409 *
407 410 */
408 411
409 412 int status;
410 413
411 414 switch (requestedMode)
412 415 {
413 416 case LFR_MODE_STANDBY:
414 417 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
415 418 status = LFR_DEFAULT;
416 419 }
417 420 else
418 421 {
419 422 status = LFR_SUCCESSFUL;
420 423 }
421 424 break;
422 425 case LFR_MODE_NORMAL:
423 426 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
424 427 status = LFR_DEFAULT;
425 428 }
426 429 else {
427 430 status = LFR_SUCCESSFUL;
428 431 }
429 432 break;
430 433 case LFR_MODE_BURST:
431 434 if ( lfrCurrentMode == LFR_MODE_BURST ) {
432 435 status = LFR_DEFAULT;
433 436 }
434 437 else {
435 438 status = LFR_SUCCESSFUL;
436 439 }
437 440 break;
438 441 case LFR_MODE_SBM1:
439 442 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
440 443 status = LFR_DEFAULT;
441 444 }
442 445 else {
443 446 status = LFR_SUCCESSFUL;
444 447 }
445 448 break;
446 449 case LFR_MODE_SBM2:
447 450 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
448 451 status = LFR_DEFAULT;
449 452 }
450 453 else {
451 454 status = LFR_SUCCESSFUL;
452 455 }
453 456 break;
454 457 default:
455 458 status = LFR_DEFAULT;
456 459 break;
457 460 }
458 461
459 462 return status;
460 463 }
461 464
462 465 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
463 466 {
464 467 if (transitionCoarseTime == 0)
465 468 {
466 469 lastValidEnterModeTime = time_management_regs->coarse_time + 1;
467 470 PRINTF1("lastValidEnterModeTime = 0x%x (transitionCoarseTime = 0 => coarse_time+1)\n", lastValidEnterModeTime);
468 471 }
469 472 else
470 473 {
471 474 lastValidEnterModeTime = transitionCoarseTime;
472 475 PRINTF1("lastValidEnterModeTime = 0x%x\n", transitionCoarseTime);
473 476 }
474 477 }
475 478
476 479 int check_transition_date( unsigned int transitionCoarseTime )
477 480 {
478 481 int status;
479 482 unsigned int localCoarseTime;
480 483 unsigned int deltaCoarseTime;
481 484
482 485 status = LFR_SUCCESSFUL;
483 486
484 487 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
485 488 {
486 489 status = LFR_SUCCESSFUL;
487 490 }
488 491 else
489 492 {
490 493 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
491 494
492 495 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime);
493 496
494 497 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
495 498 {
496 499 status = LFR_DEFAULT;
497 500 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n");
498 501 }
499 502
500 503 if (status == LFR_SUCCESSFUL)
501 504 {
502 505 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
503 506 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
504 507 {
505 508 status = LFR_DEFAULT;
506 509 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
507 510 }
508 511 }
509 512 }
510 513
511 514 return status;
512 515 }
513 516
514 517 int restart_asm_activities( unsigned char lfrRequestedMode )
515 518 {
516 519 rtems_status_code status;
517 520
518 521 status = stop_spectral_matrices();
519 522
520 523 thisIsAnASMRestart = 1;
521 524
522 525 status = restart_asm_tasks( lfrRequestedMode );
523 526
524 527 launch_spectral_matrix();
525 528
526 529 return status;
527 530 }
528 531
529 532 int stop_spectral_matrices( void )
530 533 {
531 534 /** This function stops and restarts the current mode average spectral matrices activities.
532 535 *
533 536 * @return RTEMS directive status codes:
534 537 * - RTEMS_SUCCESSFUL - task restarted successfully
535 538 * - RTEMS_INVALID_ID - task id invalid
536 539 * - RTEMS_ALREADY_SUSPENDED - task already suspended
537 540 *
538 541 */
539 542
540 543 rtems_status_code status;
541 544
542 545 status = RTEMS_SUCCESSFUL;
543 546
544 547 // (1) mask interruptions
545 548 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // mask spectral matrix interrupt
546 549
547 550 // (2) reset spectral matrices registers
548 551 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
549 552 reset_sm_status();
550 553
551 554 // (3) clear interruptions
552 555 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
553 556
554 557 // suspend several tasks
555 558 if (lfrCurrentMode != LFR_MODE_STANDBY) {
556 559 status = suspend_asm_tasks();
557 560 }
558 561
559 562 if (status != RTEMS_SUCCESSFUL)
560 563 {
561 564 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
562 565 }
563 566
564 567 return status;
565 568 }
566 569
567 570 int stop_current_mode( void )
568 571 {
569 572 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
570 573 *
571 574 * @return RTEMS directive status codes:
572 575 * - RTEMS_SUCCESSFUL - task restarted successfully
573 576 * - RTEMS_INVALID_ID - task id invalid
574 577 * - RTEMS_ALREADY_SUSPENDED - task already suspended
575 578 *
576 579 */
577 580
578 581 rtems_status_code status;
579 582
580 583 status = RTEMS_SUCCESSFUL;
581 584
582 585 // (1) mask interruptions
583 586 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
584 587 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
585 588
586 589 // (2) reset waveform picker registers
587 590 reset_wfp_burst_enable(); // reset burst and enable bits
588 591 reset_wfp_status(); // reset all the status bits
589 592
590 593 // (3) reset spectral matrices registers
591 594 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
592 595 reset_sm_status();
593 596
594 597 // reset lfr VHDL module
595 598 reset_lfr();
596 599
597 600 reset_extractSWF(); // reset the extractSWF flag to false
598 601
599 602 // (4) clear interruptions
600 603 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
601 604 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
602 605
603 606 // suspend several tasks
604 607 if (lfrCurrentMode != LFR_MODE_STANDBY) {
605 608 status = suspend_science_tasks();
606 609 }
607 610
608 611 if (status != RTEMS_SUCCESSFUL)
609 612 {
610 613 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
611 614 }
612 615
613 616 return status;
614 617 }
615 618
616 619 int enter_mode_standby( void )
617 620 {
618 621 /** This function is used to put LFR in the STANDBY mode.
619 622 *
620 623 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
621 624 *
622 625 * @return RTEMS directive status codes:
623 626 * - RTEMS_SUCCESSFUL - task restarted successfully
624 627 * - RTEMS_INVALID_ID - task id invalid
625 628 * - RTEMS_INCORRECT_STATE - task never started
626 629 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
627 630 *
628 631 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
629 632 * is immediate.
630 633 *
631 634 */
632 635
633 636 int status;
634 637
635 638 status = stop_current_mode(); // STOP THE CURRENT MODE
636 639
637 640 #ifdef PRINT_TASK_STATISTICS
638 641 rtems_cpu_usage_report();
639 642 #endif
640 643
641 644 #ifdef PRINT_STACK_REPORT
642 645 PRINTF("stack report selected\n")
643 646 rtems_stack_checker_report_usage();
644 647 #endif
645 648
646 649 return status;
647 650 }
648 651
649 652 int enter_mode_normal( unsigned int transitionCoarseTime )
650 653 {
651 654 /** This function is used to start the NORMAL mode.
652 655 *
653 656 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
654 657 *
655 658 * @return RTEMS directive status codes:
656 659 * - RTEMS_SUCCESSFUL - task restarted successfully
657 660 * - RTEMS_INVALID_ID - task id invalid
658 661 * - RTEMS_INCORRECT_STATE - task never started
659 662 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
660 663 *
661 664 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
662 665 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
663 666 *
664 667 */
665 668
666 669 int status;
667 670
668 671 #ifdef PRINT_TASK_STATISTICS
669 672 rtems_cpu_usage_reset();
670 673 #endif
671 674
672 675 status = RTEMS_UNSATISFIED;
673 676
674 677 switch( lfrCurrentMode )
675 678 {
676 679 case LFR_MODE_STANDBY:
677 680 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
678 681 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
679 682 {
680 683 launch_spectral_matrix( );
681 684 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
682 685 }
683 686 break;
684 687 case LFR_MODE_BURST:
685 688 status = stop_current_mode(); // stop the current mode
686 689 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
687 690 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
688 691 {
689 692 launch_spectral_matrix( );
690 693 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
691 694 }
692 695 break;
693 696 case LFR_MODE_SBM1:
694 697 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
695 698 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
696 699 update_last_valid_transition_date( transitionCoarseTime );
697 700 break;
698 701 case LFR_MODE_SBM2:
699 702 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
700 703 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
701 704 update_last_valid_transition_date( transitionCoarseTime );
702 705 break;
703 706 default:
704 707 break;
705 708 }
706 709
707 710 if (status != RTEMS_SUCCESSFUL)
708 711 {
709 712 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
710 713 status = RTEMS_UNSATISFIED;
711 714 }
712 715
713 716 return status;
714 717 }
715 718
716 719 int enter_mode_burst( unsigned int transitionCoarseTime )
717 720 {
718 721 /** This function is used to start the BURST mode.
719 722 *
720 723 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
721 724 *
722 725 * @return RTEMS directive status codes:
723 726 * - RTEMS_SUCCESSFUL - task restarted successfully
724 727 * - RTEMS_INVALID_ID - task id invalid
725 728 * - RTEMS_INCORRECT_STATE - task never started
726 729 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
727 730 *
728 731 * The way the BURST mode is started does not depend on the LFR current mode.
729 732 *
730 733 */
731 734
732 735
733 736 int status;
734 737
735 738 #ifdef PRINT_TASK_STATISTICS
736 739 rtems_cpu_usage_reset();
737 740 #endif
738 741
739 742 status = stop_current_mode(); // stop the current mode
740 743 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
741 744 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
742 745 {
743 746 launch_spectral_matrix( );
744 747 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
745 748 }
746 749
747 750 if (status != RTEMS_SUCCESSFUL)
748 751 {
749 752 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
750 753 status = RTEMS_UNSATISFIED;
751 754 }
752 755
753 756 return status;
754 757 }
755 758
756 759 int enter_mode_sbm1( unsigned int transitionCoarseTime )
757 760 {
758 761 /** This function is used to start the SBM1 mode.
759 762 *
760 763 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
761 764 *
762 765 * @return RTEMS directive status codes:
763 766 * - RTEMS_SUCCESSFUL - task restarted successfully
764 767 * - RTEMS_INVALID_ID - task id invalid
765 768 * - RTEMS_INCORRECT_STATE - task never started
766 769 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
767 770 *
768 771 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
769 772 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
770 773 * cases, the acquisition is completely restarted.
771 774 *
772 775 */
773 776
774 777 int status;
775 778
776 779 #ifdef PRINT_TASK_STATISTICS
777 780 rtems_cpu_usage_reset();
778 781 #endif
779 782
780 783 status = RTEMS_UNSATISFIED;
781 784
782 785 switch( lfrCurrentMode )
783 786 {
784 787 case LFR_MODE_STANDBY:
785 788 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
786 789 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
787 790 {
788 791 launch_spectral_matrix( );
789 792 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
790 793 }
791 794 break;
792 795 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
793 796 status = restart_asm_activities( LFR_MODE_SBM1 );
794 797 status = LFR_SUCCESSFUL;
795 798 update_last_valid_transition_date( transitionCoarseTime );
796 799 break;
797 800 case LFR_MODE_BURST:
798 801 status = stop_current_mode(); // stop the current mode
799 802 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
800 803 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
801 804 {
802 805 launch_spectral_matrix( );
803 806 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
804 807 }
805 808 break;
806 809 case LFR_MODE_SBM2:
807 810 status = restart_asm_activities( LFR_MODE_SBM1 );
808 811 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
809 812 update_last_valid_transition_date( transitionCoarseTime );
810 813 break;
811 814 default:
812 815 break;
813 816 }
814 817
815 818 if (status != RTEMS_SUCCESSFUL)
816 819 {
817 820 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status);
818 821 status = RTEMS_UNSATISFIED;
819 822 }
820 823
821 824 return status;
822 825 }
823 826
824 827 int enter_mode_sbm2( unsigned int transitionCoarseTime )
825 828 {
826 829 /** This function is used to start the SBM2 mode.
827 830 *
828 831 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
829 832 *
830 833 * @return RTEMS directive status codes:
831 834 * - RTEMS_SUCCESSFUL - task restarted successfully
832 835 * - RTEMS_INVALID_ID - task id invalid
833 836 * - RTEMS_INCORRECT_STATE - task never started
834 837 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
835 838 *
836 839 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
837 840 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
838 841 * cases, the acquisition is completely restarted.
839 842 *
840 843 */
841 844
842 845 int status;
843 846
844 847 #ifdef PRINT_TASK_STATISTICS
845 848 rtems_cpu_usage_reset();
846 849 #endif
847 850
848 851 status = RTEMS_UNSATISFIED;
849 852
850 853 switch( lfrCurrentMode )
851 854 {
852 855 case LFR_MODE_STANDBY:
853 856 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
854 857 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
855 858 {
856 859 launch_spectral_matrix( );
857 860 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
858 861 }
859 862 break;
860 863 case LFR_MODE_NORMAL:
861 864 status = restart_asm_activities( LFR_MODE_SBM2 );
862 865 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
863 866 update_last_valid_transition_date( transitionCoarseTime );
864 867 break;
865 868 case LFR_MODE_BURST:
866 869 status = stop_current_mode(); // stop the current mode
867 870 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
868 871 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
869 872 {
870 873 launch_spectral_matrix( );
871 874 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
872 875 }
873 876 break;
874 877 case LFR_MODE_SBM1:
875 878 status = restart_asm_activities( LFR_MODE_SBM2 );
876 879 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
877 880 update_last_valid_transition_date( transitionCoarseTime );
878 881 break;
879 882 default:
880 883 break;
881 884 }
882 885
883 886 if (status != RTEMS_SUCCESSFUL)
884 887 {
885 888 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
886 889 status = RTEMS_UNSATISFIED;
887 890 }
888 891
889 892 return status;
890 893 }
891 894
892 895 int restart_science_tasks( unsigned char lfrRequestedMode )
893 896 {
894 897 /** This function is used to restart all science tasks.
895 898 *
896 899 * @return RTEMS directive status codes:
897 900 * - RTEMS_SUCCESSFUL - task restarted successfully
898 901 * - RTEMS_INVALID_ID - task id invalid
899 902 * - RTEMS_INCORRECT_STATE - task never started
900 903 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
901 904 *
902 905 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
903 906 *
904 907 */
905 908
906 909 rtems_status_code status[10];
907 910 rtems_status_code ret;
908 911
909 912 ret = RTEMS_SUCCESSFUL;
910 913
911 914 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
912 915 if (status[0] != RTEMS_SUCCESSFUL)
913 916 {
914 917 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
915 918 }
916 919
917 920 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
918 921 if (status[1] != RTEMS_SUCCESSFUL)
919 922 {
920 923 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
921 924 }
922 925
923 926 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
924 927 if (status[2] != RTEMS_SUCCESSFUL)
925 928 {
926 929 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
927 930 }
928 931
929 932 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
930 933 if (status[3] != RTEMS_SUCCESSFUL)
931 934 {
932 935 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
933 936 }
934 937
935 938 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
936 939 if (status[4] != RTEMS_SUCCESSFUL)
937 940 {
938 941 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
939 942 }
940 943
941 944 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
942 945 if (status[5] != RTEMS_SUCCESSFUL)
943 946 {
944 947 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
945 948 }
946 949
947 950 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
948 951 if (status[6] != RTEMS_SUCCESSFUL)
949 952 {
950 953 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
951 954 }
952 955
953 956 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
954 957 if (status[7] != RTEMS_SUCCESSFUL)
955 958 {
956 959 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
957 960 }
958 961
959 962 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
960 963 if (status[8] != RTEMS_SUCCESSFUL)
961 964 {
962 965 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
963 966 }
964 967
965 968 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
966 969 if (status[9] != RTEMS_SUCCESSFUL)
967 970 {
968 971 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
969 972 }
970 973
971 974 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
972 975 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
973 976 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
974 977 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
975 978 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
976 979 {
977 980 ret = RTEMS_UNSATISFIED;
978 981 }
979 982
980 983 return ret;
981 984 }
982 985
983 986 int restart_asm_tasks( unsigned char lfrRequestedMode )
984 987 {
985 988 /** This function is used to restart average spectral matrices tasks.
986 989 *
987 990 * @return RTEMS directive status codes:
988 991 * - RTEMS_SUCCESSFUL - task restarted successfully
989 992 * - RTEMS_INVALID_ID - task id invalid
990 993 * - RTEMS_INCORRECT_STATE - task never started
991 994 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
992 995 *
993 996 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
994 997 *
995 998 */
996 999
997 1000 rtems_status_code status[6];
998 1001 rtems_status_code ret;
999 1002
1000 1003 ret = RTEMS_SUCCESSFUL;
1001 1004
1002 1005 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
1003 1006 if (status[0] != RTEMS_SUCCESSFUL)
1004 1007 {
1005 1008 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
1006 1009 }
1007 1010
1008 1011 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1009 1012 if (status[1] != RTEMS_SUCCESSFUL)
1010 1013 {
1011 1014 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
1012 1015 }
1013 1016
1014 1017 status[2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1015 1018 if (status[2] != RTEMS_SUCCESSFUL)
1016 1019 {
1017 1020 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[2])
1018 1021 }
1019 1022
1020 1023 status[3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1021 1024 if (status[3] != RTEMS_SUCCESSFUL)
1022 1025 {
1023 1026 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[3])
1024 1027 }
1025 1028
1026 1029 status[4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1027 1030 if (status[4] != RTEMS_SUCCESSFUL)
1028 1031 {
1029 1032 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[4])
1030 1033 }
1031 1034
1032 1035 status[5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1033 1036 if (status[5] != RTEMS_SUCCESSFUL)
1034 1037 {
1035 1038 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[5])
1036 1039 }
1037 1040
1038 1041 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
1039 1042 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
1040 1043 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) )
1041 1044 {
1042 1045 ret = RTEMS_UNSATISFIED;
1043 1046 }
1044 1047
1045 1048 return ret;
1046 1049 }
1047 1050
1048 1051 int suspend_science_tasks( void )
1049 1052 {
1050 1053 /** This function suspends the science tasks.
1051 1054 *
1052 1055 * @return RTEMS directive status codes:
1053 1056 * - RTEMS_SUCCESSFUL - task restarted successfully
1054 1057 * - RTEMS_INVALID_ID - task id invalid
1055 1058 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1056 1059 *
1057 1060 */
1058 1061
1059 1062 rtems_status_code status;
1060 1063
1061 1064 PRINTF("in suspend_science_tasks\n")
1062 1065
1063 1066 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1064 1067 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1065 1068 {
1066 1069 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1067 1070 }
1068 1071 else
1069 1072 {
1070 1073 status = RTEMS_SUCCESSFUL;
1071 1074 }
1072 1075 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1073 1076 {
1074 1077 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1075 1078 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1076 1079 {
1077 1080 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1078 1081 }
1079 1082 else
1080 1083 {
1081 1084 status = RTEMS_SUCCESSFUL;
1082 1085 }
1083 1086 }
1084 1087 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1085 1088 {
1086 1089 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1087 1090 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1088 1091 {
1089 1092 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1090 1093 }
1091 1094 else
1092 1095 {
1093 1096 status = RTEMS_SUCCESSFUL;
1094 1097 }
1095 1098 }
1096 1099 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1097 1100 {
1098 1101 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1099 1102 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1100 1103 {
1101 1104 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1102 1105 }
1103 1106 else
1104 1107 {
1105 1108 status = RTEMS_SUCCESSFUL;
1106 1109 }
1107 1110 }
1108 1111 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1109 1112 {
1110 1113 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1111 1114 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1112 1115 {
1113 1116 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1114 1117 }
1115 1118 else
1116 1119 {
1117 1120 status = RTEMS_SUCCESSFUL;
1118 1121 }
1119 1122 }
1120 1123 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1121 1124 {
1122 1125 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1123 1126 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1124 1127 {
1125 1128 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1126 1129 }
1127 1130 else
1128 1131 {
1129 1132 status = RTEMS_SUCCESSFUL;
1130 1133 }
1131 1134 }
1132 1135 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1133 1136 {
1134 1137 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1135 1138 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1136 1139 {
1137 1140 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1138 1141 }
1139 1142 else
1140 1143 {
1141 1144 status = RTEMS_SUCCESSFUL;
1142 1145 }
1143 1146 }
1144 1147 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1145 1148 {
1146 1149 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1147 1150 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1148 1151 {
1149 1152 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1150 1153 }
1151 1154 else
1152 1155 {
1153 1156 status = RTEMS_SUCCESSFUL;
1154 1157 }
1155 1158 }
1156 1159 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1157 1160 {
1158 1161 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1159 1162 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1160 1163 {
1161 1164 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1162 1165 }
1163 1166 else
1164 1167 {
1165 1168 status = RTEMS_SUCCESSFUL;
1166 1169 }
1167 1170 }
1168 1171 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1169 1172 {
1170 1173 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1171 1174 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1172 1175 {
1173 1176 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1174 1177 }
1175 1178 else
1176 1179 {
1177 1180 status = RTEMS_SUCCESSFUL;
1178 1181 }
1179 1182 }
1180 1183
1181 1184 return status;
1182 1185 }
1183 1186
1184 1187 int suspend_asm_tasks( void )
1185 1188 {
1186 1189 /** This function suspends the science tasks.
1187 1190 *
1188 1191 * @return RTEMS directive status codes:
1189 1192 * - RTEMS_SUCCESSFUL - task restarted successfully
1190 1193 * - RTEMS_INVALID_ID - task id invalid
1191 1194 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1192 1195 *
1193 1196 */
1194 1197
1195 1198 rtems_status_code status;
1196 1199
1197 1200 PRINTF("in suspend_science_tasks\n")
1198 1201
1199 1202 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1200 1203 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1201 1204 {
1202 1205 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1203 1206 }
1204 1207 else
1205 1208 {
1206 1209 status = RTEMS_SUCCESSFUL;
1207 1210 }
1208 1211
1209 1212 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1210 1213 {
1211 1214 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1212 1215 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1213 1216 {
1214 1217 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1215 1218 }
1216 1219 else
1217 1220 {
1218 1221 status = RTEMS_SUCCESSFUL;
1219 1222 }
1220 1223 }
1221 1224
1222 1225 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1223 1226 {
1224 1227 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1225 1228 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1226 1229 {
1227 1230 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1228 1231 }
1229 1232 else
1230 1233 {
1231 1234 status = RTEMS_SUCCESSFUL;
1232 1235 }
1233 1236 }
1234 1237
1235 1238 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1236 1239 {
1237 1240 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1238 1241 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1239 1242 {
1240 1243 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1241 1244 }
1242 1245 else
1243 1246 {
1244 1247 status = RTEMS_SUCCESSFUL;
1245 1248 }
1246 1249 }
1247 1250
1248 1251 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1249 1252 {
1250 1253 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1251 1254 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1252 1255 {
1253 1256 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1254 1257 }
1255 1258 else
1256 1259 {
1257 1260 status = RTEMS_SUCCESSFUL;
1258 1261 }
1259 1262 }
1260 1263
1261 1264 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1262 1265 {
1263 1266 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1264 1267 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1265 1268 {
1266 1269 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1267 1270 }
1268 1271 else
1269 1272 {
1270 1273 status = RTEMS_SUCCESSFUL;
1271 1274 }
1272 1275 }
1273 1276
1274 1277 return status;
1275 1278 }
1276 1279
1277 1280 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1278 1281 {
1279 1282
1280 1283 WFP_reset_current_ring_nodes();
1281 1284
1282 1285 reset_waveform_picker_regs();
1283 1286
1284 1287 set_wfp_burst_enable_register( mode );
1285 1288
1286 1289 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1287 1290 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1288 1291
1289 1292 if (transitionCoarseTime == 0)
1290 1293 {
1291 1294 // instant transition means transition on the next valid date
1292 1295 // this is mandatory to have a good snapshot period and a good correction of the snapshot period
1293 1296 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1294 1297 }
1295 1298 else
1296 1299 {
1297 1300 waveform_picker_regs->start_date = transitionCoarseTime;
1298 1301 }
1299 1302
1300 1303 update_last_valid_transition_date(waveform_picker_regs->start_date);
1301 1304
1302 1305 }
1303 1306
1304 1307 void launch_spectral_matrix( void )
1305 1308 {
1306 1309 SM_reset_current_ring_nodes();
1307 1310
1308 1311 reset_spectral_matrix_regs();
1309 1312
1310 1313 reset_nb_sm();
1311 1314
1312 1315 set_sm_irq_onNewMatrix( 1 );
1313 1316
1314 1317 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1315 1318 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1316 1319
1317 1320 }
1318 1321
1319 1322 void set_sm_irq_onNewMatrix( unsigned char value )
1320 1323 {
1321 1324 if (value == 1)
1322 1325 {
1323 1326 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
1324 1327 }
1325 1328 else
1326 1329 {
1327 1330 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
1328 1331 }
1329 1332 }
1330 1333
1331 1334 void set_sm_irq_onError( unsigned char value )
1332 1335 {
1333 1336 if (value == 1)
1334 1337 {
1335 1338 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
1336 1339 }
1337 1340 else
1338 1341 {
1339 1342 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
1340 1343 }
1341 1344 }
1342 1345
1343 1346 //*****************************
1344 1347 // CONFIGURE CALIBRATION SIGNAL
1345 1348 void setCalibrationPrescaler( unsigned int prescaler )
1346 1349 {
1347 1350 // prescaling of the master clock (25 MHz)
1348 1351 // master clock is divided by 2^prescaler
1349 1352 time_management_regs->calPrescaler = prescaler;
1350 1353 }
1351 1354
1352 1355 void setCalibrationDivisor( unsigned int divisionFactor )
1353 1356 {
1354 1357 // division of the prescaled clock by the division factor
1355 1358 time_management_regs->calDivisor = divisionFactor;
1356 1359 }
1357 1360
1358 1361 void setCalibrationData( void ){
1359 1362 unsigned int k;
1360 1363 unsigned short data;
1361 1364 float val;
1362 1365 float f0;
1363 1366 float f1;
1364 1367 float fs;
1365 1368 float Ts;
1366 1369 float scaleFactor;
1367 1370
1368 1371 f0 = 625;
1369 1372 f1 = 10000;
1370 1373 fs = 160256.410;
1371 1374 Ts = 1. / fs;
1372 1375 scaleFactor = 0.250 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
1373 1376
1374 1377 time_management_regs->calDataPtr = 0x00;
1375 1378
1376 1379 // build the signal for the SCM calibration
1377 1380 for (k=0; k<256; k++)
1378 1381 {
1379 1382 val = sin( 2 * pi * f0 * k * Ts )
1380 1383 + sin( 2 * pi * f1 * k * Ts );
1381 1384 data = (unsigned short) ((val * scaleFactor) + 2048);
1382 1385 time_management_regs->calData = data & 0xfff;
1383 1386 }
1384 1387 }
1385 1388
1386 1389 void setCalibrationDataInterleaved( void ){
1387 1390 unsigned int k;
1388 1391 float val;
1389 1392 float f0;
1390 1393 float f1;
1391 1394 float fs;
1392 1395 float Ts;
1393 1396 unsigned short data[384];
1394 1397 unsigned char *dataPtr;
1395 1398
1396 1399 f0 = 625;
1397 1400 f1 = 10000;
1398 1401 fs = 240384.615;
1399 1402 Ts = 1. / fs;
1400 1403
1401 1404 time_management_regs->calDataPtr = 0x00;
1402 1405
1403 1406 // build the signal for the SCM calibration
1404 1407 for (k=0; k<384; k++)
1405 1408 {
1406 1409 val = sin( 2 * pi * f0 * k * Ts )
1407 1410 + sin( 2 * pi * f1 * k * Ts );
1408 1411 data[k] = (unsigned short) (val * 512 + 2048);
1409 1412 }
1410 1413
1411 1414 // write the signal in interleaved mode
1412 1415 for (k=0; k<128; k++)
1413 1416 {
1414 1417 dataPtr = (unsigned char*) &data[k*3 + 2];
1415 1418 time_management_regs->calData = (data[k*3] & 0xfff)
1416 1419 + ( (dataPtr[0] & 0x3f) << 12);
1417 1420 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
1418 1421 + ( (dataPtr[1] & 0x3f) << 12);
1419 1422 }
1420 1423 }
1421 1424
1422 1425 void setCalibrationReload( bool state)
1423 1426 {
1424 1427 if (state == true)
1425 1428 {
1426 1429 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
1427 1430 }
1428 1431 else
1429 1432 {
1430 1433 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
1431 1434 }
1432 1435 }
1433 1436
1434 1437 void setCalibrationEnable( bool state )
1435 1438 {
1436 1439 // this bit drives the multiplexer
1437 1440 if (state == true)
1438 1441 {
1439 1442 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
1440 1443 }
1441 1444 else
1442 1445 {
1443 1446 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
1444 1447 }
1445 1448 }
1446 1449
1447 1450 void setCalibrationInterleaved( bool state )
1448 1451 {
1449 1452 // this bit drives the multiplexer
1450 1453 if (state == true)
1451 1454 {
1452 1455 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
1453 1456 }
1454 1457 else
1455 1458 {
1456 1459 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
1457 1460 }
1458 1461 }
1459 1462
1460 1463 void setCalibration( bool state )
1461 1464 {
1462 1465 if (state == true)
1463 1466 {
1464 1467 setCalibrationEnable( true );
1465 1468 setCalibrationReload( false );
1466 1469 set_hk_lfr_calib_enable( true );
1467 1470 }
1468 1471 else
1469 1472 {
1470 1473 setCalibrationEnable( false );
1471 1474 setCalibrationReload( true );
1472 1475 set_hk_lfr_calib_enable( false );
1473 1476 }
1474 1477 }
1475 1478
1476 1479 void configureCalibration( bool interleaved )
1477 1480 {
1478 1481 setCalibration( false );
1479 1482 if ( interleaved == true )
1480 1483 {
1481 1484 setCalibrationInterleaved( true );
1482 1485 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1483 1486 setCalibrationDivisor( 26 ); // => 240 384
1484 1487 setCalibrationDataInterleaved();
1485 1488 }
1486 1489 else
1487 1490 {
1488 1491 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1489 1492 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
1490 1493 setCalibrationData();
1491 1494 }
1492 1495 }
1493 1496
1494 1497 //****************
1495 1498 // CLOSING ACTIONS
1496 1499 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1497 1500 {
1498 1501 /** This function is used to update the HK packets statistics after a successful TC execution.
1499 1502 *
1500 1503 * @param TC points to the TC being processed
1501 1504 * @param time is the time used to date the TC execution
1502 1505 *
1503 1506 */
1504 1507
1505 1508 unsigned int val;
1506 1509
1507 1510 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1508 1511 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1509 1512 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
1510 1513 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1511 1514 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
1512 1515 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1513 1516 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
1514 1517 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
1515 1518 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
1516 1519 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
1517 1520 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1518 1521 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1519 1522
1520 1523 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1521 1524 val++;
1522 1525 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1523 1526 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1524 1527 }
1525 1528
1526 1529 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1527 1530 {
1528 1531 /** This function is used to update the HK packets statistics after a TC rejection.
1529 1532 *
1530 1533 * @param TC points to the TC being processed
1531 1534 * @param time is the time used to date the TC rejection
1532 1535 *
1533 1536 */
1534 1537
1535 1538 unsigned int val;
1536 1539
1537 1540 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1538 1541 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1539 1542 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1540 1543 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1541 1544 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1542 1545 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1543 1546 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1544 1547 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1545 1548 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1546 1549 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1547 1550 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1548 1551 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1549 1552
1550 1553 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1551 1554 val++;
1552 1555 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1553 1556 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1554 1557 }
1555 1558
1556 1559 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1557 1560 {
1558 1561 /** This function is the last step of the TC execution workflow.
1559 1562 *
1560 1563 * @param TC points to the TC being processed
1561 1564 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1562 1565 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1563 1566 * @param time is the time used to date the TC execution
1564 1567 *
1565 1568 */
1566 1569
1567 1570 unsigned char requestedMode;
1568 1571
1569 1572 if (result == LFR_SUCCESSFUL)
1570 1573 {
1571 1574 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1572 1575 &
1573 1576 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1574 1577 )
1575 1578 {
1576 1579 send_tm_lfr_tc_exe_success( TC, queue_id );
1577 1580 }
1578 1581 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1579 1582 {
1580 1583 //**********************************
1581 1584 // UPDATE THE LFRMODE LOCAL VARIABLE
1582 1585 requestedMode = TC->dataAndCRC[1];
1583 1586 updateLFRCurrentMode( requestedMode );
1584 1587 }
1585 1588 }
1586 1589 else if (result == LFR_EXE_ERROR)
1587 1590 {
1588 1591 send_tm_lfr_tc_exe_error( TC, queue_id );
1589 1592 }
1590 1593 }
1591 1594
1592 1595 //***************************
1593 1596 // Interrupt Service Routines
1594 1597 rtems_isr commutation_isr1( rtems_vector_number vector )
1595 1598 {
1596 1599 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1597 1600 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1598 1601 }
1599 1602 }
1600 1603
1601 1604 rtems_isr commutation_isr2( rtems_vector_number vector )
1602 1605 {
1603 1606 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1604 1607 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1605 1608 }
1606 1609 }
1607 1610
1608 1611 //****************
1609 1612 // OTHER FUNCTIONS
1610 1613 void updateLFRCurrentMode( unsigned char requestedMode )
1611 1614 {
1612 1615 /** This function updates the value of the global variable lfrCurrentMode.
1613 1616 *
1614 1617 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1615 1618 *
1616 1619 */
1617 1620
1618 1621 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1619 1622 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1620 1623 lfrCurrentMode = requestedMode;
1621 1624 }
1622 1625
1623 1626 void set_lfr_soft_reset( unsigned char value )
1624 1627 {
1625 1628 if (value == 1)
1626 1629 {
1627 1630 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1628 1631 }
1629 1632 else
1630 1633 {
1631 1634 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1632 1635 }
1633 1636 }
1634 1637
1635 1638 void reset_lfr( void )
1636 1639 {
1637 1640 set_lfr_soft_reset( 1 );
1638 1641
1639 1642 set_lfr_soft_reset( 0 );
1640 1643
1641 1644 set_hk_lfr_sc_potential_flag( true );
1642 1645 }
Show file before | Show file after | Hide comments
@@ -1,1639 +1,1623
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;
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2;
19 19 ring_node kcoefficient_node_1;
20 20 ring_node kcoefficient_node_2;
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+10, 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+10, 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+10, 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+10, 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+10, 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*0.25) ) - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25));
193 193 if (aux > FLOAT_EQUAL_ZERO)
194 194 {
195 195 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
196 196 flag = LFR_DEFAULT;
197 197 }
198 198 }
199 199
200 200 // SET THE PARAMETERS
201 201 if (flag == LFR_SUCCESSFUL)
202 202 {
203 203 flag = set_sy_lfr_s1_bp_p0( TC );
204 204 flag = set_sy_lfr_s1_bp_p1( TC );
205 205 }
206 206
207 207 return flag;
208 208 }
209 209
210 210 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
211 211 {
212 212 /** This function updates the LFR registers with the incoming sbm2 parameters.
213 213 *
214 214 * @param TC points to the TeleCommand packet that is being processed
215 215 * @param queue_id is the id of the queue which handles TM related to this execution step
216 216 *
217 217 */
218 218
219 219 int flag;
220 220 rtems_status_code status;
221 221 unsigned char sy_lfr_s2_bp_p0;
222 222 unsigned char sy_lfr_s2_bp_p1;
223 223 float aux;
224 224
225 225 flag = LFR_SUCCESSFUL;
226 226
227 227 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
228 228 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
229 229 flag = LFR_DEFAULT;
230 230 }
231 231
232 232 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
233 233 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
234 234
235 235 // sy_lfr_s2_bp_p0
236 236 if (flag == LFR_SUCCESSFUL)
237 237 {
238 238 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
239 239 {
240 240 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
241 241 flag = WRONG_APP_DATA;
242 242 }
243 243 }
244 244 // sy_lfr_s2_bp_p1
245 245 if (flag == LFR_SUCCESSFUL)
246 246 {
247 247 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
248 248 {
249 249 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
250 250 flag = WRONG_APP_DATA;
251 251 }
252 252 }
253 253 //******************************************************************
254 254 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
255 255 if (flag == LFR_SUCCESSFUL)
256 256 {
257 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
258 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
259 259 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
260 260 if (aux > FLOAT_EQUAL_ZERO)
261 261 {
262 262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
263 263 flag = LFR_DEFAULT;
264 264 }
265 265 }
266 266
267 267 // SET THE PARAMETERS
268 268 if (flag == LFR_SUCCESSFUL)
269 269 {
270 270 flag = set_sy_lfr_s2_bp_p0( TC );
271 271 flag = set_sy_lfr_s2_bp_p1( TC );
272 272 }
273 273
274 274 return flag;
275 275 }
276 276
277 277 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
278 278 {
279 279 /** This function updates the LFR registers with the incoming sbm2 parameters.
280 280 *
281 281 * @param TC points to the TeleCommand packet that is being processed
282 282 * @param queue_id is the id of the queue which handles TM related to this execution step
283 283 *
284 284 */
285 285
286 286 int flag;
287 287
288 288 flag = LFR_DEFAULT;
289 289
290 290 flag = set_sy_lfr_kcoeff( TC, queue_id );
291 291
292 292 return flag;
293 293 }
294 294
295 295 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 296 {
297 297 /** This function updates the LFR registers with the incoming sbm2 parameters.
298 298 *
299 299 * @param TC points to the TeleCommand packet that is being processed
300 300 * @param queue_id is the id of the queue which handles TM related to this execution step
301 301 *
302 302 */
303 303
304 304 int flag;
305 305
306 306 flag = LFR_DEFAULT;
307 307
308 308 flag = set_sy_lfr_fbins( TC );
309 309
310 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
311 merge_fbins_masks();
312
310 313 return flag;
311 314 }
312 315
313 316 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
314 317 {
315 318 /** This function updates the LFR registers with the incoming sbm2 parameters.
316 319 *
317 320 * @param TC points to the TeleCommand packet that is being processed
318 321 * @param queue_id is the id of the queue which handles TM related to this execution step
319 322 *
320 323 */
321 324
322 325 int flag;
323 326
324 327 flag = LFR_DEFAULT;
325 328
326 329 flag = check_sy_lfr_filter_parameters( TC, queue_id );
327 330
328 331 if (flag == LFR_SUCCESSFUL)
329 332 {
330 333 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
331 334 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
332 335 parameter_dump_packet.sy_lfr_pas_filter_tbad[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 0 ];
333 336 parameter_dump_packet.sy_lfr_pas_filter_tbad[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 1 ];
334 337 parameter_dump_packet.sy_lfr_pas_filter_tbad[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 2 ];
335 338 parameter_dump_packet.sy_lfr_pas_filter_tbad[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 3 ];
336 339 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
337 340 parameter_dump_packet.sy_lfr_pas_filter_shift[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 0 ];
338 341 parameter_dump_packet.sy_lfr_pas_filter_shift[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 1 ];
339 342 parameter_dump_packet.sy_lfr_pas_filter_shift[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 2 ];
340 343 parameter_dump_packet.sy_lfr_pas_filter_shift[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 3 ];
341 344 parameter_dump_packet.sy_lfr_sc_rw_delta_f[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 0 ];
342 345 parameter_dump_packet.sy_lfr_sc_rw_delta_f[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 1 ];
343 346 parameter_dump_packet.sy_lfr_sc_rw_delta_f[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 2 ];
344 347 parameter_dump_packet.sy_lfr_sc_rw_delta_f[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 3 ];
345 348
346 349 //****************************
347 350 // store PAS filter parameters
348 351 // sy_lfr_pas_filter_enabled
349 352 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
350 353 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & 0x01 );
351 354 // sy_lfr_pas_filter_modulus
352 355 filterPar.sy_lfr_pas_filter_modulus = parameter_dump_packet.sy_lfr_pas_filter_modulus;
353 356 // sy_lfr_pas_filter_tbad
354 357 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
355 358 parameter_dump_packet.sy_lfr_pas_filter_tbad );
356 359 // sy_lfr_pas_filter_offset
357 360 filterPar.sy_lfr_pas_filter_offset = parameter_dump_packet.sy_lfr_pas_filter_offset;
358 361 // sy_lfr_pas_filter_shift
359 362 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
360 363 parameter_dump_packet.sy_lfr_pas_filter_shift );
361 364
362 365 //****************************************************
363 366 // store the parameter sy_lfr_sc_rw_delta_f as a float
364 367 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
365 368 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
366 369 }
367 370
368 371 return flag;
369 372 }
370 373
371 374 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
372 375 {
373 376 /** This function updates the LFR registers with the incoming sbm2 parameters.
374 377 *
375 378 * @param TC points to the TeleCommand packet that is being processed
376 379 * @param queue_id is the id of the queue which handles TM related to this execution step
377 380 *
378 381 */
379 382
380 383 unsigned int address;
381 384 rtems_status_code status;
382 385 unsigned int freq;
383 386 unsigned int bin;
384 387 unsigned int coeff;
385 388 unsigned char *kCoeffPtr;
386 389 unsigned char *kCoeffDumpPtr;
387 390
388 391 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
389 392 // F0 => 11 bins
390 393 // F1 => 13 bins
391 394 // F2 => 12 bins
392 395 // 36 bins to dump in two packets (30 bins max per packet)
393 396
394 397 //*********
395 398 // PACKET 1
396 399 // 11 F0 bins, 13 F1 bins and 6 F2 bins
397 400 kcoefficients_dump_1.destinationID = TC->sourceID;
398 401 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
399 402 for( freq=0;
400 403 freq<NB_BINS_COMPRESSED_SM_F0;
401 404 freq++ )
402 405 {
403 406 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
404 407 bin = freq;
405 408 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
406 409 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
407 410 {
408 411 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
409 412 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
410 413 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
411 414 }
412 415 }
413 416 for( freq=NB_BINS_COMPRESSED_SM_F0;
414 417 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
415 418 freq++ )
416 419 {
417 420 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
418 421 bin = freq - NB_BINS_COMPRESSED_SM_F0;
419 422 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
420 423 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
421 424 {
422 425 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
423 426 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
424 427 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
425 428 }
426 429 }
427 430 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
428 431 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
429 432 freq++ )
430 433 {
431 434 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
432 435 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
433 436 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
434 437 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
435 438 {
436 439 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
437 440 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
438 441 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
439 442 }
440 443 }
441 444 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
442 445 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
443 446 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
444 447 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
445 448 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
446 449 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
447 450 // SEND DATA
448 451 kcoefficient_node_1.status = 1;
449 452 address = (unsigned int) &kcoefficient_node_1;
450 453 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
451 454 if (status != RTEMS_SUCCESSFUL) {
452 455 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
453 456 }
454 457
455 458 //********
456 459 // PACKET 2
457 460 // 6 F2 bins
458 461 kcoefficients_dump_2.destinationID = TC->sourceID;
459 462 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
460 463 for( freq=0; freq<6; freq++ )
461 464 {
462 465 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
463 466 bin = freq + 6;
464 467 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
465 468 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
466 469 {
467 470 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
468 471 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
469 472 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
470 473 }
471 474 }
472 475 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
473 476 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
474 477 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
475 478 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
476 479 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
477 480 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
478 481 // SEND DATA
479 482 kcoefficient_node_2.status = 1;
480 483 address = (unsigned int) &kcoefficient_node_2;
481 484 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
482 485 if (status != RTEMS_SUCCESSFUL) {
483 486 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
484 487 }
485 488
486 489 return status;
487 490 }
488 491
489 492 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
490 493 {
491 494 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
492 495 *
493 496 * @param queue_id is the id of the queue which handles TM related to this execution step.
494 497 *
495 498 * @return RTEMS directive status codes:
496 499 * - RTEMS_SUCCESSFUL - message sent successfully
497 500 * - RTEMS_INVALID_ID - invalid queue id
498 501 * - RTEMS_INVALID_SIZE - invalid message size
499 502 * - RTEMS_INVALID_ADDRESS - buffer is NULL
500 503 * - RTEMS_UNSATISFIED - out of message buffers
501 504 * - RTEMS_TOO_MANY - queue s limit has been reached
502 505 *
503 506 */
504 507
505 508 int status;
506 int k;
507 509
508 510 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
509 511 parameter_dump_packet.destinationID = TC->sourceID;
510 512
511 513 // UPDATE TIME
512 514 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
513 515 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
514 516 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
515 517 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
516 518 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
517 519 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
518 520 // SEND DATA
519 printf("f0\n");
520 for (k = 0; k<16; k++)
521 {
522 printf("%x ", parameter_dump_packet.sy_lfr_rw_mask.fx.f0_word1[k]);
523 }
524 printf("\n");
525 printf("f1\n");
526 for (k = 0; k<16; k++)
527 {
528 printf("%x ", parameter_dump_packet.sy_lfr_rw_mask.fx.f1_word1[k]);
529 }
530 printf("\n");
531 printf("f2\n");
532 for (k = 0; k<16; k++)
533 {
534 printf("%x ", parameter_dump_packet.sy_lfr_rw_mask.fx.f2_word1[k]);
535 }
536 printf("\n");
537
538 521 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
539 522 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
540 523 if (status != RTEMS_SUCCESSFUL) {
541 524 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
542 525 }
543 526
544 527 return status;
545 528 }
546 529
547 530 //***********************
548 531 // NORMAL MODE PARAMETERS
549 532
550 533 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
551 534 {
552 535 unsigned char msb;
553 536 unsigned char lsb;
554 537 int flag;
555 538 float aux;
556 539 rtems_status_code status;
557 540
558 541 unsigned int sy_lfr_n_swf_l;
559 542 unsigned int sy_lfr_n_swf_p;
560 543 unsigned int sy_lfr_n_asm_p;
561 544 unsigned char sy_lfr_n_bp_p0;
562 545 unsigned char sy_lfr_n_bp_p1;
563 546 unsigned char sy_lfr_n_cwf_long_f3;
564 547
565 548 flag = LFR_SUCCESSFUL;
566 549
567 550 //***************
568 551 // get parameters
569 552 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
570 553 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
571 554 sy_lfr_n_swf_l = msb * 256 + lsb;
572 555
573 556 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
574 557 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
575 558 sy_lfr_n_swf_p = msb * 256 + lsb;
576 559
577 560 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
578 561 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
579 562 sy_lfr_n_asm_p = msb * 256 + lsb;
580 563
581 564 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
582 565
583 566 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
584 567
585 568 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
586 569
587 570 //******************
588 571 // check consistency
589 572 // sy_lfr_n_swf_l
590 573 if (sy_lfr_n_swf_l != 2048)
591 574 {
592 575 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
593 576 flag = WRONG_APP_DATA;
594 577 }
595 578 // sy_lfr_n_swf_p
596 579 if (flag == LFR_SUCCESSFUL)
597 580 {
598 581 if ( sy_lfr_n_swf_p < 22 )
599 582 {
600 583 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
601 584 flag = WRONG_APP_DATA;
602 585 }
603 586 }
604 587 // sy_lfr_n_bp_p0
605 588 if (flag == LFR_SUCCESSFUL)
606 589 {
607 590 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
608 591 {
609 592 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
610 593 flag = WRONG_APP_DATA;
611 594 }
612 595 }
613 596 // sy_lfr_n_asm_p
614 597 if (flag == LFR_SUCCESSFUL)
615 598 {
616 599 if (sy_lfr_n_asm_p == 0)
617 600 {
618 601 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
619 602 flag = WRONG_APP_DATA;
620 603 }
621 604 }
622 605 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
623 606 if (flag == LFR_SUCCESSFUL)
624 607 {
625 608 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
626 609 if (aux > FLOAT_EQUAL_ZERO)
627 610 {
628 611 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
629 612 flag = WRONG_APP_DATA;
630 613 }
631 614 }
632 615 // sy_lfr_n_bp_p1
633 616 if (flag == LFR_SUCCESSFUL)
634 617 {
635 618 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
636 619 {
637 620 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
638 621 flag = WRONG_APP_DATA;
639 622 }
640 623 }
641 624 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
642 625 if (flag == LFR_SUCCESSFUL)
643 626 {
644 627 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
645 628 if (aux > FLOAT_EQUAL_ZERO)
646 629 {
647 630 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
648 631 flag = LFR_DEFAULT;
649 632 }
650 633 }
651 634 // sy_lfr_n_cwf_long_f3
652 635
653 636 return flag;
654 637 }
655 638
656 639 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
657 640 {
658 641 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
659 642 *
660 643 * @param TC points to the TeleCommand packet that is being processed
661 644 * @param queue_id is the id of the queue which handles TM related to this execution step
662 645 *
663 646 */
664 647
665 648 int result;
666 649
667 650 result = LFR_SUCCESSFUL;
668 651
669 652 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
670 653 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
671 654
672 655 return result;
673 656 }
674 657
675 658 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
676 659 {
677 660 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
678 661 *
679 662 * @param TC points to the TeleCommand packet that is being processed
680 663 * @param queue_id is the id of the queue which handles TM related to this execution step
681 664 *
682 665 */
683 666
684 667 int result;
685 668
686 669 result = LFR_SUCCESSFUL;
687 670
688 671 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
689 672 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
690 673
691 674 return result;
692 675 }
693 676
694 677 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
695 678 {
696 679 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
697 680 *
698 681 * @param TC points to the TeleCommand packet that is being processed
699 682 * @param queue_id is the id of the queue which handles TM related to this execution step
700 683 *
701 684 */
702 685
703 686 int result;
704 687
705 688 result = LFR_SUCCESSFUL;
706 689
707 690 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
708 691 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
709 692
710 693 return result;
711 694 }
712 695
713 696 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
714 697 {
715 698 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
716 699 *
717 700 * @param TC points to the TeleCommand packet that is being processed
718 701 * @param queue_id is the id of the queue which handles TM related to this execution step
719 702 *
720 703 */
721 704
722 705 int status;
723 706
724 707 status = LFR_SUCCESSFUL;
725 708
726 709 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
727 710
728 711 return status;
729 712 }
730 713
731 714 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
732 715 {
733 716 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
734 717 *
735 718 * @param TC points to the TeleCommand packet that is being processed
736 719 * @param queue_id is the id of the queue which handles TM related to this execution step
737 720 *
738 721 */
739 722
740 723 int status;
741 724
742 725 status = LFR_SUCCESSFUL;
743 726
744 727 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
745 728
746 729 return status;
747 730 }
748 731
749 732 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
750 733 {
751 734 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
752 735 *
753 736 * @param TC points to the TeleCommand packet that is being processed
754 737 * @param queue_id is the id of the queue which handles TM related to this execution step
755 738 *
756 739 */
757 740
758 741 int status;
759 742
760 743 status = LFR_SUCCESSFUL;
761 744
762 745 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
763 746
764 747 return status;
765 748 }
766 749
767 750 //**********************
768 751 // BURST MODE PARAMETERS
769 752 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
770 753 {
771 754 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
772 755 *
773 756 * @param TC points to the TeleCommand packet that is being processed
774 757 * @param queue_id is the id of the queue which handles TM related to this execution step
775 758 *
776 759 */
777 760
778 761 int status;
779 762
780 763 status = LFR_SUCCESSFUL;
781 764
782 765 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
783 766
784 767 return status;
785 768 }
786 769
787 770 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
788 771 {
789 772 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
790 773 *
791 774 * @param TC points to the TeleCommand packet that is being processed
792 775 * @param queue_id is the id of the queue which handles TM related to this execution step
793 776 *
794 777 */
795 778
796 779 int status;
797 780
798 781 status = LFR_SUCCESSFUL;
799 782
800 783 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
801 784
802 785 return status;
803 786 }
804 787
805 788 //*********************
806 789 // SBM1 MODE PARAMETERS
807 790 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
808 791 {
809 792 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
810 793 *
811 794 * @param TC points to the TeleCommand packet that is being processed
812 795 * @param queue_id is the id of the queue which handles TM related to this execution step
813 796 *
814 797 */
815 798
816 799 int status;
817 800
818 801 status = LFR_SUCCESSFUL;
819 802
820 803 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
821 804
822 805 return status;
823 806 }
824 807
825 808 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
826 809 {
827 810 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
828 811 *
829 812 * @param TC points to the TeleCommand packet that is being processed
830 813 * @param queue_id is the id of the queue which handles TM related to this execution step
831 814 *
832 815 */
833 816
834 817 int status;
835 818
836 819 status = LFR_SUCCESSFUL;
837 820
838 821 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
839 822
840 823 return status;
841 824 }
842 825
843 826 //*********************
844 827 // SBM2 MODE PARAMETERS
845 828 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
846 829 {
847 830 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
848 831 *
849 832 * @param TC points to the TeleCommand packet that is being processed
850 833 * @param queue_id is the id of the queue which handles TM related to this execution step
851 834 *
852 835 */
853 836
854 837 int status;
855 838
856 839 status = LFR_SUCCESSFUL;
857 840
858 841 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
859 842
860 843 return status;
861 844 }
862 845
863 846 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
864 847 {
865 848 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
866 849 *
867 850 * @param TC points to the TeleCommand packet that is being processed
868 851 * @param queue_id is the id of the queue which handles TM related to this execution step
869 852 *
870 853 */
871 854
872 855 int status;
873 856
874 857 status = LFR_SUCCESSFUL;
875 858
876 859 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
877 860
878 861 return status;
879 862 }
880 863
881 864 //*******************
882 865 // TC_LFR_UPDATE_INFO
883 866 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
884 867 {
885 868 unsigned int status;
886 869
887 870 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
888 871 || (mode == LFR_MODE_BURST)
889 872 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
890 873 {
891 874 status = LFR_SUCCESSFUL;
892 875 }
893 876 else
894 877 {
895 878 status = LFR_DEFAULT;
896 879 }
897 880
898 881 return status;
899 882 }
900 883
901 884 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
902 885 {
903 886 unsigned int status;
904 887
905 888 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
906 889 || (mode == TDS_MODE_BURST)
907 890 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
908 891 || (mode == TDS_MODE_LFM))
909 892 {
910 893 status = LFR_SUCCESSFUL;
911 894 }
912 895 else
913 896 {
914 897 status = LFR_DEFAULT;
915 898 }
916 899
917 900 return status;
918 901 }
919 902
920 903 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
921 904 {
922 905 unsigned int status;
923 906
924 907 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
925 908 || (mode == THR_MODE_BURST))
926 909 {
927 910 status = LFR_SUCCESSFUL;
928 911 }
929 912 else
930 913 {
931 914 status = LFR_DEFAULT;
932 915 }
933 916
934 917 return status;
935 918 }
936 919
937 920 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
938 921 {
939 922 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
940 923 *
941 924 * @param TC points to the TeleCommand packet that is being processed
942 925 *
943 926 */
944 927
945 928 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
946 929
947 930 bytePosPtr = (unsigned char *) &TC->packetID;
948 931
949 932 // cp_rpw_sc_rw1_f1
950 933 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1,
951 934 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
952 935
953 936 // cp_rpw_sc_rw1_f2
954 937 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2,
955 938 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
956 939
957 940 // cp_rpw_sc_rw2_f1
958 941 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1,
959 942 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
960 943
961 944 // cp_rpw_sc_rw2_f2
962 945 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2,
963 946 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
964 947
965 948 // cp_rpw_sc_rw3_f1
966 949 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1,
967 950 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
968 951
969 952 // cp_rpw_sc_rw3_f2
970 953 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2,
971 954 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
972 955
973 956 // cp_rpw_sc_rw4_f1
974 957 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1,
975 958 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
976 959
977 960 // cp_rpw_sc_rw4_f2
978 961 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2,
979 962 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
980 963 }
981 964
982 965 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag )
983 966 {
984 967 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
985 968 *
986 969 * @param fbins_mask
987 970 * @param rw_f is the reaction wheel frequency to filter
988 971 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
989 972 * @param flag [true] filtering enabled [false] filtering disabled
990 973 *
991 974 * @return void
992 975 *
993 976 */
994 977
995 978 float f_RW_min;
996 979 float f_RW_MAX;
997 980 float fi_min;
998 981 float fi_MAX;
999 982 float fi;
1000 983 float deltaBelow;
1001 984 float deltaAbove;
1002 985 int binBelow;
1003 986 int binAbove;
1004 987 int closestBin;
1005 988 unsigned int whichByte;
1006 989 int selectedByte;
1007 990 int bin;
1008 991 int binToRemove[3];
1009 992 int k;
1010 993
1011 994 whichByte = 0;
1012 995 bin = 0;
1013 996
1014 997 binToRemove[0] = -1;
1015 998 binToRemove[1] = -1;
1016 999 binToRemove[2] = -1;
1017 1000
1018 1001 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1019 1002 f_RW_min = rw_f - filterPar.sy_lfr_sc_rw_delta_f / 2.;
1020 1003 f_RW_MAX = rw_f + filterPar.sy_lfr_sc_rw_delta_f / 2.;
1021 1004
1022 1005 // compute the index of the frequency bin immediately below rw_f
1023 1006 binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) );
1024 1007 deltaBelow = rw_f - binBelow * deltaFreq;
1025 1008
1026 1009 // compute the index of the frequency bin immediately above rw_f
1027 1010 binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) );
1028 1011 deltaAbove = binAbove * deltaFreq - rw_f;
1029 1012
1030 1013 // search the closest bin
1031 1014 if (deltaAbove > deltaBelow)
1032 1015 {
1033 1016 closestBin = binBelow;
1034 1017 }
1035 1018 else
1036 1019 {
1037 1020 closestBin = binAbove;
1038 1021 }
1039 1022
1040 1023 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1041 1024 fi = closestBin * deltaFreq;
1042 1025 fi_min = fi - (deltaFreq * 0.285);
1043 1026 fi_MAX = fi + (deltaFreq * 0.285);
1044 1027
1045 1028 //**************************************************************************************
1046 1029 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1047 1030 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1048 1031 //**************************************************************************************
1049 1032
1050 1033 // 1. IF [ f_RW_min, f_RW_MAX] is included in [ fi_min; fi_MAX ]
1051 1034 // => remove f_(i), f_(i-1) and f_(i+1)
1052 1035 if ( ( f_RW_min > fi_min ) && ( f_RW_MAX < fi_MAX ) )
1053 1036 {
1054 1037 binToRemove[0] = (closestBin - 1) - 1;
1055 1038 binToRemove[1] = (closestBin) - 1;
1056 1039 binToRemove[2] = (closestBin + 1) - 1;
1057 1040 }
1058 1041 // 2. ELSE
1059 1042 // => remove the two f_(i) which are around f_RW
1060 1043 else
1061 1044 {
1062 1045 binToRemove[0] = (binBelow) - 1;
1063 1046 binToRemove[1] = (binAbove) - 1;
1064 1047 binToRemove[2] = (-1);
1065 1048 }
1066 1049
1067 1050 for (k = 0; k < 3; k++)
1068 1051 {
1069 1052 bin = binToRemove[k];
1070 1053 if ( (bin >= 0) && (bin <= 127) )
1071 1054 {
1072 1055 if (flag == 1)
1073 1056 {
1074 1057 whichByte = (bin >> 3); // division by 8
1075 1058 selectedByte = ( 1 << (bin - (whichByte * 8)) );
1076 1059 fbins_mask[15 - whichByte] = fbins_mask[15 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1077 1060 }
1078 1061 }
1079 1062 }
1080 1063 }
1081 1064
1082 1065 void build_sy_lfr_rw_mask( unsigned int channel )
1083 1066 {
1084 1067 unsigned char local_rw_fbins_mask[16];
1085 1068 unsigned char *maskPtr;
1086 1069 double deltaF;
1087 1070 unsigned k;
1088 1071
1089 1072 k = 0;
1090 1073
1091 1074 maskPtr = NULL;
1092 1075 deltaF = 1.;
1093 1076
1094 1077 switch (channel)
1095 1078 {
1096 1079 case 0:
1097 1080 maskPtr = parameter_dump_packet.sy_lfr_rw_mask.fx.f0_word1;
1098 1081 deltaF = 96.;
1099 1082 break;
1100 1083 case 1:
1101 1084 maskPtr = parameter_dump_packet.sy_lfr_rw_mask.fx.f1_word1;
1102 1085 deltaF = 16.;
1103 1086 break;
1104 1087 case 2:
1105 1088 maskPtr = parameter_dump_packet.sy_lfr_rw_mask.fx.f2_word1;
1106 1089 deltaF = 1.;
1107 1090 break;
1108 1091 default:
1109 1092 break;
1110 1093 }
1111 1094
1112 1095 for (k = 0; k < 16; k++)
1113 1096 {
1114 1097 local_rw_fbins_mask[k] = 0xff;
1115 1098 }
1116 1099
1117 1100 // RW1 F1
1118 1101 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x80) >> 7 ); // [1000 0000]
1119 1102
1120 1103 // RW1 F2
1121 1104 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x40) >> 6 ); // [0100 0000]
1122 1105
1123 1106 // RW2 F1
1124 1107 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x20) >> 5 ); // [0010 0000]
1125 1108
1126 1109 // RW2 F2
1127 1110 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x10) >> 4 ); // [0001 0000]
1128 1111
1129 1112 // RW3 F1
1130 1113 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x08) >> 3 ); // [0000 1000]
1131 1114
1132 1115 // RW3 F2
1133 1116 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x04) >> 2 ); // [0000 0100]
1134 1117
1135 1118 // RW4 F1
1136 1119 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x02) >> 1 ); // [0000 0010]
1137 1120
1138 1121 // RW4 F2
1139 1122 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x01) ); // [0000 0001]
1140 1123
1141 1124 // update the value of the fbins related to reaction wheels frequency filtering
1142 1125 if (maskPtr != NULL)
1143 1126 {
1144 1127 for (k = 0; k < 16; k++)
1145 1128 {
1146 1129 maskPtr[k] = local_rw_fbins_mask[k];
1147 1130 }
1148 1131 }
1149 1132 }
1150 1133
1151 1134 void build_sy_lfr_rw_masks( void )
1152 1135 {
1153 1136 build_sy_lfr_rw_mask( 0 );
1154 1137 build_sy_lfr_rw_mask( 1 );
1155 1138 build_sy_lfr_rw_mask( 2 );
1156
1157 merge_fbins_masks();
1158 1139 }
1159 1140
1160 1141 void merge_fbins_masks( void )
1161 1142 {
1162 1143 unsigned char k;
1163 1144
1164 1145 unsigned char *fbins_f0;
1165 1146 unsigned char *fbins_f1;
1166 1147 unsigned char *fbins_f2;
1167 1148 unsigned char *rw_mask_f0;
1168 1149 unsigned char *rw_mask_f1;
1169 1150 unsigned char *rw_mask_f2;
1170 1151
1171 1152 fbins_f0 = parameter_dump_packet.sy_lfr_fbins.fx.f0_word1;
1172 1153 fbins_f1 = parameter_dump_packet.sy_lfr_fbins.fx.f1_word1;
1173 1154 fbins_f2 = parameter_dump_packet.sy_lfr_fbins.fx.f2_word1;
1174 1155 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask.fx.f0_word1;
1175 1156 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask.fx.f1_word1;
1176 1157 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask.fx.f2_word1;
1177 1158
1178 1159 for( k=0; k < 16; k++ )
1179 1160 {
1180 1161 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1181 1162 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1182 1163 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1183 1164 }
1184 1165 }
1185 1166
1186 1167 //***********
1187 1168 // FBINS MASK
1188 1169
1189 1170 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1190 1171 {
1191 1172 int status;
1192 1173 unsigned int k;
1193 1174 unsigned char *fbins_mask_dump;
1194 1175 unsigned char *fbins_mask_TC;
1195 1176
1196 1177 status = LFR_SUCCESSFUL;
1197 1178
1198 1179 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins.raw;
1199 1180 fbins_mask_TC = TC->dataAndCRC;
1200 1181
1201 1182 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1202 1183 {
1203 1184 fbins_mask_dump[k] = fbins_mask_TC[k];
1204 1185 }
1205 1186
1206 1187 return status;
1207 1188 }
1208 1189
1209 1190 //***************************
1210 1191 // TC_LFR_LOAD_PAS_FILTER_PAR
1211 1192
1212 1193 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1213 1194 {
1214 1195 int flag;
1215 1196 rtems_status_code status;
1216 1197
1217 1198 unsigned char sy_lfr_pas_filter_enabled;
1218 1199 unsigned char sy_lfr_pas_filter_modulus;
1219 1200 float sy_lfr_pas_filter_tbad;
1220 1201 unsigned char sy_lfr_pas_filter_offset;
1221 1202 float sy_lfr_pas_filter_shift;
1222 1203 float sy_lfr_sc_rw_delta_f;
1223 1204 char *parPtr;
1224 1205
1225 1206 flag = LFR_SUCCESSFUL;
1226 1207 sy_lfr_pas_filter_tbad = 0.0;
1227 1208 sy_lfr_pas_filter_shift = 0.0;
1228 1209 sy_lfr_sc_rw_delta_f = 0.0;
1229 1210 parPtr = NULL;
1230 1211
1231 1212 //***************
1232 1213 // get parameters
1233 1214 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & 0x01; // [0000 0001]
1234 1215 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1235 1216 copyFloatByChar(
1236 1217 (unsigned char*) &sy_lfr_pas_filter_tbad,
1237 1218 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1238 1219 );
1239 1220 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1240 1221 copyFloatByChar(
1241 1222 (unsigned char*) &sy_lfr_pas_filter_shift,
1242 1223 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1243 1224 );
1244 1225 copyFloatByChar(
1245 1226 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1246 1227 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1247 1228 );
1248 1229
1249 1230 //******************
1250 1231 // CHECK CONSISTENCY
1251 1232
1252 1233 //**************************
1253 1234 // sy_lfr_pas_filter_enabled
1254 1235 // nothing to check, value is 0 or 1
1255 1236
1256 1237 //**************************
1257 1238 // sy_lfr_pas_filter_modulus
1258 1239 if ( (sy_lfr_pas_filter_modulus < 4) || (sy_lfr_pas_filter_modulus > 8) )
1259 1240 {
1260 1241 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS+10, sy_lfr_pas_filter_modulus );
1261 1242 flag = WRONG_APP_DATA;
1262 1243 }
1263 1244
1264 1245 //***********************
1265 1246 // sy_lfr_pas_filter_tbad
1266 1247 if ( (sy_lfr_pas_filter_tbad < 0.0) || (sy_lfr_pas_filter_tbad > 4.0) )
1267 1248 {
1268 1249 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1269 1250 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD+10, parPtr[3] );
1270 1251 flag = WRONG_APP_DATA;
1271 1252 }
1272 1253
1273 1254 //*************************
1274 1255 // sy_lfr_pas_filter_offset
1275 1256 if (flag == LFR_SUCCESSFUL)
1276 1257 {
1277 1258 if ( (sy_lfr_pas_filter_offset < 0) || (sy_lfr_pas_filter_offset > 7) )
1278 1259 {
1279 1260 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET+10, sy_lfr_pas_filter_offset );
1280 1261 flag = WRONG_APP_DATA;
1281 1262 }
1282 1263 }
1283 1264
1284 1265 //************************
1285 1266 // sy_lfr_pas_filter_shift
1286 1267 if ( (sy_lfr_pas_filter_shift < 0.0) || (sy_lfr_pas_filter_shift > 1.0) )
1287 1268 {
1288 1269 parPtr = (char*) &sy_lfr_pas_filter_shift;
1289 1270 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT+10, parPtr[3] );
1290 1271 flag = WRONG_APP_DATA;
1291 1272 }
1292 1273
1293 1274 //*********************
1294 1275 // sy_lfr_sc_rw_delta_f
1295 1276 // nothing to check, no default value in the ICD
1296 1277
1297 1278 return flag;
1298 1279 }
1299 1280
1300 1281 //**************
1301 1282 // KCOEFFICIENTS
1302 1283 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1303 1284 {
1304 1285 unsigned int kcoeff;
1305 1286 unsigned short sy_lfr_kcoeff_frequency;
1306 1287 unsigned short bin;
1307 1288 unsigned short *freqPtr;
1308 1289 float *kcoeffPtr_norm;
1309 1290 float *kcoeffPtr_sbm;
1310 1291 int status;
1311 1292 unsigned char *kcoeffLoadPtr;
1312 1293 unsigned char *kcoeffNormPtr;
1313 1294 unsigned char *kcoeffSbmPtr_a;
1314 1295 unsigned char *kcoeffSbmPtr_b;
1315 1296
1316 1297 status = LFR_SUCCESSFUL;
1317 1298
1318 1299 kcoeffPtr_norm = NULL;
1319 1300 kcoeffPtr_sbm = NULL;
1320 1301 bin = 0;
1321 1302
1322 1303 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
1323 1304 sy_lfr_kcoeff_frequency = *freqPtr;
1324 1305
1325 1306 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1326 1307 {
1327 1308 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1328 1309 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 10 + 1,
1329 1310 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1330 1311 status = LFR_DEFAULT;
1331 1312 }
1332 1313 else
1333 1314 {
1334 1315 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1335 1316 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1336 1317 {
1337 1318 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1338 1319 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1339 1320 bin = sy_lfr_kcoeff_frequency;
1340 1321 }
1341 1322 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1342 1323 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1343 1324 {
1344 1325 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1345 1326 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1346 1327 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1347 1328 }
1348 1329 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1349 1330 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1350 1331 {
1351 1332 kcoeffPtr_norm = k_coeff_intercalib_f2;
1352 1333 kcoeffPtr_sbm = NULL;
1353 1334 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1354 1335 }
1355 1336 }
1356 1337
1357 1338 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1358 1339 {
1359 1340 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1360 1341 {
1361 1342 // destination
1362 1343 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1363 1344 // source
1364 1345 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1365 1346 // copy source to destination
1366 1347 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1367 1348 }
1368 1349 }
1369 1350
1370 1351 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1371 1352 {
1372 1353 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1373 1354 {
1374 1355 // destination
1375 1356 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 ];
1376 1357 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 + 1 ];
1377 1358 // source
1378 1359 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1379 1360 // copy source to destination
1380 1361 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1381 1362 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1382 1363 }
1383 1364 }
1384 1365
1385 1366 // print_k_coeff();
1386 1367
1387 1368 return status;
1388 1369 }
1389 1370
1390 1371 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1391 1372 {
1392 1373 destination[0] = source[0];
1393 1374 destination[1] = source[1];
1394 1375 destination[2] = source[2];
1395 1376 destination[3] = source[3];
1396 1377 }
1397 1378
1398 1379 void floatToChar( float value, unsigned char* ptr)
1399 1380 {
1400 1381 unsigned char* valuePtr;
1401 1382
1402 1383 valuePtr = (unsigned char*) &value;
1403 1384 ptr[0] = valuePtr[0];
1404 1385 ptr[1] = valuePtr[1];
1405 1386 ptr[2] = valuePtr[2];
1406 1387 ptr[3] = valuePtr[3];
1407 1388 }
1408 1389
1409 1390 //**********
1410 1391 // init dump
1411 1392
1412 1393 void init_parameter_dump( void )
1413 1394 {
1414 1395 /** This function initialize the parameter_dump_packet global variable with default values.
1415 1396 *
1416 1397 */
1417 1398
1418 1399 unsigned int k;
1419 1400
1420 1401 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1421 1402 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1422 1403 parameter_dump_packet.reserved = CCSDS_RESERVED;
1423 1404 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1424 1405 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
1425 1406 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1426 1407 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1427 1408 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1428 1409 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
1429 1410 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1430 1411 // DATA FIELD HEADER
1431 1412 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1432 1413 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1433 1414 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1434 1415 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1435 1416 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
1436 1417 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
1437 1418 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
1438 1419 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
1439 1420 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
1440 1421 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
1441 1422 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1442 1423
1443 1424 //******************
1444 1425 // COMMON PARAMETERS
1445 1426 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1446 1427 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1447 1428
1448 1429 //******************
1449 1430 // NORMAL PARAMETERS
1450 1431 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1451 1432 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1452 1433 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1453 1434 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1454 1435 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1455 1436 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1456 1437 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1457 1438 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1458 1439 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1459 1440
1460 1441 //*****************
1461 1442 // BURST PARAMETERS
1462 1443 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1463 1444 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1464 1445
1465 1446 //****************
1466 1447 // SBM1 PARAMETERS
1467 1448 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
1468 1449 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1469 1450
1470 1451 //****************
1471 1452 // SBM2 PARAMETERS
1472 1453 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1473 1454 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1474 1455
1475 1456 //************
1476 1457 // FBINS MASKS
1477 1458 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1478 1459 {
1479 1460 parameter_dump_packet.sy_lfr_fbins.raw[k] = 0xff;
1480 1461 }
1481 1462
1482 1463 // PAS FILTER PARAMETERS
1483 1464 parameter_dump_packet.pa_rpw_spare8_2 = 0x00;
1484 1465 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = 0x00;
1485 1466 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1486 1467 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1487 1468 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1488 1469 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1489 1470 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1490 1471
1491 1472 // LFR_RW_MASK
1492 1473 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1493 1474 {
1494 1475 parameter_dump_packet.sy_lfr_rw_mask.raw[k] = 0xff;
1495 1476 }
1477
1478 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1479 merge_fbins_masks();
1496 1480 }
1497 1481
1498 1482 void init_kcoefficients_dump( void )
1499 1483 {
1500 1484 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1501 1485 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1502 1486
1503 1487 kcoefficient_node_1.previous = NULL;
1504 1488 kcoefficient_node_1.next = NULL;
1505 1489 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1506 1490 kcoefficient_node_1.coarseTime = 0x00;
1507 1491 kcoefficient_node_1.fineTime = 0x00;
1508 1492 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1509 1493 kcoefficient_node_1.status = 0x00;
1510 1494
1511 1495 kcoefficient_node_2.previous = NULL;
1512 1496 kcoefficient_node_2.next = NULL;
1513 1497 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1514 1498 kcoefficient_node_2.coarseTime = 0x00;
1515 1499 kcoefficient_node_2.fineTime = 0x00;
1516 1500 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1517 1501 kcoefficient_node_2.status = 0x00;
1518 1502 }
1519 1503
1520 1504 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1521 1505 {
1522 1506 unsigned int k;
1523 1507 unsigned int packetLength;
1524 1508
1525 1509 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1526 1510
1527 1511 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1528 1512 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1529 1513 kcoefficients_dump->reserved = CCSDS_RESERVED;
1530 1514 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1531 1515 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1532 1516 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1533 1517 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1534 1518 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1535 1519 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1536 1520 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1537 1521 // DATA FIELD HEADER
1538 1522 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1539 1523 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1540 1524 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1541 1525 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1542 1526 kcoefficients_dump->time[0] = 0x00;
1543 1527 kcoefficients_dump->time[1] = 0x00;
1544 1528 kcoefficients_dump->time[2] = 0x00;
1545 1529 kcoefficients_dump->time[3] = 0x00;
1546 1530 kcoefficients_dump->time[4] = 0x00;
1547 1531 kcoefficients_dump->time[5] = 0x00;
1548 1532 kcoefficients_dump->sid = SID_K_DUMP;
1549 1533
1550 1534 kcoefficients_dump->pkt_cnt = 2;
1551 1535 kcoefficients_dump->pkt_nr = pkt_nr;
1552 1536 kcoefficients_dump->blk_nr = blk_nr;
1553 1537
1554 1538 //******************
1555 1539 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1556 1540 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1557 1541 for (k=0; k<3900; k++)
1558 1542 {
1559 1543 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1560 1544 }
1561 1545 }
1562 1546
1563 1547 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1564 1548 {
1565 1549 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1566 1550 *
1567 1551 * @param packet_sequence_control points to the packet sequence control which will be incremented
1568 1552 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1569 1553 *
1570 1554 * If the destination ID is not known, a dedicated counter is incremented.
1571 1555 *
1572 1556 */
1573 1557
1574 1558 unsigned short sequence_cnt;
1575 1559 unsigned short segmentation_grouping_flag;
1576 1560 unsigned short new_packet_sequence_control;
1577 1561 unsigned char i;
1578 1562
1579 1563 switch (destination_id)
1580 1564 {
1581 1565 case SID_TC_GROUND:
1582 1566 i = GROUND;
1583 1567 break;
1584 1568 case SID_TC_MISSION_TIMELINE:
1585 1569 i = MISSION_TIMELINE;
1586 1570 break;
1587 1571 case SID_TC_TC_SEQUENCES:
1588 1572 i = TC_SEQUENCES;
1589 1573 break;
1590 1574 case SID_TC_RECOVERY_ACTION_CMD:
1591 1575 i = RECOVERY_ACTION_CMD;
1592 1576 break;
1593 1577 case SID_TC_BACKUP_MISSION_TIMELINE:
1594 1578 i = BACKUP_MISSION_TIMELINE;
1595 1579 break;
1596 1580 case SID_TC_DIRECT_CMD:
1597 1581 i = DIRECT_CMD;
1598 1582 break;
1599 1583 case SID_TC_SPARE_GRD_SRC1:
1600 1584 i = SPARE_GRD_SRC1;
1601 1585 break;
1602 1586 case SID_TC_SPARE_GRD_SRC2:
1603 1587 i = SPARE_GRD_SRC2;
1604 1588 break;
1605 1589 case SID_TC_OBCP:
1606 1590 i = OBCP;
1607 1591 break;
1608 1592 case SID_TC_SYSTEM_CONTROL:
1609 1593 i = SYSTEM_CONTROL;
1610 1594 break;
1611 1595 case SID_TC_AOCS:
1612 1596 i = AOCS;
1613 1597 break;
1614 1598 case SID_TC_RPW_INTERNAL:
1615 1599 i = RPW_INTERNAL;
1616 1600 break;
1617 1601 default:
1618 1602 i = GROUND;
1619 1603 break;
1620 1604 }
1621 1605
1622 1606 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1623 1607 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & 0x3fff;
1624 1608
1625 1609 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
1626 1610
1627 1611 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1628 1612 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1629 1613
1630 1614 // increment the sequence counter
1631 1615 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
1632 1616 {
1633 1617 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
1634 1618 }
1635 1619 else
1636 1620 {
1637 1621 sequenceCounters_TM_DUMP[ i ] = 0;
1638 1622 }
1639 1623 }
General Comments 0
You need to be logged in to leave comments. Login now