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

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

3.0.0.13
paul -
r250:0e4f9f3e6a3f R3a
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,1412 +1,1422
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_compute_stats_offsets();
70 70 status = spacewire_reset_link( );
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 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
98 98 }
99 99 // wake the WTDG task up to wait for the link recovery
100 100 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
101 101 status = rtems_task_suspend( RTEMS_SELF );
102 102 }
103 103 }
104 104 }
105 105
106 106 rtems_task recv_task( rtems_task_argument unused )
107 107 {
108 108 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
109 109 *
110 110 * @param unused is the starting argument of the RTEMS task
111 111 *
112 112 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
113 113 * 1. It reads the incoming data.
114 114 * 2. Launches the acceptance procedure.
115 115 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
116 116 *
117 117 */
118 118
119 119 int len;
120 120 ccsdsTelecommandPacket_t currentTC;
121 121 unsigned char computed_CRC[ 2 ];
122 122 unsigned char currentTC_LEN_RCV[ 2 ];
123 123 unsigned char destinationID;
124 124 unsigned int estimatedPacketLength;
125 125 unsigned int parserCode;
126 126 rtems_status_code status;
127 127 rtems_id queue_recv_id;
128 128 rtems_id queue_send_id;
129 129
130 130 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
131 131
132 132 status = get_message_queue_id_recv( &queue_recv_id );
133 133 if (status != RTEMS_SUCCESSFUL)
134 134 {
135 135 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
136 136 }
137 137
138 138 status = get_message_queue_id_send( &queue_send_id );
139 139 if (status != RTEMS_SUCCESSFUL)
140 140 {
141 141 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
142 142 }
143 143
144 144 BOOT_PRINTF("in RECV *** \n")
145 145
146 146 while(1)
147 147 {
148 148 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
149 149 if (len == -1){ // error during the read call
150 150 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
151 151 }
152 152 else {
153 153 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
154 154 PRINTF("in RECV *** packet lenght too short\n")
155 155 }
156 156 else {
157 157 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
158 158 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
159 159 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
160 160 // CHECK THE TC
161 161 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
162 162 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
163 163 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
164 164 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
165 165 || (parserCode == WRONG_SRC_ID) )
166 166 { // send TM_LFR_TC_EXE_CORRUPTED
167 167 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
168 168 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
169 169 &&
170 170 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
171 171 )
172 172 {
173 173 if ( parserCode == WRONG_SRC_ID )
174 174 {
175 175 destinationID = SID_TC_GROUND;
176 176 }
177 177 else
178 178 {
179 179 destinationID = currentTC.sourceID;
180 180 }
181 181 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
182 182 computed_CRC, currentTC_LEN_RCV,
183 183 destinationID );
184 184 }
185 185 }
186 186 else
187 187 { // send valid TC to the action launcher
188 188 status = rtems_message_queue_send( queue_recv_id, &currentTC,
189 189 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
190 190 }
191 191 }
192 192 }
193 193
194 194 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
195 195
196 196 }
197 197 }
198 198
199 199 rtems_task send_task( rtems_task_argument argument)
200 200 {
201 201 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
202 202 *
203 203 * @param unused is the starting argument of the RTEMS task
204 204 *
205 205 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
206 206 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
207 207 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
208 208 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
209 209 * data it contains.
210 210 *
211 211 */
212 212
213 213 rtems_status_code status; // RTEMS status code
214 214 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
215 215 ring_node *incomingRingNodePtr;
216 216 int ring_node_address;
217 217 char *charPtr;
218 218 spw_ioctl_pkt_send *spw_ioctl_send;
219 219 size_t size; // size of the incoming TC packet
220 220 rtems_id queue_send_id;
221 221 unsigned int sid;
222 222 unsigned char sidAsUnsignedChar;
223 223 unsigned char type;
224 224
225 225 incomingRingNodePtr = NULL;
226 226 ring_node_address = 0;
227 227 charPtr = (char *) &ring_node_address;
228 228 sid = 0;
229 229 sidAsUnsignedChar = 0;
230 230
231 231 init_header_cwf( &headerCWF );
232 232 init_header_swf( &headerSWF );
233 233 init_header_asm( &headerASM );
234 234
235 235 status = get_message_queue_id_send( &queue_send_id );
236 236 if (status != RTEMS_SUCCESSFUL)
237 237 {
238 238 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
239 239 }
240 240
241 241 BOOT_PRINTF("in SEND *** \n")
242 242
243 243 while(1)
244 244 {
245 245 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
246 246 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
247 247
248 248 if (status!=RTEMS_SUCCESSFUL)
249 249 {
250 250 PRINTF1("in SEND *** (1) ERR = %d\n", status)
251 251 }
252 252 else
253 253 {
254 254 if ( size == sizeof(ring_node*) )
255 255 {
256 256 charPtr[0] = incomingData[0];
257 257 charPtr[1] = incomingData[1];
258 258 charPtr[2] = incomingData[2];
259 259 charPtr[3] = incomingData[3];
260 260 incomingRingNodePtr = (ring_node*) ring_node_address;
261 261 sid = incomingRingNodePtr->sid;
262 262 if ( (sid==SID_NORM_CWF_LONG_F3)
263 263 || (sid==SID_BURST_CWF_F2 )
264 264 || (sid==SID_SBM1_CWF_F1 )
265 265 || (sid==SID_SBM2_CWF_F2 ))
266 266 {
267 267 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
268 268 }
269 269 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
270 270 {
271 271 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
272 272 }
273 273 else if ( (sid==SID_NORM_CWF_F3) )
274 274 {
275 275 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
276 276 }
277 277 else if (sid==SID_NORM_ASM_F0)
278 278 {
279 279 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
280 280 }
281 281 else if (sid==SID_NORM_ASM_F1)
282 282 {
283 283 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
284 284 }
285 285 else if (sid==SID_NORM_ASM_F2)
286 286 {
287 287 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
288 288 }
289 289 else if ( sid==TM_CODE_K_DUMP )
290 290 {
291 291 spw_send_k_dump( incomingRingNodePtr );
292 292 }
293 293 else
294 294 {
295 295 PRINTF1("unexpected sid = %d\n", sid);
296 296 }
297 297 }
298 298 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
299 299 {
300 300 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
301 301 sid = sidAsUnsignedChar;
302 302 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
303 303 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
304 304 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
305 305 {
306 306 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
307 307 }
308 308
309 309 status = write( fdSPW, incomingData, size );
310 310 if (status == -1){
311 311 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
312 312 }
313 313 }
314 314 else // the incoming message is a spw_ioctl_pkt_send structure
315 315 {
316 316 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
317 317 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
318 318 if (status == -1){
319 319 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
320 320 }
321 321 }
322 322 }
323 323
324 324 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
325 325
326 326 }
327 327 }
328 328
329 329 rtems_task wtdg_task( rtems_task_argument argument )
330 330 {
331 331 rtems_event_set event_out;
332 332 rtems_status_code status;
333 333 int linkStatus;
334 334
335 335 BOOT_PRINTF("in WTDG ***\n")
336 336
337 337 while(1)
338 338 {
339 339 // wait for an RTEMS_EVENT
340 340 rtems_event_receive( RTEMS_EVENT_0,
341 341 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
342 342 PRINTF("in WTDG *** wait for the link\n")
343 343 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
344 344 while( linkStatus != 5) // wait for the link
345 345 {
346 346 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
347 347 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
348 348 }
349 349
350 350 status = spacewire_stop_and_start_link( fdSPW );
351 351
352 352 if (status != RTEMS_SUCCESSFUL)
353 353 {
354 354 PRINTF1("in WTDG *** ERR link not started %d\n", status)
355 355 }
356 356 else
357 357 {
358 358 PRINTF("in WTDG *** OK link started\n")
359 359 }
360 360
361 361 // restart the SPIQ task
362 362 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
363 363 if ( status != RTEMS_SUCCESSFUL ) {
364 364 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
365 365 }
366 366
367 367 // restart RECV and SEND
368 368 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
369 369 if ( status != RTEMS_SUCCESSFUL ) {
370 370 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
371 371 }
372 372 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
373 373 if ( status != RTEMS_SUCCESSFUL ) {
374 374 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
375 375 }
376 376 }
377 377 }
378 378
379 379 //****************
380 380 // OTHER FUNCTIONS
381 381 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
382 382 {
383 383 /** This function opens the SpaceWire link.
384 384 *
385 385 * @return a valid file descriptor in case of success, -1 in case of a failure
386 386 *
387 387 */
388 388 rtems_status_code status;
389 389
390 390 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
391 391 if ( fdSPW < 0 ) {
392 392 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
393 393 }
394 394 else
395 395 {
396 396 status = RTEMS_SUCCESSFUL;
397 397 }
398 398
399 399 return status;
400 400 }
401 401
402 402 int spacewire_start_link( int fd )
403 403 {
404 404 rtems_status_code status;
405 405
406 406 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
407 407 // -1 default hardcoded driver timeout
408 408
409 409 return status;
410 410 }
411 411
412 412 int spacewire_stop_and_start_link( int fd )
413 413 {
414 414 rtems_status_code status;
415 415
416 416 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
417 417 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
418 418 // -1 default hardcoded driver timeout
419 419
420 420 return status;
421 421 }
422 422
423 423 int spacewire_configure_link( int fd )
424 424 {
425 425 /** This function configures the SpaceWire link.
426 426 *
427 427 * @return GR-RTEMS-DRIVER directive status codes:
428 428 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
429 429 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
430 430 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
431 431 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
432 432 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
433 433 * - 5 EIO - Error when writing to grswp hardware registers.
434 434 * - 2 ENOENT - No such file or directory
435 435 */
436 436
437 437 rtems_status_code status;
438 438
439 439 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
440 440 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
441 441
442 442 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
443 443 if (status!=RTEMS_SUCCESSFUL) {
444 444 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
445 445 }
446 446 //
447 447 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
448 448 if (status!=RTEMS_SUCCESSFUL) {
449 449 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
450 450 }
451 451 //
452 452 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
453 453 if (status!=RTEMS_SUCCESSFUL) {
454 454 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
455 455 }
456 456 //
457 457 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
458 458 if (status!=RTEMS_SUCCESSFUL) {
459 459 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
460 460 }
461 461 //
462 462 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
463 463 if (status!=RTEMS_SUCCESSFUL) {
464 464 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
465 465 }
466 466 //
467 467 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
468 468 if (status!=RTEMS_SUCCESSFUL) {
469 469 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
470 470 }
471 471 //
472 472 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
473 473 if (status!=RTEMS_SUCCESSFUL) {
474 474 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
475 475 }
476 476
477 477 return status;
478 478 }
479 479
480 480 int spacewire_reset_link( void )
481 481 {
482 482 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
483 483 *
484 484 * @return RTEMS directive status code:
485 485 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
486 486 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
487 487 *
488 488 */
489 489
490 490 rtems_status_code status_spw;
491 491 rtems_status_code status;
492 492 int i;
493 493
494 494 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
495 495 {
496 496 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
497 497
498 498 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
499 499
500 500 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
501 501
502 502 status_spw = spacewire_stop_and_start_link( fdSPW );
503 503 if ( status_spw != RTEMS_SUCCESSFUL )
504 504 {
505 505 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
506 506 }
507 507
508 508 if ( status_spw == RTEMS_SUCCESSFUL)
509 509 {
510 510 break;
511 511 }
512 512 }
513 513
514 514 return status_spw;
515 515 }
516 516
517 517 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
518 518 {
519 519 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
520 520 *
521 521 * @param val is the value, 0 or 1, used to set the value of the NP bit.
522 522 * @param regAddr is the address of the GRSPW control register.
523 523 *
524 524 * NP is the bit 20 of the GRSPW control register.
525 525 *
526 526 */
527 527
528 528 unsigned int *spwptr = (unsigned int*) regAddr;
529 529
530 530 if (val == 1) {
531 531 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
532 532 }
533 533 if (val== 0) {
534 534 *spwptr = *spwptr & 0xffdfffff;
535 535 }
536 536 }
537 537
538 538 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
539 539 {
540 540 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
541 541 *
542 542 * @param val is the value, 0 or 1, used to set the value of the RE bit.
543 543 * @param regAddr is the address of the GRSPW control register.
544 544 *
545 545 * RE is the bit 16 of the GRSPW control register.
546 546 *
547 547 */
548 548
549 549 unsigned int *spwptr = (unsigned int*) regAddr;
550 550
551 551 if (val == 1)
552 552 {
553 553 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
554 554 }
555 555 if (val== 0)
556 556 {
557 557 *spwptr = *spwptr & 0xfffdffff;
558 558 }
559 559 }
560 560
561 561 void spacewire_compute_stats_offsets( void )
562 562 {
563 563 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
564 564 *
565 565 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
566 566 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
567 567 * during the open systel call).
568 568 *
569 569 */
570 570
571 571 spw_stats spacewire_stats_grspw;
572 572 rtems_status_code status;
573 573
574 574 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
575 575
576 576 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
577 577 + spacewire_stats.packets_received;
578 578 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
579 579 + spacewire_stats.packets_sent;
580 580 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
581 581 + spacewire_stats.parity_err;
582 582 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
583 583 + spacewire_stats.disconnect_err;
584 584 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
585 585 + spacewire_stats.escape_err;
586 586 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
587 587 + spacewire_stats.credit_err;
588 588 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
589 589 + spacewire_stats.write_sync_err;
590 590 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
591 591 + spacewire_stats.rx_rmap_header_crc_err;
592 592 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
593 593 + spacewire_stats.rx_rmap_data_crc_err;
594 594 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
595 595 + spacewire_stats.early_ep;
596 596 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
597 597 + spacewire_stats.invalid_address;
598 598 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
599 599 + spacewire_stats.rx_eep_err;
600 600 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
601 601 + spacewire_stats.rx_truncated;
602 602 }
603 603
604 604 void spacewire_update_statistics( void )
605 605 {
606 606 rtems_status_code status;
607 607 spw_stats spacewire_stats_grspw;
608 608
609 609 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
610 610
611 611 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
612 612 + spacewire_stats_grspw.packets_received;
613 613 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
614 614 + spacewire_stats_grspw.packets_sent;
615 615 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
616 616 + spacewire_stats_grspw.parity_err;
617 617 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
618 618 + spacewire_stats_grspw.disconnect_err;
619 619 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
620 620 + spacewire_stats_grspw.escape_err;
621 621 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
622 622 + spacewire_stats_grspw.credit_err;
623 623 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
624 624 + spacewire_stats_grspw.write_sync_err;
625 625 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
626 626 + spacewire_stats_grspw.rx_rmap_header_crc_err;
627 627 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
628 628 + spacewire_stats_grspw.rx_rmap_data_crc_err;
629 629 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
630 630 + spacewire_stats_grspw.early_ep;
631 631 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
632 632 + spacewire_stats_grspw.invalid_address;
633 633 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
634 634 + spacewire_stats_grspw.rx_eep_err;
635 635 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
636 636 + spacewire_stats_grspw.rx_truncated;
637 637 //spacewire_stats.tx_link_err;
638 638
639 639 //****************************
640 640 // DPU_SPACEWIRE_IF_STATISTICS
641 641 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
642 642 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
643 643 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
644 644 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
645 645 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
646 646 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
647 647
648 648 //******************************************
649 649 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
650 650 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
651 651 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
652 652 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
653 653 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
654 654 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
655 655
656 656 //*********************************************
657 657 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
658 658 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
659 659 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
660 660 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
661 661 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
662 662 }
663 663
664 664 void increase_unsigned_char_counter( unsigned char *counter )
665 665 {
666 666 // update the number of valid timecodes that have been received
667 667 if (*counter == 255)
668 668 {
669 669 *counter = 0;
670 670 }
671 671 else
672 672 {
673 673 *counter = *counter + 1;
674 674 }
675 675 }
676 676
677 677 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
678 678 {
679 679
680 680 unsigned char currentTimecodeCtr;
681 681
682 682 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
683 683
684 684 if (currentTimecodeCtr == previousTimecodeCtr)
685 685 {
686 686 //************************
687 687 // HK_LFR_TIMECODE_MISSING
688 688 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
689 689 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
690 690 }
691 691 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
692 692 {
693 693 // the timecode value has changed and the value is valid, this is unexpected because
694 694 // the timer should not have fired, the timecode_irq_handler should have been raised
695 695 }
696 696 else
697 697 {
698 698 //************************
699 699 // HK_LFR_TIMECODE_INVALID
700 700 // the timecode value has changed and the value is not valid, no tickout has been generated
701 701 // this is why the timer has fired
702 702 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
703 703 }
704 704
705 705 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
706 706 }
707 707
708 708 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
709 709 {
710 /** This function checks the coherency between the incoming timecode and the last valid timecode.
711 *
712 * @param currentTimecodeCtr is the incoming timecode
713 *
714 * @return returned codes::
715 * - LFR_DEFAULT
716 * - LFR_SUCCESSFUL
717 *
718 */
719
710 720 static unsigned char firstTickout = 1;
711 721 unsigned char ret;
712 722
713 723 ret = LFR_DEFAULT;
714 724
715 725 if (firstTickout == 0)
716 726 {
717 727 if (currentTimecodeCtr == 0)
718 728 {
719 729 if (previousTimecodeCtr == 63)
720 730 {
721 731 ret = LFR_SUCCESSFUL;
722 732 }
723 733 else
724 734 {
725 735 ret = LFR_DEFAULT;
726 736 }
727 737 }
728 738 else
729 739 {
730 740 if (currentTimecodeCtr == (previousTimecodeCtr +1))
731 741 {
732 742 ret = LFR_SUCCESSFUL;
733 743 }
734 744 else
735 745 {
736 746 ret = LFR_DEFAULT;
737 747 }
738 748 }
739 749 }
740 750 else
741 751 {
742 752 firstTickout = 0;
743 753 ret = LFR_SUCCESSFUL;
744 754 }
745 755
746 756 return ret;
747 757 }
748 758
749 759 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
750 760 {
751 761 unsigned int ret;
752 762
753 763 ret = LFR_DEFAULT;
754 764
755 765 if (timecode == internalTime)
756 766 {
757 767 ret = LFR_SUCCESSFUL;
758 768 }
759 769 else
760 770 {
761 771 ret = LFR_DEFAULT;
762 772 }
763 773
764 774 return ret;
765 775 }
766 776
767 777 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
768 778 {
769 779 // a tickout has been emitted, perform actions on the incoming timecode
770 780
771 781 unsigned char incomingTimecode;
772 782 unsigned char updateTime;
773 783 unsigned char internalTime;
774 784 rtems_status_code status;
775 785
776 786 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
777 787 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
778 788 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
779 789
780 790 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
781 791
782 792 // update the number of tickout that have been generated
783 793 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
784 794
785 795 //**************************
786 796 // HK_LFR_TIMECODE_ERRONEOUS
787 797 // MISSING and INVALID are handled by the timecode_timer_routine service routine
788 798 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
789 799 {
790 800 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
791 801 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
792 802 }
793 803
794 804 //************************
795 805 // HK_LFR_TIME_TIMECODE_IT
796 806 // check the coherency between the SpaceWire timecode and the Internal Time
797 807 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
798 808 {
799 809 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
800 810 }
801 811
802 812 //********************
803 813 // HK_LFR_TIMECODE_CTR
804 814 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
805 815 if (incomingTimecode != updateTime)
806 816 {
807 817 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
808 818 }
809 819
810 820 // launch the timecode timer to detect missing or invalid timecodes
811 821 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
812 822 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
813 823 }
814 824
815 825 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
816 826 {
817 827 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
818 828 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
819 829 header->reserved = DEFAULT_RESERVED;
820 830 header->userApplication = CCSDS_USER_APP;
821 831 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
822 832 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
823 833 header->packetLength[0] = 0x00;
824 834 header->packetLength[1] = 0x00;
825 835 // DATA FIELD HEADER
826 836 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
827 837 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
828 838 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
829 839 header->destinationID = TM_DESTINATION_ID_GROUND;
830 840 header->time[0] = 0x00;
831 841 header->time[0] = 0x00;
832 842 header->time[0] = 0x00;
833 843 header->time[0] = 0x00;
834 844 header->time[0] = 0x00;
835 845 header->time[0] = 0x00;
836 846 // AUXILIARY DATA HEADER
837 847 header->sid = 0x00;
838 848 header->hkBIA = DEFAULT_HKBIA;
839 849 header->blkNr[0] = 0x00;
840 850 header->blkNr[1] = 0x00;
841 851 }
842 852
843 853 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
844 854 {
845 855 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
846 856 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
847 857 header->reserved = DEFAULT_RESERVED;
848 858 header->userApplication = CCSDS_USER_APP;
849 859 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
850 860 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
851 861 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
852 862 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
853 863 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
854 864 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
855 865 // DATA FIELD HEADER
856 866 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
857 867 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
858 868 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
859 869 header->destinationID = TM_DESTINATION_ID_GROUND;
860 870 header->time[0] = 0x00;
861 871 header->time[0] = 0x00;
862 872 header->time[0] = 0x00;
863 873 header->time[0] = 0x00;
864 874 header->time[0] = 0x00;
865 875 header->time[0] = 0x00;
866 876 // AUXILIARY DATA HEADER
867 877 header->sid = 0x00;
868 878 header->hkBIA = DEFAULT_HKBIA;
869 879 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
870 880 header->pktNr = 0x00;
871 881 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
872 882 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
873 883 }
874 884
875 885 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
876 886 {
877 887 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
878 888 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
879 889 header->reserved = DEFAULT_RESERVED;
880 890 header->userApplication = CCSDS_USER_APP;
881 891 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
882 892 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
883 893 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
884 894 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
885 895 header->packetLength[0] = 0x00;
886 896 header->packetLength[1] = 0x00;
887 897 // DATA FIELD HEADER
888 898 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
889 899 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
890 900 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
891 901 header->destinationID = TM_DESTINATION_ID_GROUND;
892 902 header->time[0] = 0x00;
893 903 header->time[0] = 0x00;
894 904 header->time[0] = 0x00;
895 905 header->time[0] = 0x00;
896 906 header->time[0] = 0x00;
897 907 header->time[0] = 0x00;
898 908 // AUXILIARY DATA HEADER
899 909 header->sid = 0x00;
900 910 header->biaStatusInfo = 0x00;
901 911 header->pa_lfr_pkt_cnt_asm = 0x00;
902 912 header->pa_lfr_pkt_nr_asm = 0x00;
903 913 header->pa_lfr_asm_blk_nr[0] = 0x00;
904 914 header->pa_lfr_asm_blk_nr[1] = 0x00;
905 915 }
906 916
907 917 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
908 918 Header_TM_LFR_SCIENCE_CWF_t *header )
909 919 {
910 920 /** This function sends CWF CCSDS packets (F2, F1 or F0).
911 921 *
912 922 * @param waveform points to the buffer containing the data that will be send.
913 923 * @param sid is the source identifier of the data that will be sent.
914 924 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
915 925 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
916 926 * contain information to setup the transmission of the data packets.
917 927 *
918 928 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
919 929 *
920 930 */
921 931
922 932 unsigned int i;
923 933 int ret;
924 934 unsigned int coarseTime;
925 935 unsigned int fineTime;
926 936 rtems_status_code status;
927 937 spw_ioctl_pkt_send spw_ioctl_send_CWF;
928 938 int *dataPtr;
929 939 unsigned char sid;
930 940
931 941 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
932 942 spw_ioctl_send_CWF.options = 0;
933 943
934 944 ret = LFR_DEFAULT;
935 945 sid = (unsigned char) ring_node_to_send->sid;
936 946
937 947 coarseTime = ring_node_to_send->coarseTime;
938 948 fineTime = ring_node_to_send->fineTime;
939 949 dataPtr = (int*) ring_node_to_send->buffer_address;
940 950
941 951 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
942 952 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
943 953 header->hkBIA = pa_bia_status_info;
944 954 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
945 955 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
946 956 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
947 957
948 958 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
949 959 {
950 960 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
951 961 spw_ioctl_send_CWF.hdr = (char*) header;
952 962 // BUILD THE DATA
953 963 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
954 964
955 965 // SET PACKET SEQUENCE CONTROL
956 966 increment_seq_counter_source_id( header->packetSequenceControl, sid );
957 967
958 968 // SET SID
959 969 header->sid = sid;
960 970
961 971 // SET PACKET TIME
962 972 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
963 973 //
964 974 header->time[0] = header->acquisitionTime[0];
965 975 header->time[1] = header->acquisitionTime[1];
966 976 header->time[2] = header->acquisitionTime[2];
967 977 header->time[3] = header->acquisitionTime[3];
968 978 header->time[4] = header->acquisitionTime[4];
969 979 header->time[5] = header->acquisitionTime[5];
970 980
971 981 // SET PACKET ID
972 982 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
973 983 {
974 984 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
975 985 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
976 986 }
977 987 else
978 988 {
979 989 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
980 990 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
981 991 }
982 992
983 993 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
984 994 if (status != RTEMS_SUCCESSFUL) {
985 995 ret = LFR_DEFAULT;
986 996 }
987 997 }
988 998
989 999 return ret;
990 1000 }
991 1001
992 1002 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
993 1003 Header_TM_LFR_SCIENCE_SWF_t *header )
994 1004 {
995 1005 /** This function sends SWF CCSDS packets (F2, F1 or F0).
996 1006 *
997 1007 * @param waveform points to the buffer containing the data that will be send.
998 1008 * @param sid is the source identifier of the data that will be sent.
999 1009 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1000 1010 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1001 1011 * contain information to setup the transmission of the data packets.
1002 1012 *
1003 1013 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1004 1014 *
1005 1015 */
1006 1016
1007 1017 unsigned int i;
1008 1018 int ret;
1009 1019 unsigned int coarseTime;
1010 1020 unsigned int fineTime;
1011 1021 rtems_status_code status;
1012 1022 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1013 1023 int *dataPtr;
1014 1024 unsigned char sid;
1015 1025
1016 1026 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1017 1027 spw_ioctl_send_SWF.options = 0;
1018 1028
1019 1029 ret = LFR_DEFAULT;
1020 1030
1021 1031 coarseTime = ring_node_to_send->coarseTime;
1022 1032 fineTime = ring_node_to_send->fineTime;
1023 1033 dataPtr = (int*) ring_node_to_send->buffer_address;
1024 1034 sid = ring_node_to_send->sid;
1025 1035
1026 1036 header->hkBIA = pa_bia_status_info;
1027 1037 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1028 1038
1029 1039 for (i=0; i<7; i++) // send waveform
1030 1040 {
1031 1041 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1032 1042 spw_ioctl_send_SWF.hdr = (char*) header;
1033 1043
1034 1044 // SET PACKET SEQUENCE CONTROL
1035 1045 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1036 1046
1037 1047 // SET PACKET LENGTH AND BLKNR
1038 1048 if (i == 6)
1039 1049 {
1040 1050 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1041 1051 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
1042 1052 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1043 1053 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
1044 1054 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1045 1055 }
1046 1056 else
1047 1057 {
1048 1058 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1049 1059 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
1050 1060 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1051 1061 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
1052 1062 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1053 1063 }
1054 1064
1055 1065 // SET PACKET TIME
1056 1066 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1057 1067 //
1058 1068 header->time[0] = header->acquisitionTime[0];
1059 1069 header->time[1] = header->acquisitionTime[1];
1060 1070 header->time[2] = header->acquisitionTime[2];
1061 1071 header->time[3] = header->acquisitionTime[3];
1062 1072 header->time[4] = header->acquisitionTime[4];
1063 1073 header->time[5] = header->acquisitionTime[5];
1064 1074
1065 1075 // SET SID
1066 1076 header->sid = sid;
1067 1077
1068 1078 // SET PKTNR
1069 1079 header->pktNr = i+1; // PKT_NR
1070 1080
1071 1081 // SEND PACKET
1072 1082 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1073 1083 if (status != RTEMS_SUCCESSFUL) {
1074 1084 ret = LFR_DEFAULT;
1075 1085 }
1076 1086 }
1077 1087
1078 1088 return ret;
1079 1089 }
1080 1090
1081 1091 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1082 1092 Header_TM_LFR_SCIENCE_CWF_t *header )
1083 1093 {
1084 1094 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1085 1095 *
1086 1096 * @param waveform points to the buffer containing the data that will be send.
1087 1097 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1088 1098 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1089 1099 * contain information to setup the transmission of the data packets.
1090 1100 *
1091 1101 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1092 1102 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1093 1103 *
1094 1104 */
1095 1105
1096 1106 unsigned int i;
1097 1107 int ret;
1098 1108 unsigned int coarseTime;
1099 1109 unsigned int fineTime;
1100 1110 rtems_status_code status;
1101 1111 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1102 1112 char *dataPtr;
1103 1113 unsigned char sid;
1104 1114
1105 1115 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1106 1116 spw_ioctl_send_CWF.options = 0;
1107 1117
1108 1118 ret = LFR_DEFAULT;
1109 1119 sid = ring_node_to_send->sid;
1110 1120
1111 1121 coarseTime = ring_node_to_send->coarseTime;
1112 1122 fineTime = ring_node_to_send->fineTime;
1113 1123 dataPtr = (char*) ring_node_to_send->buffer_address;
1114 1124
1115 1125 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
1116 1126 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1117 1127 header->hkBIA = pa_bia_status_info;
1118 1128 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1119 1129 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
1120 1130 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1121 1131
1122 1132 //*********************
1123 1133 // SEND CWF3_light DATA
1124 1134 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1125 1135 {
1126 1136 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1127 1137 spw_ioctl_send_CWF.hdr = (char*) header;
1128 1138 // BUILD THE DATA
1129 1139 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1130 1140
1131 1141 // SET PACKET SEQUENCE COUNTER
1132 1142 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1133 1143
1134 1144 // SET SID
1135 1145 header->sid = sid;
1136 1146
1137 1147 // SET PACKET TIME
1138 1148 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1139 1149 //
1140 1150 header->time[0] = header->acquisitionTime[0];
1141 1151 header->time[1] = header->acquisitionTime[1];
1142 1152 header->time[2] = header->acquisitionTime[2];
1143 1153 header->time[3] = header->acquisitionTime[3];
1144 1154 header->time[4] = header->acquisitionTime[4];
1145 1155 header->time[5] = header->acquisitionTime[5];
1146 1156
1147 1157 // SET PACKET ID
1148 1158 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1149 1159 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1150 1160
1151 1161 // SEND PACKET
1152 1162 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1153 1163 if (status != RTEMS_SUCCESSFUL) {
1154 1164 ret = LFR_DEFAULT;
1155 1165 }
1156 1166 }
1157 1167
1158 1168 return ret;
1159 1169 }
1160 1170
1161 1171 void spw_send_asm_f0( ring_node *ring_node_to_send,
1162 1172 Header_TM_LFR_SCIENCE_ASM_t *header )
1163 1173 {
1164 1174 unsigned int i;
1165 1175 unsigned int length = 0;
1166 1176 rtems_status_code status;
1167 1177 unsigned int sid;
1168 1178 float *spectral_matrix;
1169 1179 int coarseTime;
1170 1180 int fineTime;
1171 1181 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1172 1182
1173 1183 sid = ring_node_to_send->sid;
1174 1184 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1175 1185 coarseTime = ring_node_to_send->coarseTime;
1176 1186 fineTime = ring_node_to_send->fineTime;
1177 1187
1178 1188 header->biaStatusInfo = pa_bia_status_info;
1179 1189 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1180 1190
1181 1191 for (i=0; i<3; i++)
1182 1192 {
1183 1193 if ((i==0) || (i==1))
1184 1194 {
1185 1195 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1186 1196 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1187 1197 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1188 1198 ];
1189 1199 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1190 1200 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1191 1201 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1192 1202 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1193 1203 }
1194 1204 else
1195 1205 {
1196 1206 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1197 1207 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1198 1208 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1199 1209 ];
1200 1210 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1201 1211 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1202 1212 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1203 1213 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1204 1214 }
1205 1215
1206 1216 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1207 1217 spw_ioctl_send_ASM.hdr = (char *) header;
1208 1218 spw_ioctl_send_ASM.options = 0;
1209 1219
1210 1220 // (2) BUILD THE HEADER
1211 1221 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1212 1222 header->packetLength[0] = (unsigned char) (length>>8);
1213 1223 header->packetLength[1] = (unsigned char) (length);
1214 1224 header->sid = (unsigned char) sid; // SID
1215 1225 header->pa_lfr_pkt_cnt_asm = 3;
1216 1226 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1217 1227
1218 1228 // (3) SET PACKET TIME
1219 1229 header->time[0] = (unsigned char) (coarseTime>>24);
1220 1230 header->time[1] = (unsigned char) (coarseTime>>16);
1221 1231 header->time[2] = (unsigned char) (coarseTime>>8);
1222 1232 header->time[3] = (unsigned char) (coarseTime);
1223 1233 header->time[4] = (unsigned char) (fineTime>>8);
1224 1234 header->time[5] = (unsigned char) (fineTime);
1225 1235 //
1226 1236 header->acquisitionTime[0] = header->time[0];
1227 1237 header->acquisitionTime[1] = header->time[1];
1228 1238 header->acquisitionTime[2] = header->time[2];
1229 1239 header->acquisitionTime[3] = header->time[3];
1230 1240 header->acquisitionTime[4] = header->time[4];
1231 1241 header->acquisitionTime[5] = header->time[5];
1232 1242
1233 1243 // (4) SEND PACKET
1234 1244 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1235 1245 if (status != RTEMS_SUCCESSFUL) {
1236 1246 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1237 1247 }
1238 1248 }
1239 1249 }
1240 1250
1241 1251 void spw_send_asm_f1( ring_node *ring_node_to_send,
1242 1252 Header_TM_LFR_SCIENCE_ASM_t *header )
1243 1253 {
1244 1254 unsigned int i;
1245 1255 unsigned int length = 0;
1246 1256 rtems_status_code status;
1247 1257 unsigned int sid;
1248 1258 float *spectral_matrix;
1249 1259 int coarseTime;
1250 1260 int fineTime;
1251 1261 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1252 1262
1253 1263 sid = ring_node_to_send->sid;
1254 1264 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1255 1265 coarseTime = ring_node_to_send->coarseTime;
1256 1266 fineTime = ring_node_to_send->fineTime;
1257 1267
1258 1268 header->biaStatusInfo = pa_bia_status_info;
1259 1269 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1260 1270
1261 1271 for (i=0; i<3; i++)
1262 1272 {
1263 1273 if ((i==0) || (i==1))
1264 1274 {
1265 1275 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1266 1276 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1267 1277 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1268 1278 ];
1269 1279 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1270 1280 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1271 1281 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1272 1282 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1273 1283 }
1274 1284 else
1275 1285 {
1276 1286 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1277 1287 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1278 1288 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1279 1289 ];
1280 1290 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1281 1291 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1282 1292 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1283 1293 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1284 1294 }
1285 1295
1286 1296 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1287 1297 spw_ioctl_send_ASM.hdr = (char *) header;
1288 1298 spw_ioctl_send_ASM.options = 0;
1289 1299
1290 1300 // (2) BUILD THE HEADER
1291 1301 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1292 1302 header->packetLength[0] = (unsigned char) (length>>8);
1293 1303 header->packetLength[1] = (unsigned char) (length);
1294 1304 header->sid = (unsigned char) sid; // SID
1295 1305 header->pa_lfr_pkt_cnt_asm = 3;
1296 1306 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1297 1307
1298 1308 // (3) SET PACKET TIME
1299 1309 header->time[0] = (unsigned char) (coarseTime>>24);
1300 1310 header->time[1] = (unsigned char) (coarseTime>>16);
1301 1311 header->time[2] = (unsigned char) (coarseTime>>8);
1302 1312 header->time[3] = (unsigned char) (coarseTime);
1303 1313 header->time[4] = (unsigned char) (fineTime>>8);
1304 1314 header->time[5] = (unsigned char) (fineTime);
1305 1315 //
1306 1316 header->acquisitionTime[0] = header->time[0];
1307 1317 header->acquisitionTime[1] = header->time[1];
1308 1318 header->acquisitionTime[2] = header->time[2];
1309 1319 header->acquisitionTime[3] = header->time[3];
1310 1320 header->acquisitionTime[4] = header->time[4];
1311 1321 header->acquisitionTime[5] = header->time[5];
1312 1322
1313 1323 // (4) SEND PACKET
1314 1324 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1315 1325 if (status != RTEMS_SUCCESSFUL) {
1316 1326 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1317 1327 }
1318 1328 }
1319 1329 }
1320 1330
1321 1331 void spw_send_asm_f2( ring_node *ring_node_to_send,
1322 1332 Header_TM_LFR_SCIENCE_ASM_t *header )
1323 1333 {
1324 1334 unsigned int i;
1325 1335 unsigned int length = 0;
1326 1336 rtems_status_code status;
1327 1337 unsigned int sid;
1328 1338 float *spectral_matrix;
1329 1339 int coarseTime;
1330 1340 int fineTime;
1331 1341 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1332 1342
1333 1343 sid = ring_node_to_send->sid;
1334 1344 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1335 1345 coarseTime = ring_node_to_send->coarseTime;
1336 1346 fineTime = ring_node_to_send->fineTime;
1337 1347
1338 1348 header->biaStatusInfo = pa_bia_status_info;
1339 1349 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1340 1350
1341 1351 for (i=0; i<3; i++)
1342 1352 {
1343 1353
1344 1354 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1345 1355 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1346 1356 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1347 1357 ];
1348 1358 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1349 1359 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1350 1360 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1351 1361 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1352 1362
1353 1363 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1354 1364 spw_ioctl_send_ASM.hdr = (char *) header;
1355 1365 spw_ioctl_send_ASM.options = 0;
1356 1366
1357 1367 // (2) BUILD THE HEADER
1358 1368 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1359 1369 header->packetLength[0] = (unsigned char) (length>>8);
1360 1370 header->packetLength[1] = (unsigned char) (length);
1361 1371 header->sid = (unsigned char) sid; // SID
1362 1372 header->pa_lfr_pkt_cnt_asm = 3;
1363 1373 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1364 1374
1365 1375 // (3) SET PACKET TIME
1366 1376 header->time[0] = (unsigned char) (coarseTime>>24);
1367 1377 header->time[1] = (unsigned char) (coarseTime>>16);
1368 1378 header->time[2] = (unsigned char) (coarseTime>>8);
1369 1379 header->time[3] = (unsigned char) (coarseTime);
1370 1380 header->time[4] = (unsigned char) (fineTime>>8);
1371 1381 header->time[5] = (unsigned char) (fineTime);
1372 1382 //
1373 1383 header->acquisitionTime[0] = header->time[0];
1374 1384 header->acquisitionTime[1] = header->time[1];
1375 1385 header->acquisitionTime[2] = header->time[2];
1376 1386 header->acquisitionTime[3] = header->time[3];
1377 1387 header->acquisitionTime[4] = header->time[4];
1378 1388 header->acquisitionTime[5] = header->time[5];
1379 1389
1380 1390 // (4) SEND PACKET
1381 1391 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1382 1392 if (status != RTEMS_SUCCESSFUL) {
1383 1393 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1384 1394 }
1385 1395 }
1386 1396 }
1387 1397
1388 1398 void spw_send_k_dump( ring_node *ring_node_to_send )
1389 1399 {
1390 1400 rtems_status_code status;
1391 1401 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1392 1402 unsigned int packetLength;
1393 1403 unsigned int size;
1394 1404
1395 1405 PRINTF("spw_send_k_dump\n")
1396 1406
1397 1407 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1398 1408
1399 1409 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1400 1410
1401 1411 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1402 1412
1403 1413 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1404 1414
1405 1415 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1406 1416
1407 1417 if (status == -1){
1408 1418 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1409 1419 }
1410 1420
1411 1421 ring_node_to_send->status = 0x00;
1412 1422 }
Show file before | Show file after | Hide comments
@@ -1,1606 +1,1606
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_UPDT_TIME:
127 127 result = action_update_time( &TC );
128 128 close_action( &TC, result, queue_snd_id );
129 129 break;
130 130 default:
131 131 break;
132 132 }
133 133 }
134 134 }
135 135 }
136 136
137 137 //***********
138 138 // TC ACTIONS
139 139
140 140 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
141 141 {
142 142 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
143 143 *
144 144 * @param TC points to the TeleCommand packet that is being processed
145 145 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
146 146 *
147 147 */
148 148
149 149 PRINTF("this is the end!!!\n")
150 150 exit(0);
151 151 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
152 152 return LFR_DEFAULT;
153 153 }
154 154
155 155 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
156 156 {
157 157 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
158 158 *
159 159 * @param TC points to the TeleCommand packet that is being processed
160 160 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
161 161 *
162 162 */
163 163
164 164 rtems_status_code status;
165 165 unsigned char requestedMode;
166 166 unsigned int *transitionCoarseTime_ptr;
167 167 unsigned int transitionCoarseTime;
168 168 unsigned char * bytePosPtr;
169 169
170 170 bytePosPtr = (unsigned char *) &TC->packetID;
171 171
172 172 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
173 173 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
174 174 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
175 175
176 176 status = check_mode_value( requestedMode );
177 177
178 178 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
179 179 {
180 180 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
181 181 }
182 182
183 183 else // the mode value is valid, check the transition
184 184 {
185 185 status = check_mode_transition(requestedMode);
186 186 if (status != LFR_SUCCESSFUL)
187 187 {
188 188 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
189 189 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
190 190 }
191 191 }
192 192
193 193 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
194 194 {
195 195 status = check_transition_date( transitionCoarseTime );
196 196 if (status != LFR_SUCCESSFUL)
197 197 {
198 198 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n")
199 199 send_tm_lfr_tc_exe_inconsistent( TC, queue_id,
200 200 BYTE_POS_CP_LFR_ENTER_MODE_TIME,
201 201 bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME + 3 ] );
202 202 }
203 203 }
204 204
205 205 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
206 206 {
207 207 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
208 208
209 209 update_last_valid_transition_date( transitionCoarseTime );
210 210
211 211 switch(requestedMode)
212 212 {
213 213 case LFR_MODE_STANDBY:
214 214 status = enter_mode_standby();
215 215 break;
216 216 case LFR_MODE_NORMAL:
217 217 status = enter_mode_normal( transitionCoarseTime );
218 218 break;
219 219 case LFR_MODE_BURST:
220 220 status = enter_mode_burst( transitionCoarseTime );
221 221 break;
222 222 case LFR_MODE_SBM1:
223 223 status = enter_mode_sbm1( transitionCoarseTime );
224 224 break;
225 225 case LFR_MODE_SBM2:
226 226 status = enter_mode_sbm2( transitionCoarseTime );
227 227 break;
228 228 default:
229 229 break;
230 230 }
231 231 }
232 232
233 233 return status;
234 234 }
235 235
236 236 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
237 237 {
238 238 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
239 239 *
240 240 * @param TC points to the TeleCommand packet that is being processed
241 241 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
242 242 *
243 243 * @return LFR directive status code:
244 244 * - LFR_DEFAULT
245 245 * - LFR_SUCCESSFUL
246 246 *
247 247 */
248 248
249 249 unsigned int val;
250 250 int result;
251 251 unsigned int status;
252 252 unsigned char mode;
253 253 unsigned char * bytePosPtr;
254 254
255 255 bytePosPtr = (unsigned char *) &TC->packetID;
256 256
257 257 // check LFR mode
258 258 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
259 259 status = check_update_info_hk_lfr_mode( mode );
260 260 if (status == LFR_SUCCESSFUL) // check TDS mode
261 261 {
262 262 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
263 263 status = check_update_info_hk_tds_mode( mode );
264 264 }
265 265 if (status == LFR_SUCCESSFUL) // check THR mode
266 266 {
267 267 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
268 268 status = check_update_info_hk_thr_mode( mode );
269 269 }
270 270 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
271 271 {
272 272 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
273 273 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
274 274 val++;
275 275 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
276 276 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
277 277 }
278 278
279 279 // pa_bia_status_info
280 280 // => pa_bia_mode_mux_set 3 bits
281 281 // => pa_bia_mode_hv_enabled 1 bit
282 282 // => pa_bia_mode_bias1_enabled 1 bit
283 283 // => pa_bia_mode_bias2_enabled 1 bit
284 284 // => pa_bia_mode_bias3_enabled 1 bit
285 285 // => pa_bia_on_off (cp_dpu_bias_on_off)
286 286 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & 0xfe; // [1111 1110]
287 287 pa_bia_status_info = pa_bia_status_info
288 288 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1);
289 289
290 290 result = status;
291 291
292 292 return result;
293 293 }
294 294
295 295 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 296 {
297 297 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
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 transmission by the SpaceWire driver
301 301 *
302 302 */
303 303
304 304 int result;
305 305
306 306 result = LFR_DEFAULT;
307 307
308 308 setCalibration( true );
309 309
310 310 result = LFR_SUCCESSFUL;
311 311
312 312 return result;
313 313 }
314 314
315 315 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
316 316 {
317 317 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
318 318 *
319 319 * @param TC points to the TeleCommand packet that is being processed
320 320 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
321 321 *
322 322 */
323 323
324 324 int result;
325 325
326 326 result = LFR_DEFAULT;
327 327
328 328 setCalibration( false );
329 329
330 330 result = LFR_SUCCESSFUL;
331 331
332 332 return result;
333 333 }
334 334
335 335 int action_update_time(ccsdsTelecommandPacket_t *TC)
336 336 {
337 337 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
338 338 *
339 339 * @param TC points to the TeleCommand packet that is being processed
340 340 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
341 341 *
342 342 * @return LFR_SUCCESSFUL
343 343 *
344 344 */
345 345
346 346 unsigned int val;
347 347
348 348 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
349 349 + (TC->dataAndCRC[1] << 16)
350 350 + (TC->dataAndCRC[2] << 8)
351 351 + TC->dataAndCRC[3];
352 352
353 353 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
354 354 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
355 355 val++;
356 356 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
357 357 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
358 358
359 359 return LFR_SUCCESSFUL;
360 360 }
361 361
362 362 //*******************
363 363 // ENTERING THE MODES
364 364 int check_mode_value( unsigned char requestedMode )
365 365 {
366 366 int status;
367 367
368 368 if ( (requestedMode != LFR_MODE_STANDBY)
369 369 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
370 370 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
371 371 {
372 372 status = LFR_DEFAULT;
373 373 }
374 374 else
375 375 {
376 376 status = LFR_SUCCESSFUL;
377 377 }
378 378
379 379 return status;
380 380 }
381 381
382 382 int check_mode_transition( unsigned char requestedMode )
383 383 {
384 384 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
385 385 *
386 386 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
387 387 *
388 388 * @return LFR directive status codes:
389 389 * - LFR_SUCCESSFUL - the transition is authorized
390 390 * - LFR_DEFAULT - the transition is not authorized
391 391 *
392 392 */
393 393
394 394 int status;
395 395
396 396 switch (requestedMode)
397 397 {
398 398 case LFR_MODE_STANDBY:
399 399 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
400 400 status = LFR_DEFAULT;
401 401 }
402 402 else
403 403 {
404 404 status = LFR_SUCCESSFUL;
405 405 }
406 406 break;
407 407 case LFR_MODE_NORMAL:
408 408 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
409 409 status = LFR_DEFAULT;
410 410 }
411 411 else {
412 412 status = LFR_SUCCESSFUL;
413 413 }
414 414 break;
415 415 case LFR_MODE_BURST:
416 416 if ( lfrCurrentMode == LFR_MODE_BURST ) {
417 417 status = LFR_DEFAULT;
418 418 }
419 419 else {
420 420 status = LFR_SUCCESSFUL;
421 421 }
422 422 break;
423 423 case LFR_MODE_SBM1:
424 424 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
425 425 status = LFR_DEFAULT;
426 426 }
427 427 else {
428 428 status = LFR_SUCCESSFUL;
429 429 }
430 430 break;
431 431 case LFR_MODE_SBM2:
432 432 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
433 433 status = LFR_DEFAULT;
434 434 }
435 435 else {
436 436 status = LFR_SUCCESSFUL;
437 437 }
438 438 break;
439 439 default:
440 440 status = LFR_DEFAULT;
441 441 break;
442 442 }
443 443
444 444 return status;
445 445 }
446 446
447 447 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
448 448 {
449 449 lastValidEnterModeTime = transitionCoarseTime;
450 450 }
451 451
452 452 int check_transition_date( unsigned int transitionCoarseTime )
453 453 {
454 454 int status;
455 455 unsigned int localCoarseTime;
456 456 unsigned int deltaCoarseTime;
457 457
458 458 status = LFR_SUCCESSFUL;
459 459
460 460 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
461 461 {
462 462 status = LFR_SUCCESSFUL;
463 463 }
464 464 else
465 465 {
466 466 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
467 467
468 468 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime)
469 469
470 470 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
471 471 {
472 472 status = LFR_DEFAULT;
473 473 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n")
474 474 }
475 475
476 476 if (status == LFR_SUCCESSFUL)
477 477 {
478 478 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
479 479 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
480 480 {
481 481 status = LFR_DEFAULT;
482 482 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
483 483 }
484 484 }
485 485 }
486 486
487 487 return status;
488 488 }
489 489
490 490 int restart_asm_activities( unsigned char lfrRequestedMode )
491 491 {
492 492 rtems_status_code status;
493 493
494 494 status = stop_spectral_matrices();
495 495
496 496 status = restart_asm_tasks( lfrRequestedMode );
497 497
498 498 launch_spectral_matrix();
499 499
500 500 return status;
501 501 }
502 502
503 503 int stop_spectral_matrices( void )
504 504 {
505 505 /** This function stops and restarts the current mode average spectral matrices activities.
506 506 *
507 507 * @return RTEMS directive status codes:
508 508 * - RTEMS_SUCCESSFUL - task restarted successfully
509 509 * - RTEMS_INVALID_ID - task id invalid
510 510 * - RTEMS_ALREADY_SUSPENDED - task already suspended
511 511 *
512 512 */
513 513
514 514 rtems_status_code status;
515 515
516 516 status = RTEMS_SUCCESSFUL;
517 517
518 518 // (1) mask interruptions
519 519 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
520 520
521 521 // (2) reset spectral matrices registers
522 522 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
523 523 reset_sm_status();
524 524
525 525 // (3) clear interruptions
526 526 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
527 527
528 528 // suspend several tasks
529 529 if (lfrCurrentMode != LFR_MODE_STANDBY) {
530 530 status = suspend_asm_tasks();
531 531 }
532 532
533 533 if (status != RTEMS_SUCCESSFUL)
534 534 {
535 535 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
536 536 }
537 537
538 538 return status;
539 539 }
540 540
541 541 int stop_current_mode( void )
542 542 {
543 543 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
544 544 *
545 545 * @return RTEMS directive status codes:
546 546 * - RTEMS_SUCCESSFUL - task restarted successfully
547 547 * - RTEMS_INVALID_ID - task id invalid
548 548 * - RTEMS_ALREADY_SUSPENDED - task already suspended
549 549 *
550 550 */
551 551
552 552 rtems_status_code status;
553 553
554 554 status = RTEMS_SUCCESSFUL;
555 555
556 556 // (1) mask interruptions
557 557 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
558 558 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
559 559
560 560 // (2) reset waveform picker registers
561 561 reset_wfp_burst_enable(); // reset burst and enable bits
562 562 reset_wfp_status(); // reset all the status bits
563 563
564 564 // (3) reset spectral matrices registers
565 565 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
566 566 reset_sm_status();
567 567
568 568 // reset lfr VHDL module
569 569 reset_lfr();
570 570
571 571 reset_extractSWF(); // reset the extractSWF flag to false
572 572
573 573 // (4) clear interruptions
574 574 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
575 575 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
576 576
577 577 // suspend several tasks
578 578 if (lfrCurrentMode != LFR_MODE_STANDBY) {
579 579 status = suspend_science_tasks();
580 580 }
581 581
582 582 if (status != RTEMS_SUCCESSFUL)
583 583 {
584 584 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
585 585 }
586 586
587 587 return status;
588 588 }
589 589
590 590 int enter_mode_standby()
591 591 {
592 592 /** This function is used to put LFR in the STANDBY mode.
593 593 *
594 594 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
595 595 *
596 596 * @return RTEMS directive status codes:
597 597 * - RTEMS_SUCCESSFUL - task restarted successfully
598 598 * - RTEMS_INVALID_ID - task id invalid
599 599 * - RTEMS_INCORRECT_STATE - task never started
600 600 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
601 601 *
602 602 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
603 603 * is immediate.
604 604 *
605 605 */
606 606
607 607 int status;
608 608
609 609 status = stop_current_mode(); // STOP THE CURRENT MODE
610 610
611 611 #ifdef PRINT_TASK_STATISTICS
612 612 rtems_cpu_usage_report();
613 613 #endif
614 614
615 615 #ifdef PRINT_STACK_REPORT
616 616 PRINTF("stack report selected\n")
617 617 rtems_stack_checker_report_usage();
618 618 #endif
619 619
620 620 return status;
621 621 }
622 622
623 623 int enter_mode_normal( unsigned int transitionCoarseTime )
624 624 {
625 625 /** This function is used to start the NORMAL mode.
626 626 *
627 627 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
628 628 *
629 629 * @return RTEMS directive status codes:
630 630 * - RTEMS_SUCCESSFUL - task restarted successfully
631 631 * - RTEMS_INVALID_ID - task id invalid
632 632 * - RTEMS_INCORRECT_STATE - task never started
633 633 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
634 634 *
635 635 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
636 636 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
637 637 *
638 638 */
639 639
640 640 int status;
641 641
642 642 #ifdef PRINT_TASK_STATISTICS
643 643 rtems_cpu_usage_reset();
644 644 #endif
645 645
646 646 status = RTEMS_UNSATISFIED;
647 647
648 648 switch( lfrCurrentMode )
649 649 {
650 650 case LFR_MODE_STANDBY:
651 651 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
652 652 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
653 653 {
654 654 launch_spectral_matrix( );
655 655 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
656 656 }
657 657 break;
658 658 case LFR_MODE_BURST:
659 659 status = stop_current_mode(); // stop the current mode
660 660 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
661 661 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
662 662 {
663 663 launch_spectral_matrix( );
664 664 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
665 665 }
666 666 break;
667 667 case LFR_MODE_SBM1:
668 restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
668 restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
669 669 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
670 670 break;
671 671 case LFR_MODE_SBM2:
672 restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
672 restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
673 673 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
674 674 break;
675 675 default:
676 676 break;
677 677 }
678 678
679 679 if (status != RTEMS_SUCCESSFUL)
680 680 {
681 681 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
682 682 status = RTEMS_UNSATISFIED;
683 683 }
684 684
685 685 return status;
686 686 }
687 687
688 688 int enter_mode_burst( unsigned int transitionCoarseTime )
689 689 {
690 690 /** This function is used to start the BURST mode.
691 691 *
692 692 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
693 693 *
694 694 * @return RTEMS directive status codes:
695 695 * - RTEMS_SUCCESSFUL - task restarted successfully
696 696 * - RTEMS_INVALID_ID - task id invalid
697 697 * - RTEMS_INCORRECT_STATE - task never started
698 698 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
699 699 *
700 700 * The way the BURST mode is started does not depend on the LFR current mode.
701 701 *
702 702 */
703 703
704 704
705 705 int status;
706 706
707 707 #ifdef PRINT_TASK_STATISTICS
708 708 rtems_cpu_usage_reset();
709 709 #endif
710 710
711 711 status = stop_current_mode(); // stop the current mode
712 712 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
713 713 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
714 714 {
715 715 launch_spectral_matrix( );
716 716 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
717 717 }
718 718
719 719 if (status != RTEMS_SUCCESSFUL)
720 720 {
721 721 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
722 722 status = RTEMS_UNSATISFIED;
723 723 }
724 724
725 725 return status;
726 726 }
727 727
728 728 int enter_mode_sbm1( unsigned int transitionCoarseTime )
729 729 {
730 730 /** This function is used to start the SBM1 mode.
731 731 *
732 732 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
733 733 *
734 734 * @return RTEMS directive status codes:
735 735 * - RTEMS_SUCCESSFUL - task restarted successfully
736 736 * - RTEMS_INVALID_ID - task id invalid
737 737 * - RTEMS_INCORRECT_STATE - task never started
738 738 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
739 739 *
740 740 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
741 741 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
742 742 * cases, the acquisition is completely restarted.
743 743 *
744 744 */
745 745
746 746 int status;
747 747
748 748 #ifdef PRINT_TASK_STATISTICS
749 749 rtems_cpu_usage_reset();
750 750 #endif
751 751
752 752 status = RTEMS_UNSATISFIED;
753 753
754 754 switch( lfrCurrentMode )
755 755 {
756 756 case LFR_MODE_STANDBY:
757 757 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
758 758 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
759 759 {
760 760 launch_spectral_matrix( );
761 761 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
762 762 }
763 763 break;
764 764 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
765 765 restart_asm_activities( LFR_MODE_SBM1 );
766 766 status = LFR_SUCCESSFUL;
767 767 break;
768 768 case LFR_MODE_BURST:
769 769 status = stop_current_mode(); // stop the current mode
770 770 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
771 771 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
772 772 {
773 773 launch_spectral_matrix( );
774 774 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
775 775 }
776 776 break;
777 777 case LFR_MODE_SBM2:
778 778 restart_asm_activities( LFR_MODE_SBM1 );
779 779 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
780 780 break;
781 781 default:
782 782 break;
783 783 }
784 784
785 785 if (status != RTEMS_SUCCESSFUL)
786 786 {
787 787 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status)
788 788 status = RTEMS_UNSATISFIED;
789 789 }
790 790
791 791 return status;
792 792 }
793 793
794 794 int enter_mode_sbm2( unsigned int transitionCoarseTime )
795 795 {
796 796 /** This function is used to start the SBM2 mode.
797 797 *
798 798 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
799 799 *
800 800 * @return RTEMS directive status codes:
801 801 * - RTEMS_SUCCESSFUL - task restarted successfully
802 802 * - RTEMS_INVALID_ID - task id invalid
803 803 * - RTEMS_INCORRECT_STATE - task never started
804 804 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
805 805 *
806 806 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
807 807 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
808 808 * cases, the acquisition is completely restarted.
809 809 *
810 810 */
811 811
812 812 int status;
813 813
814 814 #ifdef PRINT_TASK_STATISTICS
815 815 rtems_cpu_usage_reset();
816 816 #endif
817 817
818 818 status = RTEMS_UNSATISFIED;
819 819
820 820 switch( lfrCurrentMode )
821 821 {
822 822 case LFR_MODE_STANDBY:
823 823 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
824 824 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
825 825 {
826 826 launch_spectral_matrix( );
827 827 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
828 828 }
829 829 break;
830 830 case LFR_MODE_NORMAL:
831 831 restart_asm_activities( LFR_MODE_SBM2 );
832 832 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
833 833 break;
834 834 case LFR_MODE_BURST:
835 835 status = stop_current_mode(); // stop the current mode
836 836 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
837 837 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
838 838 {
839 839 launch_spectral_matrix( );
840 840 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
841 841 }
842 842 break;
843 843 case LFR_MODE_SBM1:
844 844 restart_asm_activities( LFR_MODE_SBM2 );
845 845 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
846 846 break;
847 847 default:
848 848 break;
849 849 }
850 850
851 851 if (status != RTEMS_SUCCESSFUL)
852 852 {
853 853 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
854 854 status = RTEMS_UNSATISFIED;
855 855 }
856 856
857 857 return status;
858 858 }
859 859
860 860 int restart_science_tasks( unsigned char lfrRequestedMode )
861 861 {
862 862 /** This function is used to restart all science tasks.
863 863 *
864 864 * @return RTEMS directive status codes:
865 865 * - RTEMS_SUCCESSFUL - task restarted successfully
866 866 * - RTEMS_INVALID_ID - task id invalid
867 867 * - RTEMS_INCORRECT_STATE - task never started
868 868 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
869 869 *
870 870 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
871 871 *
872 872 */
873 873
874 874 rtems_status_code status[10];
875 875 rtems_status_code ret;
876 876
877 877 ret = RTEMS_SUCCESSFUL;
878 878
879 879 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
880 880 if (status[0] != RTEMS_SUCCESSFUL)
881 881 {
882 882 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
883 883 }
884 884
885 885 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
886 886 if (status[1] != RTEMS_SUCCESSFUL)
887 887 {
888 888 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
889 889 }
890 890
891 891 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
892 892 if (status[2] != RTEMS_SUCCESSFUL)
893 893 {
894 894 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
895 895 }
896 896
897 897 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
898 898 if (status[3] != RTEMS_SUCCESSFUL)
899 899 {
900 900 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
901 901 }
902 902
903 903 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
904 904 if (status[4] != RTEMS_SUCCESSFUL)
905 905 {
906 906 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
907 907 }
908 908
909 909 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
910 910 if (status[5] != RTEMS_SUCCESSFUL)
911 911 {
912 912 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
913 913 }
914 914
915 915 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
916 916 if (status[6] != RTEMS_SUCCESSFUL)
917 917 {
918 918 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
919 919 }
920 920
921 921 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
922 922 if (status[7] != RTEMS_SUCCESSFUL)
923 923 {
924 924 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
925 925 }
926 926
927 927 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
928 928 if (status[8] != RTEMS_SUCCESSFUL)
929 929 {
930 930 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
931 931 }
932 932
933 933 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
934 934 if (status[9] != RTEMS_SUCCESSFUL)
935 935 {
936 936 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
937 937 }
938 938
939 939 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
940 940 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
941 941 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
942 942 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
943 943 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
944 944 {
945 945 ret = RTEMS_UNSATISFIED;
946 946 }
947 947
948 948 return ret;
949 949 }
950 950
951 951 int restart_asm_tasks( unsigned char lfrRequestedMode )
952 952 {
953 953 /** This function is used to restart average spectral matrices tasks.
954 954 *
955 955 * @return RTEMS directive status codes:
956 956 * - RTEMS_SUCCESSFUL - task restarted successfully
957 957 * - RTEMS_INVALID_ID - task id invalid
958 958 * - RTEMS_INCORRECT_STATE - task never started
959 959 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
960 960 *
961 961 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
962 962 *
963 963 */
964 964
965 965 rtems_status_code status[6];
966 966 rtems_status_code ret;
967 967
968 968 ret = RTEMS_SUCCESSFUL;
969 969
970 970 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
971 971 if (status[0] != RTEMS_SUCCESSFUL)
972 972 {
973 973 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
974 974 }
975 975
976 976 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
977 977 if (status[1] != RTEMS_SUCCESSFUL)
978 978 {
979 979 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
980 980 }
981 981
982 982 status[2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
983 983 if (status[2] != RTEMS_SUCCESSFUL)
984 984 {
985 985 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[2])
986 986 }
987 987
988 988 status[3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
989 989 if (status[3] != RTEMS_SUCCESSFUL)
990 990 {
991 991 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[3])
992 992 }
993 993
994 994 status[4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
995 995 if (status[4] != RTEMS_SUCCESSFUL)
996 996 {
997 997 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[4])
998 998 }
999 999
1000 1000 status[5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1001 1001 if (status[5] != RTEMS_SUCCESSFUL)
1002 1002 {
1003 1003 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[5])
1004 1004 }
1005 1005
1006 1006 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
1007 1007 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
1008 1008 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) )
1009 1009 {
1010 1010 ret = RTEMS_UNSATISFIED;
1011 1011 }
1012 1012
1013 1013 return ret;
1014 1014 }
1015 1015
1016 1016 int suspend_science_tasks( void )
1017 1017 {
1018 1018 /** This function suspends the science tasks.
1019 1019 *
1020 1020 * @return RTEMS directive status codes:
1021 1021 * - RTEMS_SUCCESSFUL - task restarted successfully
1022 1022 * - RTEMS_INVALID_ID - task id invalid
1023 1023 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1024 1024 *
1025 1025 */
1026 1026
1027 1027 rtems_status_code status;
1028 1028
1029 1029 PRINTF("in suspend_science_tasks\n")
1030 1030
1031 1031 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1032 1032 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1033 1033 {
1034 1034 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1035 1035 }
1036 1036 else
1037 1037 {
1038 1038 status = RTEMS_SUCCESSFUL;
1039 1039 }
1040 1040 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1041 1041 {
1042 1042 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1043 1043 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1044 1044 {
1045 1045 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1046 1046 }
1047 1047 else
1048 1048 {
1049 1049 status = RTEMS_SUCCESSFUL;
1050 1050 }
1051 1051 }
1052 1052 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1053 1053 {
1054 1054 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1055 1055 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1056 1056 {
1057 1057 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1058 1058 }
1059 1059 else
1060 1060 {
1061 1061 status = RTEMS_SUCCESSFUL;
1062 1062 }
1063 1063 }
1064 1064 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1065 1065 {
1066 1066 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1067 1067 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1068 1068 {
1069 1069 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1070 1070 }
1071 1071 else
1072 1072 {
1073 1073 status = RTEMS_SUCCESSFUL;
1074 1074 }
1075 1075 }
1076 1076 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1077 1077 {
1078 1078 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1079 1079 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1080 1080 {
1081 1081 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1082 1082 }
1083 1083 else
1084 1084 {
1085 1085 status = RTEMS_SUCCESSFUL;
1086 1086 }
1087 1087 }
1088 1088 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1089 1089 {
1090 1090 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1091 1091 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1092 1092 {
1093 1093 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1094 1094 }
1095 1095 else
1096 1096 {
1097 1097 status = RTEMS_SUCCESSFUL;
1098 1098 }
1099 1099 }
1100 1100 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1101 1101 {
1102 1102 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1103 1103 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1104 1104 {
1105 1105 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1106 1106 }
1107 1107 else
1108 1108 {
1109 1109 status = RTEMS_SUCCESSFUL;
1110 1110 }
1111 1111 }
1112 1112 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1113 1113 {
1114 1114 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1115 1115 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1116 1116 {
1117 1117 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1118 1118 }
1119 1119 else
1120 1120 {
1121 1121 status = RTEMS_SUCCESSFUL;
1122 1122 }
1123 1123 }
1124 1124 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1125 1125 {
1126 1126 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1127 1127 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1128 1128 {
1129 1129 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1130 1130 }
1131 1131 else
1132 1132 {
1133 1133 status = RTEMS_SUCCESSFUL;
1134 1134 }
1135 1135 }
1136 1136 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1137 1137 {
1138 1138 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1139 1139 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1140 1140 {
1141 1141 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1142 1142 }
1143 1143 else
1144 1144 {
1145 1145 status = RTEMS_SUCCESSFUL;
1146 1146 }
1147 1147 }
1148 1148
1149 1149 return status;
1150 1150 }
1151 1151
1152 1152 int suspend_asm_tasks( void )
1153 1153 {
1154 1154 /** This function suspends the science tasks.
1155 1155 *
1156 1156 * @return RTEMS directive status codes:
1157 1157 * - RTEMS_SUCCESSFUL - task restarted successfully
1158 1158 * - RTEMS_INVALID_ID - task id invalid
1159 1159 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1160 1160 *
1161 1161 */
1162 1162
1163 1163 rtems_status_code status;
1164 1164
1165 1165 PRINTF("in suspend_science_tasks\n")
1166 1166
1167 1167 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1168 1168 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1169 1169 {
1170 1170 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1171 1171 }
1172 1172 else
1173 1173 {
1174 1174 status = RTEMS_SUCCESSFUL;
1175 1175 }
1176 1176
1177 1177 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1178 1178 {
1179 1179 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1180 1180 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1181 1181 {
1182 1182 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1183 1183 }
1184 1184 else
1185 1185 {
1186 1186 status = RTEMS_SUCCESSFUL;
1187 1187 }
1188 1188 }
1189 1189
1190 1190 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1191 1191 {
1192 1192 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1193 1193 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1194 1194 {
1195 1195 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1196 1196 }
1197 1197 else
1198 1198 {
1199 1199 status = RTEMS_SUCCESSFUL;
1200 1200 }
1201 1201 }
1202 1202
1203 1203 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1204 1204 {
1205 1205 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1206 1206 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1207 1207 {
1208 1208 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1209 1209 }
1210 1210 else
1211 1211 {
1212 1212 status = RTEMS_SUCCESSFUL;
1213 1213 }
1214 1214 }
1215 1215
1216 1216 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1217 1217 {
1218 1218 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1219 1219 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1220 1220 {
1221 1221 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1222 1222 }
1223 1223 else
1224 1224 {
1225 1225 status = RTEMS_SUCCESSFUL;
1226 1226 }
1227 1227 }
1228 1228
1229 1229 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1230 1230 {
1231 1231 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1232 1232 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1233 1233 {
1234 1234 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1235 1235 }
1236 1236 else
1237 1237 {
1238 1238 status = RTEMS_SUCCESSFUL;
1239 1239 }
1240 1240 }
1241 1241
1242 1242 return status;
1243 1243 }
1244 1244
1245 1245 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1246 1246 {
1247 1247 WFP_reset_current_ring_nodes();
1248 1248
1249 1249 reset_waveform_picker_regs();
1250 1250
1251 1251 set_wfp_burst_enable_register( mode );
1252 1252
1253 1253 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1254 1254 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1255 1255
1256 1256 if (transitionCoarseTime == 0)
1257 1257 {
1258 1258 // instant transition means transition on the next valid date
1259 1259 // this is mandatory to have a good snapshot period a a good correction of the snapshot period
1260 1260 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1261 1261 }
1262 1262 else
1263 1263 {
1264 1264 waveform_picker_regs->start_date = transitionCoarseTime;
1265 1265 }
1266 1266
1267 1267 }
1268 1268
1269 1269 void launch_spectral_matrix( void )
1270 1270 {
1271 1271 SM_reset_current_ring_nodes();
1272 1272
1273 1273 reset_spectral_matrix_regs();
1274 1274
1275 1275 reset_nb_sm();
1276 1276
1277 1277 set_sm_irq_onNewMatrix( 1 );
1278 1278
1279 1279 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1280 1280 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1281 1281
1282 1282 }
1283 1283
1284 1284 void set_sm_irq_onNewMatrix( unsigned char value )
1285 1285 {
1286 1286 if (value == 1)
1287 1287 {
1288 1288 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
1289 1289 }
1290 1290 else
1291 1291 {
1292 1292 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
1293 1293 }
1294 1294 }
1295 1295
1296 1296 void set_sm_irq_onError( unsigned char value )
1297 1297 {
1298 1298 if (value == 1)
1299 1299 {
1300 1300 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
1301 1301 }
1302 1302 else
1303 1303 {
1304 1304 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
1305 1305 }
1306 1306 }
1307 1307
1308 1308 //*****************************
1309 1309 // CONFIGURE CALIBRATION SIGNAL
1310 1310 void setCalibrationPrescaler( unsigned int prescaler )
1311 1311 {
1312 1312 // prescaling of the master clock (25 MHz)
1313 1313 // master clock is divided by 2^prescaler
1314 1314 time_management_regs->calPrescaler = prescaler;
1315 1315 }
1316 1316
1317 1317 void setCalibrationDivisor( unsigned int divisionFactor )
1318 1318 {
1319 1319 // division of the prescaled clock by the division factor
1320 1320 time_management_regs->calDivisor = divisionFactor;
1321 1321 }
1322 1322
1323 1323 void setCalibrationData( void ){
1324 1324 unsigned int k;
1325 1325 unsigned short data;
1326 1326 float val;
1327 1327 float f0;
1328 1328 float f1;
1329 1329 float fs;
1330 1330 float Ts;
1331 1331 float scaleFactor;
1332 1332
1333 1333 f0 = 625;
1334 1334 f1 = 10000;
1335 1335 fs = 160256.410;
1336 1336 Ts = 1. / fs;
1337 1337 scaleFactor = 0.250 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
1338 1338
1339 1339 time_management_regs->calDataPtr = 0x00;
1340 1340
1341 1341 // build the signal for the SCM calibration
1342 1342 for (k=0; k<256; k++)
1343 1343 {
1344 1344 val = sin( 2 * pi * f0 * k * Ts )
1345 1345 + sin( 2 * pi * f1 * k * Ts );
1346 1346 data = (unsigned short) ((val * scaleFactor) + 2048);
1347 1347 time_management_regs->calData = data & 0xfff;
1348 1348 }
1349 1349 }
1350 1350
1351 1351 void setCalibrationDataInterleaved( void ){
1352 1352 unsigned int k;
1353 1353 float val;
1354 1354 float f0;
1355 1355 float f1;
1356 1356 float fs;
1357 1357 float Ts;
1358 1358 unsigned short data[384];
1359 1359 unsigned char *dataPtr;
1360 1360
1361 1361 f0 = 625;
1362 1362 f1 = 10000;
1363 1363 fs = 240384.615;
1364 1364 Ts = 1. / fs;
1365 1365
1366 1366 time_management_regs->calDataPtr = 0x00;
1367 1367
1368 1368 // build the signal for the SCM calibration
1369 1369 for (k=0; k<384; k++)
1370 1370 {
1371 1371 val = sin( 2 * pi * f0 * k * Ts )
1372 1372 + sin( 2 * pi * f1 * k * Ts );
1373 1373 data[k] = (unsigned short) (val * 512 + 2048);
1374 1374 }
1375 1375
1376 1376 // write the signal in interleaved mode
1377 1377 for (k=0; k<128; k++)
1378 1378 {
1379 1379 dataPtr = (unsigned char*) &data[k*3 + 2];
1380 1380 time_management_regs->calData = (data[k*3] & 0xfff)
1381 1381 + ( (dataPtr[0] & 0x3f) << 12);
1382 1382 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
1383 1383 + ( (dataPtr[1] & 0x3f) << 12);
1384 1384 }
1385 1385 }
1386 1386
1387 1387 void setCalibrationReload( bool state)
1388 1388 {
1389 1389 if (state == true)
1390 1390 {
1391 1391 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
1392 1392 }
1393 1393 else
1394 1394 {
1395 1395 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
1396 1396 }
1397 1397 }
1398 1398
1399 1399 void setCalibrationEnable( bool state )
1400 1400 {
1401 1401 // this bit drives the multiplexer
1402 1402 if (state == true)
1403 1403 {
1404 1404 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
1405 1405 }
1406 1406 else
1407 1407 {
1408 1408 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
1409 1409 }
1410 1410 }
1411 1411
1412 1412 void setCalibrationInterleaved( bool state )
1413 1413 {
1414 1414 // this bit drives the multiplexer
1415 1415 if (state == true)
1416 1416 {
1417 1417 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
1418 1418 }
1419 1419 else
1420 1420 {
1421 1421 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
1422 1422 }
1423 1423 }
1424 1424
1425 1425 void setCalibration( bool state )
1426 1426 {
1427 1427 if (state == true)
1428 1428 {
1429 1429 setCalibrationEnable( true );
1430 1430 setCalibrationReload( false );
1431 1431 set_hk_lfr_calib_enable( true );
1432 1432 }
1433 1433 else
1434 1434 {
1435 1435 setCalibrationEnable( false );
1436 1436 setCalibrationReload( true );
1437 1437 set_hk_lfr_calib_enable( false );
1438 1438 }
1439 1439 }
1440 1440
1441 1441 void configureCalibration( bool interleaved )
1442 1442 {
1443 1443 setCalibration( false );
1444 1444 if ( interleaved == true )
1445 1445 {
1446 1446 setCalibrationInterleaved( true );
1447 1447 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1448 1448 setCalibrationDivisor( 26 ); // => 240 384
1449 1449 setCalibrationDataInterleaved();
1450 1450 }
1451 1451 else
1452 1452 {
1453 1453 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1454 1454 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
1455 1455 setCalibrationData();
1456 1456 }
1457 1457 }
1458 1458
1459 1459 //****************
1460 1460 // CLOSING ACTIONS
1461 1461 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1462 1462 {
1463 1463 /** This function is used to update the HK packets statistics after a successful TC execution.
1464 1464 *
1465 1465 * @param TC points to the TC being processed
1466 1466 * @param time is the time used to date the TC execution
1467 1467 *
1468 1468 */
1469 1469
1470 1470 unsigned int val;
1471 1471
1472 1472 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1473 1473 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1474 1474 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
1475 1475 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1476 1476 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
1477 1477 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1478 1478 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
1479 1479 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
1480 1480 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
1481 1481 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
1482 1482 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1483 1483 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1484 1484
1485 1485 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1486 1486 val++;
1487 1487 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1488 1488 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1489 1489 }
1490 1490
1491 1491 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1492 1492 {
1493 1493 /** This function is used to update the HK packets statistics after a TC rejection.
1494 1494 *
1495 1495 * @param TC points to the TC being processed
1496 1496 * @param time is the time used to date the TC rejection
1497 1497 *
1498 1498 */
1499 1499
1500 1500 unsigned int val;
1501 1501
1502 1502 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1503 1503 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1504 1504 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1505 1505 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1506 1506 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1507 1507 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1508 1508 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1509 1509 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1510 1510 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1511 1511 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1512 1512 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1513 1513 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1514 1514
1515 1515 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1516 1516 val++;
1517 1517 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1518 1518 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1519 1519 }
1520 1520
1521 1521 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1522 1522 {
1523 1523 /** This function is the last step of the TC execution workflow.
1524 1524 *
1525 1525 * @param TC points to the TC being processed
1526 1526 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1527 1527 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1528 1528 * @param time is the time used to date the TC execution
1529 1529 *
1530 1530 */
1531 1531
1532 1532 unsigned char requestedMode;
1533 1533
1534 1534 if (result == LFR_SUCCESSFUL)
1535 1535 {
1536 1536 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1537 1537 &
1538 1538 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1539 1539 )
1540 1540 {
1541 1541 send_tm_lfr_tc_exe_success( TC, queue_id );
1542 1542 }
1543 1543 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1544 1544 {
1545 1545 //**********************************
1546 1546 // UPDATE THE LFRMODE LOCAL VARIABLE
1547 1547 requestedMode = TC->dataAndCRC[1];
1548 1548 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1549 1549 updateLFRCurrentMode();
1550 1550 }
1551 1551 }
1552 1552 else if (result == LFR_EXE_ERROR)
1553 1553 {
1554 1554 send_tm_lfr_tc_exe_error( TC, queue_id );
1555 1555 }
1556 1556 }
1557 1557
1558 1558 //***************************
1559 1559 // Interrupt Service Routines
1560 1560 rtems_isr commutation_isr1( rtems_vector_number vector )
1561 1561 {
1562 1562 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1563 1563 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1564 1564 }
1565 1565 }
1566 1566
1567 1567 rtems_isr commutation_isr2( rtems_vector_number vector )
1568 1568 {
1569 1569 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1570 1570 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1571 1571 }
1572 1572 }
1573 1573
1574 1574 //****************
1575 1575 // OTHER FUNCTIONS
1576 1576 void updateLFRCurrentMode()
1577 1577 {
1578 1578 /** This function updates the value of the global variable lfrCurrentMode.
1579 1579 *
1580 1580 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1581 1581 *
1582 1582 */
1583 1583 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1584 1584 lfrCurrentMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
1585 1585 }
1586 1586
1587 1587 void set_lfr_soft_reset( unsigned char value )
1588 1588 {
1589 1589 if (value == 1)
1590 1590 {
1591 1591 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1592 1592 }
1593 1593 else
1594 1594 {
1595 1595 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1596 1596 }
1597 1597 }
1598 1598
1599 1599 void reset_lfr( void )
1600 1600 {
1601 1601 set_lfr_soft_reset( 1 );
1602 1602
1603 1603 set_lfr_soft_reset( 0 );
1604 1604
1605 1605 set_hk_lfr_sc_potential_flag( true );
1606 1606 }
General Comments 0
You need to be logged in to leave comments. Login now