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

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

456 532 532 533 corrected, TC_LFR_UPDATE_INFO fields copied in...
paul -
r230:f6bd2f6c2315 R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 6a30b7a924d9c3824f432332ed79bcee25954455 header/lfr_common_headers
2 82603593a3f6185e68418200fe1fee7d81fe6e3d header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,113 +1,113
1 1 TEMPLATE = app
2 2 # CONFIG += console v8 sim
3 3 # CONFIG options = verbose *** boot_messages *** debug_messages *** cpu_usage_report *** stack_report *** vhdl_dev *** debug_tch
4 4 # lpp_dpu_destid
5 5 CONFIG += console verbose lpp_dpu_destid
6 6 CONFIG -= qt
7 7
8 8 include(./sparc.pri)
9 9
10 10 # flight software version
11 11 SWVERSION=-1-0
12 12 DEFINES += SW_VERSION_N1=3 # major
13 13 DEFINES += SW_VERSION_N2=0 # minor
14 14 DEFINES += SW_VERSION_N3=0 # patch
15 DEFINES += SW_VERSION_N4=9 # internal
15 DEFINES += SW_VERSION_N4=10 # internal
16 16
17 17 # <GCOV>
18 18 #QMAKE_CFLAGS_RELEASE += -fprofile-arcs -ftest-coverage
19 19 #LIBS += -lgcov /opt/GCOV/01A/lib/overload.o -lc
20 20 #LIBS += -lgcov /opt/GCOV/HOWTO_gcov_for_lfr_test/01A/lib/overload.o -lc
21 21 # </GCOV>
22 22
23 23 # <CHANGE BEFORE FLIGHT>
24 24 contains( CONFIG, lpp_dpu_destid ) {
25 25 DEFINES += LPP_DPU_DESTID
26 26 }
27 27 # </CHANGE BEFORE FLIGHT>
28 28
29 29 contains( CONFIG, debug_tch ) {
30 30 DEFINES += DEBUG_TCH
31 31 }
32 32 DEFINES += MSB_FIRST_TCH
33 33
34 34 contains( CONFIG, vhdl_dev ) {
35 35 DEFINES += VHDL_DEV
36 36 }
37 37
38 38 contains( CONFIG, verbose ) {
39 39 DEFINES += PRINT_MESSAGES_ON_CONSOLE
40 40 }
41 41
42 42 contains( CONFIG, debug_messages ) {
43 43 DEFINES += DEBUG_MESSAGES
44 44 }
45 45
46 46 contains( CONFIG, cpu_usage_report ) {
47 47 DEFINES += PRINT_TASK_STATISTICS
48 48 }
49 49
50 50 contains( CONFIG, stack_report ) {
51 51 DEFINES += PRINT_STACK_REPORT
52 52 }
53 53
54 54 contains( CONFIG, boot_messages ) {
55 55 DEFINES += BOOT_MESSAGES
56 56 }
57 57
58 58 #doxygen.target = doxygen
59 59 #doxygen.commands = doxygen ../doc/Doxyfile
60 60 #QMAKE_EXTRA_TARGETS += doxygen
61 61
62 62 TARGET = fsw
63 63
64 64 INCLUDEPATH += \
65 65 $${PWD}/../src \
66 66 $${PWD}/../header \
67 67 $${PWD}/../header/lfr_common_headers \
68 68 $${PWD}/../header/processing \
69 69 $${PWD}/../LFR_basic-parameters
70 70
71 71 SOURCES += \
72 72 ../src/wf_handler.c \
73 73 ../src/tc_handler.c \
74 74 ../src/fsw_misc.c \
75 75 ../src/fsw_init.c \
76 76 ../src/fsw_globals.c \
77 77 ../src/fsw_spacewire.c \
78 78 ../src/tc_load_dump_parameters.c \
79 79 ../src/tm_lfr_tc_exe.c \
80 80 ../src/tc_acceptance.c \
81 81 ../src/processing/fsw_processing.c \
82 82 ../src/processing/avf0_prc0.c \
83 83 ../src/processing/avf1_prc1.c \
84 84 ../src/processing/avf2_prc2.c \
85 85 ../src/lfr_cpu_usage_report.c \
86 86 ../LFR_basic-parameters/basic_parameters.c
87 87
88 88 HEADERS += \
89 89 ../header/wf_handler.h \
90 90 ../header/tc_handler.h \
91 91 ../header/grlib_regs.h \
92 92 ../header/fsw_misc.h \
93 93 ../header/fsw_init.h \
94 94 ../header/fsw_spacewire.h \
95 95 ../header/tc_load_dump_parameters.h \
96 96 ../header/tm_lfr_tc_exe.h \
97 97 ../header/tc_acceptance.h \
98 98 ../header/processing/fsw_processing.h \
99 99 ../header/processing/avf0_prc0.h \
100 100 ../header/processing/avf1_prc1.h \
101 101 ../header/processing/avf2_prc2.h \
102 102 ../header/fsw_params_wf_handler.h \
103 103 ../header/lfr_cpu_usage_report.h \
104 104 ../header/lfr_common_headers/ccsds_types.h \
105 105 ../header/lfr_common_headers/fsw_params.h \
106 106 ../header/lfr_common_headers/fsw_params_nb_bytes.h \
107 107 ../header/lfr_common_headers/fsw_params_processing.h \
108 108 ../header/lfr_common_headers/TC_types.h \
109 109 ../header/lfr_common_headers/tm_byte_positions.h \
110 110 ../LFR_basic-parameters/basic_parameters.h \
111 111 ../LFR_basic-parameters/basic_parameters_params.h \
112 112 ../header/GscMemoryLPP.hpp
113 113
Show file before | Show file after | Hide comments
@@ -1,1312 +1,1308
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 //***********
26 26 // RTEMS TASK
27 27 rtems_task spiq_task(rtems_task_argument unused)
28 28 {
29 29 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
30 30 *
31 31 * @param unused is the starting argument of the RTEMS task
32 32 *
33 33 */
34 34
35 35 rtems_event_set event_out;
36 36 rtems_status_code status;
37 37 int linkStatus;
38 38
39 39 BOOT_PRINTF("in SPIQ *** \n")
40 40
41 41 while(true){
42 42 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
43 43 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
44 44
45 45 // [0] SUSPEND RECV AND SEND TASKS
46 46 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
47 47 if ( status != RTEMS_SUCCESSFUL ) {
48 48 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
49 49 }
50 50 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
51 51 if ( status != RTEMS_SUCCESSFUL ) {
52 52 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
53 53 }
54 54
55 55 // [1] CHECK THE LINK
56 56 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
57 57 if ( linkStatus != 5) {
58 58 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
59 59 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
60 60 }
61 61
62 62 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
63 63 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
64 64 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
65 65 {
66 66 spacewire_compute_stats_offsets();
67 67 status = spacewire_reset_link( );
68 68 }
69 69 else // [2.b] in run state, start the link
70 70 {
71 71 status = spacewire_stop_and_start_link( fdSPW ); // start the link
72 72 if ( status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
75 75 }
76 76 }
77 77
78 78 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
79 79 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
80 80 {
81 81 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
82 82 if ( status != RTEMS_SUCCESSFUL ) {
83 83 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
84 84 }
85 85 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
86 86 if ( status != RTEMS_SUCCESSFUL ) {
87 87 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
88 88 }
89 89 }
90 90 else // [3.b] the link is not in run state, go in STANDBY mode
91 91 {
92 92 status = enter_mode( LFR_MODE_STANDBY, 0 );
93 93 if ( status != RTEMS_SUCCESSFUL ) {
94 94 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
95 95 }
96 96 // wake the WTDG task up to wait for the link recovery
97 97 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
98 98 status = rtems_task_suspend( RTEMS_SELF );
99 99 }
100 100 }
101 101 }
102 102
103 103 rtems_task recv_task( rtems_task_argument unused )
104 104 {
105 105 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
106 106 *
107 107 * @param unused is the starting argument of the RTEMS task
108 108 *
109 109 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
110 110 * 1. It reads the incoming data.
111 111 * 2. Launches the acceptance procedure.
112 112 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
113 113 *
114 114 */
115 115
116 116 int len;
117 117 ccsdsTelecommandPacket_t currentTC;
118 118 unsigned char computed_CRC[ 2 ];
119 119 unsigned char currentTC_LEN_RCV[ 2 ];
120 120 unsigned char destinationID;
121 121 unsigned int estimatedPacketLength;
122 122 unsigned int parserCode;
123 123 rtems_status_code status;
124 124 rtems_id queue_recv_id;
125 125 rtems_id queue_send_id;
126 126
127 127 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
128 128
129 129 status = get_message_queue_id_recv( &queue_recv_id );
130 130 if (status != RTEMS_SUCCESSFUL)
131 131 {
132 132 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
133 133 }
134 134
135 135 status = get_message_queue_id_send( &queue_send_id );
136 136 if (status != RTEMS_SUCCESSFUL)
137 137 {
138 138 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
139 139 }
140 140
141 141 BOOT_PRINTF("in RECV *** \n")
142 142
143 143 while(1)
144 144 {
145 145 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
146 146 if (len == -1){ // error during the read call
147 147 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
148 148 }
149 149 else {
150 150 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
151 151 PRINTF("in RECV *** packet lenght too short\n")
152 152 }
153 153 else {
154 154 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
155 155 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
156 156 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
157 157 // CHECK THE TC
158 158 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
159 159 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
160 160 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
161 161 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
162 162 || (parserCode == WRONG_SRC_ID) )
163 163 { // send TM_LFR_TC_EXE_CORRUPTED
164 164 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
165 165 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
166 166 &&
167 167 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
168 168 )
169 169 {
170 170 if ( parserCode == WRONG_SRC_ID )
171 171 {
172 172 destinationID = SID_TC_GROUND;
173 173 }
174 174 else
175 175 {
176 176 destinationID = currentTC.sourceID;
177 177 }
178 178 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
179 179 computed_CRC, currentTC_LEN_RCV,
180 180 destinationID );
181 181 }
182 182 }
183 183 else
184 184 { // send valid TC to the action launcher
185 185 status = rtems_message_queue_send( queue_recv_id, &currentTC,
186 186 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
187 187 }
188 188 }
189 189 }
190 190
191 191 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
192 192
193 193 }
194 194 }
195 195
196 196 rtems_task send_task( rtems_task_argument argument)
197 197 {
198 198 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
199 199 *
200 200 * @param unused is the starting argument of the RTEMS task
201 201 *
202 202 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
203 203 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
204 204 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
205 205 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
206 206 * data it contains.
207 207 *
208 208 */
209 209
210 210 rtems_status_code status; // RTEMS status code
211 211 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
212 212 ring_node *incomingRingNodePtr;
213 213 int ring_node_address;
214 214 char *charPtr;
215 215 spw_ioctl_pkt_send *spw_ioctl_send;
216 216 size_t size; // size of the incoming TC packet
217 217 rtems_id queue_send_id;
218 218 unsigned int sid;
219 219 unsigned char sidAsUnsignedChar;
220 unsigned char type;
220 221
221 222 incomingRingNodePtr = NULL;
222 223 ring_node_address = 0;
223 224 charPtr = (char *) &ring_node_address;
224 225 sid = 0;
225 226 sidAsUnsignedChar = 0;
226 227
227 228 init_header_cwf( &headerCWF );
228 229 init_header_swf( &headerSWF );
229 230 init_header_asm( &headerASM );
230 231
231 232 status = get_message_queue_id_send( &queue_send_id );
232 233 if (status != RTEMS_SUCCESSFUL)
233 234 {
234 235 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
235 236 }
236 237
237 238 BOOT_PRINTF("in SEND *** \n")
238 239
239 240 while(1)
240 241 {
241 242 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
242 243 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
243 244
244 245 if (status!=RTEMS_SUCCESSFUL)
245 246 {
246 247 PRINTF1("in SEND *** (1) ERR = %d\n", status)
247 248 }
248 249 else
249 250 {
250 251 if ( size == sizeof(ring_node*) )
251 252 {
252 253 charPtr[0] = incomingData[0];
253 254 charPtr[1] = incomingData[1];
254 255 charPtr[2] = incomingData[2];
255 256 charPtr[3] = incomingData[3];
256 257 incomingRingNodePtr = (ring_node*) ring_node_address;
257 258 sid = incomingRingNodePtr->sid;
258 259 if ( (sid==SID_NORM_CWF_LONG_F3)
259 260 || (sid==SID_BURST_CWF_F2 )
260 261 || (sid==SID_SBM1_CWF_F1 )
261 262 || (sid==SID_SBM2_CWF_F2 ))
262 263 {
263 264 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
264 265 }
265 266 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
266 267 {
267 268 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
268 269 }
269 270 else if ( (sid==SID_NORM_CWF_F3) )
270 271 {
271 272 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
272 273 }
273 274 else if (sid==SID_NORM_ASM_F0)
274 275 {
275 276 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
276 277 }
277 278 else if (sid==SID_NORM_ASM_F1)
278 279 {
279 280 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
280 281 }
281 282 else if (sid==SID_NORM_ASM_F2)
282 283 {
283 284 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
284 285 }
285 286 else if ( sid==TM_CODE_K_DUMP )
286 287 {
287 288 spw_send_k_dump( incomingRingNodePtr );
288 289 }
289 290 else
290 291 {
291 292 PRINTF1("unexpected sid = %d\n", sid);
292 293 }
293 294 }
294 295 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
295 296 {
296 297 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
297 298 sid = sidAsUnsignedChar;
299 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
300 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
298 301 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
299 if ( (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
300 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
301 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP1_F1)
302 || (sid == SID_BURST_BP2_F0) || (sid == SID_BURST_BP2_F1)
303 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
304 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP1_F1)
305 || (sid == SID_SBM2_BP2_F0) || (sid == SID_SBM2_BP2_F1))
306 302 {
307 303 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
308 304 }
309 305
310 306 status = write( fdSPW, incomingData, size );
311 307 if (status == -1){
312 308 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
313 309 }
314 310 }
315 311 else // the incoming message is a spw_ioctl_pkt_send structure
316 312 {
317 313 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
318 314 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
319 315 if (status == -1){
320 316 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
321 317 }
322 318 }
323 319 }
324 320
325 321 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
326 322
327 323 }
328 324 }
329 325
330 326 rtems_task wtdg_task( rtems_task_argument argument )
331 327 {
332 328 rtems_event_set event_out;
333 329 rtems_status_code status;
334 330 int linkStatus;
335 331
336 332 BOOT_PRINTF("in WTDG ***\n")
337 333
338 334 while(1)
339 335 {
340 336 // wait for an RTEMS_EVENT
341 337 rtems_event_receive( RTEMS_EVENT_0,
342 338 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
343 339 PRINTF("in WTDG *** wait for the link\n")
344 340 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
345 341 while( linkStatus != 5) // wait for the link
346 342 {
347 343 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
348 344 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
349 345 }
350 346
351 347 status = spacewire_stop_and_start_link( fdSPW );
352 348
353 349 if (status != RTEMS_SUCCESSFUL)
354 350 {
355 351 PRINTF1("in WTDG *** ERR link not started %d\n", status)
356 352 }
357 353 else
358 354 {
359 355 PRINTF("in WTDG *** OK link started\n")
360 356 }
361 357
362 358 // restart the SPIQ task
363 359 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
364 360 if ( status != RTEMS_SUCCESSFUL ) {
365 361 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
366 362 }
367 363
368 364 // restart RECV and SEND
369 365 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
370 366 if ( status != RTEMS_SUCCESSFUL ) {
371 367 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
372 368 }
373 369 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
374 370 if ( status != RTEMS_SUCCESSFUL ) {
375 371 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
376 372 }
377 373 }
378 374 }
379 375
380 376 //****************
381 377 // OTHER FUNCTIONS
382 378 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
383 379 {
384 380 /** This function opens the SpaceWire link.
385 381 *
386 382 * @return a valid file descriptor in case of success, -1 in case of a failure
387 383 *
388 384 */
389 385 rtems_status_code status;
390 386
391 387 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
392 388 if ( fdSPW < 0 ) {
393 389 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
394 390 }
395 391 else
396 392 {
397 393 status = RTEMS_SUCCESSFUL;
398 394 }
399 395
400 396 return status;
401 397 }
402 398
403 399 int spacewire_start_link( int fd )
404 400 {
405 401 rtems_status_code status;
406 402
407 403 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
408 404 // -1 default hardcoded driver timeout
409 405
410 406 return status;
411 407 }
412 408
413 409 int spacewire_stop_and_start_link( int fd )
414 410 {
415 411 rtems_status_code status;
416 412
417 413 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
418 414 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
419 415 // -1 default hardcoded driver timeout
420 416
421 417 return status;
422 418 }
423 419
424 420 int spacewire_configure_link( int fd )
425 421 {
426 422 /** This function configures the SpaceWire link.
427 423 *
428 424 * @return GR-RTEMS-DRIVER directive status codes:
429 425 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
430 426 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
431 427 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
432 428 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
433 429 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
434 430 * - 5 EIO - Error when writing to grswp hardware registers.
435 431 * - 2 ENOENT - No such file or directory
436 432 */
437 433
438 434 rtems_status_code status;
439 435
440 436 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
441 437 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
442 438
443 439 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
444 440 if (status!=RTEMS_SUCCESSFUL) {
445 441 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
446 442 }
447 443 //
448 444 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
449 445 if (status!=RTEMS_SUCCESSFUL) {
450 446 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
451 447 }
452 448 //
453 449 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
454 450 if (status!=RTEMS_SUCCESSFUL) {
455 451 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
456 452 }
457 453 //
458 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
459 455 if (status!=RTEMS_SUCCESSFUL) {
460 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
461 457 }
462 458 //
463 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
464 460 if (status!=RTEMS_SUCCESSFUL) {
465 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
466 462 }
467 463 //
468 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
469 465 if (status!=RTEMS_SUCCESSFUL) {
470 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
471 467 }
472 468 //
473 469 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
474 470 if (status!=RTEMS_SUCCESSFUL) {
475 471 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
476 472 }
477 473
478 474 return status;
479 475 }
480 476
481 477 int spacewire_reset_link( void )
482 478 {
483 479 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
484 480 *
485 481 * @return RTEMS directive status code:
486 482 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
487 483 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
488 484 *
489 485 */
490 486
491 487 rtems_status_code status_spw;
492 488 rtems_status_code status;
493 489 int i;
494 490
495 491 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
496 492 {
497 493 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
498 494
499 495 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
500 496
501 497 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
502 498
503 499 status_spw = spacewire_stop_and_start_link( fdSPW );
504 500 if ( status_spw != RTEMS_SUCCESSFUL )
505 501 {
506 502 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
507 503 }
508 504
509 505 if ( status_spw == RTEMS_SUCCESSFUL)
510 506 {
511 507 break;
512 508 }
513 509 }
514 510
515 511 return status_spw;
516 512 }
517 513
518 514 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
519 515 {
520 516 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
521 517 *
522 518 * @param val is the value, 0 or 1, used to set the value of the NP bit.
523 519 * @param regAddr is the address of the GRSPW control register.
524 520 *
525 521 * NP is the bit 20 of the GRSPW control register.
526 522 *
527 523 */
528 524
529 525 unsigned int *spwptr = (unsigned int*) regAddr;
530 526
531 527 if (val == 1) {
532 528 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
533 529 }
534 530 if (val== 0) {
535 531 *spwptr = *spwptr & 0xffdfffff;
536 532 }
537 533 }
538 534
539 535 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
540 536 {
541 537 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
542 538 *
543 539 * @param val is the value, 0 or 1, used to set the value of the RE bit.
544 540 * @param regAddr is the address of the GRSPW control register.
545 541 *
546 542 * RE is the bit 16 of the GRSPW control register.
547 543 *
548 544 */
549 545
550 546 unsigned int *spwptr = (unsigned int*) regAddr;
551 547
552 548 if (val == 1)
553 549 {
554 550 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
555 551 }
556 552 if (val== 0)
557 553 {
558 554 *spwptr = *spwptr & 0xfffdffff;
559 555 }
560 556 }
561 557
562 558 void spacewire_compute_stats_offsets( void )
563 559 {
564 560 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
565 561 *
566 562 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
567 563 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
568 564 * during the open systel call).
569 565 *
570 566 */
571 567
572 568 spw_stats spacewire_stats_grspw;
573 569 rtems_status_code status;
574 570
575 571 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
576 572
577 573 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
578 574 + spacewire_stats.packets_received;
579 575 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
580 576 + spacewire_stats.packets_sent;
581 577 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
582 578 + spacewire_stats.parity_err;
583 579 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
584 580 + spacewire_stats.disconnect_err;
585 581 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
586 582 + spacewire_stats.escape_err;
587 583 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
588 584 + spacewire_stats.credit_err;
589 585 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
590 586 + spacewire_stats.write_sync_err;
591 587 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
592 588 + spacewire_stats.rx_rmap_header_crc_err;
593 589 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
594 590 + spacewire_stats.rx_rmap_data_crc_err;
595 591 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
596 592 + spacewire_stats.early_ep;
597 593 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
598 594 + spacewire_stats.invalid_address;
599 595 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
600 596 + spacewire_stats.rx_eep_err;
601 597 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
602 598 + spacewire_stats.rx_truncated;
603 599 }
604 600
605 601 void spacewire_update_statistics( void )
606 602 {
607 603 rtems_status_code status;
608 604 spw_stats spacewire_stats_grspw;
609 605
610 606 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
611 607
612 608 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
613 609 + spacewire_stats_grspw.packets_received;
614 610 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
615 611 + spacewire_stats_grspw.packets_sent;
616 612 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
617 613 + spacewire_stats_grspw.parity_err;
618 614 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
619 615 + spacewire_stats_grspw.disconnect_err;
620 616 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
621 617 + spacewire_stats_grspw.escape_err;
622 618 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
623 619 + spacewire_stats_grspw.credit_err;
624 620 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
625 621 + spacewire_stats_grspw.write_sync_err;
626 622 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
627 623 + spacewire_stats_grspw.rx_rmap_header_crc_err;
628 624 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
629 625 + spacewire_stats_grspw.rx_rmap_data_crc_err;
630 626 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
631 627 + spacewire_stats_grspw.early_ep;
632 628 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
633 629 + spacewire_stats_grspw.invalid_address;
634 630 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
635 631 + spacewire_stats_grspw.rx_eep_err;
636 632 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
637 633 + spacewire_stats_grspw.rx_truncated;
638 634 //spacewire_stats.tx_link_err;
639 635
640 636 //****************************
641 637 // DPU_SPACEWIRE_IF_STATISTICS
642 638 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
643 639 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
644 640 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
645 641 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
646 642 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
647 643 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
648 644
649 645 //******************************************
650 646 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
651 647 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
652 648 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
653 649 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
654 650 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
655 651 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
656 652
657 653 //*********************************************
658 654 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
659 655 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
660 656 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
661 657 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
662 658 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
663 659 }
664 660
665 661 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
666 662 {
667 663 // a valid timecode has been received, write it in the HK report
668 664 unsigned int *grspwPtr;
669 665 unsigned char timecodeCtr;
670 666 unsigned char updateTimeCtr;
671 667
672 668 grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
673 669
674 670 housekeeping_packet.hk_lfr_dpu_spw_last_timc = (unsigned char) (grspwPtr[0] & 0xff); // [1111 1111]
675 671 timecodeCtr = (unsigned char) (grspwPtr[0] & 0x3f); // [0011 1111]
676 672 updateTimeCtr = time_management_regs->coarse_time_load & 0x3f; // [0011 1111]
677 673
678 674 // update the number of valid timecodes that have been received
679 675 if (housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt == 255)
680 676 {
681 677 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = 0;
682 678 }
683 679 else
684 680 {
685 681 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt + 1;
686 682 }
687 683
688 684 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
689 685 if (timecodeCtr != updateTimeCtr)
690 686 {
691 687 if (housekeeping_packet.hk_lfr_time_timecode_ctr == 255)
692 688 {
693 689 housekeeping_packet.hk_lfr_time_timecode_ctr = 0;
694 690 }
695 691 else
696 692 {
697 693 housekeeping_packet.hk_lfr_time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr + 1;
698 694 }
699 695 }
700 696 }
701 697
702 698 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
703 699 {
704 700 int linkStatus;
705 701 rtems_status_code status;
706 702
707 703 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
708 704
709 705 if ( linkStatus == 5) {
710 706 PRINTF("in spacewire_reset_link *** link is running\n")
711 707 status = RTEMS_SUCCESSFUL;
712 708 }
713 709 }
714 710
715 711 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
716 712 {
717 713 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
718 714 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
719 715 header->reserved = DEFAULT_RESERVED;
720 716 header->userApplication = CCSDS_USER_APP;
721 717 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
722 718 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
723 719 header->packetLength[0] = 0x00;
724 720 header->packetLength[1] = 0x00;
725 721 // DATA FIELD HEADER
726 722 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
727 723 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
728 724 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
729 725 header->destinationID = TM_DESTINATION_ID_GROUND;
730 726 header->time[0] = 0x00;
731 727 header->time[0] = 0x00;
732 728 header->time[0] = 0x00;
733 729 header->time[0] = 0x00;
734 730 header->time[0] = 0x00;
735 731 header->time[0] = 0x00;
736 732 // AUXILIARY DATA HEADER
737 733 header->sid = 0x00;
738 734 header->hkBIA = DEFAULT_HKBIA;
739 735 header->blkNr[0] = 0x00;
740 736 header->blkNr[1] = 0x00;
741 737 }
742 738
743 739 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
744 740 {
745 741 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
746 742 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
747 743 header->reserved = DEFAULT_RESERVED;
748 744 header->userApplication = CCSDS_USER_APP;
749 745 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
750 746 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
751 747 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
752 748 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
753 749 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
754 750 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
755 751 // DATA FIELD HEADER
756 752 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
757 753 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
758 754 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
759 755 header->destinationID = TM_DESTINATION_ID_GROUND;
760 756 header->time[0] = 0x00;
761 757 header->time[0] = 0x00;
762 758 header->time[0] = 0x00;
763 759 header->time[0] = 0x00;
764 760 header->time[0] = 0x00;
765 761 header->time[0] = 0x00;
766 762 // AUXILIARY DATA HEADER
767 763 header->sid = 0x00;
768 764 header->hkBIA = DEFAULT_HKBIA;
769 765 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
770 766 header->pktNr = 0x00;
771 767 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
772 768 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
773 769 }
774 770
775 771 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
776 772 {
777 773 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
778 774 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
779 775 header->reserved = DEFAULT_RESERVED;
780 776 header->userApplication = CCSDS_USER_APP;
781 777 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
782 778 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
783 779 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
784 780 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
785 781 header->packetLength[0] = 0x00;
786 782 header->packetLength[1] = 0x00;
787 783 // DATA FIELD HEADER
788 784 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
789 785 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
790 786 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
791 787 header->destinationID = TM_DESTINATION_ID_GROUND;
792 788 header->time[0] = 0x00;
793 789 header->time[0] = 0x00;
794 790 header->time[0] = 0x00;
795 791 header->time[0] = 0x00;
796 792 header->time[0] = 0x00;
797 793 header->time[0] = 0x00;
798 794 // AUXILIARY DATA HEADER
799 795 header->sid = 0x00;
800 796 header->biaStatusInfo = 0x00;
801 797 header->pa_lfr_pkt_cnt_asm = 0x00;
802 798 header->pa_lfr_pkt_nr_asm = 0x00;
803 799 header->pa_lfr_asm_blk_nr[0] = 0x00;
804 800 header->pa_lfr_asm_blk_nr[1] = 0x00;
805 801 }
806 802
807 803 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
808 804 Header_TM_LFR_SCIENCE_CWF_t *header )
809 805 {
810 806 /** This function sends CWF CCSDS packets (F2, F1 or F0).
811 807 *
812 808 * @param waveform points to the buffer containing the data that will be send.
813 809 * @param sid is the source identifier of the data that will be sent.
814 810 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
815 811 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
816 812 * contain information to setup the transmission of the data packets.
817 813 *
818 814 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
819 815 *
820 816 */
821 817
822 818 unsigned int i;
823 819 int ret;
824 820 unsigned int coarseTime;
825 821 unsigned int fineTime;
826 822 rtems_status_code status;
827 823 spw_ioctl_pkt_send spw_ioctl_send_CWF;
828 824 int *dataPtr;
829 825 unsigned char sid;
830 826
831 827 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
832 828 spw_ioctl_send_CWF.options = 0;
833 829
834 830 ret = LFR_DEFAULT;
835 831 sid = (unsigned char) ring_node_to_send->sid;
836 832
837 833 coarseTime = ring_node_to_send->coarseTime;
838 834 fineTime = ring_node_to_send->fineTime;
839 835 dataPtr = (int*) ring_node_to_send->buffer_address;
840 836
841 837 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
842 838 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
843 839 header->hkBIA = pa_bia_status_info;
844 840 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
845 841 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
846 842 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
847 843
848 844 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
849 845 {
850 846 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
851 847 spw_ioctl_send_CWF.hdr = (char*) header;
852 848 // BUILD THE DATA
853 849 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
854 850
855 851 // SET PACKET SEQUENCE CONTROL
856 852 increment_seq_counter_source_id( header->packetSequenceControl, sid );
857 853
858 854 // SET SID
859 855 header->sid = sid;
860 856
861 857 // SET PACKET TIME
862 858 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
863 859 //
864 860 header->time[0] = header->acquisitionTime[0];
865 861 header->time[1] = header->acquisitionTime[1];
866 862 header->time[2] = header->acquisitionTime[2];
867 863 header->time[3] = header->acquisitionTime[3];
868 864 header->time[4] = header->acquisitionTime[4];
869 865 header->time[5] = header->acquisitionTime[5];
870 866
871 867 // SET PACKET ID
872 868 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
873 869 {
874 870 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
875 871 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
876 872 }
877 873 else
878 874 {
879 875 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
880 876 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
881 877 }
882 878
883 879 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
884 880 if (status != RTEMS_SUCCESSFUL) {
885 881 ret = LFR_DEFAULT;
886 882 }
887 883 }
888 884
889 885 return ret;
890 886 }
891 887
892 888 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
893 889 Header_TM_LFR_SCIENCE_SWF_t *header )
894 890 {
895 891 /** This function sends SWF CCSDS packets (F2, F1 or F0).
896 892 *
897 893 * @param waveform points to the buffer containing the data that will be send.
898 894 * @param sid is the source identifier of the data that will be sent.
899 895 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
900 896 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
901 897 * contain information to setup the transmission of the data packets.
902 898 *
903 899 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
904 900 *
905 901 */
906 902
907 903 unsigned int i;
908 904 int ret;
909 905 unsigned int coarseTime;
910 906 unsigned int fineTime;
911 907 rtems_status_code status;
912 908 spw_ioctl_pkt_send spw_ioctl_send_SWF;
913 909 int *dataPtr;
914 910 unsigned char sid;
915 911
916 912 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
917 913 spw_ioctl_send_SWF.options = 0;
918 914
919 915 ret = LFR_DEFAULT;
920 916
921 917 coarseTime = ring_node_to_send->coarseTime;
922 918 fineTime = ring_node_to_send->fineTime;
923 919 dataPtr = (int*) ring_node_to_send->buffer_address;
924 920 sid = ring_node_to_send->sid;
925 921
926 922 header->hkBIA = pa_bia_status_info;
927 923 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
928 924
929 925 for (i=0; i<7; i++) // send waveform
930 926 {
931 927 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
932 928 spw_ioctl_send_SWF.hdr = (char*) header;
933 929
934 930 // SET PACKET SEQUENCE CONTROL
935 931 increment_seq_counter_source_id( header->packetSequenceControl, sid );
936 932
937 933 // SET PACKET LENGTH AND BLKNR
938 934 if (i == 6)
939 935 {
940 936 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
941 937 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
942 938 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
943 939 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
944 940 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
945 941 }
946 942 else
947 943 {
948 944 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
949 945 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
950 946 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
951 947 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
952 948 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
953 949 }
954 950
955 951 // SET PACKET TIME
956 952 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
957 953 //
958 954 header->time[0] = header->acquisitionTime[0];
959 955 header->time[1] = header->acquisitionTime[1];
960 956 header->time[2] = header->acquisitionTime[2];
961 957 header->time[3] = header->acquisitionTime[3];
962 958 header->time[4] = header->acquisitionTime[4];
963 959 header->time[5] = header->acquisitionTime[5];
964 960
965 961 // SET SID
966 962 header->sid = sid;
967 963
968 964 // SET PKTNR
969 965 header->pktNr = i+1; // PKT_NR
970 966
971 967 // SEND PACKET
972 968 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
973 969 if (status != RTEMS_SUCCESSFUL) {
974 970 ret = LFR_DEFAULT;
975 971 }
976 972 }
977 973
978 974 return ret;
979 975 }
980 976
981 977 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
982 978 Header_TM_LFR_SCIENCE_CWF_t *header )
983 979 {
984 980 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
985 981 *
986 982 * @param waveform points to the buffer containing the data that will be send.
987 983 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
988 984 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
989 985 * contain information to setup the transmission of the data packets.
990 986 *
991 987 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
992 988 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
993 989 *
994 990 */
995 991
996 992 unsigned int i;
997 993 int ret;
998 994 unsigned int coarseTime;
999 995 unsigned int fineTime;
1000 996 rtems_status_code status;
1001 997 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1002 998 char *dataPtr;
1003 999 unsigned char sid;
1004 1000
1005 1001 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1006 1002 spw_ioctl_send_CWF.options = 0;
1007 1003
1008 1004 ret = LFR_DEFAULT;
1009 1005 sid = ring_node_to_send->sid;
1010 1006
1011 1007 coarseTime = ring_node_to_send->coarseTime;
1012 1008 fineTime = ring_node_to_send->fineTime;
1013 1009 dataPtr = (char*) ring_node_to_send->buffer_address;
1014 1010
1015 1011 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
1016 1012 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1017 1013 header->hkBIA = pa_bia_status_info;
1018 1014 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1019 1015 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
1020 1016 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1021 1017
1022 1018 //*********************
1023 1019 // SEND CWF3_light DATA
1024 1020 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1025 1021 {
1026 1022 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1027 1023 spw_ioctl_send_CWF.hdr = (char*) header;
1028 1024 // BUILD THE DATA
1029 1025 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1030 1026
1031 1027 // SET PACKET SEQUENCE COUNTER
1032 1028 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1033 1029
1034 1030 // SET SID
1035 1031 header->sid = sid;
1036 1032
1037 1033 // SET PACKET TIME
1038 1034 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1039 1035 //
1040 1036 header->time[0] = header->acquisitionTime[0];
1041 1037 header->time[1] = header->acquisitionTime[1];
1042 1038 header->time[2] = header->acquisitionTime[2];
1043 1039 header->time[3] = header->acquisitionTime[3];
1044 1040 header->time[4] = header->acquisitionTime[4];
1045 1041 header->time[5] = header->acquisitionTime[5];
1046 1042
1047 1043 // SET PACKET ID
1048 1044 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1049 1045 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1050 1046
1051 1047 // SEND PACKET
1052 1048 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1053 1049 if (status != RTEMS_SUCCESSFUL) {
1054 1050 ret = LFR_DEFAULT;
1055 1051 }
1056 1052 }
1057 1053
1058 1054 return ret;
1059 1055 }
1060 1056
1061 1057 void spw_send_asm_f0( ring_node *ring_node_to_send,
1062 1058 Header_TM_LFR_SCIENCE_ASM_t *header )
1063 1059 {
1064 1060 unsigned int i;
1065 1061 unsigned int length = 0;
1066 1062 rtems_status_code status;
1067 1063 unsigned int sid;
1068 1064 float *spectral_matrix;
1069 1065 int coarseTime;
1070 1066 int fineTime;
1071 1067 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1072 1068
1073 1069 sid = ring_node_to_send->sid;
1074 1070 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1075 1071 coarseTime = ring_node_to_send->coarseTime;
1076 1072 fineTime = ring_node_to_send->fineTime;
1077 1073
1078 1074 header->biaStatusInfo = pa_bia_status_info;
1079 1075 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1080 1076
1081 1077 for (i=0; i<3; i++)
1082 1078 {
1083 1079 if ((i==0) || (i==1))
1084 1080 {
1085 1081 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1086 1082 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1087 1083 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1088 1084 ];
1089 1085 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1090 1086 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1091 1087 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1092 1088 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1093 1089 }
1094 1090 else
1095 1091 {
1096 1092 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1097 1093 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1098 1094 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1099 1095 ];
1100 1096 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1101 1097 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1102 1098 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1103 1099 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1104 1100 }
1105 1101
1106 1102 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1107 1103 spw_ioctl_send_ASM.hdr = (char *) header;
1108 1104 spw_ioctl_send_ASM.options = 0;
1109 1105
1110 1106 // (2) BUILD THE HEADER
1111 1107 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1112 1108 header->packetLength[0] = (unsigned char) (length>>8);
1113 1109 header->packetLength[1] = (unsigned char) (length);
1114 1110 header->sid = (unsigned char) sid; // SID
1115 1111 header->pa_lfr_pkt_cnt_asm = 3;
1116 1112 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1117 1113
1118 1114 // (3) SET PACKET TIME
1119 1115 header->time[0] = (unsigned char) (coarseTime>>24);
1120 1116 header->time[1] = (unsigned char) (coarseTime>>16);
1121 1117 header->time[2] = (unsigned char) (coarseTime>>8);
1122 1118 header->time[3] = (unsigned char) (coarseTime);
1123 1119 header->time[4] = (unsigned char) (fineTime>>8);
1124 1120 header->time[5] = (unsigned char) (fineTime);
1125 1121 //
1126 1122 header->acquisitionTime[0] = header->time[0];
1127 1123 header->acquisitionTime[1] = header->time[1];
1128 1124 header->acquisitionTime[2] = header->time[2];
1129 1125 header->acquisitionTime[3] = header->time[3];
1130 1126 header->acquisitionTime[4] = header->time[4];
1131 1127 header->acquisitionTime[5] = header->time[5];
1132 1128
1133 1129 // (4) SEND PACKET
1134 1130 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1135 1131 if (status != RTEMS_SUCCESSFUL) {
1136 1132 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1137 1133 }
1138 1134 }
1139 1135 }
1140 1136
1141 1137 void spw_send_asm_f1( ring_node *ring_node_to_send,
1142 1138 Header_TM_LFR_SCIENCE_ASM_t *header )
1143 1139 {
1144 1140 unsigned int i;
1145 1141 unsigned int length = 0;
1146 1142 rtems_status_code status;
1147 1143 unsigned int sid;
1148 1144 float *spectral_matrix;
1149 1145 int coarseTime;
1150 1146 int fineTime;
1151 1147 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1152 1148
1153 1149 sid = ring_node_to_send->sid;
1154 1150 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1155 1151 coarseTime = ring_node_to_send->coarseTime;
1156 1152 fineTime = ring_node_to_send->fineTime;
1157 1153
1158 1154 header->biaStatusInfo = pa_bia_status_info;
1159 1155 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1160 1156
1161 1157 for (i=0; i<3; i++)
1162 1158 {
1163 1159 if ((i==0) || (i==1))
1164 1160 {
1165 1161 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1166 1162 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1167 1163 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1168 1164 ];
1169 1165 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1170 1166 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1171 1167 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1172 1168 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1173 1169 }
1174 1170 else
1175 1171 {
1176 1172 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1177 1173 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1178 1174 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1179 1175 ];
1180 1176 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1181 1177 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1182 1178 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1183 1179 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1184 1180 }
1185 1181
1186 1182 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1187 1183 spw_ioctl_send_ASM.hdr = (char *) header;
1188 1184 spw_ioctl_send_ASM.options = 0;
1189 1185
1190 1186 // (2) BUILD THE HEADER
1191 1187 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1192 1188 header->packetLength[0] = (unsigned char) (length>>8);
1193 1189 header->packetLength[1] = (unsigned char) (length);
1194 1190 header->sid = (unsigned char) sid; // SID
1195 1191 header->pa_lfr_pkt_cnt_asm = 3;
1196 1192 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1197 1193
1198 1194 // (3) SET PACKET TIME
1199 1195 header->time[0] = (unsigned char) (coarseTime>>24);
1200 1196 header->time[1] = (unsigned char) (coarseTime>>16);
1201 1197 header->time[2] = (unsigned char) (coarseTime>>8);
1202 1198 header->time[3] = (unsigned char) (coarseTime);
1203 1199 header->time[4] = (unsigned char) (fineTime>>8);
1204 1200 header->time[5] = (unsigned char) (fineTime);
1205 1201 //
1206 1202 header->acquisitionTime[0] = header->time[0];
1207 1203 header->acquisitionTime[1] = header->time[1];
1208 1204 header->acquisitionTime[2] = header->time[2];
1209 1205 header->acquisitionTime[3] = header->time[3];
1210 1206 header->acquisitionTime[4] = header->time[4];
1211 1207 header->acquisitionTime[5] = header->time[5];
1212 1208
1213 1209 // (4) SEND PACKET
1214 1210 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1215 1211 if (status != RTEMS_SUCCESSFUL) {
1216 1212 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1217 1213 }
1218 1214 }
1219 1215 }
1220 1216
1221 1217 void spw_send_asm_f2( ring_node *ring_node_to_send,
1222 1218 Header_TM_LFR_SCIENCE_ASM_t *header )
1223 1219 {
1224 1220 unsigned int i;
1225 1221 unsigned int length = 0;
1226 1222 rtems_status_code status;
1227 1223 unsigned int sid;
1228 1224 float *spectral_matrix;
1229 1225 int coarseTime;
1230 1226 int fineTime;
1231 1227 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1232 1228
1233 1229 sid = ring_node_to_send->sid;
1234 1230 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1235 1231 coarseTime = ring_node_to_send->coarseTime;
1236 1232 fineTime = ring_node_to_send->fineTime;
1237 1233
1238 1234 header->biaStatusInfo = pa_bia_status_info;
1239 1235 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1240 1236
1241 1237 for (i=0; i<3; i++)
1242 1238 {
1243 1239
1244 1240 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1245 1241 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1246 1242 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1247 1243 ];
1248 1244 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1249 1245 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1250 1246 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1251 1247 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1252 1248
1253 1249 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1254 1250 spw_ioctl_send_ASM.hdr = (char *) header;
1255 1251 spw_ioctl_send_ASM.options = 0;
1256 1252
1257 1253 // (2) BUILD THE HEADER
1258 1254 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1259 1255 header->packetLength[0] = (unsigned char) (length>>8);
1260 1256 header->packetLength[1] = (unsigned char) (length);
1261 1257 header->sid = (unsigned char) sid; // SID
1262 1258 header->pa_lfr_pkt_cnt_asm = 3;
1263 1259 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1264 1260
1265 1261 // (3) SET PACKET TIME
1266 1262 header->time[0] = (unsigned char) (coarseTime>>24);
1267 1263 header->time[1] = (unsigned char) (coarseTime>>16);
1268 1264 header->time[2] = (unsigned char) (coarseTime>>8);
1269 1265 header->time[3] = (unsigned char) (coarseTime);
1270 1266 header->time[4] = (unsigned char) (fineTime>>8);
1271 1267 header->time[5] = (unsigned char) (fineTime);
1272 1268 //
1273 1269 header->acquisitionTime[0] = header->time[0];
1274 1270 header->acquisitionTime[1] = header->time[1];
1275 1271 header->acquisitionTime[2] = header->time[2];
1276 1272 header->acquisitionTime[3] = header->time[3];
1277 1273 header->acquisitionTime[4] = header->time[4];
1278 1274 header->acquisitionTime[5] = header->time[5];
1279 1275
1280 1276 // (4) SEND PACKET
1281 1277 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1282 1278 if (status != RTEMS_SUCCESSFUL) {
1283 1279 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1284 1280 }
1285 1281 }
1286 1282 }
1287 1283
1288 1284 void spw_send_k_dump( ring_node *ring_node_to_send )
1289 1285 {
1290 1286 rtems_status_code status;
1291 1287 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1292 1288 unsigned int packetLength;
1293 1289 unsigned int size;
1294 1290
1295 1291 PRINTF("spw_send_k_dump\n")
1296 1292
1297 1293 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1298 1294
1299 1295 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1300 1296
1301 1297 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1302 1298
1303 1299 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1304 1300
1305 1301 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1306 1302
1307 1303 if (status == -1){
1308 1304 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1309 1305 }
1310 1306
1311 1307 ring_node_to_send->status = 0x00;
1312 1308 }
Show file before | Show file after | Hide comments
@@ -1,404 +1,408
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf0_prc0.h"
11 11 #include "fsw_processing.h"
12 12
13 13 nb_sm_before_bp_asm_f0 nb_sm_before_f0;
14 14
15 15 //***
16 16 // F0
17 17 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ];
18 18 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ];
19 19
20 20 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ];
21 21 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f0_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f0_patched_burst_sbm [ TOTAL_SIZE_SM ];
25 25 float asm_f0_reorganized [ TOTAL_SIZE_SM ];
26 26
27 27 char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
28 28 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0];
29 29 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ];
30 30
31 31 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
32 32 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
33 33
34 34 //************
35 35 // RTEMS TASKS
36 36
37 37 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
38 38 {
39 39 int i;
40 40
41 41 rtems_event_set event_out;
42 42 rtems_status_code status;
43 43 rtems_id queue_id_prc0;
44 44 asm_msg msgForMATR;
45 45 ring_node *nodeForAveraging;
46 46 ring_node *ring_node_tab[8];
47 47 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
48 48 ring_node_asm *current_ring_node_asm_norm_f0;
49 49
50 50 unsigned int nb_norm_bp1;
51 51 unsigned int nb_norm_bp2;
52 52 unsigned int nb_norm_asm;
53 53 unsigned int nb_sbm_bp1;
54 54 unsigned int nb_sbm_bp2;
55 55
56 56 nb_norm_bp1 = 0;
57 57 nb_norm_bp2 = 0;
58 58 nb_norm_asm = 0;
59 59 nb_sbm_bp1 = 0;
60 60 nb_sbm_bp2 = 0;
61 61
62 62 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
63 63 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
64 64 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
65 65 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
66 66 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
67 67
68 68 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
69 69
70 70 status = get_message_queue_id_prc0( &queue_id_prc0 );
71 71 if (status != RTEMS_SUCCESSFUL)
72 72 {
73 73 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
74 74 }
75 75
76 76 while(1){
77 77 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
78 78
79 79 //****************************************
80 80 // initialize the mesage for the MATR task
81 81 msgForMATR.norm = current_ring_node_asm_norm_f0;
82 82 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 83 msgForMATR.event = 0x00; // this composite event will be sent to the PRC0 task
84 84 //
85 85 //****************************************
86 86
87 87 nodeForAveraging = getRingNodeForAveraging( 0 );
88 88
89 89 ring_node_tab[NB_SM_BEFORE_AVF0-1] = nodeForAveraging;
90 90 for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ )
91 91 {
92 92 nodeForAveraging = nodeForAveraging->previous;
93 93 ring_node_tab[NB_SM_BEFORE_AVF0-i] = nodeForAveraging;
94 94 }
95 95
96 96 // compute the average and store it in the averaged_sm_f1 buffer
97 97 SM_average( current_ring_node_asm_norm_f0->matrix,
98 98 current_ring_node_asm_burst_sbm_f0->matrix,
99 99 ring_node_tab,
100 100 nb_norm_bp1, nb_sbm_bp1,
101 101 &msgForMATR );
102 102
103 103 // update nb_average
104 104 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0;
105 105 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0;
106 106 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0;
107 107 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0;
108 108 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0;
109 109
110 110 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
111 111 {
112 112 nb_sbm_bp1 = 0;
113 113 // set another ring for the ASM storage
114 114 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
115 115 if ( lfrCurrentMode == LFR_MODE_BURST )
116 116 {
117 117 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F0;
118 118 }
119 119 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 120 {
121 121 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F0;
122 122 }
123 123 }
124 124
125 125 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
126 126 {
127 127 nb_sbm_bp2 = 0;
128 128 if ( lfrCurrentMode == LFR_MODE_BURST )
129 129 {
130 130 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F0;
131 131 }
132 132 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 133 {
134 134 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F0;
135 135 }
136 136 }
137 137
138 138 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
139 139 {
140 140 nb_norm_bp1 = 0;
141 141 // set another ring for the ASM storage
142 142 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
143 143 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
144 144 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
145 145 {
146 146 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0;
147 147 }
148 148 }
149 149
150 150 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
151 151 {
152 152 nb_norm_bp2 = 0;
153 153 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
154 154 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
155 155 {
156 156 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0;
157 157 }
158 158 }
159 159
160 160 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
161 161 {
162 162 nb_norm_asm = 0;
163 163 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
164 164 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
165 165 {
166 166 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0;
167 167 }
168 168 }
169 169
170 170 //*************************
171 171 // send the message to MATR
172 172 if (msgForMATR.event != 0x00)
173 173 {
174 174 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
175 175 }
176 176
177 177 if (status != RTEMS_SUCCESSFUL) {
178 178 PRINTF1("in AVF0 *** Error sending message to MATR, code %d\n", status)
179 179 }
180 180 }
181 181 }
182 182
183 183 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 184 {
185 185 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 186 size_t size; // size of the incoming TC packet
187 187 asm_msg *incomingMsg;
188 188 //
189 189 unsigned char sid;
190 190 rtems_status_code status;
191 191 rtems_id queue_id;
192 192 rtems_id queue_id_q_p0;
193 193 bp_packet_with_spare packet_norm_bp1;
194 194 bp_packet packet_norm_bp2;
195 195 bp_packet packet_sbm_bp1;
196 196 bp_packet packet_sbm_bp2;
197 197 ring_node *current_ring_node_to_send_asm_f0;
198 198
199 199 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
200 200 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
201 201 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
202 202
203 203 //*************
204 204 // NORM headers
205 205 BP_init_header_with_spare( &packet_norm_bp1,
206 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
207 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
208 208 BP_init_header( &packet_norm_bp2,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
211 211
212 212 //****************************
213 213 // BURST SBM1 and SBM2 headers
214 214 if ( lfrRequestedMode == LFR_MODE_BURST )
215 215 {
216 216 BP_init_header( &packet_sbm_bp1,
217 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
218 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
219 219 BP_init_header( &packet_sbm_bp2,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
222 222 }
223 223 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
224 224 {
225 225 BP_init_header( &packet_sbm_bp1,
226 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
227 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
228 228 BP_init_header( &packet_sbm_bp2,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
231 231 }
232 232 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
233 233 {
234 234 BP_init_header( &packet_sbm_bp1,
235 235 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
236 236 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
237 237 BP_init_header( &packet_sbm_bp2,
238 238 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
239 239 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
240 240 }
241 241 else
242 242 {
243 243 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
244 244 }
245 245
246 246 status = get_message_queue_id_send( &queue_id );
247 247 if (status != RTEMS_SUCCESSFUL)
248 248 {
249 249 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
250 250 }
251 251 status = get_message_queue_id_prc0( &queue_id_q_p0);
252 252 if (status != RTEMS_SUCCESSFUL)
253 253 {
254 254 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
255 255 }
256 256
257 257 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
258 258
259 259 while(1){
260 260 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
261 261 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
262 262
263 263 incomingMsg = (asm_msg*) incomingData;
264 264
265 265 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
266 266 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
267 267
268 268 //****************
269 269 //****************
270 270 // BURST SBM1 SBM2
271 271 //****************
272 272 //****************
273 273 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
274 274 {
275 275 sid = getSID( incomingMsg->event );
276 276 // 1) compress the matrix for Basic Parameters calculation
277 277 ASM_compress_reorganize_and_divide( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
278 278 nb_sm_before_f0.burst_sbm_bp1,
279 279 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
280 280 ASM_F0_INDICE_START);
281 281 // 2) compute the BP1 set
282 282 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
283 283 // 3) send the BP1 set
284 284 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
285 285 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
286 packet_sbm_bp1.biaStatusInfo = pa_bia_status_info;
286 287 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
287 288 BP_send( (char *) &packet_sbm_bp1, queue_id,
288 289 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
289 290 sid);
290 291 // 4) compute the BP2 set if needed
291 292 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
292 293 {
293 294 // 1) compute the BP2 set
294 295 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
295 296 // 2) send the BP2 set
296 297 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
297 298 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
299 packet_sbm_bp2.biaStatusInfo = pa_bia_status_info;
298 300 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
299 301 BP_send( (char *) &packet_sbm_bp2, queue_id,
300 302 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
301 303 sid);
302 304 }
303 305 }
304 306
305 307 //*****
306 308 //*****
307 309 // NORM
308 310 //*****
309 311 //*****
310 312 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
311 313 {
312 314 // 1) compress the matrix for Basic Parameters calculation
313 315 ASM_compress_reorganize_and_divide( asm_f0_patched_norm, compressed_sm_norm_f0,
314 316 nb_sm_before_f0.norm_bp1,
315 317 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
316 318 ASM_F0_INDICE_START );
317 319 // 2) compute the BP1 set
318 320 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
319 321 // 3) send the BP1 set
320 322 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
321 323 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
324 packet_norm_bp1.biaStatusInfo = pa_bia_status_info;
322 325 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
323 326 BP_send( (char *) &packet_norm_bp1, queue_id,
324 327 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
325 328 SID_NORM_BP1_F0 );
326 329 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
327 330 {
328 331 // 1) compute the BP2 set using the same ASM as the one used for BP1
329 332 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
330 333 // 2) send the BP2 set
331 334 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
332 335 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
336 packet_norm_bp2.biaStatusInfo = pa_bia_status_info;
333 337 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
334 338 BP_send( (char *) &packet_norm_bp2, queue_id,
335 339 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
336 340 SID_NORM_BP2_F0);
337 341 }
338 342 }
339 343
340 344 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
341 345 {
342 346 // 1) reorganize the ASM and divide
343 347 ASM_reorganize_and_divide( asm_f0_patched_norm,
344 348 (float*) current_ring_node_to_send_asm_f0->buffer_address,
345 349 nb_sm_before_f0.norm_bp1 );
346 350 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
347 351 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
348 352 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
349 353
350 354 // 3) send the spectral matrix packets
351 355 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
352 356 // change asm ring node
353 357 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
354 358 }
355 359
356 360 update_queue_max_count( queue_id_q_p0, &hk_lfr_q_p0_fifo_size_max );
357 361
358 362 }
359 363 }
360 364
361 365 //**********
362 366 // FUNCTIONS
363 367
364 368 void reset_nb_sm_f0( unsigned char lfrMode )
365 369 {
366 370 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
367 371 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
368 372 nb_sm_before_f0.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 96;
369 373 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
370 374 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
371 375 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
372 376 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
373 377 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
374 378 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
375 379
376 380 if (lfrMode == LFR_MODE_SBM1)
377 381 {
378 382 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
379 383 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
380 384 }
381 385 else if (lfrMode == LFR_MODE_SBM2)
382 386 {
383 387 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
384 388 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
385 389 }
386 390 else if (lfrMode == LFR_MODE_BURST)
387 391 {
388 392 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
389 393 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
390 394 }
391 395 else
392 396 {
393 397 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
394 398 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
395 399 }
396 400 }
397 401
398 402 void init_k_coefficients_prc0( void )
399 403 {
400 404 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
401 405
402 406 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f0_norm, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_F0);
403 407 }
404 408
Show file before | Show file after | Hide comments
@@ -1,390 +1,394
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf1_prc1.h"
11 11
12 12 nb_sm_before_bp_asm_f1 nb_sm_before_f1;
13 13
14 14 extern ring_node sm_ring_f1[ ];
15 15
16 16 //***
17 17 // F1
18 18 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ];
19 19 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ];
20 20
21 21 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ];
22 22 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ];
23 23
24 24 float asm_f1_patched_norm [ TOTAL_SIZE_SM ];
25 25 float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ];
26 26 float asm_f1_reorganized [ TOTAL_SIZE_SM ];
27 27
28 28 char asm_f1_char [ TOTAL_SIZE_SM * 2 ];
29 29 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
30 30 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
31 31
32 32 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
33 33 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
34 34
35 35 //************
36 36 // RTEMS TASKS
37 37
38 38 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
39 39 {
40 40 int i;
41 41
42 42 rtems_event_set event_out;
43 43 rtems_status_code status;
44 44 rtems_id queue_id_prc1;
45 45 asm_msg msgForMATR;
46 46 ring_node *nodeForAveraging;
47 47 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0];
48 48 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
49 49 ring_node_asm *current_ring_node_asm_norm_f1;
50 50
51 51 unsigned int nb_norm_bp1;
52 52 unsigned int nb_norm_bp2;
53 53 unsigned int nb_norm_asm;
54 54 unsigned int nb_sbm_bp1;
55 55 unsigned int nb_sbm_bp2;
56 56
57 57 nb_norm_bp1 = 0;
58 58 nb_norm_bp2 = 0;
59 59 nb_norm_asm = 0;
60 60 nb_sbm_bp1 = 0;
61 61 nb_sbm_bp2 = 0;
62 62
63 63 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
64 64 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
65 65 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
66 66 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
67 67 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
68 68
69 69 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
70 70
71 71 status = get_message_queue_id_prc1( &queue_id_prc1 );
72 72 if (status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
75 75 }
76 76
77 77 while(1){
78 78 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
79 79
80 80 //****************************************
81 81 // initialize the mesage for the MATR task
82 82 msgForMATR.norm = current_ring_node_asm_norm_f1;
83 83 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 84 msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task
85 85 //
86 86 //****************************************
87 87
88 88 nodeForAveraging = getRingNodeForAveraging( 1 );
89 89
90 90 ring_node_tab[NB_SM_BEFORE_AVF1-1] = nodeForAveraging;
91 91 for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ )
92 92 {
93 93 nodeForAveraging = nodeForAveraging->previous;
94 94 ring_node_tab[NB_SM_BEFORE_AVF1-i] = nodeForAveraging;
95 95 }
96 96
97 97 // compute the average and store it in the averaged_sm_f1 buffer
98 98 SM_average( current_ring_node_asm_norm_f1->matrix,
99 99 current_ring_node_asm_burst_sbm_f1->matrix,
100 100 ring_node_tab,
101 101 nb_norm_bp1, nb_sbm_bp1,
102 102 &msgForMATR );
103 103
104 104 // update nb_average
105 105 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1;
106 106 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1;
107 107 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1;
108 108 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1;
109 109 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1;
110 110
111 111 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
112 112 {
113 113 nb_sbm_bp1 = 0;
114 114 // set another ring for the ASM storage
115 115 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
116 116 if ( lfrCurrentMode == LFR_MODE_BURST )
117 117 {
118 118 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F1;
119 119 }
120 120 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 121 {
122 122 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F1;
123 123 }
124 124 }
125 125
126 126 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
127 127 {
128 128 nb_sbm_bp2 = 0;
129 129 if ( lfrCurrentMode == LFR_MODE_BURST )
130 130 {
131 131 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F1;
132 132 }
133 133 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 134 {
135 135 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F1;
136 136 }
137 137 }
138 138
139 139 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
140 140 {
141 141 nb_norm_bp1 = 0;
142 142 // set another ring for the ASM storage
143 143 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
144 144 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 145 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 146 {
147 147 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F1;
148 148 }
149 149 }
150 150
151 151 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
152 152 {
153 153 nb_norm_bp2 = 0;
154 154 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 155 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 156 {
157 157 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F1;
158 158 }
159 159 }
160 160
161 161 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
162 162 {
163 163 nb_norm_asm = 0;
164 164 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
165 165 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
166 166 {
167 167 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1;
168 168 }
169 169 }
170 170
171 171 //*************************
172 172 // send the message to MATR
173 173 if (msgForMATR.event != 0x00)
174 174 {
175 175 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1);
176 176 }
177 177
178 178 if (status != RTEMS_SUCCESSFUL) {
179 179 PRINTF1("in AVF1 *** Error sending message to PRC1, code %d\n", status)
180 180 }
181 181 }
182 182 }
183 183
184 184 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 185 {
186 186 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 187 size_t size; // size of the incoming TC packet
188 188 asm_msg *incomingMsg;
189 189 //
190 190 unsigned char sid;
191 191 rtems_status_code status;
192 192 rtems_id queue_id_send;
193 193 rtems_id queue_id_q_p1;
194 194 bp_packet_with_spare packet_norm_bp1;
195 195 bp_packet packet_norm_bp2;
196 196 bp_packet packet_sbm_bp1;
197 197 bp_packet packet_sbm_bp2;
198 198 ring_node *current_ring_node_to_send_asm_f1;
199 199
200 200 unsigned long long int localTime;
201 201
202 202 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
203 203 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
204 204 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
205 205
206 206 //*************
207 207 // NORM headers
208 208 BP_init_header_with_spare( &packet_norm_bp1,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
211 211 BP_init_header( &packet_norm_bp2,
212 212 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
213 213 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
214 214
215 215 //***********************
216 216 // BURST and SBM2 headers
217 217 if ( lfrRequestedMode == LFR_MODE_BURST )
218 218 {
219 219 BP_init_header( &packet_sbm_bp1,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
222 222 BP_init_header( &packet_sbm_bp2,
223 223 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
224 224 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
225 225 }
226 226 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
227 227 {
228 228 BP_init_header( &packet_sbm_bp1,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
231 231 BP_init_header( &packet_sbm_bp2,
232 232 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
233 233 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
234 234 }
235 235 else
236 236 {
237 237 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
238 238 }
239 239
240 240 status = get_message_queue_id_send( &queue_id_send );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245 status = get_message_queue_id_prc1( &queue_id_q_p1);
246 246 if (status != RTEMS_SUCCESSFUL)
247 247 {
248 248 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
249 249 }
250 250
251 251 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
252 252
253 253 while(1){
254 254 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
255 255 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
256 256
257 257 incomingMsg = (asm_msg*) incomingData;
258 258
259 259 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
260 260 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
261 261
262 262 localTime = getTimeAsUnsignedLongLongInt( );
263 263 //***********
264 264 //***********
265 265 // BURST SBM2
266 266 //***********
267 267 //***********
268 268 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
269 269 {
270 270 sid = getSID( incomingMsg->event );
271 271 // 1) compress the matrix for Basic Parameters calculation
272 272 ASM_compress_reorganize_and_divide( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
273 273 nb_sm_before_f1.burst_sbm_bp1,
274 274 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
275 275 ASM_F1_INDICE_START);
276 276 // 2) compute the BP1 set
277 277 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
278 278 // 3) send the BP1 set
279 279 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
280 280 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
281 packet_sbm_bp1.biaStatusInfo = pa_bia_status_info;
281 282 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
282 283 BP_send( (char *) &packet_sbm_bp1, queue_id_send,
283 284 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
284 285 sid );
285 286 // 4) compute the BP2 set if needed
286 287 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
287 288 {
288 289 // 1) compute the BP2 set
289 290 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
290 291 // 2) send the BP2 set
291 292 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
292 293 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
294 packet_sbm_bp2.biaStatusInfo = pa_bia_status_info;
293 295 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
294 296 BP_send( (char *) &packet_sbm_bp2, queue_id_send,
295 297 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
296 298 sid );
297 299 }
298 300 }
299 301
300 302 //*****
301 303 //*****
302 304 // NORM
303 305 //*****
304 306 //*****
305 307 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
306 308 {
307 309 // 1) compress the matrix for Basic Parameters calculation
308 310 ASM_compress_reorganize_and_divide( asm_f1_patched_norm, compressed_sm_norm_f1,
309 311 nb_sm_before_f1.norm_bp1,
310 312 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
311 313 ASM_F1_INDICE_START );
312 314 // 2) compute the BP1 set
313 315 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
314 316 // 3) send the BP1 set
315 317 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
316 318 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
319 packet_norm_bp1.biaStatusInfo = pa_bia_status_info;
317 320 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
318 321 BP_send( (char *) &packet_norm_bp1, queue_id_send,
319 322 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
320 323 SID_NORM_BP1_F1 );
321 324 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
322 325 {
323 326 // 1) compute the BP2 set
324 327 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
325 328 // 2) send the BP2 set
326 329 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
327 330 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
331 packet_norm_bp2.biaStatusInfo = pa_bia_status_info;
328 332 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
329 333 BP_send( (char *) &packet_norm_bp2, queue_id_send,
330 334 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
331 335 SID_NORM_BP2_F1 );
332 336 }
333 337 }
334 338
335 339 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
336 340 {
337 341 // 1) reorganize the ASM and divide
338 342 ASM_reorganize_and_divide( asm_f1_patched_norm,
339 343 (float*) current_ring_node_to_send_asm_f1->buffer_address,
340 344 nb_sm_before_f1.norm_bp1 );
341 345 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
342 346 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
343 347 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
344 348 // 3) send the spectral matrix packets
345 349 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
346 350 // change asm ring node
347 351 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
348 352 }
349 353
350 354 update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
351 355
352 356 }
353 357 }
354 358
355 359 //**********
356 360 // FUNCTIONS
357 361
358 362 void reset_nb_sm_f1( unsigned char lfrMode )
359 363 {
360 364 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
361 365 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
362 366 nb_sm_before_f1.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 16;
363 367 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
364 368 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
365 369 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
366 370 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
367 371
368 372 if (lfrMode == LFR_MODE_SBM2)
369 373 {
370 374 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
371 375 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
372 376 }
373 377 else if (lfrMode == LFR_MODE_BURST)
374 378 {
375 379 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
376 380 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
377 381 }
378 382 else
379 383 {
380 384 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
381 385 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
382 386 }
383 387 }
384 388
385 389 void init_k_coefficients_prc1( void )
386 390 {
387 391 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
388 392
389 393 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
390 394 }
Show file before | Show file after | Hide comments
@@ -1,279 +1,281
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf2_prc2.h"
11 11
12 12 nb_sm_before_bp_asm_f2 nb_sm_before_f2;
13 13
14 14 extern ring_node sm_ring_f2[ ];
15 15
16 16 //***
17 17 // F2
18 18 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ];
19 19
20 20 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ];
21 21 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f2_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f2_reorganized [ TOTAL_SIZE_SM ];
25 25
26 26 char asm_f2_char [ TOTAL_SIZE_SM * 2 ];
27 27 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2];
28 28
29 29 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ]; // 12 * 32 = 384
30 30
31 31 //************
32 32 // RTEMS TASKS
33 33
34 34 //***
35 35 // F2
36 36 rtems_task avf2_task( rtems_task_argument argument )
37 37 {
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 rtems_id queue_id_prc2;
41 41 asm_msg msgForMATR;
42 42 ring_node *nodeForAveraging;
43 43 ring_node_asm *current_ring_node_asm_norm_f2;
44 44
45 45 unsigned int nb_norm_bp1;
46 46 unsigned int nb_norm_bp2;
47 47 unsigned int nb_norm_asm;
48 48
49 49 nb_norm_bp1 = 0;
50 50 nb_norm_bp2 = 0;
51 51 nb_norm_asm = 0;
52 52
53 53 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
54 54 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
55 55 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
56 56
57 57 BOOT_PRINTF("in AVF2 ***\n")
58 58
59 59 status = get_message_queue_id_prc2( &queue_id_prc2 );
60 60 if (status != RTEMS_SUCCESSFUL)
61 61 {
62 62 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
63 63 }
64 64
65 65 while(1){
66 66 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
67 67
68 68 //****************************************
69 69 // initialize the mesage for the MATR task
70 70 msgForMATR.norm = current_ring_node_asm_norm_f2;
71 71 msgForMATR.burst_sbm = NULL;
72 72 msgForMATR.event = 0x00; // this composite event will be sent to the PRC2 task
73 73 //
74 74 //****************************************
75 75
76 76 nodeForAveraging = getRingNodeForAveraging( 2 );
77 77
78 78 // compute the average and store it in the averaged_sm_f2 buffer
79 79 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
80 80 nodeForAveraging,
81 81 nb_norm_bp1,
82 82 &msgForMATR );
83 83
84 84 // update nb_average
85 85 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
86 86 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
87 87 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
88 88
89 89 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
90 90 {
91 91 nb_norm_bp1 = 0;
92 92 // set another ring for the ASM storage
93 93 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
94 94 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
95 95 || (lfrCurrentMode == LFR_MODE_SBM2) )
96 96 {
97 97 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F2;
98 98 }
99 99 }
100 100
101 101 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
102 102 {
103 103 nb_norm_bp2 = 0;
104 104 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
105 105 || (lfrCurrentMode == LFR_MODE_SBM2) )
106 106 {
107 107 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F2;
108 108 }
109 109 }
110 110
111 111 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
112 112 {
113 113 nb_norm_asm = 0;
114 114 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
115 115 || (lfrCurrentMode == LFR_MODE_SBM2) )
116 116 {
117 117 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F2;
118 118 }
119 119 }
120 120
121 121 //*************************
122 122 // send the message to MATR
123 123 if (msgForMATR.event != 0x00)
124 124 {
125 125 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC2);
126 126 }
127 127
128 128 if (status != RTEMS_SUCCESSFUL) {
129 129 PRINTF1("in AVF2 *** Error sending message to MATR, code %d\n", status)
130 130 }
131 131 }
132 132 }
133 133
134 134 rtems_task prc2_task( rtems_task_argument argument )
135 135 {
136 136 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
137 137 size_t size; // size of the incoming TC packet
138 138 asm_msg *incomingMsg;
139 139 //
140 140 rtems_status_code status;
141 141 rtems_id queue_id_send;
142 142 rtems_id queue_id_q_p2;
143 143 bp_packet packet_norm_bp1;
144 144 bp_packet packet_norm_bp2;
145 145 ring_node *current_ring_node_to_send_asm_f2;
146 146
147 147 unsigned long long int localTime;
148 148
149 149 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
150 150 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
151 151 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
152 152
153 153 //*************
154 154 // NORM headers
155 155 BP_init_header( &packet_norm_bp1,
156 156 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
157 157 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
158 158 BP_init_header( &packet_norm_bp2,
159 159 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
160 160 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
161 161
162 162 status = get_message_queue_id_send( &queue_id_send );
163 163 if (status != RTEMS_SUCCESSFUL)
164 164 {
165 165 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
166 166 }
167 167 status = get_message_queue_id_prc2( &queue_id_q_p2);
168 168 if (status != RTEMS_SUCCESSFUL)
169 169 {
170 170 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
171 171 }
172 172
173 173 BOOT_PRINTF("in PRC2 ***\n")
174 174
175 175 while(1){
176 176 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
177 177 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
178 178
179 179 incomingMsg = (asm_msg*) incomingData;
180 180
181 181 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
182 182
183 183 localTime = getTimeAsUnsignedLongLongInt( );
184 184
185 185 //*****
186 186 //*****
187 187 // NORM
188 188 //*****
189 189 //*****
190 190 // 1) compress the matrix for Basic Parameters calculation
191 191 ASM_compress_reorganize_and_divide( asm_f2_patched_norm, compressed_sm_norm_f2,
192 192 nb_sm_before_f2.norm_bp1,
193 193 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
194 194 ASM_F2_INDICE_START );
195 195 // BP1_F2
196 196 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
197 197 {
198 198 // 1) compute the BP1 set
199 199 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
200 200 // 2) send the BP1 set
201 201 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
202 202 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
203 packet_norm_bp1.biaStatusInfo = pa_bia_status_info;
203 204 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
204 205 BP_send( (char *) &packet_norm_bp1, queue_id_send,
205 206 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
206 207 SID_NORM_BP1_F2 );
207 208 }
208 209 // BP2_F2
209 210 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
210 211 {
211 212 // 1) compute the BP2 set
212 213 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
213 214 // 2) send the BP2 set
214 215 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
215 216 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
217 packet_norm_bp2.biaStatusInfo = pa_bia_status_info;
216 218 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
217 219 BP_send( (char *) &packet_norm_bp2, queue_id_send,
218 220 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
219 221 SID_NORM_BP2_F2 );
220 222 }
221 223
222 224 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
223 225 {
224 226 // 1) reorganize the ASM and divide
225 227 ASM_reorganize_and_divide( asm_f2_patched_norm,
226 228 (float*) current_ring_node_to_send_asm_f2->buffer_address,
227 229 nb_sm_before_f2.norm_bp1 );
228 230 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
229 231 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
230 232 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
231 233 // 3) send the spectral matrix packets
232 234 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
233 235 // change asm ring node
234 236 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
235 237 }
236 238
237 239 update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max );
238 240
239 241 }
240 242 }
241 243
242 244 //**********
243 245 // FUNCTIONS
244 246
245 247 void reset_nb_sm_f2( void )
246 248 {
247 249 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
248 250 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
249 251 nb_sm_before_f2.norm_asm = parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1];
250 252 }
251 253
252 254 void SM_average_f2( float *averaged_spec_mat_f2,
253 255 ring_node *ring_node,
254 256 unsigned int nbAverageNormF2,
255 257 asm_msg *msgForMATR )
256 258 {
257 259 float sum;
258 260 unsigned int i;
259 261
260 262 for(i=0; i<TOTAL_SIZE_SM; i++)
261 263 {
262 264 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
263 265 if ( (nbAverageNormF2 == 0) )
264 266 {
265 267 averaged_spec_mat_f2[ i ] = sum;
266 268 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
267 269 msgForMATR->fineTimeNORM = ring_node->fineTime;
268 270 }
269 271 else
270 272 {
271 273 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
272 274 }
273 275 }
274 276 }
275 277
276 278 void init_k_coefficients_prc2( void )
277 279 {
278 280 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
279 281 }
General Comments 0
You need to be logged in to leave comments. Login now